做视觉惯性里程计、激光惯性里程计或者纯惯性导航的同学大概率都被“IMU预积分”这四个字折磨过。网上讲原理的文章不少但真正把预积分从零开始推一遍、把每一个符号和离散细节都理顺的教程并不多见。这篇文章就是我当初完整推导预积分时的笔记整理里面包含了我自己踩过的坑和对很多关键公式的理解。无论你是刚入门SLAM、正在跑VINS-MONO或ORB-SLAM3还是想把手里的多传感器融合系统调得更稳这篇内容都能给你一个可复现的推导路径。先说明白预积分解决什么问题在图优化框架下两帧关键帧之间往往有几十上百帧IMU数据如果每次优化迭代都重新积分一遍计算量直接爆炸。预积分的思路是把两帧之间的IMU测量打包成一个与起始状态无关的相对增量这样优化时只需要做一次“预处理”之后每次迭代用线性近似更新这个增量即可。这篇博文会从IMU测量模型讲起推导连续时间运动学、离散化递推、噪声传播、零偏雅可比和残差构建最后补充工程实践中与IMU标定、重力对齐、yaw漂移相关的经验。1. 为什么需要预积分图优化里IMU积分的尴尬1.1 传统积分的重复计算问题先回顾一下最朴素的IMU积分思路。假设在第(i)帧关键帧时刻我们已经知道载体的世界系位姿(R_i, p_i)、速度(v_i)以及IMU零偏(b\begin{bmatrix}b_g\b_a\end{bmatrix})。从(i)到(j)帧之间每隔一个采样间隔(\Delta t)用IMU测量值去更新位置、速度和姿态[ \begin{aligned} R_{k1} R_k \cdot \mathrm{Exp}\big((\tilde{\omega}k - b_g)\Delta t\big)\ v{k1} v_k g_w\Delta t R_k(\tilde{a}k - b_a)\Delta t\ p{k1} p_k v_k\Delta t \frac{1}{2}g_w\Delta t^2 \frac{1}{2}R_k(\tilde{a}_k - b_a)\Delta t^2 \end{aligned} ]这里的(R_k)是当前时刻IMU在世界系下的旋转矩阵(\tilde{\omega}_k, \tilde{a}_k)是陀螺和加速度计原始测量(g_w)是世界系下的重力向量。这套递推虽然直接但在图优化里没法用。因为(R_k, v_k, p_k)都是从第(i)帧状态开始传播的而第(i)帧的状态尤其是旋转在优化过程中会反复更新。只要(R_i)变一点后面所有的(R_k)全部跟着变这意味着每轮迭代都要把(j-i)帧之间的全部IMU测量重新积分一遍。假设关键帧间隔0.5秒IMU频率200Hz就有100帧测量图里有几百个关键帧的话这个重复计算的代价是完全不可接受的。1.2 预积分的一手思路把积分结果从绝对状态里拆出来预积分的核心想法是“换参考系”。既然问题出在初始旋转(R_i)耦合进了积分递推那我们就从(i)时刻的IMU坐标系出发把两帧之间的相对旋转、相对速度增量、相对位移增量先算出来。这些相对量只依赖IMU原始测量和零偏不依赖(R_i, p_i, v_i)。等优化需要时再用当前估计的(R_i, p_i, v_i)去组合出绝对状态相当于把“积分”这块重活从优化迭代里完全剥离出去。打个比方不预积分时每次调整起点你都得重新测绘整条路线预积分则是先把两点间相对位移测好并画成一张表起点变了你用“起点坐标相对表项”就能快速算出终点不需要重新跑一遍路。这个思路从2012年左右开始成熟真正被广泛引用是Forster等人在2015年发表的论文把流形上的预积分推导做得很完整也是如今主流VIO/LIO系统的标准做法。2. 推导前的基础IMU测量模型与连续时间运动学2.1 加速度计测的到底是什么很多人一开始搞混加速度计模型加速度计测的是“比力”specific force不是运动加速度。简单说加速度计的敏感质量感受到的是载体相对惯性空间的加速度减去重力加速度。用公式表达[ \tilde{a}^b R_{wb}^T(a_w - g_w) b_a n_a ]下标(w)表示世界系上标(b)表示IMU体坐标系。(R_{wb})是世界系到体坐标系的旋转矩阵(a_w)是载体的真实加速度(g_w)是世界系重力向量。静止放置时(a_w0)但加速度计读数不是0而是(-R_{wb}^T g_w)也就是大概9.8 m/s²的反向重力。这一点在初始化阶段做重力对齐时很重要。陀螺仪模型比较简单[ \tilde{\omega}^b \omega^b b_g n_g ]这里(\omega^b)是真实角速度在体坐标系下的表示。噪声(n_g, n_a)通常建模为高斯白噪声零偏(b_g, b_a)建模为随机游走也就是零偏自身的导数是一个很小的白噪声。2.2 连续时间运动学方程在推导预积分之前要先把连续时间运动学写清楚。世界系下位置、速度、姿态的微分方程是[ \begin{aligned} \dot{p}{wb} v{wb}\ \dot{v}{wb} R{wb}\tilde{a}^b g_w\ \dot{R}{wb} R{wb}[\tilde{\omega}^b]_\times \end{aligned} ]这里([\cdot]_\times)表示三维向量的反对称矩阵。第一个式子很直观第二个式子里的(\tilde{a}^b)是比力所以要乘上旋转矩阵变回世界系再加回重力第三个式子是SO(3)上的运动学方程描述旋转矩阵随时间的变化率。这里有一个容易混淆的点为什么速度方程里是(R_{wb}\tilde{a}^b)而不是(\tilde{a}^b R_{wb})因为加速度计测量的是体坐标系下的向量要先旋转到世界系才能和重力向量相加。而旋转矩阵的微分方程里角速度向量乘在(R_{wb})的右边这对应“体坐标系下的角速度左乘一个反对称矩阵”是旋转矩阵右扰动约定的自然结果。3. 核心推导从重复积分到预积分3.1 相对运动量的定义现在进入正题。假设两个关键帧时刻(i)和(j)我们想构造一个只依赖IMU测量和零偏的相对增量。先定义三组预积分量[ \begin{aligned} \Delta R_{ij} R_i^T R_j\ \Delta v_{ij} R_i^T (v_j - v_i - g_w\Delta t_{ij})\ \Delta p_{ij} R_i^T \left(p_j - p_i - v_i\Delta t_{ij} - \frac{1}{2}g_w\Delta t_{ij}^2\right) \end{aligned} ]注意这里的(\Delta t_{ij})是两帧之间的总时间间隔。这些定义是从“绝对状态应该满足的关系”反推出来的。如果IMU测量是理想无噪声的那么(\Delta R_{ij}, \Delta v_{ij}, \Delta p_{ij})就等于我们把IMU测量从(i)时刻积分到(j)时刻得到的相对量。为什么要把速度增量和位移增量都左乘一个(R_i^T)因为这样才能把世界系下的增量转到(i)时刻的IMU坐标系从而消除对(R_i)的依赖。优化时(R_i)更新了这些预积分量不必重算只要在残差里用新的(R_i^T)去变换即可。3.2 离散时间下的预积分递推实际代码里处理的都是离散IMU采样。从(i)帧到(j)帧之间记第(k)个测量为(\tilde{\omega}_k, \tilde{a}_k)时间间隔为(\Delta t)。预积分量的递推关系为[ \begin{aligned} \Delta R_{k1} \Delta R_k \cdot \mathrm{Exp}\big((\tilde{\omega}k - b_g)\Delta t\big)\ \Delta v{k1} \Delta v_k \Delta R_k(\tilde{a}k - b_a)\Delta t\ \Delta p{k1} \Delta p_k \Delta v_k\Delta t \frac{1}{2}\Delta R_k(\tilde{a}_k - b_a)\Delta t^2 \end{aligned} ]初始条件为(\Delta R_i I)(\Delta v_i 0)(\Delta p_i 0)。这里(\mathrm{Exp})是SO(3)上的指数映射把旋转向量转成旋转矩阵。可以看到递推过程中完全没有出现(R_i, v_i, p_i)这就是解耦的关键。从离散形式可以看出一个重要的工程细节陀螺零偏出现在旋转增量里加速度零偏出现在速度和位移增量里但加速度零偏也会通过(\Delta R_k)间接影响速度增量——因为每步的加速度测量要先用当前相对旋转转到当前参考系。这正是后面零偏雅可比里会出现交叉项的原因。3.3 中值积分 vs 欧拉积分上面给出的是最简单的欧拉积分。实际工程中VINS-MONO和ORB-SLAM3等系统普遍使用中值积分也就是取相邻两帧IMU测量的平均值作为这一段时间内的代表值[ \begin{aligned} \omega_k \frac{\tilde{\omega}k \tilde{\omega}{k1}}{2} - b_g\ a_k \frac{\tilde{a}k \tilde{a}{k1}}{2} - b_a\ \Delta R_{k1} \Delta R_k \cdot \mathrm{Exp}(\omega_k \Delta t)\ \Delta v_{k1} \Delta v_k \Delta R_k a_k \Delta t\ \Delta p_{k1} \Delta p_k \Delta v_k \Delta t \frac{1}{2}\Delta R_k a_k \Delta t^2 \end{aligned} ]中值积分比欧拉积分精度高不少实现又简单所以是主流选择。要不要上四阶龙格库塔我的建议是没必要。IMU采样频率通常100到500Hz关键帧之间每步(\Delta t)很小中值积分已经足够四阶龙格库塔的额外开销不仅体现在计算量上还会让代码复杂度上升收益却微乎其微。除非你处理的是超高动态场景或者IMU频率很低低于50Hz否则中值积分就是性价比最高的方案。4. 噪声传播协方差矩阵怎么递推4.1 预积分量的误差从哪来预积分量是IMU测量值的函数而测量里含有白噪声(n_g, n_a)所以预积分量本身也是一个随机变量。在图优化里我们把预积分量当成一个“虚拟测量残差”参与优化的信息矩阵必须知道这个虚拟测量的不确定性否则权重就错了。于是需要在预积分递推的同时把噪声的协方差也递推出来。把预积分量写成“理想值噪声”的形式[ \begin{aligned} \Delta R_{ij} \overline{\Delta R}{ij}\cdot \mathrm{Exp}(\delta\phi)\ \Delta v{ij} \overline{\Delta v}{ij} \delta v\ \Delta p{ij} \overline{\Delta p}_{ij} \delta p \end{aligned} ]理想值(\overline{\Delta R}, \overline{\Delta v}, \overline{\Delta p})是对应无噪声测量的结果(\delta\phi, \delta v, \delta p)是噪声引起的误差量。注意旋转误差用的是(\mathrm{Exp}(\delta\phi))这种乘法扰动形式而不是简单的加法因为SO(3)是一个流形旋转量的误差必须用李代数表示。4.2 线性化误差递推把含噪声的测量代入上节的递推式做一阶泰勒展开忽略二阶以上小量误差递推可以整理成标准的线性形式[ \delta x_{k1} A_k \delta x_k B_k n_k ]其中(\delta x \begin{bmatrix}\delta\phi\\delta v\\delta p\end{bmatrix})(n_k \begin{bmatrix}n_g\n_a\end{bmatrix})。在工程上很多系统采用的简化形式如下以VINS-MONO的midIntegration为参考[ A_k \begin{bmatrix} I - [\omega_k\Delta t]\times 0 0\ -\Delta R_k [a_k\Delta t]\times I 0\ -\frac{1}{2}\Delta R_k [a_k\Delta t^2]_\times I\Delta t I \end{bmatrix} ][ B_k \begin{bmatrix} I\Delta t 0\ 0 \Delta R_k\Delta t\ 0 \frac{1}{2}\Delta R_k\Delta t^2 \end{bmatrix} ]这里(\omega_k, a_k)是去零偏后的角速度和加速度。注意这几个小块的具体位置和符号不同论文、不同开源代码的左右手性约定可能有差别用的时候一定要对照原文核清楚。协方差递推公式就是标准的卡尔曼形式[ P_{k1} A_k P_k A_k^T B_k Q B_k^T ]初始协方差(P_i 0)因为起点是确定的关键帧状态预积分的起点没有不确定性。(Q)是IMU测量噪声的协方差矩阵由陀螺仪和加速度计的噪声密度决定。这里的(A_k)和(B_k)每一帧IMU测量都要更新一次所以预积分类的内部通常会缓存一个中间协方差矩阵逐帧累乘。4.3 BCH公式和SO(3)右雅可比在推导旋转误差的递推时绕不开BCH公式。简单说SO(3)上两个旋转向量相加不能像普通向量那样直接相加而是有修正项。最常用的一阶近似是[ \mathrm{Exp}(\varphi \Delta\varphi) \approx \mathrm{Exp}(\varphi)\cdot\mathrm{Exp}\big(J_r(\varphi)\Delta\varphi\big) ]其中(J_r(\varphi))是SO(3)的右雅可比矩阵。当(\varphi)很小时(J_r(\varphi) \approx I - \frac{1}{2}[\varphi]_\times)。在预积分推导里每个(\Delta t)内的旋转增量都很小所以很多简化实现直接用这个近似误差可以忽略。但如果你要推导完整的协方差递推尤其是要考虑陀螺噪声对位移增量的交叉影响就必须用完整的右雅可比公式。坦白说手动推导BCH和伴随映射很容易把自己绕晕。我的经验是正式写代码前先看成熟实现是怎么做的。VINS-MONO的IntegrationBase里有一份完整的“误差递推协方差递推”伪代码照着实现一遍再回头去看Forster论文很多符号就对应上了。单纯从零推导可以锻炼数学能力但工程落地时站在成熟实现肩膀上会快得多。5. 零偏更新预积分量如何“一阶修正”5.1 为什么零偏更新不能重新积分预积分量虽然不依赖起始位姿但它是零偏(b_g, b_a)的函数。图优化过程中零偏作为状态变量会被不断更新。如果每次零偏更新后都要重新把全部IMU测量积一遍那预积分节省的计算量又全白费了。所以预积分量对零偏的处理方式是在优化迭代中采用一阶泰勒展开用零偏更新量(\delta b)线性修正预积分量而不是重新积分。具体做法是在预积分递推过程中同步维护几个雅可比矩阵[ \begin{aligned} \Delta R(b_g \delta b_g) \approx \Delta R(b_g)\cdot\mathrm{Exp}\left(\frac{\partial \Delta R}{\partial b_g}\delta b_g\right)\ \Delta v(b \delta b) \approx \Delta v(b) \frac{\partial \Delta v}{\partial b_g}\delta b_g \frac{\partial \Delta v}{\partial b_a}\delta b_a\ \Delta p(b \delta b) \approx \Delta p(b) \frac{\partial \Delta p}{\partial b_g}\delta b_g \frac{\partial \Delta p}{\partial b_a}\delta b_a \end{aligned} ]其中旋转增量对陀螺零偏的雅可比(\frac{\partial \Delta R}{\partial b_g})是一个3×3矩阵(\frac{\partial \Delta v}{\partial b_g})、(\frac{\partial \Delta v}{\partial b_a})、(\frac{\partial \Delta p}{\partial b_g})、(\frac{\partial \Delta p}{\partial b_a})分别是速度增量和位移增量对陀螺/加速度零偏的雅可比。这些矩阵的初值都是零每处理一帧IMU测量时按链式法则更新。5.2 雅可比递推怎么实现完整的雅可比递推公式看着很长但实现起来其实就是4~5条赋值语句。以常见的实现风格为例每步要做的更新大致是// 伪代码每处理一帧测量更新预积分量和雅可比 update(gyro, acc, dt) { // 1. 更新 ΔR, Δv, Δp中值积分或欧拉积分 deltaR deltaR * Exp((gyro - bg) * dt); deltaV deltaV deltaR * (acc - ba) * dt; deltaP deltaP deltaV * dt 0.5 * deltaR * (acc - ba) * dt * dt; // 2. 更新旋转雅可比dR/dbg dR_dbg dR_dbg - Jr((gyro - bg) * dt) * dt; // 3. 更新速度雅可比dv/dbg, dv/dba dV_dbg dV_dbg ...; // 由旋转雅可比和加速度项组合 dV_dba dV_dba - deltaR * dt; // 4. 更新位移雅可比dp/dbg, dp/dba dP_dbg dP_dbg ...; // 由速度雅可比累加 dP_dba dP_dba dV_dba * dt - 0.5 * deltaR * dt * dt; // 5. 更新协方差矩阵 covariance A * covariance * A.transpose() B * Q * B.transpose(); }这里的省略号表示旋转雅可比对速度、位移雅可比的交叉贡献项完整表达式在Forster论文和VINS源码里都有。我写这个伪代码是想说明一个关键点雅可比不是优化时才数值差分的而是在预积分的过程中逐帧递推得到的。这样优化时零偏更新只需一次矩阵乘法就能完成修正非常快。5.3 一阶修正在什么时候会失效一阶修正有一个前提零偏的变化量不能太大。在系统刚启动、零偏初值完全没初始化时优化算法可能会把零偏推向一个离预积分假设值很远的位置这时候线性外推就不再可靠。所以在实际系统中预积分对象在零偏发生较大变化后会被重置。比如VINS-MONO里如果零偏更新量超过阈值就把预积分对象重新构建一次ORB-SLAM3的IMU初始化阶段也会专门处理零偏初值。这就是为什么零偏初始化做得好不好会直接决定系统启动阶段稳不稳。这个坑我实际踩过第一次实现时为了图省事全程不做预积分重置结果优化发散。后来加了“零偏变化超过阈值就重新预积分”的逻辑系统马上就稳了。阈值一般取陀螺零偏0.01~0.02 rad/s、加速度零偏0.01~0.02 m/s²量级具体看传感器质量。6. 落到SLAM系统预积分残差、初始化和工程教训6.1 预积分残差的经典形式预积分量最终要变成图优化里的边连接两个关键帧节点。在(i)帧和(j)帧之间残差定义为理想增量与实际增量之差。一个常见的实现形式是[ \begin{aligned} r_R \mathrm{Log}\left[\left(\Delta R_{ij}\cdot\mathrm{Exp}\left(\frac{\partial \Delta R}{\partial b_g}\delta b_g\right)\right)^T R_i^T R_j\right]\ r_v R_i^T(v_j - v_i - g_w\Delta t_{ij}) - \left(\Delta v_{ij} \frac{\partial \Delta v}{\partial b}\delta b\right)\ r_p R_i^T\left(p_j - p_i - v_i\Delta t_{ij} - \frac{1}{2}g_w\Delta t_{ij}^2\right) - \left(\Delta p_{ij} \frac{\partial \Delta p}{\partial b}\delta b\right) \end{aligned} ]解释一下这三个残差的物理含义。(r_R)是旋转残差左边括号里是先对预积分旋转做零偏修正再与(R_i^T R_j)这个“实际相对旋转”比较。如果两者一致旋转残差是单位矩阵取对数映射后为零向量。(r_v)是速度残差(R_i^T(v_j - v_i - g_w\Delta t_{ij}))是把世界系下的理论速度增量转到(i)帧IMU坐标系再减去预积分速度增量。(r_p)是位移残差同理。注意这里每一项都左乘了(R_i^T)让残差在(i)坐标系下比较这也是预积分能避免重复积分的关键原因之一。在代码实现里这个残差块会进入g2o、ceres或者GTSAM。需要特别小心的是旋转残差的左右手性(\mathrm{Log})和(\mathrm{Exp})的约定、残差是左扰动还是右扰动不同库有不同的习惯直接用错会导致雅可比符号反了优化完全跑不动。我的建议是先写一个单个残差的单元测试用一个人为构造的小例子验证残差和雅可比是否满足有限差分一致性。6.2 工程实践重力对齐、外参标定和时间同步预积分本身不涉及重力对齐但整个系统要跑通重力向量的准确度直接影响速度残差和位移残差的正确性。初始化阶段常用的一种方法是让IMU静止一段时间取加速度计的平均值作为重力向量在IMU坐标系下的反向测量从而确定世界系与IMU系之间的pitch和roll。这就是常说的IMU重力对齐。注意这种方法只能确定两个自由度绕重力轴的yaw角不可观所以后续yaw的漂移要交给视觉、激光或磁力计来约束。外参标定也很关键。预积分是在IMU坐标系下做的但视觉或雷达的位姿通常对应相机或雷达坐标系两者之间的外参如果标定得不好等效于给预积分残差注入一个系统性偏差。做相机IMU联合标定可以用Kalibr这类工具做lidar imu外参标定也有专门的开源方案。我强烈建议在SLAM系统上线前先把外参标定扎实尤其是旋转外参。标定时注意IMU和相机/雷达时间戳要对齐时间偏移即便只有几毫秒也会在高速运动时产生明显误差。如果你是在Carsim这类仿真工具里设置虚拟IMU传感器注意仿真输出的加速度也是比力方向和真实IMU一致坐标系定义一定要跟算法端对齐。仿真数据的噪声参数通常和真实传感器差异很大拿仿真参数直接套真实硬件往往不合适但反过来仿真器对验证算法流程和极端工况非常有帮助。6.3 常见问题速查表现象可能原因检查方法解决办法yaw持续缓慢漂移重力对齐只能锁定pitch/rollyaw不可观外参有误差或时间不同步也会加剧漂移将IMU静止观察z轴角速度积分是否随时间线性增大对比视觉/雷达里程计的yaw增加绝对航向约束视觉/激光/磁力计重新标定外参检查时间戳偏移预积分残差始终很大零偏初值不准坐标系定义不一致旋转残差左右手性错误打印预积分前后相对位姿与真值对比用有限差分检查雅可比重新做IMU初始化对照论文检查旋转约定重置预积分对象初始化阶段轨迹发散零偏初值离真值太远一阶修正失效看零偏估计值是否跳出合理范围优化前先用静止段数据估计零偏初值零偏变化超过阈值时重建预积分协方差矩阵异常噪声密度参数错误递推矩阵A/B填错用Allan方差重新估计噪声与随机游走打印协方差轨迹观察是否合理重新标定IMU噪声参数对照源码检查误差递推矩阵6.4 一个小技巧单元测试预积分最后分享一个非常实用的调试技巧。预积分涉及大量矩阵运算和旋转操作而且和坐标系约定耦合很深写完代码后不要直接怼到SLAM系统里跑。更稳的做法是单独写一个测试给一段常值角速度和常值比力输入用高精度数值积分比如四阶龙格库塔算出参考预积分量再和你实现的预积分递推对比。如果两者误差在合理范围内说明递推逻辑基本正确然后再单独验证零偏雅可比用数值差分和解析雅可比对比。这套测试流程看起来很基础但能帮你省下大量排查系统级bug的时间。我个人在实际项目里的体会是预积分本身的数学并不比SLAM后端更难但它的实现非常容易出“静默错误”——代码不崩溃残差也计算了但优化结果就是不对。这种问题排查起来很痛苦因为问题往往在某个旋转约定的符号上。所以从最小用例开始验证每一步都打出来看是效率最高的方式。等预积分模块真正稳定了你再去回头读论文会发现很多之前抽象的符号都有了具体的图像感整个VIO/LIO系统的骨架也就清楚了大半。最后再给一个建议如果你用的是VINS-MONO或ORB-SLAM3这类开源系统不要只停留在“会调参”花一个周末把各自的IntegrationBase或ImuTypes源码逐行读懂对照这篇推导过程把协方差递推和雅可比更新的每一行注释清楚。这件事做完你对预积分的理解会超过大多数只会跑开源框架的人。