4. 预积分理论基础

在视觉惯性里程计（VIO）与惯性导航系统中，IMU 以数百赫兹的频率输出角速度与比力，而视觉观测通常只有 10～30 赫兹。若把每一帧之间的所有 IMU 采样都作为状态量参与优化，计算量将不可接受。预积分的核心思想，就是把两帧图像之间成百上千次 IMU 测量，压缩成一个与起始状态解耦的相对运动约束。理解预积分，必须先理解它背后的连续时间运动学、离散时间积分，以及预积分量本身的物理意义。

一、连续时间运动学：预积分的物理源头

在惯性坐标系下，载体的姿态、速度、位置满足如下连续时间微分方程。设 R 为从机体系到世界系的旋转矩阵，ω 为机体系角速度，a 为机体系比力，g 为世界系重力：

Ṙ(t) = R(t) · [ω(t)]×
v̇(t) = R(t) · a(t) + g
ṗ(t) = v(t)

这三个方程揭示了 IMU 测量的本质：陀螺仪测量的是机体系角速度，加速度计测量的是机体系比力（包含重力反作用）。姿态 R 出现在速度方程中，意味着速度与位置的演化依赖于姿态，而姿态又由角速度积分得到。这种耦合关系正是预积分需要处理的关键难点。

二、离散时间积分：从微分方程到可计算形式

实际系统中，IMU 以固定采样周期 Δt 输出测量值。常用的离散积分方法包括欧拉法、中值法和四阶龙格库塔法。以中值法为例，在 ti 到 ti+1 之间：

ω_mid = 0.5 · (ω_i + ω_{i+1})
a_mid = 0.5 · (a_i + a_{i+1})

R_{i+1} = R_i · Exp(ω_mid · Δt)
v_{i+1} = v_i + (R_i · a_mid + g) · Δt
p_{i+1} = p_i + v_i · Δt + 0.5 · (R_i · a_mid + g) · Δt²

其中 Exp(·) 表示 SO(3) 上的指数映射。可以看到，若直接按上式递推，每一时刻的速度和位置都显式依赖初始的 R_i、v_i、p_i。在优化过程中，一旦起始状态被调整，所有中间积分都必须重新计算，这正是预积分要解决的问题。

三、预积分量的定义：把起始状态“提”出来

预积分的做法是：把世界系下的积分式重新整理，将起始状态因子提取到积分号外，只对机体系测量进行积分。定义两帧 i 与 j 之间的预积分量为：

ΔR_ij = ∏_{k=i}^{j-1} Exp((ω_k - b_g) · Δt)
Δv_ij = Σ_{k=i}^{j-1} ΔR_ik · (a_k - b_a) · Δt
Δp_ij = Σ_{k=i}^{j-1} [ Δv_ik · Δt + 0.5 · ΔR_ik · (a_k - b_a) · Δt² ]

这三个量 ΔR_ij、Δv_ij、Δp_ij 分别表示相对旋转、相对速度和相对位置增量。它们只依赖两帧之间的 IMU 测量、零偏估计和采样周期，而与起始帧的世界系姿态、速度、位置无关。因此，在优化中调整起始状态时，预积分量无需重新积分，只需重新组合即可。

四、物理意义与工程价值

预积分量的物理意义可以这样理解：ΔR_ij 是载体从 i 到 j 在机体系下累积的旋转；Δv_ij 是在起始机体系下累积的速度变化；Δp_ij 是在起始机体系下累积的位置变化。它们描述的是“相对运动”，而不是“绝对运动”。

这种相对性带来了巨大的工程价值。在基于因子图的 VIO 中，预积分因子连接两个关键帧状态节点，其残差形式为：

r_ΔR = Log( ΔR_ij^T · R_i^T · R_j )
r_Δv = R_i^T · (v_j - v_i - g·Δt_ij) - Δv_ij
r_Δp = R_i^T · (p_j - p_i - v_i·Δt_ij - ½g·Δt_ij²) - Δp_ij

优化器调整 R_i, v_i, p_i, R_j, v_j, p_j 时，ΔR_ij, Δv_ij, Δp_ij 保持不变，只需重新计算残差，无需重新积分。这正是预积分能够大幅加速优化的根本原因。

五、常见误区与复盘

总结来说，连续时间运动学给出了物理规律，离散时间积分给出了可计算形式，而预积分量的定义则通过数学变换实现了与起始状态的解耦。三者层层递进，构成了 IMU 预积分理论的基础。掌握这一基础，才能进一步理解预积分因子的雅可比推导、零偏更新策略以及在 VIO 系统中的实际部署。