适用于高分辨率雷达的非光线投射占用栅格建图方案

1. 笔者总结

机器人系统中，高效的地图数据结构是保证整个系统效率的关键。常见的点云地图存储方式包括：关键帧集合、树形结构（kdtree、octree）、voxels，而用于导航定位路径规划的地图通常是 Gridmap、Octomap格式。然而对于高分辨率的雷达，占用栅格地图的计算效率却依旧面临挑战。

今天笔者介绍的是一项来自香港大学火星实验室新的工作，与传统的栅格地图构建方式不同，这项工作基于深度图像投影确定占用栅格状态，使用了包含哈希栅格和八叉树的混合地图结构，并且能够增量更新，在计算量和内存效率之间提供了平衡，从作者的真实应用实验中能够看出地图的轻量化、高效性。这里也推荐「3D视觉工坊」新课程《彻底理解dToF雷达系统设计[理论+代码+实战]》。

图 1. 我们提出的框架 D-Map 作为实时高分辨率占用测绘模块，用于古代堡垒中的自主无人机探索任务。(a) 无人机采集的高保真点云。(b) 场景鸟瞰图。(c) 空中平台搭载128通道激光雷达(OS1-128)执行勘探任务.

2. 摘要

占用地图是机器人系统对未知和已知环境区域进行探索的基本组成部分。本文提出了一种用于高分辨率激光雷达传感器的高效占用建图框架，称为D-Map。该框架引入了三个主要的创新点来提高占用地图的计算效率。首先，我们使用深度图像来确定区域的占用状态，而不是传统的光线投射方法。其次，我们在基于树的地图结构上引入了一种高效的树上更新策略。这两种技术避免了对小栅格的冗余访问，显著减少了要更新的栅格数量。第三，我们利用激光雷达传感器的低误报率，在每次更新时从地图中删除已知栅格。这种方法不仅通过减小地图尺寸来提高我们框架的更新效率，而且赋予它一个有趣的递减特性，我们将其命名为D-map。为了支持我们的设计，我们对深度图像投影的准确性和占用更新的时间复杂性进行了理论分析。此外，我们在公共和私人数据集中对各种激光雷达传感器进行了广泛的基准实验。与其他最先进的方法相比，我们的框架显示出卓越的效率，同时保持了相当的建图精度和高内存效率，我们展示了D-Map在手持设备和携带高分辨率激光雷达的无人机平台上用于实时占用地图的两个真实世界应用。此外，我们在GitHub上开源了D-Map的实现。

3. 主要贡献

提出了一种基于深度图像投影的占用状态确定方法，以减轻传统光线投射技术中的计算负载。这种基于投影的方法能够确定任何大小的单元的占用状态，从而允许在大规模环境中进行后续的高效更新。

提出了一种新的基于混合地图结构的树上更新策略，用于更新占用状态，在计算和内存效率之间提供了良好的平衡。混合地图结构将未知空间存储在八叉树上，这使得能够高效地表示大的未知空间，而占用的空间存储在哈希栅格地图上。就效率而言，所提出的策略允许在八叉树上确定大小区的占用状态，从而避免对小小区的不必要更新并提高效率。

利用激光雷达测量的低误报率，在每次更新时直接删除具有确定状态（即占用或空闲）的栅格。这种方法使我们的地图结构具有递减特性，因此我们将我们的框架称为D-map，从而提供更高的计算效率和更少的内存使用。

对所提出的占用状态确定方法的准确性以及D-Map中更新和查询的时间复杂性进行了深入分析。具体来说，我们导出了一个分析函数来量化相对于深度图像分辨率的精度损失。D-Map更新的时间复杂性分析为我们优于依赖射线投射的最先进方法的卓越性能提供了理论支撑。

4. 算法解析

系统框架

图2. D-Map的框架概述。蓝色块显示 D-Map 的输入，包括点云和相应的传感器里程计。橙色块是D-Map的占用图结构，由用于维护占用空间的哈希网格图和用于维护未知空间的八叉树组成。绿色块中提出了占用更新策略，该策略提取八叉树上传感区域内的单元，并根据使用深度图像的占用状态确定方法进行操作。

深度图像栅格化

图3. 该图说明了图分辨率d、检测范围R和深度图像分辨率之间的空间关系。

在为占用状态确定做准备时，将激光雷达传感器捕获的点云光栅化为当前传感器姿态的深度图像。为了确保状态确定的准确性，深度图像的分辨率应该足够小，使得从地图到深度图像的单元的投影面积大于一个像素。如图3所示，我们使用以下公式确定相对于地图分辨率d和LiDAR检测范围R的深度图像分辨率：

然而，高分辨率地图将产生巨大尺寸的高分辨率深度图像，由于点云的数量远小于深度图像的大小，因此会有许多空像素。为了解决这个问题，我们将深度图像分辨率与激光雷达角分辨率绑定，激光雷达角分辨是以旋转方式发射和接收的两个激光脉冲之间的最小角度。具体来说，我们将标准深度图像分辨率定义为

2D分段树

为了确定地图中单元的占用状态，采用了两步过程，其中首先将单元格投影到深度图像上，然后将投影的深度与相应区域的最小和最大深度值进行比较。由于单元格在深度图像上的投影面积随单元位置而变化，因此采用2-D分段树结构来加快对深度图像上最小值和最大值的有效查询，如下所述。

分段树是一种完全平衡的二叉树，通过表示一组区间来有效地提供范围查询[53]。图4描述了通过一维段树查询最小值的过程。分段树是通过递归地将数组一分为二来构建的，直到每个节点都包含一个元素。由于段树上的每个节点代表数组的一个区间，因此区间中值的汇总信息，例如最小值和最大值在构造过程中进行预处理，以加速后续查询。查询时，分段树使用节点子集（图4中的彩色节点）检索查询区间的最小表示。该结果是通过用比直接查询更少的操作来汇总来自检索到的节点的信息而获得的。一维段树上查询的时间复杂度为O（log N），其中N是离散数组中元素的数量，而直接查询的时间复杂性为O（N）。

图 4 展示了在一维线段树上快速查询 [2, 7] 像素范围内最小值的示例。范围查询从线段树的根开始，沿着树递归搜索，直到当前节点范围被查询范围完全覆盖，其中建树时该节点上已经保存的节点范围的最小值为 被退回。在此示例中，范围 [2, 7] 产生四个节点，分别表示范围 [2, 2]、[3, 4]、[5, 6] 和 [7, 7]。从这四个节点有效地获得范围的最小值，而不是计算数组中的六个元素。

将一维分段树扩展到二维结构的方法包括构建“分段树的分段树”。外层中的分段树按行分割2-D阵列，并且在外层分段树的每个节点上，构造1-D内部分段树以保持覆盖行上的列信息。对2-D分段树的查询首先在外部树中搜索表示被查询区域覆盖的行的节点，然后遍历相应节点上的内部分段树以检索覆盖的列。最后，根据存储在检索到的节点上的信息来总结查询区域上的结果。在大小为的二维数组上查询二维段树的时间复杂度为，而直接查询导致时间复杂度给定从点云光栅化的深度图像，我们构建一个2-D分段树，以保持每个树节点上的最小和最大深度值，分别表示为和。此外，我们还跟踪每个节点覆盖区域内点云占用的像素数量，表示为。

占用状态确定

我们以五个小区为例介绍了我们确定小区占用状态的方法的原理，如图5所示，编号从1到5。我们首先根据单元格是否完全位于激光雷达的传感区域内对其进行分类。如图5（b）中自上而下的视图所示，网格1、网格2和网格3完全位于感应区域内，而网格4和网格5只有一部分位于感应区域内部。在完全内部的单元格中，网格3被确定为已知的，因为它位于观察环境中所有物体的前面；网格1由于其位于对象后面而被确定为未知。网格2的占用状态仍然是不确定的，因为它的一部分位于对象的前面，而另一部分位于后面。关于其中一部分在感测区域内的单元格，网格5被确定为未知，因为它位于物体后面。尽管位于物体前方，但由于网格4的位置不完全在内部，因此网格4的占用状态仍不确定。根据上述原理，D-Map将位于激光雷达传感区域内部或与之相交的单元投影到当前深度图像上，该图像由最近的点云光栅化而成，这些点云表示环境中的对象。单元和对象之间的相对位置是通过比较它们的深度值来确定的。在Alg.1中描述了对深度图像的占用状态确定的过程。

占用栅格地图

地图结构

为了优化计算和内存效率之间的平衡，D-Map利用哈希网格映射来维持占用空间，利用八叉树来维持未知空间。

哈希栅格地图

由于环境中的占用区域通常比自由和未知区域少，我们使用体素哈希技术在哈希网格图中保持环境的占用空间，这允许在的时间复杂度下进行有效的更新和查询操作。给定点p=[x，y，z]的哈希键值由哈希函数hash计算，定义如下：

其中d表示散列网格图的分辨率。P、Q是大素数，而Q也用作哈希表的大小。mod是两个整数之间的模计算。P和Q的值经过仔细选择，以最大限度地降低冲突概率，在我们的工作中，P和Q分别设置为116101和201326611。

八叉树地图

基于树的映射结构由于其高内存效率而成为占用映射的常用方法。在各种基于树的数据结构中，八叉树[58]脱颖而出，因为它在动态更新方面优于其他空间数据结构[59]。此外，八叉树上的空间划分自然允许在树更新期间确定大单元的占用状态。因此，我们利用八叉树来组织未知空间。

在D-Map中，八叉树上的节点包含以下元素：

点阵列包含其八个子节点的地址。如果节点是叶节点，则为空。

由节点表示的单元格的中心。

节点表示的单元格的大小。

初始化

为了初始化D-Map中的映射过程，环境的占用状态被认为是完全未知的。在实现中，给定感兴趣区域的初始边界框C bbx，八叉树的根节点被初始化以表示未知立方体C根，其中心C被分配在C bbx的中心。根节点的大小L指定为边界框C bbx的最长边长，并且子节点的点阵列ChildNodes初始化为空。此外，哈希网格映射中的哈希表被初始化为空表。值得注意的是，初始边界框并不限制映射区域，因为八叉树和哈希网格映射都允许映射空间的动态增长。

占用栅格更新

占用状态索引

D-Map中使用了两个数据结构，因此执行两个步骤以获得地图分辨率下的单元占用状态。首先，如果单元的对应节点存在于八叉树上，则该单元被确定为未知。否则，该区域被确定为已知区域。随后，查询哈希地图以确定其是否被占用。如果没有，则该区域被确定为自由空间。这里也推荐「3D视觉工坊」新课程《彻底理解dToF雷达系统设计[理论+代码+实战]》。

5. 实验

实验在三个开放数据集和一个私有数据集上进行。第一个公开数据集是Kitti数据集，它通过Velodyne HDL-64E旋转3D激光扫描仪在10 Hz下捕获深度测量。考虑到将在我们的基准实验中使用的与基于网格的映射相关的巨大内存消耗，我们选择了序列Kitti 04、Kitti 06和Kitti 07来进行基准评估。第二个公共数据集是FAST-LIO中提供的户外序列MainBuilding，该序列使用半固态3D激光雷达传感器Livox-Avia以10Hz收集数据。Octopap工作中提供的第三个公共数据集包括室内序列FR-079和室外序列Freiburg。此外，我们在私人数据集Workshop上评估了递减特性的好处，在该工作坊中，使用10 Hz的3D LiDAR Livox Avia手持扫描杂乱的室内环境。值得注意的是，我们的激光雷达惯性里程计框架FAST-LIO2获得了序列车间和主楼中占用地图的里程计估计。

表1提供了上述数据集的进一步细节。

效率评估分析

表2总结了各种占用建图方法的平均更新时间 图12比较了两个序列中占用更新的时间消耗

精度评估分析

我们使用Octopap的建图结果作为真值进行精度评估。具体来说，我们通过对计算建图区域内具有正确占用状态的单元格总数来衡量D-Map的准确性。未知空间和自由空间的实验结果如表3所示。

6. 实际应用

高分辨率实时三维地图交互导航

自主无人机探索

地图重建效果

占据栅格建图效果

7. 总结

本文提出了一种新的占用映射框架D-Map，旨在为高分辨率激光雷达传感器提供有效的占用更新。我们提出的框架由三个关键技术组成。首先，提出了一种通过深度图像投影来确定任意大小单元格占用状态的方法。其次，利用高效的树上更新策略，开发了一种混合地图结构。第三，引入了一种去除策略，该策略利用激光雷达的低误报率从地图中去除已知单元格。这样可以减少需要更新的单元数量，降低地图更新的成本，从而显著提高效率。为了验证我们提出的框架，我们对其准确性和效率进行了理论分析，并在各种激光雷达数据集上进行了广泛的基准实验。结果表明，与其他最先进的建图方法相比，D-Map显著提高了效率，同时保持了相当的精度和高存储效率。另外演示了两个真实世界的应用，以展示D-Map在基于高分辨率激光雷达的应用中的有效性和效率。未来，我们可以扩展我们的D-Map框架，以支持动态环境中的占用建图，并利用并行处理。

审核编辑：黄飞