组合导航原理剖析（三）：姿态估计与左乘/右乘扰动误差传播方程

对载体准确的姿态估计是实现高精度导航的前提条件。在消费级产品的导航方案中，无人机需要准确确定当前点头角与侧滚角，进而控制系统实现姿态自稳，是无人机安全行驶至关重要的条件；在高精度导航方案中，惯导系统需在静止状态下需经几分钟至几十分钟的初始对准，该过程就是计算IMU当前的姿态角（也叫作失准角），在长时间对准过程中，Kalman滤波器逐渐达到收敛状态。

在GNSS不参与的姿态估计时，更多依赖于IMU的自修正，主要有三种模式：

① 加速度计的重力加速度分量，用于修正IMU的点头和侧滚方向角度；

② 磁力计感受到的地球磁场方向，用于修正摇头角；

③ 高精度陀螺仪能够测量地球自转的前提下，可以用于摇头角度的初始对准；

其中①和②常见于低精度的导航方案，③见于高精度惯性导航方案，要求陀螺仪的精度高于地球自转角速度（地球自转：15°/h），②不适用于高精度导航方案，因为磁场较为容易受到外界环境的干扰，观测得到的摇头角的精度往往低于陀螺仪的理论精度。不失一般性，下面的内容仅讨论①。

1. 姿态确定方法

姿态估计常采用”error-state EKF“框架，相比于传统的EKF而言，用error state 可降低非线性系统在线性化过程中的误差，是惯性导航中最为常见的方案，其核心思想是用姿态误差的协方差矩阵来描述系统的不确定度。（参考文献：Quaternion kinematics for the error-state KF）（描述不一定准确）在详细设计导航方案时，总结有下面5个步骤：

① 确定待估计的状态量和误差状态量，确定状态更新方程；

② 根据状态更新方程，推导出误差传播方程，并得到kalman滤波的转移矩阵（离散更新方程对误差状态的雅克比矩阵）和协方差更新方程，是其中的核心；

③ 确定多传感器融合的观测模型，并对观测模型推导出残差相对于误差状态的雅克比矩阵；

④ 根据各传感器的误差大小，确定协方差矩阵Q（预测更新矩阵）和R（观测矩阵）

⑤ 确定kalman滤波的更新方程；

状态量就是IMU本体坐标系到导航坐标系下的旋转矩阵（姿态角）和陀螺仪的bias，表示为

$$\xi ={\left[{\mathbf{R}}_b^w{\mathbf{b}}_g\right]}^T$$

标称状态的更新方程表示为

$$\begin{array}{l}{\dot{\mathbf{R}}}_b^w={\mathbf{R}}_b^w\left({\omega }_m-{\mathbf{b}}_g\right)\\ {\mathbf{b}}_g=0\end{array}$$

姿态的误差状态通常用旋转矢量表示，有

$$\delta \xi ={\left[\delta \theta \delta {\mathbf{b}}_g\right]}^T$$

下面对上式进行误差扰动分析，求解误差状态的传播方程。一般有两种模式：左乘扰动和右乘扰动，本文将分别推导两种形式并实现，对比两种模式在精度表现上的差异，最后解释为何会产生这样的差异。

2. 右乘扰动

首先是右乘扰动，定义旋转误差为：（其中${\mathbf{R}}_{bt}^w$为真实旋转矩阵）

$${\mathbf{R}}_{bt}^w={\mathbf{R}}_b^{w}Exp(\delta \theta )$$

推导得到error-state kinematic为：

$$\begin{array}{rl}\dot{\delta \mathbf{\theta }}& =-[{\mathbf{\omega }}_m-{\mathbf{b}}_g{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{b}}_g-{\mathbf{n}}_w\\ \dot{\delta {\mathbf{b}}_g}& ={\mathbf{n}}_g\end{array}$$

离散形式为：

$$\begin{array}{rl}\delta \mathbf{\theta }& \leftarrow \text{Exp}[({\mathbf{\omega }}_m-{\mathbf{b}}_g)\Delta t{]}^T\delta \mathbf{\theta }-\delta b_g\Delta t+{\mathbf{\theta }}_i\\ \delta {\mathbf{b}}_g& \leftarrow \delta {\mathbf{b}}_g+{\mathbf{\omega }}_i\end{array}$$

矩阵形式表示为

$$\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta {\mathbf{b}}_g\end{array}\right]=\underset{{\mathbf{F}}_x}{\underbrace{\left[\begin{array}{cc}\text{Exp}[({\mathbf{\omega }}_m-{\mathbf{b}}_g)\Delta t{]}^T& -\mathbf{I}\Delta t\\ 0& \mathbf{I}\end{array}\right]}}\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta {\mathbf{b}}_g\end{array}\right]+\underset{{\mathbf{F}}_i}{\underbrace{\left[\begin{array}{cc}\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]}}\left[\begin{array}{c}{\mathbf{\theta }}_i\\ {\mathbf{\omega }}_i\end{array}\right]$$

当IMU运动较为缓慢或静止时，加速度计的信号为重力加速度分量在IMU本体坐标系下的投影，可用于姿态角度的观测，进而降低陀螺仪输出角速度积分时产生的漂移，这种模式被称为“单矢量测量”。可以表示为

$$a_m=\mathbf{R}{}_b^{wT}\mathbf{g}+n_a$$

将旋转矩阵用欧拉角表示

$$a_m=\left[\begin{array}{l}g\sin \psi \\ -g\cos \psi \sin \theta \\ g\cos \theta \end{array}\right]$$

欧拉角：$\theta ,\psi ,\varphi$ 。根据上式可知，重力加速度只能提供点头和侧滚方向的观测，而没有办法提供摇头角度的观测，这也是“单矢量观测”的不足之处。

计算残差相对于error state的雅克比矩阵，表示为：

$$\mathbf{H}=\left[\begin{array}{cc}[{}_I^G{\mathbf{R}}^T\mathbf{g}{]}_{\times }& 0\end{array}\right]$$

其中， $\mathbf{g}={\left[\begin{array}{ccc}0& 0& -1\end{array}\right]}^T$ 为重力在导航坐标系下的方向。

kalman滤波的更新方程表示为：

$$\begin{array}{l}{P}^{-}=F_{x}P{F}_x^T+Q\\ K=P{H}^T(HP{H}^T+R{)}^{-1}\\ \delta \xi =K(a_m-{\mathbf{R}}_b^{wT}\mathbf{g})\\ P=P^{-}-K(HP{H}^T+R)K^T\end{array}$$

最后，更新状态量表示为

$$\begin{array}{l}{\mathbf{R}}_b^w\leftarrow {\mathbf{R}}_b^w\mathrm{Exp}({\omega }_m-b_g)Exp(\delta \theta )\\ {\mathbf{b}}_g\leftarrow {\mathbf{b}}_g+\delta {\mathbf{b}}_g\end{array}$$

补充以上的推导过程，首先是error-state kinematic：

3. 左乘扰动

左乘扰动中定义旋转误差为：（其中${\mathbf{R}}_{bt}^w$为真实旋转矩阵）

$${\mathbf{R}}_{bt}^w=Exp(\delta \theta ){\mathbf{R}}_b^w$$

推导得到error-state kinematic为：

$$\begin{array}{l}\delta \dot{\theta }={\mathbf{R}}_b^w\delta {\mathbf{b}}_g-{\mathbf{R}}_b^w{n}_w\\ \delta {\dot{\mathbf{b}}}_g=n_g\end{array}$$

矩阵形式表示离散更新方程表示为：

$$\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta {\mathbf{b}}_g\end{array}\right]=\underset{{\mathbf{F}}_x}{\underbrace{\left[\begin{array}{cc}\mathbf{I}& {\mathbf{R}}_b^w\Delta t\\ 0& \mathbf{I}\end{array}\right]}}\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta {\mathbf{b}}_g\end{array}\right]+\underset{{\mathbf{F}}_i}{\underbrace{\left[\begin{array}{cc}\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]}}\left[\begin{array}{c}{\mathbf{\theta }}_i\\ {\mathbf{\omega }}_i\end{array}\right]$$

观测矩阵为：

$$\mathbf{H}=\left[\begin{array}{cc}{}_I^G{\mathbf{R}}^T{\mathbf{g}}_{\times }& 0\end{array}\right]$$

补充以上的推导过程，首先是error-state kinematic：

其次是观测矩阵的推导。

4. 调参Q，R矩阵

Q为陀螺仪相关的协方差矩阵，R为加速度计相关的协方差矩阵，记前后两数据帧的时间间隔为$\Delta t$，统计MEMS传感器在静止状态下一段时间内IMU的方差作为离散噪声的方差，记为${\sigma }_g^2$和，那么根据预测更新方程和观测方程，协方差矩阵表示为：

$$\begin{array}{l}Q=diag({\sigma }_g^2\Delta t^2,10^{-12})\\ R=blkdiag({\sigma }_a^2)\end{array}$$

5. 试验结果对比

试验采用经典的高精度MEMS传感器STIM300型号IMU，陀螺仪的零偏稳定性指标为1°/h，加速度计的零偏为1mg，通过RS422传输数据至ARM计算平台，输出帧率为1000Hz，加速度计数据作101个点矩形窗的平滑，每20ms作一次滤波更新。

IMU的初始欧拉角度为0,0,0，IMU中途经过多次摆动和振动操作，历经2个小时之后，对比如下图所示，由于直接积分用旋转矢量泰勒展开近似的模式进行计算，发现仍存在较为明显的误差，实际情况下，STIM300的漂移程度应远低于图中所呈现的那样。ESKF（采用左乘扰动的模式）在点头、侧滚角上表现较好，摇头角表现较差。摇头角的漂移程度大于直接积分的方式，这是因为单矢量观测过程中可能对摇头角进行错误的观测修正。

而右乘扰动的模式中，摇头角度的漂移更大，甚至到2-3s漂移到1°，在对航向角度缺乏有效观测的条件下，摇头角的漂移程度让人吃惊。

左乘扰动

右乘扰动

6. 原因分析

对于为什么左乘扰动中摇头角漂移更小，这是因为将误差状态$\delta \mathbf{\theta }$由本体坐标系改为定义在惯性坐标系。而**王炜鑫**博士认为将姿态误差定义到惯性坐标系后，线性化的不精确就不会对不可观测的自由度做出错误的修正，详细内容可以参考他的博客。因此在高精度的惯性导航中，常采用左乘扰动的方式进行误差传播方程的推导，在精度要求不太高的场景中，相关论文中也有对右乘扰动模型的使用。