一种结合激光雷达（LiDAR）和惯性测量单元（IMU）紧密耦合技术以及扫描环境的室内地图构建算法：IS-LEGO-LOAM 云俊英（Junying Yun） 刘周峰（Zhoufeng Liu） 万新通（Xintong Wan） 段格菲（Gefei Duan） 田博文（Bowen Tian） 高雅静（Yajing Gao）

《Sensors》：An Indoor Mapping Algorithm Fusing LiDAR-IMU Tightly Coupled Fusion and Scan Context: IS-LEGO-LOAM Junying Yun, Zhoufeng Liu, Xintong Wan, Gefei Duan, Bowen Tian and Yajing Gao

　　摘要
室内环境通常包含许多结构特征稀疏的区域，例如长走廊、大型中庭和玻璃幕墙等场景。这些条件可能导致循环闭合检测困难以及累积的定位误差，从而导致定位漂移，甚至在地图构建过程中出现映射失败。本文提出了一种名为IS-LEGO-LOAM的室内映射算法，该算法结合了紧密耦合的LiDAR-IMU融合技术和Scan Context。构建了一种紧密耦合的LiDAR-IMU里程计，并设计了一种自适应协方差矩阵，以解决由于室内特征点稀疏而导致的异常LiDAR回波和有效特征提取不足的问题。通过引入Scan Context全局描述符，并采用向量最近邻搜索和相似性得分匹配策略，缓解了大规模场景中的漂移问题。最后，在KITTI数据集和真实世界场景中分别进行了验证。实验表明，改进后的IS-LEGO-LOAM实现了更优越的映射性能。

1. 引言
随着移动机器人在各种应用场景中的持续探索和进步，它们的应用范围变得越来越广泛和多样化。同时定位与映射（SLAM）已在室内机器人领域得到广泛应用。这是一种仅使用机载传感器在未知环境中进行同时定位和地图构建的技术，无需预设基站[1]。SLAM框架主要由四个部分组成：前端匹配、后端优化、循环闭合检测和地图构建[2]。图1展示了SLAM框架的组成示意图。根据前端传感器的类型，SLAM可以分为以摄像头作为前端传感器的视觉SLAM和以LiDAR作为前端传感器的LiDAR SLAM[3]。摄像头传感器成本低廉，可以获取丰富的特征信息；然而，它们无法直接测量距离，并且对光照变化非常敏感，导致精度较低[4]。相比之下，LiDAR不受光照影响，响应速度快，测距精度极高，但相对较昂贵[5,6]。随着科学技术的进步和对LiDAR应用需求的增加，LiDAR的大规模生产和普及能力逐渐提高，基于LiDAR的SLAM技术发展迅速。目前，LiDAR SLAM技术可以分为2D LiDAR SLAM和3D LiDAR SLAM[7]。2D LiDAR SLAM采用2D LiDAR作为前端传感器，只能感知环境的单平面信息，因此构建的地图大多是二维的。而在3D LiDAR中，多束激光以一定角度排列，通过光学测距旋转一圈，可以捕获大量激光点，这些激光点汇聚成反映周围环境真实信息的点云。3D LiDAR提供了直观的映射效果和高的测距精度；此外，在操作过程中不受光照变化和视角的影响。因此，3D LiDAR已成为地图构建中不可或缺的传感器。
目前，大多数具有良好地图构建性能的3D LiDAR SLAM算法应用于室外场景，尚没有专门针对室内场景优化的3D LiDAR SLAM算法。室内场景与室外场景不同，因为它们是结构化的环境。与室外场景相比，室内场景的特征点明显较少，无特征的环境会严重影响基于LiDAR的SLAM框架的性能，导致室内地图构建时出现较大漂移甚至失败。因此，本文提出了一种适用于室内的IS-LEGO-LOAM方法。(1) 在算法的前端构建了一个紧密耦合的LiDAR-IMU里程计，并设计了一种自适应协方差矩阵，以自动调整特征匹配的自由度数量，从而解决异常LiDAR回波和有效特征点提取不足的问题；(2) 在算法的循环闭合检测部分引入了Scan Context全局描述符，并采用向量k最近邻搜索和相似性得分匹配策略来缓解大规模场景中的漂移问题。

2. 相关工作
LiDAR里程计依赖于点云匹配来实现里程计算。稀疏的点云、不足的特征点以及长走廊等结构化环境会严重降低LiDAR里程计的准确性和鲁棒性[8]。研究人员通过融合IMU数据来提高LiDAR里程计的准确性和鲁棒性，这被称为LiDAR惯性里程计（LIO）[9]。LiDAR和IMU的融合主要通过两种方法实现：滤波和优化[10]。代表性的基于滤波的方法包括LINS[11]、FAST-LIO[12]、FAST-LIO2[13]、LIO-GVM[14]和Point-LIO[15]。基于卡尔曼滤波及其变体的方法将IMU预积分和LiDAR匹配误差纳入状态估计框架，实时更新姿态和传感器噪声。它们具有快速响应和低计算开销的特点，但容易受到模型线性化误差的影响。基于优化的方法基于因子图或滑动窗口，共同优化多帧LiDAR-IMU数据的约束，并通过全局调整减少累积误差，从而实现更高的定位精度。然而，它们对计算资源的要求较高，需要在实时性能和优化规模之间取得平衡。图优化的典型代表包括hdl_graph_slam[16]、LIO-SAM[17]、D-LIOM[18]和Spin-LOAM[19]。
循环闭合检测是SLAM系统的核心功能模块。其关键价值在于通过重新访问场景识别和引入全局约束来抑制累积的轨迹漂移并优化地图一致性，使其成为支持大规模室内环境高精度映射的关键技术[20]。目前，LiDAR SLAM中的循环闭合检测已经形成了系统的研究框架。全球学者围绕三个核心目标进行了深入研究：鲁棒性改进、检测效率优化和场景适应性扩展，产生了两种主流技术范式：基于几何特征匹配的方法和基于全局描述符的方法[21,22]。基于几何特征匹配的方法关注点云的几何和拓扑信息，并通过直接帧间特征匹配来识别循环闭合候选者。其理论基础由迭代最近点（ICP）算法[23]及其变体（如正态分布变换（NDT）[24]奠定。然而，这类方法在特征稀疏的场景中容易陷入局部最优解。基于全局描述符的方法通过编码场景的整体结构来减少对局部特征的依赖，因此成为研究热点。Scan Context最初由韩国KAIST大学提出[25]，通过极坐标网格划分和高度信息统计构建旋转不变表示。Kim G.等人[26]后来提出了一种基于Scan Context的空间增强描述符，提高了算法对横向和旋转变化的鲁棒性，并提高了计算效率。然而，这种方法计算复杂度高，难以在实际场景中应用。在稀疏或退化环境中，仅依靠后端匹配不足以确保鲁棒性。明确处理特征可靠性和动态/异常值去除至关重要。现有工作[27,28]表明，语义感知滤波、动态特征拒绝和特征分级可以有效提高鲁棒性，为复杂环境中的可靠特征选择提供了有价值的指导。

3. 方法论
3.1. IMU融合的I-LEGO-LOAM
LiDAR的完整扫描产生一个激光点云，可以通过点云配准获得姿态信息。IMU可以根据加速度计和陀螺仪收集的数据提供相应的速度和位置信息。在本文中，IMU以两种方式融入系统。首先，它帮助LiDAR消除点云畸变。其次，它应用于LiDAR里程计，其中采用扩展卡尔曼滤波器（EKF）在配准后融合姿态信息和IMU的实时输出，以获得更准确的姿态估计。本文采用了一种轻量级的紧密耦合架构，结合了IMU辅助的LiDAR畸变校正和EKF融合，专为室内环境中的低功耗嵌入式平台设计。IMU融合的LEGO-LOAM的示意图如图2所示。
3.1.1. IMU辅助的LiDAR畸变校正
在3D LiDAR的实际应用中，配备LiDAR的移动机器人通常是运动的而不是静止的。多束激光围绕中心轴旋转以获取环境信息。如果移动机器人在扫描过程中移动，则3D LiDAR点云的一帧的起始扫描点和结束扫描点将不一致。这会导致点云中每个点的坐标系统不同，从而使点云无法反映周围环境的真实信息。这种现象称为点云畸变。在本文中，采用了一种IMU辅助的方法来校正畸变的点云。获取的点云数据根据其采集顺序打上时间戳。其基本原理如图3所示。
3.1.2. 自适应协方差矩阵
机器人协方差矩阵用于表征LiDAR的测量误差和不确定性。它包括LiDAR在X、Y和Z方向上的位置误差，以及滚转、俯仰和偏航误差。这些误差的大小显著影响LiDAR里程计算法的性能和精度。在LEGO-LOAM算法中，协方差矩阵的值可以手动设置或通过实验测量获得。适当配置协方差矩阵可以提高LiDAR里程计的准确性和稳定性，而过大或过小的协方差值可能导致算法不稳定或精度下降。协方差矩阵通常是对角矩阵，其形式如方程（2）所示[2]。
其中，、、、、、是通过实验测试确定的常数值。函数决定了在扫描匹配中保留的自由度（DOFs）数量。这里，表示匹配的边缘特征点数量，表示匹配的平面点数量。函数的值决定了可用于匹配的自由度数量。在匹配过程中，较小的值表示只匹配了点云的一部分自由度，而较大的值表示使用了更多的自由度，从而获得更准确的匹配结果。然而，较大的值也会增加计算负担。函数在方程（3）中表达[3]。
随着特征数量的减少，SLAM系统的姿态估计误差也会增加。在本文中，LiDAR里程计中的协方差矩阵由给出[4]。
这些值通过在KITTI数据集和室内环境中进行重复实验统计得到，确保系统在正常条件下的稳定运行。、、、、、调整为、、、、、。的下限设置为0.005。当接近0时，接近其最大值；当和分别接近和时，接近其最小值。需要明确的是，所提出的自适应协方差矩阵不仅仅是基于边缘和平面特征数量的简单匹配置信度调整。相反，它在紧密耦合的LiDAR-IMU里程计的状态估计框架内执行在线不确定性建模，用于观测噪声的处理。与使用特征计数的简单后验置信度加权方法不同，我们的方法直接将特征丰富度映射到观测不确定性，并将其整合到噪声模型中进行优化和过滤，使估计器能够感知观测质量的变化，因此在理论公式和工作机制上有着根本的不同。所采用的函数具有明确的界限、单调性以及高计算效率，确保了数值稳定性和实时性能，使其成为室内稀疏特征场景的合理且有效选择。同时，这一策略与近期SLAM研究中广泛采用的质量感知加权范式保持一致。Adham等人[29]提出了基于对齐可靠性的自适应协方差方法来处理城市环境中的几何退化问题，而Mansour等人[30]则采用了自适应质量门控来在低质量观测下进行稳健的航向估计；这两种方法都根据观测质量动态调整约束可靠性。我们的方法遵循这一核心思想，使用有效特征的数量来表示观测质量，并通过自适应协方差来调整不确定性。值得注意的是，这种方法通过全局协方差缩放增强了在弱特征环境中的鲁棒性，但并未明确建模方向退化问题，因此无法完全解决走廊问题，即在长走廊场景中纵向不确定性会显著增加。未来的工作将探索方向感知的自适应策略，以更精确地处理各向异性观测退化问题。

3.2. 带有扫描上下文融合的闭环检测
LiDAR的采样频率为10 Hz，连续两个点云帧之间的差异相对较小。如果将每个扫描的帧都包含在闭环检测中，会显著减慢算法的速度。因此，本文根据时间间隔选择了一定数量的数据帧作为关键帧。如果关键帧选择的时间间隔过大，关键帧的数量将不足以准确表示真实环境；相反，如果间隔过短，特别是在障碍物密集且机器人移动缓慢的室内映射过程中，连续两帧对应的机器人位置之间的物理距离会变得很小。在这种情况下，系统很可能会在当前点云帧和前一帧之间建立闭环检测关系。如图4所示，这种闭环对应于一个无效的循环。为了避免这种无效的闭环检测，本章设置了帧计数阈值和时间阈值，具体过程如图5所示。当接收到新的激光点云帧时，该帧会被分割两次以生成扫描上下文全局描述符，然后存储在历史描述符容器中。环向量k使用方程（5）和（6）计算得出。历史描述符容器中的描述符数量设置了30帧的阈值。如果历史描述符的数量少于30个，则继续添加新的点云帧；如果数量超过30个，则所选帧之间的点云间隔必须大于30米，并基于环向量k构建KD树。在KD树最近邻搜索之前应用时间阈值，要求当前帧与历史帧之间的时间差T大于30秒。最后，使用两步最近邻搜索算法来找到相似的帧，并计算当前帧与这些候选帧之间的相似度得分。选择相似度得分最低的帧作为闭环检测的循环帧。通过上述帧计数和时间阈值设置，可以有效避免连续帧之间的无效循环。

（5）（6）
其中表示扫描上下文描述符中的一行，表示扇区数量。

4. 实验与分析
4.1. 实验环境
（1）数据集测试环境
本文提出的算法在KITTI 07数据集上进行了测试。KITTI数据集用于验证所提算法相对于基线方法的精度提升，而不是展示其在室内场景中的性能。实验中使用的处理器是第12代Intel(R) Core(TM) i9-12900H，显卡型号是GeForce RTX 3060 Laptop GPU，操作系统是Ubuntu 18.04，相应的ROS版本是ROS Melodic。实验中使用的LiDAR实验参数列在表1中。
表1. LiDAR实验参数设置。
实验参数设置如表1所示。其中，N_SCAN表示LiDAR线的数量，Horizon_SCAN表示LiDAR每完整旋转的采样点数；ang_res_x和ang_res_y分别表示LiDAR的水平和垂直角分辨率；ang_bottom表示LiDAR与水平方向之间的角度；groundScanInd表示对应于地面点的LiDAR线索引。EVO评估工具用于定量测试。采用命令evo_ape tum tum_07_gt.txt I-lego_loam.txt -r full -va --plot --plot_mode xyz和evo_ape tum tum_07_gt.txt IS_lego_loam.txt -r full -va --plot --plot_mode xyz来评估映射过程中的绝对姿态误差。evo_ape参数的详细信息列在表2中。
表2. evo_ape参数。

（2）真实世界场景测试环境
为了比较LEGO-LOAM和IS-LEGO-LOAM在真实世界场景中的表现，本文构建了一个基于3D LiDAR传感器的移动机器人平台，可以实现激光SLAM功能。在移动机器人系统中，3D LiDAR和IMU负责收集外部环境信息并将其发送到工业计算机进行处理。MCU负责驱动直流电机和传感器以及执行相应的数据处理。机器人平台如图6所示。
图6. 移动机器人平台。
当机器人在建筑物内导航时，它主要沿走廊来回移动，同时也会穿过门。本文选择了工程楼内的555 Studio和工程楼五楼的长走廊作为实验环境。
① 室内场景
工程楼内555 Studio的整体环境如图7a所示。工作室长16米，宽14.68米，工作站分布在中间，橱柜偶尔放置在周围墙壁上，上方有交叉的梁和柱子。使用笔记本电脑通过绕工作室走一圈来控制机器人构建地图，机器人的映射过程如图7b所示。
图7. 工作室环境和映射过程：（a）工作室环境；（b）映射过程。

② 大规模走廊场景
在机器人完成工作室的映射后，选择了一个大规模走廊环境进行测试。大规模走廊环境如图8a所示，通常是矩形的。该环境结构复杂，距离长且环境特征稀疏，包含许多相似的场景结构，对机器人定位和映射提出了巨大挑战。走廊长68米，宽65米，不同部分的走廊宽度不一致。机器人被控制在该长走廊环境中进行映射，映射过程如图8b所示。
图8. 实际映射场景：（a）长走廊的平面图；（b）映射过程。

4.2. 测试结果与分析
4.2.1. 数据集环境测试
（1）带有IMU融合的I-LEGO-LOAM
使用evo_ape进行评估后，LEGO-LOAM、I-LEGO-LOAM与真实值之间的绝对姿态误差（APE）如图9所示。图9a展示了LEGO-LOAM算法的绝对姿态误差，图9c展示了I-LEGO-LOAM算法的绝对姿态误差。图9b和d分别展示了LEGO-LOAM和I-LEGO-LOAM的轨迹图对应的绝对姿态误差值。虚线代表KITTI数据集提供的真实值，而彩色轨迹是由算法生成的地图轨迹。图中不同的颜色表示不同的绝对姿态误差幅度，从蓝色逐渐增加到红色。表3详细列出了LEGO-LOAM和I-LEGO-LOAM算法的绝对误差比较。
表3. LEGO-LOAM、I-LEGO-LOAM与真实值之间的绝对误差比较：（a）LEGO-LOAM的APE值；（b）LEGO-LOAM轨迹的APE值；（c）I-LEGO-LOAM的APE值；（d）I-LEGO-LOAM轨迹的APE值。
通过比较LEGO-LOAM算法和IMU融合的I-LEGO-LOAM算法的映射轨迹和绝对误差，可以从图9和表3中清楚地看到，带有IMU融合的I-LEGO-LOAM算法的轨迹更接近真实值，其映射精度也优于传统的LEGO-LOAM算法。

（2）进一步结合扫描上下文的IS-LEGO-LOAM
基于前一节中的I-LEGO-LOAM算法，本节对引入了扫描上下文全局描述符的IS-LEGO-LOAM算法、LEGO-LOAM算法以及I-LEGO-LOAM算法之间的绝对误差进行了比较实验。使用evo绘图工具来绘制IS-LEGO-LOAM与真实值之间的绝对误差。图10显示了IS-LEGO-LOAM算法与真实值之间的绝对误差，其中图10a展示了IS-LEGO-LOAM算法的APE值，图10b展示了IS-LEGO-LOAM算法轨迹的APE值。表4是LEGO-LOAM算法、I-LEGO-LOAM算法和IS-LEGO-LOAM算法之间的绝对误差比较表。
图10. IS-LEGO-LOAM的绝对误差比较：（a）IS-LEGO-LOAM的APE值；（b）IS-LEGO-LOAM轨迹的APE值。
表4. LEGO-LOAM、I-LEGO-LOAM和IS-LEGO-LOAM之间的误差比较。
通过上述实验比较，可以从表3和表4中清楚地看到，在将IMU集成到LEGO-LOAM算法后，I-LEGO-LOAM算法的所有性能指标都有所提升。随着进一步引入扫描上下文全局描述符，IS-LEGO-LOAM算法的性能得到了进一步提升。在数据集上的对比验证表明，改进后的IS-LEGO-LOAM算法在所有方面都优于传统的LEGO-LOAM算法，其中标准差减少了78.65%。

4.2.2. 真实场景测试
（1）室内场景
图11a和c显示了LEGO-LOAM算法的映射结果，而图11b和d显示了IS-LEGO-LOAM算法的映射结果。
图11. LEGO-LOAM和IS-LEGO-LOAM的映射效果图：（a）LEGO-LOAM的整体映射效果；（b）IS-LEGO-LOAM的整体映射效果；（c）LEGO-LOAM的映射细节；（d）IS-LEGO-LOAM的映射细节。
工作室场景较小，映射时间较短。从图11a和b可以清楚地看到，LEGO-LOAM和IS-LEGO-LOAM算法都能构建出完整的地图。通过比较图11c和d中的地图细节，可以观察到使用LEGO-LOAM算法时工作站映射不完整，点云地图相对稀疏。相比之下，IS-LEGO-LOAM算法能够生成包含完整工作站且点云地图相对密集的地图。

（2）大规模走廊场景
图12a显示了LEGO-LOAM算法构建的地图，图12b显示了IS-LEGO-LOAM算法构建的地图。图12c和d分别详细比较了LEGO-LOAM和IS-LEGO-LOAM构建的地图。
图12. LEGO-LOAM改进前后的地图详细比较：（a）长走廊中LEGO-LOAM的整体映射效果；（b）长走廊中IS-LEGO-LOAM的整体映射效果；（c）LEGO-LOAM的映射细节；（d）IS-LEGO-LOAM的映射细节。
机器人在长走廊中进行映射。以图13的俯视图为例，左侧和右侧对应于建筑物的立面，如图13中的红色框所示。上下方的黄色框代表房间，因此机器人在上下走廊不会产生像在左右两侧那样的粗糙边缘现象。LiDAR的最大测量距离为150米，当机器人通过左右走廊时，可以在上下房间周围看到微弱的激光光束。
图13. 地图细节说明。通过对比图12和图13可以清楚地看出，在结构相似但特征不足的长走廊场景中，使用LEGO-LOAM算法进行映射时，机器人会出现漂移现象，从而导致映射失败。而在相同的环境中，使用IS-LEGO-LOAM算法的机器人能够构建出完整的地图，且不会出现漂移或故障。实验表明，IS-LEGO-LOAM算法相较于LEGO-LOAM算法具有显著的优势。

5. 结论

本文提出了一种IS-LEGO-LOAM室内映射方法，该方法整合了紧密耦合的LiDAR-IMU融合技术和Scan Context机制。其目的是解决在室内结构特征稀疏的环境中（如走廊、大型中庭和玻璃幕墙）由于有效特征点提取不足而导致的定位漂移和映射失败问题。在算法的前端，IMU数据被紧密融合，并采用了自适应协方差矩阵，有效缓解了室内结构稀疏环境中有效特征不足的问题。同时，在闭环检测模块中引入了Scan Context全局描述符，并设置了帧数和时间阈值，以减少大规模场景中的漂移现象。

在KITTI数据集上对LEGO-LOAM、I-LEGO-LOAM和IS-LEGO-LOAM进行的比较显示，改进后的IS-LEGO-LOAM在所有误差指标上均优于传统的LEGO-LOAM。最后，在室内工作室和大型走廊环境中分别对IS-LEGO-LOAM和LEGO-LOAM进行了对比实验。实验结果表明，IS-LEGO-LOAM的优势明显，其标准差比原始算法降低了78.65%。

需要注意的是，本文主要关注室内稀疏特征、结构退化以及LiDAR异常回波现象，并未涉及动态物体干扰或动态特征过滤问题。 recent研究指出，在稀疏或退化的环境中，仅依靠后端优化无法保证系统的鲁棒性，明确建模特征可靠性以及异常特征过滤是必不可少的。在未来的工作中，我们将整合特征分级和动态感知机制，以进一步提高系统在复杂室内环境中的鲁棒性。

此外，由于当前实验平台和数据集条件的限制，尚未进行精细的室内定量比较。在未来的工作中，我们将构建专门的室内实验平台，并收集涵盖典型场景（如弱纹理和长走廊）的标准数据集，进行全面的定量测试和消融分析，以进一步验证所提出方法在室内环境中的有效性和鲁棒性。