3. 惯性导航基础：陀螺仪与加速度计的工作原理，比力方程，捷联惯导系统(SINS)的机械编排，姿态更新算法(四元数法)

各位同学，今天我们进入惯性导航的核心地带。说实话，这一章是整个紧组合定位系统的地基。地基不牢，地动山摇。我当年刚入行时，就因为对陀螺仪的零偏理解不够深，导致一个项目的数据后处理折腾了整整两周。所以，咱们今天把这几个硬骨头啃下来。

3.1 陀螺仪与加速度计的工作原理

先说说陀螺仪。很多人觉得陀螺仪就是测角速度的，没错，但它的物理本质是什么？

传统的机械转子陀螺，利用的是高速旋转刚体的定轴性。你想想看，一个高速旋转的转子，你给它施加外力矩，它会产生进动。通过测量这个进动，就能反推出角速度。当然，现在咱们用的更多的是MEMS陀螺和光纤陀螺。

MEMS陀螺的工作原理，说白了就是科里奥利效应。一个质量块在驱动方向上做往复运动，当载体转动时，质量块会受到科里奥利力，在检测方向上产生位移。我习惯把这个过程叫做「用振动换转动」。嗯，这里要注意，MEMS陀螺的精度受温度影响很大，我在项目里吃过这个亏。

加速度计就相对直观一些。它测的是比力，也就是单位质量上受到的惯性力与引力的合力。常见的MEMS加速度计采用梳齿结构，通过检测电容变化来反推加速度。说白了，就是弹簧-质量块系统，只不过这个「弹簧」是微米级的硅结构。

3.2 比力方程

比力方程是惯导系统的灵魂。我建议你把它刻在脑子里。

比力方程的形式是：

a_ib = v_eb + (2ω_ie + ω_en) × v_eb - g

这个方程在说什么？它告诉我们：加速度计测到的比力，并不等于载体的运动加速度。为什么？因为里面包含了地球自转引起的科里奥利加速度，以及载体在地球表面运动引起的向心加速度。

我曾经在调试一个车载导航系统时，发现速度误差发散得特别快。查了半天，最后发现是比力方程里的科里奥利项没加。加上之后，误差曲线立马就收敛了。所以，这个方程不是摆设，是实实在在要用的。

在实际工程中，我们通常把比力方程分解到导航坐标系（比如东北天坐标系）下。这样，积分出来的就是导航系下的速度和位置。

3.3 捷联惯导系统(SINS)的机械编排

机械编排，听起来很玄乎，其实就是惯导解算的流程。说白了，就是怎么用陀螺和加速度计的数据，算出姿态、速度和位置。

典型的SINS机械编排流程如下：

1. 姿态更新：用陀螺输出的角速度，更新载体相对于导航系的姿态矩阵
2. 比力坐标变换：用姿态矩阵，把加速度计输出的比力从载体系变换到导航系
3. 速度更新：在导航系下，对比力进行积分，同时补偿有害加速度，得到速度
4. 位置更新：对速度积分，得到位置

这个流程每来一个新数据就要跑一遍。我当年第一次写机械编排代码时，把姿态更新和速度更新的顺序搞反了，结果解算出来的轨迹在天上飞。嗯，从那以后我再也不敢搞错顺序了。

下面我用一张图来展示这个流程：

3.4 姿态更新算法：四元数法

姿态更新是机械编排里最核心的一步。为什么？因为姿态不准，后面的比力变换、速度位置全都会错。

常用的姿态表示方法有欧拉角、方向余弦矩阵和四元数。我个人最推荐四元数法。为什么？因为它没有欧拉角的万向锁问题，计算量又比方向余弦矩阵小。

四元数是一个超复数，形式为：

q = q0 + q1·i + q2·j + q3·k

其中，q0是标量部分，q1、q2、q3是矢量部分。它满足约束条件：q0² + q1² + q2² + q3² = 1。

姿态更新的核心方程是四元数微分方程：

dq/dt = 0.5 · q ⊗ ω

其中，ω是载体坐标系下的角速度，⊗表示四元数乘法。

在实际工程中，我们通常用毕卡逼近法来求解这个微分方程。对于低动态应用，一阶毕卡逼近就够了：

q(k+1) = [I + 0.5·Ω·Δt] · q(k)

其中，Ω是由角速度构成的反对称矩阵。

四元数更新完后，一定要做归一化。为什么？因为数值积分误差会导致四元数的模偏离1，如果不归一化，姿态矩阵的正交性就会丢失。我习惯每更新一次就归一化一次，虽然多花一点计算量，但心里踏实。

最后，把四元数转换成姿态矩阵，用于比力变换：

C_b^n = [q0²+q1²-q2²-q3², 2(q1q2-q0q3), 2(q1q3+q0q2);
         2(q1q2+q0q3), q0²-q1²+q2²-q3², 2(q2q3-q0q1);
         2(q1q3-q0q2), 2(q2q3+q0q1), q0²-q1²-q2²+q3²]

这个矩阵把载体系的比力变换到导航系。嗯，公式看着复杂，但代码实现起来其实就那么几行。

好了，这一章的内容就到这里。惯性导航的基础打牢了，后面讲紧组合滤波的时候，你才能跟得上节奏。

---