arxiv | 中科院联合国科大开源 TRLO：一种结合3D动态物体跟踪与移除的高效LiDAR里程计
【TRLO: An Efficient LiDAR Odometry with 3D Dynamic Object Tracking and Removal】
文章链接：[2410.13240] TRLO: An Efficient LiDAR Odometry wit...
项目主页：GitHub - Yaepiii/TRLO

TRLO: An Efficient LiDAR Odometry with 3D Dynamic Object Tracking and Removal

摘要：同步状态估计和绘图是在动态城市环境中工作的移动机器人的一项基本能力。大多数现有 SLAM 解决方案严重依赖于静态假设。然而，由于移动车辆和行人的存在，这一假设并不总是成立，导致定位精度下降和地图扭曲。为了应对这一挑战，我们提出了 TRLO，一种动态 LiDAR 里程计，可以有效提高状态估计的准确性并生成更清晰的点云图。为了有效地检测周围环境中的动态物体，应用了基于深度学习的方法，生成检测边界框。然后，我们设计了一个基于无迹卡尔曼滤波器 (UKF) 和最近邻 (NN) 策略的 3D 多对象跟踪器，以可靠地识别和删除动态对象。随后，采用快速两阶段迭代最近点求解器来使用清理后的静态点云来求解状态估计。请注意，提出了一种新颖的基于散列的关键帧数据库管理，用于快速访问搜索关键帧。此外，所有检测到的对象边界框都被用来施加姿势一致性约束，以进一步细化最终的状态估计。对 KITTI 和 UrbanLoco 数据集进行的广泛评估和消融研究表明，与基线相比，我们的方法不仅实现了更准确的状态估计，而且生成了更清晰的地图。

索引术语------定位、地图绘制、动态 SLAM、移动机器人。

一、引言

准确的状态估计对于自动驾驶车辆在没有 GPS 辅助的情况下定位自身并在未知环境中探索是必不可少的。视觉 SLAM 方法 [1]、[2] 在变化的光照或均匀纹理下可能表现不佳，与之相比，基于 LiDAR 的方法可以通过精确捕获环境的几何细节来提供更可靠的姿态估计 [3]。基于特征的方法[4]-[6]试图通过关注最具代表性的点（例如边缘或平面）来解决两个相邻扫描之间的变换。然而，他们可能会无意中丢弃结构良好的点，这有助于提高下游注册的质量。特别是，当场景缺乏明显特征时，基于特征的方法可能难以提取足够的特征点，这可能导致姿态漂移和映射失真。

如今，一种直接利用整个点云来估计 LiDAR 扫描之间的转换的替代方法（通常通过迭代最近点 (ICP) [7]）很流行。然而，在处理密集扫描时，ICP 技术的计算消耗过高，可能会影响实时性能。为了解决这个问题，提出了各种ICP变体[8]、[9]来提高计算效率。一些衍生的 LiDAR 里程计 (LO) 作品 [10]-[12] 基于改进的 ICP 算法取得了有希望的性能。

大多数现有的 LO 解决方案 [4]、[5]、[10]-[12] 严重依赖于静态环境的基本假设。虽然其中一些[11]、[12]通过一些算法设计减轻了动态物体的影响，但它们并不直接关注运动物体，导致定位精度下降或潜在的失败。此外，移动物体通常会影响测绘质量并阻碍后续导航任务。因此，有效地检测和跟踪动态物体已成为机器人在现实世界中稳健定位和建图的关键。

尽管存在一些令人印象深刻的工作[13]-[17]，引入创新方法来应对动态环境中的 SLAM 挑战，但它们仍然存在一些局限性。 RFLIO [13]、DOR-LINS [14] 和 LIMOT [18] 依赖 IMU 提供姿态先验，这可能容易受到噪声的影响。许多工作[15]、[19]将严格的语义分割纳入LO系统，其性能受到稀疏通道LiDAR的限制。 [16]和[17]都专注于提高物体跟踪的精度，而不是减少动态物体对LiDAR里程计精度的影响。在这项工作中，提出了一种新颖且高效的动态 LO，即 TRLO。为了有效地检测和跟踪动态对象，基于支柱的对象检测网络被实现为 TensorRT 模块，以及基于无迹卡尔曼滤波器（UKF）[20]和最近邻（NN）策略的多对象跟踪（MOT）方法被提议。随后，静态点云被输入Fast G-ICP [21]模块进行两阶段扫描匹配，以实现精确的定位和建图。此外，我们应用滑动窗口中累积的边界框来施加一致性约束，进一步细化机器人的姿态。图 1 显示了用我们提出的方法生成的点云图的示例。

图 1. 我们在 Urbanloco 数据集上进行动态对象移除的全局干净地图。

A和B是拥挤交通路口局部地图细节的两个例子，展示了我们的方法在地图绘制方面的优越性。

我们的工作贡献总结如下：

• 我们提出了 TRLO，一种能够准确检测和跟踪动态物体的 LiDAR 里程计，以实现高精度状态估计和清晰映射，其关键是提出的基于 UKF 的可靠 3D 多目标跟踪器和 NN。

• 我们提出基于散列的关键帧数据库管理，通过直接访问关键帧索引来加速扫描到地图的注册。

• 我们重用在短期滑动窗口中累积的边界框的姿势，以对状态估计施加一致性约束，假设所有对象都站在局部场景中的同一地面上。

• 对 KITTI 和 UrbanLoco 数据集的挑战性序列进行的大量实验表明，我们的方法在定位和绘图方面优于基线。

我们的方法的源代码和演示视频位于：https://yaepiii.github.io/TRLO/。

二.相关工作

激光雷达里程计和SLAM是机器人领域的经典主题，有大量的研究。在本节中，我们简要回顾传统的激光雷达里程计估计方法，并重点讨论动态场景下的 SLAM 解决方案。

A. 传统 LiDAR 里程计

作为最典型的基于特征的方法，LOAM [4] 引入了一种创新方法，将估计问题分为两种不同的算法，一种算法在高频下运行，精度较低，而另一种算法在低频下运行，精度较高。准确性。将两个结果融合在一起以产生高频率和高精度的最终估计。 LeGO-LOAM [5] 通过使用地面点分割和两级 Levenberg-Marquardt 算法实现轻量级 LiDAR 里程计，并使用动态小物体的剔除机制提高精度。 MULLS[22]专注于采用主成分分析（PCA）来提取更丰富和稳定的特征，提出了一种独立于扫描线的基于LiDAR的SLAM框架，从而增强了泛化能力。李等人。 [23]提出了一种基于点云等角分布特性的新型基于角度的特征提取方法，并构造固定滞后平滑来联合优化多个关键帧的姿态。

随着处理单元[24]的快速迭代进步，最近的一些工作正在探索直接利用ICP或其变体进行实时状态估计和映射。直接LiDAR里程计（DLO）[10]引入了NanoGICP，一种轻量级迭代最近点求解器，有助于精确配准密集的原始点云以确定机器人的姿态。它通过自适应方法实现合理的关键帧管理和映射。 CT-ICP[11]引入了扫描匹配中的连续性和扫描之间的不连续性的组合，以提高点云配准的准确性和鲁棒性。 KISS-ICP[12]是最新的直接注册方法，其重点是简化ICP的实施。它通过采用自适应阈值数据关联来实现准确性和速度之间的平衡。

B. 动态场景中基于 LiDAR 的 SLAM

传统方法在静态环境的主要假设下运行。然而，在动态城市环境等动态场景中，可能会出现里程计精度下降和地图失真的情况。一些 SLAM 方案致力于识别和过滤环境中的动态对象，以保证位置和地图的准确性。

早期的努力[25]、[26]使用视觉技术来识别和删除动态对象。为了实现室外大规模基于激光雷达的动态SLAM，某些将3D激光雷达传感器与其他传感器结合起来。 RF-LIO[13]基于LIO-SAM[6]框架，利用IMU提供的初始值和多分辨率深度图来检测和去除子图中的动态点，并实现基于扫描到静态的精确配准子地图。 DOR-LINS [14]引入地面伪占用来识别和移除动态物体。然而，引入其他传感器将导致昂贵的成本和额外的复杂性。相比之下，我们的方法使用激光雷达作为唯一的传感器，在降低成本的同时实现可比甚至更好的结果。

受深度学习和用于场景理解的卷积神经网络（CNN）进步的推动[27]、[28]，一些方法从 LiDAR 数据中提取语义信息以进行动态去除。 SuMa++[15]使用RangeNet++[29]进行球面投影，通过逐点概率方法提取语义信息进行动态去除。普弗罗因德舒等人。 [19]使用深度学习技术进行实时 3D 动态对象检测。他们从静态点云中提取特征来估计其自身的运动。尽管有效，但这些方法对LiDAR 里程计的推广，因为语义提取网络是专门为 64 通道或更密集的 LiDAR 定制的。因此，当采用 32 通道 LiDAR 时，系统将无法运行。我们的方法仍然在使用 32 通道 LiDAR 的数据集上产生令人惊讶的结果。

参考文献[16]和[17]与我们的工作最相似。它们都将物体的位姿集成到因子图估计框架中进行优化，从而实现对动态物体的有效跟踪。然而，[16]在里程计估计之前删除了所有检测到的物体，忽略了有价值的静态信息。由于异步状态估计，[17]不能实时运行。尽管S2MAT[30]专注于动态物体跟踪和去除，但它没有解决如何减轻动态物体对LiDAR里程计的影响以提高定位精度。此外，一些方法[31]、[32]侧重于动态场景中的地图构建，因此通常不强调实时性。

三．方法

在本文中，我们的系统架构如图2所示。我们的工作试图解决以下问题：给定k时刻的相邻原始点云扫描P′k和P′k−1，删除动态对象以获得静态点云 Pk 和 Pk−1 作为 LiDAR 里程计的输入，然后估计机器人的全局位姿 χW k ∈ SE(3) 并在世界 W 坐标系中构建全局地图 Mk，其中 χk 由旋转部分 Rk ∈ SO 组成(3) 平移部分 tk ∈ R3。

图 2.我们的系统架构。基于柱的 3D 对象检测器首先用于对原始点云进行预处理，以检测动态和半静态对象，从而生成 3D 对象边界框。随后，应用3D多目标跟踪器来识别和移除动态目标。输入相邻的静态扫描 Pk 以计算扫描到扫描（S2S）变换。初始值被传播到世界坐标系并用于扫描到地图（S2M）的辅助快速GICP。 Pk 与由选择性关键帧组成的子图 Sk 进行扫描匹配。最后，利用检测到的边界框施加的姿势一致性约束进一步优化 S2M 变换，以获得精炼的全局机器人姿势，并根据多个指标进行检查以确定是否应将其存储在关键帧哈希数据集中

A. 3D物体检测器

当我们的系统接收到360°LiDAR采集的原始点云P′k时（图3（a）），为了尽量减少噪声和原始数据的损失，保证物体检测的准确性，机器人周围仅 0.5m 盒式滤波器和 0.25m 体素滤波器用于减少误检测并消除预处理阶段的噪声。然后将处理后的点云输入 3D 物体检测器。

在这个模块中，我们使用 PointPillars [33] 作为特征图提取的主干，然后使用 Center-based [34] 为动态和半静态对象生成 3D 边界框（图 3（b）），其中半静态对象-静态对象被定义为当前可能移动的对象。值得注意的是，尽管我们选择了 Center-base [34] 来保证精度和速度之间的权衡，但我们的管道并不限于特定的对象检测方法。令 BL k,m 为 LiDAR 坐标系 L 中的边界框，在 k 处扫描所有边界框 BL k = {BL k,m|m = 1, ..., M }。为了减轻错误检测对 3D 多目标跟踪的影响，我们选择性地检测汽车和骑自行车的人，同时将阈值提高到 0.75[35]。此外，我们通过将 3D 物体检测器重写为 TensorRT 版本并将其集成到系统前端来优化系统的整体效率，从而使处理速度提高一倍以上。

图 3.所提出的动态检测和过滤的效果。 (a) 原始点云。 (b) 我们的 3D 对象检测器检测到的动态和半静态对象的 3D 边界框（蓝色框）。 (c) 由我们的 3D 多对象跟踪器识别的动态对象的 3D 边界框（绿色框）。 (d) 去除动态物体后的静态点云，将作为激光雷达里程计的输入。

B. 3D 多目标跟踪器

传统的 SLAM 系统依靠几何信息来定位观察结果并将其与地图关联。然而，它们无法考虑移动物体引起的动态场景变化，这可能导致不正确的配准和地图扭曲。动态 SLAM 算法通常配备一定的异常值拒绝功能。然而，如果我们简单地丢弃动态和半静态对象，我们就有可能失去它们对里程计估计的宝贵贡献。

我们提出了一种基于 UKF 和 NN 技术的方法来跟踪 III-A 中检测到的边界框 BL k。与EKF不同，UKF通过使用确定性采样方法而不是线性逼近来解决非线性状态估计[20]，更适合动态目标跟踪。对象跟踪过程的推论如下：

1）估计模型：我们用独立于其他对象和代理的恒定速度模型来近似每个对象的帧间位移。为了简单起见，我们省略下标 m。给定第 k 次扫描时的对象 xO k，其状态定义为：

其中，x、y、z 是机器人坐标系 B 中目标的中心坐标。注意，我们假设 L 和 B 重合。 θ是物体坐标系O和B之间的偏航角，v是需要估计的速度，l、w、h分别是边界框的长、宽、高。然后我们通过 UKF [20] 估计每个对象的状态：

2）数据关联：使用（2）和（3）估计每个目标的新状态后，我们执行数据关联步骤。我们没有采用典型的基于 IOU 的标准，而是简单地使用最近邻技术将目标分配给当前帧中的检测，因为我们发现这种简单的方法在应用中也实现了高精度。当检测与目标关联时，检测到的边界框用于更新目标状态。此外，为了减轻误检测或遮挡的影响，如果没有目标与检测关联，我们只执行预测步骤而不进行更新。

3）跟踪目标的创建和删除：当物体进入激光雷达视野时，跟踪器使用边界框的几何形状进行初始化，并将速度设置为零。由于速度是不可观测的，因此速度分量的协方差被初始化为大值，反映了这种不确定性。当物体离开 LiDAR 视野时，它不会立即删除，而是会保留 Agemax 生命周期，在此期间仅执行预测步骤。这种方法有效避免了删除场景中仍然存在但由于误检测或遮挡而无法匹配的真阳性轨迹的问题。如图3（d）所示，动态去除后我们获得了更干净的静态点云，同时保留了可以为LiDAR测距提供有价值信息的半静态对象。

C. LiDAR 里程计

LiDAR 里程计可以看作是通过将处理后的相邻静态点云与内存中的静态点云进行比较来恢复 SE(3) 变换的过程。该过程通常分两个阶段执行，首先提供最佳的瞬时猜测，然后进行细化以使其更加全局一致。 1）扫描到扫描：在第一阶段，扫描到扫描的目的是计算源点云 P s k 和目标点云 P t k 之间的相对变换 ^ χL k （其中 P t k = Ps k−1 ) 在 L 中捕获，其中

其中，N 是源 P s k 和目标 P t k 之间的对应点的数量，di = pt i − χL k ps i ， ps i ∈ Ps k，pt i ∈ Pt k，以及 Cs k,i 和 Ct k,i是估计的协方差分别对应于源云和目标云的每个点 i 的矩阵。如果有外部传感器（例如 IMU）可用，我们的系统可以通过 IMU 预积分为 Fast GICP 提供初步猜测。然而，值得注意的是，我们的系统不依赖于 IMU。在 IMU 不可用的情况下，我们仍然可以通过将初始猜测设置为单位矩阵 I 来实现鲁棒的状态估计。

最后，我们将结果 ˆ χL k 累加到之前的全局扫描到地图变换 ˆ χW k−1 ，即 ˆ χW k = ˆ χW k−1 ˆ χL k。

2）Scan-to-Map：在此阶段，我们使用 ^ χW k 作为 scan-to-map 的初始猜测，并遵循与 scan-to-scan 类似的过程。然而，这里的目标是通过将当前静态点云 P s k 与局部子图 Sk 进行匹配来获得全局一致的机器人全局状态 ˆ χW k ，使得

其中，M 是源 P s k 和子图 Sk 之间对应点的数量，CS k,j 是子图的每个点 j 对应的扫描缝合协方差矩阵，如稍后在 III-D 中定义的。

3）边界框一致性约束：我们使用III-A中生成的3D边界框对扫描到地图结果的[tz，roll，pitch]施加姿势一致性约束。具体来说，假设所有检测到的物体都站在同一平坦的地面上。因此，我们垂直投影边界框的中心点，产生对象的地面点。然后我们利用所有这些地面点进行地面拟合。如果内部点的数量超过指定的阈值，我们将利用它们来计算地面法线，该法线作为[roll，pitch]的约束。值得注意的是，在某些边界框很少的扫描中，此过程可能会失败。为了避免这个问题，我们实现了一个滑动窗口来累积几次连续扫描的边界框。该滑动窗口内的所有边界框均用于地面拟合。

此外，如果相邻扫描之间边界框的 z 轴平均变化小于 0.1m，我们就推断机器人正在穿越相对平坦的地形。在这种情况下，我们通过考虑局部窗口中的 z 轴位移接近于零来假设地面在两次连续扫描之间不会发生变化，从而对 tz 施加约束。

D. 基于哈希的关键帧管理

受视觉 SLAM 方法 [2] 的启发，我们使用关键帧来管理地图。然而，与常见的关键帧管理不同的是，我们使用哈希表来有效地访问关键帧。参考DLO[10]自适应阈值的思想，根据当前环境的宽敞程度自适应调整添加关键帧的阈值。更多详情请参阅[10]。当机器人的移动距离或旋转角度大于阈值时，我们将机器人的当前状态χk与静态点云Pk以键值对的形式关联起来，然后将其存储到基于哈希的关键帧数据库中。

当需要构建子图时，我们实现了基于最近邻和凸包、凹包的关键帧选择策略。令 Kk 为所有关键帧点云的集合，子图 Sk 定义为 K 个最近邻关键帧扫描 Qk、L 个最近邻凸包扫描 Hk 和 J 个最近邻凹包 Gk 的系列，使得 Sk = Qk ⊕Hk ⊕ Gk，其中⊕表示交集。这种组合通过最大化扫描到地图的重叠来减少整体轨迹漂移，并为系统提供多个尺度的环境特征来对齐。

四．实验

为了验证我们方法的有效性，我们在 KITTI [36] 和 UrbanLoco [37] 数据集上严格进行了广泛的实验，包括里程计基准和映射结果，以及每个组件的运行时间分析和消融研究。所有实验均在配备 20 核 Intel i9 2.50GHz CPU 和 RTX 3060 GPU 的笔记本电脑上执行。

A. 里程计基准

1）实验设置：我们进行基准评估，将我们的方法与几种传统的 LiDAR 里程计方法（A-LOAM [4]、LeGO-LOAM [5]、DLO [10]、CT-ICP [11]）进行比较和 KISS-ICP [12]）以及几种动态 LiDAR 惯性里程计方法（LIO-SEGMOT [17] 和 LIMOT [18]）。为了公平比较，LeGO-LOAM [5]、LIO-SEGMOT [17] 和 LIMOT [18] 中禁用了闭环检测模块。由于SuMa++[15]依靠语义信息提取来过滤动态对象，因此无法在32通道LiDAR收集的数据集上运行。我们重新实现了动态 LO、RF-A-LOAM，集成了基于范围的动态去除。所有实验均在 KITTI [36] 和 UrbanLoco [37] 数据集上进行，使用相对位姿误差（RPE）和绝对位姿误差（APE）的均方根误差（RMSE）作为度量。

2）实验分析：结果见表I. 与传统的 LO 系统相比，我们的方法通常通过提出的动态去除和边界框一致性约束策略取得了令人鼓舞的性能，特别是对于高度动态的序列。此外，除了Urbanloco03序列的APE低于LIMOT[18]之外，所提出的方法优于LIO-SEGMOT[17]和LIMOT[18]。值得注意的是，我们的方法仅使用 LiDAR 扫描作为输入即可获得有竞争力的结果，这证明了动态去除和强大的配准机制的有效性。

B. 绘图结果

1) 定性：如图 4 所示，我们将所提出的方法构建的地图与动态城市环境中的其他基线进行比较。在 DLO [10] 生成的地图中，我们清楚地看到地图中由移动的汽车造成的严重鬼尾。相比之下，我们的方法可以熟练地识别和过滤移动对象，同时保留有价值的半静态对象。与LIO-SEGMOT [17]、LIMOT [18]和RF-A-LOAM相比，我们的方法表现出更好的动态去除性能。最终，我们获得了高精度激光雷达里程计并构建了更清晰的地图。

图 4. 绘图结果。

（a）传统方法生成的全局地图。

(b)传统方法生成的全局地图的局部细节，其中红点代表由移动物体引起的鬼尾。

(d) 我们的方法生成的全局地图的局部细节。很明显，我们的方法有效地检测和过滤移动物体，从而产生更一致的结果。

2）定量：地图质量评价如表所示。 PR和RR见表II。表II代表静态点的保留率和动态点的去除率。从统计数据来看，我们的方法的 PR 分数高于其他基线，而 RR 分数则低于 LIMOT [18]。平衡 PR 和 RR，我们方法的 F1-Score 具有竞争力。

这项工作的动机是实现一个紧凑的动态 LO 系统，致力于通过集成目标检测、跟踪和边界框姿态一致性约束来提高里程计和建图的整体精度。我们承认，我们并不关注如何进一步提高物体检测和跟踪的性能。从大量的比较实验来看，所提出的动态本振系统在定位精度、测绘质量和效率方面与基线相比具有竞争力。

C. 消融研究

为了研究我们的系统组件的影响，我们对 KITTI07 和 UrbanLoco05 序列进行消融研究。我们比较以下版本的里程计的准确性：我们的方法没有动态去除和没有边界框一致性约束（a），我们的方法没有动态去除（b），我们的方法没有边界框一致性约束（c），我们用 EKF 代替 UKF 的方法 (d)，以及我们的完整方法 (e)。另外，根据[38]中提到的方法，我们使用RANSAC进行全局分割来拟合地面并施加地面约束。我们还比较了不同下采样大小（0.4m，0.5m）下我们的姿势一致性约束与传统地面分割方法的效果。

1）跟踪器：如表III所示，基于 UKF 的目标跟踪器的 LO 系统优于基于 EKF 的目标跟踪器，受益于更强大的动态跟踪。理论上，EKF通过使用线性逼近来解决非线性状态估计，这可能会引入跟踪偏差。

2）动态去除：如表III所示，结果表明，通过添加动态去除步骤，可以有效过滤环境中的异常值，从而获得更准确的里程估计。在处理高度动态的 UrbanLoco 数据集时，这种改进尤其明显。

3）边界框一致性约束：结果如图5和表IV所示。通过施加边界框一致性约束，有效抑制了里程计的 z 轴漂移。结合动态去除和边界框一致性约束，我们的完整系统可以获得更准确的结果。 KITTI07数据集和UrbanLoco05数据集的翻译准确率分别提高了39.3%和30.4%。如图5和表IV所示，所提出的带有边界框约束的方法以更少的时间成本实现了更高的定位精度。效率高的原因是边界框约束解放了地面分割的步骤，这是次要贡献之一。

图 5. 消融研究中所有方法的 z 轴漂移。通过施加边界框一致性约束，抑制了z轴漂移并提高了里程计精度。

D. 运行时间分析

如表 V 所示，我们的方法的时间消耗比 LIMOT [18] 快两倍，比 LIO-SEGMOT [17] 快五倍。与基线相比，所提出的基于 UKF 的 3D 多目标跟踪器需要更少的时间来保持动态去除的相似精度。除了高效率之外，我们的里程计模块还表现出具有竞争力的定位精度。综上所述，我们的系统可以运行在20Hz以上，满足实时操作。

五、结论

这项工作提出了 TRLO，一种 LiDAR 里程计，对于长期遍历的动态环境来说是准确且稳健的。我们的关键创新之一是设计了一种基于无迹卡尔曼滤波器和最近邻法的 3D 多目标跟踪器，它可以稳定地识别和删除动态目标，为两级迭代最近点激光雷达里程计提供静态扫描。此外，还提出了一种有效的基于散列的关键帧管理来快速构建子图。最后，我们重用 3D 边界框来进一步优化计算的姿势，获得全局一致的姿势和干净的地图。我们在各种具有挑战性的动态环境中进行的基准测试证实了我们方法的可靠性。通过消融研究中，我们系统地探讨了每个组成部分的贡献。未来，我们有兴趣将 IMU 集成到我们的系统中或添加闭环以实现更稳健的姿态估计。