IMU 预积分（Pre-integration）-20

这是一个非常棒的话题。如果说 李群李代数 是 SLAM 的数学基石，那么 IMU 预积分（IMU Pre-integration） 就是视觉惯性里程计（VIO）能够实时运行的工程核心。

这项技术最早在 2016 年由 Christian Forster (GTSAM 作者之一) 系统性提出，随后被 VINS-Mono、ORB-SLAM3 等主流算法采用。

我们先从“为什么要搞预积分？”这个痛点开始。

1. 痛点：传统积分在优化中太慢了

在后端优化（Bundle Adjustment）中，我们会构建一个因子图（Factor Graph）。
假设有两个关键帧： 时刻的状态 和 时刻的状态 。这两帧之间可能有 100 个 IMU 数据。

传统方法的困境

如果我们用刚才讲的“普通积分”，约束关系是这样的（以位置 为例）：

问题出在 上：
在优化过程中，只要我们调整了起始帧 的姿态 （这是常有的事），后面那一长串的积分项 虽然是基于 IMU 数据的，但它是在世界坐标系下积分的，所以必须重新算一遍。

  • 后果：优化迭代一次，就要把几百个 IMU 数据重新积分一次。如果是个大图优化，CPU 直接爆炸。

2. 核心思想：把“积分”和“初始状态”剥离

预积分的魔法在于：我们能不能算出一个只跟 IMU 数据有关，跟 无关的量？

我们将上述公式两边同乘 （也就是把世界坐标系转换到 时刻的自身坐标系）：

定义右边这一坨为 预积分量（Pre-integrated terms），记为 。

现在的优势：

  • ** 只有 IMU 数据决定**。
  • 无论优化器怎么调整 ，** 都不需要重新计算！** 它变成了一个固定的测量值（就像两帧之间的一把尺子）。

3. 三个核心预积分量

除了位置，我们需要处理旋转和速度。最终我们得到三个与绝对位姿无关的量：

  1. 旋转预积分量 ()：
    只看这两帧之间 IMU 转了多少度。

  2. 速度预积分量 ()：
    累加加速度，但排除了重力影响。

  3. 位置预积分量 ()：

这样，优化时的误差函数（Residual）就变成了：

\[\text{Error} = [\text{状态推算的相对量}] \ominus [\text{预积分测量值 } \boldsymbol{\alpha}, \boldsymbol{\beta}, \Delta R] \]

4. 进阶难点：Bias 变了怎么办？

细心的你可能发现了，上面的公式里有 **零偏 **。
在优化过程中，除了优化位姿，我们通常也会优化 Sensor Bias（因为 Bias 会随温度漂移）。

问题：如果优化器修改了 Bias 的估计值，预积分量 还是得重算啊？因为积分里用到了 Bias。

解决方案：一阶泰勒展开（The Jacobian Trick）

这是预积分最精彩的一笔。我们假设 Bias 的变化量 很小，那么我们可以线性近似更新预积分量，而不用重算积分：

这里的 是预积分关于零偏的雅可比矩阵。

  • 在我们最开始积分 IMU 数据生成 时，顺便把这个雅可比矩阵也算好存起来。
  • 当后面优化调整 Bias 时，直接用公式做一个简单的矩阵乘法补正即可。

5. 代码逻辑（伪代码/VINS风格）

为了让你理解如何在代码中实现，这里展示一个精简版的类结构：

class IMUPreintegration {
public:
    // 构造函数：需要这一帧开始时的 Bias
    IMUPreintegration(Vector3d acc_bias, Vector3d gyro_bias) 
        : b_a(acc_bias), b_g(gyro_bias) {
        // 初始化预积分量为 0 和 单位阵
        sum_P.setZero();
        sum_V.setZero();
        delta_R.setIdentity();
        // 初始化雅可比矩阵为 0
        J.setZero(); 
    }

    // 每来一个 IMU 数据调用一次
    void push_back(double dt, Vector3d acc, Vector3d gyro) {
        // 1. 获取去除 Bias 后的真实测量
        Vector3d un_acc = acc - b_a;
        Vector3d un_gyro = gyro - b_g;

        // 2. 积分旋转 (中值积分，使用 Sophus)
        // delta_R_next = delta_R_curr * Exp(omega * dt)
        Sophus::SO3d dR = Sophus::SO3d::exp(un_gyro * dt);
        delta_R = delta_R * dR;

        // 3. 积分速度 (注意：要在当前局部坐标系下累加)
        // 这里的 delta_R 是上一步的累积旋转
        Vector3d acc_world_proxy = delta_R * un_acc; 
        sum_V += acc_world_proxy * dt;

        // 4. 积分位置
        sum_P += sum_V * dt + 0.5 * acc_world_proxy * dt * dt;

        // 5. 【高能】同时迭代更新雅可比矩阵 F 和 V
        // J = F * J + V; (公式非常复杂，通常直接抄 VINS 的推导)
        updateJacobian(dt, un_acc, un_gyro);
    }

    // 当优化器改变了 bias 时，用这个函数快速修正预积分量
    void correct(Vector3d delta_ba, Vector3d delta_bg) {
        sum_P += J_P_ba * delta_ba + J_P_bg * delta_bg;
        sum_V += J_V_ba * delta_ba + J_V_bg * delta_bg;
        // 旋转的修正稍微复杂一点，是右乘一个小量
        delta_R = delta_R * Sophus::SO3d::exp(J_R_bg * delta_bg);
    }

    // 成员变量：预积分出的测量值
    Sophus::SO3d delta_R;
    Vector3d sum_P, sum_V;
    Vector3d b_a, b_g;
    MatrixXd J; // 庞大的雅可比矩阵
};

6. 总结：IMU 预积分的作用

想象你坐在高铁上（关键帧 i），你想知道走到车厢另一头（关键帧 j）相对于车厢的位置。

  1. 普通积分：你需要知道高铁在地球上的绝对速度和位置，结合你在车里的步伐，算出你在地球上的位置。如果有人修改了高铁的时刻表（ 变了），你得重新算你在地球哪儿。
  2. 预积分：你不管高铁在哪，你只记录“我在车厢里向前走了 10 米，向左转了 90 度”。这个数据（）是死的。以后不管高铁是在北京还是上海（ 怎么变），你这“10米、90度”的相对量永远不用变。

技术小结：

  1. 坐标系转换：将积分转换到上一帧的 Body Frame。
  2. 消除重力：显式地将重力项 保留在残差公式中，而不放入预积分。
  3. 泰勒展开：利用雅可比矩阵处理 Bias 的微小变化，避免重积分。

这就是 VINS-Mono 等系统能够在手机端流畅运行后端非线性优化的秘密武器。

到这里，关于旋转表达、李代数求导、IMU 积分的理论链路就打通了。接下来你是想看这套理论如何应用在具体的算法框架（比如 Kalman Filter vs 图优化）中，还是想了解关于后端优化（高斯牛顿法）的细节？

posted @ 2026-01-09 10:27  jack-chen666  阅读(7)  评论(0)    收藏  举报