在自动驾驶的定位、感知、预测、决策规划和控制等模块中，感知模块就像是人的眼睛和耳朵，负责对外部环境进行感知；控制模块就像人的双手和双脚，负责最终的加减速、转向等操作；而决策规划模块就像人的大脑，基于接收到的感知等信息进行行为决策和轨迹生成。

正如人的大脑又分为左脑和右脑一样，决策规划模块又可以继续分为行为决策层（Behavioral Layer）和运动规划层（Motion Planning）。

随着自动驾驶等级的不断提高，决策规划层作为自动驾驶的大脑，其重要性也随之提高。但与人脑相比，自动驾驶的这颗大脑还有太长的路需要追赶。本文将通过万字详述路径规划中的Motion Planning存在的问题与挑战。

1. Motion Planning常用算法

Motion Planning算法是从机器人领域发展起来的，逐渐发展出适用于自动驾驶领域的各种算法。论文[1]对Motion Planning的轨迹生成方法做了综述，介绍的方法如下图所示。

• 基于采样搜索的算法：Dijkstra、RRT、A*、hybird  A*和Lattice等；

• 基于曲线插值的算法：RS曲线、Dubins曲线、多项式曲线、贝塞尔曲线和样条曲线等；

• 基于最优化的算法：Apollo的piecewise-jerk等；

上述算法一般都是相互结合在一起使用的。比如多项式曲线需要对终端状态进行采样、贝塞尔曲线对控制点进行采样、hybird A*中使用到了RS曲线或者Dubins曲线等。

论文[1]总结了各种轨迹生成算法的优缺点，如下图所示。可见没有哪一种算法是完美的，需要结合具体的场景和工况选用合适的算法。目前行业内应用比较多的是多项式曲线插值(高速场景)和最优化的算法。

2. Motion Planning问题与挑战

上述介绍的Motion Planning的算法，基本能解决大部分的自动驾驶场景轨迹生成问题，轨迹生成算法已经不是主要瓶颈。但是在Motion Planning领域内仍然存在许多挑战需要去攻克，主要包括以下几个方面：

• 最优性问题；

• 认知推理问题；

• 不确定性问题(Uncertainty/Probability)；

• Single-Agent；

• Multiple-Agent；

• 工程化问题。

2.1 最优性问题

2.2 认知推理问题

2.2.1 地图拓扑推理

以Apollo为例，PNC Map模块从HD Map模块提取数据形成参考线，并且通过HD Map模块的API接口查询道路元素。但Motion Planning模块会忽略了一些道路的拓扑关系，例如汇入汇出路口，而这些特殊的道路拓扑是会影响到车辆的行为。

此外，在没有HD Map模块而单纯依靠视觉车道线的情况下，此时感知车道线会发生异常。在汇入汇出道路和十字路口道路中，其道路拓扑问题尤为凸显。

2.2.2 障碍物统一建模

交通场景的参与者有车辆、摩托车、自行车、行人、锥桶等。广义上来讲还包括人行横道、红绿灯、道路限速等地图静态元素，Motion Planning需要针对不同的元素做出不同的决策。障碍物统一建模可以简化问题，并且提升计算效率。

• Aopllo将所有交通参与者抽象为Static Obstacle，Dynamic Obstacle和Virtual Obstacle，Obstacle就是box， Static Obstacle和Dynamic Obstacle为车辆、行人等， Virtual Obstacle为人行横道、禁停区等。路径规划时不考虑Virtual Obstacle。

• 使用能量场相关的方法，将交通参与者使用能量函数表示。上图中间图就是清华[4]提出的行车安全场，由静止物体的势能场、运动物体的动能场和驾驶员的行为场构成。最优轨迹就是寻找一条能量和最小的轨迹。

• 论文[5]将交通参与者分为obstacle-like和constraint-like。obstacle-like是动静态车辆、红灯等，将其映射到slt的3D栅格中。constraint-like是限速、停车标志等，作为semantic boundary。根据决策序列动作在slt配置空间内生成若干cube边界供轨迹生成使用。

2.2.3 场景认知推理

由于现实中环境的复杂性，一种决策策略或者规划方法难以处理不同的工况。因此对行驶环境进行分类，在不同的场景下选择不同的策略可以提升Motion Planning的性能。那么怎么进行场景分类和场景识别，在不同的场景Motion Planning又该有哪些不同？这些问题都是需要解决的。

• Aopllo中场景分类为LANE_FOLLOW、SIDE_PASS、STOP_SIGN_UNPROTECTED等。有两种场景识别方式，一是通过规则的方法，一是通过机器学习的方法。不同的scenario有不同的stage，stage中依次执行task。即使是相同的task在不同scenario中参数配置也可能不同。

• 毫末基于城市场景路口多、拥堵多和变道多的特点，将行驶场景分为十类，显然是和Apollo中的scenario分类是不同的，然而毫末的场景识别方法却不得而知。毫末此外还提出了行驶环境熵的概念来描述行驶环境的拥堵状态。

2.3 不确定性

2.3.1 定位不确定性

在多数的Motion Planning中都是认为定位是足够准确的，但是实际场景中由于遮挡、多径干涉等问题，定位往往是不准确的。以论文[6]中的左下图所示，由于定位误差导致从HD Map模块查询到的道路边界产生误差，从而使规划和车辆行驶轨迹在道路边界上。

论文将定位不确定性假设为高斯分布，并且定位模块可以计算出概率分布的期望与方差。论文将车辆坐标系转换到了UTM坐标系下，根据定位的高速分布情况和坐标变换公式，就可以计算出车辆周围环境在定位影响下的不确定性，如上右图所示，其中颜色越深表示不确定性越大，其不确定性计算公式主要由下式得到。

可以发现距离ADC越远其不确定越高，随着车辆的前进，其不确定性会被更新。路径规划方法采用了Lattice（五次多项式曲线）的方法，在cost计算时，增加了两个项目。一个是硬约束：规划路径上点的最大不确定性不能大于某一个阈值；二是在cost function中增加了不确定性的权重和。

2.3.2 感知不确定性

由于传感器噪声、车辆震动、行驶环境和不完善的算法，感知得到结果具有不确定性，甚至是错误的。由于感知的不确定性会造成Motion Planning结果的不安全性。一种简单的处理方式是加buffer，但是粗暴的处理方式会减小Motion Planning的可行域，可能造成过于激进或者过于保守的行驶策略。

论文[7]以装备了Around View Monitoring(AVM)的泊车应用为例，由于感知误差会使路径规划在实际超车位置停车，可能会发生碰撞，如下左图所示。论文将感知的不确定性建模为高斯分布，感知效果距离ADC越远不确定性越高，如下右图所示。

论文中整体架构如下左图所示论文，采用此算法后的效果如下右图所示。

• Parking space sampling：对距离ADC最近的两个角点进行采样，将采样点看作是正态分布的，根据采样角点和设定的停空间的长度，计算ADC后轴中心的停车点；

• Path candidate generation：采用ocp理论对每个采样点进行路径规划，其中将时域问题转化为Ferent坐标系下，并使用SQP求解非线性问题；

• Optimal Path Selection：使用utility theory进行最优路径的选择。Utility function为：EU(s) = P(s) x Uideal(s) +(1-P(s)) x Ureal(s)，其中P(s) 为路径对应采样点的概率，Uideal为路径到目标点(当前时刻感知检测到的，并非采样得到的)的偏差效用函数值， Ureal为路径上到ADC当前位置的效用函数值。

2.3.3 预测不确定性

预测是实现L4以上高级别自动驾驶的重要环节。然而截至目前，预测对整个行业来说仍是一个非常难的问题。因此预测的准确性很差，在不确定性预测结果下做Motion Planning是非常重要的。

论文[8]提出了一个基于高斯分布的规划架构，处理预测和控制不确定性带来的规划轨迹不安全的问题。

• 候选轨迹生成：通过多阶段横纵向采样生成。可以理解为Aopllo Lattice方法。

• 预测轨迹生成：对于某一个车辆的轨迹进行预测(进行规划)时，认为其他车辆是匀速行驶的，并且其状态都是确定的，则通过对候选轨迹的cost计算，得到最优的预测轨迹。之后通过卡尔曼滤波计算预测轨迹的概率分布，并假设其遵从正态分布。

• ADC轨迹生成：此时需要考虑其他交通参与者的预测的不确定性。针对每一条候选轨迹，通过LQR算法计算出控制误差，然后再通过卡尔曼滤波计算出轨迹的概率分布，在轨迹评价进行cos计算时，碰撞检测是基于预测和ADC规划轨迹的概率分布的，即在所有概率分布内都不能发生碰撞。

作者认为此方法相当于给box加上一个自适应的buffer，而常规的固定大小的buffer会导致保守或者激进的驾驶行为。

论文[9]论文提出了一种可以嵌入现有Motion Planning框架的fail-safe机制，分为三部分：

• Set-based prediction：根据制定的交通参与者的驾驶策略和车辆运动学模型，将原有的交通参与者单一的预测轨迹，改为多预测轨迹；

• Fail-safe trajectory：根据预测的结果，计算原planning trajectory有碰撞风险的第一个轨迹点，然后再根据最优化理论生成轨迹；

• Online verification：将ADC在第二步生成的轨迹上进行投影，判断其是否和第一步的预测车辆轨迹是否有碰撞。

感觉此方法是又重新做了一遍Motion Planning，由于论文中没有描述fail-safe trajectory是否考虑decision的结果，可能会造成safe trajectory不满足decision结果，并且此论文只是仿真，并没有实际应用。

2.3.4 Partially Observable Environments

由于传感器自身的感知范围受限和感知结果的不确定性，在不良光照或者恶劣天气中会进一步放大。而在城市工况中，建筑物的遮挡会造成不完全感知，如下图所示。此外，大型车辆也会造成感知遮挡问题，而多数的Motion Planning都是以完全感知进行处理的，规划结果具有很大的不安全性。

论文[10]提出了一种处理不完全感知的安全的Motion Planning，使规划轨迹在最危险情况下可以在车辆最大制动能力下安全停车而不发生碰撞。分为两种情况：一是在直道上行驶考虑感知的不确定性和感知距离范围，二是在城市十字路口考虑不完全感知情况。并且容易嵌入其他的Motion Planning架构中，作者在其之前提出基于最优化方法的轨迹规划中进行了仿真验证(综述中的图(b))。

作者为其理论设计了几个假设：

• 定位的纵向位置和速度信息遵从高斯分布；

• 感知的有效范围是已知的，并且感知的结果遵从高斯分布；

• 地图信息中包含建筑物位置，且为凸多边形；

• 使用Intelligent Driver Model(IDM)进行车辆加速度预测。

由于论文分直道和十字路口两种情况处理，因此需要进行场景识别，论文采用了基于规则的方式进行场景识别。

• 上左图：红色虚线是感知观测到的环境的时间，黑色虚线是进行Motion Planning的时间，可见Motion Planning使用的感知信息是tp时刻前的。此外由于Motion Planning要保证连续性，在Motion Planning计算周期tpin时间内的规划轨迹要保证一致。更重要的是由于执行器的延迟，在tsafe时间内要保证轨迹的安全性。论文中tsafe= 2tpin；

• 上中图：在直道行驶分为感知范围内没有车辆或者感知范围内有车辆两种情况：一是感知范围内没有车辆，假设驾驶感知范围外有一个静止车辆，将其设为虚拟静止障碍物，通过其高斯分布特性可以计算得到tsafe时刻内，满足以最大制动能力刹车的纵向位移和速度约束；二是感知范围内有车辆，考虑感知不确定性情况下的最危险情况，即前车以最大制动能力刹车，通过其高斯分布特性可以计算得到tsafe时刻内，满足以最大制动能力刹车的纵向位移和速度约束；

• 上右图：在十字路口行驶，根据IDM模型计算ADC是需要让行还是有路权需要明确表明表明自己优先通过的意图。最后转换为直道行驶的两种类型的约束。

2.4 Single Agent

Single Agent认为是单智能体问题，即ADC会对周围环境做出决策，而不考虑ADC行为决策对其他交通参与者的影响，显然这种假设是不对的，但是却简化了Motion Planning问题。

行为决策是影响自动驾驶发展的另一个重要方面，随着自动驾驶的等级越高，行为决策的重要性越高。行为决策的难点是如何体现自动驾驶车辆的智能性，如何使自动驾驶车辆可以像人类驾驶员一样处理高维度、多约束的复杂场景，甚至要比人类驾驶员的表现更好。

目前多数方法是基于规则的方法，其能力有限。以基于规则方法的行为决策来说，在下匝道工况，一般会设计一个距离匝道口的距离阈值。当ADC到匝道口的距离在阈值内时，就开始向最右侧车道变道。

假设这个阈值是2km，如果ADC在匝道口2.1m处位于中间车道行驶，此时前方刚好有辆车且速度较低，基于规则的行为决策一般会选择向左侧车道变道(左侧车道限速高，超车遵从左侧超车，从小鹏NGP等可以看出也是左侧车道优先)。但是变道后距离匝道口的距离阈值小于2km，此时需要向最右侧车道变道，需要连续进行二次变道，会显得不够智能。

再比如在匝道前500m最右侧车道行驶，前方由于施工或者事故不能行驶，此时只能由驾驶员接管。由此可见，由于现实工况的复杂性，基于规则的行为决策方式很难做到良好的驾乘体验。

港科大关于OPMDP[11]的行为决策工作，相比于基于规则的方法，性能有了一定提升，其对ADC和其他交通参与者的行为进行了剪枝，降低了OPMDP的耗时。但是其考虑了其他交通参与者会对ADC的行为进行规避等，可以看出是一个Multiple Agent问题来处理。

2.5 Multiple Agent

上述的Single Agent中认为交通参与者不会对ADC的行为做出相应的决策.但实际中，当ADC做出决策后，其行为会影响到其他交通参与者的行为，而使原有的预测结果的可信性降低，尤其是有些简单基于规则的prediction不依赖于Motion Planning结果，或者使用上一帧Motion Planning的结果(Apollo)。

例如在下图左图中，当ADC L沿着trajectory1行驶时，A2可能会减速避让。当ADC L沿着trajectory2行驶时，A2可能会加速通过路口。但是当ADC L沿着trajectory2行驶时，预测A2可能会加速通过路口，但是A2可能会理解错ADC L的意图进行减速，会造成两辆车锁死。因此ADC怎么理解其他交通参与者的意图和怎么让其他交通车理解ADC的意图至关重要[12]。

2.6 工程化问题

在Motion Planning中还面临一些工程化问题，主要包括如下几个方面。

• 实时性：在第一个问题中提到的了最优性问题，如果要解决，由于在三维空间搜索计算的复杂性，其实时性很能保证，这也是限制时空联合规划应用的一个原因。此外最优化算法中的大规模约束和非线性也面临实时性的挑战。

• 完备性：插值、Lattice等算法是概率完备的，尤其在复杂多障碍物环境中，有限的采样很难获得无碰撞的轨迹。而最优化方法由于数值求解，也不能达到完备性，常用的osqp求解器甚至会给出一个错误的解。

• 难量化性：Motion Planning中的评价指标多是主观性的，比如舒适性和通过性等，很难量化评价。不同工程师调参得到体感不同，又与乘客的主观感受不同。因此提出了机器学习的方法来学习Motion Planning中的参数或者变道策略。

3. 行业解决方案

针对上述问题与挑战，行业内公司也在积极探索并提出了一些解决方案，下面列举一二。

• 轻舟智航采用了时空联合规划解决最优性问题，提高规划性能，并且自研了非线性规划器高效求解[2]。

• 图森未来新一代框架中，感知模块在提供障碍物位置、速度等信息时，同时提供不确定性或者概率信息，以保证决策规划可以提前做出安全舒适的决策[13]。

• 特斯拉将planner用于交通参与者的其他车辆。但与其他车辆交互时，不能只为ADC规划，而是要为所有交通参与者共同规划，针对整体场景的交通流进行优化。为了做到这一点，会为场景中的每个参与对象都运行autopilot规划器。除此之外，针对停车场景，采用A*搜索算法和神经网络结合策略，大大减少了A*算法的节点探索[15]。

• 小鹏和特斯拉针对车道线缺失，道路拓扑变化问题做了优化[14]。

• Waymo提出了ChauffeurNet用于提升决策性能[16]，Apollo借鉴ChauffeurNet提出了自己的强化学习架构[17]。

参考文献

[1] D. González, J. Pérez, V. Milanés and F. Nashashibi, "A Review of Motion Planning Techniques for Automated Vehicles," in IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135-1145, April 2016, doi: 10.1109/TITS.2015.2498841.

[2] 扬奇直播课堂：《自动驾驶中的决策规划技术》轻舟智航

[3] 自动驾驶中轨迹规划的探索和挑战

[4] 基于动态行车安全场的智能网联汽车决策规划方法研究 

[5] W. Ding, L. Zhang, J. Chen and S. Shen, "Safe Trajectory Generation for Complex Urban Environments Using Spatio-Temporal Semantic Corridor," in IEEE Robotics and Automation Letters, vol. 4, no. 3, pp. 2997-3004, July 2019, doi: 10.1109/LRA.2019.2923954. 

[6] Artunedo, Antonio, et al. "Motion planning approach considering localization uncertainty." IEEE Transactions on Vehicular Technology 69.6 (2020): 5983-5994. 

[7] Lee, Seongjin, Wonteak Lim, and Myoungho Sunwoo. "Robust parking path planning with error-adaptive sampling under perception uncertainty." Sensors 20.12 (2020): 3560. 

[8] W. Xu, J. Pan, J. Wei and J. M. Dolan, "Motion planning under uncertainty for on-road autonomous driving," 2014 IEEE International Conference on Robotics and Automation (ICRA), 2014, pp. 2507-2512, doi: 10.1109/ICRA.2014.6907209. 

[9] Pek, Markus Koschi Christian and Matthias Althoff. “An Online Verification Framework for Motion Planning of Self-driving Vehicles with Safety Guarantees.” (2019). 

[10] Ö. Ş. Taş and C. Stiller, "Limited Visibility and Uncertainty Aware Motion Planning for Automated Driving," 2018 IEEE Intelligent Vehicles Symposium (IV), 2018, pp. 1171-1178, doi: 10.1109/IVS.2018.8500369. 

[11] L. Zhang, W. Ding, J. Chen and S. Shen, "Efficient Uncertainty-aware Decision-making for Automated Driving Using Guided Branching," 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 3291-3297, doi: 10.1109/ICRA40945.2020.9197302. 

[12] Autonomous vehicles' intended cooperative motion planning for unprotected turning at intersections

[13] 图森未来在重卡自动驾驶的最新落地与实践 | 王乃岩

[14] 重新定义 LCC？城市道路小鹏 P5 辅助驾驶VS 特斯拉 Model 3

[15] Tesla AI Day 2021完整视频

[16] ChauffeurNet Learning to Drive by Imitating the Best and Synthesizing the Worst

[17] Apollo决策规划强化学习