3. 传统IMU积分方法：从欧拉到RK4，以及那个让人头疼的误差问题
好，咱们进入正题。上一章我们聊了IMU的测量模型和噪声特性，说白了就是知道了传感器会给我们什么数据，以及这些数据有多“脏”。那拿到数据之后呢？最直接的想法——积分呗。把角速度积分成姿态，把加速度积分成速度，再把速度积分成位置。听起来简单，但这里面的坑，我踩过不少。
今天我就把几种传统积分方法掰开揉碎了讲。欧拉积分、中值积分、RK4积分，还有那个绕不开的误差累积和发散问题。嗯，咱们一个一个来。
3.1 欧拉积分：最朴素，但也最“糙”
欧拉积分，说白了就是“用上一时刻的值，加上当前时刻的变化量”。数学上很简单：
// 欧拉积分伪代码
// 姿态更新：四元数形式
q_new = q_old + 0.5 * dt * q_old * omega_old;
// 速度更新
v_new = v_old + dt * a_old;
// 位置更新
p_new = p_old + dt * v_old;你看，代码就这么几行。但我要告诉你，实际项目中我几乎不用它。为什么？因为欧拉积分假设在dt时间内，角速度和加速度是恒定不变的。你想想看，IMU的数据频率一般是100Hz到1000Hz，dt在1ms到10ms之间。这段时间内，如果机器人剧烈运动，角速度和加速度变化很快，欧拉积分就会引入明显的误差。
3.2 中值积分：一个简单有效的改进
中值积分，顾名思义，就是用“中间时刻”的值来近似整个dt时间段内的变化。具体做法是：取当前时刻和下一时刻的测量值的平均值，作为这个dt内的“代表值”。
// 中值积分伪代码
// 姿态更新
omega_mid = (omega_old + omega_new) / 2;
q_new = q_old + 0.5 * dt * q_old * omega_mid;
// 速度更新
a_mid = (a_old + a_new) / 2;
v_new = v_old + dt * a_mid;
// 位置更新
p_new = p_old + dt * v_old + 0.5 * dt * dt * a_mid;我个人习惯在大多数场景下使用中值积分。它比欧拉积分精度高一个阶次（二阶精度），但计算量增加很少。我在做地面机器人导航时，用中值积分配合100Hz的IMU，在10秒内姿态误差能控制在0.5度以内，对于大部分应用已经够用了。
3.3 RK4积分：精度至上，但代价不小
RK4，全称四阶龙格-库塔法。它是数值积分里的“优等生”，精度高，稳定性好。但代价是——计算量大约是欧拉积分的4倍。
// RK4积分伪代码（以姿态更新为例）
function rk4_quaternion(q, omega, dt):
k1 = 0.5 * q * omega(t)
k2 = 0.5 * (q + 0.5*dt*k1) * omega(t + 0.5*dt)
k3 = 0.5 * (q + 0.5*dt*k2) * omega(t + 0.5*dt)
k4 = 0.5 * (q + dt*k3) * omega(t + dt)
q_new = q + (dt/6) * (k1 + 2*k2 + 2*k3 + k4)
return q_new你看，每个时间步要计算4次中间状态。在嵌入式系统上，如果IMU频率是1000Hz，CPU可能就有点吃不消了。我记得有一次在STM32F4上跑RK4，结果CPU占用率飙到了80%，其他任务都没法跑了。后来换成了中值积分，CPU占用率降到20%，精度损失在可接受范围内。
所以我的建议是：除非你的应用对精度要求极高（比如高精度惯性导航），否则没必要上RK4。中值积分在大多数场景下是性价比最高的选择。
3.4 积分误差累积分析：为什么IMU积分会越跑越偏？
这个问题，说白了就是“误差会累积”。IMU的测量值有噪声和偏差，积分一次，误差就累积一次。我们来做个简单的分析：
| 误差来源 | 对姿态的影响 | 对速度的影响 | 对位置的影响 |
|---|---|---|---|
| 角速度噪声 | 线性增长 | 无直接影响 | 间接影响（通过姿态） |
| 加速度噪声 | 无直接影响 | 线性增长 | 二次方增长 |
| 角速度零偏 | 二次方增长 | 无直接影响 | 间接影响 |
| 加速度零偏 | 无直接影响 | 线性增长 | 二次方增长 |
你看，位置误差对加速度零偏是二次方增长的。这意味着什么？假设你的加速度计有0.01 m/s²的零偏（这已经是很不错的传感器了），积分10秒后，位置误差大约是0.5米；积分100秒后，误差会达到50米！
核心结论： IMU积分误差随时间呈指数级增长（至少是多项式增长）。纯IMU积分无法长时间保持精度，必须引入外部观测（如视觉、GPS、激光雷达）来修正。
3.5 长时间积分发散问题：一个真实的案例
我曾经参与过一个地下矿井的定位项目。矿井里没有GPS信号，我们想用IMU做纯惯性导航。刚开始测试时，我们用了高精度的光纤陀螺IMU（价格够买一辆车），心想这总该没问题了吧？
结果呢？跑了5分钟，位置误差就超过了10米。为什么？因为即使是最好的IMU，也有零偏和噪声。在长时间积分下，这些微小的误差会被放大到不可接受的程度。
我后来总结了几条经验：
- 不要指望纯IMU积分能跑超过1分钟——除非你用的是军用级IMU（价格六位数以上）
- 零偏是最大的敌人——想办法在线估计并补偿零偏，这是IMU预积分要解决的核心问题之一
- 积分方法只能延缓发散，不能消除发散——欧拉积分发散最快，中值积分稍好，RK4最好，但最终都会发散
3.6 本章小结：一张图看懂传统IMU积分
说了这么多，我画了一张图来总结传统IMU积分的核心逻辑和误差传递路径。你看完应该能有个整体印象。
这张图把整个流程串起来了。你看，从陀螺仪和加速度计的数据输入，经过三种积分方法之一处理，输出姿态、速度、位置。但每个环节都带着误差，最终导致位置误差二次方增长，长时间必然发散。
嗯，这就是传统IMU积分的本质。下一章我们会聊IMU预积分——它正是为了解决“长时间积分发散”这个问题而生的。不过那是后话了，先把今天的内容消化掉。
公众号：蓝海资料掘金营，微信deep3321