4. 数学基础回顾（三）：IMU测量模型与运动学
好，咱们继续往下走。前面聊完了坐标系和旋转，今天要啃的这块骨头，是VIO系统里最核心的传感器——IMU。说白了，IMU就是你的“身体感知”，它告诉你“我动了多少”。但问题是，这玩意儿噪声大、漂移快，你得先搞清楚它的脾气，才能用好它。
我个人习惯，在讲任何传感器模型之前，先问自己三个问题：它测的是什么？它怎么测的？它有什么毛病？ 搞明白这三点，后面的算法才能落地。
4.1 加速度计模型
加速度计测的是什么？很多人第一反应是“加速度”。嗯，对了一半。它测的其实是比力（Specific Force），也就是物体受到的惯性力减去重力。
数学上写出来是这样的：
a_m = a_true - g + b_a + n_a其中：
a_m：加速度计的实际读数a_true：物体真实的运动加速度g：重力加速度（注意符号，通常取向上为正）b_a：加速度计偏置（Bias），随时间缓慢变化n_a：高斯白噪声
这里有个关键点：静止时，加速度计读数是重力加速度的反方向。我刚开始做VIO时，调试IMU数据，发现静止时读数不是0，吓了一跳。后来才反应过来——它测的是重力。你想想看，如果静止时读数是0，那你怎么知道地面在哪？
核心理解：加速度计不能区分重力加速度和运动加速度。这是IMU的“原罪”，也是VIO需要视觉来帮忙的原因之一。
4.2 陀螺仪模型
陀螺仪就相对直白一些。它测的是角速度，也就是物体绕自身轴旋转的快慢。
模型长这样：
ω_m = ω_true + b_g + n_g其中：
ω_m：陀螺仪读数ω_true：真实角速度b_g：陀螺仪偏置n_g：高斯白噪声
陀螺仪的偏置问题比加速度计更头疼。加速度计偏置至少还能靠重力方向来约束，陀螺仪的偏置会直接导致姿态积分发散。我记得有一次做无人机飞控，陀螺仪偏置没校准好，起飞后飞机直接来了个“托马斯全旋”……嗯，从那以后我再也不敢忽略bias初始化了。
避坑指南：我曾经在数据集上跑VIO，发现轨迹越跑越歪。排查了三天，最后发现是陀螺仪bias初始值设成了0。实际上，消费级IMU的陀螺仪bias通常在0.01~0.1 rad/s的量级，不初始化的话，几秒钟姿态就漂没了。
4.3 IMU运动积分
有了测量模型，下一步就是怎么用IMU数据来推算运动。说白了，就是积分。
给定IMU的角速度和加速度测量值，我们要更新三个量：
- 姿态：用陀螺仪积分
- 速度：用加速度积分，但要扣除重力
- 位置：对速度再积分
离散时间下的更新公式（假设时间间隔为Δt）：
// 姿态更新（四元数形式）
q_{k+1} = q_k ⊗ exp( (ω_m - b_g) * Δt / 2 )
// 速度更新
v_{k+1} = v_k + (R_k * (a_m - b_a) - g) * Δt
// 位置更新
p_{k+1} = p_k + v_k * Δt + 0.5 * (R_k * (a_m - b_a) - g) * Δt²这里有个细节：加速度积分必须在世界坐标系下进行。IMU测到的加速度是在机体坐标系下的，你得先用当前姿态把它旋转到世界系，再减去重力，才能积分出速度。
我见过不少新手犯这个错——直接把IMU的加速度读数积分，结果位置飞到了外太空。你想想看，如果IMU在水平放置时，z轴读数是9.8 m/s²，你不扣掉重力，积分一秒就“飞”了4.9米。
4.4 预积分的动机
好，现在问题来了。上面这种积分方式，每次更新姿态、速度、位置时，都需要知道当前的姿态R_k。但在VIO里，我们通常是在优化框架中处理——比如滑动窗口或者图优化。优化过程中，姿态会反复调整，每次调整，所有IMU积分都要重算，计算量爆炸。
这就是预积分（Preintegration）出现的动机。
预积分的核心思想是：把IMU测量值在机体坐标系下预先积分，得到一个相对运动增量。这个增量不依赖于绝对姿态，只依赖于IMU的测量值和bias。优化时，只需要用当前估计的姿态去“修正”这个增量就行了。
一句话总结：预积分把IMU积分从“绝对依赖姿态”变成了“相对独立于姿态”，大幅提升了优化效率。
具体来说，预积分量包括：
- ΔR：相对旋转增量
- Δv：相对速度增量
- Δp：相对位置增量
这些量只和IMU测量值以及bias有关。当bias变化时，我们可以用一阶线性近似来更新预积分量，而不需要重新积分。
4.5 知识体系总览
说了这么多，咱们用一张图把这一章的核心逻辑串起来：
这张图把咱们今天讲的内容串起来了。从底层的传感器模型，到运动积分，再到预积分的动机。说白了，预积分就是解决“优化时IMU积分太慢”这个工程痛点而生的。
注意事项：预积分虽然高效，但它假设bias变化缓慢。如果IMU的bias突然跳变（比如温度剧烈变化），预积分的线性近似会失效。实际工程中，建议在IMU数据预处理阶段做温度补偿。
好了，这一章就到这里。IMU的测量模型和运动积分是VIO的基石，理解透了，后面预积分的推导才能顺风顺水。我个人建议你动手写个小脚本，把IMU数据读进来，手动积分一下，看看漂移有多快——实践出真知。