3、惯性导航原理（上）：加速度计与陀螺仪的工作原理、比力方程与角速度积分、IMU的误差模型
各位同学，欢迎来到惯性导航原理的上半部分。说实话，惯性导航是组合导航的“脊梁骨”。你想想看，GPS会丢星，视觉会跟丢，但IMU（惯性测量单元）只要不断电，它就在那里默默地算。今天咱们就把IMU的底裤扒干净——从加速度计和陀螺仪怎么干活，到比力方程怎么推导，再到IMU那些让人头疼的误差模型。
核心观点：IMU不是直接测“位置”和“姿态”的，它测的是“比力”和“角速度”。你得通过积分和补偿，才能得到导航结果。这个过程中，误差会像滚雪球一样越滚越大——所以理解误差模型，比理解原理本身更重要。
3.1 加速度计的工作原理
加速度计测的是什么？很多人以为是“加速度”。其实不是。它测的是比力（Specific Force），也就是单位质量上受到的惯性力减去重力。
我打个比方：你站在体重秤上，秤显示的是你的体重（重力）。但如果你在电梯里加速上升，秤的读数会变大。这个“变大”的部分，就是比力。加速度计本质上就是一个“质量-弹簧-阻尼”系统。
常见的加速度计类型有：
- 电容式MEMS加速度计：利用质量块位移改变电容值。便宜、体积小，消费级IMU的主力。
- 石英谐振式加速度计：精度高，但贵。战术级和导航级IMU常用。
- 摆式加速度计：老式但稳定，航天领域还有应用。
我在项目中遇到过一件事：有一次用MEMS加速度计做行人导航，发现静止时输出一直在漂。查了半天，原来是温度补偿没做好。嗯，这里要注意——MEMS加速度计对温度极其敏感，零偏随温度变化能差出好几个mg。
实战小技巧：拿到一个加速度计，先做“六位置法”标定。把IMU分别朝上、朝下、朝东、朝西、朝北、朝南放置，记录输出。这样能快速估算出零偏和刻度因子。
3.2 陀螺仪的工作原理
陀螺仪测角速度。原理上分两大类：
- 机械转子陀螺：利用高速旋转转子的定轴性。精度极高，但体积大、功耗高、寿命短。
- 光学陀螺：包括环形激光陀螺（RLG）和光纤陀螺（FOG）。利用萨格纳克效应，没有运动部件，可靠性好。
- MEMS振动陀螺：利用科里奥利效应。便宜、小、功耗低，但精度有限。
说白了，MEMS陀螺里有一个质量块在高速振动。当壳体转动时，科里奥利力会让质量块在垂直方向产生位移。测出这个位移，就能反算出角速度。
我曾经调试过一个MEMS陀螺，发现输出噪声特别大。后来发现是电源纹波干扰了驱动电路。你想想看，驱动信号本身才几毫伏，纹波一上来，信噪比直接崩了。所以IMU的供电一定要干净，最好用LDO单独供电。
避坑指南：陀螺仪的“零偏稳定性”比“零偏重复性”更重要。零偏稳定性决定了你积分姿态的漂移速度。我曾经被一个标称零偏稳定性0.1°/h的陀螺坑过——实际上电后前10分钟漂了0.5°，因为温漂没补偿。所以，上电后一定要给IMU足够的预热时间（至少5分钟）。
3.3 比力方程
比力方程是惯性导航的“牛顿第二定律”在导航坐标系下的表达。它的形式是：
a_n = C_b^n * f_b - (2ω_ie^n + ω_en^n) × v_n + g_n其中：
a_n：导航系下的加速度C_b^n：从载体系到导航系的旋转矩阵f_b：加速度计测得的比力（载体系）ω_ie^n：地球自转角速度在导航系下的投影ω_en^n：导航系相对地球的角速度（由载体运动引起）v_n：导航系下的速度g_n：重力加速度
这个方程看着复杂，其实逻辑很简单：
- 先把比力从载体系转到导航系（用姿态矩阵）
- 减去哥里奥利加速度（2ω×v）和向心加速度（ω_en×v）
- 加上重力
- 得到的就是导航系下的真实加速度
我建议你把这个方程手推一遍。我在带新人时，要求他们必须能默写出来。为什么？因为后面所有的惯导解算算法，都是在这个方程上做文章。
关键点：比力方程中的“重力”项是最大的干扰项。在静止状态下，加速度计测到的其实是重力在载体系下的投影。所以，如果你不知道姿态，你根本分不清“加速度”和“重力”。这就是为什么惯导初始化时需要“对准”——先确定姿态，才能正确扣除重力。
3.4 角速度积分与姿态更新
陀螺仪输出的是角速度。要得到姿态，需要积分。常用的姿态更新方法有三种：
| 方法 | 原理 | 精度 | 计算量 |
|---|---|---|---|
| 欧拉角法 | 直接积分欧拉角微分方程 | 低（有奇异点） | 小 |
| 方向余弦矩阵法 | 更新旋转矩阵 | 中 | 大 |
| 四元数法 | 更新四元数 | 高（无奇异） | 中 |
我个人习惯用四元数法。原因很简单：没有奇异点，计算量适中，而且容易做插值和滤波。四元数的更新公式是：
q_k+1 = q_k ⊗ exp(0.5 * ω * Δt)其中⊗表示四元数乘法，ω是角速度向量，Δt是采样间隔。
这里有个坑：角速度积分时，如果采样率不够高，圆锥运动（coning motion）会导致严重的姿态误差。我见过一个案例，IMU采样率只有100Hz，但载体振动频率到了50Hz，结果姿态误差每秒钟漂了0.1°。解决办法是提高采样率，或者用圆锥补偿算法。
实战建议：对于MEMS IMU，采样率至少要到200Hz以上。如果做高动态场景（比如无人机、机器人），建议400Hz以上。另外，积分前一定要做“去偏”处理——减去陀螺的零偏。
3.5 IMU的误差模型
IMU不是完美的。它的误差可以分为确定性误差和随机性误差两大类。
3.5.1 确定性误差
- 零偏（Bias）：静止时输出不为零。单位：m/s²（加速度计）或°/h（陀螺仪）。
- 刻度因子（Scale Factor）：输入输出比例不准确。通常用ppm表示。
- 非正交误差（Misalignment）：三个轴不严格垂直。用旋转矩阵表示。
- 非线性误差：输出与输入不成线性关系。
3.5.2 随机性误差
- 角度随机游走（ARW）：陀螺输出中的白噪声积分后产生的角度误差。单位：°/√h。
- 速度随机游走（VRW）：加速度计白噪声积分后产生的速度误差。单位：m/s/√h。
- 零偏不稳定性（Bias Instability）：零偏随时间的低频漂移。单位：°/h或mGal。
- 量化噪声：AD转换带来的误差。
完整的误差模型可以写成：
ω_meas = (I + S_ω) * ω_true + b_ω + n_ω
f_meas = (I + S_f) * f_true + b_f + n_f其中S是刻度因子+非正交矩阵，b是零偏，n是噪声。
我在做组合导航时，最头疼的就是零偏不稳定性。它不像白噪声那样好处理，也不像确定性零偏那样可以标定补偿。它是个低频随机过程，只能用滤波去估计。常用的模型是一阶马尔可夫过程：
b_dot = -β * b + w其中β是相关时间的倒数，w是驱动白噪声。
避坑指南：不要以为标定完就万事大吉了。IMU的误差会随温度、时间、振动而变化。我曾经在一个项目中，IMU标定完放了一个月，再上电零偏变了0.5°/h。所以，组合导航中一定要把零偏作为状态量在线估计，不能只用出厂标定值。
3.6 本章知识体系
下面这张图总结了本章的核心逻辑：
这张图把本章的核心逻辑串起来了：加速度计和陀螺仪是传感器层，输出原始数据；比力方程和姿态更新是算法层，把原始数据转换成导航信息；误差模型是补偿层，告诉你这些数据有多“脏”，以及怎么处理。
好了，这一章的内容就到这儿。记住：理解IMU的误差模型，比理解它的工作原理更重要。因为在实际工程中，你很少需要自己造IMU，但你天天都要跟IMU的误差打交道。
公众号:蓝海资料掘金营，微信deep3321