4. 预积分核心概念

在IMU融合定位中，最朴素的做法是把陀螺仪和加速度计的输出直接积分，得到姿态、速度和位置，再与视觉或激光观测一起送入滤波器或优化器。然而，当系统采用因子图优化时，直接积分会带来一个致命问题：每次优化迭代改变状态量，所有历史IMU数据都必须重新积分。对于一段持续数秒、包含数百个采样点的IMU序列，这种重复计算会让优化器寸步难行。预积分正是为解决这一矛盾而提出的核心思想。

一、直接积分的困境：为什么不能“边优化边重积分”

IMU的机械编排方程在连续时间下可写为：

ṗ = v
v̇ = R·(a_m - b_a) + g
Ṙ = R·[ω_m - b_g]×

其中 p、v、R 分别是位置、速度、姿态，a_m、ω_m 是加速度计和陀螺仪测量值，b_a、b_g 是零偏，g 是重力矢量。若从时刻 i 积分到时刻 j，位置和速度的增量依赖于起始姿态 R_i、起始速度 v_i 以及重力 g。在因子图中，R_i、v_i、g 都是待优化变量，每次迭代更新后，积分结果都会变化，必须重新计算。更糟糕的是，零偏 b_a、b_g 也在优化，而它们直接出现在积分核里，导致积分结果对零偏高度敏感。

二、预积分的核心动机：把“状态相关”变成“测量相关”

预积分的思路是：将IMU积分结果中与起始状态相关的部分剥离出去，只保留与IMU测量和零偏相关的相对量。这样，在优化过程中，只要零偏没有发生大变化，预积分量就可以作为“伪测量”直接使用，无需重新积分。当零偏更新时，只需用一阶雅可比进行线性修正，而不是完整重积分。

具体来说，定义相对旋转、相对速度和相对位置增量：

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

这三个量只依赖于IMU测量、零偏和时间间隔，与起始状态 R_i、v_i、p_i 以及重力 g 解耦。在因子图中，它们构成连接状态节点 i 和 j 的二元约束，残差可写为：

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_ij、Δv_ij、Δp_ij 保持不变，只有状态节点和零偏在更新。这就是预积分最本质的价值。

三、连续时间预积分公式推导

从时刻 i 到 j，对机械编排方程两边同时左乘 R_i^T，并利用 R(t) = R_i · ΔR(t) 的关系，可以得到连续时间下的预积分微分方程：

ΔṘ(t) = ΔR(t) · [ω_m(t) - b_g]×
Δv̇(t) = ΔR(t) · (a_m(t) - b_a)
Δṗ(t) = Δv(t)

初始条件为 ΔR(i) = I，Δv(i) = 0，Δp(i) = 0。注意，重力 g 已经从预积分量中完全消失，它只在后续与状态组合时出现。这正是预积分“解耦”的关键：重力、起始速度、起始位置都不参与预积分量的计算。

四、离散时间预积分公式与实现

实际IMU以固定频率采样，常用中值积分或欧拉积分进行离散化。以中值积分为例，假设相邻采样时刻 k 和 k+1 之间的角速度和加速度取平均值：

ω_avg = ½ · [ (ω_k - b_g) + (ω_{k+1} - b_g) ]
a_avg = ½ · [ (a_k - b_a) + (a_{k+1} - b_a) ]

ΔR_{k+1} = ΔR_k · Exp( ω_avg · Δt )
Δv_{k+1} = Δv_k + ΔR_k · a_avg · Δt
Δp_{k+1} = Δp_k + Δv_k · Δt + ½ · ΔR_k · a_avg · Δt²

其中 Exp(·) 是SO(3)指数映射。中值积分比欧拉积分精度更高，在低成本MEMS IMU上尤为明显。下表对比了两种离散化方式的特点：

五、工程案例：VIO中的预积分因子

以VINS-Mono为例，其因子图中每个关键帧之间插入一个IMU预积分因子。预积分量在关键帧确定后立即计算并缓存，后续优化迭代中不再重新积分。当零偏更新时，利用预积分量对零偏的一阶雅可比 ∂ΔR/∂b_g、∂Δv/∂b_a、∂Δp/∂b_a 进行线性修正：

ΔR_ij_corrected ≈ ΔR_ij · Exp( ∂ΔR/∂b_g · δb_g )
Δv_ij_corrected ≈ Δv_ij + ∂Δv/∂b_a · δb_a + ∂Δv/∂b_g · δb_g
Δp_ij_corrected ≈ Δp_ij + ∂Δp/∂b_a · δb_a + ∂Δp/∂b_g · δb_g

这种“一次积分 + 雅可比修正”的策略，使得VIO后端优化可以在毫秒级完成，而直接积分方案往往需要数十毫秒甚至更久。

六、常见误区与复盘

• 误区一：预积分量完全不受零偏影响。实际上预积分量是在某个零偏估计值下计算的，零偏变化时必须修正。忽略修正会导致因子残差系统性偏大。
• 误区二：重力可以随意放入预积分。重力必须从预积分量中剥离，否则预积分量将依赖重力方向，失去“相对测量”的意义。
• 误区三：离散化方法无关紧要。在低成本IMU上，欧拉积分与中值积分的姿态漂移差异可达数度每秒，必须根据IMU频率和精度选择合适方法。
• 误区四：预积分只适用于VIO。预积分同样适用于激光-惯性里程计、GNSS-惯性组合导航等任何需要将IMU约束嵌入因子图的场景。

总结来说，预积分的本质是将IMU积分中与全局状态相关的部分剥离，保留与测量和零偏相关的相对量，从而在因子图优化中实现“一次积分、多次复用、线性修正”。理解连续时间微分方程与离散时间递推公式之间的对应关系，是正确实现预积分因子的前提。