4、IMU与多传感器融合：IMU预积分、ESKF滤波、LIO-SAM框架解析

多传感器融合，说白了就是让机器人「睁着眼睛走路」。相机和激光雷达是眼睛，IMU就是内耳前庭——告诉你身体怎么晃。但IMU有个毛病：积分漂移。你让它算位置，十秒后它能飘到天上去。所以得跟其他传感器绑在一起，互相兜底。

这一章我们聊三个核心：IMU预积分、ESKF滤波、LIO-SAM框架。最后我会把紧耦合和松耦合的区别讲透。嗯，都是我在实际项目中踩过坑的地方。

传统做法是把IMU积分结果当成状态量，每次优化都要重新积分。这在图优化里是个灾难——每调整一次位姿，所有IMU测量都得重算一遍。

预积分的核心思想：把IMU测量值在机体坐标系下预先积分，得到一个相对运动增量。这个增量跟位姿无关，只跟两帧之间的IMU数据有关。优化时只需要把预积分结果当成一个约束边，不用重新积分。

预积分公式（离散形式）：

ΔR_ij = ∏(exp((ω_k - b_g) * Δt))
Δv_ij = Σ(ΔR_ik * (a_k - b_a) * Δt)
Δp_ij = Σ(Δv_ik * Δt + 0.5 * ΔR_ik * (a_k - b_a) * Δt²)

我在项目里遇到过一个问题：预积分对偏置（bias）非常敏感。偏置估计不准，整个预积分结果就偏了。后来我养成了一个习惯——每次初始化时先让IMU静止2秒，算一个初始偏置，再开始预积分。

我的经验：预积分的时间窗口不要太大。一般控制在0.5-1秒之间。窗口太长，偏置漂移累积会吃掉精度；窗口太短，又失去了预积分的意义。

ESKF（Error-State Kalman Filter）是IMU融合里的经典方案。它不直接估计状态，而是估计状态误差。为什么这么做？因为IMU的噪声模型在误差空间里更接近高斯分布，线性化效果更好。

ESKF的核心流程分三步：

ESKF状态向量：

名义状态: x = [p, v, q, b_g, b_a]
误差状态: δx = [δp, δv, δθ, δb_g, δb_a]

我刚开始用ESKF时犯过一个低级错误：忘记在注入后重置误差状态协方差。结果协方差越积越大，滤波器直接发散。后来我在代码里加了一行 P = G * P * G^T 的协方差重置，问题才解决。

注意：ESKF的误差状态是极小量表示（比如旋转用δθ而不是四元数），更新时要用指数映射转回名义状态。这个细节很多人会漏掉。

LIO-SAM是Tixiao Shan在2020年提出的紧耦合LiDAR-IMU里程计框架。它把IMU预积分、因子图优化、滑窗策略揉在一起，精度和鲁棒性都很好。

我拆解一下它的核心模块：

LIO-SAM的滑窗策略很有意思。它维护一个固定大小的因子图窗口，窗口外的老帧被边缘化（marginalization）成先验因子。这样既保留了历史信息，又控制了计算量。

调参建议：LIO-SAM里有个关键参数 extrinsicRot 和 extrinsicTrans，是LiDAR到IMU的外参。我建议先用标定工具（比如lidar_align）算一遍，别手填。手填的误差会让整个系统精度掉一个数量级。

下面这张图展示了LIO-SAM的因子图结构：

这两个概念很多人搞混。我直接说人话：

松耦合：各传感器独立跑自己的里程计，最后把结果融合一下。比如视觉跑VIO，激光跑LOAM，最后用EKF把两个位姿加权平均。好处是模块解耦，坏处是信息没有充分共享。
紧耦合：所有传感器的原始测量值一起进优化器。比如LIO-SAM里，IMU的加速度、角速度，激光的点云特征，都在同一个因子图里优化。好处是精度高，坏处是计算量大，系统复杂。

避坑指南：我曾经在一个项目里强行上紧耦合，结果IMU和激光雷达的时间戳没对齐，系统直接崩了。后来我加了一个硬件同步触发线，才把问题解决。如果你没有硬件同步，至少要做软件时间戳插值对齐。

我个人习惯是：室内小场景用松耦合，因为计算资源有限，精度也够用。室外大场景用紧耦合，因为需要长距离的精度和鲁棒性。你想想看，一个扫地机器人需要紧耦合吗？没必要。但自动驾驶就不一样了，差一厘米都可能出事。

嗯，这一章的内容就到这里。IMU预积分、ESKF、LIO-SAM，这三个东西是当前多传感器融合的基石。理解了它们，你就能看懂大部分主流SLAM框架的底层逻辑。