原创：董亚微 一点人工一点智能
编辑：一点人工一点智能
原文：地下环境 | 九种3D Lidar-SLAM算法评估
当机器人处在照明条件不足且无法使用GPS的地下（SubT）环境中，其自主导航是一项极具挑战性的任务，这也促进了姿势估计和建图算法的研究。
受在此类环境中实际部署自主机器人需求的启发，本文介绍了3D SLAM算法的实验比较研究。该研究侧重于具有开源实现的最先进的激光雷达SLAM算法，这些算法是
i）仅激光雷达，如BLAM，LOAM，A-LOAM，ISC-LOAM和HDL图SLAM
ii）激光雷达惯性，如LeGO-LOAM，Cartographer，LIO-mapping和LIO-SAM。
这些方法的评估是在地下隧道执行任务期间从配备3D激光雷达Velodyne Puck Lite和IMU Vectornav VN-100的Boston Dynamics Spot机器人收集的数据集进行的。在评估过程中，将机器人位姿和SLAM算法的3D隧道重建相互比较，以找到在位姿精度和地图质量方面性能最可靠的方法。
01 介绍
同时定位与地图构建（SLAM）是一项具有挑战性的任务，它解决了未知环境下机器人自主导航的问题。在SLAM过程中，机器人使用车载感知和惯性传感器获取环境地图，同时尝试将自己定位在该地图内。鲁棒姿态估计是移动机器人控制的一项关键任务。在实验室环境中，它可以通过动作捕捉系统来解决。但并无法满足将其部署到机器人导航的任何目标区域。因此，机器人的目标之一是能够独立于基础设施进行自主导航。在户外、开阔地带，机器人可以使用全球定位系统（GPS）进行姿态估计。然而，在地下区域等GPS无效的环境中，这种解决方案将不起作用，尽快有时可以选择安装额外的设备，但是这并不是适用于任何情况的。目前，已经有了基于相机和测距传感器的SLAM算法，不过，这些算法通常需要快速移动计算机来提供实时地图构建和姿态估计。
随着便携CPU、传感器小型化和长续航电池的技术发展，SLAM算法可以在板上运行，这有助于将机器人应用带入具有挑战性的SubT环境。在这些环境中，关键因素是人类安全，可以通过机器人进行自主检查来提高人类安全，并通过提供环境重建来提高人类工人对环境的态势感知。这对地图绘制质量和定位精度提出了很高的要求。
在照明较差的SubT环境中，视觉SLAM方法往往表现出较差的性能，这是不可接受的。与之相反，基于激光雷达的方法可以在姿态估计和环境地图显示方面提供可靠的性能。然而，由于SubT环境的特殊性，如无特征、自相似的隧道区域、多尘隧道和传感器限制，它们的性能可能会随着时间的推移而下降。
在本研究中，我们将在地下环境中对九种开源的ROS兼容的3D Lidar-SLAM算法进行实验评估。
这项工作的主要贡献是：
（1）评估了九种基于SOTA激光雷达的3D SLAM方法，使用SubT数据集来证明它们在此类环境中的性能。
（2） 对所有方法的姿态估计和生成的环境3D图进行定量和定性比较，这将使机器人开发团队易于评估和理解其优缺点，包括为该应用选择SLAM算法框架。
注：评估数据集是在波士顿动力点的勘探任务期间沿着一个有多个隧道的地下区域收集的。车载传感器套件由Velodyne Puck Lite激光雷达和Vectornav vn-100 IMU组成，前者是一种用于自主导航的SoA传感器，后者是多个机器人系统中常用的硬件，与SubT研究工作高度相关。
02 SLAM算法
在这项研究中，我们选择了所有主流的SLAM算法，这些算法具备实时操作能力，并使用3D激光雷达的点云作为输入，也可以将其与IMU测量相耦合。本章节先介绍了机器人平台，其次介绍了SubT环境，最后介绍了SLAM方法。所有选择的方法都用ROS Melodic和Ubuntu 18.04进行了测试。
2.1 机器人平台和数据集
使用了由波士顿动力公司开发的机器人平台用于数据收集（图1）。该机器人能够以高达1.6米/秒的速度移动，同时携带高达14公斤的有效载荷，并穿越具有挑战性的地形。在Spot的顶部放置的有效载荷包括3D激光雷达、IMU、spotCore和电池，如图1所示。板载计算机采用Intel Core i5 CPU，16GB RAM，Ubuntu 18.04和ROS Melodic。为了为3D激光雷达提供畅通无阻的视野，它被安装为一个柱状结构，位于前端，带有独立的LED灯条Lustreon DV12V 10W，尺寸为170×15 mm，指向前、左、右。
图1 配备传感器的Spot机器人，用于在地下隧道中收集数据集
数据集是从瑞典Lulea港口的地下隧道收集的，该隧道具有手动控制的Spot，如图2所示。它被记录在一次通过中，将来自IMU和3D激光雷达的测量结果存储在ROS bag文件1中。3D重建地图的俯视图如图2所示。数据是用图1所示的传感器配置收集的，IMU发布速率设置为200Hz，3D激光雷达发射速率设为10Hz。在SubT环境中获得基本事实是一项具有挑战性的任务，因此在本研究中只提供了算法的相对比较。
图2 3D激光雷达生成的地图俯视图（数据从现场测试环境中扫描而来）。实线表示穿过隧道的导线，而箭头表示导线的方向。
在下面的小节中，我们将简要介绍所选择的算法。
2.2 BLAM
Berkeley localization and mapping（BLAM）是一个开源ROS包，用于基于图的激光SLAM。它通过使用附近姿势的扫描进行迭代最近点（ICP）扫描匹配来计算环路。对于地图优化，它使用Georgia技术平滑和建图（GTSAM）库。BLAM能够在线构建非常稠密和精确的地图，这也使得它成为一种计算成本高昂的方法Nava Chocron（2019）。
2.3 LOAM
Laser Odometry and Mapping（LOAM）或LOAM velodyne是一种实时方法，能够同时估计里程计并使用3D激光雷达绘制地图。该方法通过将SLAM任务分解为两种算法来解决SLAM任务。一个用于计算里程计，另一个用于增量地图构建，此外它还估计激光雷达的速度。LOAM没有闭环检测，这使它无法识别以前访问过的区域，相反，它实现了特征点匹配，从而确保快速的里程计计算和准确的地图构建。该方法具有IMU支持，与仅使用激光雷达相比，可以获得更高的精度。
A-LOAM是LOAM的高级实现，它使用特征库进行线性代数运算，并使用Ceres Solver 解决相应的优化问题。
Fast LiDAR Odometry and Mapping（F-LOAM）是LOAM和ALOAM的优化版本，其基于非迭代两阶段畸变补偿方法，可降低计算时间。F-LOAM结合了特征提取、畸变补偿、姿态优化和建图。
2.4 ISC-LOAM
Intensity Scan Context based Full SLAM Implementation（ISC-LOAM）是另一种为3D激光雷达设计的算法。它结合了一个全局描述子，该描述子包含几何体和强度特征。所提出的闭环检测方法基于用于位置识别的两阶段分层强度扫描上下文（ISC），这可以提高计算性能。ISC结合了基于快速二进制运算的几何索引和强度结构重新识别。
2.5 hdl graph slam
hdl graph slam是一个开源ROS包，用于与3D激光雷达SLAM。该方法基于Pose Graph SLAM，其中闭环检测是基于连续帧之间的正态分布变换（NDT）扫描匹配。与其他算法相比，无损检测方法在三维激光雷达应用中具有更好的扫描匹配性能。其中，无迹卡尔曼滤波器用于姿态估计。
2.6 LeGO-LOAM
Lightweight and ground optimized lidar odometry and mapping（LeGO LOAM）是一种用于在地形变化复杂的环境中使用无人车进行姿态估计的实时建图方法。它通过进行点云分割来利用地面分离，这可以过滤表示不可靠特征的点。LeGOLOAM使用两步优化进行姿态估计。在第一步中，从地面提取平面特征以获得z、滚转、俯仰，在第二步中，通过匹配从点云提取的特征来获得剩余的x、y、偏航。该方法还支持使用ICP实现的环路检测。
2.7 Cartographer
Cartographer是谷歌开发的一个系统，用于实时同时定位和构建2D和3D地图。它支持独特的传感器配置。对于3D SLAM，它需要具有初始猜测所需的IMU数据，以确定激光雷达扫描的方向。在最佳姿态估计下，它从激光雷达中获取扫描结果，并将其转换为概率网格，用于构建子地图。最近完成的子地图构建和扫描可以通过扫描匹配来进行回环检测。扫描匹配依赖于branch-and-boun算法。Cartographer将局部和全局SLAM方法分别结合起来。在局部SLAM中，Ceres匹配器用于找到与子地图最佳匹配的姿势。这个过程中慢慢积累的误差，可以通过基于稀疏姿态调整（SPA）的闭环机制进行消除。
2.8 LIO-mapping
LIO-mapping是一种用于3D姿态估计和建图的实时方法。在这种方法中，IMU与激光雷达紧密耦合，共同降低激光雷达和IMU测量的成本。该方法使用滑动窗口方法，通过在窗口中包括新的姿态估计和弃用最旧的姿态估计来限制计算数量。尽管如此，LIO-mapping仍然是一种计算成本很高的实时导航方法，使用16线3D激光雷达同时进行里程估计和建图需要超过0.2秒的时间。
2.9 LIO-SAM
LIO-SAM是一个实时紧耦合激光雷达惯性里程计工具包，由LeGoLOAM构建，是一种基于ICP的方法。这种方法会构建一个因子图，使得加入额外的传感器变得容易。在其实现中，LIO-SAM在贝叶斯树的增量平滑和映射方法中添加了IMU预集成。值得注意的是，这种方法处理数据的速度是实时数据的13倍。
2.10 FAST-LIO
FAST-LIO是一个激光雷达惯性里程计框架，其中使用紧耦合迭代卡尔曼滤波器将激光雷达特征点与IMU测量值融合。在这种方法中，从激光雷达点云中提取平面和边缘特征，在下一步，这些特征与IMU测量一起用于状态估计，然后使用估计的姿态将特征点匹配到全局帧中并更新全局地图。
FAST-LIO的全局帧被定义为第一个IMU的帧。这意味着IMU在一开始的x-y-z轴将是全局帧的x-y-z轴。所以机器人的方向可能不像你想象的那样。
表1总结了所有SLAM方法、所需硬件及其功能，下一步是在SLAM方法上运行记录的数据。
03 实验评估和讨论
在本节中，基于第2.1小节中引入的收集数据集，在SubT环境中对SLAM方法进行评估。
SLAM方法的基准测试是在配备Intel i7第9代CPU、64GB RAM、Ubuntu 18.04和ROS Melodic的计算机上进行的。对于算法的比较，我们已经尽了最大努力，根据自定义的package给定硬件配置调整所有方法。
3.1 轨迹的评估和比较
从地下隧道收集的数据集代表了基于激光雷达的SLAM算法在缺乏特征、重复性和窄尺寸方面具有挑战性的环境，而另外两个闭环可以用来评估方法的回环检测性能。我们尽最大努力对所有SLAM方法进行了彻底调整，然而，根据多次评估，我们得出结论，VLP16-Lite的视场不足以在垂直维度上捕获足够的数据，这导致z轴的高度不确定性，即使使用IMU也无法补偿，如图3所示。
根据我们用DeWALT激光水准仪从隧道入口到隧道中心50米的坡度测量，我们得出隧道具有正坡度，倾角等于0.95度。然而，即使在隧道的直线部分，也没有一种备选的方法能够正确估计倾角，例如，对于Cartographer来说，倾角估计约为2.91度，对于hdl graph slam来说，倾角约为0.1度，对于LIO-SAM来说，倾角大约为-1.4度，对于Fast LIO来说，倾角为-3.7度。值得注意的是，除Cartographer和hdl graph slam外，所有方法都估计了负斜率，这可能意味着其它的融合IMU数据的方法强烈依赖于激光雷达传感器，而不是IMU。
与垂直维度类似，在水平维度中，VLP16-Lite具有高数据冗余，这允许所有方法来处理x和y 2D姿态估计，如图4所示。
从图2可以看出，所有SLAM方法估计的2D姿态与隧道地图一致。总的来说，所有算法都能够估计隧道沿线的po-e，hdl graph slam作为一个明显的异常值。我们将在下一小节中继续进行这一分析，我们将在其中评估生成的地图。对于相关方法的评估，我们计算并比较了总行驶距离，如表2所示，并计算了[251:20 256:66]米的置信区间，置信水平为95%。由此可以看出，BLAM、A-LOAM、LeGO LOAM、Cartographer和FastLIO距离测量具有更值得信赖的价值。
3.2 点云的评估与比较
建图结果可以看到有环路闭合和没有环路闭合的方法之间的明显差异，这可能从定位结果中不那么明显。在图5中，描述了每个算法生成的地图。基于此，可以说loam velodyne、A-loam、F-loam和LIO-mapping中缺乏闭环，导致点云建图错位。根据这组方法，FastLIO虽然没有循环闭合功能，但可以有效地处理环境并生成正确的建图。
在实现了回环检测能力的方法中，可以说除了BLAM之外的所有方法都可以提供正确的建图。hdl graph slam虽然能够产生正确的建图，但在姿态估计中仍然是一个异常值，尽管这一发现可能是由于我们调整的潜在缺陷。根据点云配准方法的不同，在默认配置中，SLAM方法会产生以下数量的点，如表2所示，从中我们可以看出，存储地图的内存占用最少的方法是ISC-LOAM，而Fast LIO是需要最大的内存。尽管如此，地图中的总点数可以通过额外的方法配置来减少，例如在图5中，LIO-SAM方法的地图大小减少到4820点。
因此，如果我们在隧道的连接区域重新绘制图4，并只保留在图6中所示的在姿态估计和建图中提供可靠结果的方法，可以说所有方法都表现出了良好的稳定性和性能。然而，正如可以注意到的那样，在交界区域，LeGO LOAM执行了环路检测，因此对地图进行了校正，但没有对轨迹进行校正。值得注意的是，IMU会影响方法的性能，因为Fast LIO与具有回环检测的方法的性能相同。
04 结论
在本文中，我们比较了最新的和SoA激光雷达SLAM方法在苛刻的SubT环境中的性能。为此，我们使用配备了自主软件包的Spot机器人收集了数据集，并分别配置了所有SLAM方法。基于本文中进行的SLAM方法比较，可以得出结论，所配备的3D激光雷达不足以进行3D姿态估计，由于缺乏特征，导致z轴上的显著漂移。因此，我们的分析集中在2D姿态比较上。评估结果表明，BLAM、A-LOAM、LeGO LOAM、Cartographer和Fast LIO比其他方法产生了更可信的结果。尽管与Fast LIO相比，生成的地图非常稀疏，但ISC-LOAM是用于地图存储的内存成本最低的方法。此外，可以得出结论，将IMU与激光雷达融合有助于校正姿态估计。
我们之后的目标是使用具有更宽视场的激光雷达传感器，并在更大的环境和不平坦的地形中进行评估，这对腿式机器人来说是一个挑战。