【论文笔记】Fast LIDAR Localization using Multiresolution Gaussian Mixture Maps

【论文笔记】Fast LIDAR Localization using Multiresolution Gaussian Mixture Maps

~~~~~~本文提出了一种用于自动驾驶汽车城市环境中车辆定位的快速多分辨率扫描匹配器。最先进的车辆定位方法依赖于使用 (LIDAR) 扫描仪观察路面反射率，以实现厘米级精度。然而，当面临使道路油漆（例如水坑和雪堆）的视线变得模糊的不利天气条件或路面纹理不佳时，这些方法通常会失败。因此本文提出了一种新的扫描匹配算法，它利用高斯混合图来利用环境中的结构；这些地图是 z 高度分布上的高斯混合的集合。我们通过开发一种新颖的分支定界、多分辨率方法来实现实时性能，该方法利用这些高斯混合的光栅化查找表。结果显示在两个 3.0 公里的数据集上：标准轨迹和恶劣天气条件下的另一个。

高斯混合图

~~~~~~本文建议利用高斯混合图，这是一种 2D 网格结构，其中每个网格单元都维护一个高斯混合模型，表征该单元中 z 高度（即垂直结构）上的分布。 此外，我们通过高斯混合总和的光栅化提出了一个新的上限，使我们能够将扫描匹配问题表述为分支定界搜索。 有关这些地图的示例参见图 1。 本文的主要贡献是：
1.将大点云的数据简化为高斯的紧凑混合。
2.这些参数化地图的在线光栅化可实现快速推理。
3.分支定界配准公式，允许使用通用上限光栅化进行实时、有保证的最佳配准。

地图构建

~~~~~~本文集成了里程计、GPS 和扫描匹配约束，将离线构建这些地图作为一个完整的 SLAM 问题，以此来构建自洽的位姿图。不再应用人工高度先验，在全 3D 中构建图形可以构建更准确的表示。
使用优化后的姿态图，然后我们重新投影所有 3D LIDAR 扫描点以创建点云。然后在固定的网格分辨率下（我们使用 20 厘米整个），查看 z 点的每个“列”。为了捕获 LIDAR 扫描仪的方差并减少离散化错误，本文使用高斯内核用相邻的“列”模糊这些 z 点。然后使用 EM 将加权高斯混合模型拟合到这些点。在本文提供的数据集上，整个离线地图构建过程可以在大约 1 小时内完成。对于每个单元格，为不同数量的高斯重新运行 EM，选择生成的具有最大可能性的高斯混合而不会过度拟合，生成的高斯混合网格构成了我们的高斯混合图 G。
我们的高斯混合图中的推理很容易完成。 使用我们的高斯混合图 G 和在线点云扫描 C，我们可以概率性地评估给定变换 T 的可能性，从而使两者对齐。 我们通过将变换应用于点云中的每个点来评估这种可能性，C’= TC。 然后，对于这个变换后的云中的每个点，pi = (xi, yi, zi)，我们索引我们的高斯混合图以获得该单元格的高斯混合，gi ← G(xi,yi)。 从那里，我们将每个点视为一个独立的高斯观测，允许我们通过对每个点的对数似然求和来计算总对数似然

其中 wij、μij 和 σij 分别是 gi 的第 j 个分量的权重、均值和标准差。
1.配准方式
给定一个点云，我们寻求找到最大化方程 1 的最佳变换。
我们观察到，一个典型的轮式机器人平台在侧倾、俯仰和高度方面将受到相当好的限制：因为 (i) 由于观察到重力，大多数 IMU 将侧倾和俯仰限制在几度之内（注意轮式机器人平台）平台仅通过轻微的侧倾/俯仰）和 (ii) 任何轮式车辆必须停在地面上，这会限制高度与事先地图。
因此，我们可以通过在 x、y 和航向变换的范围内进行详尽搜索来根据此调整我们的搜索策略。与 [22] 一样，我们可以通过首先将航向旋转应用于所有点，然后评估 xy 平移来有效地计算这些。
随着我们的解决方案在最优值附近，然后我们执行一个简单的、受约束的 6 自由度爬山来锁定我们搜索空间上的全局最优值：(x, y, z; r, p, h)*。这允许对高度、滚动和俯仰进行微小但必要的改进。
因为我们的注册问题是由搜索边界参数化的，所以我们能够使用姿势先验来提高运行时性能。可以在算法 1 和算法 2 中找到注册到我们的高斯混合图中的详细概述。

多分辨率分支定界

~~~~~~~用高效的多分辨率分支定界搜索替换了极其昂贵的穷举搜索。

1.多分辨率方式

~~~~~~~我们的多分辨率搜索背后的想法是使用一个边界函数，该函数可以提供我们参考地图中一组单元格的上限。 这意味着大部分搜索可以在较粗的分辨率下执行，这可以在更精细的尺度上对可能性进行上限。 使用紧边界可以将上一节中介绍的穷举搜索转换为不进行贪婪假设的易于处理的搜索。 分支定界策略的结果与穷举搜索完全相同。
为了评估单个变换（即 (xi; yi)），您必须评估点云中每个点的对数似然，然后将所有这些相加以获得总对数似然。 因此，在穷举情况下，每个点都针对单个高斯混合进行评估。 为了搜索一系列变换，例如 (x_i; y_i) → (x_{i+N}; y_{i+N})，每个点都针对总共 (N + 1)^2 个高斯混合进行评估。 然而，我们地图中的每个单元在空间上都非常相似，这意味着对 (xi, yi) 的推断产生与 (xi+1, yi) 相似的对数似然，因此穷举搜索通常会在低似然区域花费不必要的时间 .
我们制定了一个分支定界搜索，它详尽地搜索了我们最粗略的分辨率，提供了一系列转换的上限。 然后将这些粗略搜索结果添加到优先级队列中，按这些上限可能性排序。 然后我们遍历这个优先级队列，分支以评估下一个更精细的分辨率，并添加回优先级队列。 一旦从优先级队列返回最精细的分辨率，搜索就完成了。
我们提出了一种与传统考虑的略有不同的多分辨率地图结构。 在许多领域，多分辨率搜索意味着构建目标数据的粗略版本并对其进行评估（例如，图像金字塔）。 然而，我们的方法创建了许多重叠的粗块（如图 4 所示）以更好地计算紧密的上限。 这种优化使得更好的界限而不是更小的内存占用进行权衡。
因为我们的地图在每个多分辨率层中具有相同的分辨率，这导致我们在较粗的分辨率中采取更大的步幅，其中(stride)步幅 = 2^{res}。 多分辨率地图的分支因子和数量完全由用户定义。 在我们的实验中，我们选择了分支因子 2 ( (x_i, y_i) : [(x_i, y_i),(x_i; y_{i+2^{res}}), (x_{i+2^{res}}, y_i); (x_{i+2^{res}}, y_{i+2^{res}} )]) 以限制不必要的工作量。
有关更详细的概述，请参阅算法 3 和图 4。

2.栅格化高斯混合贴图

~~~~~~在这里，我们为多分辨率搜索定义了边界函数。
为高斯集合找到好的参数边界是一项相当困难的任务，因此我们选择了光栅化查找表形式的非参数解决方案。 在最精细的分辨率下，我们通过评估固定离散化的对数似然，为每个网格单元生成光栅化，用光栅化版本替换我们的参数化高斯混合图。 然后可以通过在光栅化查找表中的每个离散化中取最大值来精确计算上限。 有关这些地图的直观表示，请参见图 5。

对于像我们这样的纯本地化任务，可以离线预先计算查找表。然而，我们决定只在磁盘上存储参数化的高斯混合图以避免存储非常大的图。这使我们能够使用不到 30 MB 的磁盘空间存储 1 公里的城市地图，其中我们的地图每 20 厘米网格单元的高斯分布不超过两个。然后，我们能够从我们的参数化高斯混合图作为后台作业高效地在线计算光栅化多分辨率图。这是使用每个连续的多分辨率层逐步完成的，以构建下一个。请注意，我们的光栅化多分辨率地图是一种通用表示，可用于许多地图类型，包括标准 NDT、MLS 地图、占用体素等。将这些任意地图之一转换为光栅化多分辨率地图后，我们提出的管道的其余部分可以用于点云的快速配准。该管道还可以适用于 Levinson 等人的概率强度图。 [7]。

实验结果

~~~~~~我们通过在我们的自主平台 TORC ByWire XGV 上收集的数据评估了我们的算法。 如图 6 所示，这款自动驾驶汽车配备了四台 Velodyne HDL-32E 3D LIDAR 扫描仪和一个 Applanix POS-LV 420 惯性导航系统 (INS)。所有实验均在配备 Core i7- 的笔记本电脑上运行 3820QM 中央处理器 (CPU)。
在两个主要数据集上进行了实验：
• 市中心：穿过密歇根州安娜堡市中心的 3:0 公里轨迹，其中多条道路从两个方向穿过，数据集包含多个动态障碍物。
• 下雪天：与下雪天的下雪天的下雪天相同的轨迹，积雪活跃地下落并覆盖地面，如图 7 所示。
我们还通过安娜堡市中心进行了额外的遍历，以构建我们的高斯混合图。 所有这三个数据集都使用我们的离线 SLAM 程序对齐，为我们的实验提供了足够准确的地面实况（地面实况精度比我们的定位误差大一个数量级）。

1.多分辨率配准结果

~~~~~~对于我们数据集中的一组扫描，我们通过在真实姿势的 10 m 内随机采样来评估我们的多分辨率配准。 通过在这些随机采样点周围执行搜索，我们希望看到我们的算法能够将扫描返回到真实估计； 通过 L2 距离量化我们的配准误差。
我们以两种方式呈现这些结果。 首先，我们将结果编译成一个直方图，如图 8 的第一行所示。在这里我们看到，我们提出的解决方案能够以最少的异常值返回到地面实况的 0:5 m 以内。 此外，我们看到由于我们的方法利用了 3D 结构，因此它不受恶劣天气的影响，并且我们的结果在两个数据集上相似。 此外，尽管有大量的降雪，如图 7 所示，我们的方法仍然是稳健的。
其次，我们将这个相同的配准误差显示为扫描匹配器的初始偏移量输入的函数，如图 8 的底行所示。 搜索空间能够包含完美的变换。
在图 9 中可以看到通过我们的多分辨率搜索空间进行的示例搜索。所示示例在大约 2 秒内探索了 25 m × 25 m 的区域，而在穷举搜索中只需要评估 1% 的必要变换。

滤波结果

~~~~~~我们将我们的注册算法集成到扩展卡尔曼滤波器 (EKF) 定位框架中，这是 [4] 的扩展。 唯一使用的测量值来自我们的 IMU 用于里程计，我们的多分辨率扫描匹配围绕我们的 3-σ 后验置信度进行初始化。 在将多分辨率扫描匹配纳入我们的过滤器时，我们使用固定的测量不确定度； 然而，可以使用 [22] 中的探索搜索空间来拟合保守协方差。
我们将我们的定位性能与我们自己实现的 Levinson 等人提出的最先进的基于反射率的定位进行了比较。 在 [6]、[7] 中。 我们基于反射率的定位系统使用板载的四个 Velodyne HDL-32E 构建正交地面图像； 然后可以将这些正交地面图像与使用这些扫描的累积构建的正交先验地图对齐。
由于在 Downtown Snowy 数据集期间地面上有大量积雪，我们还必须将 GPS 测量纳入我们基于反射率的定位系统（结果用星号表示）。 如果没有这些额外的测量，滤波器将不断发散，因为正交反射率匹配很少成功。
我们在图 10 中显示了 Downtown 和 Downtown Snowy 数据集随时间的横向和纵向误差。可以看出，我们提出的解决方案能够在 Downtown 数据集上实现与基于反射率的解决方案类似的性能。 此外，我们能够在 Downtown Snowy 数据集中保持良好的本地化，而基于反射率的解决方案始终存在分歧，只能将偶尔的测量值纳入其 EKF。
此外，我们的结果列在表 I 中。在这里，我们表明我们能够在 Downtown 数据集上分别实现 15:5 cm 和 10:3 cm 的纵向和横向均方根 (RMS) 误差。 此外，我们在 Downtown Snowy 数据集上分别获得了 18:0 cm 和 9:4 cm 的纵向和横向 RMS 误差。 我们的方法能够提供大约 3-4 Hz 的扫描匹配。

3.障碍物检测

~~~~~~在我们的自动车辆定位管道中使用结构的另一个好处是它提供了一种概率方法来将点云点分类为动态障碍物与否。 在生成配准的可能性时，我们根据先前的地图评估每个扫描点的可能性，这告诉我们每个扫描点是地图的一部分的可能性。 因此，通过查看与先验图对齐不佳的点（即那些可能性较低的点），我们可以进行分类。 我们通过设置马哈拉诺比斯距离阈值并将超过该阈值的所有点标记为障碍物来做到这一点。 我们的公式允许我们在逐帧的基础上进行分类并扩展我们对障碍物的感知范围。 点云分类的可视化可以在图 11 中看到。

结论

~~~~~~在本文中，我们展示了一种新的高斯混合图，可用于从观察到的点云进行快速配准。 通过使用可以在线计算的多分辨率光栅化地图，我们可以使用分支定界搜索有效地找到有保证的最优配准，而不是像现代扫描匹配器那样寻找局部最优。 最后，我们将其集成到 EKF 中，以证明自主平台可以单独使用这些测量值在先前地图中保持良好定位。 我们提出的系统能够处理恶劣的天气和质地较差的道路，这与当前最先进的自动车辆定位方法相比具有显着优势。