VoxelMap++：在线LiDAR惯性里程计实现可合并的体素建图方法

摘要

本文介绍了VoxelMap++：一种具有平面合并功能的体素建图方法，能够有效提高基于LiDAR同时定位与建图（SLAM）的准确性和效率。该地图是一个包含一个平面特征的体素集合，具有3自由度的表示和相应的协方差估计。考虑到整个地图将包含大量的共面特征（子平面），这些子平面的3自由度估计可以视为带有更大平面协方差的测量。因此，我们设计了一个基于并查集的平面合并模块，可以节省资源并进一步提高平面拟合的准确性。这个模块可以区分不同体素中的子平面，并将这些子平面合并以估计父平面。合并后父平面的3自由度表示将比子平面更精确，不确定性将显著减小，从而进一步提高了LiDAR惯性里程计的性能。在具有挑战性的环境中进行的实验，如走廊和森林，证明了我们的方法相对于其他最先进方法的高准确性和效率。顺便说一下，我们的实现VoxelMap++在GitHub上开源，适用于非重复扫描LiDAR和传统扫描LiDAR。

主要贡献

本文提出了一种新颖的在线可合并体素或者说栅格建图方法，采用3自由度平面表示，称为VoxelMap++。具体而言，本文的贡献包括：

我们通过使用最小二乘估计，将VoxelMap中的平面拟合和协方差估计方法从6自由度提升到3自由度。这一改进从工程实现的角度来看，进一步提高了协方差估计的效率，减少了内存使用量，使VoxelMap++能够轻松适应各种资源受限的嵌入式平台。

我们提出了一种新颖的在线体素合并方法，采用并查集。在体素中的每个共面特征（子平面）将被视为大平面（父平面）的测量，合并模块不仅提高了平面拟合的准确性，降低了整个地图的不确定性，还减少了地图的内存使用。

我们将VoxelMap++与其他最先进的算法在各种场景中进行了比较（结构化、非结构化和退化场景），展示了算法在准确性和效率方面的优越性。

我们使VoxelMap++适应不同类型的LiDAR（多旋转LiDAR和非传统固态LiDAR），并在GitHub上以易读性和模块化开源，以分享我们的发现并为社区做出贡献。

内容概述

VoxelMap++的流程如图1所示，LiDAR原始点预处理方法和基于迭代误差状态卡尔曼滤波器的状态估计方法与FASTLIO 类似。值得注意的是，本文的建图方法可以适用于其他最先进的LiDAR惯性算法，无论其基于卡尔曼滤波器还是优化。，在状态估计后，新扫描中的每个点都将投影到相应的体素中，然后构建或更新由哈希表组织的体素地图（键是体素ID，值是平面拟合结果P）。这些新的特征点将逐步用于进行3自由度平面拟合和协方差估计，这个模块的复杂度不会随着体素中点的数量增加而增加，因为用于拟合平面的所有值都是求和的形式，可以被缓存和逐步计算， 然后，收敛的平面将用于平面合并，在这个模块中，体素中的子平面Pk将基于并查集合并为父平面Pf，与Pk相比，Pf的平面估计结果将更加准确，这显然会改善LiDAR惯性里程计的定位结果。

图1. VoxelMap++系统概述，本文的主要贡献是用黄色表示的建图模块。平面合并算法如算法1所示

实验

我们在具备2.9GHz 8核和16Gib内存的笔记本电脑上，使用C++和机器人操作系统（ROS）实现了提出的VoxelMap++系统。实验数据包括开源数据集M2DGR和我们自己收集的具有挑战性的退化或非结构化数据集，传感器平台如图4所示。

图4：我们的数据采集设备配备了Livox HAP激光雷达和内置IMU的ZED 2i相机，这些设备已经很好地固定在手推车上。表I提供了有关A-LOAM、LeGO-LOAM、LIO-SAM、LINS、FAST-LIO2、VoxelMap和我们提出的VoxelMap++在测试路线和评估结果方面的详细信息。可以清楚地看出，这些数据集涵盖了各种用于SLAM的环境，包括远距离和短距离、室内和室外、直线和曲线路线，这些场景足以说明结构化的城市环境。

图5显示了我们方法的LiDAR轨迹以及所有样本数据集序列上的实际轨迹。

图5 我们方法LiDAR估计轨迹（蓝色）与实际轨迹（红色）在所有样本序列上的情况。如表II所示，VoxelMap和VoxelMap++在非结构化场景中比其他最先进的方法更具鲁棒性和准确性。

如表III所示，其他SLAM算法在走廊中更容易累积误差，我们提出的VoxelMap++在很大程度上比其他方法更准确，主要是因为平面合并可以更准确地估计平面的表示并实时估计它们的协方差。

我们提出的VoxelMap++的另一个优势是与其他先进方法相比，它在CPU和内存资源使用方面更少，如表IV所示。

建图效果如下

总结

本文提出了一种可合并的用于在线LiDAR惯性里程计的体素建图方法，与其他方法相比，这种方法保持了具有3自由度表示和相应协方差的平面特征，从而有效提高了计算速度并节省了内存使用，为了提高平面拟合的精度充分利用了体素之间的关系，并在平面拟合收敛后基于并查集合并了共面体素。本文还展示了如何在基于迭代扩展卡尔曼滤波的LiDAR惯性里程计中实现所提出的建图方法。在结构化的开源数据集和我们自己的具有挑战性的数据集上的实验表明，我们的方法在性能上优于其他最先进的方法。然而，该方法也存在一些缺点。例如，在动态场景（如关闭的电梯）中，鲁棒性将显著下降。因此将考虑从识别体素变化的角度对该方法进行优化。