飞行器控制笔记（二）——姿态解算之坐标变换与欧拉角更新姿态

飞行器控制笔记（二）——姿态解算之坐标变换与欧拉角更新姿态
- 一、基本假定
- 二、坐标变换矩阵
  - 2.1绕z轴旋转
  - 2.2绕y轴旋转
  - 2.3绕x轴旋转
- 三姿态角变化率与机体角速率的关系
- 参考文献

一、基本假定

若将飞行器看作刚体，则它在空间中的姿态主要是指与机体固连的机体坐标系跟与大地固连的坐标系之间的旋转关系。旋转关系可以用欧拉角描述，为了方便叙述，我先记录下面的一些基本假定：
1. 建立的坐标系均是右手系，且欧拉角的旋转方向也满足右手定则；
2. 与机体固连的机体坐标系的 x轴，y轴，z轴 x 轴， y 轴， z 轴 x轴，y轴，z轴的正方向分别为前右下，与大地固连的大地坐标系的 X轴，Y轴，Z轴 X 轴， Y 轴， Z 轴 X轴，Y轴，Z轴的正方向分别为北东地；
3. 机体坐标系绕其 z轴 z 轴 z轴旋转所得到的欧拉角称为偏航角 ψ ψ \psi,机体坐标系绕其 y轴 y 轴 y轴旋转所得到的欧拉角称为俯仰角 θ θ \theta,机体坐标系绕其 x轴 x 轴 x轴旋转所得到的欧拉角称为横滚角 ϕ ϕ \phi；
4. 用上下标 b b b 来表示向量在机体（Body）坐标系中，用上下标e" role="presentation">eee 来表示向量在大地（Earth）坐标系中；
5. 在各个坐标系中，与x坐标轴相固连的单位向量 e1,k1,n1,b1,…=[100]T e 1 , k 1 , n 1 , b 1 , … = [ 1 0 0 ] T e_1,k_1,n_1,b_1,\ldots=\begin{bmatrix}1&0&0 \end{bmatrix}^T ，与x坐标轴相固连的单位向量 e2,k2,n2,b2,…=[010]T e 2 , k 2 , n 2 , b 2 , … = [ 0 1 0 ] T e_2,k_2,n_2,b_2,\ldots=\begin{bmatrix}0&1&0 \end{bmatrix}^T ，与z坐标轴相固连的单位向量 e3,k3,n3,b3,…=[001]T e 3 , k 3 , n 3 , b 3 , … = [ 0 0 1 ] T e_3,k_3,n_3,b_3,\ldots=\begin{bmatrix}0&0&1 \end{bmatrix}^T。不论坐标系如何变换，其对应的三个正交单位向量（基底）之间的位置关系并不发生变化，因此才有这个假设。

二、坐标变换矩阵

机体坐标系在任一时刻的姿态都可以分解为通过大地坐标系绕固定点的三次旋转，每次旋转的的旋转轴对应于将要旋转的坐标系的某一坐标轴，也就是上面提到的欧拉角。旋转的次序不同，最终得到的姿态也不相同，因此，这里也规定这三次旋转的次序分别为绕先 z旋转 z 旋转 z旋转 ,再绕 y旋转 y 旋转 y旋转,最后绕 x旋转 x 旋转 x旋转,即 ψ−θ−ϕ ψ − θ − ϕ \psi-\theta-\phi.如下图所示，机体坐标系的旋转已经分解成了三次坐标系之间基本变换。下面就分别推导绕 z z z ,绕y" role="presentation">yyy,绕 x x x的坐标变换矩阵。

需要注意到的一点是，这里讨论的都是坐标系之间的变换。也就是说空间中的位置向量或坐标点本身并不发生变化，而只是将它们从一个参考坐标系变换到了另一个参考系当中。

2.1绕z轴旋转

如上图所示，当坐标系绕z轴旋转时，空间中的向量与z轴之间的相对关系不会改变，因此在旋转前后z′=z" role="presentation">z′=zz′=zz\prime=z,现在就只考虑该向量在垂直于 Z Z Z 轴的平面上的投影OA→" role="presentation">OA→OA→\vec{OA} ，分别在平面直角坐标系 OXY O X Y OXY 跟平面直角坐标系 Oxy O x y Oxy 上坐标之间的关系，如果向量 OA→ O A → \vec{OA} 的模为 r r r,它在坐标系OXY" role="presentation">OXYOXYOXY 中的坐标可以表示如下：

{x=rcos(β+ψ)y=rsin(β+ψ) { x = r cos ⁡ ( β + ψ ) y = r sin ⁡ ( β + ψ )

\begin{cases} x=r\cos(\beta+\psi)\\ y=r\sin(\beta+\psi) \end{cases}
在坐标系 Oxy O x y Oxy 中的坐标如下：

{x′=rcosβy′=rsinβ { x ′ = r cos ⁡ β y ′ = r sin ⁡ β

\begin{cases} x\prime=r\cos\beta\\ y\prime=r\sin\beta \end{cases}
联合这两组式子可以得到：

{x′y′==xcosψ+ysinψycosψ−xsinψ { x ′ = x cos ⁡ ψ + y sin ⁡ ψ y ′ = y cos ⁡ ψ − x sin ⁡ ψ

\begin{cases} x\prime&=&x\cos\psi+y\sin\psi\\ y\prime&=&y\cos\psi-x\sin\psi \end{cases}
并且已经知道 z′=z z ′ = z z\prime=z 了，所以把上面的式子写成矩阵的形式则是：

Rz(ψ)=⎡⎣⎢cosψ−sinψ0sinψcosψ0001⎤⎦⎥ R z ( ψ ) = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ]

R_z(\psi)=\begin{bmatrix}\cos\psi&\sin\psi&0\\-\sin\psi&\cos\psi&0\\0&0&1\end{bmatrix}
上面这个矩阵就描述了坐标系绕Z轴的一次旋转。

2.2绕y轴旋转

绕y轴旋转的公式推导也与上面是一样的,首先 y′=y y ′ = y y\prime=y,然后向量 OA→ O A → \vec{OA} 在坐标系 OXY O X Y OXY 中的坐标可以表示如下：

{x=rsin(β+θ)z=rcos(β+θ) { x = r sin ⁡ ( β + θ ) z = r cos ⁡ ( β + θ )

\begin{cases} x=r\sin(\beta+\theta)\\ z=r\cos(\beta+\theta) \end{cases}
在坐标系 Oxy O x y Oxy 中的坐标如下：

{x′=rsinβz′=rcosβ { x ′ = r sin ⁡ β z ′ = r cos ⁡ β

\begin{cases} x\prime=r\sin\beta\\ z\prime=r\cos\beta \end{cases}
联合这两组式子可以得到：

{x′z′==xcosθ−zsinθzcosθ+xsinθ { x ′ = x cos ⁡ θ − z sin ⁡ θ z ′ = z cos ⁡ θ + x sin ⁡ θ

\begin{cases} x\prime&=&x\cos\theta-z\sin\theta\\ z\prime&=&z\cos\theta+x\sin\theta \end{cases}
并且已经知道 y′=y y ′ = y y\prime=y 了，所以把上面的式子写成矩阵的形式则是：

Rz(ψ)=⎡⎣⎢cosθ0sinθ010−sinθ0cosθ⎤⎦⎥ R z ( ψ ) = [ cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ]

R_z(\psi)=\begin{bmatrix}\cos\theta & 0 & -\sin\theta \\0 & 1 & 0 \\\sin\theta &0 &\cos\theta \end{bmatrix}
同样的，上面这个矩阵就描述了坐标系绕Y轴的一次旋转。

2.3绕x轴旋转

绕x轴旋转的公式推导如下,首先 x′=x x ′ = x x\prime=x,然后向量 OA→ O A → \vec{OA} 在坐标系 OXY O X Y OXY 中的坐标可以表示如下：

{y=rcos(β+ϕ)z=rsin(β+ϕ) { y = r cos ⁡ ( β + ϕ ) z = r sin ⁡ ( β + ϕ )

\begin{cases} y=r\cos(\beta+\phi)\\ z=r\sin(\beta+\phi) \end{cases}
在坐标系 Oxy O x y Oxy 中的坐标如下：

{y′=rcosβz′=rsinβ { y ′ = r cos ⁡ β z ′ = r sin ⁡ β

\begin{cases} y\prime=r\cos\beta\\ z\prime=r\sin\beta \end{cases}
联合这两组式子可以得到：

{y′z′==ycosϕ+zsinϕzcosϕ−ysinϕ { y ′ = y cos ⁡ ϕ + z sin ⁡ ϕ z ′ = z cos ⁡ ϕ − y sin ⁡ ϕ

\begin{cases} y\prime&=&y\cos\phi+z\sin\phi\\ z\prime&=&z\cos\phi-y\sin\phi \end{cases}
并且已经知道 x′=x x ′ = x x\prime=x 了，所以把上面的式子写成矩阵的形式则是：

Rz(ψ)=⎡⎣⎢1000cosϕ−sinϕ0sinϕcosϕ⎤⎦⎥ R z ( ψ ) = [ 1 0 0 0 cos ⁡ ϕ sin ⁡ ϕ 0 − sin ⁡ ϕ cos ⁡ ϕ ]

R_z(\psi)=\begin{bmatrix}1 & 0 & 0\\0& \cos\phi & \sin\phi \\0 & -\sin\phi &\cos\phi \end{bmatrix}
同样的，上面这个矩阵就描述了坐标系绕X轴的一次旋转。

三姿态角变化率与机体角速率的关系

有了上面的三个坐标变换矩阵之后，就能很清楚地表示坐标系之间的旋转关系了，但是问题也来了，这三个矩阵有什么用呢，好像跟飞控的姿态解算完全联系不起来。不过注意，每次的旋转过程都是会产生机体姿态角的变化，而飞行器上的陀螺仪可以直接测得机体的角速率，这两个变化率之间似乎关系很明显啊——角速率积分就能得到角度！可是，姿态角的变化率真的就等于机体的角速率吗，这可说不准了，毕竟坐标系本身就发生了改变了的，而且机体的角速率是在机体坐标系下测得的，而姿态角的变化率又是相于大地坐标系的，这两者本来就不是一个东本。下面就记录下这两者关系的推导：

如上图所示，一次完整的旋转被分成了先后三次绕坐标轴的旋转(也可以说其实只存在初始坐标系 oe1e2e3 o e 1 e 2 e 3 oe_1e_2e_3，和当前坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3 ,中间出现的两个坐标系都是为了理解方便而假想存在过的。这样说也是合理的，因为对于飞控来说，初始时的坐标系与当前坐标系是真实存在的，也就是陀螺仪相邻两次输出时机体的姿态)。要强调的一点是，在每次绕轴旋转的过程中，旋转轴对应的坐标是不变的，即 k3=e3,n2=k2,b1=n1 k 3 = e 3 , n 2 = k 2 , b 1 = n 1 k_3=e_3,n_2=k_2,b_1=n_1.在进行一次完整旋转之后的坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3中的角速率矢量 bω=[bωxbωybωz] b ω = [ b ω x b ω y b ω z ] ^b\omega=\begin{bmatrix} ^b\omega_x&^b\omega_y&^b\omega_z \end{bmatrix}可以表示成以下的形式：

bω=ωxbb1→+ωybb2→+ωzbb3→ b ω = ω x b b 1 → + ω y b b 2 → + ω z b b 3 →

^b\omega=\omega_{xb}\vec{b_1}+ \omega_{yb}\vec{b_2}+\omega_{zb}\vec{b_3}
其中 ωxb ω x b \omega_{xb} 就是当前坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3 绕其x轴即 b1 b 1 b_1 向量旋转的角速率 ψ′ ψ ′ \psi\prime
所以，

bωx=ϕ′b1→(27) (27) b ω x = ϕ ′ b 1 →

\begin{align} ^b\omega_x&=\phi\prime \vec{b_1} \end{align}
也就是，

bωx=[100]T⋅ϕ′ b ω x = [ 1 0 0 ] T ⋅ ϕ ′

^b\omega_x=\begin{bmatrix} 1&0&0\end{bmatrix}^T\cdot\phi\prime
而 ωyb ω y b \omega_{yb} 则是在上一次绕坐标 ok1k2k3 o k 1 k 2 k 3 ok_1k_2k_3 的y轴即 k2 k 2 k_2 向量旋转中的角速率 θ′ θ ′ \theta\prime在当前坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3 中的投影。到这里就需要之前所推导的坐标变换矩阵了,绕y轴的旋转对应的变换矩阵就是 Ry(θ) R y ( θ ) R_y(\theta),因此，只要对 θ′ θ ′ \theta\prime 左乘 Ry(θ) R y ( θ ) R_y(\theta) 就可以将俯仰角的变化率变换到当前坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3 中；

bωy=Ry(θ)⋅θ′n2→ b ω y = R y ( θ ) ⋅ θ ′ n 2 →

^b\omega_y= R_y(\theta)\cdot\theta\prime\vec{n_2}
也就是,

bωy=[0cosϕ−sinϕ]T⋅θ′ b ω y = [ 0 cos ⁡ ϕ − sin ⁡ ϕ ] T ⋅ θ ′

^b\omega_y=\begin{bmatrix} 0&\cos\phi&-\sin\phi \end{bmatrix}^T\cdot\theta\prime
同样的，可以说 bωz b ω z ^b\omega_z 是初始坐标系绕其z轴即 e3 e 3 e_3 向量旋转的角速率 ψ′ ψ ′ \psi\prime 在当前坐标系 ob1b2b3 o b 1 b 2 b 3 ob_1b_2b_3 中的投影，其中先后经过了 Ry(θ)−Rx(ϕ) R y ( θ ) − R x ( ϕ ) R_y(\theta)-R_x(\phi) 两次坐标变换，也就是先对 e3 e 3 e_3 向量左乘 Ry(θ) R y ( θ ) R_y(\theta) 再左乘 Rx(ϕ) R x ( ϕ ) R_x(\phi),即：

bωz=Rx(ϕ)⋅Ry(θ)⋅ψ′k3→ b ω z = R x ( ϕ ) ⋅ R y ( θ ) ⋅ ψ ′ k 3 →

^b\omega_z=R_x(\phi)\cdot R_y(\theta) \cdot\psi\prime \vec{k_3}
也就是，

bωz=[−sinθcosθsinϕcosθcosϕ]T⋅ψ′ b ω z = [ − sin ⁡ θ cos ⁡ θ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ ] T ⋅ ψ ′

^b\omega_z=\begin{bmatrix} -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix}^T\cdot \psi\prime
结合上面的式子，可以得到：

⎡⎣⎢bωxbωybωz⎤⎦⎥=⎡⎣⎢10o0cosϕ−sinϕ−sinθcosθsinϕcosθcosϕ⎤⎦⎥⋅⎡⎣⎢ϕ′θ′ψ′⎤⎦⎥ [ b ω x b ω y b ω z ] = [ 1 0 − sin ⁡ θ 0 cos ⁡ ϕ cos ⁡ θ sin ⁡ ϕ o − sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ ] ⋅ [ ϕ ′ θ ′ ψ ′ ]

\begin{bmatrix} ^b\omega_x \\^b\omega_y \\ ^b\omega_z \end{bmatrix}=\begin{bmatrix}1&0&-\sin\theta\\0&\cos\phi&\cos\theta\sin\phi\\o&-\sin\phi&\cos\theta\cos\phi \end{bmatrix}\cdot\begin{bmatrix} \phi\prime \\ \theta\prime \\ \psi\prime \end{bmatrix}
可以看出，这个式子用姿态变化率来表示角速率，离最终的目标就差一点了，只需要进行简单的将矩阵除过去，就可以得到通过角速率表示姿态变化率的式子了：

⎡⎣⎢ϕ′θ′ψ′⎤⎦⎥=⎡⎣⎢⎢100tanθsinϕcosϕsinϕcosθtanθcosϕ−sinϕcosϕcosθ⎤⎦⎥⎥⋅⎡⎣⎢bωxbωybωz⎤⎦⎥ [ ϕ ′ θ ′ ψ ′ ] = [ 1 tan ⁡ θ sin ⁡ ϕ tan ⁡ θ cos ⁡ ϕ 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ cos ⁡ θ ] ⋅ [ b ω x b ω y b ω z ]

\begin{bmatrix} \phi\prime \\ \theta\prime \\ \psi\prime \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}\cdot\begin{bmatrix} ^b\omega_x \\^b\omega_y \\ ^b\omega_z \end{bmatrix}
到这一步，离解算出机体姿态的目标基本就已经达到了，用上一篇里提到的数值计算方法就可以来求解这组微分方程了，从而得到每个时刻的姿态，并且当俯仰与横滚角比较小时，姿态变化率与机体角速率近似相等，因此，如果姿态变化不大，姿态角度可以直接用角速率积分得到。这种使用直接使用欧拉角来更新姿态的方法物理意义直观明显，但是由于需要计算较多的三角函数，因此计算量较大，而且当俯仰角 θ θ \theta 接近 ±90∘ ± 90 ∘ \pm90^\circ 时，矩阵中的 cosθ cos ⁡ θ \cos\theta 会等于0，这会导致出现奇异性问题。这是欧拉角描述旋转本身存在的一个缺陷，即万向节死锁现象，所以这种方法在飞控当中用得不多，至少我没有看到过。此外，可以发现，其实这三个欧拉角之间并不是独立的，而是相互有耦合关系的。

参考文献

高强，王力，侯远龙，童仲志，陈机林.火控系统设计概论，北京：国防工业出版社，2016
全权.多旋翼飞行器设计与控制，北京：电子工业出版社