【论文笔记】Integrate Point-Cloud Segmentation with 3D LiDAR Scan-Matching for Mobile Robot Localization a

【论文笔记】Integrate Point-Cloud Segmentation with 3D LiDAR Scan-Matching for Mobile Robot Localization and Mapping

    ~~~          ~~~~     定位和地图绘制是自主移动系统执行导航和交互任务的关键要求。迭代最近点 (ICP) 广泛应用于机器人社区中的 LiDAR 扫描匹配。另外，标准ICP算法在迭代搜索最近点时只考虑几何信息。然而，在动态环境和高速公路等具有挑战性的环境中，ICP 单独无法实现准确的点云配准性能。此外，搜索最近点的计算是ICP算法中一个昂贵的步骤，仅限于满足实时性要求，尤其是在处理大规模点云数据时。在本文中，我们提出了一种基于分段的扫描匹配框架，用于六自由度姿态估计和映射。激光雷达在扫描时会产生大量的地面点，但这些点很多都是无用的，增加了后续处理的负担。为了解决这个问题，我们首先应用基于图像的地面点提取方法来滤除噪声和地面点。去除地面点后的点云然后被分割成不相交的集合。在这一步之后，应用标准的点对点 ICP 来计算连续扫描之间的六自由度变换。此外，一旦在环境中检测到闭环，就会采用用于全局松弛的 6D 图优化算法（6D 同时定位和映射 (SLAM)）。基于公开可用的 KITTI 数据集的实验表明，与标准 ICP 方法及其变体相比，我们的方法需要更少的运行时间，同时实现了更高的姿态估计精度。
　　在这项工作中，我们提出了一个基于片段的扫描匹配框架，用于六自由度姿态估计和映射。本文有四个贡献。
　　首先，引入了基于二维图像的地面点提取方法作为ICP匹配的预处理步骤。 LiDAR 在扫描周围场景的同时获取大量 3D 点，其中包含许多地面点。最近点的计算是标准ICP算法中的一个昂贵的步骤，不满足实时性要求。平坦道路上的地面点几乎不包含几何信息，而标准的点对点ICP算法仅考虑点对点欧几里得距离来搜索最近点。因此，这些接地点会导致较大的对应点误差。此外，地面点提取也是点云分割的关键步骤。
　　其次，去除地面点后的点云被分成许多簇。通过聚类，去除了一些没有共同属性的异常值。在这一步之后，这些不同的簇被合并成一个新的点云。与原始点云相比，新点云的地面点被去除，一些虚假地面点和噪声点也被过滤掉。这将大大提高ICP匹配的效率和准确性。
　　第三，我们通过结合分割算法扩展了 6D SLAM 的工作，相对于标准 6D SLAM 提高了姿态估计的准确性和效率。在此基础上，提出了基于城市、乡村甚至高速公路的系统评估，同时具有绝对和相对误差指标。结果验证了去除地面点确实可以提高ICP和6D SLAM的姿态估计精度。它还表明，相对于原始点云，6D SLAM 在没有地面点的点云的姿态优化方面表现更好。

系统概述

    ~~~          ~~~~     系统架构如图1所示，可分为点缩减、点云投影、地面点去除、分割、ICP和位姿图优化（6D SLAM）六个主要模块。我们首先应用基于八叉树的数据结构来减少 3D 点云。然后介绍了一种基于图像的地面点去除方法。去除地面点后的点云被进一步分割成不相交的集合。在这一步之后，应用标准的点对点 ICP 来计算连续扫描之间的六自由度变换。此外，一旦在环境中检测到闭环，就会采用全局松弛的 6D 图优化算法。我们的系统采用右手坐标系，z 轴朝上，x 轴朝前。各个模块的详细算法原理将在后面的章节中介绍。

点云缩减

    ~~~          ~~~~     激光雷达的高分辨率在扫描时获取大规模数据。例如，Velodyne HDL-64E 每秒可以生成 180 万个范围测量值。因此，要高效处理海量 3D 数据点，点云存储和缩减是至关重要的步骤。八叉树是一种用于描述三维空间的空间数据结构，可实现3D点云的高效存储、压缩和搜索。如图 2 所示，假设 3D 空间是一个立方体，根节点表示一个立方边界框，它存储点云的所有点，即 3D 坐标和反射率等附加属性。八叉树将空间分成8个部分，每个节点就是一个部分。八个子节点所代表的体积之和等于父节点的体积。在这项工作中，我们使用了一种基于八叉树的点云减少方法，在 [33] 中有详细描述。

投影到 2D 范围图像

    ~~~          ~~~~     由于后续的地面点去除和分割算法都是基于二维距离图像，我们首先需要获得柱面距离图像。 Velodyne系列等广泛使用的LiDAR通过水平和垂直扫描获取环境信息。例如，16 通道 VLP-16 的水平视场为 360 度，垂直视场为 30(±15) 度。如果将水平方位角 qh 设置为 0.2°并且我们从数据表中知道垂直分辨率 qv 为 2°，则二维距离图像的相应分辨率为 1800 x 16。给定一个点 P = (x, y, z) ，相应的 2D 距离图像由下式计算：

其中 h 和 v 是 LiDAR 坐标系中 P 的水平和垂直角度，参见。图 3. vb 表示 LiDAR 的最大垂直视场。对于 VLP-16，vb = 15。另外，hs = 1800 指的是水平分辨率，而 bc 表示相应的数字向下取整。通过投影，每个 3D 坐标点 P 由图像的唯一像素 I(r, c) 表示。对于二维距离图像，距离图像的每一行具有相同的垂直角 qv，即相同的扫描线。每列表示具有相同水平角度和不同扫描线的点。

地面点移除

    ~~~          ~~~~     地面点提取是点云处理的关键步骤。在这一部分，我们采用了一种类似于[34]的基于图像的地面点提取方法。刘等人。 [13]使用等式（2）提取地面点是基于一种直观的理解，即同一列的两个相邻点之间的z方向差异远小于x和y方向，当LiDAR扫描地面时.然而，这个假设只适用于地面车辆。对于无人机等 3D 移动机​​器人，必须考虑传感器相对于地面的姿态。此外，该算法从图像底部开始遍历 m 行的点。如果ai小于阈值q，则该对应点被认为是地面点。但是，用户必须根据 LiDAR 的安装高度设置不同的 m 值和阈值 q。

其中 dc x,i, dyc,i, dzc,i 表示第 c 列中两个相邻点之间在 x、y 和 z 方向上的差异。因此，在这项工作中，我们引入了一种更健壮和有效的方法。算法 1 描述了我们用来提取地面点的算法。首先，基于等式（2）（第 2 行）将 2D 距离图像转换为角度图像。转换后，角度图像的每个像素点由对应的ai表示。接下来，将 Savitzky-Golay 过滤算法 [35] 应用于角度图像（第 3 行）。这旨在平滑数据并去除噪声。在这一步之后，我们从过滤后的图像的左下角开始遍历每个像素。每当遇到未标记的像素时，就会执行基于该像素的广度优先搜索 (BFS)（第 7-15 行）。基本思想是BFS从像素开始，从上、下、左、右像素中找到4个邻域。如果像素与其 4 个邻域之间的差值落在阈值 g 内，则将该像素添加到队列中，即，将其分配给地面点（第 12-15 行）。请注意 Label=1 指的是地面点类。此过程停止，因为整个连接的组件都收到相同的标签。直观地，该算法从图像的左下角开始，通常被认为是地面点。我们为此点分配一个标签（第 11 行）。然后使用 BFS 不断扩展搜索，直到找到所有属于同一标签 (Label=1) 的点。该算法遍历整个图像的所有点，因此我们不必为不同的硬件平台手动选择 m。

分割

    ~~~          ~~~~     为了进一步去除噪声点和异常值，我们使用[34]中的算法在去除地面点后分割距离图像。该算法的思想类似于地面点提取。判断点是否属于同一个标签的方法如图4所示。 如图4右图所示，如果设置了合适的阈值e，可以使用b对点云进行分割。我们假设OA和OB之间距离比较远的一个是d1（kOAk），另一个是d2（kOBk），那么，计算b：

其中 q 是第 3.3 节中描述的水平方位角或垂直分辨率。该算法的伪代码如算法2所示。该算法与算法1的不同之处在于输入图像、分类标准和标签数量。 Rng 表示由点云直接投影但不包括地面点的图像。由于地面点是一个类别，算法 1 只有一个标签。但是，细分包括许多类别。因此，当一个集群完成时，标签会自动增加 1。

请注意，在实现分割算法后，可以很容易地将分组为许多子图像的 2D 图像转换为由 3D 坐标点表示的子片段。我们的目标是使用分割算法去除噪声和异常值。因此，这些不同的集群然后合并成一个新的点云。与原始点云相比，新点云的地面点被去除，一些噪声和异常点也被过滤掉。最后，然后应用标准的点对点 ICP 算法来计算连续扫描之间的六自由度变换。具体的计算过程将在下一节介绍。

ICP and 6D SLAM

在这部分中，使用点对点ICP和全局一致的扫描匹配算法来计算六自由度位姿。此外，我们还将我们的结果与标准的点对平面 ICP 方法和基于边界框滤波器的点对点 ICP 进行了比较，后者首先通过边界框滤波器去除地面点，然后执行 ICP 算法。 ICP 的概念很简单：给定初始猜测，它迭代计算点对应关系。请注意，对基于 LiDAR 的里程计执行基于 ICP 的扫描匹配时，并不严格需要初始猜测。事实上，只要传感器动态相对于帧速率不是太快，就可以假设初始旋转和平移为零，就可以运行 ICP 算法。在每次迭代中，ICP 最小化一个距离函数，根据选择的最近点计算两点云之间的变换。点对点ICP的距离函数定义为：

其中 Nm 和 Nd 是源点云 S 和目标点云 D 中的点数。 Point-to-plane ICP 最小化源点与目标点切平面之间距离的平方和。这个具体的公式如下

其中N为点数，ni为目标点对应的法向量。 T 是源点和目标点之间的刚性变换。与点对点ICP相比，点对平面ICP计算的是点的切平面。因此，它可以在几何信息较少的环境中取得更好的效果。但是，它需要计算法向量，这会降低效率。因此，在这项工作中使用点对点 ICP。 ICP通过计算相邻两次扫描之间的位姿，然后不断更新，得到轨迹。然而，姿态估计在长期或大规模场景中会受到误差累积的影响。为了解决这个问题，一旦检测到闭环，将 ICP 的姿态估计结果输入到 6D SLAM 框架中，即全局一致的扫描匹配 [32]。它在 3DTK-3D 工具包 [36] 中可用。 6D SLAM 类似于点对点 ICP 方法，但考虑所有扫描而不是仅两个相邻扫描。它同时求解所有姿势并像原始 ICP 一样迭代。它实际上是一种姿态图优化方法，使用马氏距离来表示所有姿态的全局误差。具体公式为：

其中 j 和 k 表示对 SLAM 图的扫描，Ej,k 是线性化误差度量，(E¯ j,k, Cj,k) 是高斯分布。 X j 和 Xk 是图中的两个连接节点，代表相应的位姿。我们在这里只给出一个简短的概述，详细描述在 [32] 中给出。