第二章 惯性导航基础：从原理到实战
各位同学，欢迎来到惯性导航的世界。说实话，GNSS和惯导融合这块，惯导才是真正的「硬骨头」。GNSS说白了就是个黑盒子，给数据你就用。但惯导不一样，你得懂它的脾气，知道它什么时候会「犯浑」。
我刚开始做车载定位那会儿，总觉得惯导就是个辅助角色。直到有一次在隧道里，GNSS信号全丢，全靠惯导撑着跑了三公里，最后定位误差不到2米。嗯，从那以后我再也不敢小看它了。
2.1 加速度计与陀螺仪：惯导的两只眼睛
惯导系统靠什么感知运动？两个核心器件：加速度计和陀螺仪。
加速度计测量的是比力，不是单纯的加速度。什么叫比力？就是物体受到的惯性力减去重力。你想想看，静止放在桌面上，加速度计测到的是1g向上的力，而不是0。这个坑我踩过——第一次做静态初始化，看到加速度计输出不是0，还以为传感器坏了。
关键公式：
f = a - g其中 f 是比力测量值，a 是真实加速度，g 是重力加速度。
陀螺仪测量角速度，单位是 rad/s 或 °/s。它告诉你载体转得有多快。但注意，陀螺仪测的是相对于惯性空间的角速度，不是相对于地面的。这个区别在后续姿态解算中非常重要。
| 器件 | 测量物理量 | 常见误差 | 我的经验 |
|---|---|---|---|
| 加速度计 | 比力 (m/s²) | 零偏、刻度因子、噪声 | 零偏最头疼，每次上电都不一样 |
| 陀螺仪 | 角速度 (rad/s) | 零偏、角度随机游走 | 温度变化影响很大，需要补偿 |
避坑指南：我曾经在项目里直接用加速度计输出做积分算速度，结果几分钟就漂到天上去。后来才明白，必须先扣除重力分量，再做积分。这个顺序搞反了，数据就没法看了。
2.2 坐标系定义：你得知道自己在哪
搞惯导，坐标系是基本功。我见过太多人因为坐标系搞混，调试好几天才发现问题。说白了，坐标系就是定位的「参考系」，选错了，后面全白搭。
地心地固系（ECEF）：原点在地心，Z轴指向北极，X轴指向本初子午线与赤道交点。这个坐标系随地球自转，适合描述地面点的位置。GNSS输出的经纬高，最终都要转到ECEF系才能和惯导融合。
导航系（n系）：通常用北-东-地（NED）或东-北-天（ENU）。车载定位常用NED，因为和车辆行驶方向更匹配。我习惯把导航系当作「上帝视角」，所有计算最终都要回到这个坐标系。
载体坐标系（b系）：固定在车上的坐标系。x轴指向前方，y轴指向右侧，z轴指向下方（符合右手定则）。IMU安装时，它的敏感轴要和载体坐标系对齐。这个对齐精度直接影响融合效果。
小技巧：我在实际项目中，会在车上贴一个坐标系示意图。每次安装IMU，都拿水平尺和直角尺校准。别嫌麻烦，安装误差1度，跑100米就偏1.7米，这误差你受得了吗？
2.3 姿态表示方法：三种武器，各有千秋
姿态描述，说白了就是告诉计算机「车头朝哪」。有三种主流方法：欧拉角、四元数、方向余弦矩阵。我一个个说。
欧拉角：直观，三个角度（横滚、俯仰、航向）就能描述姿态。但有个致命问题——万向锁。当俯仰角接近±90°时，横滚和航向会耦合，解算就崩了。我在做无人机项目时遇到过，飞机垂直爬升时姿态突然跳变，吓得我赶紧切回四元数。
四元数：四个参数，无奇点，计算效率高。这是目前惯导解算的主流选择。四元数本质上是一个超复数，表示绕某个轴旋转一定角度。虽然不直观，但计算机喜欢它。
// 四元数定义：q = [w, x, y, z]
// 其中 w 是标量部分，[x,y,z] 是矢量部分
// 单位四元数满足：w² + x² + y² + z² = 1
// 四元数乘法（用于姿态更新）
q_new = q_old * dq
// dq 是由角速度增量计算得到的增量四元数方向余弦矩阵（DCM）：3×3矩阵，每个元素都是两个坐标系轴之间的余弦值。物理意义清晰，但9个参数，计算量大。早期惯导系统用得多，现在基本被四元数取代了。
| 方法 | 参数数量 | 优点 | 缺点 | 我的推荐 |
|---|---|---|---|---|
| 欧拉角 | 3 | 直观，易理解 | 万向锁，计算复杂 | 仅用于显示 |
| 四元数 | 4 | 无奇点，效率高 | 不直观 | 解算首选 |
| DCM | 9 | 物理意义清晰 | 计算量大 | 理论分析用 |
实战建议：我个人习惯在解算内核用四元数，只在输出给上层应用时转成欧拉角。这样既避免了万向锁，又方便调试人员理解。转换公式网上都有，但要注意角度范围——航向角通常是0~360°，别搞成-180~180°。
2.4 惯性导航解算流程：从原始数据到位置
好了，前面都是铺垫，现在说正事。惯导解算到底怎么走？我画个流程图给你看。
原始数据 → 误差补偿 → 姿态更新 → 速度更新 → 位置更新
↓ ↓ ↓ ↓ ↓
陀螺仪/ 去除零偏 四元数积分 比力投影 位置积分
加速度计 刻度因子 更新姿态 扣除重力 得到经纬高
温度补偿 得到姿态角 积分得速度第一步：误差补偿。原始数据不能直接用。陀螺仪有零偏，加速度计有刻度因子，温度变化还会引入额外误差。我一般会在上电后做静态采集，取前100个数据平均，作为零偏估计值。
注意：我曾经犯过一个低级错误——把静态采集的零偏直接用于动态数据。实际上，零偏会随温度和时间缓慢变化。更好的做法是：在GNSS信号好的时候，用卡尔曼滤波在线估计零偏。这个后面章节会详细讲。
第二步：姿态更新。用补偿后的角速度，更新四元数。核心公式是四元数微分方程：
dq/dt = 0.5 * q * ω
其中 ω 是角速度的四元数表示 [0, ωx, ωy, ωz]实际代码里，我会用二阶龙格-库塔法做离散化。一阶精度不够，跑几分钟姿态就飘了。
第三步：速度更新。把加速度计测到的比力，用当前姿态矩阵转到导航系，然后扣除重力，再积分得到速度。
// 伪代码
a_nav = C_b_n * f_body // 比力从载体系转到导航系
a_nav[2] -= g // 扣除重力
v_new = v_old + a_nav * dt // 积分得速度第四步：位置更新。用速度积分得到位置增量，再更新经纬高。这里要注意地球曲率的影响——跑得越远，误差越大。
我的经验：位置更新时，千万别直接用平面直角坐标系的公式。地球是圆的，跑10公里就有约8米的曲率误差。用经纬度更新时，要引入子午圈曲率半径和卯酉圈曲率半径。公式虽然复杂，但精度提升明显。
好了，惯性导航的基础就这些。说白了，就是「测角速度→更新姿态→测比力→算加速度→积分得速度→再积分得位置」。每一步都有坑，但只要你理解了原理，踩过的坑都会变成经验。
下一章，我们会把GNSS和惯导真正「焊」在一起。到时候你会发现，1+1真的能大于2。