论文笔记—Robust Localization Using 3D NDT Scan Matching with Experimentally Determined Uncertainty and Road Marker Matching

文章摘要

     ~~~~          ~~~    在本文中，我们提出了一种基于点云匹配方法（正态分布变换“ NDT”）和基于光检测和测距强度的道路标记匹配的定位方法。基于点云地图的定位方法使自动驾驶汽车能够准确估计自己的位置。但是，当环境的外观发生变化时，无法进行准确的定位和“匹配误差”估计，这在乡镇环境中很常见。为了解决这些错误，我们建议在工作之前（离线）估计NDT扫描匹配的误差。然后，当车辆在环境中行驶时，将适当的不确定性分配给扫描匹配。 3D NDT扫描匹配利用离线估计的不确定性信息，并使用粒子过滤算法与道路标记匹配方法相结合。结果，可以在3D NDT失败的区域中执行准确的定位。另外，减少了定位的不确定性。实验结果表明了该方法的有效性。

导语

     ~~~~          ~~~    定位是自动驾驶的基本技术。 可以利用精确的定位来改善高级自动驾驶所需的路径跟踪和计划以及感知功能的性能。 本文提出了一种使用多层光检测和测距（LiDAR）的鲁棒且准确的定位方法。 在这项研究中，我们使用由移动制图系统（MMS）构建的三维（3D）点云和道路标记地图[1]。
　　在道路环境中，有许多不同的对象可以用作地标，例如道路标记，标志，路缘石和杆子。 先前已经提出了使用这些对象的定位方法[2]，[3]，[4]，[5]，[6]。这些方法可以在具有足够数量的地标的区域中执行准确的定位。 但是，公共道路包括没有那些地标的区域，例如，居民区街道和森林多山的道路。 在这样的区域中，需要其他类型的定位来执行高级自动驾驶。
　　Levinson等。 在[7]中提出了一种基于地图的定位方法。 使用的地图是使用安装在车辆上的LiDAR构建的。 由于此方法不需要特定的地标，因此可以在各种区域中使用。 在[8]中提出了一种类似的方法。 但是，基于地图的定位方法无法对外观发生变化的环境执行可靠的定位。 不幸的是，由于靠近森林和农田，公共道路的外观容易发生变化。 为了解决这个问题，提出了一种使用树和动态障碍物检测的定位方法，称为变化检测[9]。 但是，对象识别并非易事，而更改检测仍然是一个未解决的问题。
　　正态分布变换（NDT）可以应对轻微的环境变化，因为环境图由一组正态分布表示[10]，[11]。 特别是，使用NDT进行扫描匹配的鲁棒性要比基于点对点的迭代最近点（ICP）扫描匹配的鲁棒性更高[12]。 但是，当初始位姿的精度较低时，NDT有时会降到局部最小值。 据报道，扫描匹配失败时NDT的误差大于ICP扫描匹配[12]。 此外，NDT的收敛性能取决于环境的外观，并且在环境和地图之间存在差异的情况下，定位精度会降低。
　　在这项工作中，我们预先估计了每个区域（离线）中3D NDT扫描匹配的误差。 如图1所示，3D NDT的收敛性能取决于环境条件。 通过执行离线实验，可以在车辆在环境中导航时将适当的不确定性分配给扫描匹配。 3D NDT扫描匹配利用离线估计的不确定性信息，并使用粒子滤波（PF）算法与道路标记匹配方法相结合[13]。 结果，可以在3D NDT失败的区域中执行准确的定位。 另外，减少了定位的不确定性。 　
　　本文的贡献如下。
　　通过进行预实验可以很好地估计NDT扫描匹配的不确定性。
　　即使NDT可能失败，所提出的定位方法也可以稳健地估计车辆的位姿。
　　使用在不同季节制作的地图，可以在公共道路上实现稳健而准确的定位（地图制作是在夏季完成的，而定位实验则是在冬季进行的）。
　　本文的其余部分组成如下。 在第二节中，我们描述了相关的工作。 在第三部分中，我们介绍了有关NDT扫描匹配的详细信息，并通过执行预实验来研究其收敛性能。 然后，我们在第四节中介绍了拟议的定位方法的详细信息，同时在第五节中介绍了包括比较在内的实验。在第六节中，我们对本文进行了总结。

相关工作

     ~~~~          ~~~    在[14]中，作者提出了一种与我们提出的方法相似的方法。 此方法使用3D点云和道路标记匹配方法。 根据高度划分3D点云图，并在每层中执行ICP扫描匹配。 在他们的建议中，获得了三个ICP结果，并使用加权平均值将这些结果合并在一起，其中，通过考虑ICP的误差确定从每一层获得的估计结果的权重。 另外，路标匹配结果用于补偿横向误差。 尽管我们的建议类似于此方法，但我们新添加了基于3D点云图的定位（即3D NDT）的不确定性估计。 此估计增加了NDT中故障的鲁棒性。
　　已经提出了几种方法来估计扫描匹配算法的不确定性。 Bengtsson等。提出了一种使用误差函数的Hessian矩阵的方法[15]。但是，存在这样的问题：当没有获得扫描点与目标点之间的精确对应关系时，将产生不正确的概率分布。 Censi提出的方法不仅可以使用Hessian矩阵，而且可以使用有关扫描点的偏微分来解决此问题[16]。但是，据报道该方法的计算过程将是多余的。 Olson提出了一种蛮力搜索方法，该方法搜索离散空间以确定扫描匹配结果的概率分布[17]。 Olson使用了多种技术来减少计算时间，例如，使用查找表和GPU。但是，计算成本仍然很高。此外，应搜索各种范围以准确确定概率分布，并且还需要大量的计算资源。
　　Lilienthal等人已经广泛研究了基于NDT的定位。 他们提出了基于NDT的蒙特卡洛定位（MCL），并报告说基于NDT的MCL的精度要高于基于网格的MCL [18]。 尽管可以从MCL中获得定位结果的不确定性，但要应用到3D定位中，将需要大量的计算资源。他们还提出了一种扩展的方法，该方法使用双时标ND映射来提高对环境变化的鲁棒性[19]。 使用此方法可以在动态环境中实现高度精确的定位。 但是，如果本地化失败，则估计结果将不会收敛，因为该方法会在线构建其ND地图，并且在线地图构建会失败。
　　在我们的方法中，我们通过执行预实验来研究NDT扫描匹配的收敛性能，并利用此结果确定NDT的不确定性。 即使此方法需要进行预实验，也可以很好地估计NDT的不确定性。

NDT扫描匹配和聚合性能调查

A.实验平台

     ~~~~          ~~~    图2显示了本研究中使用的实验平台。 我们使用Ubuntu和Robot Operating System（ROS）作为开发环境。 该车辆将HDL-64E安装为LiDAR，并安装了惯性导航系统（INS），该系统包括车轮编码器和陀螺仪传感器。 由INS更新包括位置 x x x和 y y y以及偏航角 θ θ θ的二维（2D）位姿。 因为车轮编码器安装在右后轮上，所以车辆的运动模型如下所示：

其中 u t = ( Δ d t ， Δ θ t ) T u_t=(Δd_t，Δθ_t)^T ut​=(Δdt​，Δθt​)T表示由INS测量的从 t − 1 t−1 t−1到 t t t的平移和旋转运动量。 请注意，此位姿代表后轮轴的中心。

B. NDT扫描匹配

     ~~~~          ~~~    预先从3D点云构建了3D ND地图（表示为体素地图）。 令 P i = ( p i , 1 , p i , 2 , . . . , p i , N i ) P_i=(p_{i,1},p_{i,2},...,p_{i,N_i}) Pi​=(pi,1​,pi,2​,...,pi,Ni​​)是体素 i i i中包含的3D点。 体素 i i i包含3D均值 p i m p_i^m pim​和这些点的协方差 Σ i Σ_i Σi​。
　　在定位阶段，用于扫描匹配的初始位姿由运动模型确定。 使用体素网格（VG）滤波器对来自HDL-64E的测量点云进行过滤，从而减少了点数。 ND贴图和VG滤波器的体素大小均设置为1m。 我们使用滤波后的点 Q = ( q 1 , q 2 , . . . , q M ) Q=(q_1,q_2,...,q_M) Q=(q1​,q2​,...,qM​)进行扫描匹配。 通过最大化以下成本函数 J ( x ) J(x) J(x)来估算3D姿态 x \bold{x} x，其中包括位置 x x x， y y y和 z z z，以及侧倾角，俯仰角和偏航角 ψ ψ ψ， φ φ φ和 θ θ θ： 　　
其中 R R R和 t t t分别表示旋转矩阵和与车辆位姿有关的平移矢量， p i m p_i^m pim​和 Σ i Σ_i Σi​分别表示体素 i i i中包含的均值和协方差，与 q i ′ {\bold{q}_i}' qi​′对应。 我们使用高斯-牛顿法来最大化成本函数。 最后，我们使用NDT， x ^ \hat{\bold{x}} x^获得了估计的位姿。 实施的NDT扫描匹配的处理时间少于0.1s。 更多细节可以在[20]中找到。

C.NDT的收敛性能

     ~~~~          ~~~    我们使用车辆获取了日志数据，并进行了离线实验以研究NDT的收敛性能。 图3展示了由AISAN TECHNOLOGY Co. Ltd建立的实验环境鸟瞰图。在2016年夏季构建了环境的3D点云和道路标记图。数据记录于 2016 年 12 月根据黄线进行。 这表明地图和环境之间存在一些差异，这是由于作物田地丰富而导致的季节性变化。

　　在这项研究中，我们首先在更新了HDL-64E读数的每个位置（测量周期为10 Hz）中应用了NDT扫描匹配。 NDT收敛后，我们从与第一个收敛位姿略有不同的不同初始位姿再次执行NDT。请注意，我们仅更改了2D位姿的元素。结果显示在图4的顶部和底部。这些结果分别在区域A和B中获得，如图3所示。左图显示了从第一个收敛位姿仅更改 x x x和 y y y值时的结果。在这两个方面的收敛性能都很好。中间的图显示了 x x x， y y y和 θ θ θ值更改时获得的结果。 θ θ θ值以2度为增量从-2度更改为2度。两种情况下的收敛性能均略有下降。最后，右图显示了当 θ θ θ值显着变化时的结果。 θ θ θ值以1度为增量从-5度更改为5度。显然，初始位姿的航向方向的精度会显着影响收敛性能，并且收敛性能对于每个区域都会有所不同。 　　
　　图5和图6显示了在区域A和B中获得的深度图和直方图。区域B的两侧可以确认为空地，因为农田很近。 相比之下，区域A的两侧都有物体。这些结果强烈表明收敛性能取决于每个区域的环境条件。　　　
　收敛性能取决于初始位姿。 在调查涉及使用与第一个收敛位姿显着不同的初始位姿的情况下，可能存在很大的不确定性。 因此，我们根据经验只考虑了实际定位阶段会出现的错误，并研究了收敛性能。 最后，我们使用 θ θ θ值仅从 -5 度变为 5 度时获得的结果，并根据结果计算协方差矩阵 c o v ( x ^ ) cov(\hat{\bold{x}}) cov(x^)。 图 7 显示了协方差矩阵根据路径的轨迹。 　　

使用3D点云和道路标记图进行定位

     ~~~~          ~~~    本节提供了道路标记匹配的详细信息，以及使用粒子滤波器结合航位推算，NDT和道路标记匹配的定位方法。

A.道路标记图和检测

     ~~~~          ~~~    道路标记图表示为2D似然图。 图8示出了道路标记地图的示例。 地图的网格大小设置为0.2m。

　　从HDL-64E的读数中检测出道路标记。 由于道路标记是在路面上绘制的，因此首先会检测到路面点。 为了检测地面点，我们创建了读数的深度图，并计算了每个点的法线向量。 该过程的详细信息可以在[21]中找到。 将具有 z z z方向元素的值（HDL-64E的坐标）超过0.8的法线向量的点检测为路面点。 此外，我们修剪了这些点以删除明显远离路面的点。
　　HDL-64E可以测量在0-255范围内归一化的反射强度值。 道路标记被检测为反射器，因此它们的反射强度很大。 我们将强度阈值设置为170，因此将反射强度超过阈值的路面点检测为道路标记点 Z = ( z 1 , z 2 , . . . , z K ) Z=(z_1,z_2,...,z_K) Z=(z1​,z2​,...,zK​)。 图9示出了道路标记检测方法的图像的示例。 　　

B.所提出的定位系统

     ~~~~          ~~~    我们使用PF算法来实现所提出的定位系统。 尽管NDT扫描匹配估计了3D位姿，但此PF估计了2D位姿。 粒子通过等式(1)中所示的运动模型进行更新。 道路标记的观察模型 p ( Z t ∣ x t ) p(Z_t | x_t) p(Zt​∣xt​)表示如下：

　　其中 R j R_j Rj​和 t j t_j tj​分别表示从第 j j j个粒子计算出的旋转矩阵和平移矢量，而 Σ l = d i a g ( σ l 2 ， σ l 2 ) Σ_l=diag(σ_l^2，σ_l^2) Σl​=diag(σl2​，σl2​)和 µ i µ_i µi​分别表示LiDAR读数的协方差矩阵和 ( z i ) ′ (z_i)' (zi​)′对应的道路标记地图的网格位置。 通过考虑网格大小，将 σ l σ_l σl​设置为0.2。
　　在提出的方法中，NDT扫描匹配是并行运行的。 PF系统通过NDT, x ^ \hat{\bold{x}} x^接收估计的姿势。 NDT的结果具有协方差矩阵 c o v ( x ^ ) cov(\hat{\bold{x}}) cov(x^)，该矩阵是在实验前的每个位置计算得出的。 然后，我们使用以下观察模型计算出粒子的可能性： 　　
其中 N ( x t ; x ^ , c o v ( x ^ ) ) \mathcal{N}(x_t;\hat{x},cov(\hat{x})) N(xt​;x^,cov(x^))表示高斯分布，它表示NDT的不确定性。
　　为了避免粒子的错误收敛，只有在似然存在偏差的情况下才进行重采样。 在本研究中，我们使用有效样本量（ESS）[22]，并在满足以下条件时进行重采样： 　
其中 w i w_i wi​和 N p N_p Np​分别表示第i个粒子和粒子数的归一化可能性。 我们将粒子数设置为1000。此外，在执行重采样时，将混合5％的随机粒子。