高精地图和定位在自动驾驶的应用
编辑整理：Hoh Xil
内容来源：Pony.ai & DataFun AI Talk
出品社区：DataFun
注：欢迎转载，转载请注明出处。
本次分享聚焦于高精地图在自动驾驶中的应用，主要分为以下两部分：
1. 高精地图 High Definition Map
拓扑地图 Topological Map / Road Graph
3D栅格地图 3D Grid Map
2. 定位 Localization
一． 高精地图 High Definition Map
★ 拓扑地图 Topological Map / Road Graph
1. 传统地图
我们先看下传统地图，这是从百度地图里截出来的图，从这张图我们可以看到很多信息：
拓扑信息：我们可以看到北清路的主干道，可以看到北清路可以延伸到哪些道路上去，这些拓扑信息从人的角度来看是能够识别出来的；
各类 POI：我们同时还能看到许多 POI 信息，包括中关村壹号-A1座，拉卡拉大厦等；
用户是人
米级精度：精度是米级别的，这对人来说足够了，因为人会自动判别周围的环境，做出自己的变化。
但是这些信息对于自动驾驶的车辆来说，到底是不是必要的、准确的或者足够的，那我们继续往下看。
2. 典型驾驶场景
这是一个典型的驾驶场景，假设我们正在驾驶的车辆是后面的白车，可以看到他的前面有辆自行车，那么对于白车来说，他可以有俩个决策：跟车或者变道。那么对于自动驾驶车辆来说, 我们作出决策的前提第一个是要知道全局的路线规划信息，变道后是否还能够到达终点，此外还需要知道很多具体的信息：
在哪条车道上以及当前车道的具体位置
是否有隔壁车道，如果没有车道你是变不过去的
是否允许变道：车道的类型，虚线实线，变道距离是否足够长
自动驾驶车辆还不能够像人一样做出各种复杂的动作，这时需要有一些先验的约束条件，这些信息的话都应该从地图中来。
3. HD Road Map
HD Road Graph 也就是高精地图其中的一部分,又叫做拓扑地图。HD Road Graph 以厘米级精度的描述道路细节，主要包含：车道类型、车道连通性、交通标注/交通灯、人行横道、道路几何特征、其他更多语义信息。大家可以看下左边这张图，对里面的每一条路，也就是路级别，我们已经精细到每条车道，包括直行的车道，左转的车道，也包括周边的一些几何信息，总体上你会发现高精地图上的信息要比传统地图大很多。
高精地图在自动驾驶里有很多应用，我们先看一个应用场景：在自动驾驶里有感知系统，那一个典型的应用是我们会在地图里面描述红绿灯的位置，自动驾驶车辆到达路口后，可以根据当前的位置，去正确的识别当前的红绿灯到底是红色还是绿色，左边我们可以看到是一个普通的场景，但是右边这种特殊的场景有几十个红绿灯(现实情况基本不存在, 只是用来举个例子)，对人来讲也很难去知道在哪个路口应该去看哪个红绿灯，那如果我们在离线地图上能够把上面的信息验证准确的话，对自动驾驶的安全性、可靠性来说是起到很大帮助的。
我们再来看下决策规划，刚才提到的一个典型场景，你需要知道一个全局的路径信息，包括变道所需要的必要信息，那对右边来说，这里主要是强调绿化带，我们这里提供的信息，前面有提到过叫做语义信息，我们为什么要在地图里包括这些语义信息呢？如果自动驾驶车辆可以提前知道旁边是绿化带，那么他可以帮助自动驾驶车辆做一些先验的决策，他可以知道在那个时间段不会有人或者车直接从对向车道穿过来。所以从准确性来讲他可以得到很大的提升，对性能来说也会有很大的提升，因为你不需要过多关心对面车道的物体，很明显你的计算就会少，你会发现整个先验信息对决策来说必不可少，并且是非常关键的。
我们再来看下我们当前整体用到的 Pipeline，分为以下几个环节：
数据收集，地图信息的采集
数据清洗/聚合
自动化识别，把地图里所需要的必要信息处理出来，包括车道线、红绿灯、十字路口等等各种信息
人工检查/标注，现有的算法我们还做不到100%的自动抽取，所以还会加入人工检查和标注的环节
后处理和验证，整个数据出来之后，我们需要有后处理和验证，因为数据出来之后怎么样去保证你提供的信息是准确的，这是我们需要解决的问题，因为不能单靠人或者高成本的方式来验证道路信息是否准确，假设一个红绿灯位置标错了，或者自动识别错了，那可能会导致你在路测过程中会出现一些安全隐患，所以整个验证环节占有很重要的部分
Release，整个验证完之后，我们会进行 Data Release 数据的发布，数据会纳入统一的管理中，每一版 release 的地图的质量已经得到比较好的验证
这是一个常规的发布流程。现实中整个路网结构是不断往外扩张的，且周围环境会有变化，所以我们也会有增量的更新。
前面简单介绍了整个 HD Road Graph 的 Pipeline，我们来看下 HD Road Graph 里面遇到的问题：
成本
厘米级别的精度：需要各类传感器（LiDAR、cameras、GNSS/IMU），右边是苹果的一个地图车，大家可以看到上面装了很多传感器，很明显制造一辆高精地图车辆的话成本非常昂贵
数据量：海量存储资源和计算资源
覆盖范围：从局部区域逐步扩张，从中关村壹号附近地区，扩张到海淀区，再到北京市，甚至是半个中国，这里面的硬件成本和人力成本等，都会极剧增加
那么如何去解决成本问题？这需要我们在工作中想一些好的方式。
挑战-复杂性
第二个是复杂性带来的挑战，对于我们整个 Data Model 来说会包含三部分信息：
语义信息，从右边的图标志牌来看，你会发现有很多的交通标记，如何提高准确性，并且尽可能提高覆盖度
空间信息，右图中的立交桥是西直门立交桥，我们会发现通过导航软件或者老司机的经验都未必能够在某一条车道上找到准确位置，对我们来说就是如何能够把整个空间信息描述好，然后集成到地图中去
时间信息，在北京有一些潮汐街道，早高峰和晚高峰走的方向是不一样的；对一些公交车道在早高峰的时候只能公交车去跑，自动驾驶车辆是不能跑的，很明显需要把这些信息包含在地图里面
这样的话，决策模块和感知模块才能去准确的使用。
挑战-扩展性
自动驾驶车辆行驶 1KM 会产生GB级别的原始数据，处理这些数据在功能架构上会面临很大的挑战
更新，从天级别到小时级别再到分钟级别的更新，整个功能架构是不一样的，比如我需要对北京市地图做小时级别的更新，到底能不能做到，这需要很多的优化功能在里面
挑战-实时性
交通管制、环境变化等怎么反馈到 HD Road Graph 上，如果今天某条路修路了，对于人来讲我就可以直接绕过这条路，而这种信息怎么去反馈到自动驾驶里呢？是靠人去反馈，还是系统自动去识别，即使反馈之后，怎么实时更新到地图数据里，我们需要打通 offline 到 online 的环节，及时更新到我们正在运行的自动驾驶车辆里去。
★ 3D栅格地图 3D Grid Map
1. 3D Grid Map
前面介绍了拓扑地图的部分，下面介绍我们的 3D 栅格地图（3D Grid Map）。
定义：我们认为一个真实的三维世界可以用一个概率模型来表示。比如像今天这个会场，我们会把他离散化成一个个立体方格，对每个方格里面我们会存储一个概率，这个概率是说这个空间不为空的概率。
输入：收集的激光点云数据
输出：3D 网格的占据概率
用途：
定位，这个稍后会讲
静态环境感知，减少工作量，专心识别动态环境
这是我们做的一个 3D 栅格地图，他并不是一个连续的描述，从数学的角度讲，已经把他离散化，我们会保留他的一些概率值和一些属性。展现出来的话，他是一个厘米级精度的栅格,从这个图我们可以很清楚看到周围的环境，包括一些树、建筑之类的。
2. SLAM
那么我们一般会怎么去做高精地图呢？在传统上有一个方式叫 SLAM，如果是做这个方向的同学会比较清楚，但是对于做工程的同学来讲这个概念可能是第一次接触。他是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM 的应用领域主要有机器人、虚拟现实和增强现实。其用途包括传感器自身的定位，以及后续的路径规划、场景理解。
我们来看下传统和自动驾驶 SLAM 的区别：
传统 SLAM：
2D
视觉传感器，单靠摄像头，在某个空间转来转去，然后通过一些算法来建模
没有 GPS 信息，很多场景在室内的话是收集不到 GPS 信息的
生成地图和定位同时进行
自动驾驶：
3D，他描绘的是一个三维环境
激光传感器，我们需要达到比较高的精度，所以我们一般采用激光传感器
GPS 信息，室外的话我们有个优势就是很多时候我们是有 GPS 信息的
离线处理，最后我们的数据不需要在线处理，前面介绍过，在线的话我们有一些场景算法是不能很好的处理，所以我们需要抽取一些信息来做离线的验证
相比于传统 Slam，我们来看下有哪些问题是需要解决的：
全局 GPS 信息并不总是好的，左下角的图，大家可以清楚的看到，这个是我们建出的底图，里面有当时车辆的一个行驶轨迹，你会发现如果根据 GPS 的行驶轨迹的话，车已经开到马路牙子（路肩）上了，跟实际的情况是不一样的，当你拿到这样一份数据，你在算法层需要做哪些优化，把细节做好，这是需要我们考虑的一个问题
不同时间段的数据如何 align（校准），因为整个地图的采集不可能是一次就完成的，你可能需要今天采集一次，明天再去采集一次，这就需要我们在算法层面上考虑如何把不同时间段的数据 align 到一起
去除非静态的物体，我们建的是一个概率的模型，对于很多非静态的物体我们是不需要保留的，我们只需要保留一些静态的物体给感知系统去使用，所以我们在处理完数据之后，需要把一些非静态的物体去掉
3. Pose Graph
整体上我们可以认为这是一个优化问题。如果我们把每一次采集的路线，按照时间去划分，比如每隔4分钟采一个点，我们可以得到不同的位置，举个例子：我们在大厅里走来走去，我现在站在这里，一分钟后我可能在那里，再绕一圈的话可能站在门口，我们把整个 GPS 得到的位置信息离散化之后，就可以得到一个个点，很明显我们把当时的点所看到的周围的环境直接拼起来，就可能得到一个三维的地图，因为每个点看到的角度都是不一样的，那我们把不同位置的点都拼起来就可以了。但是直接拼起来会有很多问题，因为原始位置可能跟真实位置有偏差, 所以我们把这些点离散化之后，希望能够把所有点都调整到准确的位置，那我们怎么去把最终的结果求正确？因为对整个图来说, 除了顶点之外，还有边，我们希望通过边的约束把点调好，边的约束你可以认为是个相对约束，假设我知道下个点的准确位置，那么我就可以倒推我这个点的准确位置。就是在有准确的相对位置的基础上，加上一些比较少的绝对位置，我们直接可以得到一个全局的准确位置。
优化目标：我们把整个图离散化到一个点之后，我们会尝试建立一些边的约束，最终我们是希望通过优化一些点的位置来满足边的约束，最终会转化为一个优化问题。
整体的数学模型看起来是比较简单的，但是在整个细节里面是存在很多问题需要去做优化的：
如何选择顶点，比如是不是毫秒的点都要加进去，GPS 信号不好时的点要不要加进去
如何保证边的准确性，一个顶点到另一个顶点的相对位置，我要通过什么方式去建立
如何求解非线性优化，图建好之后如何去做非线性的优化
如何评估优化效果，是否和真实的环境是一致的呢？如何评估我们的结果满足要求
效率和资源，对于工程来讲，数据量这么大，怎么去设计工程算法，能够保证我们的效率和资源，比如说，是不是需要在算法基础之上去做一些定制优化，用 GPU 或者分布式的计算方式，这都是做工程时需要考虑的问题
上图右边其实是一个例子，左边是原始的图，有一个物体来回在球面上运动旋转，边与边之间我们已经建立相对约束，最终优化完之后，你会发现整个环境的重建就变成了一个比较理想、完整的球体，概括来说我们要做的事情就是需要把左边的图来变成右边的图。
4. ICP
我们前面提到我们要保证边的约束，那我们怎么知道求出这个约束，业界用的比较多的传统的经典算法叫做 ICP（Iterative Closest Point algorithm）迭代最近点算法。
举个例子：左边是一个红色的兔子，右边是一个蓝色的兔子，现实中这俩个兔子的位置是一样的，但现在俩个兔子是不一样的，说明位置存在偏差。他的输入是俩组点云 A 和 B，输出是俩组点云之间在空间上的旋转和平移。
具体算法的思路大致是：找点云集合 A 中的每个点在 B 中的对应点，通过求解最佳的刚体变换，不断的迭代优化，最终得到一个收敛解。
5. 去除非静态障碍物体
概率模型
点云处理，单纯的概率模型是不够的，比如有时候小区门口经常是有车的，但是在某时刻他是开走的，只是恰好在我们采集数据的时候这些车都在，所以我们需要依赖离线点云处理
机器学习，同时也可以利用机器学习算法去把非静态的物体从地图里面抠出来
这是一个简单的对比图，没有做处理的话，你会发现右边是有一些车的轮廓，当从空间上去除之后，会变得比较清晰。
二． 定位 Localization
接下来简单介绍下定位，定位大家都比较熟悉，对于定位系统和传统定位来说，他的位置和姿态有如下特点：
厘米级别的精度，需要精确到车在车道线的具体位置离左右边线几公分
高频低延迟，需要毫秒级别的时间延迟，告诉我的车在什么地方，否则会出现安全事故
定位系统的几种定位方式：
全球导航卫星系统 Global Navigation Satellite System
特征：
米级别精度
低频
干扰或者遮挡
解决方法：RTK（Real - time kinematic，实时动态）载波相位差分技术，可以达到厘米级别。
相对定位：惯性测量单元 IMU（Inertial Measurement Unit）
特点：
加速度以及角速度
三个方向的加速度计和三轴的陀螺仪
高频（通常100HZ）
累积误差
解决方法：惯性导航系统 Inertial Navigation System
点云定位
特点：
鲁棒性好，不依赖于 GPS 信息
依赖于先验地图，那么这个地图的准确性和实时性会导致一个定位误差
弱特征环境？有些弱特征环境是需要解决的，比如说在桥上，往前开10米和往后开10米看到的场景都是类似的，对于这类环境来说，我们如何去找到准确位置，这是需要考虑的一个问题
综合来说，我们单靠单一传感器是不够的，所以采取的是多传感器融合的方案，整体效果是会有更高的精度和更好的鲁棒性，然后我们整体上是基于卡尔曼滤波模型。
简单介绍下卡尔曼滤波（Kalman Filter）模型：
卡尔曼滤波是一种高效率的递归滤波器，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。
我们把整个当时的位置认为是高斯分布，对这个小车来说他大概在某个范围之内，在中间的可能性是最大的，然后我们会有个预测（预测是根据自己的运动方程），比如我往前走了10米，他大概在这个位置，你会发现他的误差值会变大，因为可能是 IMU 告诉你走了10米，但是传感器本身也是有误差的，那同时假设我有个 GPS，告诉我可能是在蓝色的部分，他也是包含一定误差的，这样我其实有了俩个位置，那么我就可以把俩者合在一起得到一个比较准确的可信的结果，整体可以认为卡尔曼滤波是不断的在更新迭代的，来最终得到一个比较准确的位置。
最终我们定位系统的目标是万里无忧，对我们的挑战是更低成本和更苛刻的外部环境。比如右边这个图，大堵车，这时候可能 GPS 不行，堵车堵了几个小时，走走停停，会导致累积误差，实时讯号又不好，你可能搜不到，这样的环境你会发现，他只能看到周围的很多车，也没办法跟周围的环境去做对比，知道自己在哪里，这种情况下，我们到底怎么去得到一个比较准确的定位效果，这需要我们把这些问题都解决好。