移动机器人基于结构化点云的高效动态LiDAR里程计

本文提出了一种实时动态LiDAR里程计方案，用于城市搜索与救援（Urban Search and Rescue, USAR）场景中的移动机器人。现有的动态对象检测方法通常依赖于预训练的学习网络或计算昂贵的体素地图。为了提高计算能力有限的机器人的效率，我们在里程计与检测模块之间复用数据。通过利用深度图分割技术和一种新的基于残差的启发式方法，该方法在将点云整合到地图之前区分动态和静态对象。该方法在包含大量动态对象的环境中展示了鲁棒的目标跟踪和改进的地图精度。即使是高度非刚性的对象（如奔跑的人类）也可以在点级别精确检测，而无需对点云进行先验降采样，从而避免信息丢失。通过对模拟和真实数据的评估，我们验证了其计算效率。与一种最先进的体素方法相比，该方法在检测性能相当的情况下，处理时间大幅减少，仅为动态对象检测和跟踪模块增加了14ms的处理时间。实现代码和新的真实数据集已作为开源资源提供，用于进一步研究。

图1：提出的方法在动态LiDAR里程计中的应用，支持高效分割和跟踪高度复杂的物体。顶部：在一个高度动态的场景中，检测并跟踪了一组跳跃中的人。动态点以绿色显示，轨迹以蓝色标出。底部：用于分割上述点云的距离图像。不同的颜色表示不同的静态和动态物体。

本文提出了一种新的动态LiDAR里程计方法，与其他最新工作相比，本文采用无网格的方法，直接处理结构化点云，从而即使在大规模环境中也能快速处理高分辨率扫描。我们使用最先进的LiDAR里程计方法作为骨干，在深度图上对点云进行分割，并独立跟踪对象，使其可用于其他流程。同时基于动态对象在扫描匹配过程中比静态对象表现出更高残差的观察，我们提出了一种新的启发式方法来区分两类对象。动态对象随后可以在整合到地图之前从扫描中移除。该方法专为USAR应用的特定需求设计，尽可能通用，仅对环境做出少量假设。主要目标是获得一个轻量级、可理解的系统，该系统可以在基于地面的机器人上实时运行，仅需一个高分辨率LiDAR传感器。总结如下，本研究的主要贡献包括：

• 一种轻量级的动态LiDAR里程计端到端方法，包括里程计、对象检测、跟踪和建图。

• 一种基于残差的新的启发式方法，用于区分动态和静态对象。

• 在公共基准数据集和创建的真实数据上的评估。

• 提供开源代码和数据集。

总体概述

在基于Chen等人提出的LiDAR里程计方法进行扩展，采用两阶段的广义迭代最近点（GICP）方法进行快速配准。

• 第一阶段：通过对连续扫描和进行配准，计算机器人的运动初始估计。

• 第二阶段：将与子地图对齐，进行精细配准。

配准后残差被投影到图像上，并与深度图结合，将输入分割为各个几何对象。跟踪与关联模块更新每个对象的卡尔曼滤波器，关联新检测的对象，并为每个对象分配动态状态。最后将标记为非静态的点从当前扫描中移除，然后将扫描整合到关键帧数据库中。

图2：系统概览。两帧连续扫描通过里程计模块（蓝色）进行配准。检测模块（绿色）将投影的距离图像和残差图像分割为独立的物体。

跟踪与关联模块（黄色）为每个物体分配动态状态。在将当前扫描整合到关键帧数据库之前，非静态点会被移除。基于物体的边界框，从全局地图（红色）中移除“幽灵”轨迹。

检测模块

为了检测场景中的动态元素，我们首先通过深度图表示的扫描云对相关段进行分割。然后利用扫描匹配的残差将这些段分类为动态或静态。

基于深度图的分割

几何分割点云的计算成本较高，因为空间聚类需要最近邻搜索，这对于大规模点云来说开销很大。我们采用Bogoslavskyi等人提出的深度图分割方法。首先，通过映射将点云投影到柱状图像上。每个点被分配到图像坐标：其中，表示传感器的上视角，和分别表示水平和垂直分辨率。该方法适用于任何点云，无论其稀疏程度和点序列如何。图像像素存储深度，即点到传感器的距离：没有点对应的像素被标记为无效。在分割之前，我们对深度图应用粗略的地面移除方法。然后，采用区域生长算法对剩余点进行聚类，标记为不同的段。

图3：由GICP算法生成的残差图像。为便于观察，配准操作是在原始点云上完成的，而实际方法使用的是下采样和滤波后的点云。彩虹色映射突出显示了具有较高残差的点，黑色像素表示无效点。

基于扫描匹配残差的分类

为了在跟踪阶段区分静态和动态对象，我们提出使用扫描匹配算法（如GICP）生成的残差图，这些残差反映了源点云到目标点云中最近邻的欧几里得距离。残差值较高的点更可能属于动态对象。通过对段的残差平均值进行计算，可以分类段的动态状态。这些残差图是里程计模块的副产物，计算开销极小，易于集成到方案中。

跟踪与状态更新模块

跟踪与关联

对象必须在全局坐标系中进行跟踪，因为与机器人平行的移动在机器人坐标系中会显得静止。在每个时间步，检测模块提供一组检测结果。由于每个图像像素对应点云中的唯一点，我们可以直接从点云中恢复它们的3D形状。为了有效地表示状态，我们通过主成分分析（PCA）计算对象的边界框，并存储点索引以及段的平均残差。然后为每个对象初始化一个卡尔曼滤波器，该滤波器内部使用一个10维状态表示，包括对象的位置、绕z轴的旋转、边界框尺寸和速度。在每个时间步，滤波器会更新与之相关联的检测结果。在接收到新检测结果时，我们使用Weng等人提出的方法将其与现有对象关联。数据关联基于匈牙利算法（Hungarian Algorithm），通过最小化关联成本的总和确定最佳匹配。

动态状态更新

在数据关联步骤之后，我们更新每个对象的状态。对象可以处于以下三种状态之一：未定义、静态或动态。新对象的默认状态为未定义。如果在次检测中未满足动态的条件，则其被标记为静态。为了从静态或未定义状态转变为动态，我们检查以下条件：

• 检测次数必须超过以避免误报。

• 段在残差图中的平均残差值超过阈值。

• 对象的位移必须超过一定距离。

如果满足上述条件，对象被标记为动态，且在其生命周期中保持此状态。

图4：动态状态更新。在每个时间步，所有跟踪的物体都会更新其状态，并可根据检测次数（命中次数）、平均残差以及与起始位置的位移来改变其动态状态。

动态点移除模块

跟踪模块将所有属于动态对象的点的索引传递给里程计模块。这些点会被直接从转换后的输入扫描中移除。随后，经过过滤和降采样的点云被添加到关键帧数据库中以生成子地图和全局地图。为了避免最初静止的对象在地图中留下幽灵轨迹，我们维护一个所有对象边界框的滚动窗口历史。如果一个静态对象变为动态，我们会从全局地图中移除这些边界框内的所有点。由于全局地图仅用于可视化，该步骤对里程计计算的实时性没有影响，因此是异步执行的。

动态点移除发生在扫描间配准步骤之后，意味着这些点在GICP模块的源点云中存在，但在目标点云中不存在。这是因为分割步骤是在输入点云被转换到全局坐标系后执行的。所需的变换是配准的结果，这导致了一个"先有鸡还是先有蛋"的问题。尽管可以通过重复配准和清理输入点云的方式解决，但这种方法会显著增加计算开销。

实验设置

我们使用一台四轮差速驱动机器人进行数据采集，该机器人配备了一个Ouster OS-0 128线LiDAR传感器，安装在顶部以实现无障碍的360°视野。传感器以10 Hz的速率提供大小为128×1024的结构化点云。实验在一台配备Intel Core i7-10875H CPU和32 GB内存的消费级笔记本电脑上进行。

图5：在DOALS小镇模拟序列中，不同残差阈值下的IoU、精度和召回率。

数据集

为了验证方法的有效性，我们录制了一个名为"kantplatz"的数据集，该数据集公开可用。轨迹总长度约为500米，持续时间为480秒，录制于包含各种动态对象（如行人、自行车、汽车和公共汽车）的城市环境中。此外，我们还使用DOALS数据集中的小镇模拟序列进行比较。

图6：kantplatz数据集的分割示例。动态物体用绿色边界框和点标出，静态物体用红色边界框标出。顶部：检测到几名行人和骑行者。左下：动态和静态物体，包括街灯柱。一辆经过的厢型车被检测到，背景中的另一辆车被错误分类为静态物体。右下：停放的汽车被检测为静态物体。

点分类精度

实验结果表明，我们的方法可以有效检测和跟踪大多数动态对象，特别是在中近距离深度内表现出较高的精度。然而，对于稀疏点云表示的远距离对象，检测性能有所下降。

图7：kantplatz数据集上地图生成结果的比较。顶部：没有动态物体处理的纯DLO，底部：我们的方法。

图8：来自kantplatz数据集的动态场景。我们的 approach（顶部）准确地检测到跳跃的人，仅有一只手臂部分被截断，Dynablox（底部）仅检测到人的部分，并倾向于过度分割。

处理时间分析

由于我们的方法旨在提供轻量级解决方案，我们分析了各算法组件的处理时间。对于每帧扫描，我们的方法平均处理时间为46.0 ms，能够实现21.7 Hz的帧率，其中动态对象检测的额外开销仅为14.3 ms。