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

对载体准确的姿态估计是实现高精度导航的前提条件。在消费级产品的导航方案中，无人机需要准确确定当前点头角与侧滚角，进而控制系统实现姿态自稳，是无人机安全行驶至关重要的条件；在高精度导航方案中，惯导系统需在静止状态下需经几分钟至几十分钟的初始对准，该过程就是计算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$$

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

R ˙ b w = R b w ( ω m − b g ) b g = 0

R˙bw​=Rbw​(ωm​−bg​)bg​=0​

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

$$\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为：

δ θ ˙ = − [ ω m − b g ] × δ θ − δ b g − n w δ b g ˙ = n g

δθ˙δbg​˙​​=−[ωm​−bg​]×​δθ−δbg​−nw​=ng​​​

离散形式为：

δ θ ← Exp [ ( ω m − b g ) Δ t ] T δ θ − δ b g Δ t + θ i δ b g ← δ b g + ω i

\\ δθδbg​​←Exp[(ωm​−bg​)Δt]Tδθ−δbg​Δt+θi​←δbg​+ωi​​

矩阵形式表示为

[ δ θ δ b g ] = [ Exp [ ( ω m − b g ) Δ t ] T − I Δ t 0 I ] ⏟ F x [ δ θ δ b g ] + [ I 0 0 I ] ⏟ F i [ θ i ω i ] \left[

\right] = \underbrace{\left[ \right]}_{\bm F_x} \left[\right] + \underbrace{ \left[ \right] }_{\bm F_i }\left[\right] \\ [δθδbg​​]=Fx​ [Exp[(ωm​−bg​)Δt]T0​−IΔtI​]​​[δθδbg​​]+Fi​ [I0​0I​]​​[θi​ωi​​]

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

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

将旋转矩阵用欧拉角表示

a m = [ g sin ⁡ ψ − g cos ⁡ ψ sin ⁡ θ g cos ⁡ θ ] {a_m} = \left[

\right] am​=⎣ ⎡​gsinψ−gcosψsinθgcosθ​⎦ ⎤​

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

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

H = [ [ I G R T g ] × 0 ] \bm H = \left[

\right] \\ H=[[IG​RTg]×​​0​]

其中， g = [ 0 0 − 1 ] T \bm g = \left[

\right]^T g=[0​0​−1​]T 为重力在导航坐标系下的方向。

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

P − = F x P F x T + Q K = P H T ( H P H T + R ) − 1 δ ξ = K ( a m − R b w T g ) P = P − − K ( H P H T + R ) K T

P−=Fx​PFxT​+QK=PHT(HPHT+R)−1δξ=K(am​−RbwT​g)P=P−−K(HPHT+R)KT​

最后，更新状态量表示为

R b w ← R b w E x p ( ω m − b g ) E x p ( δ θ ) b g ← b g + δ b g

Rbw​←Rbw​Exp(ωm​−bg​)Exp(δθ)bg​←bg​+δbg​​

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

3. 左乘扰动

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

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

推导得到error-state kinematic为：

δ θ ˙ = R b w δ b g − R b w n w δ b ˙ g = n g

δθ˙=Rbw​δbg​−Rbw​nw​δb˙g​=ng​​

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

[ δ θ δ b g ] = [ I R b w Δ t 0 I ] ⏟ F x [ δ θ δ b g ] + [ I 0 0 I ] ⏟ F i [ θ i ω i ] \left[

\right] = \underbrace{\left[ \right]}_{\bm F_x} \left[\right] + \underbrace{ \left[ \right] }_{\bm F_i }\left[\right] \\ [δθδbg​​]=Fx​ [I0​Rbw​ΔtI​]​​[δθδbg​​]+Fi​ [I0​0I​]​​[θi​ωi​​]

观测矩阵为：

H = [ I G R T g × 0 ] \bm H = \left[

\right] H=[IG​RTg×​​0​]

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

其次是观测矩阵的推导。

4. 调参Q，R矩阵

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

Q = d i a g ( σ g 2 Δ t 2    ,    1 0 − 12 ) R = b l k d i a g ( σ a 2 )

Q=diag(σg2​Δt2,10−12)R=blkdiag(σa2​)​

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 }$由本体坐标系改为定义在惯性坐标系。而**王炜鑫**博士认为将姿态误差定义到惯性坐标系后，线性化的不精确就不会对不可观测的自由度做出错误的修正，详细内容可以参考他的博客。因此在高精度的惯性导航中，常采用左乘扰动的方式进行误差传播方程的推导，在精度要求不太高的场景中，相关论文中也有对右乘扰动模型的使用。