4. IMU惯性导航：加速度计与陀螺仪原理、捷联惯导解算、IMU误差模型
各位同学，咱们今天聊聊IMU。说实话，IMU这玩意儿在自动驾驶里，就像人的小脑——它不负责“看路”，但负责“感知身体姿态”。没有它，你的车就是个瞎子，连自己正着还是歪着都不知道。
我刚开始做定位时，总觉得IMU就是个辅助，GPS才是老大。直到有一次在隧道里做测试，GPS信号全丢，车却靠着IMU稳稳开了两公里。嗯，从那以后，我再也不敢小看它了。
4.1 加速度计与陀螺仪：IMU的两大核心
IMU里装了两个关键传感器：加速度计和陀螺仪。它们各自干各自的活，但配合起来才能算出完整的姿态。
4.1.1 加速度计：测量“比力”
加速度计测的不是你通常理解的“加速度”。它测的是“比力”——也就是物体受到的惯性力与重力的合力。说白了，它告诉你“我现在被推着往哪个方向走”。
举个例子：你站在电梯里，电梯加速上升时，你会感觉身体变重。加速度计测到的就是这个“变重”的感觉。在静止状态下，它测到的就是重力加速度g，方向竖直向下。
我个人习惯把加速度计的输出写成这样：
a_meas = a_true + g + b_a + n_a其中：
a_true是真实的运动加速度g是重力加速度（9.81 m/s²）b_a是零偏（bias）n_a是噪声
注意，加速度计没法区分“运动加速度”和“重力加速度”。所以当你用它来算位置时，必须先减去重力分量。我在项目中遇到过有人直接积分加速度计输出，结果位置飞到了天上去——因为没去掉重力。
4.1.2 陀螺仪：测量角速度
陀螺仪测的是物体绕三个轴的旋转角速度。单位是 rad/s 或 °/s。它告诉你“我现在转得有多快”。
陀螺仪的原理，说白了就是科里奥利效应。一个振动的质量块，当它旋转时，会受到一个垂直于振动方向和旋转轴方向的力。测这个力的大小，就能算出角速度。
陀螺仪的输出模型类似：
ω_meas = ω_true + b_g + n_g其中：
ω_true是真实角速度b_g是零偏n_g是噪声
陀螺仪有个特点：它不怕重力，但怕温度变化。温度一变，零偏就漂。我曾经在夏天做路测，车在太阳下晒了半小时，陀螺仪的零偏直接翻了一倍。所以，温度补偿是必须做的。
4.2 捷联惯导解算：从原始数据到位置姿态
有了加速度和角速度，怎么算出位置和姿态？这就是捷联惯导解算（Strapdown Inertial Navigation）干的事。
“捷联”的意思是，IMU直接“绑”在车体上，不像老式平台惯导那样用物理平台保持水平。所以，解算全靠数学。
整个流程分三步：
- 姿态更新：用陀螺仪的角速度更新姿态
- 速度更新：用加速度计的输出，去掉重力，积分得到速度
- 位置更新：对速度积分得到位置
下面这张图是我画的捷联惯导解算流程，你一看就明白：
你看，整个流程就是“积分套积分”。陀螺仪积分得到姿态，加速度计去重力后积分得到速度，速度再积分得到位置。每一步都有误差累积，所以IMU单独用不了多久就会漂。
4.2.1 姿态更新的数学表达
姿态更新最常用的方法是四元数。为什么用四元数？因为欧拉角有万向锁问题，旋转矩阵有9个参数太冗余。四元数只有4个参数，没有奇点，完美。
四元数更新公式：
q_new = q_old ⊗ exp(0.5 * ω * dt)其中：
q是四元数，表示从载体坐标系到导航坐标系的旋转ω是角速度向量dt是采样时间间隔⊗表示四元数乘法
我建议你直接用现成的库，比如 Eigen 或 ROS 里的 tf2。手写四元数乘法容易出错，我当年就踩过坑——符号搞反了，结果车头朝北，解算出来朝南。
4.2.2 速度与位置更新
速度更新时，最关键的一步是“去重力”。加速度计测到的比力包含重力，必须用当前姿态把重力投影到载体坐标系，然后减去。
v_new = v_old + (R(q) * a_meas - g) * dt其中 R(q) 是四元数对应的旋转矩阵，把加速度从载体坐标系转到导航坐标系。
位置更新就简单了：
p_new = p_old + v_new * dt嗯，这里要注意：IMU的采样频率通常很高（100Hz~1000Hz），所以积分步长很小。但高频也意味着计算量大，嵌入式平台上要优化。
4.3 IMU误差模型：为什么IMU会漂？
IMU不是完美的。它有很多误差源，导致解算结果随时间发散。我把它归纳为三类：
| 误差类型 | 来源 | 影响 | 典型值 |
|---|---|---|---|
| 零偏（Bias） | 制造工艺、温度变化 | 姿态和位置随时间线性漂移 | 陀螺仪：0.1~10 °/h 加速度计：1~10 mg |
| 比例因子（Scale Factor） | 传感器非线性 | 测量值与真实值成比例偏差 | 0.1%~1% |
| 随机噪声（Random Noise） | 电子噪声、量化误差 | 高频抖动，积分后变为随机游走 | 角随机游走：0.01~0.1 °/√h 速度随机游走：0.01~0.1 m/s/√h |
其中，零偏是最头疼的。它会导致姿态误差随时间线性增长，而姿态误差又会导致重力投影错误，进而让速度和位置误差二次方增长。这就是为什么纯IMU定位几分钟就废了。
4.3.1 零偏的建模与补偿
零偏不是常数，它会随时间缓慢变化。通常建模为一阶马尔可夫过程：
b_dot = -b / τ + w其中：
τ是相关时间常数w是高斯白噪声
在实际系统中，我们会在IMU静止时做“零偏标定”。让IMU静置几分钟，取平均值作为零偏估计值。但注意，这个值只对当前温度有效。温度一变，零偏就变。
4.3.2 艾伦方差分析
怎么评估一个IMU的好坏？用艾伦方差（Allan Variance）。它能把不同时间尺度上的噪声成分分离出来。
简单说，你让IMU静止，采集几小时数据。然后计算不同时间间隔下的方差，画成对数图。你会看到：
- 短时间（<1s）：主要是角度随机游走（噪声）
- 中等时间（1s~100s）：主要是零偏不稳定性
- 长时间（>100s）：主要是速率随机游走（零偏漂移）
我曾经用艾伦方差分析过一款国产IMU，发现它的零偏不稳定性标称是0.5°/h，但实际测出来是2°/h。嗯，标称值看看就好，自己测才靠谱。
4.4 本章小结
IMU是自动驾驶定位的“内耳”。它不依赖外部信号，但误差会累积。加速度计和陀螺仪各有各的脾气，捷联惯导解算就是靠数学把它们拧在一起。误差模型告诉我们：IMU不能单独用太久，必须配合其他传感器（比如GPS、视觉）做融合。
下一章，我们会讲怎么把IMU和GPS融合在一起，用卡尔曼滤波来抑制IMU的漂移。嗯，那才是真正的重头戏。