3. 惯性导航解算（INS）：姿态更新、速度更新、位置更新与圆锥/划桨效应补偿
各位同学，欢迎来到惯性导航解算这一章。说实话，INS 是整个融合定位的“发动机”。你前面标定得再好，IMU 数据再干净，最后能不能算出靠谱的位置和姿态，全看这一块的算法功底。
我个人习惯把 INS 解算拆成三块：姿态更新、速度更新、位置更新。但光有这三块还不够，实际项目中你会发现，如果直接拿原始角速度和加速度去积分，结果会飘得离谱。为什么？因为有两个“隐形杀手”——圆锥效应和划桨效应。今天咱们就把这些讲透。
核心逻辑：INS 解算的本质是“积分”。角速度积分得姿态，加速度积分得速度，速度积分得位置。但积分方式不同，精度天差地别。
3.1 姿态更新：四元数与旋转矩阵
姿态更新是 INS 里最核心、也最容易出错的一环。说白了，就是利用陀螺仪测到的角速度，不断更新载体相对于导航系的朝向。
我早期做项目时，一直用欧拉角更新姿态。结果有一次在车上做测试，车在坡道上稍微颠了一下，欧拉角直接“万向锁”了，姿态瞬间乱跳。从那以后，我坚决只用四元数或旋转矩阵来做姿态更新。
3.1.1 四元数更新法
四元数更新，本质上是在解这个微分方程：
dq/dt = 0.5 * q ⊗ ω其中 ω 是角速度的四元数形式（实部为0）。实际离散化时，我们常用一阶或二阶龙格-库塔法。我个人偏爱用二阶龙格-库塔，精度够用，计算量也不大。
小技巧：四元数更新后一定要归一化！我见过太多人忘了这步，结果姿态越算越歪。归一化很简单：q = q / ||q||。
3.1.2 旋转矩阵更新法
旋转矩阵更新，用的是方向余弦矩阵（DCM）的微分方程：
dC/dt = C · [ω]×其中 [ω]× 是角速度的反对称矩阵。这个方法的好处是直观，但计算量比四元数大。我在实际项目中，一般只在需要频繁做向量旋转时才用旋转矩阵，平时都用四元数。
注意：旋转矩阵更新后，要检查正交性。如果矩阵不再正交，姿态会慢慢漂移。可以用 C = C · (C^T · C)^{-1/2} 来修正。
3.2 速度更新：比力方程与重力补偿
速度更新，说白了就是加速度计测到的“比力”去掉重力，再积分。公式长这样：
dv/dt = C · a_imu - g + 2·ω_ie × v这里 C 是当前姿态矩阵，a_imu 是加速度计读数，g 是重力矢量，最后一项是哥里奥利力（地球自转引起的）。
嗯，这里要注意：加速度计测的是“比力”，不是真正的加速度。它测的是载体受到的除了重力以外的合力。所以必须先把重力分量去掉，才能得到真正的运动加速度。
我在项目中遇到过一个问题：车停在坡道上，速度更新结果却显示在缓慢移动。排查了半天，发现是重力补偿时用的姿态不准，导致重力分量没完全去掉。后来我改用双天线RTK辅助初始化姿态，这个问题就解决了。
3.3 位置更新：速度积分与地球曲率
位置更新最简单，就是速度对时间积分：
p_new = p_old + v · dt但如果你做的是全球导航，还得考虑地球曲率。说白了，就是位置更新要在大地坐标系（比如经纬高）下做，而不是在笛卡尔坐标系下直接加。
举个例子：你在北纬45度向东走100米，经度变化和你在赤道上走100米是不一样的。因为纬度越高，经度圈越小。这个修正公式在几乎所有INS教材里都有，我就不重复了。
经验之谈：位置更新虽然简单，但误差会随时间平方增长。所以纯INS的位置输出，几分钟内还能看，超过10分钟基本就废了。这也是为什么一定要融合其他传感器（比如GPS、高精地图）的原因。
3.4 圆锥效应与划桨效应补偿
这两个效应，是INS解算里最容易忽略、但影响最大的两个坑。
3.4.1 圆锥效应
圆锥效应发生在角速度积分过程中。想象一下：你的载体在做一个圆锥运动（比如直升机旋翼的挥舞），陀螺仪测到的角速度是变化的。如果你直接用简单的梯形法积分，算出来的姿态会慢慢偏离真实值。
为什么会这样？因为角速度是矢量，而旋转不是简单的矢量加法。两个不同轴的旋转叠加，结果不等于各自旋转的矢量和。这个现象在数学上叫“旋转的非交换性误差”。
补偿方法有很多，最常用的是多子样算法。比如用两个相邻时刻的角速度采样，做一个二阶补偿：
Δθ_comp = Δθ_1 + Δθ_2 + (1/3) · (Δθ_1 × Δθ_2)这个叉积项就是圆锥补偿项。采样率越高，补偿效果越好。我一般建议用200Hz以上的陀螺仪数据来做圆锥补偿。
避坑指南：我曾经在一个项目中，陀螺仪采样率只有100Hz，圆锥补偿效果很差。后来发现是IMU的安装位置离载体振动中心太远，导致角速度变化剧烈。把IMU往质心位置挪了挪，问题就缓解了。
3.4.2 划桨效应
划桨效应是加速度积分中的类似问题。想象你坐在船上划桨：桨入水时船身会有一个俯仰振动，同时桨的推力方向也在变化。如果你直接积分加速度，算出来的速度会包含一个虚假的“漂移”。
划桨效应的补偿公式和圆锥效应很像：
Δv_comp = Δv_1 + Δv_2 + (1/2) · (Δθ_1 × Δv_2 + Δθ_2 × Δv_1)注意这里交叉的是角速度增量和速度增量。物理意义是：当载体同时存在旋转和线加速度时，旋转会改变加速度的方向，导致积分误差。
重要提醒：圆锥效应和划桨效应补偿，必须同时做！只补偿一个，另一个照样会毁掉你的导航精度。我在早期做无人机INS时，只做了圆锥补偿没做划桨补偿，结果悬停时位置漂移得厉害。后来两个都加上，效果立竿见影。
3.5 实际项目中的INS解算流程
好了，理论讲完了。我给大家总结一下实际项目中INS解算的完整流程：
- 读取IMU数据：陀螺仪角速度 ω，加速度计比力 a
- 圆锥/划桨补偿：用多子样算法计算补偿后的 Δθ 和 Δv
- 姿态更新：用补偿后的 Δθ 更新四元数或旋转矩阵
- 速度更新：用当前姿态将比力转换到导航系，去掉重力，加上哥里奥利力
- 位置更新：用更新后的速度积分得到新位置
- 归一化与正交化：确保四元数归一化，旋转矩阵正交
这个流程看起来简单，但每一步都有细节。我建议初学者先用仿真数据跑一遍，看看每一步的输出是否合理。比如姿态更新后，四元数的模长应该始终为1；速度更新后，静止时的速度应该为0。
最后说一句：INS解算是一门“细节决定成败”的学问。圆锥补偿的系数差一点，姿态可能就偏一度；重力补偿的精度差一点，速度可能就偏几厘米每秒。但只要你把每一步都做扎实了，INS在短时间内（比如10秒内）的精度是非常可观的。
好了，这一章就到这里。记住：姿态是基础，速度是关键，位置是结果，补偿是灵魂。下一章我们会讲如何用卡尔曼滤波把INS和其他传感器融合起来，敬请期待。
可可精品资料库