4. 惯性导航解算

各位同学，今天我们来聊聊惯性导航解算的核心内容。说实话，我刚入行时觉得这玩意儿就是一堆公式堆砌，直到第一次在跑车实验里看到数据飞掉，才明白每个细节都藏着坑。

捷联惯导算法，说白了就是把加速度计和陀螺仪的原始数据，变成我们能用的位置、速度和姿态。我习惯把它拆成三个部分：姿态更新、速度更新、位置更新。嗯，咱们一个一个说。

1.1 姿态更新与圆锥补偿

姿态更新是惯导解算的基石。你想想看，如果连飞机朝哪飞都搞不清楚，后面的速度位置全是白搭。

常用的姿态更新方法有欧拉角法、方向余弦法和四元数法。我个人最推荐四元数法——它没有奇点问题，计算量也适中。我在做无人机项目时，就吃过欧拉角的亏，俯仰角到90度时直接炸了。

但这里有个关键问题：圆锥补偿。为什么会需要它？

因为陀螺仪采样是离散的，而实际角运动是连续的。当载体做圆锥运动时，如果直接用简单累加，姿态误差会像滚雪球一样越来越大。我曾经在振动台上测试过，不加圆锥补偿，10分钟姿态就漂了5度。

圆锥补偿的经典公式是这样的：

// 二子样圆锥补偿算法
delta_theta = delta_theta1 + delta_theta2;
delta_phi = delta_theta + (2/3) * cross(delta_theta1, delta_theta2);

这里 delta_theta1 和 delta_theta2 是两个半采样周期的角增量。那个叉乘项就是补偿项。说白了，就是修正角速度积分时丢掉的非线性部分。

1.2 速度更新与划桨补偿

速度更新比姿态更新多了一个麻烦——比力方程里包含有害加速度。说白了，你得把重力加速度和哥氏加速度去掉，才能得到真正的运动加速度。

速度更新的离散形式：

v_new = v_old + (f + g - 2 * omega_ie x v_old) * dt;

但这里有个和圆锥补偿对应的概念：划桨补偿。你想想看，当载体同时做线振动和角振动时，比力积分会引入交叉耦合误差。就像划船时，船桨入水角度和力度配合不好，船就会偏航。

划桨补偿的公式和圆锥补偿很像：

// 二子样划桨补偿
delta_v = delta_v1 + delta_v2;
delta_v_comp = delta_v + (1/2) * cross(delta_theta, delta_v);

注意这里的 delta_theta 是总的角增量，delta_v 是总的比力增量。那个叉乘项就是划桨补偿项。

1.3 位置更新与涡卷补偿

位置更新相对简单，就是对速度积分。但地球是圆的，你得考虑曲率。在导航坐标系下，位置更新公式：

position_new = position_old + (v_new + v_old) * dt / 2;

这里用的是梯形积分，比矩形积分精度高一些。

涡卷补偿是个什么鬼？说白了，它是划桨补偿在位置域的表现。当载体同时做角运动和线运动时，位置积分也会产生交叉耦合误差。这个误差的形状像涡卷，所以叫涡卷补偿。

涡卷补偿的公式：

// 涡卷补偿项
delta_p_comp = (1/6) * cross(delta_theta, delta_v) * dt;

嗯，这个项通常比圆锥和划桨补偿小一个数量级。但在高精度导航中，比如导弹制导或精密测量，它还是不能忽略的。

1.4 知识体系总览

下面这张图是我自己画的，把本章的核心逻辑串起来了：

1.5 工程实现要点

最后，我给大家列个清单，做惯导解算时一定要检查：

• 采样率要够：陀螺仪采样率至少是运动频率的10倍。我见过有人用50Hz采样做旋翼无人机，结果姿态全糊了。
• 补偿项不能省：圆锥、划桨、涡卷，一个都不能少。除非你的应用精度要求极低。
• 注意数值精度：四元数更新要用双精度。单精度在长时间积分后，误差会累积到不可接受。
• 初始化要稳：上电后先静止几秒，做初始对准。我习惯用这组数据估算陀螺零偏和加速度计偏置。