第4章惯性导航原理(INS)

4. 惯性导航原理(INS)：加速度计与陀螺仪工作原理、比力方程、姿态更新算法

各位同学，今天我们进入INS的核心地带。说实话，惯性导航这玩意儿，看着公式多，其实底层逻辑很朴素——就是测量加速度和角速度，然后积分、积分、再积分。我当年第一次跑通INS解算时，看着轨迹在屏幕上飘，心里那个激动啊。但后来发现，真正难的不是算法本身，而是怎么跟误差做斗争。

先说说这两个传感器。加速度计测的是比力，不是重力。陀螺仪测的是角速度，不是角度。这个区别很重要。

加速度计内部有个质量块，被弹簧悬着。当载体加速时，质量块会位移，通过检测位移量就能反推出加速度。但这里有个坑——它测的是比力，也就是物体受到的合力减去重力。

比力方程的核心思想：

加速度计输出 = 载体加速度 - 重力加速度

或者说：f = a - g

所以静止时，加速度计测到的是向上的1g，不是0。我刚开始做项目时，有次发现静止时加速度计输出不是0，还以为传感器坏了，查了半天资料才反应过来——嗯，这就是比力。

陀螺仪就更有意思了。MEMS陀螺仪利用科里奥利效应——一个振动的质量块，当载体旋转时，会受到一个垂直于振动方向的力。这个力的大小正比于角速度。

我建议你记住一个关键点：陀螺仪测的是角速度，不是角度。要得到姿态，必须积分。而积分就会带来漂移。我在项目里见过最夸张的情况，一个低成本的MEMS陀螺仪，静止时输出角速度的零偏稳定性能做到0.1°/s，积分10分钟，姿态就偏了60度。所以，别指望纯惯性能撑太久。

比力方程是INS的核心。它描述了载体加速度、比力和重力之间的关系。说白了，就是怎么从加速度计的输出中，把真正的运动加速度提取出来。

在导航坐标系（n系）下，比力方程长这样：

V̇^n = C_b^n · f^b - (2ω_ie^n + ω_en^n) × V^n + g^n

看着复杂？我拆开给你看：

我的经验：实际编程时，比力方程中的哥里奥利修正项，在低速运动时可以忽略。我做过一个车载项目，车速不超过30m/s，去掉这项后位置误差不到0.1%。但如果你做高动态飞行器，这玩意儿必须算，否则误差会累积得很快。

姿态更新是INS里最核心、也最容易出bug的部分。常用的方法有两种：四元数法和等效旋转矢量法。

四元数用四个参数表示旋转，没有奇点，计算效率高。姿态更新公式是：

q_{k+1} = q_k ⊗ q(Δθ)

其中q(Δθ)是由角增量构造的旋转四元数。具体实现时，我建议用一阶或二阶龙格库塔法。但要注意——四元数必须归一化。我见过太多新手忘了这步，结果姿态越算越歪。

避坑指南：我曾经在一个项目里，四元数归一化写成了每100步做一次，想着省点计算量。结果姿态误差在10分钟内累积到5度。后来改成每步归一化，问题立刻解决。记住：归一化不是可选项，是必选项。

等效旋转矢量法解决了四元数法在高动态环境下的不可交换误差。说白了，当载体在短时间内绕多个轴旋转时，角速度的积分顺序会影响结果。等效旋转矢量法通过补偿这个误差，让姿态更新更准。

公式长这样：

Φ = Δθ₁ + Δθ₂ + (2/3) · (Δθ₁ × Δθ₂)

其中Δθ₁和Δθ₂是两个半采样间隔的角增量。那个叉乘项就是补偿项。我建议你在做高动态应用（比如无人机、导弹）时，一定要用等效旋转矢量法。低动态场景（比如车载、船载），四元数法就够用了。

下面这张图，是我自己整理的INS核心逻辑。你看一遍，应该能对整个流程有个清晰的印象。

说了这么多理论，最后给点实在的。我根据项目经验，总结了一个选择表：

我的建议：如果你刚开始做INS，先用四元数法跑通整个流程。等你能把位置误差控制在10%以内了，再考虑升级到等效旋转矢量法。一口吃不成胖子，INS这东西，得慢慢来。

好了，这一章的内容就到这儿。记住：INS的核心就是积分，而积分的敌人就是误差。下一章我们会聊怎么用GPS来修正这些误差——那就是松耦合的开始。