【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
进行交流: