自动驾驶高精地图与定位

首页发布

「Pony Tech」是小马智行Pony.ai推出的深度技术科普专栏，由专业的工程师总结归纳出自动驾驶领域的技术实践及研究成果。本文主题为自动驾驶高精地图与定位，主要内容包括：

» 高精地图之拓扑地图的应用

» 高精地图之3D栅格地图的应用

» 自动驾驶车辆如何定位

1 高精度地图之拓扑地图的应用

传统拓扑地图

我们先了解下传统拓扑地图，这是从地图app里截出来的图，从这张图我们可以得到很多信息：

◆ 拓扑信息：我们可以从地图里辨认出北清路的主干道，以及了解到北清路能延伸向哪些其他道路等，这些都是人们可辨认的拓扑信息。

◆ 各类 POI（信息点）：我们能看到许多 POI 信息，包括中关村壹号-A1座，拉卡拉大厦等。

◆ 地图的用户是人。

◆ 米级精度：地图精度是米级别的，对于人类使用这已经足够了，因为人会自觉判别周围的环境，做出自己的变化。

但是这些信息对于自动驾驶的车辆来说，是不是必要的、准确的或者足够的呢？

一个自动驾驶典型场景：

上图展示了一个典型的驾驶场景——假设我们正在驾驶的车辆是后面的白车，可以看到我们前方有辆自行车，那么对于白车而言，可以有两个决策：跟车或者变道。

对于自动驾驶车辆来说, 它作出变道决策需要知道很多具体的信息：

◆ 在哪条车道上以及当前车道的具体位置。

◆ 是否有隔壁车道，如果没有车道你是变不过去的。

◆ 是否允许变道，车道的类型，车道线是虚线还是实线，变道距离是否足够长，变道后是否能够达到终点。

自动驾驶车辆不能像人一样做出各种复杂的动作，这时就需要有一些先验的约束条件，那么这些先验信息都可以从地图中来。

HD Road Graph如何应用于自动驾驶

HD Road Graph （高精道路图）也是高精地图的一部分，又叫做拓扑地图。HD Road Graph 以厘米级精度来描述道路细节，主要包含：车道类型、车道连通性、交通标注/交通灯、人行横道、道路几何特征、其他语义信息。

大家可以看一下上图左侧，每一条路都已精细到车道，包括直行或左转的车道，当然也包括周边的一些几何信息。总体上你会发现高精地图上的信息要比传统地图大很多。

感知系统+高精地图

高精地图在自动驾驶里有很多应用，首先是与感知系统相结合。例如，高精地图里会描述红绿灯所在位置，当自动驾驶车辆到达路口后，可以根据当前的位置，更加准确地识别前方的红绿灯。

上图左边是一个普通的道路场景，而右边则是安装有几十盏红绿灯的特殊场景（其实现实情况基本不存在），在这种极端情况下，人类都很难分辨在哪个路口对应哪盏红绿灯。但如果前期在离线地图上就能把这个对应信息验证准确，无疑将对自动驾驶的安全性、可靠性起到很大帮助。

决策规划+高精地图

我们再来看下高精地图在决策规划方面的应用。前面我提到了一个典型驾驶场景，以及在此情况下，自动驾驶车辆的行驶需要得到一个全局的路径信息以及变道所需要的信息。

上图右侧的地图强调了绿化带，这其实为自动驾驶提供了语义信息。而地图里包含的语义信息将帮助自动驾驶做出更好的行驶决策。

还是以绿化带为例，当自动驾驶车辆能够提前知道旁边是绿化带，它便可以做一些先验的决策，例如可以提前判断在行驶中是否有行人、车辆直接从对向车道穿过来。这不仅提升了决策的准确性，也使得系统性能得到提升，因为自动驾驶车辆此时不需要过多关心对面车道的物体，计算量也因此减少。通过上面的例子，你会发现先验信息对自动驾驶决策来说必不可少，并且非常关键。

高精地图生成与发布

当前高精地图生成发布用到的流程（Pipeline）一般分为以下几个环节：

◆ 数据收集，地图信息的采集。

◆ 数据清洗/聚合。

◆ 自动化识别，把地图里的必要信息处理出来，包括车道线、红绿灯、十字路口等等各种信息。

◆ 人工检查/标注，现有的算法还做不到100%的自动抽取，所以仍会加入人工检查和标注的环节。

◆ 后处理和验证，整个数据出来之后，还需要进行后处理和验证。处理的最终目的是保证提供的地图信息是准确的，假设一个红绿灯位置标错了或者自动识别错了，那可能会导致路测过程中会出现安全隐患，所以验证环节是很重要的部分。

◆ Release（发布），验证通过后，将会进行数据的发布（Data Release），所有数据会纳入统一的管理中，每一版本Release的地图的质量已得到严格验证。

这是一个常规的发布流程。现实中整个路网结构是不断往外扩张的，且周围环境会有变化，所以也会有增量的更新。

成本与挑战

前面简单介绍了整个HD Road Graph的生成发布流程，我们来看下HD Road Graph可能会遇到的问题与挑战。

成本

◆ 厘米级别的精度：需要各类传感器（LiDAR、Cameras、GNSS/IMU），上图右边是苹果的地图车，大家可以看到上面装了很多传感器，显然制造这样一辆高精地图车辆的成本不低。

◆ 数据量：海量存储资源和计算资源。

◆ 覆盖范围：当地图从局部区域逐步向外扩张，例如从中关村壹号附近地区，扩张到海淀区，再到北京市，甚至是半个中国，这其中涉及的硬件成本和人力成本等，都会相应增加。

复杂性挑战

第二个是高精地图的复杂性带来的挑战。复杂性体现在地图Data Model（数据模型）需要包含三部分信息：

◆ 语义信息，从上图里的标志牌示例图可以看出，目前道路存在各式各样的交通标记。因此，高精地图需提高语义信息的准确性，并且尽可能提高覆盖度。

◆ 空间信息，上图中的立交桥是以复杂闻名的西直门立交桥——有人说，即使借助导航软件，你都未必能够从纵横交错的道路中准确找到下一条你要行驶的车道。因此挑战就是如何能够把整个空间信息描述好，集成到地图中去。

◆ 时间信息，北京有一些潮汐街道，早高峰和晚高峰走的方向是不一样的；或者某些公交车道在早高峰的时候只允许公交车行驶，其他车辆禁止使用。这类时间信息也需要包含进地图里。

当三类信息准确地整合进高精地图时，决策规划模块和感知模块才能有效地运作。

拓展性挑战

◆ 自动驾驶车辆每行驶一公里会产生GB级别的原始数据，处理这些数据会使得功能架构面临很大的挑战。

◆ 地图更新方面，从“天”级别到“小时”级别再到“分钟”级别的更新，对应的整体功能架构是不一样的。

实时性挑战

交通管制、环境变化等怎么反馈到HD Road Graph上？假设今天某条路修路，对于人类驾驶员而言，他可以选择绕过这条路。对于自动驾驶而言，这样的信息如何反馈到行驶过程呢？是靠人去反馈，还是系统自动去识别？得到反馈之后，怎么实时更新到地图数据里？

所以说，自动驾驶需要打通offline（线下）到online（线上）的环节，及时更新到我们正在运行的自动驾驶车辆里去。

2 高精地图之3D栅格地图的应用

什么是3D栅格地图

前面介绍了拓扑地图的部分，接下来介绍3D栅格地图（3D Grid Map）。3D栅格地图的定义认为一个真实的三维世界可以用一个概率模型来表示。

比如像今天这个会场，我们可以将它离散化成一个个立体方格，每个方格里会存储一个概率，这个概率即方格空间不为空的概率。

上图是一个 3D 栅格地图，从数学的角度讲，它并不是一个连续的描述，而是离散化后的地图，只会保留一些概率值和属性。这里展示的是一张厘米级精度的栅格地图，我们可以很清楚看到周围的环境，包括树、建筑等等。

SLAM与自动驾驶

那么，如何制作高精地图呢？传统上有一种地图制作方法叫SLAM（即时定位与地图构建）。它指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM的应用领域主要有机器人、虚拟现实和增强现实。其用途包括传感器自身的定位，以及后续的路径规划、场景理解。

传统SLAM与自动驾驶SLAM的区别

传统 SLAM：

2D；
视觉传感器，单靠摄像头，在某个空间转来转去，最终通过算法来建模；
没有 GPS 信息，很多室内场景是收集不到 GPS 信息的；
生成地图和定位同时进行。

自动驾驶SLAM：

3D，描绘的是一个三维环境；
激光传感器，需要达到比较高的精度；
GPS 信息，室外场景的优势就是可以接收 GPS 信息；
离线处理，数据不需要在线处理，前面介绍过，如果选择在线，某些场景算法无法得到良好的处理，所以需要抽取一些信息来做离线的验证。

3D栅格地图的挑战

相比于传统 SLAM，我们来看下有哪些问题是3D栅格地图需要解决的：

全局 GPS 信息并不总是好的。上图左下角是3D栅格地图的底图，里面包含一条当时车辆的行驶轨迹。你会发现依据 GPS 的这条行驶轨迹，这辆车已经开到马路牙子（路肩）上了，但实际情况并非如此。因此，当拿到这样一份地图数据时，我们需要从算法层做出优化，并且做好细节。

不同时间段的数据如何align（校准）。一般来说，地图的采集不可能一次完成，因此，从算法层面上看，3D栅格地图需要处理把不同时间段的数据校准到一起的问题。

去除非静态的物体。我们所建立的是一个概率的模型，3D栅格地图只需要保留一些静态的物体以辅助感知系统，许多非静态的物体都需要从地图中去除。

应对挑战：位姿图 Pose Graph

位姿图其实是一个优化问题

整体上我们认为位姿图（Pose Graph）是一个优化问题。假设将路线采集以时间去划分——比如每隔4分钟采一个点，最后能得到不同的位置的环境。举个例子：在一个大厅里，我现在站在某点，一分钟后我可能站在另一处，再绕一圈的话后站在门口，通过把整个GPS获得的位置信息离散化之后，将我在每一点所看到的周围的环境直接拼起来，就可能得到一个三维的地图，因为每个点看到的角度都是不一样的。

但因为原始位置可能跟真实位置有偏差，直接拼起来常常导致很多问题。所以我们把这些点离散化之后，希望能够把所有点都对齐调整到准确的位置。那怎么确保最终的结果是正确的？

对整个图来说, 除了顶点之外，还有边。我们希望通过边的约束将点调好，边的约束你可以认为是个相对约束，假设我知道下个点的准确位置，那么便可倒推另一个点的准确位置。也就是说，在有准确的相对位置的基础上，辅以一些比较少的绝对位置，就可以得到一个全局的准确位置。

总的来说，位姿图的优化目标是把整个图离散化到一个个点之后，通过建立一些边的约束，最终通过优化某些点的位置来满足边的约束。位姿图也就成为了一个优化问题。

位姿图的挑战

位姿图整体的数学模型看似简单，但是存在许多细节问题需要做优化：

◆ 如何选择顶点。比如需要考虑是不是毫秒的点都要加进去，GPS 信号不好时的点要不要加进去。

◆ 如何保证边的准确性。一个顶点到另一个顶点的相对位置，需要通过什么方式去建立。

◆ 如何求解非线性优化。图建好之后如何去做非线性的优化。

◆ 如何评估优化效果。图是否和真实的环境一致，以及如何评估结果满足要求。

◆ 效率和资源。对于工程来讲，数据量这么大，你需要考虑怎么设计工程算法以保证效率和资源。比如说，考虑是不是需要在算法基础上进行定制优化，如何用 GPU 或者分布式的计算方式。

我想通过上图右边的例子解释位姿图——左侧原始的图展示的是有一个物体来回在球面上运动旋转，通过在边与边之间建立相对约束，进行优化后，你会发现整个环境的重建成了右侧一个比较理想、完整的球体，概括来说，我们的工作就是需要把左侧的图来变成右侧的图。

迭代最近点算法：保证边的约束

我们前面提到位姿图需要保证边的约束，为了计算出这个约束，业界用的比较多的传统经典算法叫做 ICP（Iterative Closest Point algorithm）——迭代最近点算法。

举个例子：左边是只红色的兔子，右边是只蓝色的兔子，现实中这两只兔子的位置是一样的，但在图片里两只兔子位置不一样，说明位置存在偏差。也就是说，输入是两组点云 A 和 B，输出是两组点云之间在空间上的旋转和平移。

具体算法的思路大致是：找到点云集合 A 中的每个点在集合 B 中的对应点，通过求解最佳的刚体变换，不断的迭代优化，最终得到一个收敛解。

应对挑战：去除非静态障碍物体

去除非静态障碍体的方法大致有三类：

◆ 通过概率模型去除。

◆ 点云处理，单纯的概率模型是不够的，例如有时候小区路边经常停着车，而某些时候车是被开走的。数据采集时，恰好车停在路边，后续那就需要依赖离线点云处理。

◆ 机器学习，即利用机器学习算法去把非静态的物体从地图里面抠出来。

上图是一个简单的去除非静态障碍物前后对比图，处理之前，你会发现右侧道路有一些车的轮廓，当从空间上去除之后，地图变得清晰许多。

3 自动驾驶车辆如何定位

自动驾驶定位的特点

对于定位系统和传统定位来说，自动驾驶车辆的位置和姿态有以下特点：

◆ 厘米级别的精度，需要精确到车在车道线的具体位置，例如距离左右边线几公分。

◆ 高频低延迟，需要毫秒级别的时间延迟，实时传输车的位置，否则会出现安全事故。

几种定位方式

全球导航卫星系统

特点：

米级别精度
低频
干扰或者遮挡

解决方法：RTK（Real - time kinematic，实时动态）载波相位差分技术，可以达到厘米级别。

相对定位：惯性测量单元IMU

特点：

加速度以及角速度
三个方向的加速度计和三轴的陀螺仪
高频（通常100HZ）
累积误差

解决方法：惯性导航系统

点云定位

鲁棒性好，不依赖于 GPS 信息。
依赖于先验地图，地图的准确性和实时性可能会导致定位误差。
需要解决：弱特征环境。比如说在桥上，往前开10米和往后开10米看到的场景都是类似的，对于这类环境来说，如何去找到准确位置，这是必须考虑的问题。

定位系统

多传感器融合定位

综合来说，靠单一传感器定位是不够的，所以采取多传感器融合的方案。基于卡尔曼滤波模型，整体定位效果会有更高的精度和更好的鲁棒性。

卡尔曼滤波模型

卡尔曼滤波是一种高效率的递归滤波器，它能够从一系列的不完全和包含噪声的测量中，估计动态系统的状态。

基于卡尔曼滤波模型，我们把自动驾驶车辆当时的位置看作是高斯分布，也就是说，车辆定位在某个区域范围之内，且位于中间的可能性是最大的。

随着车辆行驶，根据运动方程，定位系统能够得到车辆的位置预测。假设一辆车往前行驶了10米，于是车辆产生一个预测位置，然而实际行驶中，预测位置和实际位置的误差逐渐变大。这是因为，此数据可能是由IMU给出，而IMU作为传感器自然会存在误差。同时假设这辆车还装有GPS，那么GPS给出的位置信息便相当于上图中的蓝色的部分，这个定位数据也是包含一定误差的。

于是，我们得到关于这辆车的两个位置信息，将二者结合后便可得到较为准确且可信的结果。由此可见，卡尔曼滤波是不断在更新迭代的，最终实现得到一个比较准确位置的目标。