1. 课程导入
在自动驾驶、移动机器人、无人机、AR/VR 以及室内外无缝定位等场景中，稳定、连续、可解释的位姿估计是系统能否真正落地的关键。GNSS 在开阔环境下可以提供绝对位置，但进入城市峡谷、隧道、地下车库或室内后，信号遮挡与多径会让定位精度急剧下降；激光雷达和视觉可以提供丰富的环境约束，却容易受到纹理缺失、光照变化、动态物体和计算资源的影响。此时，IMU 凭借高频率、短时高精度、不依赖外部环境的特性，成为定位系统中不可替代的传感器。
然而，IMU 单独积分会迅速漂移，直接把它放进优化问题又会带来一个工程难题：IMU 测量频率通常为 100~1000 Hz，如果每一帧都作为优化变量，因子图规模会爆炸。IMU 预积分正是解决这一矛盾的核心技术：它在两个关键帧之间把大量 IMU 测量压缩成一个相对运动约束，使优化器只处理关键帧状态，同时保留 IMU 的高频信息。因子图则把这些约束、视觉/激光观测、GNSS 观测统一建模为概率图，通过非线性优化获得全局一致的轨迹与地图。
一、应用背景：为什么需要 IMU 预积分与因子图融合
先看一个典型场景：一辆园区配送车从室外驶入地下车库。室外阶段，GNSS 与激光雷达可以提供绝对位置和全局地图匹配；进入车库后，GNSS 失效，视觉可能因光照不足退化，激光雷达也可能因长廊结构相似而出现误匹配。此时，IMU 预积分可以在关键帧之间提供高频相对运动约束，因子图则把“IMU 相对约束 + 轮速计 + 激光/视觉观测 + 历史地图”联合优化，使车辆在无 GNSS 环境下仍能保持连续、平滑、可回环的定位输出。
这类系统的核心价值可以概括为三点：
- 连续性：IMU 提供高频运动先验，避免关键帧之间出现“跳变”或“空白”。
- 一致性：因子图把多源观测统一到同一优化框架，减少传感器之间互相矛盾。
- 可解释性：每个因子对应明确的物理约束，便于排障、调参和工程验收。
二、核心原理：从 IMU 积分到预积分，再到因子图
IMU 原始测量包含角速度 ω 和比力 a。若直接积分，需要从初始状态开始逐步递推，一旦初始状态在优化中改变，所有积分都要重算。预积分的思路是：把两个关键帧之间的 IMU 测量预先积分成相对旋转、相对速度、相对位置增量，并保留噪声传播与零偏更新的一阶雅可比。这样，优化变量从“每一帧 IMU 状态”变成“关键帧状态 + 零偏”，计算量大幅下降。
因子图则把待估计变量表示为节点，把观测约束表示为因子。一个典型的融合定位因子图包含：
- IMU 预积分因子：连接相邻关键帧的位姿、速度、零偏。
- 视觉/激光里程计因子：提供局部相对观测。
- GNSS 因子：提供绝对位置约束。
- 回环因子：提供长距离全局约束。
- 零偏随机游走因子：约束零偏缓慢变化。
优化目标是最小化所有因子的残差平方和。常用求解器包括 GTSAM、Ceres、g2o 等。工程上，我们通常采用“滑动窗口 + 边缘化 + 回环”的策略，在精度与实时性之间取得平衡。
三、方法流程：一个可落地的融合定位流水线
从工程实现角度看，IMU 预积分与因子图融合定位通常遵循以下流程：
- 数据接入与时间同步：统一 IMU、相机、激光、GNSS 的时间戳，处理硬件触发与插值。
- IMU 预积分：在相邻关键帧之间积分，计算 ΔR、Δv、Δp 及协方差、雅可比。
- 前端观测构建：视觉特征跟踪、激光点云配准、GNSS 坐标转换与回环检测。
- 因子图构建：添加 IMU 因子、视觉/激光因子、GNSS 因子、回环因子与零偏因子。
- 非线性优化：滑动窗口内求解，边缘化旧状态，保持计算量可控。
- 输出与评估：发布轨迹、地图与协方差，使用 ATE、RPE 等指标评估精度。
下面给出一个典型的配置文件片段，用于说明工程中需要关注的关键参数：
imu_preintegration:
acc_noise: 0.02 # 加速度计噪声密度
gyro_noise: 0.001 # 陀螺仪噪声密度
acc_bias_rw: 0.0001 # 加速度计零偏随机游走
gyro_bias_rw: 0.00001 # 陀螺仪零偏随机游走
integration_rate: 200 # 预积分频率 Hz
factor_graph:
window_size: 10 # 滑动窗口关键帧数量
solver: "GTSAM"
loop_closure: true
gnss_factor: true
max_iterations: 30
四、案例与场景：从园区配送车到无人机巡检
以园区配送车为例，室外段 GNSS 与激光雷达融合，进入地下车库后 GNSS 失效，系统切换到“IMU 预积分 + 激光里程计 + 轮速计”模式。因子图中，IMU 预积分因子提供高频相对运动，激光里程计因子提供环境约束，轮速计因子提供平面运动约束。当车辆重新驶出车库并收到 GNSS 信号时，GNSS 因子重新加入，因子图通过回环与全局优化修正累计漂移。
再以无人机巡检为例，飞行过程中视觉可能因快速旋转而模糊，IMU 预积分可以在视觉短暂失效时维持姿态与位置递推；因子图则把视觉、IMU、气压计和 GNSS 统一优化，保证在桥梁、隧道等复杂场景下的定位连续性。
| 模块 | 输入 | 输出 | 关键挑战 |
|---|---|---|---|
| IMU 预积分 | ω、a、零偏估计 | ΔR、Δv、Δp、协方差、雅可比 | 零偏变化、数值积分精度 |
| 视觉/激光前端 | 图像、点云 | 相对位姿、特征关联 | 光照、纹理、动态物体 |
| 因子图优化 | 多源因子 | 全局一致轨迹 | 计算量、边缘化一致性 |
| 回环与 GNSS | 历史地图、卫星观测 | 全局约束 | 误回环、多径、坐标对齐 |
五、常见误区与复盘
六、课程整体框架与实战项目
本课程共 30 章，整体分为五个阶段：
- 基础篇：IMU 误差模型、坐标系、旋转表示、预积分推导。
- 核心篇：预积分雅可比、零偏更新、因子图建模、滑动窗口与边缘化。
- 融合篇：视觉/激光/GNSS/轮速计因子构建，回环检测与全局优化。
- 工程篇：GTSAM/Ceres 实战、时间同步、参数调优、实时性优化。
- 实战篇：完成一个“园区配送车多传感器融合定位”项目，包含数据回放、建图、定位、评估与报告。
实战项目将提供 IMU、相机、激光、GNSS 与轮速计数据，要求你从零搭建预积分模块，构建因子图，完成轨迹估计，并与真值对比。最终交付物包括：可运行代码、参数配置、轨迹评估报告与问题复盘文档。