港大开源Voxel-SLAM：完整、精确、多功能的激光雷达-惯性SLAM

0. 论文信息

标题：Voxel-SLAM: A Complete, Accurate, and Versatile LiDAR-Inertial SLAM System

作者：Zheng Liu, Haotian Li, Chongjian Yuan, Xiyuan Liu, Jiarong Lin, Rundong Li, Chunran Zheng, Bingyang Zhou, Wenyi Liu, Fu Zhang

机构：香港大学

原文链接：https://arxiv.org/abs/2410.08935

代码链接：https://github.com/hku-mars/Voxel-SLAM

1. 摘要

在这项工作中，我们提出了Voxel-SLAM:一个完整、精确和通用的激光雷达-惯性SLAM系统，它充分利用短期、中期、长期和多地图数据关联来实现实时估计和高精度制图。该系统由五个模块组成:初始化、里程计、局部映射、环路闭合和全局映射，所有这些都采用相同的映射表示，即自适应体素映射。初始化为后续模块提供了精确的初始状态估计和一致的局部映射，使得系统能够以高度动态的初始状态开始。里程计利用短期数据关联，快速估计当前状态并检测潜在的系统偏差。利用中期数据关联的局部制图采用局部激光雷达-惯性束平差(BA)来细化最近激光雷达扫描的滑动窗口内的状态(和局部地图)。环路闭合检测当前和所有先前会话中先前访问过的地点。全局映射使用有效的分层全局BA来细化全局映射。环路闭合和全局映射都利用了长期和多映射数据关联。我们在三个代表性场景的30个序列中与其他最先进的方法进行了全面的基准比较，包括使用手持设备的狭窄室内环境、使用空中机器人的大规模荒野环境和车辆平台上的城市环境。其他实验证明了初始化的鲁棒性和效率、在多个会话中工作的能力以及在退化环境中的重新定位。

2. 引言

三维光探测与测距（LiDAR）技术因其直接、密集、主动和准确（DDAA）的深度测量特性，在自动驾驶车辆和无人无人机等自主移动机器人领域已成为一种流行的传感技术。LiDAR同步定位与地图构建（SLAM）和LiDAR里程计（LO）利用LiDAR的测量数据为机器人提供必要的状态反馈和周围环境的3D密集点云，供后续过程（如规划、控制）使用。此外，随着LiDAR技术的发展，固态LiDAR因其体积小、重量轻、成本低等优点而得到了广泛研究，并已被应用于一些LO和LiDAR SLAM系统中。

LiDAR SLAM与LiDAR里程计（LO）的区别在于，LiDAR SLAM的目标是在移动机器人上实时构建环境的一致地图，并估计在该地图中的当前位姿，而LO系统则专注于实时定位并累积地图，而不考虑通过优化来减轻漂移。遵循ORB-SLAM3的概念，LiDAR SLAM相较于LO的优越之处在于，它能够充分利用四种数据关联来匹配和优化先前的测量数据：

1）短期数据关联：将当前扫描与地图相关联，并尽可能高效地估计当前自我运动，以供后续过程（如规划、控制）使用。大多数LO/LIO系统具有短期数据关联，并将当前LiDAR扫描累积到其地图中，而不考虑对状态或地图进行进一步细化，这会导致累积漂移。

2）中期数据关联：将多个最新扫描与地图相关联，并据此细化地图。这通常通过光束平差（BA）技术来实现，以在最近一段时间内同时细化状态和局部地图，从而减轻累积漂移并提高鲁棒性。

3）长期数据关联：将所有先前的LiDAR扫描相关联，以实现全局地图的一致性。长期数据关联允许通过位姿图优化（PGO）来检测之前访问过的地方并重置漂移。全局BA也可用于进一步提高准确性。

4）多地图数据关联：将多个地图会话相关联。它使用与长期数据关联相似的方法来合并不同的地图会话，这些会话可能是由于不同的采集时间或由于LiDAR退化而重新初始化新会话而产生的。

在本工作中，我们充分利用了这四种数据关联，并提出了Voxel-SLAM，它包括五个关键模块：初始化、里程计、局部地图构建、回环检测和全局地图构建。Voxel-SLAM建立在BALM2的高效LiDAR光束平差、HBA的全局地图构建方法、BTC的位置识别工作以及VoxelMap的地图表示之上。

3. 效果展示

一个具有较大初始运动的序列。(a) Voxel-SLAM 成功初始化并完成整个序列。(b) 由于初始运动过于剧烈，FAST-LIO2 发生发散。(c)-(h) 当从序列的1/7到6/7位置开始时，Voxel-SLAM 成功初始化，这证明了其初始化的鲁棒性。

(a) 序列“urban1”的点云图。(b)-(g) 是从(a)中黄色框所示位置开始初始化所获得的点云图，该位置的初始速度为9.6 m/s。图(b, d, f)的左列是初始化中束调整（Bundle Adjustment，BA）之前的扫描状态不准确的地图，右列图(c, e, g)是初始化中BA之后的地图。

4. 主要贡献

除了开源系统本身外，Voxel-SLAM的主要贡献如下：

1）在所有任务中使用相同的地图格式：初始化、里程计、局部地图构建、回环检测和全局地图构建。Voxel-SLAM采用高效且通用的自适应体素地图，为不同任务提供足够特征，并很好地适应各种场景。

2）鲁棒且快速的初始化。Voxel-SLAM仅需短时间数据（在我们的实现中为1秒）即可进行初始化（或在系统发散时进行重新初始化），并且可以在静态和动态初始状态下进行初始化。初始化提供了准确的状态和一致的地图以供后续模块使用。推荐课程：彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战。

3）高效的局部地图构建。Voxel-SLAM使用高效的LiDAR-惯性光束平差来细化最近LiDAR扫描滑动窗口内的局部体素地图和状态，从而提高系统的准确性和鲁棒性。在具有有限计算资源的机器人机载计算机上，局部地图构建（窗口大小为10）可以实时以与其里程计（10 Hz）相同的速率运行。

4）多会话回环检测。Voxel-SLAM能够检测当前和先前会话中的回环，并全局优化所有涉及的扫描位姿。

5）高效且准确的单会话或多会话全局地图构建。Voxel-SLAM采用分层光束平差来实现扫描位姿和地图一致性的高效全局优化。

6）充分利用四种数据关联。Voxel SLAM在其里程计和局部地图构建中分别利用短期和中期数据关联，在回环检测和全局地图构建中利用长期和多地图数据关联，以实现实时操作和全局地图一致性。

5. 方法

系统概览如图1所示。图1中的绿色部分是Voxel-SLAM中的不同模块，同一线程中的模块被组合在同一个灰色虚线框内。蓝色部分是数据金字塔，红色部分是相关模块所使用的自适应体素地图。

所提激光雷达-惯性束调整的因子图表示。

在我们的框架中，闭环检测具有两个重要作用。首先，通过利用长期数据关联，它有效地通过检测先前访问过的位置来减轻漂移，并通过位姿图优化（Pose Graph Optimization，PGO）来校正累积误差。其次，闭环检测将当前会话与同一世界坐标系中的先前会话进行匹配，以实现多地图数据关联。此过程能够整合和对齐从不同映射会话中收集的数据，从而促进创建统一的环境地图。

6. 实验结果

表II列出了所有方法在里程计和SLAM中的绝对轨迹误差（ATE）。

Hilti数据集的主要难点在于穿越狭窄的楼梯，这对于激光雷达里程计来说是一个巨大的挑战，因为特征约束有限，且可能发生退化（例如序列03、05、07和13），如图9中的红色框所示。LeGO-LOAM、LiLi-OM、LINS和LIO-SAM在这些具有挑战性的序列中失败了。包括FAST-LIO2、Faster-LIO和Point-LIO在内的“LIO”系列方法在处理楼梯时表现出更强的鲁棒性，但仍然存在异常大的位姿误差。我们的基于体素地图的里程计通过采用自适应分辨率缓解了这个问题，从而在这些场景中显著减小了位姿误差。此外，通过结合局部建图，我们的里程计显著提高了楼梯的精度，并在所有序列中实现了最高精度。这主要是因为与所有先前LIO方法所使用的扫描到地图配准中的短期数据关联相比，我们利用了更长的数据关联进行估计。

表II还列出了所有带有闭环检测（LC）的SLAM方法的ATE。一般来说，由于考虑了额外的闭环约束，所有SLAM方法的精度都比其对应的里程计有所提高。在所有SLAM方法中进行比较，即使我们的方法没有全局建图，也在所有序列中实现了最高精度。有了全局建图后，我们的方法进一步降低了所有轨迹误差。这些精度提升证实了全局建图由于利用了长期数据关联，在所有序列中都产生了更优的优化结果。即使在未检测到闭环检索的情况下（例如序列04、09、10、11、13），全局建图仍然可以提高精度，而其他方法则缺乏进一步细化其估计的手段。

2）MARS-LVIG航拍测绘数据集：MARS-LVIG数据集具有从地面约100米高处向下的独特视角。由于LeGO-LOAM和LINS的特殊特征提取不适用于Livox AVIA激光雷达，因此它们无法运行此数据集。我们遵循LIO-SAM的说明将其适配到该激光雷达，其余方法无需修改即可自然地在Livox AVIA激光雷达上工作。该数据集上所有方法的ATE结果列在表III中。序列名称中第一个数字相同的序列具有相同的路径，但飞行速度不同。与Hilti数据集中的结构化和小环境以及缓慢移动不同，MARS-LVIG在大场景中快速飞行，最高速度达到12 m/s，包含大量的自然场景，如树林、山丘和河流，如图10(a)所示。我们的方法在这些具有快速运动的非结构化环境中仍然保持鲁棒性，并在大多数序列的里程计和SLAM方法中实现了最佳精度。特别是第四组序列“mars4-1”、“mars4-2”和“mars4-3”呈现了最具挑战性的场景。当无人机飞到图10(a)-(b)所示的山顶时，会发生短暂的退化。虽然除了LiLi-OM之外的大多数里程计方法都存活了下来，但它们在山顶之后的轨迹变得倾斜，ATE显著大于表III中的其他序列。由于退化，这些里程计方法的点云地图出现不一致，如图10(c)-(d)所示，其中山顶的点被分成两层。可以看出，由于使用了中期数据关联，我们的带有局部建图的里程计（图10(d)）的分离程度明显小于代表性LIO方法FASTLIO2（图10(c)）。因此，如表III所示，我们的带有局部建图的里程计的定量ATE明显优于其他LIO方法。对于完整的SLAM，所有方法都未能在这三个序列中识别出有效的闭环检测，导致ATE与其对应的里程计相同。然而，通过粗细体素化和BA优化（第IX节），我们的方法仍然可以提高轨迹精度。粗细体素化将分离的点云层体素化为相同的体素，从而将它们关联为在后续BA优化中优化的一个特征。因此，该位置的地图一致性得到了显著提高，如图10(e)所示。相应地，如表III所示，在这三个序列中，我们的带有全局建图的完整方法的ATE也得到了显著改善。

UrbanNav自动驾驶数据集：自动驾驶是激光雷达SLAM的一个重要应用领域，我们在UrbanNav自动驾驶数据集上评估了我们的方法。UrbanNav数据集是在城市场景中收集的，具有更高的速度（最高速度13 m/s）、更长的采集时间（最长56分钟）、更多的动态物体以及之前回访过的地点，这些都比Hilti数据集更复杂。表IV中的结果证明了我们的方法在城市场景中的有效性，在里程计和完整SLAM的两种情况下，它都在所有序列和所有评估方法中实现了最高精度。

7. 总结 & 未来工作

在本文中，我们提出了Voxel-SLAM：一个完整、准确且多功能的激光雷达-惯性SLAM（Simultaneous Localization and Mapping，同步定位与地图构建）系统，该系统包括初始化、里程计、局部建图、回环检测和全局建图等模块，所有这些模块均采用相同的自适应体素地图结构。初始化模块表现出卓越的速度和鲁棒性，为后续模块提供了准确的状态和一致的地图。里程计模块能够在感知潜在系统发散的同时，快速估计当前状态。局部建图模块利用高效且紧密耦合的激光雷达-惯性束调整（Bundle Adjustment，BA）算法，同时对状态和地图进行细化，从而提高准确性和鲁棒性。回环检测模块能够检测跨多个会话的重复访问地点。我们设计了一种高效且准确的全局建图方法，该方法采用数据金字塔结构。综上所述，这些模块充分利用了四种数据关联：短期、中期、长期和多地图数据关联。

该系统可扩展至融合图像测量，这能在某些退化环境中增强鲁棒性，为点云提供颜色信息，并提升地点识别性能。此外，尽管当前系统已足够高效，能够在车载计算机上实时运行，但GPU并行化可以进一步提升系统效率，尤其是在全局建图方面。体素地图框架非常适合并行操作。

对更多实验结果和文章细节感兴趣的读者，可以阅读一下论文原文~

本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊可提供顶会论文的课题如下：