Motion Planning中的问题与挑战

在自动驾驶系统中包含定位、感知、预测、决策规划和控制算法模块，其中决策规划模块相当于自动驾驶系统的大脑。SAE将自动驾驶分为L0-L5六个等级，随着自动驾驶等级的提供，决策规划模块的重要性也随之提高。本文将介绍自动驾驶motion planning领域目前存在问题与挑战。

1. 轨迹生成

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

• 基于采样搜索方法：Dijkstra、RRT、A*、hybird A*和Lattice等；
• 基于曲线插值：RS曲线、Dubins曲线、多项式曲线、贝塞尔曲线和样条曲线等；
• 基于最优化方法：Apollo的piece wise jerk等；
  上述方法一般都是相互结合一起使用的，比如多项式曲线需要对终端状态进行采样，贝塞尔曲线对控制点采样，hybird A*中使用到了RS曲线或者Dubins。
  
  下图是各种轨迹或者路径生成方法的优缺点，可见没有哪种方法是完美的，因此不同的工况下需要使用不同的算法。目前行业内主要应用的是多项式插值(高速)和最优化的方法。
  

2. motion planning问题与挑战

上述Motion Planning的方法，基本是解决轨迹生成问题，不同的方法适用于不同的场景。目前行业内的轨迹生成方法已经不是主要瓶颈，并且主流是采用最优化的方法。但是在Motion Planning领域内仍然存在许多挑战需要去攻克。主要分几个方面介绍。

• 最优性问题；
• 认知推理问题；
• 不确定性问题(Uncertainty/Probability)；
• Sigle Agent;
• Multiple Agent;
• 工程化问题；

2.1 最优性问题

全局最优是NP-hard问题[3]，为了实时性，行业内多数采用横纵向解耦的规划方法，但是这么做会牺牲最优性，在一些工况下不能得到良好的车辆行为，比如在超车[2]、对向来车、向心加速度约束处理、横向规划需要考虑纵向规划能力等。例如，当Ego Vehicle前方有一个减速行驶车辆时，横纵向解耦的方法一般只有当前方车辆车速降低到一定值时才会超车行驶，Ego Vehicle的行为表现就是会减速甚至停车，然后再绕障行驶，显然不是最优的行驶策略。如果采用时空一体化规划方法，则可以避免减速或者停车行为。下图中左图是解耦方法的示例，在前方有减速停车车辆是，ego vehicle会进行减速，右图是时空规划的示例，在前方车辆减速时会进行超车。

2.2 认知推理问题

2.2.1 地图拓扑推理

以Apollo为例，PNCMap模块从HDMap提取数据形成参考线，并且通过HDMap API查询道路元素，但同时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]中的左下图所示，由于定位误差导致从HDMap查询到的道路边界产生误差，从而使规划和车辆行驶轨迹在道路边界上。

论文将定位不确定性假设为高斯分布，并且定位模块可以计算出概率分布的期望与方差。论文将车辆坐标系转换到了UTM坐标系下，根据定位的高速分布情况和坐标变换公式，就可以计算出车辆周围环境在定位影响下的不确定性，如上右图所示，其中颜色越深表示不确定性越大，其不确定性计算公式主要由下式得到。可以发现距离Ego Vehicle越远其不确定越高，随着车辆的前进，其不确定性会被更新。路径规划方法采用了Lattice（五次多项式曲线）的方法，在cost计算时，增加了两个项目。一个是硬约束：规划路径上点的最大不确定性不能大于某一个阈值；二是在cost function中增加了不确定性的权重和。

2.3.2 感知不确定性

由于传感器噪声、车辆震动、行驶环境和不完善的算法，感知得到结果具有不确定性，甚至是错误的。由于感知的不确定性会造成motion planning结果的不安全性。一种简单的处理方式是加buffer，但是粗暴的处理方式会减小motion planning的可行域，可能造成过于激进或者过于保守的行驶策略。
论文[7]以装备了Around View Monitoring(AVM)的泊车应用为例，由于感知误差会使路径规划在实际超车停车位置，可能会发生碰撞。如下左图所示。论文将感知的不确定性建模为高斯分布，感知效果距离ego vehicle越远不确定性越高，如下右图所示。

以论文中整体架构如下左图所示：

• Parking space sampling：对距离ego vehicle最近的两个角点进行采样，将采样点看作是正态分布的，根据采样角点和设定的停空间的长度，计算ego vehicle后轴中心的停车点；
• Path candidate generation：采用ocp理论对每个采样点进行路径规划，其中将时域问题转化为Ferent坐标系下，并使用SQP求解非线性问题；
• Optimal Path Selection：使用utility theory进行最优路径的选择。Utility function为： $EU(s)=$