百度Apollo高精度定位解决方案解析

总结

自动驾驶已经成为一个非常热门的话题。但事实上，当美国斯坦福大学在美国国防部举办的无人驾驶挑战赛中获得第一名时，自动驾驶时代已经到来。自动驾驶系统分为很多部分。从传感器的感知到后续的识别、定位、决策，再到控制系统，涉及到很多技术。高精度定位是自动驾驶非常重要的一环。

自动驾驶技术的核心之一是高精度定位。因为车辆想要在道路上实现自动驾驶，车辆本身的位置需要达到厘米级的定位精度。高精度定位的精度会受到传感器噪声、卫星导航多径传播效应、车辆自定位问题和车辆动力学问题等因素的影响。每一个小问题都会导致以后错误的积累。当误差积累到一定程度时，最终得到的结果就不再可信。即使使用的传感器是最好的，在实践中也会受到很多方面的影响。所以我们还需要使用额外的算法来辅助车辆获取高精度位置信息。这涉及到卡尔曼滤波、扩展卡尔曼滤波、无损卡尔曼滤波、粒子滤波器等等。

本文对百度Apollo的高精度定位方案进行分析，通过查看百度Apollo的Github上的定位模块代码来分析Apollo是如何实现L4级高精度定位的。

关于高精度定位

自动驾驶已经发展多年。自动驾驶是未来出行的解决方案。许多主要汽车制造商和互联网公司都在研究如何开发安全舒适的自动驾驶汽车。目前的自动驾驶系统框架包括感知、定位、决策、路径规划、控制、高精度地图、云计算等核心内容。从层次上来说，定位模块与其他模块具有层次关系。感知属于最底层，定位是感知模块的上层。后续的控制和决策必须依靠定位模块进行计算。

这里简单分析一下定位与其他模块的关系。

感知层通过传感器感知物理世界。传感器根据一定的数据对物理世界进行编码，传输给感知模块。然后感知模块可以通过相关算法提取物理世界模型。不仅如此，感知还可以提供检测、分类等功能。定位模块利用感知模块的传感器来确定车辆在世界坐标系中的位置。定位模块使用的核心传感器有RTK GPS、GNSS、IMU、LIDAR、Camera等。

定位模块利用这些数据通过定位算法输出车辆在全局坐标系中的位置。规划决策模块利用感知、定位、高精度地图等数据进行综合决策。最终决策层的输出将被发送到控制层，然后通过控制层对车辆上的执行器进行线控驱动。云端还将实时接收车辆上的所有数据。云端确定车辆位置后，将利用云端的计算能力和数据库为车辆提供更好的驾驶能力。

理想条件下，定位模块可以利用RTK GPS系统对无人车进行导航定位。 GPS系统通过计算电磁波的传播时间来确定车辆的位置。但现实中，电磁波的传播会因为大气、高层建筑、天气等因素造成时间误差。时间误差意味着计算出的距离数据不够准确。距离不够准确意味着我们无法准确确定自动驾驶车辆的位置。无人车在城市道路上的定位要求是误差小于50cm。除了这些数据之外，来自定位模块的数据不能用于自动驾驶。为了解决这些噪声问题和单个传感器精度不足的问题，研究人员通过融合多个传感器来最大程度地解决误差问题。

下面分析一下百度Apollo平台的高精度定位解决方案。

百度Apollo高精度定位解决方案

首先我们来分析一下百度Apollo自动驾驶平台的硬件架构。其中包括激光雷达、毫米波雷达、摄像头、GPS系统、工业计算机和IMU（惯性测量单元）。

图1 百度Apollo自动驾驶平台硬件配置

这里的大多数传感器都可以用于定位。例如，激光雷达可以与高精度地图结合，实现特征匹配和定位。摄像头可以结合高精度地图进行特征匹配，实现定位。通过GPS可以实现定位，通过IMU可以实现定位，GPS+IMU可以实现定位，GPS+IMU+激光雷达+雷达也可以实现定位。即可实现定位。 Apollo平台是以IMU+RTK GPS为主，其他定位方式为辅的定位模式。

事实上，除了这些交叉定位之外，还有另一种定位方法，即利用街景来定位车辆[1]。 Waymo 利用 Google 强大的街景数据来离线定义街道标志号码、商店标志、交通信号灯等静态数据，以确定其车辆的位置。总体思路是，车辆本身只要知道了摄像头识别的商店牌匾、红绿灯等特征的全局坐标系，就可以通过透视原理或者相对距离知道自己车辆的全局坐标。这种方法简单直接，但涉及的问题是数据量巨大且需要人工标注。这种方法的优点是，如果地图有标注，那么车辆本身就有摄像头，可以实现准确的定位导航。

但即使地图标记得很好，也存在一些问题。这就是基于相机的图像识别的问题。由于相机通过采集光线来采集数据，因此会受到强光的影响。因此，随着光线强度的变化，相机的识别精度会输出不稳定的识别结果。如果识别的图像不够准确和稳定，那么根据识别结果推导出的位置计算将不够准确。而基于图像的深度数据提取能力也值得进一步研究。毕竟，在从2D转换为3D的过程中，很容易出现相关的数据噪声和丢失。不仅如此，在高速公路等场景中，周围没有特殊的可识别特征可以进行定位。因此，Waymo 的做法或许提供了新的思路，但就目前来看，无人驾驶汽车定位仍然依靠 RTK GPS 的帮助来完成精确定位。

图2 Waymo基于街景标志识别的定位方法

可以看到定位的方法有很多种。但问题是哪种方法能够实现高精度和高鲁棒性。所以在目前的技术框架下，主流还是采用RTK GPS+IMU进行精准定位。为什么不使用其他东西呢？因为准确性和鲁棒性。在半自动驾驶的L3阶段和完全自动驾驶的L4阶段，如果车辆丢失位置信息或者获得错误的位置信息，那么车辆本身做出的决策从根本上就是错误的。当无法确定自己的位置时，变道、跟随前车、规划路径只会导致错误的结果。不仅如此，由于L4车辆对车辆拥有完全的控制权，丢失位置信息的车辆可能会造成严重事故。因此，对于全自动驾驶汽车来说，准确定位固然重要，但更重要的是鲁棒性更重要。否则，无人车根本无法判断当前位置信息的置信度，从而无法输出正确的结果。

目前的无人车定位技术框架大多采用RTK GPS+IMU方式进行主定位，其他方式进行辅助定位。那么为什么RTK GPS而不是普通GPS呢？因为普通GPS是通过计算与卫星的三角距离来确定自己的位置的。这里最重要的是波传播的速度。只要电波传播速度理想，使用最基本的GPS就没有问题。问题是人造卫星发射的电波会受到大气层、卫星轨道、卫星时钟误差和多路径的影响。因此，单纯的GPS无法提供准确的位置信息。而RTK则首先通过建立基站找到载波偏差，然后GPS处理器通过这个偏差来修正接收到的数据。但到目前为止，这种方案的问题是需要建立大量基站来覆盖无人车的运动范围。不仅如此，建设基站所需的设备和人力也是一个大问题。但就目前来看，该方案的鲁棒性和准确性都非常好，因此被广泛应用于无人车的定位算法中。

但即使基站建成、覆盖范围足够广，仍然会存在一些场景无法满足定位精度要求。例如，在树荫下行驶、在隧道中行驶、在极端环境下行驶等都会导致RTK GPS失去精度。然而，自动驾驶汽车将不可避免地在这种环境下行驶。因此，我们需要额外的传感器或算法来协助我们的自动驾驶车辆。 Apollo自动驾驶系统提供的解决方案包括激光雷达定位、摄像头定位、多传感器融合定位等。

▼下面对这些算法进行分析▼1 基于激光雷达定位

激光雷达可以准确获取车辆与周围环境的相对距离信息。同时，激光雷达还可以辅助相关算法提供基于点运动数据的物体识别。现在学术界普遍采用摄像头、激光雷达、IMU等传感器进行融合。融合的一般输出是飞行器的位置和相应的姿态状态。 Hanieh Deilamsalehy 和 Timothy C. Havens 提出的理论是利用 IMU、摄像头和激光雷达来估计飞行器或无人机的三维姿态[2]。这里没有使用车辆动力学方程，而是通过上述三个传感器来估计车辆的姿态。这里涉及到的问题是如何整合2D和3D数据。 Hyungjin Kim、Bingbing Liu 和 Hyun Myung 也进行了关于如何匹配点云地图和点云特征来实现车辆定位的研究[3]。具体过程是提前采集点云信息和激光发射信息构建激光雷达地图，然后将车辆上的激光雷达传感器接收到的实时数据与激光雷达地图进行比较，从而获得车辆的相对位置。

图3 亮度图和预标记特征

图4 实时匹配特征，提前定位车辆

Apollo的激光雷达定位解决方案与文献[3]非常相似。通过预先收集的定位图、反射值图和高度值图，可以最大程度地收集物理世界的特征。然后，车辆上的传感器将其获取的数据与来自云端的数据实时匹配。在匹配过程中，所有数据都是数值，因此也可以看作是一个优化问题。只要定义了成本函数，使成本函数最小的点就是车辆最可能的位置。

图5 提前收集物理世界数据

图6 找到Cost function最小的点

2基于相机定位

基于相机的定位方法与基于激光雷达的定位方法非常相似。但激光雷达本身会收集3D数据，因此可以直接对车辆进行3D定位。相对而言，摄像头的问题在于，它采集的数据是2D数据，所以要找到车辆的位置，需要距离等信息。因此，利用摄像头定位的研究一般是通过几何方法投影2D图像，然后利用投影变换后的3D数据来获取本车与周围物体的距离信息，从而实现自车定位。

所谓深度相机，也是利用算法来补充相机只能提取2D数据的缺陷。但这种方法在从2D到3D的转变过程中往往会丢失大量的位置信息，因此这种方法的精度也有待提高。单纯使用摄像头对车辆进行高精度定位是不合适的选择。相机的主要功能仍然是物体识别和物体跟踪。不仅如此，相机还涉及到校准、校正、畸变等步骤，每一个步骤都会产生相应的噪声。不过，由于该摄像头价格便宜，预计在某些场景下可用于定位。

3基于IMU和RTK GPS的一体化导航定位

RTK GPS本身因为有基站的缘故，可以为无人车提供非常高的定位精度。抛开价格和部署密度不谈，还需要考虑的是RTK GPS的定位更新频率和定位精度。如果 RTK GPS 可以（假设）100hz 频率更新，那么自动驾驶汽车就不需要额外的设备。但事实上，自动驾驶汽车的GPS只能以（假设）10hz的频率更新。但 0.1 秒对于车辆来说太长了。在采样时间的0.1s内，无人车只能知道t-1时刻的位置数据。对于无人车来说，它的定位是必须在每个时间点进行更新，以保证车辆的安全。所以还需要IMU（惯性测量单元）。

IMU使用陀螺仪和加速度计来测量车辆的姿态数据。为什么需要姿势数据？因为我们可以通过卡尔曼滤波器、粒子滤波器等融合算法对姿态数据进行转换，最终可以估计出车辆在100hz频率下的位置信息。也就是说，在t时刻到t+0.1时刻，IMU会通过算法提供车辆的实时位置信息。这种方法称为组合导航系统。通过融合多个系统预测值和观测值，可以获得更准确、更快速的位置数据输出。它的优点是超越了单个子系统的性能，弥补了各个单个子系统的缺点。并且由于多个系统共同输出位置数据，当某个系统出现故障时，其他子系统也可以作为冗余备份系统进行定位。这种方法还提高了数据的稳健性。

图7 百度Apollo组合导航系统

定位模块代码分析

下面对Apollo定位模块的代码进行分析。 Apollo在Github上的代码如下所示。在进行分析定位之前，我们需要使用Calibration等模块。不过这里我们先抛开其他一切，只分析定位相关的代码结构。

图8 百度Apollo Github库

图9 Module

使用的坐标系是 ECEF-WGS84 coordinate

RTK模块有Status RTKLocalization::Start()、RTKLocalization::Stop()、RTKLocalization::OnTimer()、RTKLocalization::InterpolateIMU()、RTKLocalization::PrepareLocalizationMsg()等函数。这里可以看到百度采用RTK+GPS来定位车辆

你可以在Localization的msf的common文件中看到velodyne_utility文件。并且公用文件中还有一个本地文件夹。这是百度通过激光雷达与当地高精度地图匹配来获取定位数据的代码。这里的功能包括2D矩形标注、时间戳、点云坐标变换、体素相关头文件

Params提供GNSS参数、Velodyne激光雷达等设备参数调节

MSF还提供了一些相关的测试数据。

百度Apollo的github并没有提供如何融合这些传感器的代码，所以看不到更核心的内容。不过核心代码不公开也是合理的。

总结

定位本身就是一个难题。因为定位还涉及到很多特征提取问题，所以定位和感知是绑定在一起的。如果感知不好，定位精度也不会好。同时，噪声对定位的影响也很多，如GPS时钟误差、多径问题、遮挡问题、IMU误差累积等。基于激光雷达和摄像头的定位很大程度上依赖于离线生成的高精度地图。高精度地图的制作和RTK基站的部署耗时、耗力、成本高，给自动驾驶的普及带来了很大的困扰。 Apollo系统目前基于RTK GPS + IMU，其他定位方式均为辅助定位方式。当然，还需要依赖高精度地图。当前的解决方案不适合推广。不过，相信随着技术的发展，Apollo平台能够提供更好的定位算法和可执行的解决方案。