室内环境下鲁棒的多LiDAR惯性里程计和建图技术

文章：Robust Multi-Modal Multi-LiDAR-Inertial Odometry and Mapping for Indoor Environments

作者：Li Qingqing , Yu Xianjia , Jorge Pena Queralta，Tomi Westerlund

编辑：点云PCL

代码：https://github.com/TIERS/multi-modal-loam

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。未经博主同意请勿擅自转载。

摘要

将多个LiDAR传感器集成可以显著提高机器人对环境的感知能力，使其能够捕捉同时定位和建图（SLAM）所需的足够测量数据，实际上，在机器人应用中，固态LiDAR可以以较低的成本带来高分辨率，从而取代传统的旋转式LiDAR。然而，它们较小的视场（FoV）限制了性能，尤其是在室内，本文提出了一种紧密耦合的多模态多LiDAR惯性SLAM系统，用于测量和建图任务，通过利用固态和旋转式LiDAR以及内置的惯性测量单元（IMU），我们在各种具有挑战性的室内环境中实现了稳健低漂移的自我估计和高分辨率地图（例如小型、无特征的房间）。首先，我们使用空间-时间校准模块对传感器之间的时间戳进行对齐和校准外参，然后，我们从LiDAR数据中提取两组特征点，包括边缘点和平面点，接下来，使用预集成的IMU数据，对LiDAR点云数据应用畸变校正模块。最后，将校正后的点云合并为一个点云，并使用基于滑动窗口的优化模块进行处理，通过大量的实验结果，该方法在不同的环境中展现出与最先进的仅使用旋转式LiDAR或仅使用固态LiDAR的SLAM系统相竞争的性能。更多的结果、代码和数据集 https://github.com/TIERS/multi-modal-loam

图1：我们的方法在具有挑战性的环境中展示了高分辨率的地图结果和鲁棒的性能

主要贡献

本文是迄今为止第一个能够在单个估计和优化框架中有效集成具有异构扫描模式的LiDAR传感器的多LiDAR惯性SLAM系统，本文受到我们之前工作中不同LiDAR传感器存在的局限性的启发，其中我们展示了低成本固态LiDAR在室外环境中优于高分辨率自转LiDAR，同时在室内环境中表现不佳。我们的工作的独特特点和主要贡献可以总结如下： 

1）提出了一个多模态LiDAR空间时间校准和特征提取的完整解决方案。该方法采用基于ICP的扫描匹配方法来获取外参，基于分割和合并的时间戳对齐，以及针对旋转式和固态激光雷达的统一通道特征提取。

 2）设计和实现了一种新颖的紧密耦合的多模态多LiDAR惯性建图框架，能够将具有不同扫描模式和IMU的LiDAR合并用于测距估计。 

3）展示了一种SLAM方法，利用低成本自转式LiDAR和固态LiDAR，其在高分辨率地图的细节水平上表现优于现有技术。为紧密耦合的多模态LiDAR和IMU融合提供了一种独特的开源实现，可供社区使用。通过广泛的实验，我们提出的方法在各种环境中展现出现有技术的能力，并具有可比的测距估计和更高的地图质量。

为了提高SLAM系统的稳健性，我们提出了一种新颖的紧耦合多模态多LiDAR惯性测距和建图系统，利用旋转式LiDAR的大水平视场和固态LiDAR的稠密测量，如表格I所示。

主要内容

图2：所提出的多模态LiDAR惯性测距与建图框架的流程图，系统从预处理模块开始，该模块接收传感器输入并进行IMU预积分、校准和去畸变处理。扫描配准模块提取特征并将其发送到紧密耦合的滑动窗口测距模块，最后，建立一个姿态图以维护全局一致性

为了将高情境感知能力和鲁棒性相结合，提出了多模态激光雷达惯性里程计和建图方案，系统设计受到了我们之前工作[15]的启发，其中固态激光雷达在户外表现出显著性能，但在室内测试中均失败。本文考虑由多个激光雷达和惯性测量单元组成的感知系统，激光雷达传感器不由基于硬件的外部时钟触发，所提出方法的流程如图2所示，硬件系统如图5所示，我们的硬件由Velodyne VLP-16旋转激光雷达，Livox Horizon低成本固态激光雷达和其内置的惯性测量单元组成。

该系统以数据预处理模块开始，其中IMU数据进行预积分，校准传感器之间的外参，将具有不同开始时间的点云的时间戳对齐，利用IMU预积分结果将多个激光雷达的点云进行去畸变。预处理后，提取表示平面点和边缘点的特征点云并将其合并为一个点云。合并的特征点云将被发送到滑动窗口里程计模块，其中该特征点云将与局部地图进行匹配。通过预积分IMU，固定大小的特征点云和局部地图，估计并优化6自由度的自我运动和IMU参数。在后端，系统维护具有选择的关键帧的全局姿态图。使用ICP在关键帧基础图中检测回环闭合，并调用全局图优化来保证重构的地图具有全局一致性。

图4：办公室环境下的多模态LiDAR外参校准结果，红色点来自Horizon，绿色点来自VLP-16，俯视图（左）和匹配细节（右）

图3：办公室环境中提取的特征点（左）和一个门物体的放大视图（右）。Velodyne的平面和边缘特征点分别为红色和蓝色，Horizon的特征点分别为绿色和白色

实验

A. 传感器配置和实现

我们使用ROS Melodic环境，用C++实现了所提出的多模态多LiDAR惯性里程计和建图系统，以便在机器人社区中共享。如图2所示，该系统结构化为四个节点：预处理、特征提取、扫描匹配和图优化。因子图优化由GTSAM 4.0 维护，非线性优化由Ceres Solver 2.0 执行。本文提出的框架使用Velodyne VLP-16(V)、Livox Horizon(H) 3D LiDAR及其内置IMU(I)收集的数据集进行验证。运行Velodyne和Horizon的ROS驱动程序，并以rosbag格式记录数据。

B. 定性实验

在将我们提出的系统与典型且具有挑战性的室内环境进行比较：办公室、长廊和大厅。如图5所示。我们将我们提出的方法与几种最先进的SLAM算法进行比较：LeGO-LOAM、Fast-LIO和LILI-OM。LeGO-LOAM 是仅使用LiDAR的里程计，Fast-LIO和LILI-OM 是紧耦合的LiDAR惯性里程计，可以同时使用固态和旋转LiDAR。Fast-LIO 采用紧耦合的迭代扩展卡尔曼滤波器框架和iKD-tree数据结构，表现出高效且稳健的性能 。与我们提出的方法类似，LILI-OM采用基于关键帧的滑动窗口优化，但仅融合单个LiDAR和预积分的IMU测量。为了比较里程计的准确性，所有三个数据集的起点和终点都相同，起点和终点之间的平均平方距离（MSE）被视为误差。所选方法在所有数据集中产生的结果如表II所示。

C. 室外建图

对地图点云的定性分析显示出高水平的细节，然而，外参校准方法是为具有足够的边缘和平面特征的室内环境设计的，两个LiDAR传感器之间的重叠FoV内；因此，外部参数在森林环境中未经很好的校准，导致最终地图的清晰度降低。在任何情况下，这证明了潜在的推广到更不规则的环境，并且即使在亚优化的外部参数校准下，也能够实现高稠密建图，提出的多模态多LiDAR惯性方法展示了与其他选择的方法相比的竞争性和一致性性能。然而，尽管利用了来自环境的更多测量，但我们观察到我们的方法并不总是优于其他方法，其中一个可能的原因是传感器之间的时间同步不准确，我们的LiDAR之间的时间同步在软件层面上具有亚微秒的精度，每个点的不准确时间戳会在云畸变和云合并步骤中给系统带来误差，这很难消除。

图8：使用所提出的方法在户外进行建图的结果：城市街道（左）和森林环境（右）

D. 建图质量比较 

多LiDAR系统的一个关键优势是其高感知能力，在这里，我们比较了分辨率方面的建图质量，我们的提出方法的部分映射结果如图1a和1b所示，其中颜色表示强度值。从结果中，我们可以看到许多物体（例如门，墙上的字母）。我们比较了我们提出的方法在图6a和Fast LIO（VI）在图6b中的建图结果，我们的方法在办公室环境中的实验中表现最稳定，通过放大相同区域，我们可以看到使用我们的方法生成的地图中，从墙壁和电视机中得到的点云更加均匀。

图6：在办公室数据集序列中，地图细节的定性比较，点的颜色表示原始传感器数据提供的反射率，点的大小为1 cm³，透明度设置为0.05，中间两列显示了墙壁（顶部）和电视机（底部）的放大视图

表III中显示了每帧的平均运行时间，表IV中显示了特征数量，预处理和特征提取非常轻量级，运行时间主要受基于滑动窗口的姿态估计和优化的影响。

总结

本文介绍了一种紧密耦合的多模态多LiDAR惯性测距和建图框架，并采用滑动窗口优化姿态估计。据我们所知，这是第一个在单个框架中利用旋转LiDAR和固态LiDAR的优点的SLAM算法，具体而言，在适当组合低成本传感器的情况下，可以实现高鲁棒性测距和高质量建图，所提出的系统有效地将固态LiDAR提供的密集点云的高环境感知与旋转LiDAR提供的更大视场融合，尽管在某些环境中，我们的测距精度与其他方法相比，但我们方法的关键特性是环境间的一致性和特别是更高质量的地图，可以看到环境细节，在接下来的步骤中，我们计划进一步探索提供类似于图像数据的多模态LiDAR传感器和单个框架内的多IMU融合。

参考代码

1 https://github.com/Livox-SDK/livox_ros_driver

2 https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

3 https://github.com/hku-mars/FAST_LIO

4 https://github.com/KIT-ISAS/lili-om