摸清自动驾驶车辆规划技术的第一步

--后台回复“资料”，领取特斯拉专利技术解析报告--

文章转自：汽车人手记

智驾最前沿

实时，权威，专业剖析自动驾驶领域！

11篇原创内容

公众号

↓聊聊自动驾驶数据源↓

❤更多精彩视频欢迎关注视频号❤

一辆自动驾驶车辆的典型架构如图1所示。从图中可以看到，自动驾驶车辆从系统功能上划分，可以分为环境感知模块、决策规划模块和车辆控制模块。如果把感知模块比作人的眼睛和耳朵，那么决策规划就是感知信息汇聚的模块，扮演着车辆“大脑”的角色。大脑在接收到传感器的各类感知信息后，对当前周边环境态进行分析，然后对车辆控制模块下达指令。决策规划模块决定了自动驾驶车辆可以适应多么复杂的场景，能否高效安全的行驶，是评级自动驾驶能力的最核心指标之一，具有十分重要的学术研究价值。

图1：自动驾驶车辆的典型架构[1]

图解决策规划模块架构

当前自动驾驶汽车存在三大类典型的感知决策控制方法，如图2所示。

由上至下依次为[2]：

Sequential Planning、

Behavior-awarePlanning和End-to-end Planning。

图2：当前自动驾驶汽车存在三大类感知决策控制方法

Sequential Planning

Sequential Planning是层次最简明的一种方法，将先进的感知和决策方法生成运动规划和控制的输入。其作用是实时地求解出一序列连续的控制输入或可行运动状态使得汽车能够安全地由初始状态到达终止状态，并且求解出结果不仅要满足汽车的运动学及动力学约束，同时还要满足结构化道路带来的几何约束以及交通法规约束。

整体架构如图3所示[3]，可以分为全局路径规划层（Route Planning）、行为决策层（Behavioral Layer）和运动规划层（Motion planning）。

图3：Sequential Planning 整体架构

其中：

全局路径规划在接收到行驶目的地之后，结合地图信息，生成起点到终点的路径寻找，也可称为任务规划（Mission planning）；

行为决策层也可称之为行为决策（Decision），在接收到全局路径信息后，结合当前交通信息和障碍物等情况，给运动规划模块输入一系列的决策信息，如在十字路口通行优先级、交通堵塞处理、车辆汇流合理判断问题等；

运动规划层根据具体的行为决策信息，规划生成一条满足特定约束条件的运动轨迹，以达到某种目的，如安全避障、准确进入停车位等。

Behavior-aware Planning

Behavior-aware Planning引入驾驶员与自动驾驶汽车交互、自动驾驶汽车与道路场景交互、自动驾驶汽车与周边参与者交互，在决策规划的过程中考虑外部环境的不确定性，从而增加安全性。

目前在研究中，经常用到的五类方法是[2]：Cooperation and Interaction (协同与交流)、Games-Theoretic Approaches（博弈论）、Probabilistic Approaches（概率方法）、Partially Observable Markov Decision Processes（隐马尔可夫）、Learning-Based Approaches（基于学习的方法）。

图4：Behavior-aware Planning常用研究方法

End-to-end Planning

End-to-end Planning通过深度学习等系统驱动，从离线收集的真实数据中学习驾驶员驾驶行为，直接通过输入的图像或者视频信息得到汽车驾驶行为的指令，包括方向盘转角、油门大小、踩刹车的程度等。英伟达[4]使用卷积神经网络（CNN）将从前向摄像机得到的原始图像映射成自动驾驶汽车的驾驶命令，如图5所示。三架摄像机安装在数据采集汽车的挡风玻璃后面，而来自摄像机的时间戳视频是与人类驾驶员的转向角度同时被捕获的。图像被送入一个卷积神经网络，然后计算一个被推荐的转向命令。这个被推荐的转向命令会与该图像的期望命令相比较，卷积神经网络的权重就会被调整以使其实际输出更接近期望输出。

图5：End-to-end Planning系统框图

详解Sequential Planning

本文将主要介绍Sequential Planning的分层架构和常见算法。

全局路径规划

全局路径规划就是指在给定车辆当前位置与终点目标后，通过搜索选择一条最优的路径。这里的“最优”通常包括时间最短或者路程最短等。通俗意义上讲，这一过程和我们生活中用到的“导航”功能类似，区别在于自动驾驶的全局路径规划使用高精度地图，里面包括更多的路段和车道信息。

最基础和普适的全局路径规划算法是Dijkstra算法和A*算法。

• Dijkstra算法

Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用来寻找图形中节点之间的最短路径。在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点，直到到达终点为止。

简单说，就是从起点开始向外扩展，直至达到目标，从中找到一条最短路径。如下图中[5]，粉红色的方块是起点，蓝色的方块是目标，而蓝绿色区域显示了Dijkstra算法扫描的区域。图a表示在地图没有障碍物；图b表示地图存在凹形障碍物。

a.地图没有障碍物

b.地图存在障碍物

图6：Dijkstra算法搜索示意图

• A*算法

在理解A*算法之前，除了Dijkstra算法，还需要理解最佳优先搜索算法。贪婪的最佳优先搜索算法以与Dijkstra类似的方式工作，但是使用的是以每个节点到达终点的距离作为优先级。最佳优先搜索算法比Dijkstra算法运行更快。图7中，黄色表示具有较高启发式值（达到目标的成本较高）的那些节点，而黑色表示具有较低启发式值（达到目标的成本较低）的那些节点。在地图中存在障碍物时，最佳优先搜索是“贪婪的”，即使不是正确的道路，它也会尝试朝目标迈进。由于它只考虑达到目标的成本，而忽略了到目前为止的成本，因此即使它所走的路径已经很长，它也会继续前进，如图7中b所示。

a.地图没有障碍物

b.地图存在障碍物

图7：最佳优先搜索算法示意图

结合最佳优先搜索算法和Dijkstra算法的优点不好吗？A*最初发表于1968年，由Stanford研究院的Peter Hart、Nils Nilsson以及Bertram Raphael发表。它结合了Dijkstra算法的特点（靠近起点的最佳点）和贪婪的“最佳优先搜索”使用的特点（靠近目标的最佳点），A*算法通过下面这个函数来计算每个节点的优先级。

f(n)=g(n)+h(n)

其中：

f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。

g(n)是节点n距离起点的代价。

h(n)是节点n距离终点的预计代价，同时也是A*算法的启发函数。

行为决策

有研究者在研究人类驾驶行为时，将人类驾驶任务分为了3个不同层次：导航任务、引导任务与稳定任务。与之对应的是在顺序规划中，自动驾驶车辆根据任务进行分层，分别执行全局路径规划、行为决策、轨迹生成任务。行为决策层在道路行驶过程中根据周边交通环境，做出车道保持、换道、超车、驶过路口等决定。

在这个过程中，面临几个主要问题：

1. 真实的驾驶环境中，交通场景和道路是复杂的，如何能够在任意场景下做出正确决策。
2. 自动驾驶车辆在环境中面临无法预测的突发情况时，如何能够做出正确的决策。
3. 自动驾驶车辆在无法完全感知周围交通环境的情况下，如何进行正确决定。

行为决策是自动驾驶车辆技术的一项关键技术，提高自动驾驶车辆在城区复杂、不确定环境下的行为决策规划水平，可以提高自动驾驶车辆的智能化水平。

当前，主要存在三种主要的行为决策模型。

• 有限状态机模型

有限状态机（Finite State Machine, FSM）表示有限个状态及在这些状态之间的转移和动作等行为的数学模型。在自动驾驶中，车辆根据交通场景和交通规则选择合适的驾驶行为，如车道保持、前车跟随、停车等待等模式，状态机模型通过构建有限的有向连通图来描述不同的驾驶状态以及状态之间的转移关系，从而生成驾驶动作。

经典的有限状态机：“Junior”是斯坦福大学在2007年参加DARPA城市挑战赛时的无人车，它是第二名完成该比赛的无人车，Junior的行为规划系统就是通有限状态机实现的，如图8。

图8：“Junior”行为决策有限状态机

其中：

LOCATE_VEHICLE: 表示Junior的初始状态，确定无人车在地图中的位置。

FORWARD_DRIVE: 表示在普通道路上的行驶状态，包含车辆直行、车道保持和障碍物避障等。

PARKING_NAVIGATE：表示停车场内的普通驾驶模式。

UTURN_STOP: 表示U型掉头前的停止状态。

CROSS_DIVIDER: 表示跨车道行驶。

STOP_SIGN_WAIT: 表示当无人车在十字路口等待时，进入此状态。

CROSS_INTERSECTION：表示在这个状态下无人车处理十字路口通过这一场景，无人车会等待直到确认能够安全通过。

UTURN_DRIVE: 表明在U型掉头时调用的状态。

TRAFFIC_JAM和ESCAPE：处理交通阻塞时的两个状态。

BAD_RNDF: 表示如果当前道路和预先做的路网图不同的时候，即进入该状态，在这个状态下，无人车会采用混合A*算法完成车辆的路径规划。

MISSION_COMPLETE: 当挑战赛（DARPA）结束，无人车进入该状态，即整个状态机的结束状态。

有限状态机结构简单，控制逻辑明确，是无人驾驶领域使用比较广泛的行为决策模型，但是当驾驶状态和模式过多的时候，有限状态机的修改和更新变得非常繁琐。

•  决策树模型

决策树模型[7]和有限状态机模型类似，被广泛应用到自动驾驶车辆行为规划中。决策树（Decision Tree）由一个决策图和可能的结果组成，用来创建到达目标的规划。在自动驾驶行为规划算法中将驾驶状态和控制逻辑转化为树形结构，通过自顶向下的“轮询”机制进行驾驶策略的搜索和选择。决策树由非叶节点和叶节点组成，非叶节点对应驾驶状态，叶节点对应驾驶动作。这类决策模型具备可视化的控制逻辑，并且控制节点可复用，但需要针对每个驾驶场景离线定义决策网路，当状态空间、行为空间较大时，控制逻辑将比较复杂。另外，该类模型同样无法考虑交通环境中存在的不确定性因素。

• 基于知识的推理模型

基于知识的推理决策模型由“场景特征-驾驶动作”的映射关系来模仿人类驾驶员的行为决策过程，该类模型通过识别当前场景，查询存储在知识库或者神经网络中驾驶知识，然后推理出对应的驾驶动作。采集了人类驾驶员的行为数据，按照场景的不同分类存储，形成行为库。通过基于信息论、粒子计算的知识获取表达，形成相应的知识库。这里的知识既包括数据统计分析得到的数学模型，也包括通过机器学习得到“黑箱”。根据感知系统实时信息，利用知识库进行行为决策。通过不同的仿真场景和实车实验对知识库进行反复验证，迭代更新，逐渐完善。

运动规划

运动规划是无人驾驶顺序规划中的最后一步，输入是车辆当前的状态（车辆当前位置、感知障碍物位置速度、地图信息等）和具体的驾驶行为，目标输出是一系列运动轨迹和控制信息（速度等）。运动规划要做的就是生成当前状态和目标的一系列映射，实现车辆按照规划轨迹进行安全驾驶。

从机器人领域的角度看，运动规划的本质是一个搜索问题，将交通环境以确定的方法离散成一个图，然后利用各种搜索算法寻找最短路径或者安全路径。

从数学角度看，运动规划的本质是一个优化问题。在确定的状态空间中，寻找一个满足一定约束条件的映射。这些约束包括：

1. 局部约束问题：比如要考虑进行障碍物的避障。
2. 微分约束问题：要考虑曲线的曲率大小是否合适，曲率是否连续。
3. 全局约束问题：要考虑两点之间的最短路径问题。
4. 车辆的动力学约束问题：要考虑是否满足车辆横纵向动力学。

在自动驾驶场景中，驾驶环境是动态变化的，因此在运动规划中需要加入时间约束，为动态的行驶过程提供有效解。时间约束的增加给自动驾驶的运动规划带来了新的挑战。目前，常用的自动驾驶算法有基于图搜索的方法、基于随机采样的方法、基于曲线插值的方法、基于直接优化的方法等。在实际应用中，运动规划往往需要上述几种方法进行结合，才能达到比较好的规划效果。

• 基于图搜索的运动规划方法

解决运动规划的一大类算法是启发性搜索算法，其基本思想是将状态空间通过确定的方式离散成一个图，然后利用各种启发式搜索算法搜索可行解甚至是最优解。其中包含两个过程，状态空间建模和启发式搜索两个过程。

在离散搜索思路下，状态空间通过被建模为占据栅格(Occupancy Grid)或状态晶格(State Lattice)。占据栅格法将空间建模为离散栅格，根据障碍物的分布确定栅格是否被占据。与占据栅格不同，状态晶格法将规划区域划分为表征状态的栅格，两个栅格的连接则表征了它们之间的状态转移。

在状态空间建模后，利用图搜索算法得到从初始状态到终止状态的轨迹。图搜索算法包括Dijkstra算法和A*算法等，以及A*算法的变种。与占据栅格法相比，状态晶格法搜索得到的任意两栅格之间的路径可以用预先计算的晶格表示。

State Lattice图9所示[8]。将网格称为状态晶格，在其上应用了运动规划搜索，路径搜索基于包含所有可行特征的一组格子或图元的局部查询，从而使车辆从初始状态行驶到其他状态。在一个环境下先计算出来车辆所有可能的路径曲线，然后根据碰撞检测、交通规则等判断计算可行的路径，利用成本函数决定之间的最佳路径。成本函数可以是：任务到达cost、横向偏移cost、碰撞cost、纵向冲击度 cost、横向加速度cost、向心加速度cost等。

图9：State Lattice

• 基于随机采样的运动规划方法

通过对连续的状态空间进行采样，从而将原问题近似成一个离散序列的优化问题，从中筛选出目标样本点序列作为路径计算结果。最为常见的是概率路线图方法（PRM，Probabilistic Roadmap Method）以及快速扩展随机树方法（RRT，Rapidly exploring Random Tree）。

概率路线图(PRM):

PRM 算法首先在可行使空间中进行离线采样，构造出路线网络图（Roadmap），随后根据起始点与目标点在路线网络图中进行查询，得到可行的行驶路径。整个过程分为预处理阶段和查询阶段。

1. 预处理阶段：对状态空间内的安全区域均匀随机采样ｎ个点，每个采样点分别与一定距离内的邻近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图。
2. 查询阶段：对于给定的一对初始和目标状态，分别将其连接到已经构建的图中，再使用搜索算法寻找满足要求的轨迹。

快速扩展随机树方法（RRT）:

RRT采样过程中并不关心整个完整的空间，而是通过一个稠密的采样序列的引导，在每个扩张周期中对空间进行一次采样从而逐步递增的向整个空间扩张。随着采样次数的增加，搜索树逐步的覆盖整个搜索空间，如图10所示。

初始状态为根节点构建快速搜索树。每一个扩张周期算法通过随机采样序列的引导在状态空间中得到一个采样节点；随后，遍历搜索树，通过一个度量函数找到搜索树上距离采样节点最近的节点；连接两节点并选择合适的控制输入使得规划对象由搜索树节点向采样点运动扩张，得到扩张节点；最后将扩张节点添加到搜索树上完成该周期的搜索。重复上述周期性扩张，直到目标节点添加到搜索树上或到达最大循环迭代次数为止。

图10：均匀采样RRT扩张示意图

• 基于曲线插值的运动规划方法

曲线插值的轨迹规划方法的主要思想是基于预设的目标轨迹点（waypoint）

序列，重构出满足连续性、舒适性以及车辆动力学约束的路径。常用的曲线插值

算法包括 Clothoid 曲线拟合、多项式曲线拟合、样条曲线拟合、贝塞尔曲线拟合等。

Clothoid曲线[9]：

Clothoid 曲线是指曲率连续线性变化的一类曲线，因此 Clothoid 曲线生成的路径曲率是连续的，经常被用来连接直线路径与圆弧路径。实际上，许多行车道路的设计均是按照 Clothoid 曲线设计的。

Polynomial Curves（多项式拟合）[10]：

基于多项式轨迹规划方法的设计思想为根据车辆的初始状态和目标状态对变道轨迹进行规划，使车辆在指定的时间到达相邻车道。采样函数是函数f(x,y,t)，从描述的函数族类中寻找一条轨迹，能充分描述车辆从起始位置过渡到目标位置过程的动态特性。

随着多项式次数的变大，曲线的拟合效果越好，但次数的增也会导致参数求解的运算量指数增长，通常选用五次多项式进行变道轨迹的规划。在 x 向、y 向分别选用五次多项式构造变道轨迹的曲线簇。

样条曲线拟合：

样条是一种分段的多项式参数曲线，它可以被定义为多项式曲线、B样条曲线等。样条插值是一种工业设计中常用的、得到平滑曲线的一种插值方法。

三次样条也是其中用的较为广泛的一种。说白了，就是通过一系列值点，根据插值连续性、微分连续性和边界条件等构建一条光滑曲线的过程。

贝塞尔曲线拟合：

贝塞尔曲线有着很多特殊的性质,在图形设计和路径规划中应用都非常广泛。贝塞尔曲线完全由其控制点决定其形状,n个控制点对应着n-1阶的贝塞尔曲线，并且可以通过递归的方式来绘制。

• 基于直接优化的运动规划方法

基于优化的轨迹规划方法根据一个需要最大或最小化的优化指标及一些约束决策出一条最优轨迹。经典的框架是KIT[11]在Frenet lattice将横向轨迹和纵向轨迹进行分别优化。坐标系下的运动规划的输出式一个轨迹，轨迹是一个时间到位置的函数，即t->(x，y)。当求解这个函数时，其复杂度较高且计算量大，所以将规划分成横向规划和纵向规划，会有显著的特点。

Frenet坐标系

由于真实世界中的道路都是弯曲的，为了简化求解优化问题的参数表达，在自动驾驶中通常采用Frenet坐标系。笛卡尔坐标系下的主车位置以及障碍物位置被投影到Serret-Frenet坐标系下，以车辆前进方向为Station坐标轴，车辆前进方向的法线方向为Lateral坐标轴，车辆路径规划将在Station-Lateral坐标系（S-L）下进行。

图11：Frenet坐标系

因为在公路行驶中，我们总是能够简单的找到道路的参考线（即道路的中心线），那么基于参考线的位置的表示就可以简单的使用纵向距离S（即沿着道路方向的距离）和横向距离L（即偏离参考线的距离）来描述。

横纵向解耦优化

横向规划就是行车方向上的规划，可以看成是如何打方向盘的规划，它决定了轨迹的形状。

这个问题通常的解法分两种：

1. 一种是无车道的场景，比如在Free Space（自由空间）中规划或者泊车之类的场景，车辆一般处在低速行驶状态，缺乏车道线等先验信息，业界大多都用搜索等路径生成的方式去处理无车道场景。
2. 另一种是有车道的情况。虽然可以参考车道线信息，但是规划上想输出s→(x,y)函数，难度并不小。常见的做法是离线生成参考线，随后就可以将求解s→(x,y)的问题变为求解s→L的问题，L是指车辆在这个参考线上的横向偏移量。确定参考线后，通过把参考线离散化，采一些点出来，那么横向规划问题就转化为求解一个离参考线偏移距离的一个问题，即转化成s→L的问题。其计算约束是车辆行驶不跨越边界，避免碰撞，而优化的目标是要离参考线近，要离障碍物远，曲率不大，曲率变化率不大等。

纵向规划本质是对车辆在设定好的路径上的速度规划，决定了车辆在整个轨迹上的运动过程。求解这类优化问题，第一个约束是遵守交规（信号灯、限速、停车让行等），第二个约束是避免碰撞，第三个约束是乘坐舒适，也意味着车辆的速度变化率不大，加速度变化率不大，行驶速度也要尽量快一点（限速内）等。

本文参考资料

[1]  Bacha A, Bauman C, Faruque R, et al. Odin: Team victortango's entry in the darpa urban challenge[J]. Journal of field Robotics, 2008, 25(8): 467-492.

[2] Schwarting W, Alonso-Mora J, Rus D. Planning and Decision-Making for Autonomous Vehicles[J]. Annual Review of Control Robotics & Autonomous Systems, 2018, 1(1): annurev-control-060117-105157.

[3] Paden B, Cap M, Yong S Z, et al. A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles[J]. IEEE Transactions on Intelligent Vehicles, 2016, 1(1):33-55.

[4] Mariusz Bojarski, Davide Del Testa, Daniel Dworakowski, et al. End to End Learning for Self-Driving Cars. https://arxiv.org/abs/1604.07316.

[5]  Heuristics [EB/OL]. [2016-07-22].

http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html.

[6]  Montemerlo M, Becker J, Bhat S, et al. Junior: The stanford entry in the urban challenge[J]. Journal of field Robotics, 2008, 25(9): 569-597.

[7]  Olsson M. Behavior Trees for decision-making in Autonomous Driving[M]. 2016.

[8]  J. Ziegler and C. Stiller, “Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios,” in Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on. IEEE, 2009, pp. 1879–1884.

[9]  J. Funke, P. Theodosis, R. Hindiyeh, G. Stanek, K. Kritatakirana, C. Gerdes, D. Langer, M. Hernandez, B. Muller-Bessler, and B. Huhnke, “Up to the limits: Autonomous audi tts,” in Intelligent Vehicles Symposium (IV), 2012 IEEE, June 2012, pp. 541–547.

[10] W. Xu, J. Wei, J. Dolan, H. Zhao, and H. Zha, “A real-time motion planner with trajectory optimization for autonomous vehicles,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, May 2012, pp. 2061–2067.

[11] Werling M, Ziegler J, Kammel S, et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame[C]. //Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010.

转载自汽车人手记，文中观点仅供分享交流，不代表本公众号立场，如涉及版权等问题，请您告知，我们将及时处理。