- 1. Synthetic 2D LIDAR for Precise Vehicle Localization in 3D Urban Environment
- 2. 摘要
- 3. 介绍
- 4. 虚拟平面上的定位
- 5. 3D感知模块
- 6. 在线定位
Synthetic 2D LIDAR for Precise Vehicle Localization in 3D Urban Environment
摘要
本文提出了一种精确的三维城市环境中车辆定位算法，该算法只有一个二维激光雷达和里程计信息。提出了一种新的合成二维激光雷达思想来解决虚拟二维平面上的定位问题。基于合成激光雷达测量和里程计信息，采用蒙特卡罗定位方案进行车辆位置估计。通过在NUS校园区域内1.5公里的驾驶测试中进行实时定位，证明了该算法的精度和鲁棒性。
介绍
本文提出了一种新的合成激光雷达概念，以二维的方式解决三维定位问题。合成激光雷达是由3D滑动窗口中提取的兴趣点来实时构建的。基本假设是城市环境中的许多平面在垂直方向上是直线的。从直线表面提取兴趣点，然后投影到虚拟水平面上，形成合成激光雷达。合成激光雷达作为世界三维环境和虚拟化的水平二维平面之间的桥梁。利用合成激光雷达的思想，二维定位算法可以很容易地适应三维问题。本文提出了一种基于合成激光雷达的蒙特卡罗定位算法，并通过实验验证了该算法的准确性和鲁棒性。
本文的其余部分组织如下。第二部分讨论了将3D世界投射到2D平面的想法，并给出了定位系统的概述。第三节介绍了合成激光雷达的构成。定位算法见第四节，实验结果及分析见第五节。
虚拟平面上的定位
将3D世界投影到2D平面
机器人在平面上的定位已经研究了几十年，提出了许多算法。二维扫描匹配算法由于其准确性和鲁棒性可能是最受欢迎的选择[27]。然而，它不能直接应用于在3D世界中移动的车辆。由于室外道路有时非常崎岖，平面激光雷达的激光点可能会投射到路面上，而不是像[2]中所讨论的那样投射到所需的垂直物体上。我们之前的研究[22]利用倾斜向下的激光雷达在城市道路上提取道路边界特征，然后利用这些特征进行车辆定位。实际上，城市环境中还有许多其他显著的特征有利于定位。
提取哪些特征，如何提取，以及如何将它们提供给定位方案是需要进一步解决的问题。
通常需要三维距离数据来提取机器人在三维世界中导航的特征[20]。在本文中，我们使用倾斜的激光雷达在倾斜扫描中生成环境的3D点云。我们不是直接对原始数据进行三维扫描匹配[2]，而是尝试从三维点云中提取特征，并使用垂直特征进行定位。我们的方法假设城市环境中有丰富的垂直表面，如路边、建筑物的墙壁，甚至垂直的树干。
“垂直世界”假设实际上是许多文学作品中普遍使用的假设。Harrison等人在[8]中提出了一种在机器人运动时生成高质量三维激光距离数据的方法。通过利用垂直世界的假设，可以推断出有用的信息(例如，滚转角和俯仰角)。Kohlbrecher等人[13]仅使用单个2D LIDAR和IMU就实现了2D SLAM和6DOF姿态估计。虽然没有明确解释，但这项工作的基本假设是环境包含许多垂直表面。
Weingarten等[30]利用这一假设实现了结构化环境的快速重构。在本文中，由于室外环境可能比结构环境具有更多的任意形状物体，因此在使用垂直假设之前必须进行分类步骤。在分类过程中，基于表面法向估计提取垂直表面上的激光点。当倾斜的激光雷达扫描环境时，一些垂直表面将被连续激光扫描从下到上扫描到。如果我们对这个扫描过程进行鸟瞰，并将垂直特征投射到虚拟水平面上，它与在二维表面上移动水平激光雷达的机器人完全相同。从数学的角度来看，垂直表面限制了不同高度的激光点如何相互匹配。基于上述直觉，提出了合成激光雷达的思想。合成激光雷达是在投影虚拟平面上的平面二维激光雷达，其激光束的终点是三维环境中垂直表面的投影点。
合成激光雷达的思想有助于解决二维平面上的三维定位问题。虽然车辆以6DOF在3D世界中移动，但一般来说，地面车辆最感兴趣的是其2D姿态向量(x, y，偏航)。通过将三维垂直特征投影到虚拟平面上，可以通过标记这些垂直特征来使用二维占用网格地图。这样，就可以在合成激光雷达的思想下，利用SLAM获得先验地图。需要说明的是，我们的算法仅适用于只有一个垂直可遍历级别的环境。对于具有更多可遍历级别的情况，可以使用一些其他2.5D或全3D算法，例如[12]。
系统概述
定位系统主要由两部分组成，即提取关键特征点的三维感知和解决水平面上定位的二维定位。合成激光雷达作为连接3D世界和2D虚拟平面的桥梁，如图1（a）所示。
该系统使用IMU和车轮编码器来提供6DOF里程计信息，使用2D向下倾斜的LIDAR来提供激光扫描，并使用占用网格图作为定位的先验。使用简单的航位推算来获得里程计信息。假设车轮编码器在第n个时间步长测量的距离为\(r_n\)，旋转由pitch角θ和偏航角Ψ给出，则车辆位置的变化由下式给出：
3D感知假设里程计系统在短时间内足够精确，并积累激光扫描的3D点云数据。然后应用分类程序从累积的数据中提取兴趣点。然后将提取的激光点投影到虚拟水平面上(忽略它们的z值)，并构建合成二维激光雷达。在蒙特卡罗定位方案中，二维定位融合了来自里程计的信息和来自合成二维激光雷达的测量结果。通过事先生成的垂直特征先验地图，可以实现二维水平面上的定位。
3D感知模块
对合成激光雷达的要求之一是具有良好的适应能力，在城市场景中能够适应不同类型的环境。我们通过在合成激光雷达生成之前，从重建的环境模型中适当提取兴趣点构成先验地图来实现这一点。为了能够识别垂直于地面的特征，一个精确的世界模型是必要的。
有许多方法可以建立精确的环境模型，包括nodding LIDAR和Velodyne。如第II-A节所述，一个固定的、向下倾斜的单平面激光雷达可以通过扫描地面来精确地重建环境。这是一个有吸引力的解决方案，因为它成本低，只需要传感器的刚性安装。它还允许进行优化，从而允许对该配置特有的特征提取进行实时计算。
3D滑动窗口
重建采用滚动窗采样，以保持测距传感器反射最近样本的高概率。因此，使用3D滚动窗口来积累短距离内记录的不同扫描。窗口的大小是灵活的，滑动窗口形成了3D环境的局部地图，即它与车辆一起滚动，新的传入扫描将添加到窗口中，而旧的样本将被丢弃。
更具体地，给定窗口大小\(w\)，点\(p\)为第n次扫描的点，\(P_{n}\)通过累积得到：
如图6所示，如图1（b）所示，\(w\)用于控制累积扫描的数量，使得窗口的大小不会无限增长。此外，只有当达到足够的距离β时，才插入新的扫描，这有两个影响，一个小的β会有更密集的点，但整个窗口大小会变短，反之亦然。滑动窗口在系统的里程计框架中工作，其中物理激光雷达的每次扫描都是基于IMU和车轮编码器得出的里程计信息来进行投影的，如第II-B节所述。
垂直点云分类
为了提取垂直于地面的特征，使用表面法线的估计。虽然存在许多方法[11]，但我们使用了[3]提出的法向量估计，基于一阶3D平面拟合，其中通过对点的局部邻域\({P^{K}}\)[25]执行最小二乘平面拟合来近似空间中每个点的法线。
平面由一个点\(x\)和法向量\(n\)与点\(p_i \in P^{K}\)的距离\(d_i\)表示，其中\(d_i\)定义为:
通过取\(x=\bar{p} = \frac{1}{k} \sum_{k=1}^{k} p_i\)作为\(p_{k}\)的质心，可以在最小二乘意义上计算\(n\)的值，使得\(d_i=0\)，通过计算以下协方差矩阵\(C\in \R^{3x3}\)的特征值和特征向量，可以得到法向量\(n\)的解[24]：
其中，\(k\)是局部邻域中的点数，\(\bar{p}\)是邻域的质心，\(λ_j\)是第j个特征值，\(v_j\)是第j个特征向量。\(P^{K}\)的主分量于特征向量\(v_j\)相关联。因此，可以从最小特征值\(λ_0\)中找到法向量\(n\)的近似值。一旦找到法向量\(n\)，就可以通过简单地取沿z轴的阈值\(n\)来获得垂直点，例如0.5。这取决于传感器数据的噪声程度。
为了有效地找到局部邻域点，从滚动窗口中获得的所有点构建KDtree[19]，并在每个点执行固定半径搜索。尽管曲面法线可以作为一个整体进行计算，但在滚动窗口中的每个点执行法线计算可能非常昂贵。为了进一步降低计算复杂度，保留了两个连续的滚动窗口，其中:
其中Φ可以是任何点分类函数，\(P^{\phi}\)包含处理过点，P包含原始点。这样，相比于直接计算滚动窗口Pn+1，需要的计算量小得多。换句话说，这确保了只对新累积的点云进行分类，并且可以重用以前实例中处理过的点。
合成激光雷达构造
来自分类点的结果由3D中的兴趣点的集合组成。对于合成LIDAR的构建，将3D中的兴趣点投影到虚拟水平面（z＝0）中。可以看出，这种合成激光雷达有一个非常特殊的特点：能够“看透”障碍物。这是可能的，因为点的解释是在3D中完成的。
合成激光雷达的构建是通过将虚拟传感器放置在车辆底部并将所有兴趣点从里程计转换到车辆底部来完成的。
在许多需要标准激光雷达（等距角度增量）的应用中，可以进一步重建合成激光雷达以满足这一约束。这将涉及以每个固定角度增量执行光线跟踪，以从可能的端点获得最小范围值。总体3D感知可以在图2中进行总结。3D感知是通过PointCloud库[23]完成的，该库提供了本节中描述的许多操作。
在线定位
MCL定位
本文采用[1]中的蒙特卡罗定位（MCL）方案来估计车辆姿态。MCL是一种基于贝叶斯定理和蒙特卡罗思想的概率定位方法[28]。MCL的核心是粒子滤波器，其中车辆位置的置信度由一组粒子维持。MCL主要由预测、校正和重采样三个步骤组成。对于预测步骤所需的运动模型，使用我们之前工作[22]中的“伪3D”里程测量运动模型。观测模型的选择如下所述。
虚拟雷达观测模型
为了将测量用于定位，合成激光雷达需要一个测量模型。合成激光雷达采用似然模型。由于虚拟光束的端点是兴趣点从垂直表面的投影，因此来自不同垂直表面的不同点可能具有相同的角度。在其他工作中，存在两个角度相同但具有两个不同范围值的激光束。因此，合成2D激光雷达是一种特殊的激光雷达，它只检测垂直表面，也可以透过这些表面。有鉴于此，仅需要激光束端点的似然模型非常适合于合成激光雷达。
实验
Experiment Setup
我们的测试台是雅马哈G22E高尔夫球车，配备了各种类型的传感器。硬件配置如图3所示。安装在前上方的倾斜式激光雷达是用于定位的SICK LMS-291激光雷达。
4层激光雷达SICK LD-MRS400001安装在腰部，用于障碍物检测。高尔夫球车的两个后轮都安装有编码器，用于提供行驶距离的估计值。惯性测量单元（IMU）MicroStrain 3DM-GX3-25安装在实际车轴的中心，以提供车辆的方位信息。
定位算法在新加坡国立大学工程学院进行了测试，那里的道路上下起伏，道路外有许多高层建筑。
通过使用合成激光雷达作为输入，首先使用图优化SLAM技术生成先验地图。为了进行姿态优化，[29]被用作检测环路闭合的前端。然后，使用[10]中的isam优化库恢复完全优化的姿态。
为了评估从合成激光雷达构建的恢复地图的质量，将地图投影到卫星地图上，如图5所示。该地图显示出与卫星地图的一致性和良好相关性，面积约为550米×487米。尽管地图左侧存在差异（由于道路沿线的地理特征），但总体拓扑结构得到了保持。这表明该地图可以用于精确定位。
Experimental Results
合成激光雷达能够在带有酷睿i7处理器的笔记本电脑上以50Hz的输出速率执行，这表明合成激光雷达可以用于执行实时定位。定位结果如图6所示。从之前的地图来看，我们算法的定位结果总是与我们的行驶路径一致，在长距离的道路上清晰地显示出与道路边界的平行线。
由于我们的算法不依赖于GPS，我们的估计在高楼密集的区域附近仍然表现良好。注意，在实验中，给出了一个粗略的初始位置，因此定位主要与姿态跟踪有关。
然而，该系统能够处理小的绑架问题，例如来自激光雷达的短暂数据错误，因为里程计系统仍然能够提供信息。如果发生大的绑架，例如车辆在放置之间移动而没有打开定位模块，则可以提供粗略的初始位置以加快收敛速度。
图4显示了“定位方差”与“行驶距离”的关系：
角度估计方差通常小于1◦, 如图4（a）所示。图4（b）显示了相对于车辆在纵向和横向方向上的“位置估计方差”与“行驶距离”的关系。结果表明，在整个测试过程中，两个方向的方差都很小。较差的方差发生在纵向上，其值约为0.2m。这表明定位算法对其位姿估计具有高置信度。同时，还可以看出，横向方差通常小于纵向方差。这与我们之前的工作[22]中讨论的事实一致，即在城市道路环境中，横向的特征比纵向的特征丰富得多。
这种提出的方法已被用于执行类似于[22]的自主导航。为了表明定位结果是一致的，进行了两次自主运行，如图7所示。高尔夫球车有一条从A到E的路径。为了验证定位的准确性，选择了5个检查点，其中执行从每次自主跑步中捕获的2个灰度图像之间的每像素绝对差。为了确保相同的照明条件，两次自主运行在同一天连续进行。图中的5张较小的图像。图7是视觉验证的结果。图像的尖锐边缘提供了强有力的证据，证明定位系统能够重复地提供精确的位置。请注意，图像B、C和E中出现的黑点是移动物体的自然结果。这表明，只需一个二维激光雷达就可以实现精确导航。
