第二章：惯性导航系统(INS)原理

各位同学，今天我们来啃一块硬骨头——惯性导航系统。说实话，我刚入行那会儿，觉得惯导这东西玄乎得很，几个传感器就能算出位置？后来亲手调过几套系统，才明白里面的门道。这一章，我就把加速度计和陀螺仪怎么工作、怎么解算姿态速度位置、以及捷联惯导的力学编排，掰开了讲清楚。

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

先说说这两个核心传感器。加速度计测的是什么？说白了，它测的是比力，不是单纯的加速度。你想想看，静止放在桌面上，它测的是1g向上的力，而不是0。这个坑我刚开始做项目时踩过——拿着加速度计数据直接积分算速度，结果飘到天上去。

陀螺仪呢，测的是角速度。MEMS陀螺仪的原理是科里奥利效应，一个质量块在振动，转动时会产生一个垂直于振动方向的力。嗯，这里要注意，陀螺仪输出的是角速率，不是角度。要得到角度，得积分。

我在项目中遇到过一件事：某款陀螺仪常温下零偏很稳，但温度一变化，零偏直接漂了0.1°/s。这种误差如果不补偿，姿态几分钟就废了。所以实际工程中，一定要做温度补偿和艾伦方差分析。

2.2 惯性导航解算：姿态、速度、位置

惯导解算的核心，就是三个字：积、积、积。姿态积分、速度积分、位置积分。但顺序不能乱，得先算姿态，再算速度，最后算位置。为什么？因为速度解算需要姿态把比力从载体坐标系转到导航坐标系。

2.2.1 姿态解算

姿态解算的方法有好几种，我个人习惯用四元数法。为什么？因为避免了欧拉角的万向锁问题，而且计算量小。四元数更新公式是这样的：

// 四元数更新（离散化）
q_new = q_old + 0.5 * dt * q_old ⊗ ω

其中⊗表示四元数乘法，ω是角速度向量。每次更新完，记得归一化，否则四元数会越积越离谱。

姿态解算的另一种常用方法是等效旋转矢量法，尤其在高动态场景下，它能补偿不可交换误差。说白了，就是角速度方向变化太快时，简单积分会引入误差，等效旋转矢量法能把这个误差补回来。

2.2.2 速度解算

速度解算的公式：

v_new = v_old + dt * (Cbn * f - g)

Cbn是姿态矩阵，把加速度计测的比力从载体坐标系转到导航坐标系。这里有个细节：减去的g是当地重力矢量，不是标量9.8。因为g的方向是垂直向下的，在导航坐标系里是[0, 0, -g]。

2.2.3 位置解算

位置解算最简单，直接对速度积分：

p_new = p_old + dt * v

但要注意，位置是用经纬高表示的，速度是东北天方向的。所以积分时要把速度投影到经纬度变化率上，公式稍微复杂一点：

lat_new = lat_old + dt * v_n / (R + h)
lon_new = lon_old + dt * v_e / ((R + h) * cos(lat))
h_new = h_old + dt * v_u

R是地球半径，h是高度。这里R取6378137米，但实际工程中要考虑地球椭球模型，用WGS84的曲率半径。

2.3 捷联惯导系统(SINS)的力学编排

捷联惯导，说白了就是把传感器固定在载体上，没有物理平台。所有计算都在计算机里完成。力学编排就是描述这个计算过程的数学模型。

我画了一张图，帮你理解整个力学编排的流程：

从图上你能看到，整个流程是串行的：陀螺仪数据进姿态解算，姿态矩阵出来后，配合加速度计数据做速度解算，最后速度积分得到位置。而且位置会反馈回来修正重力矢量，因为重力随纬度和高度变化。

姿态：Cbn_dot = Cbn * [ω_bn ×]
速度：v_dot = Cbn * f - (2ω_ie + ω_en) × v + g
位置：lat_dot = v_n / (R + h)
      lon_dot = v_e / ((R + h) * cos(lat))
      h_dot = v_u

这里面有个容易忽略的点：速度方程里的(2ω_ie + ω_en) × v项，是哥里奥利力和向心力的组合。我刚开始做惯导时，觉得地球自转速度才15°/h，能有多大影响？结果在长航时测试中，不补偿这项，位置误差每小时能漂几百米。所以，千万别小看地球自转效应。

最后说一句，捷联惯导的力学编排，说白了就是一套微分方程组的数值积分。你只要把每个模块的物理意义搞清楚，代码实现其实不难。难的是误差补偿——零偏、刻度因子、安装误差、温度漂移，这些才是工程中的大头。

好了，这一章的内容就到这里。惯导原理是融合导航的基石，后面讲组合导航时，你会发现很多算法都是在惯导解算的基础上做修正。先把基础打牢，后面才能走得更远。

公众号:蓝海资料掘金营，微信deep3321