深度 | 无人驾驶的决策规划控制技术

雷锋网按：本文由李力耘与刘少山联合创作，前者为百度美国研发中心高级架构师，后者系PerceptIn联合创始人。文章首发于《程序员》，雷锋网(公众号：雷锋网)已获授权发布，如欲转载，请联系Heyc@csdn.net。

无人车作为一个复杂软硬件结合的系统，其安全可靠运行需要车载硬件、传感器集成、感知预测以及控制规划等多个模块的协同配合。本文作者认为最关键的部分是感知预测和决策控制规划的紧密配合。

狭义上的决策规划控制部分，包含了无人车行为决策（Behavior Decision）、动作规划（Motion Planning）以及反馈控制（Feedback Control）这三个模块。而从更宽泛的概念来说，无人车的决策规划控制模块，紧密依赖于上游的路由寻径（Routing）以及交通预测（Prediction）的计算结果，所以本文也对路由寻径和交通预测模块进行介绍。

系统框架和模块划分

图1展示了一种无人车软件系统的典型功能模块划分。

图1 无人车软件系统模块

其中感知（Perception）模块负责从传感器数据中探测计算出周边环境的物体及其属性。这些物体信息经过预测模块的计算，生成预测轨迹传递给决策规划控制系统中的行为决策模块。

决策规划控制系统的另一个上游模块是路由寻径模块，其作用在简单意义上可以理解为无人车软件系统内部的导航，即在宏观层面上指导无人车软件系统的控制规划模块按照什么样的道路行驶从而实现从起始点到目的地点。值得注意的是这里的路由寻径虽然在一定程度上类似传统的导航，但其细节上紧密依赖于专门为无人车导航绘制的高精度地图，所以和传统的导航还是有本质的不同。

一般来说，路由寻径会作为单独的模块来进行实现，而交通预测部分，则既可以做为感知模块的业务延伸，也可以看成是决策规划控制模块的外围模块而单独进行实现。

决策规划控制（Decision，Planning&Control）系统的任务，就是在对感知到的周边物体的预测轨迹的基础上，结合无人车的路由意图和当前位置，对车辆做出最合理的决策和控制。整个决策规划控制软件系统，可以按照解决问题的不同层面，如图1所示自上而下划分为行为决策（Behavioral Decision）、动作规划（Motion Planning）以及反馈控制（Feedback Control）这三个模块。

其中行为决策模块（Decision），可以直观理解成无人车的“副驾驶”。

行为决策接受路由寻径的结果，同时也接收感知预测和地图信息。综合这些输入信息，行为决策模块在宏观上决定了无人车如何行驶。宏观层面的决策包括在道路上的正常跟车，在遇到交通灯和行人时的等待避让，以及在路口和其他车辆的交互通过等。

例如，在路由寻径要求无人车保持当前车道（Lane）行驶，感知发现前方有一辆正常行驶的车辆，行为决策的决定便很可能是跟车行为。 

动作规划模块，在图1的划分中，解决的是具体的无人车动作（Motion）的规划问题。其功能可以理解为，在一个较小的时空区域内，具体解决无人车从A点到B点如何行驶的问题。动作规划模块在这里解决的问题，相对行为决策，又更加具体了一步。动作规划需要具体把一个短暂时间t内从A到B的中间路径点做出规划，包括选择途经哪些具体的路径点，以及到达每个路径点时，无人车的速度、朝向、加速度等。不仅如此，动作规划还需要保证两点：一是在后续时间内，生成从A到B的时空路径需要保持一定的一致性；二是，这些生成的A到B之间的路径点，包括到达每个点的速度朝向加速度等，都在下游的反馈控制的实际可操作的物理范围之内。

决策规划控制系统最下层的模块是反馈控制模块。这是一个直接和无人车底层控制接口CAN-BUS对接的模块。其核心任务是消化上层动作规划模块的输出轨迹点，通过一系列结合车身属性和外界物理因素的动力学计算，转换成对车辆Drive-By-Wire控制的油门、刹车以及方向盘信号，从而尽可能地控制车去实际执行这些轨迹点。反馈控制模块主要涉及对车辆自身控制，以及和外界物理环境交互的建模。

上述模块的划分方法，非常有效地将无人车决策控制规划这样一个复杂问题，按照计算逻辑从抽象到具体的做出了非常合理的切分。这样的划分使得每个模块可以各司其职专注解决本层次的问题，从而提升了整个复杂软件系统的开发效率。

预测模块（Prediction）

作为决策规划控制模块的直接数据上游之一，预测模块的作用是对感知所探测到的物体进行行为预测，并且将预测的结果具体化为时间空间维度的轨迹传递给下游模块。

一般而言，感知模块所输出的物体信息包括位置、速度、朝向以及物体分类（如车辆，行人，自行车）等物理属性。这些感知所计算输出的物体属性偏向于客观的物理属性。利用这些输出的属性，结合客观的物理规律，可以对物体做出一个在非常短时间内的“瞬时预测”。

预测模块所需要解决的问题，不仅仅局限于结合物理规律对物体做出预测，往往更重要的是结合物体和周边环境，以及积累的历史数据知识，对感知到的物体做出更为宏观的行为预测。例如在图2中，行为预测需要在宏观层面预测图中的车辆是否会保持直行还是右转通过路口。

图2 无人车周边物体行为预测

行为预测的轨迹，既包括了障碍物在将来一段时间内运动的方向，还体现了它们在运动中的速度变化。譬如行人过马路的时候会预测他们使用较为恒定的步行速度，车辆转弯的时候会先减速后加速，而加减速的快慢也取决于弯道的弧度和长短。实际的无人车系统中，往往将宏观层面的行为预测和轨迹生成抽象成两个问题来解决。

宏观层面的行为预测问题，往往可以抽象成经典的机器学习问题，并且利用基于大数据的深度学习技术来解决。例如， 在假设车辆按照高精地图划分的道路（Lane）行驶的前提下，我们可以认为在任何一个时刻，车辆可行驶的每一个Lane序列都是一个需要进行Binary Classification（二元分类）的样本。在这个假设下，我们不需要对直行、并道、路口拐弯等场景进行区分处理，因为无论是直行、并道，和路口拐弯，都可以统一看成是车辆在不同Lane序列上的行驶。车辆的宏观行为预测问题，便简化为对于Lane序列的Binary Classification问题。

图3 无人车行为预测中的Lane序列Binary Classification抽象

如图3所示，在t时刻，无人车主车位于Lane 1，此时按照Lane序列的可能展开途径，我们考虑三条轨迹：

• Trajectory 1： Lane 1、Lane 2、Lane 3对应路口右转；

• Trajectory 2：Lane 1、Lane 6、Lane 8对应路口直行；

• Trajectory 3：Lane 1、Lane 4、Lane 5、Lane 7对应换道后直行通过路口。

假设在t+w时刻，无人车经过Lane 6行驶到Lane 8的位置，那么轨迹Trajectory 2便成为该Binary Classification的正样本，其余两条轨迹便成为负样本。在这种基于Lane序列的问题抽象下，所有的正负样本可以从历史数据的回放中获得，并成为模型训练的样本数据。另一方面，模型的特征抽取可以结合一定时间内的如下信息来设计：

• 车辆本身的物理信息：速度、朝向等；

• 车辆相对于道路的信息：在Lane上的横向/纵向位移和速度，相对Lane边界的距离等；

• 车辆周边的其他物体的信息：车辆周围例如左右相邻Lane是否有障碍物等。

在上述的样本标签定义和特征抽取下，无人车Prediction部分的宏观行为预测可以很好的抽象成典型的机器学习问题来解决。在预测得出的宏观行为基础上，相关的轨迹生成和速度预测可以通过特定的规则或者物理模型来实现。

路由寻径（Routing）

无人车路径规划的Routing寻径问题，虽然也是要解决从A点到B点的路由问题，但由于其输出结果并不为实际的驾驶员所使用，而是给下游的行为决策（Decision）和动作规划（Planning）等模块作为输入，其路径规划的层次要更加深入到无人车所使用的高精地图的车道（Lane）级别。

如图4所示，其中的箭头线段代表高精地图级别的道路划分和方向。

图4 无人车路由寻径模块（Routing）的高精地图道路（Lane）级别寻径路由

Lane1、Lane2………Lane8构成了一条Routing输出的路由片段序列。可以看到，无人车地图级别的Lane划分并非和实际的自然道路划分对应。比如Lane2、Lane5、Lane7都代表了由地图定义绘制的“虚拟”转向Lane。类似的，一条较长的自然道路，也可能被划分为若干个Lane（例如Lane3、Lane4）。

作为整体无人车决策控制规划（Decision，Planning&Control）系统的最上游模块，路由寻径模块的输出严格依赖于无人车高精地图（HD-Map）的绘制。在高精地图定义绘制的路网（Road Graph）的道路（Lane）划分的基础上，以及在一定的最优策略定义下，路由寻径模块需要解决的问题是计算出一个从起点到终点的最佳道路（Lane）行驶序列：{(lane,start_posotion,end_position)i}，其中，(lane,start_posotion,end_position)i我们称作一个Routing Segment（路由片段），所在的道路由lane来标识，start_posotion,end_position分别代表在这条道路上的起始纵向距离和结束纵向距离。

图5 无人车寻径（Routing）基于Lane Point的有向带权图上的最短路径问题抽象

我们可以把无人车在高精地图的Lane级别寻径问题，抽象成一个在有向带权图上的最短路径搜索问题（如图5所示）。

路由寻径（Routing）模块首先会基于Lane级别的高精度地图，在一定范围内所有可能经过的Lane上进行分散“撒点”，我们称这些点为“Lane Point”。这些点代表了对无人车可能经过的Lane上的位置的抽样。这些点与点之间，由有向带权的边进行连接。Lane Point之间连接的权，代表了无人车从一个点行驶到另一个点的潜在代价（Cost）。在这样的有向带权图的问题抽象下，路由寻径问题可以利用常见的A*算法或者Dijkstra算法来进行实现。

行为决策（Behavioral Decision）

行为决策（Behavior Decision）层在整个无人车决策规划控制软件系统中扮演着“副驾驶”的角色。这个层面汇集了所有重要的车辆周边信息，不仅包括了无人车本身的当前位置、速度、朝向以及所处车道，还收集了无人车一定距离以内所有重要的感知相关的障碍物信息以及预测轨迹。行为决策层需要解决的问题，就是在知晓这些信息的基础上，决定无人车的行驶策略。这些信息具体包括：

• 所有的路由寻径结果：比如无人车为了达到目的地，需要进入的车道是什么（Target Lane）。

• 无人车的当前自身状态：车的位置速度朝向，以及当前主车所在的车道。

• 无人车的历史信息：在上一个行为决策（Behavioral Decision）周期，无人车所做出的决策是什么？是跟车，停车，转弯或者是换道？

• 无人车周边的障碍物信息：无人车周边一定距离范围内的所有障碍物信息。例如周边的车辆所在的车道，邻近的路口有哪些车辆，它们的速度位置如何？以及在一个较短的时间内它们的意图和预测的轨迹。周边是否有自行车或者行人，以及他们的位置速度轨迹等。

• 无人车周边的交通标识信息：一定范围内的Lane的变化情况。比如路由寻径的结果是在Lane1的纵向位移10m处换道进入对应的相邻Lane2的纵向位移20m处，那么Lane1的合法的纵向位移换道空间是多大？比如从一个直行Lane行驶结束，需要进入下一个左转Lane，两条Lane的交界处是否有红绿灯或者人行道？

• 当地的交通规则：例如道路限速，是否可以红灯右拐等等。

无人车的行为决策模块， 就是要在上述所有信息的基础上，做出如何行驶的决策。

可以看出，无人车的行为决策模块是一个信息汇聚的地方。由于需要考虑如此多种不同类型的信息以及受到非常本地化的交规限制，行为决策问题往往很难用一个单纯的数学模型来进解决。往往更适合行为决策模块的解决方法，是利用一些软件工程的先进观念来设计一些规则引擎系统。

例如在DARPA无人车竞赛中，Stanford的无人车系统“Junior”利用一系列cost设计和有限状态机（Finite State Machine）来设计无人车的轨迹和操控指令。在近来的无人车规划控制相关工作中，基于马尔可夫决策过程（Markov Decision Process）的模型也开始被越来越多得应用到无人车行为层面的决策算法实现当中。

简而言之，行为决策层面需要结合路由寻径的意图，周边物体和交通规则，输出宏观的行为层面决策指令供下游的动作规划模块去更具体地执行。其具体的指令集合设计则需要和下游的动作规划模块达成一致。

动作规划（Motion Planning）

在行为决策层下游的模块是动作规划（Motion Planning）。

其任务是具体将行为决策的宏观指令解释成一条带有时间信息的轨迹曲线，来给最底层的反馈控制来进行实际对车的操作。更具体而言，动作规划模块试图解决在一定的约束条件下优化某个范围内的时空路径问题。这里的“时空路径”指车辆在一定时间段行驶的轨迹。该轨迹不仅包括位置信息，还包括了整条轨迹的时间信息和车辆姿态：即到达每个位置的时间，速度，以及相关的运动变量如加速度，曲率，曲率的高阶导数等。

动作规划可以拆分成为两个问题：轨迹规划（Trajectory Planning）和速度规划（Speed Planning）来解决。其中轨迹规划只解决在二维平面上，根据行为决策和综合地图信息定义的某种Cost函数下，优化轨迹的问题；而速度规划问题则是在选定了一个或者若干个轨迹（Trajectory）之后，解决用什么样的速度来行驶的问题。，其中（x，y）表示车辆在二维平面的位置，θ表示车辆的朝向，k表示曲率（也即朝向θ的变化率），v表示车辆的速度（即轨迹任意点的切线速度）。车辆的这些姿态变量的标量大小满足如下关系： 

其中曲率的k大小往往由系统的输入限制条件决定。在此基础上，考虑一条由车辆运动产生的连续轨迹（Path）。我们称沿着轨迹的方向的位移为S方向。轨迹相对于车辆姿态的系统关系由下列偏微分方程式给出： 

我们的轨迹规划（Trajectory Planning）算法非常依赖于地图对于道路的定义。这里我们定义道路由其道路中心线（Center Line）所定义，且定义道路的采样函数为：，其中s代表道路的中心线切向方向的位移（也称为纵向位移s）。与此对应的是道路的中心线垂直方向位移l，也称之为横向位移。如果考虑一个车辆的姿态点p点在道路上（s，l）坐标，那么其实际的姿态和（s，l）的关系满足：

其中曲率K_r定义为在道路转弯的内侧曲率加大（随纵向位移l加大），外侧曲率则减小。我们使用右手坐标系，所以如图6所示在靠近原点处朝x轴的正方向，纵向位移l朝着y轴正方向加大。假设对于某条道路Lane(k)，其纵向宽度lk保持不变。那么该条道路变可以表示成为一个随着中心线横向位移s的点集{p(s,lk):s∈R+} 。我们称这样的一个坐标系统为坐标系统。

图6 XY平面下的SL坐标系统及其网格划分

在上述的车辆模型和道路模型下，我们讨论轨迹规划所产生的轨迹曲线。

首先我们定义车辆的轨迹（Trajectory）为一个从[0,1]区间到车辆姿态向量集合C={x⃗ }的连续映射：ρ:[0,1]→C 。其中，车辆的初始姿态向量为x⃗ =(x,y,θ,k)。每条轨迹终点处如图7所示，轨迹1的终点姿态为ρ₁(1)=q_end1轨迹2的终点姿态向量为ρ₂(1)=q_end2 ，初始姿态为ρ₁(0)=ρ₂(0)=q_init。

轨迹优化的目标便是在所有可能的轨迹曲线中，筛选出满足边界条件的轨迹曲线，再寻找一条/若干条平滑且Cost函数最低的曲线。其中轨迹的候选曲线我们用类似在路由寻径（Routing）模块中介绍的“撒点”的采样方式来生成。

参考图7，在某条Lane的SL坐标系下，我们按照均匀切分的S和L方向的方格内，在固定S和L间隔下，考虑每个（s_i，l_j）区域的中心点（如图7所示，又称为轨迹点Trajectory Point）。一条候选的轨迹（Trajectory）便可以看做是沿着Lane的中心线纵向位移s方向连接不同Trajectory Point的平滑曲线。在图7所示的道路SL分割和采样下，可能的Trajectory Point有16个（4个s位置，4个l位置），从车辆的初始位置出发，我们只考虑在s方向单调增大的可能，不考虑城市综合道路行驶中的倒车情况，那么总的候选曲线的总条数为4₄=256条。轨迹优化便是要在这256条候选的曲线中找出Cost最优的轨迹。

图7 SL坐标系下道路的分割采样以及可能的轨迹

我们采用多项式螺旋线来连接轨迹点Trajectory Point，从而生成候选的曲线。多项式螺旋线，如图8所示，代表了一类曲率可以用弧长（对应我们轨迹中的s方向）的多项式函数来表示的曲线簇。我们使用三阶（Cubic）或者五阶（Quintic）的多项式螺旋线，其曲率K和轨迹弧长S的关系K(S)为：或者。

图8 多项式螺旋线以及车辆姿态的螺旋线示意图

基于这种使用三阶（五阶）螺旋线连接的轨迹（Trajectory），其参数可以快速有效的通过梯度下降（Gradient Descent）的方法来搜索。以三阶多项式为例，我们考虑从车辆初始姿态q_init=(x_I,y_I,θ_I,K_I)到目标姿态q_goal=(x_G,y_G,θ_G,K_G)，且具有连续曲率的三阶螺旋线：

在初始状态 时，考虑曲率的一阶导数和二阶导数均需要满足初始状态的限制，我们可以得到：

这样使得实际未知参数减少到2个（K₃,S_G）,利用梯度向量我们可以快速寻找到非常接近初始状态限制的三阶螺旋线的参数。在上述的所有候选曲线中，我们可以根据业务的不同需要来设置Cost函数，然后选择出在任何时间点Cost最小且满足边界条件限制的曲线。由于候选曲线随着我们采样间隔随指数增长，往往将Trajectory Point建立成某种有向带权图，然后利用图论中的搜索方法结合Cost设置来选取最优曲线。具体的搜索方法可以参考中的动态编程方法。

在轨迹规划选定了一条或者若干条曲线后，速度规划部分将决定车辆以什么样的速度来通过这条曲线。轨迹规划在选取曲线时的Cost设置偏重于静态障碍物，而速度规划在选取曲线时的Cost设置应该注意以下几点：

• 动态的障碍物信息：如Prediction模块预测的轨迹信息；

• 上游Decision输出的宏观层面指令：如对某个障碍物需要避让（Yield）；

• 状态量的连续性限制：如速度，加速度等均不能跳变。

在此基础之上，速度规划可以将曲线的纵向位移S和时间变量T建立一个二维平面，将上述的限制投影在该平面上求解这个问题。这种考虑曲线纵向位移s和时间t的速度规划求解方式称之为S-T求解。类似得，如果考虑横向位移，也可以在三维的S-L-T空间内进行求解。

反馈控制（Feedback Control）

无人车反馈控制模块中常用的车辆控制模型为自行车模型。在该模型中，车辆姿态（Pose）是处于一个二维的平面坐标系内，并且可以由车辆所处的位置（position）以及车身和坐标平面的夹角（heading）来完全描述。同时我们假设车辆前后轮由一个刚性（rigid）不变的轴连接，其中车辆的前轮可以在一定的角度范围内自由转动，而车辆的后轮保持和车身的平行关系不能转动。前轮的转动对应实际车辆控制中方向盘的转动。

车辆的自行车模型所代表的车辆姿态如图9所示。

图9 车辆控制的自行车模型

这里我们使用一个基于x-y的二维平面，其中和分别代表其x和y方向的单元向量。向量p_r和向量p_f分别代表车辆后轮和前轮与地面的接触点。车辆的朝向角θ代表车辆和x轴的夹角（即向量p_r和单元向量的夹角）。方向盘转角δ定义为前轮朝向和车辆朝向角的夹角。其中前后轮与地面接触点的向量p_f和p_r之间满足： 

其中和分别代表车辆前后轮在和地面接触点处的瞬时速度向量。考虑车辆的后轮速度在x-y轴的投影标量和以及后轮的切向速度，那么上述的向量p_f和p_r之间的关系限制在后轮相关分量上的表现形式为：

其中 l 代表车辆前后轴中心间距。类似地，用车辆前轮相关分量的表现形式为：

这里前后轮的切向速度标量大小满足： 。

在上述的车辆模型下，反馈控制（Feedback Control）需要解决的问题便是找到满足车辆动态姿态限制的方向盘转角δ∈[δ_min,δ_max]的以及前向速度v_r∈[δ_min,δ_max]。而对这些状态量的控制可以是一个典型的PID反馈控制系统（如图10所示）。其中e(t)代表当前的跟踪误差，而这个跟踪的变量误差可以是轨迹的纵向/横向误差，角度/曲率误差或者是若干车辆姿态状态变量的综合误差。其中P控制器代表对当前误差的反馈，其增益由K_p控制；I和D控制器分别代表积分项和微分项，其增益分别有K_I和K_D来控制。

图10 基于PID的反馈控制系统

具体到无人车的反馈控制（Feedback Control）模块，我们需要解决的问题是控制车辆尽可能遵循上游动作规划（Motion Planning）所输出的时空轨迹。可以使用两个基于PID反馈控制的控制器来分别控制方向盘转角δ以及前进速度vs。

结语

笔者认为整个无人车广义决策规划控制（Decision，Planning&Control）范畴下的路由寻径（Routing），行为决策（Behavior Decision），动作规划（Motion Planning）以及反馈控制（Feedback Control）等几大模块，在当前的学术和工业界都有一些较为成熟的解决方案可以借鉴。

这些解决方案有些有着牢固的理论基础和数学推导，还有的在实际的无人车相关比赛中有着的出色发挥。事实上，我们认为单独看每个层面需要解决的问题，都不是非常困难。如何将整个无人车决策规划控制的问题有效清晰地划分到不同的模块，并且将各个上下游模块的解决方案配合起来达到整体的协调效果，才是无人车广义决策规划控制的难点和挑战所在。

因此我们并不着力于以调研的形式介绍所有的各个模块层面的现存解决方案，而是着眼于清晰地提供一套有效划分无人车决策控制规划这一复杂问题到不同层面子问题的方法。我们试图向读者展示，如何有效地将无人车决策控制规划这样一个复杂问题自上而下地进行分割，并且明确每个层面需要解决的具体问题的范围和限制。我们希望通过展示这样的“分而治之”的决策控制规划解决思路，能有益于读者对整个无人车软件系统的运作的了解。

雷锋网注：

李力耘，百度美国研发中心高级架构师。在百度无人车部门负责无人车行为预测方向的系统架构及算法优化，拥有多项国际专利，其中已递交三十余项无人车决策预测相关专利申请。

刘少山，PerceptIn联合创始人。加州大学欧文分校计算机博士，研究方向：智能感知计算、系统软件、体系结构与异构计算。现在PerceptIn主要专注于SLAM技术及其在智能硬件上的实现与优化。

雷锋网版权文章，未经授权禁止转载。详情见。