3. 传感器基础（下）
好，咱们接着聊IMU。上一节我们把IMU的测量模型和噪声特性讲清楚了，这一节要啃的是一块硬骨头——IMU预积分。说实话，我第一次接触这个概念时也觉得头大，但后来在实际项目中用顺手了，才发现它真的是多传感器融合里的“神兵利器”。
3.1 为什么需要IMU预积分？
先问一个问题：在激光SLAM或者视觉SLAM中，IMU数据通常以100Hz甚至更高频率涌入，而激光雷达或相机可能只有10Hz。如果你每次优化都要重新积分所有IMU数据，那计算量会爆炸。更关键的是——每次优化调整位姿时，IMU的积分起点就变了，你得重新算一遍。
我早期做的一个项目里，就因为这个原因导致优化速度慢得离谱。后来用了预积分，才把实时性提上来。
预积分的核心思想：把IMU测量值相对于某个参考时刻（比如第i帧）的积分结果独立出来，变成一个“相对运动约束”。这样当优化调整第i帧的位姿时，预积分结果可以直接复用，不用重新积分。
3.2 离散时间下的IMU预积分推导
我们先从离散时间入手。IMU的测量值通常是离散采样的，假设时间间隔为Δt。
IMU的测量模型（忽略地球自转等小量）：
ω̃ = ω + b_g + η_g
ã = a + g + b_a + η_a其中ω是角速度，a是加速度，g是重力向量，b_g、b_a是零偏，η_g、η_a是高斯白噪声。
离散时间下，从时刻i到j的预积分量定义为：
ΔR_ij = ∏_{k=i}^{j-1} Exp((ω̃_k - b_g) · Δt)
Δv_ij = ∑_{k=i}^{j-1} ΔR_ik · (ã_k - b_a) · Δt
Δp_ij = ∑_{k=i}^{j-1} [Δv_ik · Δt + 0.5 · ΔR_ik · (ã_k - b_a) · Δt²]这里有个关键点：预积分量只依赖于IMU测量值和零偏，与绝对位姿无关。这就是它名字里“预”字的由来——你可以提前算好，等着优化时直接用。
我的经验：在实际代码中，我习惯把预积分量设计成一个类，内部维护ΔR、Δv、Δp以及对应的协方差矩阵。这样每次优化迭代时，只需要根据零偏更新预积分量，而不是重新积分所有原始数据。
3.3 连续时间下的IMU预积分推导
连续时间版本更优雅，也更适合理论分析。假设IMU测量值在时间区间[t_i, t_j]上连续，预积分量满足以下微分方程：
ΔṘ(t) = ΔR(t) · (ω̃(t) - b_g)^∧
Δv̇(t) = ΔR(t) · (ã(t) - b_a)
Δṗ(t) = Δv(t)初始条件：ΔR(t_i) = I，Δv(t_i) = 0，Δp(t_i) = 0。
连续时间的好处是，你可以用数值积分方法（比如RK4）来逼近，精度比简单的欧拉积分高不少。我在一个高精度定位项目里就用了RK4积分，效果明显优于欧拉法。
注意：连续时间推导中，我们假设了零偏在预积分区间内保持不变。如果零偏变化剧烈，需要分段处理。我曾经踩过这个坑——零偏估计不准导致预积分结果发散，整个优化都崩了。
3.4 IMU预积分的C++/Eigen实现
好了，理论说完了，咱们直接上代码。我习惯用Eigen库做矩阵运算，配合Sophus处理李群李代数。
// IMU预积分器类
class IMUPreintegrator {
public:
// 构造函数：初始化零偏
IMUPreintegrator(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba)
: bg_(bg), ba_(ba) {
delta_R_ = Sophus::SO3d();
delta_v_ = Eigen::Vector3d::Zero();
delta_p_ = Eigen::Vector3d::Zero();
cov_ = Eigen::Matrix<double, 9, 9>::Zero();
}
// 积分一个IMU测量值
void Integrate(const IMUData& imu, double dt) {
// 去零偏
Eigen::Vector3d gyro = imu.gyro - bg_;
Eigen::Vector3d acc = imu.acc - ba_;
// 更新旋转预积分
Sophus::SO3d dR = Sophus::SO3d::exp(gyro * dt);
delta_R_ = delta_R_ * dR;
// 更新速度预积分
Eigen::Vector3d acc_world = delta_R_ * acc;
delta_v_ += acc_world * dt;
// 更新位置预积分
delta_p_ += delta_v_ * dt + 0.5 * acc_world * dt * dt;
// 更新协方差（简化版，实际需要雅可比）
UpdateCovariance(gyro, acc, dt);
}
// 根据新的零偏更新预积分量（一阶近似）
void UpdateByBias(const Eigen::Vector3d& new_bg,
const Eigen::Vector3d& new_ba) {
// 计算零偏变化量
Eigen::Vector3d dbg = new_bg - bg_;
Eigen::Vector3d dba = new_ba - ba_;
// 一阶泰勒展开更新
delta_R_ = delta_R_ * Sophus::SO3d::exp(-J_R_bg_ * dbg);
delta_v_ += J_v_bg_ * dbg + J_v_ba_ * dba;
delta_p_ += J_p_bg_ * dbg + J_p_ba_ * dba;
bg_ = new_bg;
ba_ = new_ba;
}
private:
Sophus::SO3d delta_R_; // 旋转预积分
Eigen::Vector3d delta_v_; // 速度预积分
Eigen::Vector3d delta_p_; // 位置预积分
Eigen::Matrix<double, 9, 9> cov_; // 协方差矩阵
Eigen::Vector3d bg_, ba_; // 当前零偏
// 雅可比矩阵（用于零偏更新）
Eigen::Matrix<double, 3, 3> J_R_bg_;
Eigen::Matrix<double, 3, 3> J_v_bg_, J_v_ba_;
Eigen::Matrix<double, 3, 3> J_p_bg_, J_p_ba_;
};避坑指南：我曾经在UpdateByBias函数里忘记更新雅可比矩阵，导致零偏更新后预积分量误差越来越大。调试了整整两天才发现问题。所以记住——雅可比矩阵也要随着预积分量一起更新。
3.5 坐标系定义与转换
做多传感器融合，坐标系定义是基本功。搞错了坐标系，后面所有算法都是白搭。
我们通常涉及三个坐标系：
| 坐标系 | 符号 | 说明 |
|---|---|---|
| 世界系 | W | 通常取东北天(ENU)或北东地(NED)，重力方向对齐 |
| IMU系 | I | IMU芯片自身的坐标系，x-y-z通常符合右手定则 |
| 雷达系 | L | 激光雷达的坐标系，通常x向前，y向左，z向上 |
坐标系之间的转换关系：
// IMU到雷达的外参（需要标定）
Sophus::SE3d T_LI; // 从IMU系到雷达系的变换
// 世界系下的雷达位姿 = 世界系下的IMU位姿 * IMU到雷达的外参
Sophus::SE3d T_WL = T_WI * T_LI.inverse();
// 反过来，IMU在世界系下的位姿
Sophus::SE3d T_WI = T_WL * T_LI;重要提醒：我见过太多人在坐标系转换上翻车。一个常见的错误是搞混了“从A到B的变换”和“B在A系下的坐标”。我的建议是——统一使用变换矩阵T_AB表示“从B系到A系的变换”，这样T_AB * p_B = p_A，逻辑清晰不易出错。
3.6 预积分在融合定位中的作用
预积分最终要嵌入到因子图优化中。一个典型的IMU预积分因子长这样：
// 残差定义
Eigen::Matrix<double, 9, 1> ComputeResidual(
const Sophus::SE3d& T_WI_i, // 第i帧IMU位姿
const Sophus::SE3d& T_WI_j, // 第j帧IMU位姿
const Eigen::Vector3d& v_i, // 第i帧速度
const Eigen::Vector3d& v_j) // 第j帧速度
{
// 预积分预测的相对运动
Sophus::SO3d pred_R = delta_R_;
Eigen::Vector3d pred_v = delta_v_;
Eigen::Vector3d pred_p = delta_p_;
// 实际相对运动
Sophus::SO3d actual_R = T_WI_i.so3().inverse() * T_WI_j.so3();
Eigen::Vector3d actual_v = T_WI_i.so3().inverse() * (v_j - v_i - g * dt);
Eigen::Vector3d actual_p = T_WI_i.so3().inverse() *
(T_WI_j.translation() - T_WI_i.translation() -
v_i * dt - 0.5 * g * dt * dt);
// 残差 = 实际 - 预测
Eigen::Matrix<double, 9, 1> residual;
residual.block<3,1>(0,0) = (pred_R.inverse() * actual_R).log();
residual.block<3,1>(3,0) = actual_v - pred_v;
residual.block<3,1>(6,0) = actual_p - pred_p;
return residual;
}说白了，预积分因子就是在约束“两帧之间的相对运动应该等于IMU测量值的积分结果”。这个约束加上激光雷达的帧间匹配约束，再加上可能有的GPS约束，就构成了一个完整的因子图。
核心总结：IMU预积分的本质是把高频IMU数据“压缩”成帧间的相对运动约束，让优化问题变得高效。它不改变IMU的物理模型，只是改变了计算的组织方式。
3.7 本章知识体系
下面这张图展示了IMU预积分的完整知识结构，从理论到实现再到应用：
嗯，这一节内容确实不少。从离散时间到连续时间的推导，从C++代码实现到坐标系转换，每一步都是后面实战的基础。我个人建议你把代码亲手敲一遍，尤其是预积分器的零偏更新部分，多跑几个测试用例，感受一下不同参数对结果的影响。