Voxel-SLAM: 一个完整、精确且多功能的LiDAR-惯性SLAM系统

这项工作提出了Voxel-SLAM：一个完整、精确且多功能的LiDAR-惯性SLAM系统。该系统充分利用了短期、中期、长期及多地图数据关联，以实现实时位姿估计和高精度建图。系统由五个模块组成：初始化、里程计、局部建图、回环检测和全局建图，所有模块都采用同一种地图表示形式——自适应体素地图。初始化模块提供准确的初始状态估计和一致的局部地图，为后续模块提供支持，使系统能够在高度动态的初始状态下启动。里程计模块利用短期数据关联，快速估计当前状态并检测潜在的系统发散。局部建图模块利用中期数据关联，通过局部LiDAR-惯性后端优化（BA）在最近的LiDAR扫描滑动窗口中优化位姿和局部地图。回环检测模块能够在当前及所有之前的会话中检测到先前访问过的地点,全局建图模块通过高效的分层全局BA优化全局地图。回环检测和全局建图均利用长期及多地图数据关联。在30个序列中对三类典型场景进行了全面的基准测试比较，这些场景包括使用手持设备的狭窄室内环境、使用飞行器的广阔野外环境以及使用车辆平台的城市环境。其他实验还展示了初始化的鲁棒性和效率、多会话工作的能力以及在退化环境中的重定位能力。为回馈社区，代码开源。

三维激光雷达（LiDAR）因其直接、密集、主动且精确的（DDAA）深度测量，已成为自主移动机器人领域（如自动驾驶车辆和无人机）中广泛使用的传感技术。LiDAR同时定位与建图（SLAM）和LiDAR里程计（LO）利用LiDAR的测量数据，为机器人提供必要的状态反馈和周围环境的三维密集点云，以支持后续的处理（如规划、控制）。此外，随着LiDAR技术的发展，固态LiDAR因其体积小、重量轻和价格适中，已被广泛研究并应用于某些LO和LiDAR SLAM系统。

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

• 短期数据关联：将当前扫描与地图关联，并尽可能高效地估计当前自运动，供后续处理（例如规划、控制）使用。大多数LO/LIO系统都具备短期数据关联，并将当前的LiDAR扫描累积到它们的地图中，但不考虑进一步优化状态或地图，这会导致累积漂移。

• 中期数据关联：将多个最近的扫描与地图关联，并相应优化地图。通常通过后端优化（BA）技术实现，同时优化最近时间内的状态和局部地图，减轻累积漂移并提高系统的鲁棒性。

• 长期数据关联：将所有之前的LiDAR扫描进行关联，以实现全局地图的一致性。长期数据关联允许通过位姿图优化（PGO）检测到先前访问的地方，以消除漂移。全局BA也可用于进一步提高精度。

• 多地图数据关联：将多个地图会话进行关联。该方法类似于长期数据关联，通过合并因不同采集时间或LiDAR退化导致的会话重启而产生的不同地图会话，形成统一的地图。

在这项工作中，充分利用了这四种数据关联方式，并提出了Voxel-SLAM，该系统包含五个关键模块：初始化、里程计、局部建图、回环检测和全局建图。Voxel-SLAM基于BALM2的高效LiDAR后端优化、HBA的全局建图方法、BTC的地点识别工作以及VoxelMap的地图表示。除了开源系统本身，Voxel-SLAM的主要贡献如下：

• 所有任务使用相同的地图格式：Voxel-SLAM在所有任务中使用高效且多功能的自适应体素地图，该地图为不同任务提供了足够的特征，并适应各种场景。

• 鲁棒且快速的初始化：Voxel-SLAM仅需要短时间数据（在我们的实现中为1秒）即可初始化（或在系统发散时重新初始化），并且能够在静态和动态初始状态下完成初始化。初始化为后续模块提供了精确的状态和一致的地图。

• 高效的局部建图：Voxel-SLAM使用高效的LiDAR-惯性后端优化（BA），在最近的LiDAR扫描滑动窗口内优化局部体素地图和状态，从而提高系统的精度和鲁棒性。局部建图（窗口大小为10）能够在资源有限的机器人板载计算机上以与里程计相同的速率（10 Hz）实时运行。

• 跨多个会话的回环检测：Voxel-SLAM能够检测当前和之前会话中的回环，并对涉及的所有扫描位姿进行全局优化。

• 高效且精确的单会话或多会话全局建图：Voxel-SLAM采用分层BA来实现扫描位姿的高效全局优化和地图一致性。

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

A. 工作流程

如图1所示，Voxel-SLAM系统由五个模块组成，并在三个并行线程中运行。首先，系统进行初始化过程（如果在启动或重启后未完成），基于短时间段（在我们当前的实现中为1秒）的LiDAR扫描和IMU数据。初始化通过一个专门的LiDAR-惯性后端优化（BA），估计所有扫描的状态、初始局部地图和世界坐标系下的重力向量。在状态、局部地图和重力向量初始化完成后，里程计紧密融合LiDAR和IMU的测量数据，实时估计当前状态并检测可能由LiDAR退化引起的系统发散。

图1框架

接下来，局部建图将当前扫描数据加入滑动窗口，并利用LiDAR-惯性后端优化同时优化滑动窗口内的所有状态和局部地图。滑动窗口中的最旧扫描会被边缘化以积累关键帧，关键帧随后由回环检测模块提取回环描述符，检测当前和先前会话中的回环。当回环检测成功时，当前或先前会话中的相关位姿将被用于构建位姿图进行优化。在位姿图优化（PGO）之后，全局建图模块进行关键帧的后端优化，并将关键帧实时合并到子地图中。当接收到会话结束信号时，全局建图会对所有子地图执行全局后端优化，并进行自上而下的优化，以获得所有扫描的准确位姿。

B. 数据金字塔

一个包含三层的数据结构金字塔用于存储输入的LiDAR点云序列，底层是原始的LiDAR扫描数据，直接由LiDAR传感器采集，通常为10Hz。LiDAR扫描用于里程计和局部建图，以实现实时状态估计。10个LiDAR扫描合并为一个关键帧，关键帧用于回环检测以提取回环描述符。最后10个关键帧进一步合并为一个子地图，用于全局建图模块。LiDAR扫描合并为关键帧、关键帧合并为子地图的过程通过LiDAR后端优化实现，该优化同时对所有LiDAR扫描（或关键帧）相对于各自合并窗口中的第一个扫描（或关键帧）的位姿进行优化。此外在关键帧合并为子地图的过程中，两个连续的合并窗口共享5个重叠的关键帧（即，大小为10且步长为5的滑动窗口），以增加两个连续子地图之间的可见区域比例。另一方面扫描帧合并为关键帧的窗口没有重叠（大小为10且无步长的滑动窗口），因为LiDAR的长测量距离已经确保了两个连续关键帧中扫描之间的较大重叠。

C. 自适应体素地图

自适应体素地图在所有模块中发挥着重要作用，用于提取平面特征并提供跨多个扫描、关键帧或子地图的特征关联。体素地图维护了环境中不同大小的平面特征。为此它将空间划分为统一的体素（即根体素），每个体素的大小为Lr。统一大小的体素通过哈希表组织，每个哈希键将每个LiDAR点按其位置建图到相应的体素。一个体素包含一个多层的八叉树数据结构，其中每个叶节点代表一个平面，不同层次的叶节点具有不同的大小，代表不同尺寸的平面特征。

自适应体素地图的构建过程如下：对于每个扫描、关键帧或子地图，其点云根据位置分布到相应的体素中。如果体素中的点位于同一平面上（点协方差矩阵的最小特征值与第二小特征值的比值小于指定值），则该体素被视为平面体素，并将其LiDAR点保存以供后续使用；否则该体素将递归地分裂为更小的子体素，直到点位于同一平面上或满足终止条件（例如，达到最小子体素大小或点数）。

自适应体素地图结构在不同模块中用于不同的目的。首先在距离LiDAR为Lm的范围内共享一个局部自适应体素地图，用于初始化、里程计和局部建图模块，以实时估计LiDAR扫描的状态（见图1中的体素地图）。初始化首先获取一些初始扫描来初始化体素地图。里程计通过将当前扫描与体素地图中的平面对齐来估计当前状态。局部建图模块滑动局部地图，并通过高效的LiDAR-惯性后端优化（BA）优化当前和最近扫描的状态。除了局部体素地图外，还有两个其他的自适应体素地图结构：一个由回环检测模块BTC用于关键帧中的描述符提取，另一个由全局建图模块HBA用于不同关键帧和子地图之间的平面特征提取和关联。

BA优化模块主要用于初始化和局部建图，以同时估计多个扫描状态，与滤波器方法相比，LiDAR-惯性BA通过更长的数据关联提供更高的准确性和鲁棒性，但其计算时间较长，通常难以用于实时应用。为了解决这一问题，BALM2引入了高效的LiDAR BA方法，包括：(1) 解析几何特征，降低优化维度；(2) 使用点簇数据结构，以紧凑的方式表示点数据，减少处理时间；(3) 推导出解析的二阶导数，开发出高效的二阶求解器。这些技术使LiDAR BA优化能满足实时处理的需求。在此基础上，BALM2结合了IMU预积分，构建出LiDAR-惯性BA优化，以同时估计多个状态和重力向量。与视觉SLAM系统中固定尺度的重力向量不同，LiDAR测量具备准确的尺度信息，因此可以优化重力向量的大小。通过解析形式的雅可比矩阵和海森矩阵，优化过程进一步加速，确保系统的实时性。

图 2. 所提出的 LiDAR 惯性困集调整的因子图表示

初始化

许多基于滤波器的LIO系统假设传感器最初是静止的。然而，对于非静止的初始状态（例如在序列中途重新启动），这种静止假设会导致状态传播错误，从而影响LiDAR扫描的失真校正和扫描配准中的特征关联。这会显著影响初期状态估计的准确性，导致不一致甚至错误的初始地图，进而降低后续里程计的精度。为了解决这一限制，本文提出了一种基于LiDAR-惯性BA优化的鲁棒且高效的初始化阶段，能够在存在非静止初始状态时提供稳健的初始状态估计、重力向量以及一致的初始局部地图。此外，该模块还可以确认初始化是否成功，并将重力向量重新对齐到世界坐标系的z轴。初始化在前N个LiDAR扫描及其对应的IMU测量上进行。具体而言，首先运行LIO方法以实时估计前N个扫描的状态和重力向量。然后，这些估计的状态和重力向量作为后续从粗到细的体素化和BA优化的初始值。优化后的状态和重力向量将用于判断初始化是否成功。如果成功，初始化完成；如果不成功，系统将收集接下来的N个LiDAR扫描并重复上述初始化过程，直至成功。

里程计

在Voxel-SLAM中，里程计通过扫描与地图的配准利用短期数据关联。里程计的目标是以最小的延迟估计当前状态，这是其他任务（如规划和控制）的必要条件。估计的状态还为后续的局部建图模块提供了一个相对准确的初始值。

A. 状态预测与更新

为了在局部自适应体素地图上进行状态估计，使用了扫描与地图对应的方法以及扩展卡尔曼滤波器（EKF）。具体来说，里程计通过IMU传播预测状态，同时补偿LiDAR扫描的运动畸变。经过畸变补偿后的扫描点根据预设分辨率进行空间下采样，并通过已知的LiDAR-IMU外部参数从LiDAR体坐标系转换到IMU体坐标系。每个扫描点使用协方差矩阵投射到世界坐标系，与体素地图中匹配的平面进行配准。点到平面的距离作为系统的测量值，更新IMU预测状态，估计后的状态和LiDAR扫描将发布给其他模块进行进一步处理。

B. 系统发散检测

当机器人遇到缺乏足够几何特征的退化环境时，状态估计变得不适定，可能导致SLAM系统发散并失败，这需要主动监测和处理。在Voxel-SLAM中，每次扫描配准后都会检测系统潜在的发散。配准完成后的平面将用于评估退化。如果某次扫描中偶然发生退化，由于IMU预测在短期内的相对准确性，不会视为发散。但如果连续多次扫描中发生退化，则会视为发散。在这种情况下，当前数据会被保存，整个系统将从一个远离当前会话的新原点重新启动，以防止下一会话中的点与当前会话的点重叠。重新启动也会触发新的初始化，以获取下一会话的初始状态和局部地图。

局部建图

局部建图通过中期数据关联，优化局部体素地图及滑动窗口内的所有最近状态。相比于里程计，局部建图更加准确和稳健，因为其LiDAR-惯性BA优化利用了来自LiDAR和IMU的更长时间段的测量数据进行状态估计。

A. 关键帧加载与体素化

当局部建图接收到IMU测量数据以及来自里程计的当前LiDAR扫描及其估计状态时，扫描中的点会根据其在世界坐标系中的位置分布到相应的体素中。同时，IMU测量数据根据[26]进行预积分，以获得IMU预积分因子。由于局部地图仅维护当前LiDAR位置周围距离为Lm的区域，体素地图无法在当前或先前会话中回访某个地方时确保长期数据关联。为了建立长期关联，系统动态加载那些时间上较远（例如，超过1分钟以避免最近的关键帧）、但空间上较近（例如，距离不超过10米）的关键帧。这一技术在[10]中被证明有效。关键帧中的点被分布到相应的体素中，作为滑动窗口BA优化的固定历史地图约束。在将当前扫描或动态加载的关键帧的点分布到体素时，我们会跟踪那些新增加点的体素。这些体素会被枚举，其中包含的所有点，无论是来自当前扫描、关键帧还是已有点，都会经过自适应体素化，直到每个叶节点拥有有效的平面。在每个叶节点，所有点都会被编码为紧凑的结构“点簇”（point cluster），滑动窗口内的不同扫描会有不同的点簇。叶节点中剩余的点被称为“固定”点，作为滑动窗口优化的地图约束。这些“固定”点被编码为单个点簇，滑动窗口中的每次扫描的点簇将与之直接用于BA优化，无需枚举每个单独的点。

B. 滑动窗口BA优化

在获得点簇关联和IMU预积分后，滑动窗口BA优化会使用第IV节中的LiDAR-惯性BA优化来优化滑动窗口内的所有状态。滑动窗口BA优化与第IV节的不同之处在于，它包含了每个体素中“固定”点的约束，作为地图约束，以确保在世界坐标系中估计的准确性。另一个不同点是重力向量被固定，因为初始化阶段已较好地估计了重力向量。图3展示了滑动窗口LiDAR-惯性BA优化的因子图表示。

图 3. 局部建图中使用的 LiDAR-惯性捆集调整的因子图表示。“单位矩阵”表示来自“固定”点（以点簇的形式表示）的地图约束，这些点不在滑动窗口优化范围内，并在世界框架中表示。

地图边缘化

在BA优化完成后，局部体素地图中的点簇和平面参数（平面中心和法向量）会根据优化后的状态进行更新。更新后的平面将用于里程计模块中下一次的扫描与地图配准。此外，滑动窗口中最早的扫描将被边缘化。也就是说，最早的扫描会从滑动窗口中移除，并且其在每个叶节点中的点将合并到“固定”点簇中，作为地图约束。边缘化完成后，最早的扫描及其位姿将被发送进行闭环检测，以便进一步处理。此外，那些叶节点中没有与滑动窗口内状态对应的点、或者距离当前位置超过局部地图大小Lm的体素将从局部地图中移除，以减少内存占用。

闭环检测

闭环检测在系统框架中起着两个重要作用。首先，它通过利用长期数据关联，检测到之前访问过的位置，并通过位姿图优化（PGO）校正累计误差，从而有效减小漂移。其次，闭环检测将当前会话与同一世界坐标系中的之前会话进行匹配，实现多地图数据关联。这一过程能够整合并对齐不同建图会话中的数据，帮助创建统一的环境地图。

A. 关键帧生成与闭环检测

当一个扫描从局部建图的滑动窗口中被边缘化后，它将被推送到闭环检测模块中的位姿图中。N个连续被边缘化的扫描会累积成一个帧，若当前帧与最新关键帧之间的移动距离和旋转角度超过预定的阈值（例如，0.5米和5度），则该帧会被选为关键帧。关键帧用于提取BTC描述符并与当前会话及之前会话中的候选关键帧匹配。与此同时，关键帧会被发送到全局建图线程进行进一步处理。在确定闭环候选帧后，需验证其真实性以避免误检。除了BTC自带的几何验证，我们采用了两项额外标准来增强闭环检测的可靠性：

• (1) 当前关键帧与候选关键帧的点-平面对齐应包含三个线性独立方向的平面约束；

• (2) 漂移距离与行进距离的比值应足够小（例如，小于等于1%）。

该标准适用于候选闭环关键帧位于当前会话中（如图4(a)）或之前会话中第二次（或更多次）匹配的情况。如果该比值过大，则闭环匹配很可能是错误的。此外，如果候选闭环关键帧首次位于之前会话中，则该第二标准不适用，第一标准以及BTC中的几何验证[18]应该更加严格，因为如果首次跨会话闭环检测错误，会对系统的后续操作产生不利影响。

图 4. 漂移距离和行进距离。(a) 候选循环关键帧位于当前会话中。行进距离是沿当前会话从候选循环关键帧 B 到当前关键帧 A 累积的距离。(b) 候选循环关键帧位于前一个会话中。行进距离是沿前一个会话从候选循环关键帧 B 到前一个循环关键帧 C 累积的距离，然后沿当前会话从 C 到当前关键帧 A 累积的距离。对于这两种情况，漂移距离是当前关键帧 A 和候选循环帧 B 之间的相对距离，它是根据循环检测方法 BTC计算得出的

B. 位姿图优化与地图重建

在成功检测到闭环后，如果检测到的闭环关键帧是首次位于之前的会话中，当前会话会与之前会话对齐，通过将当前会话中的所有位姿对齐到之前会话的世界坐标系中，并将两会话的位姿图连接起来。否则，如果检测到的闭环关键帧位于之前会话的第二次（或更多次）或者当前会话中，位姿图内就会形成一个闭环，这将触发通过GTSAM[48]进行的位姿图优化（PGO）。

在通过PGO更新扫描位姿后，关键帧和子地图的位姿也会同步更新，与其对应的扫描位姿一致，如图1的数据金字塔所示（参见图5中的红色节点）。此外，位于位姿图之外的扫描位姿也需更新，以确保系统的一致性。这些扫描包括：

• (1) 闭环执行期间被边缘化的扫描（参见图5中的绿色节点）；

• (2) 当前滑动窗口中的所有扫描，这些扫描自上次扫描添加到位姿图后，滑动窗口已经前移（参见图5中的蓝色节点）。

这两类扫描的位姿均通过图5中展示的位姿图中最后一个扫描的位姿修正变换∆Tp进行修正。更新后的扫描连同位姿图中的最新五个关键帧中的所有扫描将用于构建新的自适应体素地图。在新的体素地图的每个叶节点中，关键帧和被边缘化的扫描点作为“固定”点，充当地图约束，因此它们会被编码为一个单独的点簇。而当前滑动窗口中的扫描点则分别编码为独立的点簇。最终，新的体素地图将替换原有地图，供后续的里程计和局部建图模块使用。为了节约计算资源，PGO和地图重建仅在漂移距离超过预定阈值（例如，0.1米）时才会执行。

图 5. PGO 之前和之后的扫描。红色节点是已添加到姿势图中的扫描。绿色节点是循环闭合执行期间来自局部建图的边缘化扫描，因此尚未添加到姿势图中。蓝色节点是局部建图当前滑动窗口中的扫描。顶部和底部的节点分别是 PGO 之前和之后的扫描。∆Tp 是姿势图中最后一个扫描的姿势校正，用于校正姿势图之外的后续扫描。

全局建图

在闭环检测过程中，尤其是在多会话SLAM中，点云地图可能会出现不一致的情况。这是因为位姿图优化（PGO）仅考虑位姿约束，而没有直接捕捉地图一致性约束。为了解决这一问题，我们引入了全局建图，使用分层全局束调整（BA）方法，以减少过多的优化维度。

基于图1中的数据金字塔，全局建图线程包含三个步骤。首先，在从闭环检测线程接收到关键帧后，全局建图线程会实时对一个关键帧窗口执行束调整（BA）。窗口大小为10个关键帧，滑动步长为5。在每次滑动窗口的BA中，第一个关键帧的位姿固定为PGO的结果，剩余的9个关键帧位姿在优化过程中同时被优化。之后，每个窗口中的10个关键帧将合并为一个子地图。其次，一旦当前会话结束（例如数据采集结束或系统重启），系统将对子地图执行全局BA。由于PGO返回的扫描位姿可能无法充分保证全局地图的一致性，因此在全局子地图的BA中，采用粗到细体素化和BA优化方法，但移除了IMU预积分因子和重力优化。从关键帧到子地图，再从子地图到全局地图的BA优化，被称为自底向上的BA优化，与直接对关键帧或扫描进行全局BA相比，显著减少了优化维度。最后，通过结合PGO的自顶向下优化，进一步提高全局一致性。

我们进行了五组实验，以验证系统的准确性和多功能性，具体包括：

• 初始化的鲁棒性、准确性和时间消耗评估。

• 与其他里程计和SLAM方法的单会话SLAM基准比较。

• 多会话SLAM实验。

• 在计算能力受限的计算机上进行的多会话SLAM与在线重定位。

• 上述实验的计算时间。

实验使用了三个显著不同的公共数据集：Hilti、MARS-LVIG和UrbanNav，总共包含30个序列，另有一个私有数据集包含2个序列。 Hilti数据集在室内和室外结构化建筑环境中收集，使用Hesai XT-32 LiDAR和Bosch BMI085 IMU进行手持测量，能导出绝对轨迹误差（ATE）结果。MARS-LVIG数据集在无人机上收集，使用向下观察的Livox Avia LiDAR。UrbanNav是一个城市机器人汽车数据集，结合了IMU和LiDAR。我们的私有数据集在轻量手持设备上收集，包含Livox Avia LiDAR及其内部IMU。如图13

图 13。序列“private2”。（a）手持设备。（b）序列中运行的电梯和自动扶梯。由于空间狭小以及 LiDAR 和 IMU 之间的测量不一致，电梯将导致系统发散。（c）在第一个会话（绿色）中检测到发散后以及第二个会话（红色）成功重新初始化后的地图和轨迹。（d）在第一个会话（绿色）中重新定位第二个会话（红色）后的地图和轨迹。（e）数据收集结束时的地图和轨迹。（f）自动扶梯的放大地图。

前三个实验在配备高性能处理器和32 GB内存的笔记本电脑上进行，第四个实验则在性能较低的车载计算机上进行。所有实验的体素大小和下采样分辨率在不同环境中保持一致。局部体素地图大小和最大层数也保持不变，初始化和BA过程中的滑动窗口大小为10。所有参数在各数据集和序列中保持一致。

A. 初始化

1. 定性分析： 为了展示初始化的鲁棒性，我们在一个名为“private1”的私有序列上进行了测试。该序列是在一个非结构化的森林环境中收集，具有快速旋转和剧烈晃动的特性。尽管存在较大的初始条件，Voxel-SLAM仍能成功初始化，并生成反映真实运动的轨迹及一致的地图。而FAST-LIO2由于缺乏强大的初始化模块而出现发散。为验证初始化模块的有效性，我们在序列的不同起始时间进行了多次测试，结果进一步证明了模块在多种初始条件下的鲁棒性。

2. 定量分析 ：我们还对UrbanNav数据集中“urban1”序列的初始化进行了定量分析。该数据集提供了真实的位姿和速度，并允许我们评估不同起始时间的初始化效果。我们评估了影响初始化的两个关键参数：初始速度和重力向量。通过比较估计的IMU状态和真实值，我们发现无论滑动窗口的大小如何，速度的估计都表现良好。重力向量的估计误差随着滑动窗口的增大而减少，这表明较大的滑动窗口可以利用更多的IMU数据，提高信息量。最终的结果表明，即使在高初始速度的情况下，系统也能有效初始化状态和局部地图。在滑动窗口超过10时，速度和重力向量的估计误差未显著降低，因此我们在Voxel-SLAM中将窗口大小固定为10。

单次会话SLAM实验

本实验将VoxelSLAM系统与当前最先进的开源LiDAR-惯性里程计和SLAM系统进行比较。对比的里程计方法包括LINS、FAST-LIO2、Faster-LIO、Point-LIO等，而SLAM系统包括LeGO-LOAM、LiLi-OM、LIO-SAM和LTA-OM。所有对比方法使用默认参数。在对里程计精度进行评估时，禁用了所有SLAM方法的环闭（LC）功能。我们还对自身系统进行了消融实验，分析了里程计、局部建图、环闭和全局建图模块的影响。

1) Hilti手持数据集

在Hilti手持数据集上的实验中，主要挑战是穿越狭窄的楼梯，这对LiDAR里程计构成了巨大挑战，尤其是在特征有限或环境退化的情况下。LeGO-LOAM、LiLi-OM、LINS和LIO-SAM在处理这些困难场景时失败了，而FAST-LIO2、Faster-LIO和Point-LIO表现较为稳健，但在穿越楼梯时依然出现较大的姿态误差。VoxelSLAM通过采用自适应分辨率的体素地图有效减小了这些误差。尤其是在结合局部建图后，系统的精度进一步提高，成为所有序列中最准确的方法。其主要原因在于相比其他方法的短期数据关联，VoxelSLAM利用了更长的关联时间。表II列出了所有方法在里程计和SLAM中的绝对轨迹误差（ATE），同时，所有带有环闭的SLAM方法相较于仅使用里程计的方式，其精度都有显著提升。即便在某些序列中没有检测到环闭（如序列04、09、10、11、13），VoxelSLAM的全局建图功能依然能够通过长时间数据关联提高精度，进一步减少轨迹误差。

2) MARS-LVIG航拍数据集

MARS-LVIG数据集来自无人机在100米高空俯视的航拍场景，与Hilti数据集中的小型结构化环境和低速运动相比，该数据集具有较快的飞行速度（最高12 m/s）和大规模自然场景。LeGO-LOAM和LINS因特征提取方式的局限，无法处理该数据集，而LIO-SAM通过调整可以适应Livox AVIA LiDAR。在大多数序列中，VoxelSLAM在这些快速运动的非结构化环境中表现出极强的鲁棒性，取得了最好的精度。特别是第四组序列“mars4-1”、“mars4-2”和“mars4-3”，这些场景在飞行器到达山顶时出现了短暂的退化现象，导致大多数方法生成的轨迹在山顶后发生倾斜。VoxelSLAM通过中期数据关联和局部建图减少了这种现象，生成的点云地图更加一致。即便在没有检测到环闭的情况下，VoxelSLAM的全局建图功能依然通过粗到细的体素化和优化提高了精度。表III列出了该数据集上所有方法的ATE。

3) UrbanNav机器人车城市数据集

UrbanNav数据集收集于城市环境中，代表了LiDAR SLAM在自动驾驶中的一个重要应用场景。相比Hilti数据集，UrbanNav具有更高的行驶速度（最高13 m/s）、更长的采集时间（最长56分钟），并且包含更多动态物体和重复访问的地点。实验结果表明，无论是在里程计还是完整SLAM任务中，VoxelSLAM在所有序列中都表现出最高的精度，尤其在城市环境下效果显著。

图6. 一个具有大初始运动的序列。（a）Voxel-SLAM成功初始化并完成整个序列。（b）FAST-LIO2由于剧烈的初始运动而发散。（c）-（h）Voxel-SLAM在从序列的1/7到6/7处开始时成功初始化，证明了初始化的鲁棒性。

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

多次会话SLAM实验

1. 多会话SLAM

Hilti 09-13序列是在同一建筑工地收集的，它们的真实轨迹共享相同的世界坐标系。实验中，依次处理了“hilti13”、“hilti12”、“hilti11”、“hilti10”和“hilti09”这五个序列。每处理完一个序列，Voxel-SLAM将其保存为一个会话，使系统能够在后续序列中检索环闭检测。最终，Voxel-SLAM成功构建了一个连接图，图中显示在不同序列的重访区域检测到了环闭，例如图11(a)中标注的区域（1-4）。这使得所有五个序列的姿态通过姿态图优化（PGO）并结合全局建图进行联合优化。

尽管Voxel-SLAM中使用的BTC描述符非常准确且鲁棒，但在某些区域（如图11(b)中的黑色虚线框和图12(b)中的白色虚线框）可能会漏检环闭。这种漏检发生在狭窄走廊中，导致“hilti13”和“hilti12”之间缺乏约束，从而限制了PGO的精度。在这些区域的点云也表现出明显的不一致性，如图12(c)所示。全局建图通过更好地优化这些区域的点云，有效解决了这个问题，如图12(d)所示。图12(a, b)展示了经过全局建图优化后的完整点云地图，其中不同序列以不同颜色显示。表V列出了通过PGO和全局建图估计的轨迹的绝对轨迹误差（ATE），结果显示，全局建图不仅提高了所有五个序列的多会话精度，还改进了每个单独会话的精度，这是由于充分利用了多地图数据关联。

2. 重定位

该实验评估了系统在退化后重定位的能力。实验使用Intel i3-N305 CPU的车载计算机进行，使用的序列“private2”是通过手持设备收集的。实验涵盖了系统的各项功能，包括初始化、重新初始化、里程计、局部建图、跨会话环闭检测、重定位和全局建图。实验环境如图13所示，包含校园内有许多移动的行人、高度变化大、狭窄空间（如电梯）等挑战。图14展示了整个轨迹的路径点A-G，随着轨迹的进展，系统在遇到电梯等空间限制时，会检测到发散并启动新会话。在系统走出电梯后成功初始化了新的局部地图，图13(c)中的红色点云展示了这一过程。当重新访问起始点时，系统检测到环闭并进行重定位，将第二个会话与第一个会话连接起来。随着路径的继续，Voxel-SLAM能够利用先前会话的长时间关联保持轨迹一致性。实验最终通过全局建图优化了整个点云地图，保证了系统的鲁棒性和可靠性。图15(b)展示了优化后的全局点云地图，与Google Earth地图良好对齐。

图 12. (a)-(b) 从顶视图和侧视图获得的五个会话的全局点云图。(c)-(d) 分别从 PGO 和全局建图获得的 (b) 中白色虚线框内区域的放大点云，该区域也是图 11(b) 中黑色虚线框内的区域

3. 计算时间

我们评估了所有数据集和实验的平均计算时间和总内存消耗。里程计和局部建图能够以与LiDAR扫描频率相同的速度（10Hz）实时运行。环闭检测和关键帧优化是事件触发的，通过累积整个序列的总计算时间并除以LiDAR扫描的次数来得到平均时间。结果表明，Voxel-SLAM的所有计算模块的时间消耗远小于LiDAR扫描的时间间隔，确保了即使在资源受限的车载计算机上也能实现实时性能。

表VI展示了所有序列的时间和内存消耗结果。全局建图在每个会话结束后执行的时间仅占数据收集时间的0.6%到4.7%，证明了该模块的高效性。例如，在最长的“urban3”序列中，数据采集持续了56分钟，轨迹长度为4.86公里，而全局建图的时间仅为79.7秒。内存使用也远低于物理内存的限制，证明了Voxel-SLAM在长时间数据关联中的高效性。

本文介绍了Voxel-SLAM：一个完整、准确且多功能的LiDAR-惯性SLAM系统，包含初始化、里程计、局部建图、环闭检测和全局建图模块，所有这些模块都采用相同的自适应体素地图结构。初始化模块展现了出色的速度和鲁棒性，能够为后续模块提供准确的状态和一致的地图。里程计迅速估计当前状态，同时感知潜在的系统发散。局部建图使用高效、紧耦合的LiDAR-惯性平差（BA），同时优化状态和地图，提升了精度和鲁棒性。环闭检测能够在多个会话中检测到重访的位置。全局建图则使用数据金字塔设计，具备高效且精确的性能。这些模块结合利用了短期、中期、长期以及多地图数据关联。该系统可以扩展以融合图像测量，这能够增强在某些退化环境中的鲁棒性，为点云提供颜色信息，并提升地点识别的表现。此外，尽管当前系统已经能够在车载计算机上实现实时运行，GPU并行化能够进一步加速系统效率，特别是在全局建图中。体素地图框架非常适合进行并行操作。