自动驾驶02：点云预处理——02（运动补偿篇LIO-SAM）

当激光雷达（LiDAR）在运动中采集点云时，每个点的时间戳不同，而车辆在移动，导致点云在不同时间点的坐标与实际情况不符，这种现象称为运动畸变（Motion Distortion）。为了得到无畸变的点云，需要进行运动畸变补偿。

LIO-SAM（IMU + LiDAR）：https://github.com/TixiaoShan/LIO-SAM

https://ieeexplore.ieee.org/document/9341176

LIO-SAM 是一个基于因子图优化的激光雷达-惯性导航里程计（LiDAR-Inertial Odometry, LIO）的开源 SLAM 框架。它结合了激光雷达点云与IMU 数据，通过优化算法（GTSAM）进行高精度定位和建图，特别适用于动态环境和运动畸变补偿。

LIO-SAM 主要用于：

点云去畸变补偿（消除 LiDAR 在运动中的扭曲）
实时激光 SLAM（局部里程计 + 全局地图优化）
自动驾驶、无人机、机器人导航等场景

研究背景

LOAM是目前为止激光里程计（LO）领域最经典最广泛使用的方法。但是它存在一个问题，就是它直接存储全局体素地图而不是局部地图，从而很难执行回环检测以修正漂移，或者组合GPS（提供绝对位置）等测量进行位姿修正。并且体素地图的使用效率会随时间降低。为了克服该问题，作者只独立地存储每个关键帧的特征，而不是在位姿估计完成后就将特征加入到全局地图中。

另一方面，IMU和LiDAR的联合位姿估计已经被广泛研究，大致分为两类。第一类是松耦合的方法，例如LOAM和LeGO-LOAM中使用IMU去除LiDAR点云的运动畸变，以及[8]-[12]使用EKF整合LiDAR和IMU的测量。第二类是紧耦合的方法，例如R-LINS[15]，使用误差状态卡尔曼滤波器迭代地修正机器人的位姿估计，再比如LIOM [16]联合优化LiDAR和IMU测量。但是LIOM一次性处理所有测量，因此不能实时运行。而本文也属于紧耦合的激光-惯性里程计方法，只是采用了因子图优化而不是滤波的方法。

LOAM 主要基于激光点云特征匹配来估计位姿，每一帧都会根据上一帧的位姿进行增量更新。但这种方法会导致：

短期误差累积：每一帧匹配的误差会逐步积累，导致位姿逐步偏移。
长时间运行后误差显著：如果机器人或无人车长时间运行，没有全局优化，就可能出现位置漂移，使得建图产生错位。

回环检测：当机器人/无人车回到某个曾经到达的地方，通过匹配当前点云和历史点云，可以发现当前估计的位姿和真实位姿的偏差。

1. LIO-SAM 运动畸变补偿原理

LiDAR 由于逐点扫描，每个点的采集时间不同，因此在车辆运动时，同一帧点云可能包含不同时间的位置信息，导致点云扭曲（Motion Distortion）。

图2展示了LIO-SAM的因子图。图中主要包含四种因子。第一种是IMU预积分因子（橙色），由两个相邻关键帧之间的IMU测量积分得到。第二种是激光里程计因子（绿色），由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。第三种是GPS因子（黄色），由每个关键帧的GPS测量得到。第四种是回环因子（黑色），由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。

因子图是什么？

因子图是一种概率图模型，它用于优化机器人在不同时间步的位姿（Position & Orientation），使其符合传感器观测数据的约束。在LIO-SAM中，因子图的优化由 iSAM2（增量平滑和映射）算法完成。

因子图的本质：

变量（节点）：表示待优化的机器人关键帧位姿（位置、方向）。

约束（因子）：连接不同变量的观测数据，提供优化条件。

IMU预积分因子（橙色）

数据来源：IMU传感器（惯性测量单元）。

作用：利用IMU数据，估计两个相邻关键帧之间的位移和旋转。

原理：

由于IMU可以高频率提供加速度和角速度信息，因此可以通过 数值积分 来推算位姿变化。

但单独使用IMU会导致 漂移（drift），因此需要与其他传感器（如Lidar）进行融合校正。

激光里程计因子（绿色）

数据来源：激光雷达点云的匹配结果（帧间匹配）。

作用：通过点云匹配计算关键帧之间的位姿变换，提高里程计的精度。

原理：

LIO-SAM在每个关键帧与前 n 个关键帧之间进行点云匹配，计算相对位姿变化。

这个因子提供了短时间尺度的几何约束，用于修正IMU的漂移。

GPS因子（黄色）

数据来源：GPS 传感器。

作用：提供全局定位信息，防止长期漂移。

原理：

GPS测量的是绝对坐标（经纬度或UTM坐标），可以为因子图提供全球位置信息。

由于GPS可能存在噪声（如城市环境中的多路径效应），LIO-SAM并不直接采用GPS的数值，而是将其作为软约束，用于优化位姿。

回环因子（黑色）

数据来源：基于激光雷达点云的回环检测（Loop Closure）。

作用：当机器人经过曾经到达过的地方，检测并校正累积漂移，提高长期建图精度。

原理：

通过扫描匹配（Scan Matching），检测当前关键帧是否与历史关键帧匹配。

如果匹配成功，则添加一个回环因子，提供全局几何约束，用于修正误差。

LIO-SAM使用了 2m+1 个关键帧进行匹配，以确保匹配稳定性。

因子图优化（iSAM2）

LIO-SAM使用 iSAM2（增量平滑和映射）算法进行因子图优化。

iSAM2的特点：

通过增量更新的方式，避免每次重新计算整个因子图，提高计算效率。

只在新增关键帧时进行局部优化，保证了实时性。

结合 IMU、Lidar、GPS、回环检测 的多传感器信息，提高位姿估计的精度和鲁棒性。

2. 核心思路及步骤

LIO-SAM 通过IMU 预积分（IMU Pre-integration），将 IMU 计算出的运动轨迹与 LiDAR 点云的时间戳对齐，从而补偿 LiDAR 运动导致的畸变。核心思路如下：

步骤 1：获取 LiDAR 和 IMU 数据

LiDAR 点云帧（Scan）：包含多个点，每个点有自己的时间戳。
IMU 数据（惯性测量）：
- 加速度计（Acceleration）
- 陀螺仪（Gyroscope）
- 角速度 & 线速度
时间同步：确保 IMU 与 LiDAR 的时间戳对齐（通常用硬件同步，或软件插值处理）。

步骤 2：IMU 预积分（Pre-integration）

IMU 在两个时刻之间的测量值（加速度 & 角速度）可以计算增量运动轨迹，即：

旋转增量 $R_{i+1} = R_i \cdot \text{exp}(\omega \Delta t)$
位置增量 $p_{i+1} = p_i + v_i \Delta t + \frac{1}{2} a_i \Delta t^2$
速度增量 $v_{i+1} = v_i + a_i \Delta t$

LIO-SAM 通过 IMU 预积分，在 LiDAR 点云的采集时间范围内，估计出每个时间点的位姿 $T_w(t)$ ，用于运动畸变补偿。

步骤 3：将点云变换到统一坐标系

设 $p_i$ 是 LiDAR 采集的某个点
设 $T_w(t_i)$ 是该点采集时刻的世界坐标系位姿
目标是将所有点转换到相同时间（通常是当前帧起始时间 $t_0$ ）：

其中：
- $p_i^{(w)}$ 是补偿后的世界坐标
- $p_i^{(l)}$ 是 LiDAR 采集时的局部坐标
- $T_w(t_0)$ 是扫描开始时刻的位姿
- $T_w(t_i)$ 是该点采集时刻的位姿