【论文笔记】Map-Based Localization Method for Autonomous Vehicles Using 3D-LIDAR

~~~ ~~~~ 精确和稳健的定位是复杂场景中自动驾驶汽车的一项重要任务。自动驾驶汽车的准确位置对于决策和路径规划是必要的。在本文中，提出了一种使用 3D-LIDAR 传感器精确定位自动驾驶汽车的新方法。首先，执行路缘检测算法。接下来，梁模型用于提取多帧路缘的轮廓。然后，采用迭代最近点算法和两个卡尔曼滤波器，基于高精度地图估计自动驾驶汽车的位置。最后，实验结果证明了所提出方法的准确性和鲁棒性。
　　在本文中，使用 Velodyne 3D-LIDAR 传感器并生成多帧特征来表示完整的两侧路缘石。路缘检测算法是鲁棒的，不需要假设路面平坦。为了减少离群点的影响，实现更高的定位精度，推出了几种波束模型来消除大部分的离群点。

基于CURB-MAP的定位

~~~ ~~~~ 基于路边地图的定位算法首先对单帧点云进行路缘检测算法。基于车辆动力学，通过将以前的路缘投影到当前车辆坐标系中来使检测到的路缘更加密集。然后，应用梁模型来提取密实路缘石的轮廓。最后通过ICP算法将提取的轮廓与高精度地图进行匹配。所提出算法的框架如图1所示。

路缘检测

~~~ ~~~~ 在本文中，激光雷达传感器安装在图 2（a）所示的自动驾驶汽车的顶部。原始数据是在 3D 极坐标中获得的，如图 2（b）所示。路缘是从它们的空间特征中检测出来的。大多数城市地区的路缘石高度是统一的，通常比路面高出 10 至 15 厘米。此外，高度在垂直方向上急剧变化。基于这些特征，Zhang 等人提出了一种鲁棒的方法。 (2015) 从单个框架中识别路缘石。检测结果如图3所示。
　　检测到的路缘石的密度随着距离的增加而减小。为了获得路缘的完整表示，基于车辆动力学将多帧路缘转换为当前车辆坐标系。传感器坐标系随着自动驾驶汽车的变化而变化。车轮速度传感器和惯性导航系统 (INS) 用于测量车辆的运动。两帧之间的位置和航向偏差定义为 x-y 平面上的 dx、dy 和 dθ。检测到的单帧路缘用 Qk 表示，其中 Qk = [xk yk]T 是第 k 帧路缘坐标。因此，多帧路缘表示为：

其中 fn(Q)=f(f(⋅⋅⋅f(Q)))f^n(Q) = f(f(· · · f (Q)))fn(Q)=f(f(⋅⋅⋅f(Q)))。函数 f 定义为：

轮廓提取

~~~ ~~~~ 由于错误检测，致密的多帧路缘中存在一些异常值。根据大多数异常值位于道路外的特性。 Thrun (2006) 提出的 Beam 模型是测距仪的近似物理模型，广泛应用于机器人领域。它是一系列具有共同初始点的波束，测距仪位于该点并且均匀分布，角度分辨率 δ=2π/nδ = 2π/nδ=2π/n。本文采用beam模型提取轮廓并消除异常值。在自动驾驶汽车的每个轨迹点上设置了几个光束模型。梁模型表示如下：

其中ri = (xi, yi) 是R 中的第i 个路缘坐标。Zk 表示第k 个梁区域。坐标(xini,j, yini,j)为梁模型的第j个发射点。 dk 是路缘 ri 中距离最短的路缘的索引。因此，提取所有第 dk 个路缘以表示轮廓。过程如图5所示。在图5（a）中，黄色点代表自主车辆的轨迹点，浅蓝色线代表在每个轨迹点发射的光束线。在图 5（b）中，粉红色的圆点表示使用梁模型方法提取的路缘石轮廓。

地图匹配

~~~ ~~~~ 地图匹配过程旨在估计自动驾驶车辆检测到的路缘与数字地图提供的路缘之间的偏差。 ICP 算法是 Besl 和 McKay (1992) 提出的匹配算法。提取轮廓后，轮廓的点用C表示，地图中的特征点用M表示。 ICP算法的目的是通过最小化代价函数找到一个变换T：

其中 dist 表示欧几里得距离函数。优化问题可以通过如下迭代方法解决：
1 使用k维树找出M中每个点Ci的对应关系。
2 基于奇异值分解 Arun 等人计算对应关系的变换 T。 (1987)。
3 应用变换 C = TC 并计算成本 J。
4 当 J 的变化低于预设阈值 τ 时终止迭代。
在迭代过程之后获得变换矩阵T。图 6 显示了我们案例的地图匹配。粉红色的圆点代表上面描述的提取的轮廓，绿色的圆点代表地图的点。图 6（a）显示了等高线和地图的初始位置。经过多次ICP迭代，轮廓与图6（d）中的地图几乎重合，并计算出变换矩阵。

定位

~~~ ~~~~ 得到变换矩阵后，采用两个卡尔曼滤波器过滤噪声，提供自动驾驶汽车相对准确的位置。双卡尔曼滤波器的框架如图 7 所示。自主车辆由时间 t 的点位置 pv(t) = [xv(t) yv(t) θv(t)]T 描述。基于运动学模型计算预测的车辆位置：

其中 ˆpv(t + 1) = xˆv(t + 1) ˆ yv(t + 1) θˆv(t + 1) T 是时间 t + 1 的预测车辆位置，Δt 是时间步长。第一个卡尔曼滤波器用于通过集成低成本 GPS 接收器测量的位置来校正预测的车辆位置。通过第一个卡尔曼滤波器的校正位置表示为：
pv,gps(t + 1) = xˆv,gps(t + 1) ˆ yv,gps(t + 1) θˆv,gps(t + 1) T 然后，第二个卡尔曼滤波器用于估计位置自动驾驶车辆通过整合地图匹配的变换矩阵 T 和校正位置 ˆpv,gps(t + 1)。进行了多次实验以验证所提出方法的准确性。

实验结果

~~~ ~~~~ 为了评估所提出的方法，本地化实验在离线和在线进行。我们使用的高精度地图如图8所示。实时运动（RTK）GPS用于记录轨迹和LIDAR传感器数据，然后应用路缘检测方法自动提取路缘。之后，我们还手动修改地图点以生成高精度地图。蓝点代表道路边界，用作所提出方法的参考。

离线实验

~~~ ~~~~ 该数据集由我们的自动驾驶汽车在同济大学校园道路上记录。记录的数据集由 4260 帧组成。我们的自动驾驶汽车配备了以 100 Hz 运行的 Oxford Inertial+2 和 NovAtel GPS 接收器。定位系统通过无线电调制解调器从 GPS 基站接收无线电技术委员会 (RTCM) 校正。 RTKGPS 为评估定位算法的准确性提供了参考依据。我们将所提出的方法与仅使用低成本 GPS、使用低成本 GPS 与 INS 以及使用考虑单帧路缘的低成本 GPS 与 INS 的方法进行比较。仿真是用 Matlab 实现的。定位的横向、纵向和航向误差如图 9 所示，五种方法的统计结果如表 1 所示。图 9（a）显示了每个车辆轨迹点的横向误差。红线和蓝线分别代表基于单帧路缘和多帧路缘的横向误差。线越宽，横向误差越大。图 9(b)©(d) 显示了数据集中定位误差的波形。与单帧特征相比，所提出的方法在横向定位方面有 58.24% 的改进。由于直路路缘特征没有纵向区分，纵向误差几乎不变。此外，该方法的平均处理周期约为 50 ms，这对于自动驾驶汽车的实时操作是可能的。

在线实验

~~~ ~~~~ 经过定位方法的比较，所提出的算法在Windows-8操作系统上用C/C++实现。在线实验在我们的自动驾驶汽车平台上进行了测试。控制器是具有 16GB RAM 和 Intel Core i7-3610QE CPU 2.3GHz 的 ADLINK 工业个人计算机 (IPC)。在线实验在我校进行。轨迹长度为3.1公里，车辆平均速度为26.9公里/小时。实时操作窗口如图10所示
　　我们的自动驾驶汽车的状态，如速度、纬度、经度和卫星数量也显示在操作窗口中。在图 10 中，白点表示提取的路缘石轮廓，红点表示存储在高精度地图中的路缘石。我们还在白框中标记了基于 GPS 的位置，并且在汽车模型中标记了基于所提出方法的位置。