IMU预积分：从测量模型到因子图优化的完整解析

做SLAM状态估计的人，迟早会撞上"IMU预积分"这面墙。我第一次在VIO代码里看到预积分类时，第一反应是：为什么不把两帧之间的IMU数据直接积分成位姿变化当测量？认真走了一遍从测量模型到因子图优化的完整流程后，才明白预积分不是一种技巧，而是一种被频率差逼出来的必然选择。这篇文章我想把预积分的来龙去脉讲透：为什么需要它、公式是怎么推出来的、代码里怎么实现、采样率和标定这些外部因素又怎么影响它。内容主要面向做激光惯性里程计、视觉惯性里程计或无人机状态估计的同学，也适合刚接触惯性导航的初学者，前期只需要基础矩阵知识和一点优化概念。

1. 为什么要有预积分：因子图优化的频率困局

1.1 高频测量与低频优化节点之间存在数量级差异

IMU的输出频率通常在100到500Hz，视觉相机一般10到30帧，机械式激光雷达通常10Hz，固态LiDAR可能更高一些但也远达不到IMU的频次。在因子图里，位姿节点往往只建在视觉或激光关键帧上，两个关键帧之间往往夹着几十帧甚至上百帧IMU测量。更麻烦的是，如果直接把每一帧IMU数据都当独立因子放进优化图里，图的节点和边的数量会按数量级膨胀，非线性优化的线性化求解代价高到无法接受。

那能不能绕开这个问题？比如做帧间位姿预测时，只用最近一帧IMU的单次测量做短时间外推？这在数据融合里叫作松耦合思路，确实可以用，但松耦合会丢掉高频运动信息，无法提供真正有约束力的运动轨迹信息。在高动态场景下，只用单帧测量做预测，位姿预测的精度会明显下降，VIO和LIO算法都因此倾向于紧耦合方案，而紧耦合又面临处理大量高频测量的问题。预积分正是在这个矛盾里产生的妥协方案：保留IMU高频测量的全部信息，但把计算成本从"每次优化迭代都承担"转换成"一次计算，反复使用"。

1.2 没有预积分时的重复积分灾难

假设我们不用预积分，直接把两关键帧间的IMU数据当作一个"位姿增量测量"。听起来没问题，但你一旦开始做批量优化就会发现问题：位姿节点的估计值在每次迭代都会更新，而传统的IMU积分是从上一帧位姿的当前估计值出发的。位姿变，积分起点就变，那么两关键帧之间的这几十上百帧数据每次迭代都要重新积分一遍。后端优化通常要迭代几十次，每次迭代里每个关键帧对都要重复做高频积分，CPU时间全耗在重复计算上，系统实时性无从谈起。

更深一层的问题是在滑动窗口里，关键帧会被反复边缘化、重新线性化，如果积分结果对绝对位姿有依赖，那每次窗口变化都意味着重新算一遍所有相关预积分约束，逻辑复杂度和故障率都会上升。预积分把"绝对状态依赖"和"测量积分"解耦之后，这些重复计算就变得没有必要：约束是固定的，变化的部分只有对bias的轻量修正。

1.3 预积分的设计思想：一次计算，多次使用

所以预积分真正做的事情是，定义一个只依赖IMU测量值和bias的"相对运动增量"，并在关键帧间持续累加。每次来一帧IMU数据，预积分类的内部状态就更新一次；当检测到新的关键帧时，把累加好的相对增量、协方差、以及相对初始起点的时间差打包成一个预积分因子，塞进因子图。优化器迭代时，这个因子对应的残差和雅可比可以直接计算，不再需要触碰原始IMU序列。从工程角度看，这也让代码结构更干净：IMU数据流在前端做一次预处理，后端拿到的是压缩后的约束。

到这里，很多人会问：预积分量的定义具体是什么？残差又怎么构建？那就需要把IMU测量模型和运动学公式先摆出来，否则后面的推导没根基。

2. IMU测量模型与运动学方程：预积分的起点

2.1 加速度计与陀螺仪输出的是什么

先回到传感器物理层，IMU的两个核心部件输出的是比力和角速度，不是直接能用的加速度和角增量。用公式看更清楚。陀螺仪的测量值 ω_m 可以写成角速度 ω 加上慢变的偏差 b_g 和噪声 n_g：

加速度计的测量值 a_m 写出来更能体现"比力"的概念：

其中 R 是载体坐标系到世界坐标系的旋转，a_W 是世界坐标系下载体除重力以外的加速度，g_W 是世界系下的重力矢量。也就是说，加速度计静止放在桌上时，读到的数值并不是0，而是约9.8 m/s²、方向竖直向上的反作用力投影。这就是"IMU重力加速度补偿"这个热词说的东西：如果你想从加速度计读数里恢复载体的真实运动加速度，必须先把重力项加回去，而不是简单粗暴地把读数减掉9.8。

热词里还有"imu加速度怎么测量"，本质上也是在问这个模型。回答是：你没法直接从加速度计读数里分离重力和运动加速度，必须依靠外部信息（比如视觉/激光位姿估计）先确定重力方向和大小。这也是为什么所有初始化流程里都要先估计重力矢量。

2.2 偏差和噪声是预积分误差的两大来源

b_g 和 b_a 不是常数，而是随时间慢变的随机游走过程。预积分公式里都必须显式带偏差项，否则积分时间一长，一个小偏差会在二次积分中被放大成不可忽视的位置漂移。工程上，bias是被放到优化状态里去估计的，所以预积分残差最终必须对 b_g、b_a 求雅可比，这样优化器才能根据整个系统的误差反馈去修正偏差估计。

噪声方面，n_g、n_a 通常建模为白噪声。白色噪声经过一次积分会变成随机游走，经过两次积分变成布朗运动，方差随时间的立方增长。因此两个关键帧相隔越远，预积分的噪声方差越大，这是构建协方差矩阵的基础，后面第五章会说清楚。

2.3 离散化方式：欧拉积分与中值积分的差别

IMU的测量是离散的，连续运动学方程必须离散化。最简单的欧拉积分用当前时刻的测量值近似整个间隔，实现简单但精度有限。更常用的是中值积分，取 k 和 k+1 两帧测量的平均值作为间隔内的角速度和加速度代表值。实际测试中，中值积分的精度在振动环境下明显更好，尤其当载体存在较大角速度变化时，欧拉积分会产生显著的旋转误差，进而污染速度和位置的预积分。业界主流框架，例如GTSAM的预积分实现，默认采用中值积分，这也是工程实践给出的答案。到这里，测量模型和运动学的底子算是打好了，接下来正式进入预积分量的推导。

3. 预积分量的数学定义与推导过程

3.1 从两关键帧间的状态传播公式出发

设两个关键帧 i 和 j，时间间隔 Δt。世界系下，从 i 到 j 的位姿和速度传播关系写出来是这样的：

看到这个结构就能明白，ΔR_ij、Δv_ij、Δp_ij 扮演的是"本体系内的相对运动"角色：姿态变化直接是相对旋转，速度变化和位移变化则被 R_i 从本体系旋转到世界系。这些量正是我们要预积分的对象。

3.2 将绝对状态依赖从积分式中剥离

接下来定义这些量的精确表达式。以离散形式、中值积分为例：

注意这里的下标约定：ΔR_ik 是从 i 到第 k 步的累积相对旋转，它把第 k 时刻的加速度测量从当前本体系旋转到 i 时刻本体系下表示，再做累加。这个定义里完全没有出现 R_i、v_i、p_i 任何一个绝对状态，它只依赖 i 到 j 之间的IMU测量值以及 b_g、b_a。这就是"预积分"的本质：把绝对初始状态从积分过程里彻底剥离掉。

因此，预积分量可以在还没开始优化前就被计算并缓存起来。每当有新的IMU测量到达，只需要一个递推操作，用当前预积分状态更新下一个状态即可。

3.3 预积分残差的具体形式

有了 ΔR_ij、Δv_ij、Δp_ij，真正进入因子图的是它们的残差。从上一节的状态传播公式倒推，预积分残差定义如下：

三个残差分别对应旋转、速度、位移。旋转残差中的 Log 是一个映射，把旋转矩阵差异转回李代数向量。速度残差和位移残差形式上很像"测量值减去预测值"：先把优化器当前估计的位姿/速度按照运动学推导出期望的 Δv、Δp，再减去预积分给出的 Δv、Δp。当状态估计和预积分测量一致时，残差趋近于0。

到这里，预积分量本身已经完整定义。但这个定义是在 bias 固定时成立的。实际优化时 bias 是不断更新的，那 Δ量是不是要跟着变？这正是第四章要解决的问题。

4. 偏差随机游走与预积分修正：工程落地的关键

4.1 优化器更新Bias时预积分量怎么办

b_g 和 b_a 作为优化状态的一部分，每一轮迭代都会拿到新的增量。预积分量严格来说是在旧bias（记为 b̄）下积出来的，如果直接把新bias带进去，预积分结果就会失配。最暴力的做法是每次bias一变，就把两帧之间所有IMU测量重新积分一遍，但这个"重积分"恰恰是预积分当初要消灭的重复计算，等于回到原点。于是就有了折中方案：用一阶泰勒展开近似bias扰动对预积分量的影响。

4.2 预积分量对Bias的一阶雅可比

对任意预积分量 x（可以是 ΔR、Δv 或 Δp），当bias的扰动为 δb 时，做一阶近似：

这些雅可比矩阵 J_R、J_v_δb_g、J_v_δb_a、J_p_δb_g、J_p_δb_a 可以在预积分递推过程中同步计算出来，不需要事后做数值差分。GTSAM 的内部实现就是在 integrateMeasurement 里随状态一起更新这些雅可比。使用这套雅可比后，优化器每一轮迭代只需要根据bias增量的预测做一次矩阵乘法和指数映射，就把预积分量拉回到与当前bias一致的状态，避免了重积分。

4.3 修正策略的两种写法

实际工程上有两种处理方式。第一种是"修正后使用"：优化迭代结束拿到新的bias之后，用上式修正缓存的预积分量，更新基准bias，保证下一次残差计算的输入与最新bias一致。第二种是"残差中显式建模"：预积分量维持旧bias版本，但在残差函数里把bias变化量作为额外的雅可比支路传给优化器。两种方式在数学上等价，区别在于实现位置。我见过不少代码在第二种方式上漏掉雅可比项，导致bias估计长期不收敛，表现症状就是加速度计bias被推到很大，轨迹出现缓慢漂移。如果你在调类似系统，建议优先选第一种，逻辑清晰也容易排查。

5. 噪声传播与协方差传递：给残差一把可靠的尺子

5.1 线性化噪声传播过程

预积分不只是算一个均值，还要算不确定性。因为预积分对噪声是线性的（在流形上做线性化），可以用经典的高斯传播公式：

其中 A 是对状态变量的雅可比，B 是对噪声的雅可比。这里的"状态变量"指预积分内部状态（ΔR、Δv、Δp），噪声指每一帧IMU测量的白噪声。积分间隔越长、包含的测量帧越多，累积的协方差就越大，这个规律正好对应前文说的噪声经过多次积分被放大。

工程上还有一个细节：真实IMU的噪声特性需要先标定。把IMU静止放在桌面上采集一小时数据，用Allan方差分析可以分离出角度随机游走、速度随机游走、bias不稳定性等分量。把标定出的噪声参数填进预积分的噪声矩阵，协方差这才有物理意义。如果偷懒用一个经验值矩阵，预积分因子的权重就只是"大约可用"，系统收敛后往往会有可见的误差。

5.2 信息矩阵与优化的物理含义

因子图优化里，残差进入最小二乘时会乘一个权重矩阵，这个权重就是预积分协方差的逆，也就是信息矩阵。信息矩阵的数值直接决定了这条约束在全局优化中的话语权：两帧很近、IMU测量很多时，预积分置信度高，信息矩阵元素大，约束权重也大；两帧离得远、测量时间长，信息矩阵元素小，约束权重变弱。这个物理属性是自动的，前提是协方差传播正确。如果你把协方差固定成单位阵，等于让所有预积分约束不论间隔长短都有相同权重，这在快速运动和大间隔场景下会引入错误倾向，可能让系统更信任一条本应很弱的长间隔约束。

6. 从预积分到完整系统：采样率、标定与重力补偿的耦合问题

6.1 IMU采样率不足的影响有多严重

热词里有个高频疑问：无人机IMU采样率达不到200Hz会造成什么影响。从预积分的角度看，采样率决定了离散积分的步长和积分近似的精度。中值积分假设每个间隔内角速度和加速度近似线性变化，采样率越低，每个间隔内的真实运动可能偏离这个假设越多，尤其在高机动、急停急转的场景下，近似误差会直接体现在预积分残差上。更隐蔽的问题是频域混叠：IMU测量是离散采样，高频振动如果超过采样率一半，会折返成低频分量，污染bias估计和重力方向估计。我在实际测试中遇到过一模一样的情况，电机高速旋转时状态估计出现周期性抖动，把采样率从100Hz提到250Hz之后，抖动明显缓解，但残差和协方差完全收敛的代价是新的硬件成本。如果你的飞行器只有100到150Hz的IMU，尽量在机载端做一定的低通滤波或过采样平均，减少混叠的注入量。

6.2 LiDAR-IMU外参与内参标定对预积分的直接影响

预积分残差把IMU测量当作在IMU本体系内的量，而LiDAR或相机点云是落在各自传感器坐标系下的。外参标定（常见的lidar imu标定）给出传感器坐标系之间的旋转和平移。外参误差虽然不直接进预积分公式，但会经过"把IMU测量投影到其他坐标系"这个环节污染所有与视觉/激光观测相关的残差。最终表现往往是：预积分约束本身没问题，但它和LiDAR/相机约束互相矛盾，优化器在两者之间反复折中，轨迹出现弯曲。内参标定问题更直接：尺度因子和轴偏差会改变IMU读数的数值，也就是改变预积分量本身的准确性。热词里的"imu内参的标定""imu校准"说的就是这件事。所以调系统时，若预积分残差长期偏大，先检查标定，再怀疑算法。

6.3 重力矢量处理是预积分残差的隐含变量

回到前面提到的比力模型，预积分残差里 v 和 p 的公式都带 g_W。g_W 一般被当作可估计变量放进系统状态，初始化时通过静止段估计出来。重力矢量初始化不准，影响不是"每条残差加一个常数"那么简单：接收优化器更新时，g 会参与残差计算并形成系统性的方向偏差。VIO系统要求静止初始化，本质就是在让加速度计先通过比力测量把重力方向估计出来。开机后如果没做静止初始化，预积分约束会与重力方向矛盾，轨迹一开始就斜着漂。这是很多新手调试时最容易忽略的一个点。

7. 代码实现与调试经验：直接可用的工程建议

7.1 预积分类的状态划分

写预积分类的时候，内部状态至少需要这几块：预积分量本身（ΔR、Δv、Δp）、协方差矩阵、对bias的雅可比、累计时间、参考时刻的IMU测量（用于中值积分）。接口上，我习惯提供三个核心方法：integrateMeasurement 处理新到的IMU数据并更新内部状态；reset 在关键帧建立后清零；recompute 在bias更新后重算雅可比修正。做完这三个方法，预积分类就可以在VIO、LIO框架里独立复用，也方便单元测试。这里一个小的工程建议：reset 的时机一定要放在新关键帧真正被选中的瞬间，如果在预积分过程中被别的模块误调用，数据会被截断，约束就丢了，这类bug很难从日志里查出来。

7.2 代码实现中我踩过的几个坑

第一个坑是协方差忘记更新。有些实现的预积分类只更新 Δ量，忽略了协方差传播，导致因子图中信息矩阵一直是初值，系统对长时间间隔的约束权重失真。第二个坑是对bias雅可比使用数值差分但没有控制步长，造成海森矩阵病态，优化收敛慢。第三个坑是IMU测量坐标系和加速度坐标系方向不一致，比如陀螺仪横滚方向为正、加速度计Z轴向下为正，这类约定问题会让预积分残差出现固定偏差，检查方式很简单：让载体静止不动，看预积分产生的 Δv 是否为0，如果没有静止初始化或坐标约定错误，会看到一个常值漂移。

7.3 优化器对接与调试的观察方法

因子图对接上，GTSAM 有现成 PreintegratedImuMeasurements 类型，Ceres 也有示例，自己写也不难，核心就是提供残差函数和雅可比。调试时最有效的观察方式是拿到每个预积分因子的残差占比：如果某个关键帧对的旋转残差长期大于0.1 rad，而视觉或激光残差很小，可以先怀疑数据时间戳不同步，再怀疑外参标定错误。时间戳抖动对预积分的破坏往往被低估，IMU数据和视觉数据的时间轴要是对不齐，哪怕差10毫秒，在快速运动下预积分残差都会明显偏大，这也是我调试LIO系统时最终定位到的一个隐蔽问题。

最后说点我个人体会。预积分这套理论发展到今天，算法本身已经很成熟，真正让系统精度拉开差距的，往往不是预积分代码写得好不好，而是输入的数据干不干净：bias初始化、重力估计、内外参标定、采样率、时间同步。遇到IMU积分发散，别第一时间重写预积分，先把数据源查一遍。这个习惯帮我少走了很多弯路。