【论文笔记】A Novel Global Localization Method Using 3D Laser Range Data in Large-Scale and Sparse Environ

【论文笔记】A Novel Global Localization Method Using 3D Laser Range Data in Large-Scale and Sparse Environments

基于粒子滤波框架的全局定位在没有任何先验知识的情况下仍然是一个具有挑战性的问题。类似场景下加速粒子收敛、提高收敛精度等问题需要解决。本文提出了一种新的全局定位方法，将全局定位问题视为位置识别和姿位姿估计问题。具体来说，首先利用随机森林算法训练分类器来预测两个 3D LiDAR 观测是否来自同一个地方。然后，分类器用于将当前观察结果与先验图进行匹配，以估计机器人可能的初始位姿。最后，提出了一种多假设粒子滤波算法来实现最终定位。实验结果表明，该算法具有更快的收敛速度和更高的精度。

传统的粒子滤波算法难以在大规模稀疏环境下高效解决全局定位问题。有以下两个问题：

    ~~~         ~~~     1.在稀疏环境中，3D LIDAR 传感器感知的稀缺环境信息很容易使算法在将当前帧与每个粒子的先验图匹配时陷入错误的姿态估计。
　　 2.在大规模环境中，需要大量粒子才能收敛到正确的位姿，这将导致更高的计算复杂度。 因此，我们需要考虑一种更快的方法来加速收敛。　
本文首先提出了一种 3D LiDAR 传感器数据的直方图表示，可以应用于一些非平坦道路和动态场景，同时没有计算速度的损失。 然后，我们利用随机森林算法进行地点识别，在此基础上，提出了一种可以避免快速陷入单个高似然姿态的多假设粒子滤波算法来实现最终定位。

本文提出的方法可分为以下三个模块：

    ~~~         ~~~     1.在学习模块中，使用基于随机森林的二元分类器来识别两个激光观察是否来自同一场景。
　　2.在建图模块中，先验图由LiDAR-SLAM生成的点云图、LiDAR-SLAM中间过程中固定距离生成的关键帧的直方图表示和关联位姿三部分组成。
　　3.在定位模块中，首先将第一个 3D LiDAR 帧转换为直方图表示，将其与先验地图中的所有表示成对，然后将这些对放入分类器中进行预测。被预测为正对的相关姿势是初始姿势假设。
　　最后，运用文中提出的多假设粒子滤波器算法，通过跟踪多个不同的姿势假设直到收敛到最终姿势来执行姿势估计。
　　

方法描述

1.直方图表示

    ~~~          ~~~~     来自 3D LiDAR 数据的帧 P 中的每个激光点都可以使用笛卡尔坐标中的 (x y z) 来描述，也可以使用球坐标中的 (r θ ϕ) 来描述。 考虑到 Velodyne VLP-16 等通用 3D LiDAR 传感器，参数θ在 -15 到 +15 度之间离散，垂直角分辨率为 2 度。 因此，帧P可以被划分为多个环。 我们将每个环表示为 $S_N^i$，其中 N 表示环的总数，i 是环的索引。 对于每个环，参数ϕ从 -180 度到 +180 度离散，具有水平角分辨率，其中每个点由 $p_k^i$ 表示，其中 i 是环的索引，k 是由ϕ确定的索引。
　　给定环 $S_N^i$ ，可以通过以下点投影函数计算激光点：

该函数具有旋转不变性，并且对滚转和俯仰方向的噪声表现出敏感性。 由于地面移动机器人将在地面上方保持相当恒定的高度，因此向上向量 u 可以固定为 $(0,0,1{)}^T$ 。距离间隔可以预先指定为 $I\in [f_{min},f_{max}]$ 。 然后，假设b表示区间的数量，我们可以将距离区间I划分为许多子区间，如下所示：

每个区间的距离是

对于环 $S_N^i$ 中的每个激光点 $p_k^i$，我们可以利用（1）将其转换为一维实数 $f_k^i$ 。 我们可以找到它属于哪个区间。 因此，环 $S_N^i$ 的直方图可以写成：

我们对所有环都做同样的事情，然后直方图从环的底部到顶部变平。 最后得到一个N×b 一维直方图。 以下是帧 P 的最终直方图表示：

上述方法表明直方图表示具有旋转不变性。 此外，当机器人的姿态有轻微干扰时，与基于距离的方法相比，所提出的点投影函数可以在一定程度上提高平移不变性[10]。 同时，当由于机器人周围的动态物体存在干扰时，它只影响几个环的直方图表示，大多数环与这些动力学解耦。

2. 随机森林学习

    ~~~          ~~~~     在学习模块中，我们利用随机森林 [14] 学习算法进行训练。 给定两个帧 Pk 和 Pk+1 ，可以通过前面的介绍得到对应的直方图表示，如下所示：

定义下面的函数

实验中采用2-范数，如下所示：

作为训练数据，提供了 n 个预先标记的激光对：

其中 yi 是二元变量 yi ∈ {0,1} 。 yi ∈ 0 表示两个激光对不是来自同一场景，yi ∈ 1 表示两个激光对来自同一场景。
随机森林算法使用集成学习思想来集成多个弱学习器。 它的基本学习器是决策树。 组合多个弱学习器通常可以在单个学习器上实现显着的泛化性能。具体步骤如图1所示。

3.全局定位实施

    ~~~          ~~~~     现在，让我们讨论所提出的全局定位算法的实现。 在本文中，映射模块不仅像 LOAM [15] 一样记录了我们的 LiDAR-SLAM 算法生成的点云图，而且还保存了在LiDAR-SLAM 的中间过程固定距离处生成的关键帧的直方图表示和关联位姿。 因此，先验图由以下三部分组成：

其中 M 表示点云图，{R} 表示关键帧的直方图表示集，{X} 表示关键帧的相关姿势集。

在定位模块中，假设 P1 表示 3D LiDAR 传感器的第一帧，R1 表示 P1 使用上述方法的直方图表示。让 R1 和来自 {R} 的关键帧的每个直方图表示成对，将它们输入到学习模块中描述的随机森林分类器中。如果预测结果为正，则说明两帧来自同一个场景，相关位姿为候选初始位姿。然后，我们可以通过使用 ICP [16]、NDT [17] 和其他算法进行姿态估计来获得最终的初始姿态。然而，这些方法由于初始值不好，容易陷入局部最小值，同时考虑到大规模环境中存在较大的相似性，随机森林分类器可能预测多个正激光对，这意味着有多个初始位姿假设。标准粒子滤波算法[1]可以通过在所有假设中散射大量粒子来解决这个问题。然而，当从后验概率采样时，它可能很快收敛到一个单一的、高可能性但错误的姿势。

因此，我们提出了一种受[8]启发的多假设粒子滤波算法。 它通过独立跟踪多个不同的初始姿势假设来执行姿势估计，直到收敛到最终姿势。 假设M是预测过程后初始姿态假设的数量，我们根据高斯分布在每个姿态假设初始化一个粒子集。 设 $n_j,j=1,...,M$ 表示第 j 个粒子集的个数，$B_{j,t}$ 表示第 j 个粒子集在时间 t 的权重，$w_{j,t}^i$ 表示第 j 个粒子集的第i个粒子在时间t时的权重。 它满足以下等式：
等式（11）表示在某时刻t,j个粒子集的权重和为1
等式（12）表示在某时刻，某个粒子集的粒子权重和为1

整个后验概率分布可以表示为：

其中 $x_t$ 表示时间 t 的姿态，$z_{1:t}$ 是时间 t 之前的激光观察，而 u_{1:t} 是时间 t 之前的控制输入。 p j ( x t ∣ z 1 : t , u 1 : t = { x j , t i , w j , t i } ) p_j(x_t|z_{1:t},u_{1:t}= \{x_{j,t}^i,w_{j,t}^i\}) pj​(xt​∣z1:t​,u1:t​={xj,ti​,wj,ti​}) 是第 j 个粒子集在时间 t 的后验概率分布，由 $n_j$ 个加权粒子分布近似。 粒子的权重可以计算如下：

${\eta }_{j,t}$ 表示归一化因子。 另一方面，根据贝叶斯准则，$p_j(x_t|z_{1:t},u_{1:t})$也可以表示为以下公式：

结合(13)和(15)，可以得到粒子权重的更新方程：

粒子集的有效数量用于表示粒子集权重的退化：

当该值小于一个阈值时，选择粒子集权重最大的位姿假设进行后续的位姿跟踪过程。

实验结果

如图 2 所示，移动机器人用于验证所提出的算法。 配备里程表、IMU、3D LIDAR（Velodyne VLP 16）。 VLP-16 可以高精度测量 150 m，2 cm，并且它具有 -15 到 +15 度的垂直视野。 它以 10 Hz 的帧速率和 300,000 点/秒的速度连续扫描 360 度周围环境。将从两个方面测试本文提出的算法：地点识别性能和全局定位性能。

1.地点识别性能

    ~~~          ~~~~     在训练随机森林分类器之前需要一个训练数据集。传统的手工标注方法相当繁琐和困难。所以我们更喜欢以更无监督的方式收集数据集。为了测试地点识别性能，我们在一个具有高动态特性的室内停车场收集了我们自己的数据集。由于缺乏地面实况，我们利用我们的 SLAM 算法生成偏差在 5 厘米以内的精确位置。由我们的 SLAM 算法构建的全局点云图如图 3 所示。蓝线表示长度约为 2 公里的行驶路径。考虑到 3D LiDAR 传感器的高频，我们可以将连续两次观测视为同一个地方。此外，我们故意控制移动机器人重新访问行驶路径中的同一位置以生成更多正样本。综上所述，在SLAM算法过程中产生的一系列激光数据和位姿中，我们可以通过以下方式判断两个激光观测是正样本还是负样本：


　　其中 P1 和 P2 是相关的姿势，dis 表示两个姿势之间的距离。 因此，我们可以简单地生成大量正负样本来训练随机森林分类器。 在这个数据集中，我们最终生成了大约 17900 个正样本和 21700 个负样本。
　　构建数据集后，我们将我们提出的算法与其他两种算法进行比较，它们是基于帧描述符的方法：小尺寸签名（S-SS）[11] 和快速直方图（FH）[10]。 对于这三种方法基于直方图表示，bucket(区间)设置为相同的值 b=50 。 请注意，我们提出的方法的最终尺寸与 3D LiDAR 传感器的环有关。 在实验中，我们使用具有 16 个环的 VLP-16 传感器。 为了衡量不同方法的性能，我们引入了分类精度和接收器操作特征 (ROC) 曲线。

1. 分类准确度：我们使用如下所示的错误率来衡量预测结果的准确度：
   
   2）ROC 曲线：它绘制了假阳性率 (FPR) 与真阳性率 (TPR) 的关系。称为 AUC 的曲线下面积越大，性能越好。图4（a）显示了三种算法的ROC曲线，d = 3m 。与其他两条 AUC (0.87, 0.80) 相比，黄色曲线的 AUC (0.95) 更大。绿色曲线代表随机猜测。在本文中，如果一对激光观测值的相关位姿的欧几里得距离小于阈值 d 的值，则将其视为正样本。不同的阈值 d 可能会影响算法的性能。因此，我们选择不同的 d 为 2m、3m、5m 和 10m，主要在 S-SS 和我们提出的算法上进行测试，如图 4（b）所示。显然，如果阈值 d 很大，我们将很难正确识别，因为很难描述两次激光观察之间的差异。但是，考虑到初始位姿可能与先验图中关键帧的位姿不太接近，因此阈值d不能设置得太小。我们在实验中设置了 d = 3m。
   　　在室内停车场中，有很多动态物体，如行人、车辆等，对场所识别问题影响较大。 然而，实验结果表明我们提出的算法具有更好的性能。 主要原因是在所提出方法的直方图表示中考虑了 3D LiDAR 传感器不同环的影响。 同时，我们利用随机森林算法作为一种监督学习方法来执行地点识别，其结果优于相似性度量函数。

2.全局定位性能

    ~~~          ~~~~     我们的全局定位框架基于粒子滤波算法。 因此，我们可以利用所有粒子的收敛状态来评估全局定位性能，其中包含三个指标：收敛成功率、收敛时间和姿态跟踪精度。 我们将我们提出的算法与标准粒子滤波器 (S-PF) 算法 [1] 和基于快速直方图 [10] 的粒子滤波器算法 (FH-PF) 进行比较。 我们特意选择了一个室内停车场和两个变电站的实验场景，如图5所示。在变电站A中，道路不平坦，有很多类似的场景，如图6所示。在变电站B中 如图7所示，用电设备分布更稀疏，场景相似度更高，面积也更大。
1）收敛成功率和收敛时间：这些指标评估全局定位算法对自身进行定位的性能。 给定移动机器人的起始姿势，我们控制机器人行驶固定路径和距离。 如果所有粒子的最终状态收敛到一个正确的位姿，这个位姿可以从当前观测与全局点云图的匹配状态得知，则表明移动机器人定位成功。 收敛成功率可以计算如下：

可以计算从开始姿势到最终收敛到正确姿势的收敛时间。

在每个场景中，选择了 100 个开始姿势。移动机器人以漫游方式行驶。由于S-PF算法需要粒子的全局散射，我们设置粒子数为40000，其他的粒子数为2000。平均收敛时间和成功率见表 2.
　　 显然，我们的算法与其他算法相比具有更高的收敛成功率，并且在收敛时间上与FH-PF算法具有几乎相同的性能。它再次验证了我们的算法在这三个场景中的性能更加稳健。由于我们在匹配过程中使用基于帧的描述符而不是所有激光点数据，因此它比 S PF 算法执行更快的收敛速度。大量相似的场景很容易导致收敛到错误的姿势。标签。图 2 显示 FH-PF 算法在两个变电站中具有大约 70% 的收敛成功率，但我们提出的算法有大约 20% 的改进。主要原因是我们提出的多假设粒子滤波算法独立跟踪每个假设，并最终收敛到一个姿态，直到移动机器人来到一个差异化的场景。
2) 姿态跟踪：移动机器人完成初始定位后，采用粒子滤波算法进行姿态跟踪。 机器人的定位误差ε表示如下：

其中 Pi 表示我们的全局定位算法的输出位姿。 $P{i}^{\ast }$表示真实姿势。 $||\cdot$是本文中使用的 2-范数。 对于机器人在 2D 环境中导航，$e_i$ 表示为$ (x_e y_e z_ e) $。 那么ε可以分为两部分：平移误差和旋转误差。

在图 5 所示的每个场景中，从机器人的轨迹中选择 20 个点来计算定位误差。 三个场景的定位误差结果见表3。显示平移误差在5厘米以下，旋转误差在2度以下。

总结

    ~~~          ~~~~     本文提出了一种新的全局定位方法，将全局定位问题视为位置识别和姿态估计问题。 具体来说，我们首先使用随机森林算法训练分类器来预测两个观察是否来自同一个地方。 然后，分类器用于将当前观察结果与先验图进行匹配，以估计机器人可能的初始姿态。 最后，多假设粒子滤波算法实现最终定位。 与其他算法相比，我们在三个数据集上展示了算法性能。 实验结果表明，该算法具有更快的收敛速度和更高的收敛精度。 未来，我们将通过深度学习考虑更健壮的特征，进一步减少耗时。
优点：改进了蒙特卡洛的粒子滤波方法，先进行位置识别，再进行全局定位。
缺点：定位耗时较长。