【论文笔记】3D LiDAR-Based Precision Vehicle Localization with Movable Region Constraints

【论文笔记】3D LiDAR-Based Precision Vehicle Localization with Movable Region Constraints

~~~~~~~本文讨论了一种基于已知地图信息的高性能相似度测量方法，称为交叉均值绝对差（CMAD）方法。应用传统的归一化互相关 (NCC) 特征配准方法需要足够数量的特征点，这些特征点也必须表现出接近正态分布。然而，现场扫描和收集的光探测和测距 (LiDAR) 测距点云数据很少，并且不满足接近正态分布。因此，当 NCC 特征与地图特征配准时，会发生相当大的定位错误。因此，提出了CMAD方法来有效提高NCC算法和定位精度。由于定位传感器的不确定性会导致定位过程的偏差，因此建立了可驱动移动区域 (DMR) 以限制位置搜索的范围，过滤掉不合理的轨迹，并提高定位速度和性能。对基于窗口的、DMR-CMAD 和 DMR-NCC 方法的定位结果以及同时定位和映射方法的定位结果进行了误差比较。 DMR-CMAD 方法在精度上与基于窗口的方法没有太大区别：室内实验的均方根误差不高于 10 cm，室外实验的均方根误差为 10-30 cm。此外，DMR-CMAD 方法是三种方法中耗时最少的，DMR-NCC 比其他两种方法产生更多的定位误差并需要更多的定位时间。最后，DMR-CMAD 算法被用于汽车的成功现场即时定位。
本研究采用合成激光雷达作为其基本框架，提出了一种测量交叉平均绝对差（CMAD）的高稳健性相似度测量方法，并将CMAD集成到可驱动移动区域（DMR）中以进行即时定位。这种定位方法比传统的归一化互相关 (NCC) 方法更能令人满意地检测到移动物体。值得注意的是，这里只使用了 3D LiDAR，没有加入额外的里程表或 IMU 数据来支持。因此，该搜索方法没有运动模型特征，无法准确识别车辆行驶信息。此外，当使用基于窗口的方法进行定位时，估计的位置会突然向侧面或向后移动；因此，添加了 DMR 以限制不合理的运动区域，过滤不适当的定位轨迹，并提高定位系统的速度和性能。根据结果，虽然DMR方法在定位精度上与基于窗口的方法没有明显区别，但与基于窗口的方法相比，它定位车辆所需的时间更少。此外，当使用粒子过滤器进行定位时，粒子不需要散布在整个窗口区域；相反，它们只需要传播到尽可能远的可驾驶区域，从而缩短收敛时间并实现即时定位。

方法

定位算法的程序结构

~~~~~~~图 1 说明了地图定位算法的结构。 3D 点云被映射、校准和分割以获得所需的点云信息，然后将信息转换为网格图。最后，采用虚拟激光雷达扫描方法提取环境特征并建立地图数据库。上述程序也用于 LiDAR 现场扫描过程。提取所有特征后，通过平均能量法估计初始位置和方向，并在特征配准过程中加入DMR，以识别车辆的当前位置和方向。

校准

~~~~~~~在实验中，LiDAR 的位置没有校准，因此偏离路面，导致点云信息模式如图 2 所示。因此，从点云的 X-Z 平面中选取四个点，描绘在图 2a 中，选择了两个单独的点用于（1）中的斜率计算。然后使用反三角函数 tan−1tan^{-1} 来计算角度 φAφ_A 和 φBφ_B，如（2）所示。随后，求出两个角度之间的均值来识别旋转角度q，如（3）所示，并对整个3D点云进行校准，以方便地图数据库的构建。图 2b 描绘了 LiDAR 航向角与车辆头部方向之间的分歧。

分割

~~~~~~~校准后的点云被分割以捕获与路面正交的特征。图 3a 说明了原始点云的示意图，其中绿色、蓝色、红色和紫色点分别代表墙壁、道路、车辆和管道。实验中只需要环境轮廓；不需要车辆和管道。因此，云被分割在一个大于车辆高度（Hvehicle）但小于管道高度（Hline pipe）的范围内，如图 3b 所示。但是，如果是杂点，例如图中的紫色点，分割后可能无法完全呈现所需的轮廓。
分割的目标是获得分割点云的法向量与道路点云的法向量之间的内积。因此，必须保留垂直于道路的特征。内积在阈值之间的范围内，代表了所需的特征点。计算内积需要对道路的点云信息进行分割（图3d），计算分割后的点云和道路点云的法向量。向量是通过将最小二乘平面拟合到局部相邻点来计算的 [35]
在计算分割点云和道路点云的法向量后（图 3c，e），并获得道路点云的法向量的平均值。随后，计算分割云的法向量与上述平均值之间的内积。最后，为内积建立阈值，保留-0.005和0.005之间的点云信息，如图3f，g所示。图 3b，仍然存在。

将点云转换为网格图

~~~~~~~将 3D 点云转换为 2D 网格图，用于提取特征并在其分割和校准后构建地图数据库。图 4a 展示了分割后的 3D 点云信息，通过变换压缩成网格图（图 4b）。网格图中包含有关墙和柱的环境信息，其中每个网格为 10 × 10 cm2。此外，结合网格地图以建立如图 4c 所示的地图数据库，其中绿色区域表示车辆的可能位置；值得注意的是，该数据库具有 x 轴、y 轴和特征信息。 LiDAR 现场扫描过程也采用上述程序来捕捉现场环境特征，然后将其与地图数据库中的特征数据进行注册，以确定最佳位置和方向

特征提取

~~~~~~~环境特征是从网格图中提取的，用于与 LiDAR 数据配准。值得注意的是，使用虚拟 LiDAR 扫描而不是全局扫描来提取特征，因为局部扫描的特征比全局扫描的特征更准确。从半径为 R（最大扫描范围的 30%）的圆内扫描特征，如图 5a 所示；初始扫描点（即 0°）在虚拟 LiDAR 的左侧，逆时针扫描数据。提取的特征表示为距离-角度数据（图 5b），其中 x 轴代表角度，y 轴代表距离。因此，LiDAR 扫描的每个周期都会产生 360 度的距离-角度数据。假设网格数据库展示了 N 个可能的车辆位置，如图 5a 中的绿色区域所示，LiDAR 扫描的每个周期都会产生 360 度的距离-角度数据。此外，由于 N 个位置中的每一个都表现出不同的环境特征，因此该数据库共有 N×360 个特征数据。然后将这些数据合并到仅包含位置信息的地图数据库中。

特征配准

~~~~~~~将地图和激光雷达特征进行配准，以获得车辆在后续定位计算中的最佳位置和航向角。当前流行的模板匹配技术涉及通过 NCC 方法进行相似性测量 [36-38]。
参考信号和测试信号用t和s表示，其在测试信号上的移动角度用t表示。 s(t) 表示具有偏移 t 角的测试信号。 NCC的相似度水平用c(t)表示，计算方法如(4)所示：

其中 mi 和 ms(t) 分别代表 t 和 s(t) 的平均值； st和ss(t)分别代表t和s(t)的标准差(SD)； N代表参考信号的度数（设置为360）。该过程通过频域中的傅立叶变换进行，以降低计算成本。具体而言，当 c(t) 最大化时，t 表示测试和参考信号之间的最佳匹配角度。 c(t) 的值介于 -1 和 1 之间：它越接近 1，表明测试和参考信号越相似。
应用 NCC 需要足够数量的匹配点，这些匹配点也必须表现出接近正态分布。在这里，通过激光雷达扫描的点云信息相对稀缺。因此，当 NCC 特征与 LiDAR 和地图的特征匹配时，会导致较大的定位误差。为了增强 NCC 算法，应用 CMAD 方法来比较地图和 LiDAR 特征之间的相似性。这个过程中涉及的参数列举如下

其中 PLidar 表示现场 LiDAR 特征（图 6b），PMap 表示虚拟扫描的 LiDAR 特征（图 6a），W 表示在 PLidar 和 PMap 中扫描的障碍点集，PDMR 表示 DMR 中的位置点集，V 代表通过轮询机制选择的最佳位置和航向角。
如图 6d 所示，模板 (PLidar) 与固定信号 (PMap) 重叠以进行特征配准。模板在固定信号上移动的角度用t表示，模板与信号的相似度用c(t)表示，计算如下

其中 M 表示 W 的度数，t 的范围从 1° 到 360°。当 c(t) 最小时，LiDAR 和地图特征最匹配（图 6c）。因为（5）只涉及一个点的特征配准，而DMR总共有L个点，所以用L个点中最小的c(t)来确定最优位置PBest，如（6）所示：

其中对应的 t 代表最佳航向角 qHeading

估计初始位置

~~~~~~~特征提取在本地进行。因此，有些角度没有显示任何距离值，因为该角度没有扫描障碍物或距离值超过扫描半径 R。 接下来，通过平均能量法计算 LiDAR 和地图特征，其中包括添加距离将所有扫描的能量点的值加在一起并将总和除以能量点的数量。等式（7）衡量的是map的能量，其中N代表扫描的能量点数，dMap,i代表第i次的距离值，（8）衡量的是LiDAR的能量，其中M代表能量的数量扫描的点，dLidar,j 表示第 j 个度的距离值。至此，地图的能量数据在地图数据库中建立并实现

平均能量法也被应用于全局搜索车辆的初始位置和航向角。通过上述平均能量值选择车辆可能的位置，其中 ELidar 是 LiDAR 的能量。设置了搜索范围来过滤靠近ELidar的地图能量如下
其中 t 表示可容忍误差，在室内较小，室外较大。由于地图的点云比激光雷达的现场点云（相对稀缺）更全面，因此地图的大部分能量值高于激光雷达的能量值。理论上可以通过全局搜索地图和激光雷达特征的最佳配准来获得初始位置。为了减少计算时间，还应用平均能量方法来全局搜索车辆的粗略初始位置和航向角。通过上述平均能量值选择车辆的可能位置。使用平均能量方法获得的选择结果在图 7a 中以红色表示。为了获得最佳初始位置和航向角，CMAD 将这些选定位置的特征与在必要时刻扫描的 LiDAR 特征进行注册（图 7b）。

基于窗口的定位

~~~~~~~通过平均能量法确定车辆的初始位置后，通过基于窗口的定位确定其在下一个时间点的位置。与应用于全局搜索的平均能量方法不同，基于窗口的定位涉及通过窗口搜索方法在本地搜索车辆可能的位置，并且所需的计算时间要少得多。首先，将通过平均能量法计算的初始位置设置为中心。创建了一个 w × h 窗口，如图 8a 中的紫色矩形框所示。其次，根据初始位置坐标确定当前时间点车辆在窗口内的可能位置，如图8b中的蓝点所示。最后，将这些位置的特征与并发 LiDAR 特征进行注册以获得车辆的当前位置，如图 8c 中的橙色点所示。传感器 2019, 19, x 12 of 24 通过窗口搜索方法搜索车辆的可能位置，并且所需的计算时间显着减少。首先，将通过平均能量法计算的初始位置设置为中心。创建了一个 w × h 窗口，如图 8a 中的紫色矩形框所示。其次，根据初始位置坐标确定当前时间点车辆在窗口内的可能位置，如图8b中的蓝点所示。最后，将这些位置的特征与并发的 LiDAR 特征进行注册以获得车辆的当前位置，如橙色所示

基于 DMR 的定位

~~~~~~~由于仅使用 3D LiDAR 搜索车辆位置，没有额外的里程表或 IMU 数据，并且由于系统不包含任何运动模型，因此无法识别车辆运动信息。因此，还进行了基于 DMR 的定位来计算近似于实际车辆运动的轨迹。尽管通过基于窗口的定位估计的运动轨迹可能导致与正常运动状态的侧向或向后偏差，如图 9a、b 中的红色圆圈所示，但 DMR 旨在解决此问题并减少估计车辆位置

与基于窗口的定位类似，基于 DMR 的定位涉及通过平均能量方法估计车辆的初始位置和方向。以此初始位置为中心，建立具有最大移动距离半径 R 和旋转半径 r 的 DMR（由图 11a 中的紫色圆圈表示）。整个 DMR 如图 10 所示，其中 po 表示先前估计的位置和初始点。 pi（附近位置）与po之间的距离不应大于R，且pi与(po + r)之间以及pi与(po − r)之间的距离必须大于或等于r。满足这两个条件的 pi 是位于 DMR 内的一个点，由（10）确定。由于已知初始位置的坐标，因此可以计算 DMR 内的其他位置；车辆可能的当前位置由图 11b 中的蓝点表示。与基于窗口的估计位置不同，DMR 根据车辆航向角的变化进行旋转。因此，使用前一个时间点的航向角作为旋转角度，使用 z 轴旋转整个搜索区域。最后，将区域内位置的特征与并发 LiDAR 特征进行注册，以隔离车辆在地图上的当前位置，由图 11c 中的橙色点标识

实验

室内定位实验

~~~~~~~本研究选择的室外环境是国立台北科技大学 (NTUT) 地下 5 层的停车场。图13描绘了整个停车场的轮廓，小车的运动轨迹用红色圈起来。

基于窗口的定位

~~~~~~~通过基于窗口的定位检测到的运动轨迹使用 EKF 进行平滑，并与通过 SLAM 方法识别的轨迹进行比较。图 14a 描绘了通过基于窗口的定位识别的轨迹。然而，由于 Window 轨迹与 Kalman 轨迹非常相似，因此轨迹的反转部分被局部放大（图 14b，其中 SLAM 轨迹由圆圈表示，Window 轨迹由十字表示）。值得注意的是，当汽车倒车时，基于窗口的定位会出现侧向偏差。图 14c 说明了两个计算轨迹之间的横向误差。从第 368 帧开始（即汽车开始倒车时），误差大幅扩大到最大 2.07 个网格（20.7 厘米）。同时，最大纵向误差约为 1.36 个网格（13.6 厘米）。

基于 DMR 的定位

~~~~~~~通过基于 DMR 的定位检测到的运动轨迹使用 EKF 进行平滑并与 SLAM 轨迹进行比较。 DMR-NCC 轨迹也与 SLAM 轨迹进行了比较。图 15a 描绘了 DMR-CMAD 轨迹。然而，由于上述三个轨迹相当相似，轨迹的反转部分被局部放大（图 15b，其中 DMR-NCC 轨迹由点表示，SLAM 轨迹由圆圈表示，DMR-CMAD轨迹由十字表示）。值得注意的是，当汽车倒车时，DMR-CMAD 轨迹比 Window 轨迹更稳定，但 DMR-NCC 轨迹表现出相当大的侧向误差。图 15c 描绘了三个计算轨迹之间的横向误差。从第 372 帧开始（即汽车开始倒车时），误差大幅增加到最大 2.63 格（26.3 厘米）。最大纵向误差约为 1.58 个网格（15.8 厘米）。根据 DMR-NCC 轨迹和 SLAM 轨迹之间的误差比较，在汽车前进和倒车时，误差的变化是相当大的。最大横向和纵向误差分别约为 3.05 格（30.5 厘米）和 2.9 格（29 厘米）。所提出的 DMR-CMAD 算法的性能通过停车场试验场上的车辆测试进行了验证。所提出的算法将有助于实现自主泊车控制

户外定位实验

~~~~~~~这项研究的室外环境是 NTUT 校园。图 16 显示了校园的 2D 轮廓，包括汽车的运动轨迹。车子从中正纪念楼前出发，在第六教学楼前的球状地标上掉头，按原路经过第二教学楼，停在蒋介石前-石纪念楼。

基于窗口的定位

~~~~~~~通过基于窗口的定位检测到的运动轨迹使用 EKF 进行平滑，并与通过 SLAM 方法识别的轨迹进行比较。图 17a 描绘了窗口轨迹。然而，由于Window轨迹与Kalman轨迹非常相似，所以轨迹的三个部分被局部放大：图17b显示了开始和结束部分；图17c描绘了中间部分；图 17d 说明了围绕地标的掉头。具体来说，SLAM轨迹用圆圈表示，Window轨迹用十字表示。值得注意的是，当汽车倒车时，基于窗口的定位会出现侧向偏差。如图 17e 所示，从第 368 帧到第 409 帧（即汽车掉头时）的横向误差最大，最大横向误差约为 4.71 格（47.1 厘米）。相比之下，纵向误差在掉头前显着，在掉头后减小；最大纵向误差为 2.99 格（29.9 厘米）。

基于 DMR 的定位

~~~~~~~通过基于 DMR 的定位检测到的基于 DMR 的定位使用 EKF 进行平滑，并与通过 SLAM 方法识别的轨迹进行比较。 DMR-NCC 轨迹也与 SLAM 轨迹进行了比较。图 18a 描绘了 DMR-CMAD 轨迹。然而，因为所有三个轨迹都非常相似，所以轨迹的三个部分被局部放大：图 18b 显示了开始和结束部分，图 18c 描绘了中间部分，图 18d 说明了围绕地标的 U 形转弯。具体来说，DMR-NCC轨迹用点表示，SLAM轨迹用圆圈表示，DMR-CMAD轨迹用十字表示。值得注意的是，当汽车倒车时，基于窗口的定位会出现侧向偏差。如图 18e 所示，第 404 帧（即汽车掉头时）的横向误差最大，最大横向误差约为 6.31 格（63.1 厘米）。相比之下，纵向误差在掉头前显着，在掉头后减小；接近终点的最大纵向误差为 3.34 个网格（33.4 厘米）。同时，在汽车转弯前、汽车转弯时和汽车接近终点时，DMR-NCC 轨迹的​​定位误差变化很大。最大横向误差约为 19.04 格（190.4 厘米）；最大纵向误差为 26.9 格（269 厘米）。所提出的 DMR-CMAD 算法的性能通过在室外试验场上的车辆测试得到验证。所提出的算法将有助于实现车道级自动驾驶控制

室内外误差对比

~~~~~~~表 1 列出了室内和室外误差的均方根误差 (RMSE) 和 SD 的比较。 DMR-CMAD 轨迹的精度近似于 Window 轨迹的精度，但 DMR-NCC 轨迹的​​误差，尤其是那些户外轨迹的，更显着

在实验中，Velodyne LiDAR（VLP-16）被用来使用激光扫描周围环境并建立3D点云。用于车辆定位的计算机平台配备了 Intel i5 CPU、8 GB DDRIII。表 2 列出了在室内（总共 734 帧）和室外（总共 1274 帧）实验中检查的所有四种方法的平均定位时间。 DMR-CMAD方法的定位时间最短，DMR-NCC方法的定位时间是Window和DMR-CMAD方法的2-3倍，SLAM方法的定位时间最长。值得注意的是，室内和室外环境的定位时间也有所不同，部分原因是室内场地的面积更小，汽车的速度比室外场地更慢。