第四章：惯性导航机械编排
各位同学，今天我们聊点硬核的——惯性导航的机械编排。说白了，就是IMU怎么从原始的加速度和角速度，一步步算出位置、速度和姿态。这部分内容，我当年刚入行时啃了很久，踩过不少坑，今天我把这些经验都揉碎了讲给你们听。
4.1 比力方程——IMU测量的到底是什么？
先问大家一个问题：IMU输出的加速度，真的是物体的运动加速度吗？
不是的。IMU测量的是「比力」，也就是载体受到的惯性力与重力的合力。用公式表达就是：
f = a - g其中f是比力，a是绝对加速度，g是重力加速度。你想想看，如果IMU静止放在桌上，它测到的加速度是0吗？不对，它测到的是向上的1g——因为它在抵抗重力。
核心理解：比力方程的本质，就是从IMU测量的比力中，扣除重力项，才能得到真实的运动加速度。
我在做车载导航时遇到过一个问题：车辆停在坡道上，IMU输出的加速度不为零。当时我以为是传感器坏了，后来才意识到——这就是比力在起作用。嗯，从那以后我再也不敢忽略重力补偿了。
4.2 速度更新算法——从加速度到速度
速度更新，说白了就是对比力进行积分。但这里有个关键点：积分是在导航坐标系下进行的，而IMU测量是在载体坐标系下。
所以流程是这样的：
- 读取IMU的加速度计输出（载体坐标系）
- 用当前姿态矩阵，把加速度转换到导航坐标系
- 扣除重力项和哥氏加速度
- 对时间积分，得到速度增量
v_k = v_{k-1} + [C_b^n * f_b - (2ω_ie^n + ω_en^n) × v_{k-1} + g^n] * Δt这个公式看着复杂，其实拆开看就三部分：比力投影、有害加速度补偿、重力补偿。我建议你们写代码时，先把这三部分分开实现，调试通过后再合并。
个人经验：速度更新最容易出问题的地方是坐标系转换。我曾经因为姿态矩阵更新顺序搞反了，导致速度发散，查了两天bug。建议每次更新后都做一次单位化检查。
4.3 位置更新算法——从速度到位置
位置更新相对简单，就是对速度积分。但要注意，位置更新是在地球坐标系下进行的，涉及到经纬度和高度的计算。
| 参数 | 更新公式 | 说明 |
|---|---|---|
| 纬度 | φ_k = φ_{k-1} + v_N * Δt / (R_M + h) | R_M为子午圈曲率半径 |
| 经度 | λ_k = λ_{k-1} + v_E * Δt / [(R_N + h) * cos(φ)] | R_N为卯酉圈曲率半径 |
| 高度 | h_k = h_{k-1} + v_U * Δt | 直接积分 |
这里有个坑：经度更新公式分母里有cos(φ)，当纬度接近90度时，这个值趋近于0，计算会爆炸。极地导航是个专门的课题，我们这里先不展开。
4.4 圆锥误差与划船误差——高动态下的噩梦
这两个误差，我当年做无人机飞控时被折磨得不轻。先说说它们是怎么来的。
圆锥误差：当载体同时存在两个正交方向的角振动时，即使每个方向的角速度均值为零，但积分后会出现一个沿第三轴的方向漂移。形象点说，就像圆锥的顶点在画圈。
划船误差：类似的情况，发生在加速度计上。当载体同时存在线振动和角振动时，速度积分会出现虚假的漂移。
注意：这两种误差在低动态环境下可以忽略，但在高动态场景（如无人机、赛车）中，如果不补偿，几分钟内位置误差就能到几十米。
为什么会这样？因为IMU的采样是离散的，而实际运动是连续的。离散化过程中，高频振动信息被丢失了，但它们的耦合效应却累积了下来。
4.5 双子样算法——工程界的标准解法
解决圆锥误差和划船误差，最经典的方法就是双子样算法。名字听着玄乎，其实原理很简单：
在一个更新周期内，采集两个子样（两个时刻的IMU数据），用它们的差值来估计角速度和加速度的变化率，从而补偿离散化带来的误差。
// 双子样圆锥补偿算法（简化版）
Δθ = Δθ_1 + Δθ_2
Δθ_cone = (2/3) * (Δθ_1 × Δθ_2)
// 双子样划船补偿算法（简化版）
Δv = Δv_1 + Δv_2
Δv_scull = (2/3) * (Δθ_1 × Δv_2 + Δv_1 × Δθ_2)你看，补偿项其实就是两个子样的叉积，再乘上一个系数。这个系数2/3是怎么来的？是通过优化算法推导出来的，能让误差最小化。
我的建议：实际工程中，如果IMU的采样率足够高（比如1kHz以上），单子样算法也够用。但如果采样率只有100-200Hz，一定要用双子样甚至三子样算法。我曾经在200Hz的IMU上只用单子样，结果圆锥误差导致姿态发散，教训深刻。
知识体系总览
下面这张图，是我自己整理的机械编排核心逻辑，你们可以对照着理解：
这张图把整个机械编排的流程串起来了。从IMU原始数据出发，经过比力方程处理，分三路进行速度、位置、姿态更新，最后用双子样算法补偿误差，得到最终的导航输出。
好了，这一章的内容就到这里。机械编排是惯性导航的基石，理解透了，后面的融合算法才能做得扎实。你们写代码时，建议先从速度更新开始，一步步调试，别想一口吃成胖子。
可可精品资料库