学术研究报告：SCE-LIO：一种通过构建子图约束增强的激光雷达惯性里程计
第一， 研究的主要作者、机构、发表期刊及时间
本研究由 Sun Chao、Huang Zhishuai、Wang Bo、Xiao Mancheng、Leng Jianghao 以及 Li Jiajun（通讯作者）共同完成。主要作者来自北京理工大学深圳汽车研究院（Sun Chao， Wang Bo， Li Jiajun）以及北京理工大学机械与车辆学院国家电动汽车工程实验室（Huang Zhishuai， Leng Jianghao），Xiao Mancheng 则来自深圳市无限传感技术有限公司。本研究以“SCE-LIO: An Enhanced Lidar Inertial Odometry by Constructing Submap Constraints”为题，发表在 IEEE Robotics and Automation Letters 期刊，于2024年9月30日在线发表，刊载于2024年11月出版的第9卷第11期。
第二， 研究的学术背景
本研究属于机器人学与自动驾驶领域的同时定位与建图 (Simultaneous Localization and Mapping， SLAM) 范畴。具体聚焦于激光雷达惯性里程计 (Lidar Inertial Odometry， LIO) 技术。激光雷达（Lidar）因其能提供精确的深度信息，在SLAM系统中被广泛应用。然而，在特征稀疏或退化的环境（如长走廊、开阔广场）中，仅依赖激光雷达的SLAM算法性能会严重下降甚至失效。融合惯性测量单元（Inertial Measurement Unit， IMU）数据能有效提升短时鲁棒性，因此LIO系统成为研究热点。尽管如此，长时间运行的累积误差（Cumulative Error）问题仍是挑战。
传统上，解决累积误差的通用方法是引入回环检测 (Loop Closure Detection) 模块，通过识别重访地点为后端优化提供约束，从而校正全局轨迹。然而，回环检测存在明显局限：其约束是稀疏且间断的，只在检测到回环时才提供，无法在无回环的场景中生成约束。这意味着在实时运行过程中，里程计的精度无法得到持续的保证。此外，依赖描述子（Scan Context， M2DP等）的回环检测方法在复杂动态环境中可能失效。因此，本研究旨在解决现有LIO系统对回环检测的依赖，提出一种能够周期性、连续地提供约束以抑制累积误差的方法，从而实现在各种场景下更稳定、更精确的位姿估计。
本研究的主要目标是：提出一个增强型LIO系统（SCE-LIO），通过周期性构建子图约束 (Submap Constraints) 并集成到位姿图优化 (Pose Graph Optimization， PGO) 后端中，持续优化里程计精度，降低其对稀疏回环事件的依赖。
第三， 详细研究工作流程
本研究提出的SCE-LIO系统框架主要包括四个核心模块：数据预处理（Data Preprocessing）、扫描到地图匹配（Scan-to-Map Matching）、子图约束（Submap Constraints）以及后端优化（Back-End Optimization）。整个流程围绕如何生成和利用子图约束来展开。
1. 扫描到地图匹配的先决条件模块： 为提高扫描到地图匹配的精度，研究者预先设计了三个关键子模块，作为整个系统的基石。 * 集成初始定位模块 (Integrated Initial Positioning， IIP)：初始姿态估计的准确性直接影响后续匹配的精度和效率。该模块融合了IMU预积分（IMU Preintegration）和扫描到扫描匹配（Scan-to-Scan Matching）两种方法。IMU预积分在特征稀疏区域表现稳定，但其精度受硬件和运动强度影响；扫描到扫描匹配在特征丰富区域效果好，但在特征稀疏时不佳。IIP模块利用IMU预积分的结果作为扫描匹配的初始值，再进行扫描到扫描匹配，从而综合两者优势，提供更鲁棒和精确的初始位姿。 * 特征提取模块 (Feature Extraction， FE)：传统LIO系统通常只提取边缘（Edge）和平面（Planar）特征点。但在下采样过程中，平面特征点的减少可能导致地面点不足，削弱Z方向（垂直方向）的约束，增加误差。为此，本研究提出了一种基于地面分割的特征提取方法。首先，将扫描数据分割为地面点（Ground Points）和非地面点。然后，对非地面点基于曲率进一步分类为边缘点和平面点。最后，对这三类特征点（边缘点、平面点、地面点）分别采用不同的下采样率进行降采样。这样可以在保留足够地面点的同时，有效控制计算量。 * 特征点对应关系模块 (Feature Points Correspondence， FPC)：传统方法通常基于欧几里得距离为扫描中的特征点寻找地图中的对应平面或边缘。但仅凭距离可能产生错误对应。如图2所示，激光雷达点A可能错误地与平面πi对应，而实际上其法向量更接近平面πj。因此，研究者提出了一种基于向量误差的点到平面对应方法。具体步骤为：首先将扫描特征点转换到地图坐标系；然后通过欧氏距离法找到地图中最近的平面；接着分别计算扫描特征点的法向量和地图中对应平面的法向量；最后，计算两个法向量之间的夹角（见公式2），若夹角小于预设阈值，才确认对应关系。这种方法考虑了局部特征的方向一致性，能产生更准确的匹配对。
2. 子图约束的构建流程： 这是本研究的核心创新点。其目标是不依赖回环，定期生成子图之间的相对位姿变换作为约束。整个过程由算法1（子图创建与对齐）概述，包含以下子步骤： * 子图创建 (Submap Creation， SC)：系统将连续的激光雷达关键帧聚合成子图。每个子图由固定数量的连续关键帧构成（本文中设定为15个，记为nsub）。每个子图S_f包含从第i帧到第i+n帧的所有边缘特征点、平面特征点和地面点（见公式3）。这意味着每个子图代表一小段轨迹及其对应的局部环境信息。子图的全局初始位姿由构建它的第一帧关键帧的里程计位姿给出。 * 重叠点提取 (Extracting Overlapping Points， EOP)：为了高效对齐两个子图，并非使用整个子图的所有点云。研究者提出，只需利用两个子图之间的重叠区域点云进行匹配即可。算法3描述了这一过程：给定两个子图sc(i)和sc(j)，首先将它们各自的点云通过各自的初始位姿转换到世界坐标系。然后，为其中一个子图（如sc(i)）的点云构建KD-Tree。接着，遍历另一个子图（sc(j)）的每一个点，在KD-Tree中搜索其在sc(i)点云中的最近邻点。如果该最近邻点距离足够近，则认为这两个点是“重叠点”，并分别将它们收集到代表重叠区域的子集soverlap_i和soverlap_j中。最后，将这些重叠点云重新转换回它们各自的子图坐标系，形成两个只包含重叠信息的精简子图sp和sq，用于后续匹配。 * 子图到子图匹配 (Submap to Submap Matching， SSM)：获得两个重叠子图sp和sq后，需要计算它们之间的最优位姿变换。这本质上是一个点云配准问题。研究者采用迭代最近点 (Iterative Closest Point， ICP) 算法，结合点到线 (point-to-line) 和点到面 (point-to-plane) 距离度量进行优化。算法4描述了该过程：以子图sp相对于sq的初始变换（公式4，基于创建子图时的里程计位姿）为起点，迭代地进行以下操作：为sp中的每个特征点在sq中找到对应的边缘线或平面；根据这些对应关系，计算点到线距离（公式5）和点到面距离（公式6）；构建雅可比矩阵和误差向量；最后通过最小二乘优化（最小化公式7）求解位姿增量δT。迭代直到收敛或达到最大次数，最终得到最优的相对位姿变换T_si_sj，该变换即被定义为一个子图约束。为了控制计算开销，系统会维持一个固定大小的子图集合，并使用多线程技术来加速EOP和SSM模块。
3. 后端位姿图优化： 系统构建了一个因子图（Factor Graph），其中变量节点是关键帧的位姿。约束（因子）主要来自两类：相邻关键帧约束（激光雷达里程计因子） 和 子图约束因子。相邻关键帧约束由前端里程计提供，表示连续关键帧之间的相对运动。子图约束因子则由上述SSM模块生成。这两种约束都被建模为高斯误差模型（公式9）。优化的目标是最大化所有约束因子的联合概率，即通过公式10求解所有关键帧的最优位姿估计x*。本研究选择使用GTSAM库中的增量平滑与建图 (Incremental Smoothing and Mapping， iSAM) 方法进行优化，因其具有良好的实时性和鲁棒性。通过将周期性生成的、时间跨度较短的子图约束与相邻帧约束一同优化，系统能够持续地校正累积误差，而无需等待回环事件。
第四， 研究的主要结果
研究者通过仿真、公开数据集和真实车辆实验，全面评估了SCE-LIO算法的性能，并与多个先进算法（LEGO-LOAM， LIO-SAM， FAST-LIO2， DLO）进行了对比。
1. CARLA仿真验证： 在CARLA仿真平台的Town1， Town4， Town10三个场景中采集数据，生成Carla1， Carla4， Carla10三个数据集。评估指标为绝对轨迹误差 (Absolute Trajectory Error， ATE) 的均方根误差（RMSE）。结果显示，在不使用GPS或回环约束的条件下，SCE-LIO算法在三个仿真场景中的平移RMSE均显著低于作为基准的LIO-SAM算法和FAST-LIO2算法。这初步验证了子图约束机制在仿真环境中对于提升轨迹精度的有效性。
2. 真实环境实验验证： 实验使用了公开数据集（UrbanLoco中的hkdata20190117和hkdata20190317）以及自采数据集（在校园和公共道路上采集的outdoor0至outdoor5共6个数据集）。自采数据使用16线机械激光雷达和IMU/GPS设备（Huace 430）。评估包括定性分析和定量分析。 * 定性分析（地图质量）：通过对比SCE-LIO与LIO-SAM（均未使用回环或GPS）生成的地图，可以直观看出SCE-LIO的优越性。例如，在图6（outdoor2数据集）红圈区域，SCE-LIO的地图对齐偏差更小。在图7（outdoor3数据集）中，SCE-LIO构建的大型圆柱形容器结构更完整，无明显错位。在图8（hkdata20190117数据集）红框区域内，SCE-LIO生成的地图中墙壁的“重影”现象比LIO-SAM显著减少。这些定性结果表明，SCE-LIO的累积误差更小，建图一致性更高。 * 定量分析（轨迹精度）： * 绝对轨迹误差 (ATE)：如表III所示，在大多数数据集上，SCE-LIO的ATE（RMSE）表现最佳。平均而言，其精度比FAST-LIO2高出约30%。值得注意的是，在outdoor0至outdoor4这些路径长度递增的数据集上，随着路径增长，SCE-LIO的优势逐渐扩大。这证明了随着子图约束数量的增加，其抑制长距离累积误差的效果愈发明显。 * 最终位置漂移误差 (Final Position Drift Error， FPDE)：对于包含回环场景的数据集（outdoor2， outdoor3， hkdata20190117），研究者进一步评估了起点和终点（理论上应重合）之间的位置误差。如表IV所示，在这三个数据集上，SCE-LIO的FPDE均为最小。这强有力地证明了该算法在闭合轨迹中有效减小了累积漂移。 * 与LIO-SAM的详细对比：以LIO-SAM为基线，SCE-LIO在ATE指标上最高提升了77%，平均提升约30%；在FPDE指标上最高提升了80%，平均提升约54%。这些显著的提升归因于：1）FPC， FE， IIP三个基础模块提升了前端匹配的精度和鲁棒性；2）子图约束机制提供了持续、有效的额外约束；3）位姿图优化整合了这些约束，实现了全局一致性优化。
3. 消融研究： 为评估FPC， FE， IIP三个模块（合称FIF模块）对系统性能的贡献，研究者进行了消融实验。N-FIF代表移除了这三个模块的SCE-LIO， W-FIF代表完整的SCE-LIO。如表V所示，在四个数据集上，包含FIF模块的版本其ATE精度均优于不含的版本，平均提升约12%。这定量证明了这三个模块对提升整个系统精度具有积极作用。
4. 运行时间分析： 研究者分析了算法在处理不同下采样率数据时的运行时间（表VI）。结果显示，SCE-LIO处理单帧激光雷达数据的平均时间始终低于70毫秒。当ds_ground（地面点下采样分辨率）在0.25-0.45米之间，ds_surf（平面点下采样分辨率）在0.45-0.65米之间时，算法在精度和运行时间上达到最优平衡，平均每帧处理时间约为40-50毫秒，满足实时性要求。文中还对算法1的整体时间复杂度进行了分析。
第五， 研究的结论与价值
本研究提出并验证了一种名为SCE-LIO的新型增强型激光雷达惯性里程计系统。其核心贡献在于周期性构建子图约束的创新机制。该方法通过定期将激光雷达关键帧聚合成子图，提取子图间的重叠区域，并利用ICP算法精确计算子图间的相对位姿变换，从而生成密集、连续的约束因子。这些约束与传统的相邻帧约束一同被送入基于iSAM的后端位姿图进行优化，实现了对里程计累积误差的持续校正。
该研究的科学价值在于：1）为LIO/SLAM领域提供了一种不依赖于稀疏回环检测的、新的累积误差抑制思路；2）系统性地整合了改进的特征提取（FE）、特征对应（FPC）和初始定位（IIP）模块，提升了前端匹配的鲁棒性；3）通过详尽的仿真与真实实验，证明了该方案在多种场景下的有效性和优越性。
其应用价值体现在：为自动驾驶车辆、移动机器人等平台在无GPS或特征退化环境中实现高精度、高鲁棒性的实时定位与建图提供了可行的技术方案。子图约束机制使得系统即使在无回环的长直走廊或开阔区域，也能保持良好的精度。
第六， 研究的亮点
第七， 其他有价值的内容
研究者在文中提到了未来的工作计划：计划引入相机传感器，并融合深度学习方法生成语义信息，以进一步提升里程计的精度。这指明了将几何SLAM与语义信息相结合的后续研究方向，具有前瞻性。此外，文中提供的算法伪代码（Algorithm 1-4）清晰描述了子图创建、重叠点提取、子图匹配等核心过程的逻辑，对理解和复现该工作具有重要参考价值。