多旋翼飞行器设计与控制（五）—— 坐标系和姿态表示
一、坐标系
一般的，对坐标轴与旋转正方向的定义采用右手定则。介绍两种坐标系：地球固联坐标系 与 机体坐标系 。
地球固联坐标系用于研究多旋翼飞行器相对于地面的运动状态，确定机体的空间位置坐标。它忽略地球曲率，即将地球表面假设成一张平面。通常以多旋翼起飞位置或者地心作为坐标原点。先让x轴在水平面内指向某一方向，z轴垂直于地面向下，然后按右手定则确定y轴。
机体坐标系与多旋翼固联，其原点取在多旋翼的重心位置。x轴在多旋翼对称平面内指向机头。z轴在飞机对称平面内垂直x轴向下，然后按右手定则确定y轴。
两个坐标系之间可以利用矩阵变换进行转换，地球固联坐标系绕固定点转动三次即可使它与机体坐标系的三轴指向完全一致，转动角度即为欧拉角
二、欧拉角
定义：机体坐标系与地面地球固联坐标系之间的夹角就是飞机的姿态角，又称欧拉角。俯仰角为机体轴与水平面之间的夹角，飞机抬头为证。偏航角为机体轴在水平面上的投影与地轴之间的夹角，以机头右偏为正。滚转角为飞机对称面绕机体轴转过的角度，右滚为正。
欧拉角变化率与机体角速度的关系
可以通过三次旋转的矩阵相乘得到总的转换矩阵表示欧拉角变化速率与机体角速度的关系。
公式如下：
b ω = [ ω x b ω y b ω z b ] T {^b} \omega = [\omega_{xb}~~~ \omega_{yb} ~~~ \omega_{zb} ]^T bω=[ωxb ωyb ωzb]T
b ω = Ψ ′ ⋅ b k 3 + θ ′ ⋅ n 2 + ϕ ′ ⋅ b 1 {^b} \omega = \Psi ^\prime \cdot {^b}k_3 + \theta ^\prime \cdot n_2 + \phi ^\prime \cdot b_1 bω=Ψ′⋅bk3+θ′⋅n2+ϕ′⋅b1
其 中 k 3 ， n 2 ， b 1 即 为 旋 转 轴 其中k_3，n_2，b_1即为旋转轴 其中k3，n2，b1即为旋转轴
最终两者之间的关系可表示为
欧拉角变化速率 ——> 机体角速度：
[ ω x b ω y b ω z b ] = [ 1 0 − s i n θ 0 c o s ϕ c o s θ s i n ϕ 0 − s i n ϕ c o s θ c o s ϕ ] [ ϕ ′ θ ′ Ψ ′ ] \begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix} = \begin{bmatrix} 1& 0 & -sin\theta \\ 0& cos\phi& cos\theta sin\phi \\ 0 & -sin\phi& cos\theta cos\phi \end{bmatrix} \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} ⎣⎡ωxbωybωzb⎦⎤=⎣⎡1000cosϕ−sinϕ−sinθcosθsinϕcosθcosϕ⎦⎤⎣⎡ϕ′θ′Ψ′⎦⎤
从该式子不难发现从欧拉角变化速率转换到角速的转换矩阵中并没有偏航角的参与
机体角速度 ——> 欧拉角变化速率：
[ ϕ θ Ψ ] = [ 1 t a n θ s i n ϕ t a n θ c o s ϕ 0 c o s ϕ − s i n ϕ 0 s i n ϕ c o s θ c o s ϕ c o s θ ] [ ϕ ′ θ ′ Ψ ′ ] \begin{bmatrix} \phi \\ \theta \\ \Psi \end{bmatrix} = \begin{bmatrix} 1& tan\theta sin\phi & tan\theta cos\phi \\ 0& cos\phi& -sin\phi \\ 0 & \frac{sin\phi}{cos\theta}& \frac{cos\phi}{cos\theta} \end{bmatrix} \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} ⎣⎡ϕθΨ⎦⎤=⎣⎡100tanθsinϕcosϕcosθsinϕtanθcosϕ−sinϕcosθcosϕ⎦⎤⎣⎡ϕ′θ′Ψ′⎦⎤
从这个式子中不难发现当俯仰角为正负90度时，矩阵中的元素出现被除数为0的情况，这便是解的奇异性问题。从物理意义来说这叫万向节死锁现象。即俯仰角为正负90度时，偏航角对应了无数种情况，不能唯一确定。所以用欧拉角表示姿态存在缺陷
当俯仰角和滚动角幅度不大，近似为零时，上式可近似为：
[ ϕ ′ θ ′ Ψ ′ ] = [ ω x b ω y b ω z b ] \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} = \begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix} ⎣⎡ϕ′θ′Ψ′⎦⎤=⎣⎡ωxbωybωzb⎦⎤
三、旋转矩阵
从地球固联坐标系到机体坐标系的旋转可以通过三步完成：
其中
R z ( Ψ ) = [ c o s Ψ s i n Ψ 0 − s i n Ψ c o s Ψ 0 0 0 1 ] R_z(\Psi) = \begin{bmatrix} cos \Psi & sin \Psi & 0\\ -sin \Psi & cos \Psi & 0 \\ 0&0&1 \end{bmatrix} Rz(Ψ)=⎣⎡cosΨ−sinΨ0sinΨcosΨ0001⎦⎤
R y ( θ ) = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] R_y(\theta) = \begin{bmatrix} cos \theta & 0 & -sin \theta \\ 0&1&0 \\ sin \theta&0& cos \theta \end{bmatrix} Ry(θ)=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤
R x ( ϕ ) = [ 1 0 0 0 c o s ϕ s i n ϕ 0 − s i n ϕ c o s θ ] R_x(\phi) = \begin{bmatrix} 1 & 0 & 0\\ 0 & cos \phi & sin \phi \\ 0& -sin \phi & cos \theta \end{bmatrix} Rx(ϕ)=⎣⎡1000cosϕ−sinϕ0sinϕcosθ⎦⎤
从机身坐标转换到地球固联坐标系可以通过上面三个矩阵相乘得到
R b e = [ c o s θ c o s Ψ c o s Ψ s i n θ s i n ϕ − s i n Ψ c o s ϕ c o s Ψ s i n θ c o s ϕ + s i n Ψ s i n ϕ c o s θ s i n Ψ s i n Ψ s i n θ s i n ϕ + c o s Ψ c o s ϕ s i n Ψ s i n θ c o s ϕ − c o s Ψ s i n ϕ − s i n θ s i n ϕ c o s θ c o s ϕ c o s θ ] R^e_b = \begin{bmatrix} cos \theta cos \Psi & cos \Psi sin \theta sin \phi - sin \Psi cos \phi & cos \Psi sin \theta cos \phi + sin \Psi sin \phi\\ cos \theta sin \Psi & sin \Psi sin \theta sin \phi + cos \Psi cos \phi & sin \Psi sin \theta cos \phi - cos \Psi sin \phi \\ -sin \theta & sin \phi cos \theta & cos \phi cos \theta \end{bmatrix} Rbe=⎣⎡cosθcosΨcosθsinΨ−sinθcosΨsinθsinϕ−sinΨcosϕsinΨsinθsinϕ+cosΨcosϕsinϕcosθcosΨsinθcosϕ+sinΨsinϕsinΨsinθcosϕ−cosΨsinϕcosϕcosθ⎦⎤
上 式 可 以 简 化 为 R b e = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] 上式可以简化为 R^e_b = \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23} \\ r_{31}& r_{32} & r_{33} \end{bmatrix} 上式可以简化为Rbe=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
我们可以通过旋转矩阵反求欧拉角
{ Ψ = a r c t a n r 21 r 11 θ = a r c s i n ( − r 31 ) ϕ = a r c t a n r 32 r 33 \left\{ \begin{array}{c} \Psi = arctan \frac{r_{21}}{r_{11}} \\ \theta = arcsin(-r_{31}) \\ \phi = arctan \frac{r_{32}}{r_{33}} \end{array} \right. ⎩⎨⎧Ψ=arctanr11r21θ=arcsin(−r31)ϕ=arctanr33r32
但是用这种方式表示仍然存在万向节死锁问题，在这种情况下，人为设定滚转角为0 此时便有对应的偏航角
旋转矩阵导数与机体角速度关系
d R b e d t ∣ = R b e [ b ω ] x \left. \frac{{\rm d} R^e_b}{{\rm d}t} \right| = R^e_b [ {^b}\omega]_x dtdRbe∣∣∣∣=Rbe[bω]x
采用旋转矩阵表示避免了奇异性问题。然而，以上方程含有9个自由变量，因此求解微分方程的计算量比较大。
通过对欧拉角和旋转矩阵的学习，我们发现欧拉角表示存在万向节死锁问题，旋转矩阵表示存在计算量过大问题，而人们发现使用四元数表示姿态可以解决上述问题
四、四元数
四元数定义：四元数一般用向量形式表示为：
q = [ q 0 q v ] q= \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} q=[q0qv]
其 中 q 0 为 四 元 数 的 标 量 部 分 ， q v = [ q 1 q 2 q 3 ] T 为 四 元 数 的 向 量 部 分 。 其中q_0 为四元数的标量部分， q_v = [q_1 ~~ q_2 ~~ q_3]^T 为四元数的向量部分。 其中q0为四元数的标量部分，qv=[q1 q2 q3]T为四元数的向量部分。
对 于 一 个 实 数 S ， 其 四 元 数 表 示 形 式 为 q = [ s 0 3 × 3 ] 对于一个实数S，其四元数表示形式为 q = \begin{bmatrix} s \\ 0_{3 \times 3} \\ \end{bmatrix} 对于一个实数S，其四元数表示形式为q=[s03×3]
对 于 一 个 纯 向 量 v , 其 四 元 数 表 示 形 式 q = [ 0 v ] 对于一个纯向量v,其四元数表示形式 q = \begin{bmatrix} 0 \\ v \\ \end{bmatrix} 对于一个纯向量v,其四元数表示形式q=[0v]
四元数的基本运算法则：
- 四元数加减法：
p ± q = [ p 0 p v ] ± [ q 0 q v ] = [ p 0 ± q 0 p v ± q v ] p \pm q = \begin{bmatrix} p_0 \\ p_v \\ \end{bmatrix} \pm \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} = \begin{bmatrix} p_0 \pm q_0 \\ p_v \pm q_v \\ \end{bmatrix} p±q=[p0pv]±[q0qv]=[p0±q0pv±qv]
- 四元数乘法：
p ⨂ q = [ p 0 p v ] ⨂ [ q 0 q v ] = [ p 0 q 0 − q v T p v p v × q v + p 0 q v + q 0 p v ] p \bigotimes q = \begin{bmatrix} p_0 \\ p_v \\ \end{bmatrix} \bigotimes \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} = \begin{bmatrix} p_0q_0 - q^T_vp_v \\ p_v \times q_v + p_0q_v +q_0p_v \\ \end{bmatrix} p⨂q=[p0pv]⨂[q0qv]=[p0q0−qvTpvpv×qv+p0qv+q0pv]
-
四元数共轭：
若 q = [ q 0 q v ] ， 则 q ∗ = [ q 0 − q v ] 若q= \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix}， 则q^*= \begin{bmatrix} q_0 \\ -q_v \\ \end{bmatrix} 若q=[q0qv]，则q∗=[q0−qv] -
四元数范数:
∣ ∣ q ∣ ∣ 2 = ∣ ∣ q ⨂ q ∗ ∣ ∣ = q 1 2 + q 2 2 + q 3 2 + q 4 2 \mid \mid q \mid \mid ^2 = \mid \mid q \bigotimes q^* \mid \mid = q^2_1+q^2_2+q^2_3+q^2_4 ∣∣q∣∣2=∣∣q⨂q∗∣∣=q12+q22+q32+q42
- 四元数的逆：
q ⨂ q − 1 = [ 1 0 3 × 3 ] q \bigotimes q^-1 = \begin{bmatrix} 1 \\ 0_{3 \times 3} \\ \end{bmatrix} q⨂q−1=[103×3]
q − 1 = q ∗ ∣ ∣ q ∣ ∣ q^-1 = \frac{q^*}{\mid \mid q \mid \mid} q−1=∣∣q∣∣q∗
- 基本运算性质：
q ⨂ ( r + m ) = q ⨂ r + q ⨂ m q \bigotimes (r+m) = q \bigotimes r + q \bigotimes m q⨂(r+m)=q⨂r+q⨂m
q ⨂ r ⨂ m = ( q ⨂ r ) ⨂ m = q ⨂ ( r ⨂ m ) q \bigotimes r \bigotimes m = (q \bigotimes r) \bigotimes m = q \bigotimes (r \bigotimes m ) q⨂r⨂m=(q⨂r)⨂m=q⨂(r⨂m)
s q = q s = [ s q 0 s q v ] sq =qs = \begin{bmatrix} sq_0 \\ sq_v \\ \end{bmatrix} sq=qs=[sq0sqv]
[ 0 u ] ⨂ [ 0 v ] = [ − u T v u × v ] \begin{bmatrix} 0 \\ u \\ \end{bmatrix} \bigotimes \begin{bmatrix} 0 \\ v \\ \end{bmatrix} = \begin{bmatrix} -u^Tv \\ u \times v \\ \end{bmatrix} [0u]⨂[0v]=[−uTvu×v]
四元数与旋转：
这 个 旋 转 过 程 可 以 表 示 为 向 量 v 1 绕 着 v 轴 按 照 右 手 定 则 旋 转 θ 变 换 到 了 向 量 v 1 ′ 这个旋转过程可以表示为向量v_1绕着v轴按照右手定则旋转 \theta 变换到了向量v_1 \prime 这个旋转过程可以表示为向量v1绕着v轴按照右手定则旋转θ变换到了向量v1′
四元数与旋转矩阵的转换：
四元数与欧拉角的转换：
四元数变化率与机体角速度关系：
五、三种姿态表示方法的比较
