4、捷联惯导解算：速度更新算法、位置更新算法、重力模型、地球自转补偿
各位同学，欢迎来到第四章。前面我们聊了姿态更新，那是惯导解算的「脊梁骨」。今天我们要把剩下的三块拼图补上：速度怎么算、位置怎么推、以及重力这个「捣蛋鬼」怎么处理。
说实话，这三块内容在教科书里往往被一笔带过。但我得提醒你——实际工程中，80%的导航误差都出在这里。不是姿态算不准，而是速度和位置被重力模型和地球自转带偏了。
4.1 速度更新算法：比力方程离散化
速度更新的核心，就是解这个比力方程：
dV/dt = f^n - (2ω_ie^n + ω_en^n) × V + g^n看着挺吓人，其实就三部分：
- f^n：加速度计测的比力，投影到导航系
- -(2ω_ie^n + ω_en^n) × V：哥里奥利力和向心力的合体
- g^n：重力加速度
离散化怎么做？我个人习惯用二子样法。为啥？因为简单、够用。
// 速度更新（二子样法）
ΔV = ΔV_f + ΔV_g + ΔV_cor
// 比力增量
ΔV_f = C_b^n(t) * (ΔV_1 + ΔV_2) / 2
// 重力增量
ΔV_g = g^n * Δt
// 哥里奥利力修正
ΔV_cor = -(2ω_ie^n + ω_en^n) × V(t) * Δt重要提醒：比力增量一定要用平均姿态矩阵来投影。直接用t时刻的矩阵，误差会随时间累积。我在项目里吃过这个亏，跑10分钟就偏了50米。
4.2 位置更新算法：经纬高怎么推
位置更新相对直观，但有个坑——地球曲率。你不能把地球当平面算，否则飞个几百公里就偏到姥姥家了。
位置更新公式：
// 纬度更新
φ(t+Δt) = φ(t) + V_N * Δt / (R_M + h)
// 经度更新
λ(t+Δt) = λ(t) + V_E * Δt / ((R_N + h) * cos(φ))
// 高度更新
h(t+Δt) = h(t) + V_U * Δt这里R_M和R_N分别是子午圈曲率半径和卯酉圈曲率半径。你想想看，地球是个椭球，不同纬度的曲率半径是不一样的。
| 参数 | 含义 | 典型值（纬度45°） |
|---|---|---|
| R_M | 子午圈曲率半径 | 约 6,367 km |
| R_N | 卯酉圈曲率半径 | 约 6,389 km |
| g | 重力加速度 | 约 9.806 m/s² |
我的小技巧：高度更新时，别忘了用气压高度做辅助。纯惯导的高度通道是发散的，这是物理规律，没办法。所以实际系统中，高度一定要用外部传感器修正。
4.3 重力模型：别用9.8糊弄事
很多初学者喜欢写死g=9.8。嗯，这里要注意——重力是随纬度和高度变化的。
常用的重力模型：
g(φ, h) = g_0 * (1 + 0.0052884 * sin²(φ) - 0.0000059 * sin²(2φ))
- 0.000003086 * hg_0是赤道海平面重力，约9.780 m/s²。你看，纬度从0°到90°，重力变化约0.05 m/s²。别小看这0.05，积分10分钟就是30m/s的速度误差。
我曾经在一个高纬度项目中，忘了改重力模型，结果导航误差直接爆表。后来查了三天才找到原因——就是重力公式里少了个sin²(2φ)项。
避坑指南：我曾经在极地地区测试，发现惯导解算结果异常。后来发现，标准重力模型在纬度>80°时误差会急剧增大。建议高纬度地区使用EGM2008重力场模型，或者至少用高阶展开式。
4.4 地球自转补偿：别让地球「偷」你的速度
地球自转角速度ω_ie ≈ 7.292115 × 10⁻⁵ rad/s。看着很小，但影响不小。
主要体现在两个地方：
- 哥里奥利力：物体在旋转参考系中运动时受到的偏转力。比如北向运动会引起东向的加速度。
- 向心加速度：地球自转产生的离心力，其实已经包含在重力模型里了。
补偿公式：
// 地球自转角速度在导航系下的投影
ω_ie^n = [ω_ie * cos(φ), 0, -ω_ie * sin(φ)]^T
// 哥里奥利力加速度
a_cor = -2 * ω_ie^n × V^n说白了，就是你在北半球往北走，地球自转会让你往东偏。这个效应在高速运动时特别明显。比如飞机以250m/s的速度向北飞，哥里奥利力产生的东向加速度约0.036 m/s²。10分钟后，东向速度误差约22m/s。
核心要点：地球自转补偿不能忽略，尤其是在高精度导航或长时间导航场景。对于消费级IMU（比如手机里的），这个效应可以忽略。但对于航空级惯导，必须精确补偿。
4.5 完整解算流程
好了，我们把姿态、速度、位置串起来，看看完整的捷联惯导解算流程：
// 每个解算周期（比如1ms）
1. 读取陀螺仪角增量 Δθ，加速度计速度增量 ΔV
2. 姿态更新：用Δθ更新四元数/方向余弦矩阵
3. 速度更新：
a. 用平均姿态矩阵投影ΔV到导航系
b. 计算哥里奥利力修正项
c. 计算重力修正项
d. 更新速度
4. 位置更新：用更新后的速度计算经纬高
5. 输出导航结果这个流程看起来简单，但每个步骤都有细节。我建议你写代码时，先跑一个纯仿真环境，给一个已知轨迹，看看解算结果能不能跟真值对上。
调试技巧：如果发现速度或位置发散，先检查重力模型是否正确，再看哥里奥利力补偿有没有加反符号。这两个是新手最容易犯错的地方。
4.6 本章知识体系
下面我用一张图来总结本章的核心逻辑：
这张图把今天讲的内容串起来了。你仔细看，速度更新是承上启下的关键环节——它既要用到姿态更新的结果，又要为位置更新提供输入。所以速度更新算法做得好不好，直接决定了整个导航系统的精度。
好了，这一章就到这里。记住我说的：重力模型别偷懒，地球自转别忽略，速度更新用平均姿态矩阵。做到这三点，你的惯导解算至少不会「翻车」。
可可精品资料库