基于标签一致性检测的动态驾驶场景下激光雷达-惯性里程计

Zikang Yuan¹, Xiaoxiang Wang², Jingying Wu², Junda Cheng² and Xin Yang^2∗ ¹Zikang Yuan is with Institute of Artificial Intelligence, Huazhong University of Science and Technology, Wuhan, 430074, China. (E-mail: yzk2020@hust.edu.cn)²Xiaoxiang Wang, Jingying Wu, Junda Cheng and Xin Yang^∗ are with the Electronic Information and Communications, Huazhong University of Science and Technology, Wuhan, 430074, China. (* represents the corresponding author. E-mail: m202272556@hust.edu.cn; m202177065@hust.edu.cn; jundacheng@hust.edu.cn; xinyang2014@hust.edu.cn)

摘要

本文提出了一种消除动态驾驶场景中运动物体影响的激光雷达-惯性里程计 (LIO) 方法。该方法为当前扫描的 3D 点构建二值化标签，并利用每个点与其在 map 中周围点之间的标签差异来识别运动物体。首先，使用地面分割将二值化标签（即地面和非地面）分配给当前扫描中的每个 3D 点。在实际驾驶场景中，动态物体总是位于地面上。对于从运动物体扫描的大多数点，它们无法与空间中的任何现有结构重合。对于一小部分靠近地面的运动物体点，它们的标签与周围地面点存在差异。因此，运动物体上的点由于在 map 中缺乏最近邻或与周围地面点的标签不一致而被识别出来。使用基于体素位置的最近邻搜索对来自全局 map 的最近邻进行定位，并通过将标签一致性与最近邻进行比较来评估一致性，而无需进行任何大量计算。最后，运动物体上的点被移除。该方法被嵌入到一个自主研发的 LIO 系统（即 Dynamic-LIO）中，使用六个公共数据集进行评估，并在动态和静态环境中进行测试。实验结果表明，我们的方法可以以极低的计算开销 (即 1 $\sim$ 9ms/sweep) 来识别运动物体，并且我们的 Dynamic-LIO 可以在静态和动态场景中实现最先进的姿态估计精度。我们已发布了这项工作的源代码，供社区开发使用。

关键词：

SLAM，定位，传感器融合。

我简介

近年来，基于 3D 光探测和测距 (LiDAR) 的状态估计方法，包括 LiDAR-惯性里程计 (LIO)，由于 LiDAR 在感知 3D 空间方面的强大优势，在自动驾驶领域发挥了重要作用。这些方法可以实时提供车辆平台的 15 个自由度 (DOF) 状态估计，并恢复周围环境的 3D 结构。然而，它们应用的场景严格受到静态环境假设的限制。在实际驾驶场景中，移动的车辆或行人会在地图上留下鬼影轨迹（如图 1 (a) 所示），导致状态估计的累积误差，并为障碍物回避提供错误的观测信息。鉴于 LIO 系统中实时状态估计和映射的必要性，移除动态对象的计算成本必须在单次扫描的处理时间预算内。因此，从 3D 点云中有效地识别移动物体至关重要。

Refer to caption — 图 1: (a) 包含动态点的示例点云地图的说明，其中绿色点是移动车辆的鬼影轨迹。 (b) 静态点云地图，其中动态点已被我们的标签一致性检测方法检测到并移除。

为了解决由鬼影轨迹引起的映射重建不良和状态估计不准确的问题，研究人员采用了一系列方法，例如点相关性、可见性、占用概率和语义信息来识别和移除输入 LiDAR 扫描中的动态 3D 点。对于点相关性 [8]，静态点之间存在相关性，但动态点和静态点之间不存在相关性。地图点的连通性可以用来将移动物体与静态环境分离。然而，相关性图的构建和维护涉及计算批处理 3D 点的成对欧几里得距离，整个过程需要大量的计算资源。对于可见性 [18, 39, 10]，重新投影图像平面上的静态像素在多个扫描中保持不变，而动态像素会发生位移。然而，重新投影图像平面的生成需要一个计算过程，将大量的 LiDAR 点从其原始的 3D 空间映射到 2D 图像平面。对于占用概率 [28, 38]，对应于静态环境的体素的占用状态随时间保持不变，而被动态物体占用的体素的占用状态会随着时间而变化。然而，估计占用栅格地图需要组合多个最近的静态子地图，并统计每个子地图中每个体素的占用状态。上述三种方法利用大量的定量几何计算和全局统计来识别 3D 点云中的移动物体，并且它们会产生高昂的计算成本，这限制了它们与 LIO 系统的集成。深度学习方法 [7, 21] 使用学习到的语义信息快速地定性地分离动态物体。但是，它们依赖于大量的标记数据，在没有标记的类别下容易出错，并且需要强大的图形处理单元 (GPU) 来进行实时操作，这会占用后续任务（如轨迹规划）所需的资源。

与现有的涉及大量定量几何计算、全局统计或先验语义信息来对移动物体和静态环境进行分类的方法相比，在本研究中，使用基于与最近邻居标签差异的定性识别标准来识别移动物体上的 3D 点。由于移动车辆和行人都在地面上，因此潜在的动态点仅存在于非地面空间中，而移动物体的底座则紧邻地面。基于这一特点，提出了一种标签一致性检测方法，该方法可以在没有任何先验语义信息的情况下快速识别移动物体，从而对驾驶场景中的移动物体和静态环境进行分类。所提出的标签一致性检测方法的核心思想包括两个部分。首先，利用快速二维连通分量 [14] 将当前扫描的 3D 点划分为二值化标签，即地面点和非地面点。然后，通过与最近邻居的标签一致性进行比较来确定动态点，最近邻居通过基于体素位置的最近邻居搜索方法直接定位。整个动态点识别过程，包括基于体素位置的最近邻居搜索和标签一致性比较，都没有任何关于几何计算或全局统计的操作。这一特点确保了所提出的方法具有极低计算开销的优势（即 1 $\sim$ 9ms/扫描）。最后，提出了一种使用该标签一致性检测方法的自研 LIO 系统（即 Dynamic-LIO），用于消除驾驶场景中移动物体的影响。为了保证最终重建的地图仅包含静态点，任何检测到的动态点都将被排除在全局地图之外（如图 1 (b) 所示）。随后，干净的静态全局地图可用于对下一扫描执行标签一致性检测，从而确保所提出方法的可持续性。

在三个公共动态环境数据集上进行的实验结果，即 $s e m a n t i c$ - $k i t t i$ [1]、 $u l h k$ - $C A$ [32] 和 $u r b a n$ - $N a v$ ，表明我们的方法在保留率 (PR)、拒绝率 (RR) 和绝对轨迹误差 (ATE) 方面取得了可比的性能，并且在极低的计算开销 (1 $\sim$ 9ms/扫描) 方面显著优于在线动态点检测方法 [10, 38, 25, 33]。此外，在三个公共静态环境数据集上进行的实验结果，即 $n c l t$ [3]、 $u t b m$ [37] 和 $u l h k$ - $H K$ [32]，表明我们的 Dynamic-LIO 在静态场景中也优于最先进的 LIO 系统，因为其 ATE 更小。

总之，这项工作的主要贡献包括三个方面：1) 我们提出了一种标签一致性检测方法，用于快速识别 3D 动态点。该方法避开了大量几何计算或全局统计的操作，从而实现了轻量级；2) 我们开发了一个 LIO 系统，并将提出的标签一致性检测方法以统一的方式集成到这个 LIO 中，通过消除重建地图中的幻影轨迹，提高了动态场景中位姿估计的精度；3) 我们已经发布了我们方法的源代码，以促进社区的发展¹¹1https://github.com/ZikangYuan/dynamic_lio。

本文的其余部分结构如下。第 II 节回顾了相关文献。第 III 节提供了预备知识。第 IV 节详细介绍了我们的标签一致性检测方法，第 V 节介绍了我们的系统 Dynamic-LIO，然后是第 VI 节的实验评估。第 VII 节总结了本文。

II 相关工作

在本节中，我们回顾了关于现有无深度学习的 3D 动态点检测方法 [24, 8, 39, 18, 15, 27, 20, 5, 28, 38] 和当前主流针对静态和动态场景的 LIO 系统 [30, 26, 36, 12, 4, 6, 42, 41, 23, 25, 33, 34, 44] 的相关工作。尽管有一些基于深度学习的 3D 动态点检测方法 [7, 21]，但它们与这项工作关系不大，因此我们省略了对它们的详细讨论。

Pomerleau 等人 al. [24] 利用点的运动模式来表示相关性。他们计算了每个 3D 点的运动模式并推断出地图中主要的运动模式，然后确定不符合运动模式的点作为动态点。 Dai 等。人。 [8] 利用两点之间的相对位置来表示相关性，然后利用相对位置随时间的变化幅度作为一致性标准来识别动态点。然而，在大型室外场景中计算运动模式和维护地图点相关性的计算开销是无法接受的。 Yoon 等。人。 [39] 提出简单地将一次扫描与另一次扫描进行查询，并识别具有明显可见性差异的点作为动态点。 Removert [18] 提出了一种基于多分辨率范围图像的错误预测恢复算法。该方法首先保守地保留确定的静态点，然后通过扩大查询到地图关联窗口的大小来迭代地恢复更多不确定的静态点。然而，基于可见性的方法通常会遇到入射角模糊和遮挡问题。此外，在基于可见性的方法中，在球面图像平面上生成多个投影以及为可见性方法中的每个点分配一个静态值都需要很高的计算开销。 OctoMap [15] 首次提出了一种框架来生成体积 3D 环境模型，该框架基于八叉树并使用概率占用估计。给定一组已配准的 3D 点，Schauer 等。人。 [27] 建立了一个规则的体素占用网格，然后沿着传感器和测量点之间的视线遍历它，以识别多个扫描之间体素占用的差异。 Erasor [20] 提出了称为伪占用的概念来表达单位空间的占用，然后区分不同占用的空间。然后，采用区域地平面拟合 (R-GPF) 方法来区分可能包含动态点的候选 bin 内的静态点和动态点。 DORF [5] 提出了一种新颖的从粗到细的离线框架，该框架利用全局 4D 空间时间 LiDAR 信息来实现干净的静态点云地图生成。 DORF 首先利用后退视界采样 (RHS) 机制保守地保留了确定的静态点，然后在城市环境中动态对象的固有特征的引导下，逐渐恢复了更多模糊的静态点。 [28] 提议通过在在线机器人操作期间对传感、状态估计和映射限制进行建模和解释来增量估计高置信度自由空间区域。它可以在复杂无结构环境中实现鲁棒的移动物体检测。 RH-Map [38] 提出了一个基于 3D 区域级哈希地图结构的新型地图构建框架，该框架采用两层 3D 区域级哈希地图结构和区域级地面平面估计来移除动态物体。基于占用地图的方法通常伴随着最近邻搜索、置信度计算、占用概率统计、相对空间位置计算以及其他需要批量几何计算的操作，这会导致巨大的计算负担。

近年来，机器人社区中提出了各种 LIO 系统。 LIO-SAM [30] 首次将 LIO 形式化为因子图，允许将来自不同来源的循环闭合等多种相对和绝对测量值作为因子合并到系统中。在 LIN [26] 中，在误差状态迭代卡尔曼滤波器 (ESIKF) 框架内完成了 6 轴 IMU 和 3D LiDAR 的开创性集成。这种设计确保了系统的计算需求保持可处理。基于数学基础，Fast-LIO [36] 采用了一种解决卡尔曼增益 [31] 的技术，绕过了对高阶矩阵求逆的需要，从而显着减轻了计算负担。在 Fast-LIO 的进步基础上，Fast-LIO2 [35] 引入了一种创新的 ikd 树算法 [2]。与传统的 kd 树相比，该算法在树构建、遍历和元素删除等过程中提供了更少的时耗。 Point-LIO [12] 提出了一个点对点 LIO 框架，该框架在每个 LiDAR 点测量时更新状态，从而允许极高的输出频率。 DLIO [4] 提议在状态预测和点畸变校准领域内保留三阶最小值，从而促进更精确的姿态估计的获取。 IG-LIO [6] 将广义 ICP (GICP) 约束和惯性约束整合到一个统一的估计框架中。此外，iG-LIO 采用基于体素的表面协方差估计器来估计扫描的表面协方差，并利用增量体素地图来表示周围环境的概率模型。半弹性-LIO [42] 提出了一种基于半弹性优化的 LiDAR-惯性状态估计方法，该方法赋予状态足够的弹性，使其能够优化到正确的值。 SR-LIO [41] 采用了扫描重建方法 [43, 40]，该方法对来自旋转 LiDAR 的原始输入扫描进行分割和重建，以获得更高频率的重建扫描。因此，估计姿态的频率也随之提高。 Pfreundschuh 等。人员 [23] 提出了一种端到端的基于占用网格的管道，该管道可以自动标记各种任意动态物体，并将该网络嵌入到 LiDAR 里程计系统中。 RF-LIO [25] 利用自适应多分辨率范围图像来首先去除动态物体，然后将 LiDAR 扫描与地图进行匹配以进行状态估计。 ID-LIO [33] 提出了一种基于索引点和延迟移除策略的 LiDAR-惯性里程计，用于动态场景，该方法建立在 LIO-SAM 基础上。尽管 RF-LIO 和 ID-LIO 能够在动态场景中执行状态估计，但巨大的计算开销使其无法稳定地实时运行。

III 预备知识

III-A 坐标系

我们将 $(\cdot)^{w}$ ， $(\cdot)^{l}$ 和 $(\cdot)^{b}$ 表示为世界坐标系、LiDAR 坐标系和 IMU 坐标系中的一个 3D 点。世界坐标系与 $(\cdot)^{b}$ 在起始位置重合。

我们将在 $t_{i}$ 时间进行 $i_{th}$ 扫描的LiDAR坐标表示为 $l_{i}$ ，将 $t_{i}$ 处对应的IMU坐标表示为 $b_{i}$ ，那么从 $l_{i}$ 到 $b_{i}$ 的变换矩阵（即外部参数）记为 $\mathbf{T}_{l_{i}}^{b_{i}}\in SE(3)$ ，它由旋转矩阵 $\mathbf{R}_{l_{i}}^{b_{i}}\in SO(3)$ 和平移向量 $\mathbf{t}_{l_{i}}^{b_{i}}\in\mathbb{R}^{3}$ 。外部参数通常在离线校准一次，并在在线姿态估计过程中保持不变。因此，为了简单起见，我们可以将 $\mathbf{T}_{l_{i}}^{b_{i}}$ 表示为 $\mathbf{T}_{l}^{b}$ 。从 IMU 坐标 $(\cdot)^{b_{i}}$ 到世界坐标 $(\cdot)^{w}$ 的姿态严格定义为 $\mathbf{T}_{b_{i}}^{w}$ 。

III-B 体素地图管理

整个系统维护两个全局地图：跟踪地图和输出地图。前者用于状态估计，而后者用于标签一致性检测，并作为最终的重建结果。在 Dynamic-LIO 中，跟踪地图已经去除了绝大多数的动态点。但是，为了防止过度过滤导致 LIO 几何信息不足，我们避免对跟踪地图进行进一步处理，而是专注于输出地图（如 Sec. IV-E 所示）。因此，与跟踪地图相比，输出地图中的动态点被更彻底地移除。跟踪地图和输出地图都由体素管理，体素分辨率为 $1.0\times 1.0\times 1.0$ （单位：m），每个体素最多包含 20 个点。

IV 标签一致性检测

标签一致性检测旨在规避批次几何计算和全局统计，这些计算和统计在现有的动态点检测方法中很普遍，从而促进快速识别 3D 动态点。据我们所知，大多数现有方法主要涉及在最近邻搜索和一致性评估方面进行批次几何计算和全局统计。因此，我们致力于在我们的方法中实现这两个方面的轻量级。

标签一致性检测的核心前提是，驾驶场景中的移动物体与地面接触。在此前提下，我们首先通过从当前输入扫描中分割地面点来构建每个 3D 点的二值化标签（即地面标签和非地面标签）（如图 2 (a) 所示）。所有地面点本质上都是静止的，潜在的动态点仅存在于非地面点中。如果我们已经准备好了前一刻的静态全局地图，除了要添加的距离更远的点之外，当前时刻的每个静态点都可以在地图更新期间在全局地图中找到其对应的最近邻。对于从移动物体扫描的 LiDAR 点，全局地图中缺乏结构信息会阻止当前位置与空间中任何现有的静态几何结构重合。（如图 2 (b-1) 中的绿色区域所示）。因此，大多数从移动物体扫描的 LiDAR 点在配准过程中通常无法找到最近邻，我们将这些点识别为动态点（如图 2 (b-2) 中的绿色点所示）。至于剩下的 LiDAR 点的小子集（如图 2 (b-2) 中的粉色点所示），它们可能会找到地面点作为其最近邻。然后，我们根据最近邻中地面点的比例来确定是否将它们分类为动态点。很明显，在评估标签一致性的整个过程中，我们只需要计算最近邻中地面点的比例，而无需进行任何批次几何计算和全局统计。此外，我们利用基于体素位置的最近邻搜索来获取最近邻，这些最近邻可以直接定位，无需进行任何定量几何距离计算。这两个核心方面的轻量级确保了我们方法的低计算开销。一旦识别出当前时刻的动态点，我们利用 LIO 估计的姿态将静态点注册到全局地图中，以完成地图更新，这可以用于识别下一扫描的动态点，从而确保我们方法的可持续性。

特别地，标签一致性检测方法分为五个步骤：二值化标签构建、背景分离、基于体素位置的最近邻搜索、动态点确定和未确定点重新确定。下面，我们将对每个步骤进行详细描述。

IV-A 二值化标签构建

在动态点识别之前，我们首先为当前扫面的每个 3D 点构建二值化描述符，即地面标签和非地面标签。我们利用一种快速 2D 连通分量方法 [14]，与 LeGO-LOAM [29] 相同，以极低的计算成本将地面点与当前输入扫面分离。默认情况下，LiDAR 水平安装在车辆上，LiDAR 的 $z$ 轴垂直于地面。如果 LiDAR 以倾斜角度安装，则可以引入外部参数以确保 $z$ 轴保持垂直。将 $z$ 轴改为垂直方向后，我们将范围图像 $i m g$ [22]配置为以LiDAR线数（ $N$ $s c a n$ ，如图3（a）所示）作为纵轴，以水平分辨率（ $h o r i z o n$ $s c a n$ ，如图3（a）所示）作为横轴，并将当前扫描的3D点根据其水平和垂直线索引放置在范围图像中的相应位置。距离图像中，每个点在 $(\cdot)^{l}$ 中配备有 $x$ 、 $y$ 、 $z$ 坐标，以及一个布尔变量 $t a g$ ，用于表示该点是否为地面点。最初，所有 $t a g$ 都设置为false。我们将点 $img(1,1)$ 处的 $t a g$ 设置为 $t r u e$ 。然后，沿着图3（a）中所示的红色箭头，我们分别计算相邻位置 $img(1,2)$ 和 $img(2,1)$ 处两个3D点的俯仰角，相邻位置指的是 $img(1,1)$ 处。然后， $img(2,1)$ 到 $img(1,1)$ 的俯仰角可以计算为：

\begin{gathered}diff_{x}=img(2,1).x-img(1,1).x\\ diff_{y}=img(2,1).y-img(1,1).y\\ diff_{z}=img(2,1).z-img(1,1).z\\ pitch=arctan\left(diff_{z},sqrt\left(diff_{x}{}^{2}+diff_{y}{}^{2}\right)% \right)\end{gathered}

(1)

如果计算出的俯仰角小于某个阈值（例如，在我们系统中为5度），则 $img(2,1).tag$ 设置为 $t r u e$ ，这意味着相应的3D点被确定为地面点。同样，我们可以计算 $img(1,2)$ 到 $img(1,1)$ 的俯仰角，并为 $img(1,2).tag$ 设置值。如果位于 $img(1,2)$ 或 $img(2,1)$ 的3D点被确定为地面点，则我们沿着图3（a）中所示的蓝色箭头来识别与它们相邻的其他点。整个过程递归执行，直到范围图像中的所有像素都被访问或递归退出条件满足。分割后的地面点的可视化结果如图3（b）所示，其中橙色点被标记为“地面点”，白色点被标记为“非地面点”。

IV-B 背景分离

在执行标签一致性检测的过程中，需要找到当前扫描的每个点的最近邻点。靠近车辆平台的点可以可靠地找到它们的最近邻点，而更远的点则可能由于其位置重建不完整而无法找到。在室外驾驶场景中（排除极端遮挡），车辆平台周围30米半径内的地图结构通常已经重建完成。因此，我们设定了 30 米的经验阈值，并将距离车辆平台 30 米以内的点定义为前点，而超出 30 米的点定义为后点。对于前点和后点，我们采用了专门针对其特征的确定方法。

IV-C 基于体素位置的最近邻搜索

对于具有“非地面点”标签的特定点 $\mathbf{p}^{w}$ ，为了确定其标签是否与全局地图中周围点的标签一致，我们首先需要搜索 $\mathbf{p}^{w}$ 的最近邻。一个直观的替代方案是使用 8 近邻搜索，它与 LIO 中用于点到平面距离计算的最近邻搜索方法相同（如图 4 (a) 所示）。特别地，我们定位了 $\mathbf{p}^{w}$ 所属的体素 $V$ 以及与 $V$ 相邻的 8 个体素，并将这些体素中的所有点设为候选点。随后，通过比较到 $\mathbf{p}^{w}$ 的欧几里德距离的大小，从 9 个候选体素中识别出 $\mathbf{p}^{w}$ 的 20 个最近邻点。

然而，计算每个候选点到 $\mathbf{p}^{w}$ 的欧几里德距离是一个非常耗时的过程。当 LIO 建立点到平面距离残差时，为了确保拟合平面能够尽可能多地反映 $\mathbf{p}^{w}$ 周围的几何信息，我们必须使用传统的 8 近邻搜索。幸运的是，我们的系统只需要 600 个点到平面距离残差来估计每次扫描的位姿，因此总的计算开销是可以接受的。然而，为了确保最终输出地图不包含动态点，有必要确定当前扫描的每个点，这需要从全局地图中搜索每个点的最近邻。一次 32 线 LiDAR 扫描可以产生超过 50,000 个点，这使得传统的 8 个最近邻搜索在此不可行。

在提出的标签一致性检测方法中，我们通过比较非地面点 $\mathbf{p}^{w}$ 的标签与其周围点的标签来定性地评估其是否是动态点。由于周围点不参与定量计算，因此不需要严格满足最近邻的概念。相反，可以采用一种近似方法，即基于体素位置的最近邻搜索，以显著降低计算成本。如图 4 (b) 所示，我们定位 $\mathbf{p}^{w}$ 所属的体素 $V$ ，并将 $V$ 内的其他点视为近似最近邻。由于体素地图具有计算复杂度为 $O(1)$ 的查询操作，因此整个最近邻搜索过程非常快。此外，与欧几里德距离相关的计算开销也得到了节省。基于体素位置的最近邻搜索在确保标签一致性检测的低计算成本方面起着至关重要的作用，这从消融研究的结果中可以得到证明，这些结果记录在第 VI-G 节中。

IV-D 动态点确定

在第 IV-B 节中，我们将当前扫描中的点根据其与车辆平台的距离分为前点和后点。为了动态地确定前点和后点，我们采用了以下两种不同的模式。

前点模式。如果最近邻的数量低于某个阈值（在我们系统中为 5），则表明 $\mathbf{p}^{w}$ 的位置最初是空闲的，因此 $\mathbf{p}^{w}$ 被分类为动态点。如果最近邻的数量足够大（大于 5），我们计算所有最近邻中非地面点的比例。如果该比例足够低（小于 30 $\%$ ），则 $\mathbf{p}^{w}$ 被分类为静态点，并添加到跟踪地图和输出地图中。相反，如果该比例大于 30 $\%$ ，则 $\mathbf{p}^{w}$ 被分类为动态点，并从地图中排除。不可避免地，这种确定标准可能会导致错误地移除一些靠近地面的静态点。然而，最常受到这种误过滤影响的点位于墙壁和地面之间的过渡区域。尽管存在这种误过滤的可能性，但场景的整体几何完整性仍然保持不变，并且不会影响 LIO 系统的性能。前点的动态点确定结果可视化如图 5 所示。

后向点的模式。如果最近邻的数量低于某个阈值（在我们系统中为 5），我们无法将后向点识别为动态点，因为该位置可能尚未重建，从而无法获取最近邻。这些点被标记为未确定点，一旦车辆平台继续移动并恢复这些点位置的几何结构，就会做出判断。为了确保在状态估计过程中新获取的点云能够正确配准，有必要将未确定点纳入跟踪地图。这不会显着影响状态估计的准确性，因为即使后向点中存在动态物体，扫描到它们上的 LiDAR 点数量也非常稀疏。关于最终输出地图，必须确保它包含尽可能少的动态点，因此将随后进行对未确定点的判断。当最近邻的数量足够大（大于 5）时，处理方法与前向点相同，静态点将被添加到跟踪地图和输出地图中。后向点动态点判定结果的可视化如图 6 所示。

IV-E 未确定点重新判定

如第 IV-D 节所述，由于其位置的重建不完整，一些后向点可能无法找到最近邻。对于此类点，我们将其标记为未确定点，并将其放置在一个容器中。当车辆平台继续前进时，先前未重建位置的几何结构信息被恢复（如图 7 所示）。然后，我们可以对这些未确定点进行重新判定。当未确定点容器中的一个点 $\mathbf{p}_{u}^{w}$ 接近车辆平台的当前位置（小于 30 米）时， $\mathbf{p}_{u}^{w}$ 周围的几何结构信息很可能已被重建。然后，我们可以确定 $\mathbf{p}_{u}^{w}$ 是否为动态点。如果最近邻点的数量低于某个阈值（在我们系统中为 5），则表明 $\mathbf{p}_{u}^{w}$ 的位置最初是空的，导致 $\mathbf{p}_{u}^{w}$ 被归类为动态点。如果最近邻点的数量大于 5 的阈值，则我们计算所有最近邻点中非地面点的比例。如果此比例足够低（小于 30 $\%$ ），则将其分类为静态点并添加到输出地图中。相反，如果该比例不小于 30 $\%$ 的阈值，则将其分类为动态点，并且不会包含在输出地图中。如果一个未确定点在连续 10 次扫描中与车辆平台位置的距离超过 30 米，则它很可能是一个远处稀疏的背景点。因此，我们直接将其分类为静态点并将其添加到输出地图中。

V 我们的系统 Dynamic-LIO

V-A 系统概述

图 8 说明了我们自主研发的系统 Dynamic-LIO 的框架，该框架由四个主要模块组成：云处理、静态初始化、基于 ESIKF 的状态估计和动态点识别。云处理模块为当前扫描的每个 3D 点构建二值化标签（即地面标签或非地面标签）。随后，它执行空间下采样以确保当前点云的均匀密度。静态初始化模块 [11] 利用 IMU 测量结果来估计一些状态参数，例如重力加速度、加速度计偏差、陀螺仪偏差和初始速度。基于 ESIKF 的状态估计模块估计当前扫描的状态，并利用估计的姿态将当前扫描的所有点从 $(\cdot)^{l}$ 转换为 $(\cdot)^{w}$ 。动态点识别模块从当前输入点云数据中识别动态点，以确保全局地图只包含静态点。黄色的矩形表示整个系统框架中标签一致性检测五个步骤的具体位置。

V-B 下采样

为了减轻由 LiDAR 在一次扫描中收集的大量 3D 点带来的巨大计算量，我们在点云上实现了十进制策略。首先，应用均匀子采样方法，每组四个点保留一个点。随后，我们将均匀子采样的点整合到一个由 $0.5\times 0.5\times 0.5$ （单位：m）分辨率定义的体素网格中，确保每个体素只包含一个点。

值得注意的是，当前输入扫描的下采样必须在二值化标签构建之后进行。原因是下采样会破坏距离图像中 3D 点的邻接关系，这会影响二值化标签构建的执行。

V-C 静态初始化

在我们的 Dynamic-LIO 中，采用静态初始化过程来估计基本参数，包括初始姿态、初始速度、重力以及加速度计和陀螺仪测量中的偏差。有关方法的详细阐述，请参阅参考文献 [11]。

V-D 基于 ESIKF 的状态估计

我们采用误差状态迭代卡尔曼滤波器 (ESIKF) 来执行状态估计，这与 Fast-LIO2 [35] 相同。 Fast-LIO2 利用他们自己开发的工具箱 IKFoM [13] 来实现流形上的卡尔曼滤波，而我们则利用更广为人知的 Eigen3 库 [9] 来实现此功能。我们已经在附录中记录了 ESIKF 的整个执行过程，以便读者参考实现细节。

值得一提的是，在状态估计期间，我们需要从全局地图中找到当前扫描中随机选择的 600 个点的最近邻，并使用这些最近邻拟合平面来构建点到平面距离约束。为了确保最终拟合的平面尽可能准确地反映周围的几何信息，我们在这里仍然使用传统的 8 个最近邻搜索方法。此外，我们根据跟踪地图搜索最近邻。与输出地图相比，跟踪地图更有利于 LIO 的准确和稳健运行。因为跟踪地图保留了足够的几何细节，同时去除了绝大多数动态点。

VI 实验

我们在六个自动驾驶场景数据集上评估了我们方法的整体性能： $s e m a n t i c$ - $k i t t i$ [1]， $u l h k$ - $C A$ [32]， $u r b a n$ - $N a v$ [16]， $n c l t$ [3]， $u t b m$ [37] 和 $u l h k$ - $H K$ [32]。其中， $s e m a n t i c$ - $k i t t i$ ， $u l h k$ - $C A$ 和 $u r b a n$ - $N a v$ 是三个在动态场景中收集的公共数据集， $n c l t$ ， $u t b m$ 和 $u l h k$ - $H K$ 是三个在静态场景中收集的公共数据集。 $s e m a n t i c$ - $k i t t i$ 是由 64 线 Velodyne LiDAR 收集的，每个 LiDAR 点都有其独特的语义标签。因此， $s e m a n t i c$ - $k i t t i$ 用于评估所提出的基于标签一致性的动态点检测和去除方法的保留率 (PR) 和拒绝率 (RR)。 $u l h k$ - $C A$ 由 32 线 Robosense LiDAR 和 IMU 收集， $u r b a n$ - $N a v$ 由 32 线 Velodyne LiDAR 和 IMU 收集。这两个数据集用于评估动态点检测和去除方法在姿态估计方面的改进，改进指标为绝对轨迹误差 (ATE)。 $n c l t$ 、 $u t b m$ 和 $u l h k$ - $H K$ 均由 32 线 Velodyne LiDAR 和 IMU 收集。这三个数据集用于展示我们自主研发的 LIO 系统的出色性能，并证明所提出的动态点检测和去除方法对静态场景中 LIO 的精度没有负面影响。本节中使用的所有 24 个序列的详细信息，包括名称、持续时间和是否包含动态对象，如表 I 所示。所有实验均使用配备英特尔酷睿 i7-11700 和 32 GB 内存的消费级计算机。

表 I：所有评估序列的数据集

Name

Duration

(min:sec)

Whether Dynamic

Objects are Included

kitti\_1

semantic-kitti-00