4. 惯性导航基础（三）：捷联惯导解算算法

好，咱们继续往下聊。前面两章我们把惯性导航的传感器原理和坐标系折腾清楚了，这一章终于要动真格的了——捷联惯导解算。

说白了，就是拿着陀螺和加速度计的数据，算出载体当前的位置、速度和姿态。嗯，听着简单，但里面坑不少。我当年刚入行时，觉得不就是积分嘛，结果第一次跑出来的轨迹直接飞到了太平洋上空……后来才知道，圆锥误差和划船误差这两个魔鬼藏在细节里。

姿态更新是捷联惯导的核心。你想想看，陀螺输出的是角速度，我们要把它变成姿态角或者姿态矩阵。

我个人习惯用四元数来做姿态更新。为什么？因为它没有欧拉角的万向锁问题，计算量也比方向余弦矩阵小。

四元数更新的核心公式长这样：

q_k+1 = q_k ⊗ exp(Δθ/2)

其中 Δθ 是角增量，⊗ 表示四元数乘法。这里有个关键点——角增量不是简单的 ω·Δt。为什么？因为陀螺采样期间，角速度是变化的。你想想看，如果载体在剧烈振动，角速度方向一直在变，直接积分会出大问题。

重要概念：姿态更新必须考虑角速度的不可交换性误差。这就是圆锥误差的根源。

圆锥误差，英文叫 Coning Error。我在项目中遇到过最头疼的问题就是这个。

想象一个场景：载体在做一个圆锥运动——绕两个轴同时做正弦振荡。这时候，陀螺输出的角速度看起来是零均值，但实际姿态却在慢慢漂移。为什么会这样？

因为旋转不可交换。你想想看，先绕X轴转一点，再绕Y轴转一点，和反过来转，结果不一样。而我们的陀螺采样是离散的，每次只能得到一个平均角速度，这个平均过程丢失了旋转顺序信息。

补偿圆锥误差，业界常用的方法是多子样算法：

Δθ_comp = Δθ + 1/12 * (Δθ_1 × Δθ_2)

这是二子样补偿。三子样、四子样精度更高，但计算量也更大。我个人建议，在车载场景下，二子样就够了。但在航空场景，我见过用六子样的。

我的经验：圆锥误差补偿的系数不是随便取的。不同运动场景，最优系数不同。我曾经在无人机上试过，把系数调偏了10%，姿态漂移直接翻倍。

速度更新，核心是比力方程：

v̇ = C_b^n · f_b - (2ω_ie^n + ω_en^n) × v + g^n

看着复杂，其实意思很简单：加速度计测到的比力，先转到导航系，然后扣除哥氏加速度和重力，剩下的就是真正的运动加速度。

但这里有个坑——划船误差。英文叫 Sculling Error。

划船误差和圆锥误差是对偶的。圆锥误差是角运动导致的姿态误差，划船误差是线运动和角运动耦合导致的速度误差。

想象你在划船：船体在上下颠簸（线运动），同时也在左右摇摆（角运动）。这时候，加速度计测到的比力里，有一个虚假的加速度分量。如果不补偿，速度积分会越偏越远。

注意：划船误差补偿和圆锥误差补偿必须同时做。只做一个，另一个会放大。我见过有人只做了圆锥补偿，结果速度误差比不做还大。

划船误差的补偿公式（二子样）：

Δv_comp = Δv + 1/2 * (Δθ_1 × Δv_2 + Δv_1 × Δθ_2)

嗯，这里要注意，Δθ 和 Δv 必须是同一时间段的。不同步的话，补偿效果会大打折扣。

位置更新相对简单，就是速度积分：

p_k+1 = p_k + v_k · Δt + 1/2 · a_k · Δt²

但如果你用的是经纬度表示位置，那就要考虑地球曲率了。纬度变化会影响子午圈半径，经度变化还和纬度余弦有关。

我个人习惯用地心地固系（ECEF）来做位置更新，最后再转成经纬高。这样中间计算不用频繁处理曲率半径，代码更干净。

下面这张图，是我自己梳理的捷联惯导解算的知识结构。你看一眼，心里就有谱了。

最后，分享几个我在工程中踩过的坑：

采样率不够：我曾经在一个项目里用了100Hz的陀螺，结果圆锥误差补偿效果很差。后来换到200Hz，问题解决了。高频振动场景下，采样率至少要是振动频率的10倍。
时间戳对齐：陀螺和加速度计的时间戳必须严格对齐。差1毫秒，补偿效果就大打折扣。我习惯在驱动层就做硬件时间戳同步。
初始化姿态：捷联惯导需要初始姿态。如果初始姿态不准，后面再怎么补偿也没用。我一般会用加速度计和磁力计做初始对准，或者用高精地图的绝对位置信息辅助。
数值精度：四元数更新后要归一化。我见过有人忘了这步，跑了10分钟，四元数模长变成了1.2，姿态全乱套了。

避坑指南：我曾经在一个车载项目中，发现速度误差越来越大。排查了三天，最后发现是划船补偿的Δθ和Δv时间窗口没对齐。嗯，从那以后，我每次写补偿代码，第一件事就是检查时间同步。

好了，这一章的内容就到这儿。捷联惯导解算，说白了就是三个积分——姿态积分、速度积分、位置积分。但每个积分里都有魔鬼，圆锥误差和划船误差就是那两个最狡猾的。理解了它们，你的惯导系统才算真正入门。