【TIPS】PX4姿态解算基本原理（下）

陀螺仪测量绕三个轴转动的角速度，因此利用陀螺仪的输出值进行一段时间的积分可以得到一组姿态角。但是这组角度值为陀螺仪自身坐标系下的角度值，若要求得导航坐标系下的姿态角，一般利用陀螺信号通过与欧拉角、方向余弦矩阵或四元数进行微分的方法进行姿态信息的更新。

欧拉角法

假设陀螺仪在第n个时刻的航向角为$\varphi$，横滚角为$\theta$，俯仰角为$\psi$，其含义为陀螺仪从初始位置，经过绕$z$轴旋转$\varphi$角度，绕$y$轴旋转$\theta$角度，绕$x$轴旋转$\psi$角度，得到了n时刻的姿态，此时需要计算n+1时刻的姿态。设n+1时刻的航向角为$\varphi +\Delta \varphi$，横滚角为$\theta +\Delta \theta$，俯仰角为$\psi +\Delta \psi$，该姿态也是n时刻经过了三次旋转得到的，要想计算n+1时刻的姿态，只要在n时刻姿态的基础上，加上对应的姿态角度变化量即可，姿态角度的变化量可以通过角速度与采用时间周期积分即可。

$$\{\begin{array}{l}\psi (n+1)=\psi (n)+\Delta \psi =\psi (n)+\left(\frac{d\psi }{dt}\right)\Delta t\\ \\ \theta (n+1)=\theta (n)+\Delta \theta =\theta (n)+\left(\frac{d\theta }{dt}\right)\Delta t\\ \\ \varphi (n+1)=\varphi (n)+\Delta \varphi =\varphi (n)+\left(\frac{d\varphi }{dt}\right)\Delta t\end{array}$$

从n时刻到n+1时刻需要经过三次绕轴旋转，三次绕轴旋转的角速度分别为$\frac{d\psi }{dt},\frac{d\theta }{dt},\frac{d\varphi }{dt}$，姿态角是以导航坐标系为参考，而陀螺仪读出的角速度${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}^T$是以载体坐标系为参考，因此需要将读到的数据经过变换，才能用于计算姿态的变化。

绕$z$轴旋转的角速度为$\frac{d\varphi }{dt}$，三次旋转首先就是绕着$z$轴旋转，${\left[\begin{array}{ccc}0& 0& \frac{d\varphi }{dt}\end{array}\right]}^T$表示第一步绕$z$轴转动的角速度，由于之后该坐标系还要经过绕$y$轴和绕$x$轴的两次旋转，因此${\left[\begin{array}{ccc}0& 0& \frac{d\varphi }{dt}\end{array}\right]}^T$经历两次旋转后也要经历两次变换，$M_x,M_y$表示绕$x,y$轴的旋转矩阵，${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}_z^T$表示绕$z$轴的旋转角速度在两次旋转后的等效旋转角速度。

$${\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]}_z=M_x{M}_y\left[\begin{array}{c}0\\ 0\\ \frac{d\varphi }{dt}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & sin\psi \\ 0& -sin\psi & cos\psi \end{array}\right]\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{c}0\\ 0\\ \frac{d\varphi }{dt}\end{array}\right]$$

同理，绕$y$轴旋转的角速度${\left[\begin{array}{ccc}0& \frac{d\theta }{dt}& 0\end{array}\right]}^T$还要经过绕$x$轴的旋转，$M_x$表示绕$x$轴的旋转矩阵，${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}_y^T$表示绕$y$轴的旋转角速度在一次旋转后的等效旋转角速度。

$${\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]}_y=M_x\left[\begin{array}{c}0\\ \frac{d\theta }{dt}\\ 0\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & sin\psi \\ 0& -sin\psi & cos\psi \end{array}\right]\left[\begin{array}{c}0\\ \frac{d\theta }{dt}\\ 0\end{array}\right]$$

同理，绕$x$轴旋转的角速度${\left[\begin{array}{ccc}\frac{d\psi }{dt}& 0& 0\end{array}\right]}^T$不需要经过旋转，${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}_x^T$表示绕$x$轴的旋转角速度的等效旋转角速度。

$${\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]}_x=\left[\begin{array}{c}\frac{d\psi }{dt}\\ 0\\ 0\end{array}\right]$$

将绕三轴旋转的角速度经过旋转变换的等效旋转角速度进行相加，即是n+1时刻陀螺仪读出的角速度${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}^T$。

$$\begin{array}{rl}\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]& =\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & sin\psi \\ 0& -sin\psi & cos\psi \end{array}\right]\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{c}0\\ 0\\ \frac{d\varphi }{dt}\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & sin\psi \\ 0& -sin\psi & cos\psi \end{array}\right]\left[\begin{array}{c}0\\ \frac{d\theta }{dt}\\ 0\end{array}\right]+\left[\begin{array}{c}\frac{d\psi }{dt}\\ 0\\ 0\end{array}\right]\\ & =\left[\begin{array}{ccc}1& 0& -sin\theta \\ 0& cos\psi & cos\theta sin\psi \\ 0& -sin\psi & cos\theta cos\psi \end{array}\right]\left[\begin{array}{c}\frac{d\psi }{dt}\\ \frac{d\theta }{dt}\\ \frac{d\varphi }{dt}\end{array}\right]\end{array}$$

对矩阵求逆，得：

$$\left[\begin{array}{c}\frac{d\psi }{dt}\\ \frac{d\theta }{dt}\\ \frac{d\varphi }{dt}\end{array}\right]=\frac{1}{cos\theta }\left[\begin{array}{ccc}cos\theta & sin\theta sin\psi & sin\theta cos\psi \\ 0& cos\theta cos\psi & -cos\theta sin\psi \\ 0& sin\psi & cos\psi \end{array}\right]\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]$$

由上式即可用n+1时刻陀螺仪读出的角速度${\left[\begin{array}{ccc}{g}_x& g_y& g_z\end{array}\right]}^T$求出姿态角的旋转角速度，已知n时刻的姿态角，即可求出n+1时刻的姿态角。

这种使用直接使用欧拉角来更新姿态的方法物理意义直观明显，但是由于需要计算较多的三角函数，因此计算量较大，而且当俯仰角接近±90°时，矩阵中的$cos\theta$会等于0，这会导致出现奇异性问题，即万向节死锁现象。

方向余弦法

设$r_n$是导航坐标系中的矢径，$r_b$是载体坐标系中的矢径，${\omega }_n$是以导航坐标系为参考的载体坐标系旋转的角速度矩阵，由科氏定理有：

$$\frac{d{r}_n}{dt}=\frac{d{r}_b}{dt}+{\omega }_n\times r_n$$

在载体坐标系内，矢径$r_b$的大小和方向是固定不变的，因此：

$$\frac{d{r}_b}{dt}=0$$

从而有：

$$\frac{d{r}_n}{dt}={\omega }_n\times r_n$$

陀螺仪测得的角速度是以载体坐标系为参考的旋转角速度矩阵${\omega }_b={\left[\begin{array}{ccc}{\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]}^T$，所以将等式两边都变换到载体坐标系中，即：

$$\frac{d(R_b^n{r}_b)}{dt}=R_b^n({\omega }_b\times r_b)$$

$$\frac{d{R}_b^n}{dt}{r}_b+R_b^n\frac{d{r}_b}{dt}=R_b^n({\omega }_b\times )r_b$$

将$\frac{d{r}_b}{dt}=0$代入，$({\omega }_b\times )$为旋转角速度矩阵的反对称矩阵，记作${\Omega }_b$，有：

$$\frac{d{R}_b^n}{dt}{r}_b=R_b^n{\Omega }_b{r}_b$$

最终得到旋转矩阵变化率与载体旋转角速率之间的关系：

$$\frac{d{R}_b^n}{dt}=R_b^n{\Omega }_b$$

式中：

$${\Omega }_b=({\omega }_b\times )=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

对这个时变系数齐次微分方程可以利用毕卡逼近法求解，可得：

$$R_b^n(t)=R_b^n(0)+{\int }_0^t{R}_b^n(\tau ){\Omega }_b(\tau )d\tau$$
$$R_b^n(t)=R_b^n(0)[I+{\int }_0^t{\Omega }_b(\tau )d\tau +{\int }_0^t{\int }_0^{\tau }{\Omega }_b({\tau }_1)d{\tau }_1{\Omega }_b(\tau )d\tau +{\int }_0^t{\int }_0^{\tau }{\int }_0^{{\tau }_1}{\Omega }_b({\tau }_2)d{\tau }_2{\Omega }_b({\tau }_1)d{\tau }_1{\Omega }_b(\tau )d\tau +\cdots ]$$

级数收敛，若积分时段内${\omega }_b$的方向保持不变可得闭合解：

$$R_b^n(t)=R_b^n(0)exp\left({\int }_0^t{\Omega }_b(\tau )d\tau \right)$$

进一步进行泰勒展开，得：

$$\begin{array}{rl}exp(\Delta \theta \times )& =I+(\Delta \theta \times )+\frac{(\Delta \theta \times {)}^2}{2!}+\frac{(\Delta \theta \times {)}^3}{3!}+\cdots \\ & =I+[1-\frac{\Delta {\theta }^2}{3!}+\frac{\Delta {\theta }^4}{5!}-\cdots ](\Delta \theta \times )+[\frac{1}{2}-\frac{\Delta {\theta }^2}{4!}+\frac{\Delta {\theta }^4}{6!}-\cdots ](\Delta \theta \times {)}^2\\ & =I+\frac{sin\Delta \theta }{\Delta \theta }(\Delta \theta \times )+\frac{1-cos\Delta \theta }{\Delta {\theta }^2}(\Delta \theta \times {)}^2\end{array}$$

若等式$(\Delta {\theta }_0\times )={\int }_t^{t+\Delta t}{\Omega }_b(\tau )d\tau$成立，则微分方程的精确解为：

$$R_b^n(t+\Delta t)=R_b^n(t)[I+\frac{sin\Delta {\theta }_0}{\Delta {\theta }_0}(\Delta {\theta }_0\times )+\frac{1-cos\Delta {\theta }_0}{\Delta {\theta }_0^2}(\Delta {\theta }_0\times {)}^2]$$

式中：

$$\begin{gathered} [\Delta {\theta }_0\times ]=\left[\begin{array}{ccc}0& -\Delta {\theta }_z& \Delta {\theta }_y\\ \Delta {\theta }_z& 0& -\Delta {\theta }_x\\ -\Delta {\theta }_y& \Delta {\theta }_x& 0\end{array}\right] \\ \Delta {\theta }_0^2=(\Delta {\theta }_x{)}^2+(\Delta {\theta }_y{)}^2+(\Delta {\theta }_z{)}^2 \end{gathered}$$

运用方向余弦法进行姿态解算，避免了欧拉角法中的万向节死锁问题，可全姿态工作。但是解算矩阵微分方程时实质上是解算九个线性微分方程，计算量大，实时计算困难，所以工程上并不实用。

四元数法

四元数$Q$的定义为：

$$Q=q_0+q_1\vec{i}+q_2\vec{j}+q_3\vec{k}$$

式中$q_0,q_1,q_2,q_3$是实数，$\vec{i},\vec{j},\vec{k}$是相互正交的单位向量。

定义四元数的范数和模分别为：

$$\begin{gathered} \parallel Q\parallel =q_0^2+q_1^2+q_2^2+q_3^2 \\ |Q|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2} \end{gathered}$$

若$\parallel Q\parallel =1$，则称$Q$为规范化四元数。

设$P=p_0+p_1\vec{i}+p_2\vec{j}+p_3\vec{k}$，则两个四元数相乘的表达式为：

$$P\otimes Q=r_0+r_1\vec{i}+r_2\vec{j}+r_3\vec{k}$$

将上式写成矩阵形式，有:

$$\left[\begin{array}{c}{r}_0\\ r_1\\ r_2\\ r_3\end{array}\right]=\left[\begin{array}{cccc}{p}_0& -p_1& -p_2& -p_3\\ p_1& p_0& -p_3& p_2\\ p_2& p_3& p_0& -p_1\\ p_3& -p_2& p_1& p_0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

四元数乘法满足分配率和结合律，不满足交换律。

四元数本身在物理意义上代表一个转动，又可以看成一个算子。一个坐标系相对另一个坐标系的相对转动可以用四元数唯一地表示出来，用四元数来描述载体坐标系相对导航坐标系的转动时，可得姿态矩阵的四元数表达式为：

$$R_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

可得出四元数与欧拉角的关系为：

$$\{\begin{array}{l}\varphi =arctan\frac{2(q_2{q}_3+q_0{q}_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\\ \\ \theta =-arcsin[2(q_1{q}_3-q_0{q}_2)]\\ \\ \psi =arctan\frac{2(q_1{q}_2+q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\end{array}$$

$$\{\begin{array}{l}{q}_0=cos\frac{\varphi }{2}cos\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\varphi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}\\ \\ q_1=sin\frac{\varphi }{2}cos\frac{\theta }{2}cos\frac{\psi }{2}-cos\frac{\varphi }{2}sin\frac{\theta }{2}sin\frac{\psi }{2}\\ \\ q_2=cos\frac{\varphi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}+sin\frac{\varphi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}\\ \\ q_3=cos\frac{\varphi }{2}cos\frac{\theta }{2}sin\frac{\psi }{2}+sin\frac{\varphi }{2}sin\frac{\theta }{2}cos\frac{\psi }{2}\end{array}$$

四元数的微分方程为：

$$\dot{Q}=\frac{1}{2}Q\otimes {\omega }_b=\frac{1}{2}M({\omega }_b)Q$$

式中：

$$M({\omega }_b)=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$

把四元数微分方程展开成矩阵形式，得：

$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

由上式可求得姿态四元数$Q$，将其代入姿态矩阵解析式可求姿态矩阵$R_b^n$，进而求得姿态角。

对于一阶常微分方程$\{\begin{array}{l}{y}^{{}^{\prime }}=f(x,y)\\ y(x_0)=y_0\end{array}$，采用四阶龙格-库塔法求解，其数值解表达式为：

$$\{\begin{array}{l}{y}_{n+1}=y_n+\frac{h}{6}(K_1+2K_2+2K_3+K_4)\\ K_1=f(x_n,y_n)\\ K_2=f(x_n+\frac{h}{2},y_n+\frac{h}{2}{K}_1)\\ K_3=f(x_n+\frac{h}{2},y_n+\frac{h}{2}{K}_2)\\ K_4=f(x_n+h,y_n+h{K}_3)\end{array}$$

对于四元数微分方程，应用上述公式，得到数值解表达式为：

$$\{\begin{array}{l}Q(t_{n+1})=Q(t_n)+\frac{h}{6}(K_1+2K_2+2K_3+K_4)\\ K_1=\frac{1}{2}M[{\omega }_b(t_n)]Q(t_n)\\ K_2=\frac{1}{2}M[{\omega }_b(t_n+\frac{h}{2})][Q(t_n)+\frac{K_1}{2}h]\\ K_3=\frac{1}{2}M[{\omega }_b(t_n+\frac{h}{2})][Q(t_n)+\frac{K_2}{2}h]\\ K_4=\frac{1}{2}M[{\omega }_b(t_n+h)][Q(t_n)+K_{3}h]\end{array}$$

式中：

$$h=t_{n+1}-t_n$$

四元数的初值由惯导的初始对准确定。

$$\{\begin{array}{l}{R}_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\\ q_0^2+q_1^2+q_2^2+q_3^2=1\end{array}$$

可得四元数初值$q_0,q_1,q_2,q_3$的大小为：

$$\{\begin{array}{l}|q_0|=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\\ |q_1|=\frac{1}{2}\sqrt{1+r_{11}-r_{22}-r_{33}}\\ |q_2|=\frac{1}{2}\sqrt{1-r_{11}+r_{22}-r_{33}}\\ |q_3|=\frac{1}{2}\sqrt{1-r_{11}-r_{22}+r_{33}}\end{array}$$

根据$\{\begin{array}{l}4q_0{q}_1=r_{32}-r_{23}\\ 4q_0{q}_2=r_{13}-r_{31}\\ 4q_0{q}_3=r_{21}-r_{12}\end{array}$，$q_0,q_1,q_2,q_3$的符号可由下式确定：

$$\{\begin{array}{l}sign(q_1)=sign(q_0)sign(r_{32}-r_{23})\\ sign(q_2)=sign(q_0)sign(r_{13}-r_{31})\\ sign(q_3)=sign(q_0)sign(r_{21}-r_{12})\end{array}$$

描述刚体旋转的四元数为规范化四元数，但由于计算误差等各种因素的影响，计算过程中四元数会逐渐失去规范化特征，所以需要对四元数进行规范化处理。

$$q_i=\frac{{\hat{q}}_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}$$

式中：$i=0,1,2,3$，${\hat{q}}_0,{\hat{q}}_1,{\hat{q}}_2,{\hat{q}}_3$是四元数更新所得的值。

四元数法只需求解四个未知量的线性微分方程组，在进行数值积分时只需要进行加减法和乘法运算，计算量比欧拉角法和方向余弦法都小，且算法简单，易于操作，是较实用的工程方法。但四元数法对有限转动引起的不可交换误差的补偿程度不够,所以只适用于低动态运载体如运输机等的姿态解算；而对高动态运载体，姿态解算时的算法漂移会十分严重。