简述自动驾驶路径规划的常用算法

自动驾驶自诞生那天起，其志向便已立下，成为熟知城市每一处道路的“老司机”，成为乘客更安全、更舒适、更高效出行的“守护神”。在搞钱捞钱的大背景下，这个无私追求朴素得令人敬畏。 在自动驾驶的分工中，决策规划将承担上述志向实现的大部分工作，也因此被毫不吝啬的称为自动驾驶的大脑。决策规划这块网上已经有数量众多的优秀科普文章，但他们都没有长成《十一号组织》的样子。抱着将所有自动驾驶知识都“撸一遍”的伟大理想，我决定用两万字简述决策规划的常用算法。

01 概述

1. 1 自动驾驶系统分类

自动驾驶系统没有严谨的分类，但行业内普遍喜欢将自动驾驶系统区别为模块化的和端到端的，图1所示为两者系统的原理框图对比。

图1 模块化和端到端自动驾驶系统原理简图

1.1.1 模块化自动驾驶系统

这是最经典也是业界采用最多的一种自动驾驶系统，也是最简明清爽的一种结构，其作用是实时地求解出连续的控制输出使得自动驾驶车辆可以安全地由初始位置行驶到目标位置。基于模块化的思想，将自动驾驶系统划分为三层：环境感知层、决策规划层和控制执行层。每一层还可以划分为不同的模块，每个模块还可以划分为不同的子模块……。

环境感知层就像是人的眼睛和耳朵，负责对外部环境进行感知并将感知结果送入决策规划层。决策规划层就像是人的大脑，在接收到感知信息后进行分析、决策，并生成加减速、变道、直行等控制命令。控制执行层就像人的双手和双脚，在接收到控制命令后控制执行器完成加速、转向等操作。 模块化自动驾驶系统中每一层都是关键和核心。但从实现自动驾驶功能的角度，环境感知层是基础，决策规划层是核心，控制执行层是保障。作为核心的决策规划层带着自动驾驶往“更安全、更舒适、更高效”的道路上狂奔，毕竟小小的失误小则影响乘坐舒适性、通行效率，大则影响人身财产安全。

在模块化自动驾驶系统中，不同团队负责不同的模块，可以实现更好的分工合作，从而提高开发效率。同时团队内部可以对负责的模块进行充分的评估，了解各模块的性能瓶颈所在，从而让我们能对最后的0.1%的不足有更清晰的认知，技术的迭代、更新。

缺点就是整个系统非常复杂、庞大、需要人工设计成百上千个模块。二是对车载硬件计算能力要求高，如果越来越多的子模块采用深度学习网络，这将带来灾难性的计算需求爆炸。基于模块化的自动驾驶系统，我们可能花10%的时间就实现了99.9%的问题，但我们还需要花90%的时间去解决最后0.1%的不足。 这个系统的难度之大，已经远超一家公司的能力范围，需要一个协作的生态。

1.1.2 端到端自动驾驶系统

术语端到端（End to End）来源于深度学习，指的是算法直接由输入求解出所需的输出，即算法直接将系统的输入端连接到输出端。2016年NVIDIA将端到端的深度学习技术应用在自动驾驶汽车之后，端到端自动驾驶迅速捕获圈内一众大佬的芳心，各种demo更是层出不穷。 所谓端到端自动驾驶是指车辆将传感器采集到的信息（原始图像数据、原始点云数据等），直接送入到一个统一的深度学习神经网络，神经网络经过处理之后直接输出自动驾驶汽车的驾驶命令（方向盘转角、方向盘转速、油门踏板开度、制动踏板开度等）。

2016年NVIDIA发表了论文《End to End Learning for Self-Driving Cars》，拉开了端到端自动驾驶内卷的序幕。 论文首先展示了训练数据的采集系统，如图2所示。论文中只涉及了车道保持功能，因此训练数据也只对摄像机的视频数据和人类驾驶员操作方向盘的角度数据进行了采集。

图2 数据采集系统框图 三架摄像机安装在采集车的挡风玻璃后面，并按照左中右依次布置，这样布置是为了捕获完整的前向路面信息。一台NVIDIA DRIVETM PX被用来作为采集车的计算单元。摄像机生成的每一帧视频数据（30FPS）都与人类驾驶员的转向角度进行时间同步。 采集车最终在各式道路以及多样照明和天气条件组合下采集了72小时的驾驶数据。训练数据包含视频采样得到的单一图像，搭配相应的转向命令。

但是只有来自人类驾驶员的正确数据是不足以完成训练的，神经网络还必须学习如何从任何错误中恢复，否则自动驾驶汽车就将慢慢偏移道路。因此训练数据还扩充了额外的图像，这些图像显示了远离车道中心的偏离程度以及不同道路方向上的转动。两个特定偏离中心的变化图像可由左右两个摄像机捕获。 训练数据准备完毕之后，将其送入一个卷积神经网络（CNN），训练系统框图如图3所示。

图3 训练系统框图 CNN计算一个被推荐的转向命令，这个被推荐的转向命令会与该图像的期望命令相比较，CNN权重就会被调整以使其实际输出更接近期望输出。在这个框架中，只要提供足够的训练数据，即人类驾驶员驾驶携带有摄像头的车辆累计驾驶大量的里程，再加上人为创造系统的“极限”道路状态——偏离道路线的各种工况，CNN就会得到充分的训练，而变得足够强大。 一旦训练完成，网络就能够从单中心摄像机（single center camera）的视频图像中生成转向命令，图4展示了这个配置。

图4 训练过的网络用于从单中心前向摄像机中生成转向命令

在端到端自动驾驶中，没有人工设计的繁复规则，只需要极少的来自人类的训练数据，深度学习神经网络就会学会驾驶。且不用关心有没有高精地图覆盖、此时是行驶在高速主干路还是城区道路、道路上车道线有没有缺失等。 相比模块化自动驾驶系统，端到端自动驾驶系统设计难度低，硬件成本小，还能借助数据的多样性获得不同场景下的泛用性。各方面条件得天独厚，从理论层面看堪称自动驾驶的终极梦想。

然而端到端深度学习神经网络是一个完完全全的黑盒子，不具解释分析性，可靠性、灵活性差，工程师们没有办法对它进行系统化的解释分析，而是只能依靠推测和实验进行调整。最终带来的结果是安全难以得到保障，而自动驾驶最最关注的恰是安全。

比如端到端自动驾驶系统下汽车做出一个汽车减速左转的行动，工程师们无法确定这是因为汽车看到行人，还是因为看到较远处的红灯。但是，在模块化的自动驾驶系统下，由于多个识别系统嵌套，相对好理解到底汽车所做的每一个举动背后的逻辑。

这也意味着，如果端到端系统出现问题时，工程师们并不能对其对症下药，做出合理的应对。更多情况下甚至只能简单向模型灌注更多的数据，希冀它能在进一步的训练中“自行”解决问题。这也会大大降低端到端自动驾驶系统原本开发简单的优势。

1.2 决策规划分层架构

决策规划的任务，就是在对感知到的周边物体的预测轨迹的基础上，结合结合自动驾驶车辆的和当前位置，对车辆做出最合理的决策和控制。 正如人的大脑又分为左脑和右脑、并负责不同的任务一样，模块化自动驾驶系统中决策规划层也可以继续细分为执行不同任务的子层。而这一分层设计最早其实是源自2007年举办的DAPRA城市挑战赛，比赛中多数参赛队伍都将自动驾驶系统的决策规划方式包括三层：全局路径规划层（Route Planning）、行为决策层（Behavioral Layer）和运动规划层（Motion Planning），如图5所示。

图5决策规划分层架构

全局路径规划层聚焦在相对顶层的路径规划，聚焦在分钟到小时级别的规划。该层在接收到输入的目的地信息后，基于存储的地图信息搜索出一条自起始点至目标点的一条可通过的路径。如图6所示，在蓝色起点和黄色终点之间，黑色就是搜索出来的一条可通行的路径，当然路径不止一条，如何搜索出最优是下文将要介绍的内容。

在全局路径规划的时候，也可以基于地图精度和丰富度，提前考虑道路曲率半径、坡度等信息，来避免搜索出部分参数超出ODD要求的全局路径。但是高度随机的交通参与者、高度动态的交通流以及高度复杂的道路结构，全局路径规划是无法考虑周到的，因此还需要基于具体的行为决策进行后面的运动规划，也就是局部路径规划。

行为决策层在收到全局路径后，结合感知环境信息、交通规则信息、车辆状态信息、驾驶场景信息等，推导判断下一分钟或下一秒时刻的情况，作出车道保持、车辆跟随、车道变换和制动避撞等的适合当前交通环境的驾驶行为。如图7所示，自车在检测到前方存在低速行驶车辆，且右侧车道满足变道条件后，作出向右变道的驾驶行为决策。

运动规划层也被称为局部路径规划层，与全局路径规划聚焦在分钟到小时级别的规划不同，运动规划聚焦在毫秒级到秒级的规划。规划的时候，根据输入的行为决策信息、结合车辆实时位姿信息、局部环境信息、全局路径参考信息等，在“安全、舒适、效率”的精神引领下，规划生成一条满足特定约束条件的平滑轨迹轨迹（包括行驶轨迹、速度、方向等），并输入给控制执行层。 如图8所示，在车辆收到行为决策层的左变道指令后，主车基于各种信息规划出几条可行的路径，如何规划出最优的路径也是下文要介绍的内容。

全局路径规划与运动规划作为两个层级的不同规划，现将其特点汇总为表1。

表1 全局路径规划与运动规划特点对比

02 全局路径规划常用算法

正菜之前，我们先来了解一下图（包括有向图和无向图）的概念。图是图论中的基本概念，用于表示物体与物体之间存在某种关系的结构。在图中，物体被称为节点或顶点，并用一组点或小圆圈表示。节点间的关系称作边，可以用直线或曲线来表示节点间的边。

如果给图的每条边规定一个方向，那么得到的图称为有向图，其边也称为有向边，如图9所示。在有向图中，与一个节点相关联的边有出边和入边之分，而与一个有向边关联的两个点也有始点和终点之分。相反，边没有方向的图称为无向图。

图9 有向图示例

数学上，常用二元组G =（V，E）来表示其数据结构，其中集合V称为点集，E称为边集。对于图6所示的有向图，V可以表示为{A，B，C，D，E，F，G}，E可以表示为{，，，，，，}。表示从顶点A发向顶点B的边，A为始点，B为终点。

在图的边中给出相关的数，称为权。权可以代表一个顶点到另一个顶点的距离、耗费等，带权图一般称为网。

在全局路径规划时，通常将图10所示道路和道路之间的连接情况，通行规则，道路的路宽等各种信息处理成有向图，其中每一个有向边都是带权重的，也被称为路网（Route Network Graph）。

那么，全局路径的规划问题就变成了在路网中，搜索到一条最优的路径，以便可以尽快见到那个心心念念的她，这也是全局路径规划算法最朴素的愿望。而为了实现这个愿望，诞生了Dijkstra和A*两种最为广泛使用的全局路径搜索算法。

2.1Dijkstra算法

戴克斯特拉算法（Dijkstra’s algorithm）是由荷兰计算机科学家Edsger W. Dijkstra在1956年提出，解决的是有向图中起点到其他顶点的最短路径问题。

假设有A、B、C、D、E、F五个城市，用有向图表示如图11，边上的权重代表两座城市之间的距离，现在我们要做的就是求出起点A城市到其它城市的最短距离。

图11 五个城市构建的有向图

用Dijkstra算法求解步骤如下：

（1）创建一个二维数组E来描述顶点之间的距离关系，如图12所示。E[B][C]表示顶点B到顶点C的距离。自身之间的距离设为0，无法到达的顶点之间设为无穷大。

图12 顶点之间的距离关系

（2）创建一个一维数组Dis来存储起点A到其余顶点的最短距离。一开始我们并不知道起点A到其它顶点的最短距离，一维数组Dis中所有值均赋值为无穷大。接着我们遍历起点A的相邻顶点，并将与相邻顶点B和C的距离3（E[A][B]）和10（E[A][C]）更新到Dis[B]和Dis[C]中，如图13所示。这样我们就可以得出起点A到其余顶点最短距离的一个估计值。

图13 Dis经过一次遍历后得到的值

（3）接着我们寻找一个离起点A距离最短的顶点，由数组Dis可知为顶点B。顶点B有两条出边，分别连接顶点C和D。因起点A经过顶点B到达顶点C的距离8（E[A][B] + E[B][C] = 3 + 5）小于起点A直接到达顶点C的距离10，因此Dis[C]的值由10更新为8。同理起点A经过B到达D的距离5（E[A][B] + E[B][D] = 3 + 2）小于初始值无穷大，因此Dis[D]更新为5，如图14所示。

图14Dis经过第二次遍历后得到的值

（4）接着在剩下的顶点C、D、E、F中，选出里面离起点A最近的顶点D，继续按照上面的方式对顶点D的所有出边进行计算，得到Dis[E]和Dis[F]的更新值，如图15所示。

图15 Dis经过第三次遍历后得到的值

（5）继续在剩下的顶点C、E、F中，选出里面离起点A最近的顶点C，继续按照上面的方式对顶点C的所有出边进行计算，得到Dis[E]的更新值，如图16所示。

图16 Dis经过第四次遍历后得到的值

（6）继续在剩下的顶点E、F中，选出里面离起点A最近的顶点E，继续按照上面的方式对顶点E的所有出边进行计算，得到Dis[F]的更新值，如图17所示。

图17 Dis经过第五次遍历后得到的值

（7）最后对顶点F所有点出边进行计算，此例中顶点F没有出边，因此不用处理。至此，数组Dis中距离起点A的值都已经从“估计值”变为了“确定值”。

基于上述形象的过程，Dijkstra算法实现过程可以归纳为如下步骤：

（1）将有向图中所有的顶点分成两个集合P和Q，P用来存放已知距离起点最短距离的顶点，Q用来存放剩余未知顶点。可以想象，一开始，P中只有起点A。同时我们创建一个数组Flag[N]来记录顶点是在P中还是Q中。对于某个顶点N，如果Flag[N]为1则表示这个顶点在集合P中，为1则表示在集合Q中。

（2）起点A到自己的最短距离设置为0，起点能直接到达的顶点N，Dis[N]设为E[A][N]，起点不能直接到达的顶点的最短路径为设为∞。

（3）在集合Q中选择一个离起点最近的顶点U（即Dis[U]最小）加入到集合P。并计算所有以顶点U为起点的边，到其它顶点的距离。例如存在一条从顶点U到顶点V的边，那么可以通过将边U->V添加到尾部来拓展一条从A到V的路径，这条路径的长度是Dis[U]+e[U][V]。如果这个值比目前已知的Dis[V]的值要小，我们可以用新值来替代当前Dis[V]中的值。

（4）重复第三步，如果最终集合Q结束，算法结束。最终Dis数组中的值就是起点到所有顶点的最短路径。

2.2A*算法

1968年，斯坦福国际研究院的Peter E. Hart, Nils Nilsson以及Bertram Raphael共同发明了A*算法。A*算法通过借助一个启发函数来引导搜索的过程，可以明显地提高路径搜索效率。

下文仍以一个实例来简单介绍A*算法的实现过程。如图18所示，假设小马要从A点前往B点大榕树底下去约会，但是A点和B点之间隔着一个池塘。为了能尽快提到达约会地点，给姑娘留下了一个守时踏实的好印象，我们需要给小马搜索出一条时间最短的可行路径。

A*算法的第一步就是简化搜索区域，将搜索区域划分为若干栅格。并有选择地标识出障碍物不可通行与空白可通行区域。一般地，栅格划分越细密，搜索点数越多，搜索过程越慢，计算量也越大；栅格划分越稀疏，搜索点数越少，相应的搜索精确性就越低。

如图19所示，我们在这里将要搜索的区域划分成了正方形（当然也可以划分为矩形、六边形等）的格子，图中蓝色格子代表A点（小马当前的位置），紫色格子代表B点（大榕树的位置），灰色格子代表池塘。同时我们可以用一个二维数组S来表示搜素区域，数组中的每一项代表一个格子，状态代表可通行和不可通行。

图19 经过简化后的搜索区域

接着我们引入两个集合OpenList和CloseList，以及一个估价函数F = G + H。OpenList用来存储可到达的格子，CloseList用来存储已到达的格子。G代表从起点到当前格子的距离，H表示在不考虑障碍物的情况下，从当前格子到目标格子的距离。F是起点经由当前格子到达目标格子的总代价，值越小，综合优先级越高。

G和H也是A*算法的精髓所在，通过考虑当前格子与起始点的距离，以及当前格子与目标格子的距离来实现启发式搜索。对于H的计算，又有两种方式，一种是欧式距离，一种是曼哈顿距离。

欧式距离用公式表示如下，物理上表示从当前格子出发，支持以8个方向向四周格子移动（横纵向移动+对角移动）。

曼哈顿距离用公式表示如下，物理上表示从当前格子出发，支持以4个方向向四周格子移动（横纵向移动）。这是A*算法最常用的计算H值方法，本文H值的计算也采用这种方法。

现在我们开始搜索，查找最短路径。首先将起点A放入到OpenList中，并计算出此时OpenList中F值最小的格子作为当前方格移入到CloseList中。由于当前OpenList中只有起点A这个格子，所以将起点A移入CloseList，代表这个格子已经检查过了。

接着我们找出当前格子A上下左右所有可通行的格子，看它们是否在OpenList当中。如果不在，加入到OpenList中计算出相应的G、H、F值，并把当前格子A作为它们的父节点。本例子，我们假设横纵向移动代价为10，对角线移动代价为14。

我们在每个格子上标出计算出来的F、G、H值，如图20所示，左上角是F，左下角是G，右下角是H。通过计算可知S[3][2]格子的F值最小，我们把它从OpenList中取出，放到CloseList中。

图20 第一轮计算后的结果

接着将S[3][2]作为当前格子，检查所有与它相邻的格子，忽略已经在CloseList或是不可通行的格子。如果相邻的格子不在OpenList中，则加入到OpenList，并将当前方格子S[3][2]作为父节点。

已经在OpenList中的格子，则检查这条路径是否最优，如果非最优，不做任何操作。如果G值更小，则意味着经由当前格子到达OpenList中这个格子距离更短，此时我们将OpenList中这个格子的父节点更新为当前节点。

对于当前格子S[3][2]来说，它的相邻5个格子中有4个已经在OpenList，一个未在。对于已经在OpenList中的4个格子，我们以它上面的格子S[2][2]举例，从起点A经由格子S[3][2]到达格子S[2][2]的G值为20（10+10）大于从起点A直接沿对角线到达格子S[2][2]的G值14。显然A经由格子S[3][2]到达格子S[2][2]不是最优的路径。当把4个已经在OpenList 中的相邻格子都检查后，没有发现经由当前方格的更好路径，因此我们不做任何改变。

对于未在OpenList的格子S[2][3]（假设小马可以斜穿墙脚），加入OpenList中，并计算它的F、G、H值，并将当前格子S[3][2]设置为其父节点。经历这一波骚操作后，OpenList中有5个格子，我们需要从中选择F值最小的那个格子S[2][3]，放入CloseList中，并设置为当前格子，如图21所示。

图21第二轮计算后的结果

重复上面的故事，直到终点也加入到OpenList中。此时我们以当前格子倒推，找到其父节点，父节点的父节点……，如此便可搜索出一条最优的路径，如图22中红色圆圈标识。

图22 最后计算得到的结果

基于上述形象的过程，A*算法实现过程可以归纳为如下步骤：

（1）将搜索区域按一定规则划分，把起点加入OpenList。

（2）在OpenList中查找F值最小的格子，将其移入CloseList，并设置为当前格子。

（3）查找当前格子相邻的可通行的格子，如果它已经在OpenList中，用G值衡量这条路径是否更好。如果更好，将该格子的父节点设置为当前格子，重新计算F、G值，如果非更好，不做任何处理；如果不在OpenList中，将它加入OpenList中，并以当前格子为父节点计算F、G、H值。

（4）重复步骤（2）和步骤（3），直到终点加入到OpenList中。

2.3两种算法比较

Dijkstra算法的基本思想是“贪心”，主要特点是以起点为中心向周围层层扩展，直至扩展到终点为止。通过Dijkstra算法得出的最短路径是最优的，但是由于遍历没有明确的方向，计算的复杂度比较高，路径搜索的效率比较低。且无法处理有向图中权值为负的路径最优问题。

A*算法将Dijkstra算法与广度优先搜索（Breadth-First-Search，BFS）算法相结合，并引入启发函数（估价函数），大大减少了搜索节点的数量，提高了搜索效率。但是A*先入为主的将最早遍历路径当成最短路径，不适用于动态环境且不太适合高维空间，且在终点不可达时会造成大量性能消耗。

图24是两种算法路径搜索效率示意图，左图为Dijkstra算法示意图，右图为A*算法示意图，带颜色的格子表示算法搜索过的格子。由图23可以看出，A*算法更有效率，手术的格子更少。

图23 Dijkstra算法和A*算法搜索效率对比图（图片来源：https://mp.weixin.qq.com/s/myU204Uq3tfuIKHGD3oEfw）

03 行为决策常用算法

作为L4级自动驾驶的优秀代表Robotaxi，部分人可能已经在自己的城市欣赏过他们不羁的造型，好奇心强烈的可能都已经体验过他们的无人“推背”服务。作为一个占有天时地利优势的从业人员，我时常在周末选一个人和的时间，叫个免费Robotaxi去超市买个菜。

刚开始几次乘坐，我的注意力全都放在安全员的双手，观察其是否在接管；过了一段时间，我的注意力转移到中控大屏，观察其梦幻般的交互方式；而现在，我的注意力转移到了智能上，观察其在道路上的行为决策是否足够聪明。 而这一观察，竟真总结出不少共性问题。比如十字路口左转，各家Robotaxi总是表现的十分小心谨慎，人类司机一脚油门过去的场景，Robotaxi总是再等等、再看看。且不同十字路口同一厂家的Robotaxi左转的策略基本一致，完全没有人类司机面对不同十字路口、不同交通流、不同天气环境时的“随机应变”。

面对复杂多变场景时自动驾驶行为决策表现出来的小心谨慎，像极了人类进入一个新环境时采取的猥琐发育策略。但在自动驾驶终局到来的那天，自动驾驶的决策规划能否像人类一样，在洞悉了人情社会的生活法则之后，做到“见人说人话”、“见人下饭”呢？ 在让自动驾驶车辆的行为决策变得越来越像老司机的努力过程中，主要诞生了基于规则和基于学习的两大类行为决策方法。