VoxelMap:Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry

一、摘要

本文提出了一种高效且概率自适应的体素建图方法，用于LiDAR里程计。该方法通过构建包含平面特征的体素地图，实现对环境的概率表示和LiDAR扫描的准确配准。通过将体素地图组织为哈希表和八叉树的结构，实现了地图的高效构建和更新。将提出的体素地图应用于迭代扩展卡尔曼滤波器（iEKF），并构建了最大后验概率问题进行姿态估计。在KITTI数据集上进行的实验证明了我们的方法相对于其他最先进的方法具有高精度和高效性。在使用固态LiDAR和非重复扫描LiDAR进行的室内和非结构化室外环境实验进一步验证了我们的建图方法对不同环境和LiDAR扫描模式的适应性。

二、背景

1. 过去方案:

现有LiDAR（惯性）里程计中主要采用的建图方法是点云地图，该地图由原始或选定的边缘或平面点组成。然而，点云地图的一个主要缺点是难以考虑由LiDAR测量噪声引起的地图不确定性。

2. 论文的Motivation:

为了解决点云地图的不确定性问题，本文提出了一种新颖的在线自适应体素建图方法，该方法通过构建适应环境结构和点密度变化的不同大小的体素来跟踪显著特征（本文使用平面）。这种显式的特征参数化允许考虑当前点和地图的不确定性，从而提高了配准的准确性。此外，本文还提出了一种基于哈希表和八叉树的数据结构，用于高效构建、更新和查询体素地图。通过在真实世界的各种环境（结构化和非结构化）和LiDAR（多旋转LiDAR和非传统固态LiDAR）的数据集上进行全面验证，证明了所提出方法在KITTI数据集上相对于其他最先进的方法的优越性。

三、方法

1. 概率平面表示

本文提出的方法主要集中在LiDAR里程计中的概率平面表示。体素地图中的每个体素包含一个概率特征，其中使用平面特征是因为它们在环境中易于获取。推导了平面特征的不确定性模型，考虑了LiDAR传感器的测量噪声（距离和角度）和LiDAR姿态估计误差（lidar坐标系到全局坐标系）。分析了LiDAR点的不确定性，并确定了世界坐标系中LiDAR点的不确定性模型。还提出了平面法线的不确定性模型。

2.由粗到细的体素地图构建

a.motivation

当点云相对稀疏时，常见的基于面元的精细到粗略建图方法通常只能获得极少量的平面，因此其应用仅限于高分辨率激光雷达和相对较低的扫描速率（如 10Hz），这样才能积累足够数量的点。为了解决这个问题，我们提出了一种从粗到细的体素建图方法，这种方法可以在点云稀疏时建立粗略的体素地图，并在收到更多点时对映射进行细化。

b.体素地图构建

该方法还包括使用哈希表和八叉树组织的自适应体素地图的构建。首先将空间（在全局坐标系中）切割成体素，每个体素的大小与粗地图分辨率相同。然后，在定义世界帧的第一次激光雷达扫描中，将包含的点分布到体素中。包含点的体素被索引到哈希表中。然后，对于每个填充的体素，如果所有包含的点都位于一个平面上（点协方差矩阵的最小特征值小于指定阈值），就存储平面点，并计算平面参数（n, q）及其不确定性Σn,q；否则，当前体素将分成八个八面体，并在每个八面体中重复平面检查和体素切割，直到达到最大层数。体素的大小各不相同，每个体素包含一个由所含激光雷达原始点拟合的平面特征。

c.体素地图更新

通过使用估计的姿态（ III-D）将新的LiDAR点云注册到全局地图中，更新体素地图。当新点位于未填充的体素中时，它将构建该体素。否则，当新点被添加到已有的体素中时，体素中平面的参数和不确定性都需要更新。这将导致处理时间随着不断接收新点而增加。为了解决这个问题，我们发现平面参数的不确定性会很快收敛，如下图所示，图中每个点的位置都带有均值为零、方差为 0.1m2 的高斯噪声。可以看出，当点的数量达到 50 个时，法向量的不确定性就会收敛。不确定性收敛后，我们丢弃所有历史点，保留估计的平面参数 (n, q) 和协方差 Σn,q。一旦有更多新点出现，我们只保留最近的 10 个点，并计算由这 10 个点组成的新平面法向量。如果新的法向量和之前收敛的法向量继续出现比较大的差异，我们就认为这个区域的地图发生了变化，需要按照第 III-B2 节的方法进行重建。

3. 点面匹配

基于点和平面的精确不确定性建模，可以轻松实现点到平面的扫描匹配。给定一个在世界坐标系中用姿态先验预测的激光雷达点 WPi，我们首先通过其哈希键找到它位于root voxel(粗地图分辨率）中。然后，对所有包含的子象素进行轮询，寻找与该点的可能匹配点。具体来说，如果一个子象素包含一个法线为 ni、中心为 qi 的平面，我们就会计算点到平面的距离：

如果点位于候选平面上，其距离 di 应该服从（10）中的分布。因此，设该分布的标准偏差为 σ = √Σwi ，我们就可以考察测量到的点到平面的距离是否在 3σ 以内。如果在 3σ 以内，则该点将被选为有效匹配点。此外，如果一个点根据 3σ 标准匹配多个平面，则匹配概率最高的平面。如果没有平面通过 3σ 检验，则丢弃该点，以消除体素量化可能造成的错误匹配。

4. 状态估计

基于类似于 FAST-LIO2  的迭代扩展卡尔曼滤波器，建立了一个激光雷达（惯性）测距系统。假设我们得到了一个具有协方差 ̂ Pk 的状态估计先验值 ̂ xk。对于纯激光雷达测距（实验 A 和 B），该先验值可通过匀速假设提供；对于激光雷达-惯性测距（实验 C），该先验值可通过 IMU 传播提供。该先验值将与第 III-C 节中匹配的点-平面距离融合，形成最大后验（MAP）估计值。具体来说，第 i 个有效的点-面匹配会导致观测方程

结合状态先验和所有有效测量，我们可以得到 MAP 估计：

其中第一部分是状态先验，第二部分是测量观测。

四、结果

本文在三种不同的环境（城市环境、室内环境和非结构化室外环境）上评估了该建图方法使用的三种不同的LiDAR（Velodyne HDL-64E S2、Realsense L515和Livox Avia）。实验将所提出的方法与FAST-LIO2、MULLS、LiTAMIN2、Suma和Lego-Loam等最先进的方法进行了比较。

实验结果显示，所提出的方法在绝对轨迹误差（ATE）方面相对于其他算法实现了最佳的整体精度。该方法在长期序列中表现特别出色。通过绘制估计的LiDAR轨迹并与真实值进行比较，进一步证明了所提出方法的高精度。

五、优化方向

在体素中添加边缘等其他特征，以提高该方法的普遍性。