3. 定位基础：IMU原理与预积分技术，IMU噪声模型与标定
IMU这东西，说白了就是自动驾驶的“小脑”。视觉和激光雷达是眼睛，但眼睛会花、会看不清。IMU不会。它每秒几百次告诉你“我在加速”、“我在转”，哪怕车开进隧道、被大卡车挡住视线，它依然在默默工作。
我最早接触IMU是在做无人机的时候。那时候不懂噪声模型，直接拿原始数据做积分，结果飞了不到十秒就漂到天边去了。嗯，从那以后我才真正重视起IMU的原理和标定。
3.1 IMU测量原理与坐标系
IMU内部有两个核心传感器：加速度计和陀螺仪。加速度计测量比力（specific force），陀螺仪测量角速度。注意，加速度计测的不是纯运动加速度，而是包含了重力分量。
核心公式：
a_m = R^T * (a_w - g) + b_a + n_a
ω_m = ω_w + b_g + n_g其中a_m是测量值，a_w是世界系下的真实加速度，g是重力向量。R是旋转矩阵。b_a和b_g是零偏，n_a和n_g是高斯白噪声。
坐标系这块，我个人习惯把IMU坐标系记为B系（body frame），世界坐标系记为W系。IMU输出的数据都是在B系下的，而我们要做SLAM，最终需要把轨迹对齐到W系。这个转换关系，就是后面预积分要解决的核心问题。
3.2 IMU噪声模型
IMU的噪声主要分两类：白噪声和随机游走。白噪声是高频的、不相关的；随机游走是低频的、慢慢漂移的。
| 噪声类型 | 来源 | 数学描述 | 影响 |
|---|---|---|---|
| 高斯白噪声 | 电子热噪声、量化噪声 | n ~ N(0, σ²) | 短期积分误差 |
| 零偏随机游走 | 温度漂移、机械老化 | b_dot = n_b, n_b ~ N(0, σ_b²) | 长期漂移 |
| 尺度因子误差 | 制造公差 | s ~ 1 ± δ | 比例偏差 |
| 交轴耦合 | 安装误差 | 非正交矩阵 | 轴间串扰 |
我在项目中遇到过最坑的事：一款IMU的datasheet上只给了白噪声参数，没给零偏稳定性。结果我们按白噪声建模做EKF融合，跑半小时后位置漂了上百米。后来才发现，零偏随机游走才是长时漂移的元凶。
我的经验：选IMU时，除了看角度随机游走（ARW）和速度随机游走（VRW），一定要看零偏稳定性（Bias Instability）。这个指标决定了你的系统能撑多久不重定位。
3.3 IMU标定方法
标定IMU，说白了就是估计出上面表格里的那些误差参数。常用的方法有：
- 六面法：把IMU放在六个正交方向上，利用重力已知来标定加速度计尺度因子和零偏。
- 转台标定：用高精度转台提供已知角速度，标定陀螺仪。
- 多位置静态标定：采集多个静态位置的IMU数据，用优化方法同时估计所有参数。
我个人推荐第三种。虽然计算量稍大，但不需要转台，现场就能做。我曾经在车上用这个方法标定，半小时搞定，效果和转台标定差不到5%。
注意：标定过程中一定要保证IMU温度稳定。温度变化1°C，零偏可能漂移0.01°/s。如果你在户外标定，记得等IMU预热10分钟以上。
3.4 IMU预积分技术
预积分是VINS（视觉惯性导航系统）的核心技术之一。为什么要预积分？因为视觉和IMU的频率不一样。视觉10Hz，IMU 200Hz。你不能每来一帧图像就把所有IMU数据重新积分一遍，那样计算量太大了。
预积分的思路是：在两帧图像之间，把IMU的测量值相对积分起来，得到一个相对运动增量。这个增量只和IMU数据有关，和绝对位姿无关。这样，当优化过程中绝对位姿发生变化时，预积分量不需要重新计算。
预积分核心公式：
ΔR_ij = ∏ exp((ω_k - b_g) * Δt)
Δv_ij = ∑ R_k * (a_k - b_a) * Δt
Δp_ij = ∑ (Δv_k * Δt + 0.5 * R_k * (a_k - b_a) * Δt²)其中i和j是两帧图像的索引，k是两帧之间的IMU测量序号。
你想想看，如果没有预积分，每次优化迭代都要重新积分200个IMU数据点。有了预积分，只需要维护三个增量（ΔR, Δv, Δp）和对应的协方差矩阵，计算量直接降了两个数量级。
3.5 预积分与视觉的协同
预积分量怎么和视觉观测融合？通常的做法是：
- 视觉提供两帧之间的相对位姿约束（通过特征匹配和本质矩阵分解）
- IMU预积分提供另一个相对运动约束
- 两者在因子图中作为不同的因子，共同优化
我在实际项目中遇到过一个问题：视觉和IMU的尺度不一致。视觉是单目，没有绝对尺度；IMU预积分有尺度。如果不处理好这个尺度对齐，融合结果会非常奇怪。解决办法是在优化中把尺度作为一个显式变量，和位姿一起优化。
避坑指南：我曾经在初始化阶段直接用视觉估计的深度去缩放IMU预积分，结果深度估计不准导致整个系统发散。后来改用松耦合初始化：先单独用视觉做SFM，再用IMU预积分对齐重力方向，最后联合优化尺度。这样稳定多了。
3.6 本章知识体系
下面这张图总结了IMU原理、噪声模型、标定和预积分之间的逻辑关系：
从这张图可以看得很清楚：IMU硬件产生原始测量，测量受噪声污染，标定是为了去除系统误差，预积分是为了高效利用高频数据，最后和视觉融合得到稳定位姿。每一步都环环相扣。
嗯，这一章的内容就到这里。IMU这块东西，理论看着简单，但真正用好需要大量实践。我建议你找个开源数据集，自己动手跑一遍预积分代码，感受一下噪声和标定对结果的影响。比看十遍公式都管用。