【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器
【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
【ROS&GAZEBO】多旋翼无人机仿真（七）——四元数姿态控制
从直观来看，欧拉角或者姿态角是最直观的能感受到三维旋转的表示方法。因此，在做多旋翼或者其他机器人的姿态控制时，最能够直观接受的方法就是欧拉角姿态控制。可能很多人会觉得，为什么需要那么多花里胡哨的方法呢，控制俯仰-滚转-航向三个姿态角就能够稳定姿态了。但是，用姿态角进行控制是有前提的，那就是小角度假设，如果不满足小角度的条件，那么姿态角控制就不适用。姿态角是存在旋转顺序的，并且存在万向节死锁，具体可以看上一篇【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
因此，我们想要一种参数少，并且不存在旋转顺序和死锁的姿态描述方法。因此，四元数表示法就被提出来了，四元数通常表示形式是这样的：
q
=
[
q
0
q
1
q
2
q
3
]
\mathbf q=\begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}
q=[q0q1q2q3]
采用轴角方式表达如下：
q
=
[
c
o
s
α
2
u
s
i
n
α
2
]
\mathbf q=\begin{bmatrix}cos\frac\alpha2\\\mathbf usin\frac\alpha2\end{bmatrix}
q=[cos2αusin2α]
它对应于绕一个向量
v
=
(
v
x
,
v
y
,
v
z
)
\mathbf v=\left(v_x,v_y,v_z\right)
v=(vx,vy,vz)为轴旋转
θ
\theta
θ的操作（右手法则的旋转）：
q
=
c
o
s
θ
2
+
i
s
i
n
θ
2
+
j
s
i
n
θ
2
+
k
s
i
n
θ
2
\mathbf q=cos\frac\theta2+\mathbf isin\frac\theta2+\mathbf jsin\frac\theta2+\mathbf ksin\frac\theta2
q=cos2θ+isin2θ+jsin2θ+ksin2θ
我们知道，姿态矩阵左乘向量即可得到旋转后的向量，四元数同样也可以对向量进行旋转，四元数对三维向量的旋转公式如下：
v
′
=
q
v
q
∗
\mathbf v'=\mathbf q\mathbf{vq}^\ast
v′=qvq∗
其中
v
\mathbf{v}
v为纯四元数：
v
=
[
0
v
⃗
]
\mathbf v=\left[\begin{array}{l}0\\\vec v\end{array}\right]
v=[0v
]
v
⃗
\vec v
v
就是三维向量，实部为0，这个公式也是四元数旋转中用得最广泛的一个公式。本篇讲的是如何做四元数误差控制，因此证明过程这里不做讲解
有了这个公式，我们就可以定义四元数的连续旋转。我们来看下面的情况。首先对
v
1
\mathbf {v_1}
v1进行旋转，这里旋转的四元数是
q
1
\mathbf q_1
q1
v
1
′
=
q
1
v
1
q
1
∗
\mathbf {v_1}'=\mathbf q_1\mathbf {v_1}\mathbf {q_1}^\ast
v1′=q1v1q1∗
接着用
q
2
\mathbf {q_2}
q2对
v
1
′
\mathbf {v_1}'
v1′进行旋转：
v
′
′
=
q
2
v
′
q
2
∗
\mathbf v''={\mathbf q}_2\mathbf v'\mathbf q_2^\ast
v′′=q2v′q2∗
可以发现，这里的
v
′
′
\mathbf v''
v′′是
v
1
\mathbf {v_1}
v1经过了两次旋转得到的结果，那
v
′
′
\mathbf v''
v′′和
v
1
\mathbf {v_1}
v1的关系是什么样的呢？下面的式子很明显成立
v
′
′
=
q
2
(
q
1
v
q
1
∗
)
q
2
∗
\mathbf v''={\mathbf q}_2\left({\mathbf q}_1\mathbf v{\mathbf q}_1{}^\ast\right){\mathbf q}_2{}^\ast
v′′=q2(q1vq1∗)q2∗
然后根据单位四元数共轭的性质：
q
∗
=
q
−
1
\mathbf q^\ast=\mathbf q^{-1}
q∗=q−1
可得
v
′
′
=
(
q
2
q
1
)
v
(
q
2
q
1
)
∗
\mathbf v''=\left({\mathbf q}_2{\mathbf q}_1\right)\mathbf v\left({\mathbf q}_2{\mathbf q}_1\right)^\ast
v′′=(q2q1)v(q2q1)∗
因此，
q
2
q
1
{\mathbf q}_2{\mathbf q}_1
q2q1是这两次旋转复合的四元数。也说明了四元数乘法在几何上的意义就是连续的旋转。好了，有了这个性质，我们就可以去计算四元数的姿态误差了。我们来看下面这个图，这个图表示的就是连续旋转两次的意思。
那如果我们把
θ
2
\theta_2
θ2换一下，用
Δ
θ
\Delta\theta
Δθ来表示，
v
\mathbf {v}
v表示初始水平姿态的向量，
v
s
\mathbf {v_s}
vs表示当前姿态的向量，
v
d
\mathbf {v_d}
vd表示期望姿态的向量。那么连续旋转公式变为：
q
d
=
Δ
q
⋅
q
s
{\mathbf q}_d=\Delta\mathbf q\cdot{\mathbf q}_s
qd=Δq⋅qs
Δ
q
\Delta\mathbf q
Δq表示的是旋转
Δ
θ
\Delta\theta
Δθ对应的四元数，
q
d
\mathbf q_d
qd表示旋转
θ
d
\theta_d
θd对应的四元数，用图来表示如下：
Δ
q
\Delta\mathbf q
Δq很明显，就是从当前的姿态到期望姿态的误差。那如何得到
Δ
q
\Delta\mathbf q
Δq呢？很明显，这不能直接通过减法得到，将上面的式中进行变换：
q
d
⋅
(
q
s
)
−
1
=
Δ
q
⋅
q
s
⋅
(
q
s
)
−
1
Δ
q
=
q
d
⋅
(
q
s
)
−
1
\begin{array}{c}{\mathbf q}_d\cdot\left({\mathbf q}_s\right)^{-1}=\Delta\mathbf q\cdot{\mathbf q}_s\cdot\left({\mathbf q}_s\right)^{-1}\\\Delta\mathbf q={\mathbf q}_d\cdot\left({\mathbf q}_s\right)^{-1}\end{array}
qd⋅(qs)−1=Δq⋅qs⋅(qs)−1Δq=qd⋅(qs)−1
终于大功告成，误差四元数计算公式就有了。然后怎么得到各轴误差角度呢？假设期望和当前的误差角很小，那么下式中的
s
i
n
θ
sin\theta
sinθ和
θ
\theta
θ就可以替换：
q
=
c
o
s
θ
2
+
i
s
i
n
θ
2
+
j
s
i
n
θ
2
+
k
s
i
n
θ
2
\mathbf q=cos\frac\theta2+\mathbf isin\frac\theta2+\mathbf jsin\frac\theta2+\mathbf ksin\frac\theta2
q=cos2θ+isin2θ+jsin2θ+ksin2θ
因此，得到的误差四元数的虚部就是各轴的姿态误差分量，但是还忽略了一个重要的问题，那就是同样的旋转，四元数并不是唯一的，可以通过图来直观的理解一下：
从
v
\mathbf v
v到
v
d
\mathbf v_d
vd除了虚线表示的
θ
d
\theta_d
θd之外，
2
π
−
θ
d
2\mathrm\pi-\theta_d
2π−θd很显然也可以达到同样的旋转，这其中就有一个最短旋转角度的问题，但是这两个旋转是有关系的，即
2
π
−
θ
d
2\mathrm\pi-\theta_d
2π−θd的四元数为
−
q
-\mathbf q
−q。所以，假想一下，飞机从15度回到0度有两种方式，一种是绕轴旋转15度，还有一种是绕轴旋转365度，程序算出来说，哎，我就是玩，我不直接用最小的角度回去，我要绕轴旋转365度回去，，，，好像从原理上来说没毛病，没有错，都可以回到0度，但是实际上没人也不会有人用后面这种方式去这样做吧。。除了谁有十年的脑血栓。
回到正题，我们得到误差的时候也需要去回避这个问题，那怎么去回避呢，上面说过了， − q -\mathbf q −q旋转的角度为 2 π − θ d 2\mathrm\pi-\theta_d 2π−θd，那就好办了， q \mathbf q q的实部 q 0 \mathrm q_0 q0是 c o s θ 2 cos\frac\theta2 cos2θ，而 c o s θ 2 cos\frac\theta2 cos2θ只有在 [ − π , π ] \left[-\pi,\pi\right] [−π,π]上为正，超过这个范围就为负，那可不可以根据 q 0 \mathrm q_0 q0来判断旋转角是不是最小的呢，答案很明显是的。
因此，只要判断
q
0
\mathrm q_0
q0的正负，就可以判断误差四元数是不是最小的旋转角，如果不是，我们将四元数取反，那最终根据四元数求得到的各轴误差角度分量，误差角度也就是期望的角速度，计算公式就可以整理出来了：
ω
d
=
k
s
g
n
(
q
e
r
r
,
0
)
q
e
r
r
,
1
:
3
q
e
r
r
=
q
d
⋅
q
s
−
1
\begin{array}{c}{\mathbf\omega}_d=ksgn\left({\mathbf q}_{err,0}\right){\mathbf q}_{err,1:3}\\{\mathbf q}_{err}={\mathbf q}_d\cdot{\mathbf q}_s{}^{-1}\end{array}
ωd=ksgn(qerr,0)qerr,1:3qerr=qd⋅qs−1
k
k
k是可调节的误差系数，因为是近似的，所以
k
k
k可以用来调节误差得到期望角速度的大小。需要注意的是，这里面的乘法是四元数乘法，并且四元数都是单位四元数。
好了，关于四元数姿态控制的内容就讲到这里，下一篇，将介绍如何将四元数控制方法用在四旋翼的仿真中。
最后再讲两句，鄙人也在深入研究四元数、李群和李代数这部分的内容，也深刻体会到四元数误差的推导过程可能会有一点抽象，因为四元数是四维空间的变量，所以并不能很直观的理解，这里需要小伙伴去深入琢磨。此外，四元数是复数的扩展，可能有的小伙伴会说，我连复数都没玩明白，复数其实就可以表示旋转，只不过是2维的旋转，四元数是3维的旋转，因此，复数和四元数有许多相似的性质，讲到这里，是不是感觉有些奇妙和不可思议，这就是数理美妙的所在。欢迎感兴趣的小伙伴添加微信 Reed_UAV 进行交流：
