企业官网建设流程全解析

做视觉惯性里程计、激光惯性里程计或者纯惯性导航的同学，大概率都被“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_{k+1} &= R_k \cdot \mathrm{Exp}\big((\tilde{\omega}k - b_g)\Delta t\big)\ v{k+1} &= v_k + g_w\Delta t + R_k(\tilde{a}k - b_a)\Delta t\ p{k+1} &= 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_w=0)，但加速度计读数不是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_{k+1} &= \Delta R_k \cdot \mathrm{Exp}\big((\tilde{\omega}k - b_g)\Delta t\big)\ \Delta v{k+1} &= \Delta v_k + \Delta R_k(\tilde{a}k - b_a)\Delta t\ \Delta p{k+1} &= \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}{k+1}}{2} - b_g\ a_k &= \frac{\tilde{a}k + \tilde{a}{k+1}}{2} - b_a\ \Delta R_{k+1} &= \Delta R_k \cdot \mathrm{Exp}(\omega_k \Delta t)\ \Delta v_{k+1} &= \Delta v_k + \Delta R_k a_k \Delta t\ \Delta p_{k+1} &= \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_{k+1} = 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_{k+1} = 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 常见问题速查表

6.4 一个小技巧：单元测试预积分

最后分享一个非常实用的调试技巧。预积分涉及大量矩阵运算和旋转操作，而且和坐标系约定耦合很深，写完代码后不要直接怼到SLAM系统里跑。更稳的做法是单独写一个测试：给一段常值角速度和常值比力输入，用高精度数值积分（比如四阶龙格库塔）算出参考预积分量，再和你实现的预积分递推对比。如果两者误差在合理范围内，说明递推逻辑基本正确；然后再单独验证零偏雅可比，用数值差分和解析雅可比对比。这套测试流程看起来很基础，但能帮你省下大量排查系统级bug的时间。

我个人在实际项目里的体会是：预积分本身的数学并不比SLAM后端更难，但它的实现非常容易出“静默错误”——代码不崩溃，残差也计算了，但优化结果就是不对。这种问题排查起来很痛苦，因为问题往往在某个旋转约定的符号上。所以，从最小用例开始验证，每一步都打出来看，是效率最高的方式。等预积分模块真正稳定了，你再去回头读论文，会发现很多之前抽象的符号都有了具体的图像感，整个VIO/LIO系统的骨架也就清楚了大半。

最后再给一个建议：如果你用的是VINS-MONO或ORB-SLAM3这类开源系统，不要只停留在“会调参”，花一个周末把各自的IntegrationBase或ImuTypes源码逐行读懂，对照这篇推导过程，把协方差递推和雅可比更新的每一行注释清楚。这件事做完，你对预积分的理解会超过大多数只会跑开源框架的人。