深入理解 LIO-SAM：紧耦合激光雷达-惯性里程计框架

本文旨在系统解析 LIO-SAM（Lidar Inertial Odometry via Smoothing and Mapping）这一高精度、实时的激光雷达-惯性里程计框架。文章面向机器人、自动驾驶及SLAM领域的研究者、工程师与学习者，旨在帮助读者深入理解其基于因子图优化的核心思想、多传感器紧耦合的工作机制，以及在实际系统中的应用价值。

全文将从 LIO-SAM 的核心思想——因子图优化入手，详细剖析其四大关键约束因子（IMU预积分、激光雷达里程计、GPS与回环检测）的作用与流程，并通过系统工作流程图直观展示其数据融合与优化过程。最后，文章将总结框架的优势、适用场景，并展望未来的技术发展方向。希望通过本文，读者能够全面掌握 LIO-SAM 的设计精髓，并将其思想应用于自己的研究与工程实践中。

1. 引言

LIO-SAM（Lidar Inertial Odometry via Smoothing and Mapping）是一个实现高精度、实时机器人轨迹估计和地图构建的紧耦合激光雷达-惯性里程计框架。它由 Tixiao Shan 等人提出，并发表在 IROS 2020 上。

与传统的滤波方法不同，LIO-SAM 的核心思想是基于因子图的后端优化。你可以把它想象成一个多传感器信息融合中心，它整合了多种信息作为“约束因子”，共同优化出机器人的最佳位姿轨迹，从而在复杂环境中实现鲁棒、低漂移的状态估计。

2. 核心思想：因子图优化

LIO-SAM 摒弃了传统滤波方法（如扩展卡尔曼滤波）递推估计的方式，转而采用基于因子图（Factor Graph）的平滑与映射（Smoothing and Mapping）方法。其核心优势在于：

• 全局优化：因子图将整个运动过程中的所有状态（位姿）作为节点，将传感器观测数据作为连接这些节点的约束（因子）。通过一次性优化所有节点，可以更有效地利用历史信息，纠正累积误差。
• 紧耦合融合：不同于松耦合将各传感器结果简单融合，紧耦合在优化层面直接融合原始观测数据，使不同传感器相互修正，达到“1+1>2”的效果。
• 灵活性：因子图结构允许方便地添加或移除各种类型的约束因子（如 GPS、回环），系统扩展性强。

2.2 iSAM2：增量平滑与建图算法

LIO-SAM 采用 iSAM2（Incremental Smoothing and Mapping 2）作为其因子图优化的后端求解器。iSAM2 是 Michael Kaess 等人提出的增量式贝叶斯树优化算法，专门为实时 SLAM 问题设计，能够高效处理大规模非线性因子图。

2.2.1 iSAM2 核心原理

iSAM2 的核心思想是将因子图转换为贝叶斯树（Bayes Tree），通过增量更新而非完全重新计算来实现高效优化。其工作流程如下：

1. 因子图构建：将传感器观测建模为因子，状态变量建模为节点，构建非线性因子图。
2. 贝叶斯树转换：通过 QR 分解将因子图转换为贝叶斯树结构，其中每个节点对应一个状态变量簇。
3. 增量更新：当新因子加入时，只更新受影响的部分子树，而非整个图。
4. 流形优化：在流形空间（如 SO(3)、SE(3)）上进行优化，保持几何约束。

iSAM2 的增量更新公式为：

$${\mathbf{\Theta }}^{\ast }=\arg \underset{\mathbf{\Theta }}{\min }\sum _i\Vert {\mathbf{e}}_i(\mathbf{\Theta }){\Vert }_{{\Sigma }_i}^2$$

其中 $\mathbf{\Theta }=[{\mathbf{x}}_1^{\top },\dots ,{\mathbf{x}}_n^{\top }{]}^{\top }$ 是所有状态变量的集合。通过 QR 分解，信息矩阵 $\mathbf{\Lambda }={\mathbf{J}}^{\top }\mathbf{J}$（$\mathbf{J}$ 为雅可比矩阵）被分解为上三角矩阵 $\mathbf{R}$：

$$\mathbf{J}=\mathbf{QR}\Rightarrow {\mathbf{R}}^{\top }\mathbf{R}\delta {\mathbf{\Theta }}^{\ast }=-{\mathbf{J}}^{\top }\mathbf{e}$$

2.2.2 流形优化与李群表示

在 SLAM 中，机器人的姿态属于特殊欧氏群 SE(3)，旋转属于特殊正交群 SO(3)。iSAM2 使用李群流形表示这些状态，避免奇异性问题。

李群基础：

• SO(3)={R∈R3×3∣R⊤R=I,det⁡(R)=1}SO(3) = \{\mathbf{R} \in \mathbb{R}^{3\times3} | \mathbf{R}^\top\mathbf{R} = \mathbf{I}, \det(\mathbf{R}) = 1\}SO(3)={R∈R3×3∣R⊤R=I,det(R)=1}
• $SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\top }& 1\end{array}\right]|\mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$

李代数表示：
对于旋转矩阵 $\mathbf{R}\in SO(3)$，其李代数 $\mathbf{\varphi }\in \mathfrak{so}(3)$ 满足：

$$\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge })\approx \mathbf{I}+{\mathbf{\varphi }}^{\wedge }+\frac{1}{2}({\mathbf{\varphi }}^{\wedge }{)}^2+\cdots$$

其中 ${\mathbf{\varphi }}^{\wedge }$ 是反对称矩阵算子：${\mathbf{\varphi }}^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$。

姿态 $\mathbf{T}\in SE(3)$ 的李代数表示为 $\mathbf{\xi }=[{\mathbf{\rho }}^{\top },{\mathbf{\varphi }}^{\top }{]}^{\top }\in \mathfrak{se}(3)$，其中 $\mathbf{\rho }\in {\mathbb{R}}^3$ 为平移部分，$\mathbf{\varphi }\in {\mathbb{R}}^3$ 为旋转部分。

流形上的优化更新：
优化更新在切空间进行：

$$\mathbf{T}\leftarrow \mathbf{T}\cdot \exp (\delta {\mathbf{\xi }}^{\wedge })$$

其中 $\exp :\mathfrak{se}(3)\to SE(3)$ 是指数映射，$\delta \mathbf{\xi }\in \mathfrak{se}(3)$ 是更新量。

流形上的雅可比计算：
残差函数在流形上的雅可比需要考虑流形结构。对于位姿 $\mathbf{T}\in SE(3)$ 的残差 $\mathbf{e}(\mathbf{T})$，其雅可比为：

$$\mathbf{J}=\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}{|}_{\delta \mathbf{\xi }=0}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\mathbf{e}(\mathbf{T}\cdot \exp (\delta {\mathbf{\xi }}^{\wedge }))-\mathbf{e}(\mathbf{T})}{\delta \mathbf{\xi }}$$

流形优化的数学形式：
在流形上的优化问题可以表示为：

$$\underset{{\mathbf{T}}_1,\dots ,{\mathbf{T}}_n\in SE(3)}{\min }\sum _{k=1}^m\Vert {\mathbf{e}}_k({\mathbf{T}}_{i_k},{\mathbf{T}}_{j_k},{\mathbf{z}}_k){\Vert }_{{\Sigma }_k}^2$$

其中 ${\mathbf{e}}_k$ 是第 $k$ 个因子的残差函数，${\mathbf{T}}_{i_k},{\mathbf{T}}_{j_k}$ 是相关的位姿变量，${\mathbf{z}}_k$ 是观测值。

2.2.3 iSAM2 增量更新算法

iSAM2 的核心是增量 QR 分解和贝叶斯树更新：

1. 贝叶斯树构建：

   • 通过变量消元顺序（通常使用 COLAMD 算法）对因子图进行排序
   • 执行 QR 分解：$\mathbf{J}=\mathbf{QR}$
   • 从 $\mathbf{R}$ 矩阵构建贝叶斯树

2. 增量更新：

   • 当新因子加入时，找到受影响的变量子集
   • 只对受影响的部分进行重新线性化和 QR 更新
   • 使用 Givens 旋转或 Householder 变换进行高效 QR 更新

3. 流形上的线性化：
   在流形 $SE(3)$ 上，线性化点 ${\mathbf{T}}_0$ 处的残差函数为：

   $$\mathbf{e}({\mathbf{T}}_0\cdot \exp (\delta {\mathbf{\xi }}^{\wedge }))\approx \mathbf{e}({\mathbf{T}}_0)+\mathbf{J}\delta \mathbf{\xi }$$

   其中 $\mathbf{J}$ 是流形上的雅可比矩阵。

2.2.4 iSAM2 在 LIO-SAM 中的实现

在 LIO-SAM 中，iSAM2 的具体实现包括以下关键步骤：

1. 因子图构建

// 伪代码示例：构建因子图
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialEstimate;

// 添加先验因子
graph.add(gtsam::PriorFactor<gtsam::Pose3>(X(0), priorPose, priorNoise));

// 添加IMU预积分因子（使用流形上的IMU因子）
auto imuFactor = gtsam::ImuFactor(
    X(i), V(i), X(i+1), V(i+1), B(i), 
    preintegratedImuMeasurements);
graph.add(imuFactor);

// 添加激光雷达里程计因子（流形上的Between因子）
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
    X(i), X(i+1), relativePose, lidarNoise));

// 添加GPS因子（可选）
graph.add(gtsam::GPSFactor(X(i), gpsPosition, gpsNoise));

// 添加回环因子
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
    X(loop_id), X(current_id), loopTransform, loopNoise));

2. 流形上的增量优化流程

// 创建 iSAM2 求解器，配置流形优化参数
gtsam::ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;  // 重线性化阈值
parameters.relinearizeSkip = 1;           // 重线性化跳过次数
parameters.enablePartialRelinearizationCheck = false;
parameters.factorization = gtsam::ISAM2Params::CHOLESKY;
parameters.cacheLinearizedFactors = false;

// 设置流形优化选项
parameters.setOptimizationParams(
    gtsam::ISAM2DoglegParams(),  // 使用Dogleg优化器
    gtsam::ISAM2GaussNewtonParams()  // 或使用Gauss-Newton
);

gtsam::ISAM2 isam(parameters);

// 初始批量优化
isam.update(graph, initialEstimate);
gtsam::Values currentEstimate = isam.calculateEstimate();

// 增量更新：当新关键帧到来时
gtsam::NonlinearFactorGraph newFactors;
gtsam::Values newValues;

// 添加新的流形约束因子
newFactors.add(gtsam::BetweenFactor<gtsam::Pose3>(
    X(n), X(n+1), newRelativePose, newNoise));

// 添加新的初始值（在流形上）
newValues.insert(X(n+1), newPose);

// 执行增量优化
isam.update(newFactors, newValues);
currentEstimate = isam.calculateEstimate();

3. 流形上的协方差恢复

// 获取特定变量在流形上的边际协方差
gtsam::Matrix covariance = isam.marginalCovariance(X(i));

// 在流形上计算不确定性椭球
gtsam::Pose3 meanPose = currentEstimate.at<gtsam::Pose3>(X(i));
gtsam::Matrix6 infoMatrix = covariance.inverse();

// 提取旋转和平移的不确定性
gtsam::Vector3 rotStd = gtsam::Rot3::Logmap(
    gtsam::Rot3::Expmap(covariance.block<3,3>(0,0).llt().matrixL()));
gtsam::Vector3 transStd = covariance.block<3,3>(3,3).diagonal().cwiseSqrt();

2.2.5 关键参数与调优

1. 重线性化阈值（relinearizeThreshold）：控制何时重新线性化因子，典型值 0.01-0.1。较小的值提高精度但增加计算量。

2. 流形优化器选择：

   • Dogleg：结合最速下降和高斯-牛顿，适合非线性强的问题
   • Gauss-Newton：标准选择，收敛速度快但可能陷入局部最优
   • Levenberg-Marquardt：鲁棒性强，适合病态问题

3. Wildfire 阈值：控制增量更新的传播范围，平衡精度与效率：

   parameters.wildfireThreshold = 1e-5;  // 默认值
   

4. 启用 RTTI：对于复杂因子类型需要启用运行时类型信息：

   parameters.enableRelinearization = true;
   parameters.evaluateNonlinearError = true;
   

2.2.6 工具与库支持

1. GTSAM（Georgia Tech Smoothing and Mapping）：

# 安装 GTSAM（支持流形优化）
git clone https://github.com/borglab/gtsam.git
cd gtsam && mkdir build && cd build

# 配置编译选项
cmake -DGTSAM_USE_SYSTEM_EIGEN=ON \
      -DGTSAM_BUILD_EXAMPLES=OFF \
      -DGTSAM_BUILD_TESTS=OFF \
      -DGTSAM_BUILD_UNSTABLE=ON \
      -DGTSAM_WITH_TBB=ON \
      ..

make -j$(nproc)
sudo make install

2. 流形优化专用工具：

// 使用GTSAM中的流形工具
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>

// 创建流形上的位姿变量
gtsam::Pose3 pose1 = gtsam::Pose3(
    gtsam::Rot3::RzRyRx(0.1, 0.2, 0.3),  // 旋转（流形）
    gtsam::Point3(1.0, 2.0, 3.0)         // 平移
);

// 流形上的插值
gtsam::Pose3 pose2 = pose1.compose(
    gtsam::Pose3(gtsam::Rot3::Rz(0.5), gtsam::Point3(0.5, 0, 0))
);

// 计算流形上的距离
double distance = pose1.range(pose2);

3. 可视化与调试工具：

• gtsam_visualization：GTSAM自带的因子图可视化工具
• plotjuggler：实时分析优化过程中的状态变化
• rviz：ROS中的3D轨迹和地图可视化
• evo：轨迹评估工具，支持APE、RPE指标计算

# 使用evo评估流形上的轨迹精度
evo_ape kitti ground_truth.txt estimated.txt -r trans_part \
    --align --correct_scale --plot

4. 性能分析工具：

// 启用性能分析
gtsam::ISAM2Params params;
params.enableDetailedResults = true;
params.print();

// 获取优化统计信息
gtsam::ISAM2Result result = isam.update(newFactors, newValues);
std::cout << "非线性迭代次数: " << result.nonlinearIterations << std::endl;
std::cout << "变量重新线性化数: " << result.variablesRelinearized << std::endl;
std::cout << "误差变化: " << result.errorAfter - result.errorBefore << std::endl;

2.2.7 流形优化实践技巧

1. 选择合适的参数化：

   • 旋转：使用四元数（无奇异性）或旋转矩阵（计算稳定）
   • 位姿：使用 $SE(3)$ 流形保持几何约束

2. 雅可比计算优化：

   // 使用自动微分计算流形雅可比
   auto factor = gtsam::BetweenFactor<gtsam::Pose3>(...);
   gtsam::Matrix H1, H2;
   gtsam::Vector error = factor.evaluateError(pose1, pose2, H1, H2);
   

3. 处理流形上的奇异性：

   // 使用四元数避免万向锁
   gtsam::Quaternion q = pose.rotation().toQuaternion();
   // 或使用旋转向量
   gtsam::Vector3 rotVec = pose.rotation().logmap();
   

4. 增量优化中的流形处理：

   // 在流形上添加新变量
   gtsam::Pose3 newPose = oldPose.compose(relativePose);
   newValues.insert(X(new_id), newPose);
   
   // 流形上的边缘化
   isam.marginalize(X(old_id));
   

iSAM2 的流形优化特性使其特别适合 LIO-SAM 这类实时系统，能够在保持几何约束的同时实现高效增量优化，是紧耦合激光雷达-惯性里程计框架的核心优化引擎。通过合理的参数配置和流形处理技巧，可以在保证精度的同时满足实时性要求。

3. 四大关键因子

LIO-SAM 的因子图主要整合了以下四种信息作为“约束因子”：

3.1 IMU 预积分因子

• 作用：利用 IMU（惯性测量单元）的高频（通常 100-200 Hz）角速度和加速度数据，提供激光雷达扫描周期内帧间运动的初始估计。
• 关键流程：
  1. 预积分：在两个激光雷达关键帧之间，对 IMU 数据进行积分，得到相对位姿变化，避免每次优化时重复积分。
  2. 点云去畸变：利用 IMU 估计的运动模型，对单帧激光雷达扫描过程中因机器人运动造成的点云畸变进行校正，为后续精准匹配做准备。
• 贡献：提供了高频率的航迹推测，并改善了点云质量。

3.1.1 IMU 预积分公式推导

IMU 预积分的关键在于避免在每次优化时重复积分。设相邻两个激光雷达关键帧时刻为 $t_k$ 和 $t_{k+1}$，IMU 的角速度和加速度测量为 ${\mathbf{\omega }}_t$ 和 ${\mathbf{a}}_t$。

在 $t_k$ 到 $t_{k+1}$ 时间段内，IMU 预积分量 $\Delta {\mathbf{R}}_{k,k+1}$、$\Delta {\mathbf{v}}_{k,k+1}$、$\Delta {\mathbf{p}}_{k,k+1}$ 定义为：

$$\begin{array}{rl}\Delta {\mathbf{R}}_{k,k+1}& =\prod _{t=t_k}^{t_{k+1}}\exp \left(({\mathbf{\omega }}_t-{\mathbf{b}}_t^g)\Delta t\right)\\ \Delta {\mathbf{v}}_{k,k+1}& =\sum _{t=t_k}^{t_{k+1}}\Delta {\mathbf{R}}_{k,t}({\mathbf{a}}_t-{\mathbf{b}}_t^a)\Delta t\\ \Delta {\mathbf{p}}_{k,k+1}& =\sum _{t=t_k}^{t_{k+1}}\left[\Delta {\mathbf{v}}_{k,t}\Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{k,t}({\mathbf{a}}_t-{\mathbf{b}}_t^a)\Delta t^2\right]\end{array}$$

其中 ${\mathbf{b}}_t^g$ 和 ${\mathbf{b}}_t^a$ 是陀螺仪和加速度计的零偏。预积分因子残差定义为：

$${\mathbf{e}}_{\text{IMU}}=\left[\begin{array}{c}\log \left(\Delta {\mathbf{R}}_{k,k+1}^{\top }{\mathbf{R}}_k^{\top }{\mathbf{R}}_{k+1}\right)\\ {\mathbf{v}}_{k+1}-{\mathbf{v}}_k-\mathbf{g}\Delta t_{k,k+1}-\Delta {\mathbf{v}}_{k,k+1}\\ {\mathbf{p}}_{k+1}-{\mathbf{p}}_k-{\mathbf{v}}_k\Delta t_{k,k+1}-\frac{1}{2}\mathbf{g}\Delta t_{k,k+1}^2-\Delta {\mathbf{p}}_{k,k+1}\end{array}\right]$$

这里 $\mathbf{R}$、$\mathbf{v}$、$\mathbf{p}$ 分别表示姿态、速度和位置，$\mathbf{g}$ 为重力向量，$\log (\cdot )$ 将旋转矩阵映射到李代数。

3.2 激光雷达里程计因子

• 作用：通过匹配连续两帧校正后的点云，提供相邻帧间精确的相对位姿约束，是里程计的核心。
• 关键流程：
  1. 特征提取：从当前帧点云中提取特征点（主要是边缘点和平面点）。
  2. 特征匹配：将当前帧的特征点与局部子地图中对应的边缘线、平面进行匹配，通过最小化点到线/面的距离，求解出两帧之间的相对位姿变换。
  3. 生成因子：将计算出的相对位姿作为约束因子，添加到因子图中。
• 贡献：提供了主体、高精度的局部相对运动约束。

3.2.1 激光雷达里程计因子数学表达

激光雷达里程计通过点到线和点到面的距离最小化来求解位姿。设当前帧点云为 ${\mathcal{P}}_k$，局部子地图为 ${\mathcal{M}}_{k-1}$。

对于边缘点 ${\mathbf{p}}_i\in {\mathcal{P}}_k$，找到 ${\mathcal{M}}_{k-1}$ 中距离最近的两个点 ${\mathbf{p}}_j$ 和 ${\mathbf{p}}_l$，构成一条直线。点到线的距离残差为：

$$d_{\text{edge}}=\frac{\Vert ({\mathbf{p}}_i-{\mathbf{p}}_j)\times ({\mathbf{p}}_i-{\mathbf{p}}_l)\Vert }{\Vert {\mathbf{p}}_j-{\mathbf{p}}_l\Vert }$$

对于平面点 ${\mathbf{p}}_i$，找到 ${\mathcal{M}}_{k-1}$ 中距离最近的三个点 ${\mathbf{p}}_j$、${\mathbf{p}}_l$、${\mathbf{p}}_m$，构成一个平面。点到面的距离残差为：

$$d_{\text{plane}}=\frac{|({\mathbf{p}}_i-{\mathbf{p}}_j{)}^{\top }(({\mathbf{p}}_l-{\mathbf{p}}_j)\times ({\mathbf{p}}_m-{\mathbf{p}}_j))|}{\Vert ({\mathbf{p}}_l-{\mathbf{p}}_j)\times ({\mathbf{p}}_m-{\mathbf{p}}_j)\Vert }$$

通过李群 $\mathrm{SE}(3)$ 上的变换 ${\mathbf{T}}_k=\left[\begin{array}{cc}{\mathbf{R}}_k& {\mathbf{t}}_k\\ 0& 1\end{array}\right]$，将当前帧点变换到地图坐标系：${\mathbf{p}}_i^{\prime }={\mathbf{T}}_k{\mathbf{p}}_i$。

激光雷达里程计因子残差为所有特征点距离残差的集合：

$${\mathbf{e}}_{\text{LiDAR}}=\sum _{i\in \mathcal{E}}{d}_{\text{edge}}({\mathbf{p}}_i^{\prime })+\sum _{i\in \mathcal{P}}{d}_{\text{plane}}({\mathbf{p}}_i^{\prime })$$

其中 $\mathcal{E}$ 和 $\mathcal{P}$ 分别表示边缘点和平面点集合。通过高斯-牛顿或列文伯格-马夸尔特方法最小化该残差，得到最优位姿变换 ${\mathbf{T}}_k^{\ast }$。

3.3 GPS 因子（可选）

• 作用：在户外且 GPS 信号良好的场景下，提供全局绝对位置约束，有效防止长时间运行产生的累积漂移。
• 集成方式：当收到有效的 GPS 信号时，将 GPS 测量值（经度、纬度、高度，通常转换为 UTM 坐标）与当前机器人估计位姿构建一个约束因子，添加到因子图中。
• 贡献：将局部地图与全球坐标系对齐，实现全局一致性。

3.3.1 GPS 因子数学模型

GPS 因子提供全局绝对位置约束。设 GPS 接收机在时刻 $t_k$ 的测量值为 ${\mathbf{z}}_{\text{GPS},k}=[{\text{lon}}_k,{\text{lat}}_k,{\text{alt}}_k{]}^{\top }$，经过坐标转换（如 UTM）得到笛卡尔坐标 ${\mathbf{p}}_{\text{GPS},k}\in {\mathbb{R}}^3$。

GPS 因子残差定义为机器人在世界坐标系下的估计位置 ${\mathbf{p}}_k$ 与 GPS 测量位置之差：

$${\mathbf{e}}_{\text{GPS}}={\mathbf{p}}_k-{\mathbf{p}}_{\text{GPS},k}$$

对应的信息矩阵（协方差逆矩阵）${\Sigma }_{\text{GPS}}^{-1}$ 反映了 GPS 测量的不确定性，通常与卫星几何分布、信噪比等因素相关：

$${\Sigma }_{\text{GPS}}=\left[\begin{array}{ccc}{\sigma }_{\text{horiz}}^2& 0& 0\\ 0& {\sigma }_{\text{horiz}}^2& 0\\ 0& 0& {\sigma }_{\text{vert}}^2\end{array}\right]$$

其中 ${\sigma }_{\text{horiz}}$ 和 ${\sigma }_{\text{vert}}$ 分别表示水平和垂直方向的标准差。在开阔天空环境下，${\sigma }_{\text{horiz}}\approx 1\text{-}3\text{m}$，${\sigma }_{\text{vert}}\approx 2\text{-}5\text{m}$；在城市峡谷中，这些值可能增大一个数量级。

3.4 回环检测因子

• 作用：当机器人识别出回到了之前访问过的区域时，提供一个强约束来闭合轨迹，大幅消除累积误差。
• 关键流程：
  1. 场景识别：使用基于扫描上下文的描述子等方法，快速检测当前帧是否与历史关键帧相似。
  2. 精细匹配：对候选回环帧进行点云配准（如 ICP），得到精确的位姿变换关系。
  3. 生成因子：将当前位姿与历史位姿通过回环变换连接起来，形成一个闭合约束因子加入因子图。
• 贡献：是消除漂移、构建全局一致地图的关键。

3.4.1 回环检测因子数学表达

回环检测通过识别重访位置来消除累积漂移。设当前关键帧 $k$ 与历史关键帧 $l$ 构成回环，通过点云配准（如 ICP）得到相对位姿变换 ${\mathbf{T}}_{l,k}$。

回环因子残差定义为：

$${\mathbf{e}}_{\text{loop}}=\log \left({\mathbf{T}}_{l,k}^{-1}\cdot {\mathbf{T}}_w^{-1}(l)\cdot {\mathbf{T}}_w(k)\right)$$

其中：

• ${\mathbf{T}}_w(k)$ 和 ${\mathbf{T}}_w(l)$ 分别是关键帧 $k$ 和 $l$ 在世界坐标系下的位姿估计
• ${\mathbf{T}}_{l,k}$ 是通过配准得到的从帧 $l$ 到帧 $k$ 的相对变换
• $\log (\cdot )$ 将 $\mathrm{SE}(3)$ 变换映射到李代数 $\mathfrak{se}(3)$

在 $\mathfrak{se}(3)$ 中，残差可线性化为 6 维向量：${\mathbf{e}}_{\text{loop}}\approx [{\mathbf{\delta \theta }}^{\top },{\mathbf{\delta p}}^{\top }{]}^{\top }$，其中 $\mathbf{\delta \theta }\in {\mathbb{R}}^3$ 是旋转部分，$\mathbf{\delta p}\in {\mathbb{R}}^3$ 是平移部分。

回环因子的协方差矩阵 ${\Sigma }_{\text{loop}}$ 反映了配准的不确定性，通常通过 ICP 的协方差估计或固定值设置。回环约束的强度远大于里程计约束，能有效纠正长期漂移。

4. 系统工作流程

LIO-SAM 的系统运行可以概括为以下流程：

1. 数据预处理：新激光雷达帧到达，利用 IMU 预积分数据进行点云运动畸变校正。
2. 局部里程计：提取特征并匹配，计算激光雷达里程计因子。
3. 关键帧判断：根据运动距离或时间判断当前帧是否为关键帧。非关键帧仅用于更新局部子地图。
4. 因子注入：如果是关键帧，将激光雷达里程计因子加入因子图。同时，检查并注入可用的 GPS 因子和回环检测因子。
5. 全局优化：触发因子图优化（例如使用 iSAM2 增量求解器），优化所有关键帧的位姿。
6. 输出与更新：输出优化后的全局轨迹，并利用优化后的位姿更新全局地图。

5. 总结与展望

LIO-SAM 通过紧耦合的因子图优化框架，巧妙地融合了多传感器数据，在保持实时性的同时，实现了高精度、低漂移的定位与建图。其设计思想对后续的 LIO 系统（如 FAST-LIO2）产生了深远影响。

主要优势：

• 精度高，漂移小。
• 支持多传感器（IMU、LiDAR、GPS）紧耦合。
• 因子图结构灵活，易于扩展。
• 代码开源，工程实现完整。

适用场景：自动驾驶、无人机、移动机器人等需要在复杂、大规模环境中进行精准定位与建图的应用。

随着传感器和算力的发展，如何在更极端的环境下（如动态物体密集、传感器退化）保持鲁棒性，以及如何进一步降低计算复杂度，仍是业界持续探索的方向。