L5如何实现：从特斯拉Autopilot2.0谈车辆精确自定位技术
编者按：本文来自微信公众号“新智元”（ID:AI_era）,作者：视觉的守望者,小猴机器人;36氪经授权发布。
此前，特斯拉发布了新版Autopilot2.0自动驾驶系统，宣称达到Level5水平，并公开了在Model X上长达10分钟左右的实验视频。从视频中可以看出，在城市路段中，Autopilot2.0可以正确的检测车辆、行人、车道线、红绿灯等目标，完成正常的自动驾驶任务。不难看出，高精度地图的使用和精确车辆自定位技术的设计，可以被认为是Autopilot2.0的最大亮点。众所周知，高精度地图和传统地图相比，其主要优势就是厘米级的目标精度，而精确的车辆自定位技术也是如此。本文结合AutoPilot2.0的情况，介绍自定位技术的原理，可能的技术方案，以及我们在这个过程中的实践。
革命性的AutoPilot2.0
与其他基于激光雷达（LiDAR）设计自动驾驶方案的公司不同的是，特斯拉依然在摄像头为主的道路上独自前行。那么相比较Autopilot1.0，新版的自动驾驶系统的改进包括哪些呢？
硬件设备的优化。如我们上次复盘特斯拉若干起事故的文章所述，多传感器的融合能够提高障碍物的检测率。因此，新版Autopilot2.0增加了6个摄像头，包括前视、侧视和后视；提高了现有毫米波雷达和超声波雷达的感知距离和能力。
算法性能的优化。引入了高精度地图，通过GPS精确定位车辆位置，即可确定周边环境的基础道路模型，提取护栏、灯杆、标牌、路口等固定信息，进而帮助后续决策和控制模块的设计。
自动驾驶等级的升级。Autopilot1.0可以看作是沿用Mobileye的Level 2等级，而Level 2等级是无法完成上下匝道、路径规划以及有目的变道等复杂功能的。新版本通过引入高精度地图和精确车辆自定位技术，完成了真正意义上的自动驾驶。
不难看出，高精度地图的使用和精确车辆自定位技术的设计，可以被认为是Autopilot2.0的最大亮点。众所周知，高精度地图和传统地图相比，其主要优势就是厘米级的目标精度，而精确的车辆自定位技术也是如此。
对于传统的车辆自定位，一般精度在1~10m左右；很常见的问题就是无法分辨主路还是辅路、桥上还是桥下等，这对于自动驾驶是致命的。
自动驾驶要求精确的车道编号和车道偏移，只有这样才能安全指导后续的规划和决策。自动驾驶要求的车辆自定位精度通常在10cm左右。
自动驾驶中的精确车辆自定位
传统的车辆自定位通过一个普通的GPS即可完成。而在自动驾驶中，常见的一种方案是通过高精度的差分GPS+惯导IMU+地面基站联合完成。但动辄几十万的硬件成本，在现有情况下很难完成自动驾驶车辆的量产需求。
上述方案是基于网络通信+信号处理实现的，而业界中另外一种方案是通过高精度地图+感知设备+精确车辆定位来完成的。这种方案可以明显降低车辆的生产成本：
高精度地图有多种制作方法，包括基于LiDAR点云的、基于视觉SLAM的，可以用于权衡定位精度和制作成本；此外，将高精度地图的制作成本平摊到每辆车，亦可显著降低成本。
这里的感知设备包括摄像头、GPS、4/8线激光雷达等相对廉价的设备。
同样，精确车辆定位算法的设计成本也是可以平摊到每辆车上的。
那么如何完成上述精确车辆自定位方案的设计呢？我们先看看下面这幅示意图。
如果我们有“优衣库”的精确位置，而且能够测量出“优衣库”距离自己的精确距离，那么以“优衣库”为中心画一个圆，自己一定处于圆周之上。这时候如果我们还能确定“目标1”的精确位置和距离自己的精确距离，而且知道朝向，就可以唯一确定自己的精确位置了。这就是自定位方案最基本的原理。
而上述方案的精度主要取决于目标位置的精度和距离的精度。
目标位置的精度主要取决于地图数据的质量。如果地图本身就是不精确的，那么很难让人相信自定位能够达到10cm左右的精度。因此，精确自定位的首要要求就是高精度地图的制作；高精度地图通常要求相对精度在厘米量级。
而距离的精度则取决于感知设备的选择和自定位算法的设计。首先，通过感知设备精确采集、分割、识别目标本身就是一件困难的事情；其次，将目标和高精度地图中制作的目标进行匹配和对应也存在一定的误差。
当然，如果给定的目标越多，例如上图中增加“目标2”等，由于误差的平均减少原理，自然能够增加自定位的精度。
从上面的分析不难看出，自定位方案依赖于高精地图，而感知设备的选择和自定位算法的设计决定了最终的定位精度。下面将对几种典型的精确车辆自定位方案进行探讨（包括感知设备的选择和相应的自定位算法的设计）。
感知设备的选择和自定位算法的设计
常用的感知设备和相应的自定位算法可归纳如下：
1.激光点云数据+3D配准
最开始也是最直观的方案是直接对激光点云数据进行3D配准。激光点云数据不会受到光照变化、天气、遮挡等因素的影响，而且能够直接得到和目标的物理距离，因此利用多视角几何（multi-view geometry）即可完成目标的自定位。在下图中，左侧为实时采集的激光点云数据，而右侧为高精度地图中存储的局部激光点云数据。
根据多视角几何理论，上述两个场景的相机存在姿态上的差异，而这个差异可以通过3D点的坐标变换来补偿；变换矩阵主要包括3D旋转变换和3D平移变换共6个自由度。如果实时采集的点云数据 
一般来说，4组点对就可以唯一的确定上述变换矩阵。
但上述分析面临两个难题：
4/8线激光点云的数据是稀疏的，每个点的描述也只有反射率和相机映射过去的彩色值，那么如何确定对应点对？
通常点对精度不够，而且存在大量的误匹配点对，如何滤除存在的噪声？
一种解决方法是利用ICP算法（Iterative Closest Point，迭代最近点），它的主要思路是先利用某些几何特征来假设一些点对，并计算变换矩阵；再对所有点进行变换，并更新假设的匹配点对；也就是利用这种不断迭代的方式来优化自定位结果。当然，还有很多其他的非线性优化算法可以解决上述问题。
激光点云数据+3D配准方案存在的缺点包括：
初值非常重要，不合适的初值极大的影响了自定位结果。
上述优化过程很难收敛到全局最优解，而局部最优解难以保证自定位的精度和稳定性。
变换矩阵的自由度较高，而且匹配失败的时候难以及时得到确定。
2. 激光点云数据+2D配准
考虑到3D配准存在的自由度高、非线性优化难度大的问题，人们考虑利用一些人为的干预来解决。2D配准的方案，就是将点云数据往xy平面投影，从而生成一张从上到下的俯视图来进行匹配运算。在下面的示意图中，(a)可以认为是高精度地图中存储的投影结果，而(b)是实时采集的点云数据的投影结果。
(a)
(b)
经过这种处理，高精地图中存储的投影结果和实时采集的点云数据的投影结果仅存在旋转角、x方向平移量和y方向平移三个自由度。
2D配准还有一个明显的好处就是在这种投影变换下，xy平面可以约等于路面。而路面中存在的车道线、地面标记、护栏等大量具有鉴别意义的区域，可以有效的提高自定位的精度和稳定性。对于上述投影结果的匹配，通常的配准算法都可以胜任，包括基于频域的配准算法（模板匹配、Lucas-Kanade配准）、基于频域的配准算法（傅立叶-梅林算法）以及基于特征域的配准算法（SIFT、SURF、MSER）。
激光点云+2D配准方案也存在一些缺点：
2D配准主要依赖路面信息，忽视了交通标牌、红绿灯等3D信息，降低了方案的普适性。
当遇到缺乏纹理或者纹理大量重复的路面环境时（例如高速、匝道等），很容易配准失败且不自知。
图像数据+视觉地图
图像数据对比激光点云数据主要的优点是成本更低，因此很早之前就有很多计算机视觉领域的方案致力于解决自定位的问题。其中一种典型的方案就是视觉地图的方案。它主要使用了特征点的定位和描述、以及立体几何方面的知识，包括粗定位和细定位两步。
粗定位指的是米量级以内的定位。如下图所示，该步骤类似于计算机视觉中的场景分类技术。在高精度地图的制作过程中，每行驶1m就采集一张图像，并利用SIFT、HOG等特征描述子提取图像特征。在实时定位时，也对当前采集的图像提取图像特征并和高精度地图局部范围内的存储结果进行匹配。最终将图像特征最为接近的那一张作为参考图像，完成米量级的定位。
而细定位则需要定位到厘米量级。通过对参考图像和实时采集的图像进行多视角几何分析，确定两张图像对应的相机间的3D旋转和3D平移变换来完成自定位。这个很类似于激光点云+3D配准方案，但与其不同的是，利用图像层面的特征点提取、分析和匹配，我们已经可以确定匹配的点对了。
图像数据+视觉地图面临的问题包括：
参考图像的确定主要依赖图像整体特征的提取和匹配，对于缺乏纹理或者纹理大量重复的区域（例如高速），自定位精度大大降低。
与激光点云数据不同，高精度地图存储结果和实时采集的图像受到日期、天气、路况等多种因素的影响会产生明显差异，可能导致匹配失败。
精确自定位算法取决于图像特征点的提取和匹配，但一般很难达到10cm左右定位精度的要求。
图像数据+关键目标的检测和匹配
如果说上述方案都是基于点层面的下层（low-level）信息，那么关键目标的检测和匹配的方案则利用了物体级别的上层（high-level）信息。通过提取一些关键目标和对应的精确的几何特征，并将其和高精度地图中存储的信息进行对应和匹配，从而完成自定位。该方案更加接近于人类的直观思维。
这里的关键目标包括车道线、地面标记、交通标牌以及红绿灯等。在该方案中，将自定位划分成x方向和y方向分别进行处理。
x方向指的是垂直于路面前进的方向，通常利用车道线、路沿、护栏来确定。通过深度学习的技术，可以很稳定的在光照变化、场景突变等情况下完成上述目标的检测和几何拓扑的分析。只要和高精度地图中相应目标的存储数据进行对应，即可完成x方向的定位，如下图所示。
y方向则对应平行于路面前进的方向，通常利用地面标记、交通看板、红绿灯来确定。同样的，需要利用深度学习技术精确提取上述目标，并和高精地图中存储的目标进行精确匹配，从而找到一系列的2D-3D的对应点。参考激光点云数据+2D配准的相关技术，即可唯一的确定变换矩阵，从而完成y方向的精确定位。
在实际使用中，x方向自定位依赖的关键目标是稠密的，而y方向则是稀疏的。因此通常在缺乏y方向定位目标的时候，需要引入其他插值算法进行弥补，常见的包括SLAM、速度计等。
图像数据+关键目标的检测和匹配方案存在的缺点包括：
关键目标的检测、识别、分割依赖于深度学习技术，而深度学习技术的稳定性有待检验和提高。
定位精度取决于成像的分辨率，而分辨率过高会影响计算速度，从而限制自定位的定位频率。
结语
现在的自定位大多基于高精度的差分GPS+惯导IMU+地面基站联合完成，其昂贵的成本直接限制了自动驾驶技术的应用和推广，因此基于高精度地图的精确车辆自定位技术将成为未来发展的关键。
现有的车辆自定位方案主要依靠摄像头、GPS、4/8线激光雷达进行数据的采集，通过深度学习、多视角几何等技术进行像素、关键点以及目标的匹配来完成精确自定位；稳定性有待确认和提高。如果能够对上述系列方案进行融合，我们应该能够完成更加精确、稳定的车辆自定位系统的设计。
