1、惯性导航基础：惯性导航原理、常用坐标系、地球模型与重力场
各位同学，咱们今天聊聊惯性导航的底子。说白了，惯导就是靠传感器自己算自己位置的技术，不依赖任何外部信号。我当年刚入行时，觉得这玩意儿挺玄乎，后来亲手调过几套系统才明白——核心原理其实就三个字：积分解算。
1.1 惯性导航原理：从加速度到位移
惯导的基本逻辑很简单：测加速度 → 积分得速度 → 再积分得位置。你想想看，如果你知道一个物体初始位置和速度，又知道它每一瞬间的加速度，那它的运动轨迹是不是就能算出来？
但这里有个坑——加速度计测的是比力，不是真正的运动加速度。比力 = 加速度 - 重力加速度。所以解算前必须先把重力补偿掉，否则位置会飞掉。我曾经在项目里见过一个新手，直接拿加速度计原始数据积分，结果位置误差几分钟就跑到几公里外去了……嗯，这就是没做重力补偿的后果。
核心公式（导航坐标系下）：
速度更新：v^n_k = v^n_{k-1} + [C^n_b * f^b - (2ω^n_ie + ω^n_en) × v^n + g^n] * Δt
位置更新：p^n_k = p^n_{k-1} + v^n_k * Δt其中 C^n_b 是姿态矩阵，负责把载体坐标系下的比力转换到导航坐标系。
💡 我个人习惯在代码里把重力补偿单独写成一个函数，这样调试时能快速定位是积分问题还是补偿问题。
1.2 常用坐标系：选对坐标系，解算就成功了一半
惯导里坐标系特别多，搞混了会出大问题。我建议你记住三个最常用的：
- 惯性坐标系（i系）：不动不转的绝对参考系，理论上牛顿定律在这里成立。实际用起来，我们近似取地心惯性系。
- 地球坐标系（e系）：跟着地球自转的坐标系，原点在地心，Z轴指向北极。卫星导航给的位置通常就是e系下的经纬高。
- 导航坐标系（n系）：我们最常用的——东-北-天（ENU）或北-东-地（NED）。我习惯用ENU，因为高度向上为正，符合直觉。
为什么强调坐标系？因为不同坐标系下的物理量需要转换。比如加速度计测的是b系（载体坐标系）下的比力，但解算要在n系下做，中间就得靠姿态矩阵C^n_b来转。这个矩阵如果算错了，整个导航结果都是错的。
⚠️ 注意：坐标系定义一定要和你的传感器对齐！我曾经遇到一个项目，IMU的X轴指向机头，但代码里默认X轴指向右翼，结果解算出来的航向角差了90度……排查了整整两天。
1.3 地球模型：不是圆的，是椭球
地球不是完美的球体，而是两极稍扁、赤道略鼓的椭球。惯导里最常用的模型是WGS-84（全球大地测量系统1984版）。
| 参数 | 符号 | 数值 |
|---|---|---|
| 长半轴 | a | 6378137.0 m |
| 短半轴 | b | 6356752.3142 m |
| 扁率 | f | 1/298.257223563 |
| 自转角速度 | ω_ie | 7.2921150 × 10⁻⁵ rad/s |
为什么要用椭球模型？因为曲率半径会影响位置解算。在子午圈和卯酉圈上，地球的曲率半径不一样，这直接决定了经纬度变化率与速度之间的关系。我刚开始做惯导时，偷懒用了球体模型，结果在极区附近误差大到离谱……后来老老实实换回WGS-84，问题才解决。
曲率半径计算公式：
子午圈曲率半径：R_M = a(1-e²) / (1 - e²sin²φ)^(3/2)
卯酉圈曲率半径：R_N = a / sqrt(1 - e²sin²φ)其中 e² = 2f - f² 是第一偏心率平方，φ 是纬度。
1.4 重力场：不是常数，是变化的
重力g不是9.8m/s²那么简单。它随纬度和高度变化：赤道附近g小，两极g大；越高g越小。惯导里常用的重力模型是：
g(φ, h) = g₀(φ) * (R₀ / (R₀ + h))²
其中 g₀(φ) = 9.780327 * (1 + 0.0053024sin²φ - 0.0000058sin²2φ)这个公式我建议你直接写进代码里，别用常数。为什么？因为重力误差会直接耦合进加速度积分。你想想看，如果g算错了1%，那垂直通道的加速度就错了0.098m/s²，积分10秒后位置误差就接近5米了。对于高精度惯导来说，这完全不可接受。
💡 我习惯在初始化时先根据当地经纬高算一次g值，然后作为常数用。但如果飞行高度变化很大（比如从海平面到万米高空），就必须实时更新g值。
知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以看到：传感器输出 → 坐标系转换 → 重力补偿 → 积分解算，每一步都环环相扣。
⚠️ 最后提醒一句：初始对准是惯导的命门。初始位置、速度、姿态只要有一个不准，后面解算全白搭。我见过太多人花大把时间调滤波参数，结果初始对准没做好——嗯，方向错了，跑得再快也没用。
公众号:蓝海资料掘金营，微信deep3321