4、惯性导航原理（下）：捷联惯导解算算法

各位同学，欢迎来到第四章的下半场。上一节我们把惯性导航的误差模型和初始对准讲透了，这一节咱们来啃硬骨头——捷联惯导解算算法。

说白了，就是怎么用陀螺和加速度计的数据，实时算出载体的姿态、速度和位置。嗯，这里要注意，捷联惯导和平台惯导最大的区别就是：没有物理平台，全靠数学计算来“虚拟”一个导航坐标系。

我个人习惯把捷联解算分成三块：姿态更新、速度更新、位置更新。这三块是串在一起的，姿态算错了，速度和位置全完蛋。所以咱们一个一个来拆解。

4.1 姿态更新算法

姿态更新是捷联惯导的核心中的核心。为什么？因为加速度计测的是比力，要投影到导航坐标系才能积分求速度，而投影矩阵就是姿态矩阵。姿态一偏，整个导航就歪了。

常用的姿态更新方法有三种：欧拉角法、方向余弦法、四元数法。我直接告诉你结论：工程上99%的情况用四元数法。为什么？欧拉角有万向锁问题，方向余弦法计算量太大，四元数既没奇异性，计算又轻量。

四元数的微分方程长这样：

dq/dt = 0.5 * q ⊗ ω

其中ω是载体坐标系下的角速度。实际解算时，我们用的是毕卡逼近法或者龙格-库塔法。我个人偏爱毕卡逼近，因为它对高动态场景的适应性更好。

q(k+1) = [cos(Δθ/2) * I + (sin(Δθ/2)/Δθ) * Ω] * q(k)

4.2 圆锥误差补偿

说到姿态更新，就不得不提圆锥误差。我在项目中遇到过好几次，明明陀螺数据看起来没问题，但姿态就是慢慢漂移。后来排查发现，是圆锥误差没补偿。

为什么会这样？你想想看，当载体做圆锥运动时，角速度矢量在空间中以圆锥轨迹旋转。如果只用简单的角增量累加，会丢失一部分旋转信息。说白了，就是有限转动不可交换性带来的误差。

补偿方法也很直接——用多子样算法。我常用的三子样算法：

Δθ_comp = Δθ1 + Δθ2 + Δθ3 + (1/3) * (Δθ1 × Δθ3) + (2/3) * (Δθ2 × (Δθ1 + Δθ3))

嗯，这里要注意，子样数越多，补偿精度越高，但计算量也越大。一般工程上2~3子样就够用了。

4.3 速度更新算法

速度更新相对简单一些，但坑也不少。核心方程是比力方程：

dv/dt = C_b^n * f_b - (2ω_ie + ω_en) × v + g

这里面有三个关键项：比力投影、哥氏加速度、重力。实际解算时，我们通常用梯形积分法或龙格-库塔法。

我个人习惯用梯形积分，因为它简单直观，而且对中低动态场景精度足够。代码实现大概这样：

v(k+1) = v(k) + 0.5 * (a(k) + a(k+1)) * dt

但要注意，这里的a(k)和a(k+1)必须是导航坐标系下的加速度。也就是说，加速度计测的比力要先经过姿态矩阵投影，再扣除哥氏项和重力项。

4.4 划船误差补偿

划船误差和圆锥误差是“一对兄弟”。圆锥误差影响姿态，划船误差影响速度。为什么叫划船？因为当载体做类似划船的运动时（线振动+角振动耦合），速度积分会产生累积误差。

我记得第一次接触划船误差是在一个船载惯导项目里。船在波浪中上下颠簸，速度解算结果居然比实际大了好几倍。后来一查，是划船误差在作怪。

补偿方法同样用多子样算法。速度的划船补偿项：

Δv_scull = 0.5 * (Δθ1 × Δv2 + Δv1 × Δθ2)

其中Δθ是角增量，Δv是速度增量。如果是三子样，公式会更复杂一些，但原理一样——用交叉项补偿耦合误差。

4.5 位置更新算法

位置更新是最直观的。有了速度，积分就是位置。但这里有个细节——地球曲率。如果载体运动范围很大，必须考虑地球是圆的。

位置更新通常用经纬高表示：

L(k+1) = L(k) + v_N * dt / (R_M + h)
λ(k+1) = λ(k) + v_E * dt / ((R_N + h) * cos(L))
h(k+1) = h(k) + v_U * dt

其中R_M是子午圈曲率半径，R_N是卯酉圈曲率半径。这两个参数随纬度变化，高精度导航时必须实时计算。

嗯，这里要注意，如果只是短时间导航（几分钟），可以忽略地球曲率，直接用平面近似。但超过10分钟，就必须考虑曲率了。

4.6 整体解算流程

最后，我把整个捷联解算的流程串起来。下面这张图是我自己画的，你一看就明白：

流程很清晰：陀螺数据先做圆锥补偿，更新姿态；加速度数据结合姿态矩阵投影，再做划船补偿，更新速度；最后用速度更新位置。每一步都环环相扣。

好了，这一节的内容就到这儿。捷联解算算法是惯性导航的“心脏”，你把它搞透了，后面学组合导航就轻松多了。下一节咱们开始讲卡尔曼滤波——嗯，先卖个关子，到时候见。

公众号:蓝海资料掘金营，微信deep3321