4、惯性导航系统（INS）基础：IMU工作原理、比力方程与姿态更新、捷联惯导解算（姿态、速度、位置）、INS误差传播特性
4.1 IMU工作原理：陀螺仪与加速度计
说到惯性导航，核心硬件就是IMU（惯性测量单元）。它里面装了两个关键传感器：陀螺仪和加速度计。
陀螺仪测什么？测角速度。说白了，就是物体转得有多快。加速度计呢？测比力。这个「比力」不是我们平时说的力，而是物体受到的惯性力与重力的合力。嗯，这个概念容易搞混，我后面会细讲。
我记得刚入行那会儿，总觉得加速度计测的就是加速度。后来在项目里调试数据才发现，静止状态下加速度计输出的是重力加速度g，而不是0。这才真正理解了「比力」的含义。
核心要点：
- 陀螺仪输出：角速度（单位：rad/s 或 °/s）
- 加速度计输出：比力（单位：m/s²）
- IMU输出频率通常较高：100Hz~1000Hz
4.2 比力方程：从加速度计到导航加速度
比力方程是INS解算的基石。它长这样：
a_n = C_b^n * f_b - (2ω_ie^n + ω_en^n) × v_n + g_n看着复杂？我拆开给你讲：
- a_n：导航系下的加速度（我们真正想要的）
- C_b^n * f_b：把IMU测的比力从载体系转到导航系
- (2ω_ie^n + ω_en^n) × v_n：地球自转和载体运动引起的哥氏加速度
- g_n：重力加速度补偿
为什么要有这个方程？因为加速度计测的不是纯粹的加速度，而是包含了重力在内的比力。你想想看，如果直接积分加速度计的输出，静止时你会得到一个向上飞的轨迹——这显然不对。
我的经验：在实际项目中，比力方程中的哥氏项和重力项很容易被忽略。我曾经在一个低精度MEMS项目里偷懒没加哥氏补偿，结果在高速运动时位置误差直接发散。后来老老实实加上了，精度提升了一个数量级。
4.3 姿态更新：四元数法与等效旋转矢量
姿态更新是INS解算中最核心也最容易出问题的一环。常用的方法有两种：
| 方法 | 优点 | 缺点 |
|---|---|---|
| 四元数法 | 无奇点、计算量小、适合实时 | 需要归一化、存在漂移 |
| 等效旋转矢量法 | 精度高、能补偿圆锥运动 | 计算稍复杂 |
我个人习惯用四元数法做姿态更新，因为它简单高效。但要注意——四元数必须定期归一化，否则数值误差会累积。我见过有人忘了这步，结果姿态越算越偏。
四元数更新公式：
q_k+1 = q_k ⊗ exp(Δθ/2)其中Δθ是角增量，由陀螺仪积分得到。
避坑指南：我曾经在一个高动态项目中直接用角速度乘以dt代替角增量，结果在高频振动下姿态发散得一塌糊涂。后来改用等效旋转矢量法补偿圆锥运动，问题才解决。记住：高动态场景下，角增量比角速度更可靠。
4.4 捷联惯导解算：姿态、速度、位置
捷联惯导解算的流程其实很清晰：
- 姿态更新：用陀螺仪数据更新四元数/姿态矩阵
- 比力转换：用姿态矩阵把比力转到导航系
- 速度更新：对比力积分，加上哥氏和重力补偿
- 位置更新：对速度积分得到位置
说白了，就是「积分再积分」的过程。但每一步都有坑。
速度更新公式：
v_k+1 = v_k + [C_b^n * f_b - (2ω_ie^n + ω_en^n) × v_k + g_n] * Δt位置更新公式（经纬度形式）：
φ_k+1 = φ_k + v_N * Δt / (R_M + h)
λ_k+1 = λ_k + v_E * Δt / ((R_N + h) * cos(φ))
h_k+1 = h_k + v_U * Δt这里R_M和R_N分别是子午圈和卯酉圈曲率半径。嗯，地球不是完美的球体，所以要用椭球模型。
关键点：捷联惯导解算是一个递推过程，每一步都依赖上一步的结果。这意味着——误差会累积，而且会越积越大。这就是为什么纯INS不能长时间独立工作。
4.5 INS误差传播特性：为什么纯INS会发散？
INS的误差传播，说白了就是「误差的误差」。我画了一张图帮你理解：
从图上你能看到：陀螺仪误差直接导致姿态误差，姿态误差又导致比力投影错误，进而产生速度误差，速度误差积分出位置误差。而且——姿态误差还会反过来影响后续的比力转换，形成正反馈。
这就是为什么纯INS的位置误差会随时间的三次方增长。你想想看，一个0.01°/h的陀螺仪漂移，在1小时内就能产生几百米的位置误差。
注意：INS误差传播有几个典型周期：
- 舒勒周期：约84.4分钟，由重力恢复力引起
- 地球周期：24小时，由地球自转引起
- 傅科周期：随纬度变化，由地球自转和运动耦合引起
这些周期在长时间导航中会显现出来。我曾在一次船载测试中，看到位置误差呈现明显的84分钟振荡——那就是舒勒周期在作怪。
4.6 本章小结
INS基础这块，说白了就是三件事：
- IMU测什么：陀螺仪测角速度，加速度计测比力
- 怎么解算：比力方程去重力、去哥氏，四元数更新姿态，积分得速度位置
- 误差怎么传：陀螺仪误差→姿态误差→速度误差→位置误差，层层放大
我个人觉得，理解INS误差传播特性比会写解算代码更重要。因为只有知道误差从哪来、怎么传，你才能在融合定位时设计出有效的补偿策略。下一章我们会讲如何用外部传感器来抑制这些误差——嗯，那就是组合导航的核心了。
一个小建议：如果你刚开始接触INS，建议先用手写代码实现一遍完整的捷联惯导解算。别用现成的库。我当年就是这么干的，虽然踩了不少坑，但踩完之后对INS的理解就再也不会忘了。
公众号:蓝海资料掘金营，微信deep3321