3、捷联惯导系统：姿态更新算法、速度更新算法、位置更新算法
各位同学，咱们今天聊点硬核的。捷联惯导系统的三大核心算法——姿态更新、速度更新、位置更新。说白了，就是回答三个问题：我现在朝哪看？我跑多快？我在哪？
我个人习惯把这三大算法比作「导航三兄弟」。老大是姿态，它最累，因为速度和位置都得靠它撑着。老二速度，老三位置，一个比一个简单。但别小看老三，它最容易飘。
核心逻辑链：
陀螺仪测角速度 → 姿态更新 → 加速度计测比力 → 投影到导航系 → 扣除重力 → 速度更新 → 位置更新
每一步都依赖上一步的结果，环环相扣。
3.1 姿态更新算法——导航的「脊梁骨」
姿态更新，说白了就是算「载体坐标系」和「导航坐标系」之间的旋转关系。你想想看，加速度计测的是载体坐标系的比力，但导航需要的是北东地方向的加速度，没有姿态矩阵，后面全白搭。
常用的方法有三种：
- 欧拉角法：直观，但有万向锁问题。俯仰角接近±90°时直接炸裂。我早期做无人机飞控时踩过这个坑，飞机垂直爬升时姿态解算直接发散，吓得我赶紧切了四元数。
- 方向余弦矩阵法：无奇点，但9个参数，计算量大。嵌入式平台扛不住。
- 四元数法：目前的主流。4个参数，无奇点，计算量适中。我个人习惯用四元数+毕卡逼近法。
我的经验：工程上最常用的是四元数更新 + 等效旋转矢量。特别是高动态场景（比如车载剧烈颠簸），一定要用多子样算法补偿不可交换误差。我曾经在跑车测试中，只用单子样算法，结果姿态误差每分钟漂了0.5°，后来换成双子样算法，误差降到0.05°以内。
四元数更新公式（离散化）：
// 等效旋转矢量法（双子样）
Δθ = ω₁ * T + ω₂ * T // 角增量
Φ = Δθ + (1/12) * (Δθ₁ × Δθ₂) // 不可交换误差补偿
q(k+1) = q(k) ⊗ exp(Φ/2)注意：四元数更新后一定要归一化！否则模长会慢慢偏离1，导致姿态矩阵不正交。我见过有人忘了归一化，跑了10分钟后姿态直接乱套。归一化就一行代码：q = q / norm(q)，别省。
3.2 速度更新算法——比力方程的应用
速度更新，核心就是比力方程。说白了：加速度计测的比力，投影到导航系，减去重力，再积分。
公式长这样（导航系取北东地）：
v̇ⁿ = C_bⁿ * fᵇ - (2ω_ieⁿ + ω_enⁿ) × vⁿ + gⁿ嗯，这里要注意几个坑：
- 哥里奥利项：2ω_ieⁿ × vⁿ，地球自转引起的。低速场景（比如行人导航）可以忽略，但车载、航空必须算。我做过一个车载项目，忽略哥里奥利项后，10分钟速度误差大了0.3 m/s。
- 重力模型：别用简单的9.8。高精度场景要用正常重力公式（比如WGS84模型），考虑纬度和高度变化。
- 积分方法：梯形法比矩形法好。我习惯用二阶龙格-库塔，精度够用，计算量也不大。
避坑指南：我曾经在速度更新时，直接用上一时刻的姿态矩阵投影比力。结果姿态更新和速度更新不同步，产生了不可忽略的误差。后来我改用「中间时刻姿态矩阵」——也就是取k和k+1时刻姿态的平均值来投影，精度提升明显。
速度更新伪代码：
// 速度更新（二阶龙格-库塔）
f_n = C_b_n_mid * f_b // 用中间时刻姿态矩阵投影
a_n = f_n - (2*omega_ie + omega_en) × v + g
v_new = v_old + T * a_n3.3 位置更新算法——最简单的也最容易飘
位置更新，说白了就是速度积分。公式简单：
p_new = p_old + v * T但为什么说它最容易飘？因为误差会累积。速度有一点误差，位置误差就随时间线性增长。你想想看，速度误差0.1 m/s，跑1小时位置就偏了360米。
位置更新的两种模式：
| 模式 | 适用场景 | 精度 | 备注 |
|---|---|---|---|
| 经纬高模式 | 全球导航 | 中等 | 需考虑地球曲率，纬度变化影响曲率半径 |
| 直角坐标模式 | 局部区域 | 较高 | 适合小范围（<10km），忽略地球曲率 |
我个人习惯在车载导航中用经纬高模式。因为车跑得远，动不动跨几个城市，直角坐标模式会累积曲率误差。
小技巧：位置更新时，速度取「当前时刻」和「上一时刻」的平均值。也就是用梯形法积分，比矩形法准不少。代码就一行：p_new = p_old + (v_old + v_new) * 0.5 * T
3.4 三大算法的协同与注意事项
三大算法不是孤立的。它们的关系就像流水线：
- 陀螺仪数据 → 更新姿态
- 姿态矩阵 → 投影比力
- 投影后的比力 → 更新速度
- 速度 → 更新位置
几个关键点：
- 更新频率：姿态更新要快（100Hz以上），速度和位置可以慢一些（50Hz）。因为陀螺仪动态高，加速度计相对平稳。
- 时间同步：陀螺仪和加速度计的数据必须时间对齐。我遇到过IMU内部时钟不同步，导致姿态和速度更新错位，误差大了好几倍。
- 初始化：姿态初始化要用加速度计和磁力计（或外部参考）做初始对准。速度和位置初始化一般靠GNSS或人为给定。
血的教训：我曾经在初始化时，忘了做初始对准，直接假设姿态为0。结果系统一启动就带着巨大的初始姿态误差，速度和位置直接发散。后来我加了一段「静基座对准」逻辑——静止状态下用加速度计算俯仰和横滚，用磁力计算航向。虽然麻烦，但值得。
好了，三大算法就聊到这儿。记住一句话：姿态是灵魂，速度是骨架，位置是血肉。 任何一个环节出问题，整个导航系统就废了。做工程时，多想想误差怎么来的，怎么补偿，比死记公式有用得多。