这篇研究发表于期刊ieee robotics and automation letters，2024年11月第9卷第11期。主要作者为Chao Sun、Zhishuai Huang、Bo Wang、Mancheng Xiao、Jianghao Leng以及通讯作者Jiajun Li。作者单位包括北京理工大学深圳汽车研究院、北京理工大学机械工程学院的电动汽车国家工程实验室，以及深圳元戎传感技术有限公司。
本研究属于机器人学与自动驾驶领域，聚焦于同时定位与地图构建（SLAM） 技术，特别是其中的激光雷达惯性里程计（LIO）系统。研究的核心背景在于，激光雷达SLAM系统在长期运行中会累积误差，影响定位和建图精度。虽然闭环检测（Loop Closure Detection）是缓解这一问题的经典方法，但它仅在检测到“回环”时才提供约束，无法在非回环场景中持续发挥作用。因此，研究团队旨在开发一种能够周期性提供约束、从而持续优化里程计精度的方法。具体目标是提出一种名为SCE-LIO的增强型激光雷达惯性里程计系统，其核心是通过构建子地图约束（Submap Constraints），并结合位姿图优化（Pose Graph Optimization），来持续、主动地校正累积误差，提升系统的整体精度和鲁棒性。
第三部分：研究工作的详细流程 SCE-LIO系统是一个集成多种模块的复杂框架，其工作流程可概括为数据预处理、扫描到地图的匹配、子地图约束构建以及后端优化四个主要部分。系统以机器人操作系统（ROS）为平台实现。
1. 数据预处理与特征提取 这一部分为后续模块提供高质量的数据基础。首先，系统接收原始激光雷达点云和惯性测量单元（IMU）数据。针对激光雷达，系统执行运动补偿以校正激光雷达运动过程中产生的点云畸变。IMU数据则进行预积分（Preintegration），提供短时间内的相对运动估计。
特征提取（FE）模块是关键一环。为了增强系统在垂直方向（Z轴）的约束，本文提出了一种基于地面分割（Ground Segmentation） 的特征点提取方法。具体而言，系统首先将每一帧激光雷达点云分割为地面点（Ground Points）和非地面点。随后，非地面点根据其曲率进一步分类为平面特征点（Planar Features） 和边缘特征点（Edge Features）。最后，对这三类特征点（地面点、平面点、边缘点）采用不同的降采样率进行处理，确保了特征的多样性和计算效率的平衡。
2. 扫描到地图匹配的准备与执行 这是前端里程计的核心，负责将当前激光雷达扫描帧与局部地图进行对齐，估算位姿。为提升匹配精度，系统引入了三个子模块： * 集成初始定位模块（IIP）：该模块融合了IMU预积分的结果和扫描到扫描匹配（Scan-to-scan Matching） 的结果。IMU预积分提供不受特征影响但可能存在漂移的初始估计，而扫描到扫描匹配在特征丰富的环境中表现良好。两者结合，可以互相补充，提供更鲁棒、更准确的初始位姿估计，作为扫描到地图匹配的起始点。 * 特征点对应关系模块（FPC）：传统的点-面匹配仅基于欧氏距离寻找最近邻，可能导致错误的对应。如图2所示，点A可能错误地对应到平面πi。本文提出了一种基于法向量差异的点-面对应方法。具体流程是：在找到欧氏距离最近的候选平面后，分别计算扫描点A的法向量和候选平面π的法向量。通过公式 |acos((n_l · n_m) / (|n_l| × |n_m|))| < φ 计算两者夹角，如果夹角小于预设阈值φ，则建立对应关系。这种方法能更准确地筛选出几何结构一致的匹配对。 * 扫描到地图匹配：利用IIP提供的初始位姿和FPC建立的特征点对应关系，系统采用迭代最近点（ICP）算法，通过最小化点到线（用于边缘点）和点到面（用于平面和地面点）的距离，求解出当前帧相对于局部地图的最优位姿变换。这个过程形成了相邻关键帧之间的约束（里程计约束）。
3. 子地图约束的构建（核心创新模块） 这是本文的核心贡献，旨在生成周期性的、不依赖于回环的全局约束。该模块主要包含三个子步骤，如算法1所示： * 子地图创建（SC）：系统将连续的激光雷达关键帧（Keyframes） 分组，构建子地图。每个子地图包含连续15个（参数n_sub）关键帧中的边缘点、平面点和地面点特征集合。每个子地图拥有自己的局部坐标系，其原点位姿由该组关键帧中第一个关键帧的位姿定义。子地图被存储在一个集合中，并保持固定大小以控制内存和计算量。 * 重叠点提取（EOP）：两个子地图进行匹配时，并非整个地图的所有点都参与。系统通过KD-Tree最近邻搜索，高效地找出两个待匹配子地图之间的重叠区域点云。具体方法是：将两个子地图的点云都转换到世界坐标系，对于子地图B中的每个点，在子地图A的KD-Tree中搜索其最近邻点。如果两者距离足够近，则认为这些点是重叠点。随后，这些重叠点被转换回各自子地图的坐标系，形成两个新的、只包含重叠信息的精简子地图，用于后续匹配。此步骤显著减少了计算量。 * 子地图到子地图匹配（SSM）：使用从重叠点提取步骤得到的精简子地图，系统再次运用ICP算法（结合点到线和点到面距离）进行精确对齐。为了加速计算，这一模块采用了多线程技术。匹配求解出的最优位姿变换（即两个子地图坐标系之间的相对位姿）被定义为子地图约束。这种约束周期性地生成，为后端优化提供了丰富的、时间跨度适中的全局信息。
4. 后端优化 系统采用因子图优化框架，具体使用了来自GTSAM库的增量平滑与建图（iSAM2） 方法，因其具有良好的实时性和鲁棒性。如图3所示，优化图（位姿图）中包含两类约束因子： * 里程计因子：由前端扫描到地图匹配产生的相邻关键帧之间的相对位姿约束。 * 子地图约束因子：由子地图匹配模块生成的子地图之间的相对位姿约束。 系统通过高斯误差模型将这两类相对位姿约束建模为因子。优化的目标是通过调整图中所有关键帧和子地图的位姿变量，使得所有因子的联合概率最大化，从而得到全局一致的最优位姿估计。这个过程有效利用了子地图约束来校正前端里程计积累的漂移。
第四部分：研究的主要结果 研究团队通过仿真实验、公开数据集测试和真实车辆实验，全面验证了SCE-LIO算法的性能，并与多个先进的SLAM算法进行了对比。
1. CARLA仿真验证 在CARLA自动驾驶模拟器中，团队在三个虚拟城市场景（Town1, Town4, Town10）生成了数据集（Carla1, Carla4, Carla10）。评估指标为绝对轨迹误差（ATE） 的均方根误差（RMSE）。如表I所示，SCE-LIO在所有场景下都显著优于基线算法LIO-SAM和Fast-LIO2。例如，在Carla4数据集上，SCE-LIO的平移RMSE相较于LIO-SAM降低了约37%，这初步证明了子地图约束机制的有效性。
2. 真实环境实验验证 实验分为公开数据集（UrbanLoco的hkdata20190117和hkdata20190317）和自采集车辆数据集（outdoor0至outdoor5共6组）两部分。对比算法包括LEGO-LOAM, Fast-LIO2, LIO-SAM和DLO。评估分为定性分析和定量分析。
定性分析（地图质量）：通过对比SCE-LIO和LIO-SAM生成的地图，可以直观看出精度差异。在图6（outdoor2数据集）的红色圆圈区域，SCE-LIO地图中的建筑结构对齐得更好，偏差更小。图7（outdoor3数据集）显示，对于大型圆柱形储罐，SCE-LIO构建的模型更完整，未出现明显错位。图8（hkdata20190117数据集）中，LIO-SAM地图在红色区域的墙壁出现了明显的“重影”（Ghosting），而SCE-LIO的地图则清晰、对齐准确。这些视觉对比强有力地说明了SCE-LIO在减少累积误差方面的优势。
定量分析（轨迹精度）：
3. 与基线算法的深度对比 以LIO-SAM为基线，SCE-LIO在ATE方面取得了最高77%、平均30%的性能提升；在FPDE方面取得了最高80%、平均约54%的提升。这一卓越性能归因于三个关键技术贡献的叠加：FPC/FE/IIP模块提升了前端匹配精度；子地图约束提供了持续、有效的全局校正信息；位姿图优化框架高效地融合了所有约束。
4. 消融实验 为了量化FPC、FE和IIP这三个模块（合称FIF模块）的贡献，研究进行了消融实验。如表V所示，比较了“包含FIF模块的SCE-LIO（W-FIF）”和“不包含FIF模块的SCE-LIO（N-FIF）”在四个数据集上的ATE。结果表明，W-FIF的性能始终优于N-FIF，平均ATE精度提升了约12%。这明确证明了这三个模块各自对提升系统整体精度具有正向作用。
5. 运行时间分析 实时性是SLAM系统的重要指标。研究分析了在不同降采样率参数下，SCE-LIO处理outdoor2数据集单帧点云的平均运行时间。如表VI所示，在合适的参数范围内（地面点降采样率ds_ground在0.25-0.45，平面点降采样率ds_surf在0.45-0.65），算法能在保持高精度的同时，将单帧平均处理时间控制在40-50毫秒之间，满足实时性要求（低于激光雷达的100毫秒周期）。
第五部分：结论与意义 本研究的结论是，所提出的SCE-LIO算法通过周期性构建和利用子地图约束，成功实现了对激光雷达惯性里程计的持续增强。该系统不仅在仿真和真实环境中均表现出比现有先进算法更高的定位精度，还能有效减少长距离运行时的累积漂移。其科学价值在于，提出并验证了一种不依赖于特定场景（如回环）的、主动的全局约束生成与优化范式，为SLAM领域解决累积误差问题提供了新的思路和技术路径。其应用价值在于，该系统在复杂、动态或缺乏明显回环的城市场景和园区环境中具有更强的鲁棒性和实用性，能够为自动驾驶车辆、移动机器人等提供更可靠、更精确的定位与地图服务。
第六部分：研究的亮点 1. 核心方法新颖：首次系统性地提出并实现了“周期性子地图约束”这一概念，将其作为独立于回环检测的、常规化的全局优化手段，是SLAM后端优化思路的一个重要创新。 2. 技术集成与优化：不仅提出了核心的子地图框架，还针对前端匹配的薄弱环节，提出了基于法向量差异的特征点对应方法、基于地面分割的特征提取策略以及融合IMU与扫描匹配的初始定位模块，形成了一个从数据预处理、前端匹配到后端优化的完整、强化的技术链条。 3. 验证全面充分：通过仿真、公开数据集、自采数据集，以及详尽的定性（地图质量）、定量（ATE, FPDE）、消融实验和运行时分析，多维度、多层次地验证了算法的有效性和优越性，结论坚实可信。 4. 兼顾性能与效率：算法设计考虑了实时性，通过KD-Tree搜索重叠点、多线程加速子地图匹配、对特征点分类降采样等手段，在显著提升精度的同时，将计算开销控制在可接受范围内。
第七部分：其他有价值的内容 本文还提到，SCE-LIO系统的后端优化框架具有可扩展性，除了子地图约束和里程计约束，还可以方便地集成来自GPS或其他传感器的约束。这为系统的进一步应用和定制提供了灵活性。此外，作者在展望中指出，未来计划融合相机传感器并引入深度学习方法生成语义信息，以进一步提升里程计的精度和环境理解能力，指明了后续研究的一个有潜力的方向。