高精定位模块分析
北京大学自动驾驶课程大作业
摘要
自动驾驶变成了一个非常热门的话题。但实际上，在美国国防部组织的无人驾驶挑战赛上美国的斯坦福大学获得第一名的时候开始自动驾驶的时代已经到来了。自动驾驶的系统，它分为很多部分。从传感器的感知到之后的识别,定位,决策，再到之后的控制系统，涉及到很多的技术。其中高精定位是自动驾驶非常重要的一个部分。自动驾驶技术核心之一就是它的高精定位。因为车辆想在道路上好好的行驶需要对自身的位置有厘米级的定位精度。只有这样才能达到无人驾驶的技术要求。高精定位的精度会受到传感器的噪声，卫星导航的多路径传播效应，车辆自身定位问题，车辆动力学问题等等的影响。每一个小问题都会导致之后误差积累。误差积累到一定程度,得到的最终结果就不可信。就算所有传感器都是最好的，但实际上也会受到很多影响。所以我们还需要用额外的算法去辅助车辆获得高精度的位置信息。这里面涉及到的就是卡尔曼滤波，扩展卡尔曼滤波，无损卡尔曼滤波,还有粒子滤波器等等。本文通过对百度阿波罗的高精定位方案进行分析，并通过查看百度阿波罗的github上的定位模块代码，分析阿波罗是如何达到L4级别的高精定位。
介绍
自动驾驶已经发展了很多年了。自动驾驶作为未来出行解决方案，很多大车厂和互联网公司都在研究如何去开发安全舒适的自动驾驶车辆。现阶段的自动驾驶系统框架有感知，定位，决策，路径规划，控制，高精地图，云计算等核心内容。层级上来说，定位模块和其他模块有上下层级关系。感知属于最底层，定位就是感知模块的上一层。之后的控制，决策都要依赖定位模块进行运算。
这里简单分析一下定位和其他模块的关系。
感知层是通过传感器感知物理世界。传感器把物理世界按照一定的数据编码好，传送给感知模块。那么感知模块就可以通过相关的算法提取物理世界模型。不仅如此，感知还可以提供detection，classification等功能。定位模块就是利用感知模块的传感器来确定车辆在世界坐标系里面的位置。定位模块用到的核心传感器是RTK GPS, GNSS, IMU，LIDAR，Camera等。定位模块利用这些数据，通过定位算法输出车辆在全球坐标系里面的位置。规划决策模块利用感知，定位，高精地图等数据进行整合和决策。最终决策层的output会发送到控制层，然后通过控制层对车辆上的执行器进行drive by wire控制。云端也会实时的接收车辆上的所有数据。云端确定车辆位置以后，会利用云端上的计算能力和数据库为车辆提供更好的驾驶能力。
理想条件下，定位模块利用RTK GPS系统就可以对无人车进行导航及定位。GPS系统是通过计算电磁波的传播时间来判断车辆的位置。但是现实中电磁波的传播会因为大气层，高楼大厦，天气等因素产生时间误差。时间误差就意味着计算的距离数据不够准确。不够准确的距离就导致了我们根本不能确定我们的无人车到底在哪个位置。无人车在城市道路中的定位要求是误差小于50cm，超出这个数据，定位模块的数据就不能用来做自动驾驶。所以这些噪声问题和单个传感器不够精准的问题，研究人员就通过融合多个传感器，最大限度的解决误差问题。下面分析一下百度阿波罗平台的高精定位解决方案。
百度阿波罗高精定位解决方案
首先分析一下百度阿波罗自动驾驶平台的硬件架构。其中有激光雷达，毫米波雷达，摄像机,GPS系统，工业电脑，IMU(惯性测量单元)。
Fig 1 百度阿波罗自动驾驶平台硬件配置
这里所有的传感器大部分都可以用来做定位。比如激光雷达可以结合高精地图实现特征匹配定位，摄像机可以结合高精地图进行特征匹配从而实现定位，通过GPS实现定位，通过IMU实现定位，GPS+IMU实现定位，GPS+IMU+Lidar+Radar也可以实现定位。阿波罗平台是IMU+RTK GPS为主，其他定位方法为辅的定位模式。
其实除了这些交叉定位以外，还有一种定位方式是利用街景给车辆定位[1].Waymo利用谷歌强大的街景数据线下定义出路牌号码，商店标志，红路灯等等静态数据来确定自身车辆所在的位置。大体的思路就是，只要车辆本身知道摄像机识别到的商店牌匾，红绿灯和其他特征的全球坐标系的话，通过透视原理或者相对距离就可以知道自身车辆的全球坐标。这种方法简单又直接，但是涉及到的问题是巨大的数据量和人工标注。这种方法的优点是如果地图都标记好了，那么车辆本身有个摄像机就可以实现精准的定位和导航。不过就算地图标注好了，但是有一些问题。那就是基于摄像机的图像识别问题。摄像机由于是通过采集光线的方式收集数据，所以会受到强光影响。因此摄像机的识别精度会随着光照强度的变化输出不够稳定的识别结果。如果识别到的图像不够精确，稳定，那么基于这个识别结果导出的位置计算也是不够精准的。且基于图像的深度数据提取的能力也是值得深入考究的。毕竟从2D转化到3D的过程中，很容易发生相关的数据噪声和损失。不仅如此，像在高速公路等场景。，周边没有特别的可识别的特征供定位。所以，waymo的这种方式或许可以提供一种新的思路，但暂时无人车定位还是要依赖RTK GPS的帮助来完成精准定位。
Fig 2 Waymo基于街景路牌识别的定位方式
可以看到定位的方法有很多种。但是问题就是哪种方法可以达到很高的精度且鲁棒性还高呢。那么现在的技术框架下，主流的还是在用RTK GPS+ IMU的方式进行精准定位。为什么不用其他的？因为准确度和鲁棒性。在半自动驾驶L3,和完全自动驾驶的L4阶段，如果车辆失去了位置信息或者得到了错误的位置信息，那么车辆所做出的决策本身从根源就是错误的。自身位置都确定不了的情况下，变道，跟随前车，路径规划都只会得到错误的结果。不仅如此，L4级别的车辆因为有对车辆的完全控制权，所以失去位置信息的车辆可能会导致严重的事故。所以对于完全自动驾驶的无人车来说，定位的精准固然重要，但是更重要的鲁棒性要好。否则，无人车根本不能判断现在的位置信息的置信度，从而也无法输出正确的结果。
现在大部分的无人车定位技术框架是用RTK GPS+IMU的方法进行主定位，其他方式辅助定位。那么为什么是RTK GPS 而不是 普通的GPS呢？因为普通的GPS是通过计算和卫星的三角距离来确定自己的位置的。这里最重要的就是波的传播速度。只要波的传播速度是理想状态，那么用最基本的GPS也是没什么问题的。问题就在于波从人造卫星发出，会受到大气层，卫星轨道，卫星钟差和多路径等影响。所以单纯的GPS不能提供准确的位置信息。而RTK则是通过建立基站的方式先找出载波的偏差，然后GPS的处理器再通过这个偏差来修正接收的数据。但是目前为止，这个方案的问题是需要建立大量的基站，才能覆盖无人车的移动范围。不仅如此基站建立的设备，人力昂贵也是一大问题。不过，暂时这个方案的鲁棒性和准确性很好，所以广泛应用在无人车的定位算法上。
但是就算基站建好了，覆盖的范围够广阔了，但是还有会有一些场景不能达到定位精度要求。比如树荫下行驶，隧道里行驶，极端环境下行驶都会让RTK GPS失去精准性。然而无人车却不可避免的在这种环境下行驶。所以我们需要额外的传感器或者算法去辅助我们的无人车。阿波罗自动驾驶系统所提供的解决方案有激光雷达定位，摄像机定位，多传感器融合定位。下面分别对这些算法进行分析。
基于激光雷达定位
激光雷达可以精确的获取车辆和周边环境的相对距离信息。同时激光雷达还可以辅助相关的算法可提供基于点运数据的物体识别。现在学术界普遍利用摄像机，激光雷达，IMU等传感器进行融合。一般融合的输出是车辆的位置和相应的姿态状态。 Hanieh Deilamsalehy，Timothy C. Havens提出的理论就是利用IMU，摄像机和激光雷达对车辆或者无人机的三维姿态进行估算[2]。这里不使用车辆动力学方程，只是通过上述三个传感器对车辆的姿态进行估算。这里涉及到的问题就是，怎么把2D和3D的数据融合在一起。还有Hyungjin Kim, Bingbing Liu, and Hyun Myung进行的研究是如何匹配点云地图和点云特征来实现车辆定位[3]。具体流程是，事先采集点云信息和激光放射信息构建激光雷达地图，然后通过车辆上的激光雷达传感器实时接收的数据与激光雷达地图比较，得到自身车辆的相对位置。
Fig 3亮度图谱和事先标记的特征
Fig 4 实时匹配特征，从而事先对自车定位
百度的激光雷达定位方案跟文献[3]很类似。通过事先采集的定位地图，反射值地图和高度值地图，最大限度的搜集物理世界的特征。然后通过车辆上的传感器实时匹配自身获取的数据和来自云端的数据。在匹配过程中，所有数据都是数值，所以也可以看做是optimization问题。只要定义好cost function，那么让cost function最小的点就是车辆最可能在的位置。
Fig 5 事先采集物理世界数据
Fig 6求Cost function最小的点
基于摄像机定位
基于摄像机的定位方法和基于激光雷达定位的方法很像。但是激光雷达采集的本身就是3D数据，所以他可以对车辆直接进行3D位置定位。相对来说，摄像机的问题就是它本身采集的数据是2D数据，那么位置找到车辆的位置，就需要距离等信息。所以利用摄像机定位的研究一般是把2D的图像通过几何方法进行投影变化，之后再利用投影变换过后的3D数据获得自身车辆与周边物体的距离信息，从而实现自车定位。所谓的深度摄像机也是通过算法补充摄像机只能提取2D数据的缺陷。不过这种方法往往因为2D到3D变化的过程中会损失不少位置信息，所以这种方法的准确性也有待提高。单纯的利用摄像机对车辆进行高精定位是不合适的选择。摄像机的主要功能还是在物体识别和物体追踪上。不仅如此，摄像机还涉及到标定，校正，扭曲等步骤，每一个步骤都会导致相应的噪声。然而因为摄像机的价格便宜的原因，预计会在一些场景中可以用来做定位。
基于IMU和RTK GPS的组合导航定位
RTK GPS本身因为有基站的原因，可以为无人车提供相当高的定位精度。抛开价格和部署密度，还需要考虑的是RTK GPS的定位更新频率和定位精度。如果说，RTK GPS可以以（假设）100hz的频率更新，那么无人车则不需要额外的设备。但是实际上无人车的GPS只能以（假设）10hz的频率更新。但是0.1s对于车辆来说太长了。采样时间里0.1s里，无人车能知道的只有t-1时间的位置数据。对于无人车来说，他的定位是要每个时间点都要更新才能保证车辆安全。所以还需要IMU(惯性测量单元)。IMU是利用陀螺仪和加速度计来测量车辆的姿态数据的。为什么需要姿态数据？因为我们可以通过卡尔曼滤波和粒子滤波器等融合算法对姿态数据进行转换，最终可以以100hz的频率估计出车辆的位置信息。也就是说时间t到时间t+0.1的时间里，IMU会通过算法提供车辆的实时位置信息。这种方式就叫做组合导航系统。他可以通过融合多个系统预测值，观测值来获取更精准，更快速的位置数据输出。他的优点是超越了单个子系统的性能，并且弥补各个单一子系统的缺点。而且因为是多个系统共同输出位置数据，所以当某个系统出现故障的时候，其他子系统也可以作为冗余备份系统进行定位。这种方式也提高了数据的鲁棒性。
Fig 7 百度阿波罗自动驾驶系统组合导航系统
百度阿波罗自动驾驶定位模块代码分析
这里分析一下，阿波罗的定位模块的代码。阿波罗的github代码如下图。分析定位之前calibration等模块我们都是要用到的。但是这里先撇开其他所有东西，我们只分析一下定位相关的的代码结构。因为核心代码模块好像是不开放的。
Fig 8 百度阿波罗github库
Fig 9 Module里面的localization模块
用到的坐标系是ECEF-WGS84 coordinate
RTK模块有Status RTKLocalization::Start（）， RTKLocalization::Stop（）， RTKLocalization::OnTimer（）， RTKLocalization::InterpolateIMU（）， RTKLocalization::PrepareLocalizationMsg（）等等函数。这里就可以看出，百度是利用RTK+GPS的方式对车辆进行定位的
Localization的msf的common文件里可以看到有velodyne_ultility文件。且在common文件里面也有local文件夹。这就是百度通过激光雷达和本地高精地图匹配来获得定位数据的代码。这里的函数包括2D矩形标注，timestamp，点云坐标变换，voxel相关的头文件
Params里面提供了GNSS的参数，velodyne激光雷达等设备的参数调校
Msf里面也提供了一些相关测试数据。
百度阿波罗的github里面并没有提供如何融合这些传感器的代码，所以看不到更核心的内容。不过作为核心代码不开放也是合理的。
总结
定位本身是比较难的问题。因为定位还涉及到很多特征提取问题，所以定位和感知是绑定在一起的。感知做的不好，那么定位的精准度也不会好。同时定位还有很多噪声影响，比如GPS的钟差问题，多路径问题，遮挡问题，比如IMU的误差积累问题。基于激光雷达和摄像机的定位又严重依赖线下制作的高精地图。高精地图的制作和RTK基站的部署又很耗时耗力耗财，这些都为自动驾驶的普及带来了很多麻烦。阿波罗系统现在是RTK GPS + IMU为主，其他定位方式为辅助的方式进行定位。当然还要依赖高精地图。现阶段的解决方案还不适合普及。不过相信随着技术的发展，阿波罗平台可以提供更好的定位算法和可执行的解决方案。
参考文献
[1]Sacha Arnoud, Director of Engineering, Waymo, https://www.youtube.com/watch?v=LSX3qdy0dFg
[2] Hanieh Deilamsalehy, Timothy C. Havens, Sensor Fused Three-dimensional Localization Using IMU, Camera and LiDAR
[3] Hyungjin Kim, Bingbing Liu, and Hyun Myung, Senior Member, IEEE，Road-Feature Extraction using Point Cloud and 3D LiDAR Sensor for Vehicle Localization
谢谢支持，各位看官的关注就是持续更新的动力~
看完就别吝啬点赞加关注啦~
同时也希望朋友往咱们专栏投稿，让我们在无人车算法的造诣上不停的成长~！