飞行器控制笔记（三）——姿态解算之方向余弦更新姿态

飞行器控制笔记（三）——姿态解算之方向余弦更新姿态

0向量叉乘的性质

对于向量 $m=\begin{bmatrix} b_1&b_2&b_3 \end{bmatrix}$ , 和向量 $\vec n=\begin{bmatrix} k_1&k_2&k_3 \end{bmatrix}$ ,它们之间的叉乘 $m \times n=[m]_{\times}\cdot n$ ,其中,

[m]_{\times} = [\begin{matrix} 0 & - b_{3} & b_{2} \\ b_{3} & 0 & - b_{1} \\ - b_{2} & b_{1} & 0 \end{matrix}]

$[m]_{\times}= \begin{bmatrix} 0&-b_3&b_2 \\b_3&0&-b_1\\-b_2&b_1&0 \end{bmatrix}$

一旋转矩阵

我上一篇博客记录了当前的机体坐标可以通过对大地坐标进行三次绕轴的基本旋转得到，用坐标变换矩阵的形式可以表示为 $R_e^b=R_x(\phi)R_y(\theta)R_z(\psi)$ ,这表示了从大地坐标到机体坐标的变化，而姿态解决是通过机体坐标的变化来计算机体坐标相对于大地坐标的状态，因此需要求得 $R_b^e$ ,也就是对应地从机体坐标系按 $先旋转-\phi再旋转-\theta最后旋转-\psi$ 从而转换到大地坐标，即

\begin{aligned} (19) & R_{b}^{e} & = R_{z} (- ψ) R_{y} (- θ) R_{x} (- ϕ) \\ (20) & = [\begin{matrix} \cos ψ & - \sin ψ & 0 \\ \sin ψ & \cos ϕ & 0 \\ 0 & 0 & 1 \end{matrix}] \cdot [\begin{matrix} \cos θ & 0 & \sin θ \\ 0 & 1 & 0 \\ - \sin θ & 0 & \cos θ \end{matrix}] \cdot [\begin{matrix} 1 & 0 & 0 \\ 0 & \cos ϕ & \sin ϕ \\ 0 & - \sin ϕ & \cos ϕ \end{matrix}] \\ (21) & = [\begin{matrix} \cos θ \cos ψ & \cos ϕ \sin θ \sin ϕ - \sin ψ \cos ϕ & \cos ψ \sin θ \cos ϕ + \sin ψ \sin ϕ \\ \cos θ \sin ψ & \sin ψ \sin θ \sin ϕ + \cos ψ \cos ϕ & \sin ψ \sin θ \cos ϕ - \cos ψ \sin ϕ \\ - \sin θ & \sin ϕ \cos θ & \cos ϕ \cos θ \end{matrix}] \end{aligned}

$\begin{align} R_b^e&=R_z(-\psi)R_y(-\theta)R_x(-\phi)\\ &=\begin{bmatrix} \cos\psi & -\sin\psi & 0\\ \sin\psi & \cos\phi &0 \\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \cdot \begin{bmatrix} 1&0&0 \\ 0& \cos\phi & \sin\phi \\ 0& -\sin\phi &\cos\phi \end{bmatrix}\\ &=\begin{bmatrix} \cos\theta\cos\psi & \cos\phi\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} \end{align}$
上面这个就是从机体到大地的旋转矩阵，有的书上也叫姿态矩阵，因为可以通过这个矩阵求出欧拉角，因此，先把

R_{b}^{e}

$R_b^e$ 写成下面的形式：

R_{b}^{e} = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}]

$R_b^e=\begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33}\end{bmatrix}$
这两种形式相对比可以得到：

{\begin{cases} ψ & = \arctan \frac{r_{21}}{r_{11}} \\ θ & = \arcsin - r_{31} \\ ϕ & = \arctan \frac{r_{32}}{r_{33}} \end{cases}

$\begin{cases} \psi&=\arctan \frac{r_{21}}{r_{11}}\\ \theta&=\arcsin-r_{31} \\ \phi&=\arctan\frac{r_{32}}{r_{33}} \end{cases}$
从上面这个式子可以看出，要想求得机体的姿态，需要先推导出旋转矩阵与机体角速率之间的关系，最后求解一组微分方程得到旋转矩阵，再通过上式转换为欧拉角。

二旋转矩阵与机体角速率之间的关系

感觉其实求解机体姿态的过程就是在讨论机体动参考系相对了于大地定参考系的运动，回忆下大二学的理论力学，这个运动是叫做牵连运动的，而由于牵连运动会在动参考系中产生科氏加速度。科氏加速度对于我这飞机的飞行应该是没多大的影响，但与之相关的科氏定理则可以将旋转矩阵与机体角速率联系起来。
科氏定理如下：

\frac{d^{e} r}{d t} = \frac{d^{b} r}{d t} +^{e} ω \times^{e} r

$\frac{d^er}{dt}=\frac{d^br}{dt}+^e\omega\times^er$
其中上标

e

$e$ 是指向量在大地坐标系中的投影，上标

b

$b$ 是指向量在机体坐标系中的投影。如果只考虑机体坐标系的转动，那么在机体坐标系中矢径

^{b} r

$^br$ 相对于机体的方向与大小都将是恒定的，因此

\frac{d^{b} r}{d t} = 0

$\frac{d^br}{dt}=0$ 从而有

\frac{d^{e} r}{d t} =^{e} ω \times^{e} r

$\frac{d^er}{dt}=^e\omega\times^er$ 而陀螺仪所测得的是在机体坐标系下的角速率

^{b} ω

$^b\omega$ ,因而为了方便地实际应用，很自然地就想到将这个式子的左右两边都变换到机体坐标系中，即：

\begin{aligned} (4) & \frac{d [R_{b}^{e} \cdot^{b} r]}{d t} & = R_{b}^{e} \cdot^{b} ω \times R_{b}^{e} \cdot^{b} r \\ (5) & R_{b}^{e}' \cdot^{b} r + R_{b}^{e} \cdot \frac{d^{b} r}{d t} & = R_{b}^{e} \cdot (^{b} ω \times^{b} r) \\ (6) & R_{b}^{e}' \cdot^{b} r & = R_{e}^{b} \cdot [^{b} ω]_{\times} \cdot^{b} r \end{aligned}

$\begin{align} \frac{d[R_b^e\cdot^br]}{dt}&=R_b^e\cdot ^b\omega\times R_b^e\cdot^br\\ R_b^e\prime\cdot^br+R_b^e\cdot \frac{d^br}{dt} &= R_b^e\cdot(^b\omega\times^br)\\ R_b^e\prime\cdot^br &=R_e^b\cdot[^b\omega]_{\times}\cdot^br \end{align}$
最终得到旋转矩阵变化率与机体角速率之间的关系：

R_{b}^{e}' = R_{b}^{e} \cdot [^{b} ω]_{\times}

$R_b^e\prime=R_b^e\cdot[^b\omega]_{\times}$
很完美，这又是一组简单的一阶微分方程，初始时刻

R_{b}^{e} = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}]

$R_b^e=\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}$ ,通过使用笔记一当中的数值计算方法，很容易就能解出来，进而得到姿态角。这种方法的好处在于更新旋转矩阵的过程中不会出现万向节死锁的现象，但是要计算的参数比较多，一共有九个未知量，计算量较大，但是好像APM飞控解算姿态用的就是这种方法，国内的开源飞控以及某宝上卖的姿态传感器模块基本都是用的四元数法更新姿态。

参考文献

全权，多旋翼飞行器设计与控制.北京：电子工业出版社，2018
秦永元，惯性导航.北京：科学出版社，2006