从零掌握LIO-SAM算法核心原理

1. 核心思想：基于因子图的紧耦合优化

LIO-SAM的核心创新在于其后端优化框架，摒弃了LOAM及其衍生系列（如LeGO-LOAM）中采用的帧到局部地图匹配+滤波的松耦合方式，转而采用了因子图模型进行紧耦合优化。

紧耦合 ： 原始IMU数据和激光雷达特征点数据在同一个优化问题中被共同处理，同时估计出所有的状态变量（位姿、速度、偏差等），使得任何一种传感器的信息都能直接约束所有状态，精度更高。

因子图 ： 一种用于表示复杂优化问题的概率图模型，由两类节点组成：

• 变量节点 ： 需要估计的状态，例如各时间点的位姿 X₀, X₁, …, Xₖ。
• 因子节点 ： 表示对状态变量的约束，来源于各种传感器测量或先验知识，例如，IMU测量因子、激光里程计因子。

LIO-SAM的最终目标就是构建一个最大后验概率估计问题，其解即为最可能的状态序列，因子图将这个最大后验概率估计问题直观地表示为所有因子乘积的最大化：

其中 X 是状态变量集合，zizi 是第 i 个传感器测量值。

在这里插入图片描述

2. 数学基础：IMU预积分

这是理解现代惯性辅助SLAM系统的关键，也是LIO-SAM能够高效融合IMU数据的数学工具。

2.1 预积分的目的

IMU频率极高（通常100-500Hz），而激光雷达频率较低（通常10-20Hz），如果直接在两个激光雷达帧 k 和 k+1 之间对IMU数据进行传统的积分，会产生大量冗余的状态变量（每个IMU时刻都有一个状态），计算量巨大。

预积分的思想： 将两个关键帧（激光雷达帧）之间的所有IMU测量值积分到一个相对的运动增量 ΔR, Δv, Δp 上，这个增量与状态变量中的零偏有关，但与两帧之间的绝对状态（位姿、速度）无关，这样，当对状态进行优化调整时，无需重新积分所有IMU数据，只需根据偏差的变化对预积分量进行一阶近似修正即可，极大提高了效率。

2.2 预积分模型 (基于欧拉积分的离散形式)

IMU的动力学模型在全局坐标系下为：

在时间间隔 [tₖ, tₖ₊₁] 内，对 i 个IMU测量值进行积分，得到预积分量 ΔRᵢⱼ, Δvᵢⱼ, Δpᵢⱼ，它们的定义是在机器人坐标系B（通常是IMU系）下的增量：

旋转预积分 ΔRᵢⱼ:

2.3 预积分量的偏差修正

预积分量是在假设偏差不变的情况下计算的，但实际上，在优化过程中偏差是会被微调估计的，如果偏差变化了 δb，则不需要重新积分，而是使用一阶线性近似来更新预积分量，这是预积分理论高效的关键：

雅可比矩阵 ∂ΔR/∂b, ∂Δv/∂b, ∂Δp/∂b 可以在积分过程中通过递推得到。

3. 系统流程与因子图构建

LIO-SAM的流程可以概括为以下几步，其核心是不断向因子图中添加四种因子：

3.1 IMU预积分因子

• 来源： 两个激光雷达关键帧 i 和 j 之间的所有IMU数据。
• 数学形式： 构建一个残差项，表示预积分测量值与状态预测值之间的差异。

• 作用： 紧耦合IMU数据，提供帧间高频率的平滑运动约束，并在线估计IMU的偏差。

3.2 激光里程计因子

• 来源： 对当前帧提取的特征点（平面、边缘）与局部地图（由附近若干关键帧组成）进行匹配，匹配算法与LeGO-LOAM类似，使用点到面和点到线的ICP。
• 数学形式： 通过匹配得到一个相对位姿变换 T_odom，残差即当前状态估计的位姿 T_j 与匹配得到的位姿 T_odom 之间的差异（在李群上计算）。

作用： 校正累积误差，是SLAM系统实现长期一致性的关键。

3.3 GPS因子

• 来源： 当收到GPS信号时，将GPS的经纬高坐标转换到局部ENU或UTM坐标系下，得到全局位置 p_gps。
• 数学形式： 只有当估计的位姿 p_j 与 p_gps 的差值超过一个阈值时，才加入因子图，避免过约束，残差是位置差异。

• 作用： 提供精确的帧间位姿约束，是消除IMU漂移的主要信息来源。

3.4 回环因子

• 来源： 使用一种简单的几何一致性检测：当最新关键帧的位姿与历史中某个关键帧的位姿足够接近时，认为检测到回环，然后使用ICP进行精细匹配，得到闭环校正位姿 T_loop。
• 数学形式： 与激光里程计因子类似，残差是历史位姿 T_hist、当前位姿 T_j 和闭环位姿 T_loop 之间的差异。

• 作用： 提供绝对位置信息，防止系统在长时间、大范围运行中产生累积漂移。

4. 操作流程

从底层原理看，LIO-SAM的运行流程是一个持续的预测-测量-优化循环：

• 预测： 当新的激光帧到来时，利用上一时刻的状态和IMU预积分量，预测当前机器人的大致位姿 T_predicted，为下一步的激光匹配提供了一个初始估计，确保了匹配的速度和成功率。
• 测量 : 对去畸变后的当前激光帧进行特征提取，用预测的位姿 T_predicted 作为初始值，将当前帧的特征与局部地图进行匹配，求解出更精确的激光里程计位姿 T_lidar，如果收到有效的GPS或检测到回环，生成相应的约束。
• 优化: 将新的激光里程计因子、IMU预积分因子、以及可能的GPS因子和回环因子，全部加入到因子图中，调用GTSAM等优化库，一次性优化所有关键帧的状态（位姿、速度、偏差），优化目标是使所有因子的残差平方和最小（最大似然估计）。

• 更新: 用优化后的最新状态更新全局地图，为下一次迭代做准备。