一、解耦合策略

1、路径-速度解耦策略概述

        路径-速度解耦指的是将车辆的运动分成路径规划和速度规划两部分，对两个部分分别进行研究。

• 路径规划：

        假设环境是“静态的”，将障碍物投射到参考路径上，并规划一条避开它们的路径；

• 速度规划：

        接收路径规划中给定的路径，将静态和动态障碍物都投影到st图上，并在图中规划大致的速度。

        在路径规划中，基于当前的“静态”环境，生成路径边界来描述可通过的区域。并且这个可通行区域一般要求是凸集，这样能保证优化的质量。

        并且路径规划在frenet坐标系和cartesian坐标系下都可实现，如下图所示：

        在速度规划中，投影就是根据障碍物的预测信息，用红色和橙色标记为阻挡路径的障碍物。通过st图中障碍物投影与自车运动曲线（下图中红色曲线）的相对关系可以判断自车做出了何种策略。如假定最上方红色曲线为自车运动曲线，则自车对黄车做出了抢行的行为，因为在相同时间t，自车曲线具有更大的s；而对于下方第二条自车运动曲线，则是对黄车做出了让行的行为。

        s(t)的高阶导数信息也包含在st图中。

        在论文a real-time motion planner with trajectory optimization for autonomous vehicles中给出了路径速度解耦合的框架。

 

        在该论文中作者使用四次螺旋多项式对路径进行规划生成轨迹，再在速度规划中利用三次多项式在st图中生成对应的曲线。 路径速度解耦方法中，是先通过路径的集合和速度的集合生成对应轨迹的集合，再经过代价值计算选取最优曲线，再对最优曲线中的路径和速度进行优化。

        上图是百度Apollo EM Motion Planner的框架，它的大体思想是对多条lane进行并行操作，生成对应的轨迹并打分，选择其中最优的。在每条lane的内部处理中分为E-step和M-step，E-step主要是进行障碍物的投影，M-step则是进行路径或速度的动态规划和二次规划，结合路径和速度的规划生成轨迹交给控制模块处理，同样也传送给最初的输入。大致流程和上篇论文的思想相同，下面对这个EM Planner进行一些具体的介绍。

2、EM Planner路径规划

SL Projection

        路径优化器使用上一个周期的大致速度来估计与迎面来车和低速动态障碍物的相互作用。基于这个投影的过程，我们可以假设得到一个“静态的”环境 。

DP Path Decision        

        在该部分，在自车前采样多行点。行点之间的间距取决于车速、道路结构等；不同行的离散点通过五次多项式曲线进行平滑连接；通过上述步骤得到的一系列曲线，我们可以使用DP（动态规划）在图中找到最优路径，这样就得到了一条能够避开障碍物类似安全通道的粗略解。

        在动态规划过程中每条边的代价值计算设计如下：

        这个代价函数还是比较好理解的，第一项是衡量驾驶平顺性，主要考虑车辆的速度、加速度和加加速度三个指标；第二项主要考量车辆与障碍物的距离，采用分段函数的思想设置不同的值：当距离大于安全距离时为0，小于危险距离时给定一个较大的值，当介于安全距离和危险距离也根据距离设置惩罚值；第三项主要为车辆离道路中心的偏移距离设置代价值。

QP Path Optimization

        虽然DP方法有最优性保证，但搜索出来得到最优解只是在离散的图上达到最优，在连续空间上无法实现最优。为了保证最终下发的质量足够高要对dp的结果进行二次优化。在经过DP后，我们得到了一个可通行区域（凸集），对应上图黄色阴影部分，利用五次样条二次规划进行优化，目标函数的设计如下：

        在该函数中g(s)表示DP得到的最优路径，而不是上一步的车道中心线。

        在这一步中，对车辆施加线性化约束，不再看做简单的质点模型，此时车辆的运动要考虑航向的影响。

         该不等式成立，要满足航向角足够小。

        一般来说，在优化问题中若生成的最终曲线是多条五次样条曲线组合而成的，需要施加显性约束保证连接处的状态连续，即连接处的、和相等。

2、EM Planner速度规划

 SL Projection 

        与EM Planner路径规划的第一步相同，也是将障碍物投影到st中，只是在路径规划中只针对静态障碍物和低速的动态障碍物，而在速度规划中，将静态障碍物和动态障碍物都投影到路径规划器计算得到的路径上。

        速度规划的任务就是在st上找到最优曲线s(t)。

DP Speed Decision 

         与路径优化器类似，速度优化器也是在st图上找到最佳速度曲线，dp的结果包括各分段的大致速度，一个可行的凸区域以及障碍物速度决策。

        在离散栅格中，用有限差分法逼近导数：

        代价函数可设计为：

 QP Speed Optimization 

        分段线性速度曲线不能满足动态要求，需要样条QP曲线步骤来填补这一空白。与路径优化器中类似，使用基于五次样条的二次规划来生成更精细的廓形。五次样条速度优化的目标函数设计为：

         对该目标函数有如下线性约束：

         下面对EM Planner路径规划和速度规划中的DP和QP目的进行总结：

• 使用DP是在网格内进行搜索得到一个粗略的解；
• DP的解用于生成凸区域并指导QP过程；
• QP用于在最有可能包含全局最优的凸区域中搜索最优解。

3、EM Planner缺陷

• 动态规划的耗时太长

        DP为了获得最优解需要遍历图中的所有节点；

        相对于基于A*的算法，一个好的启发式函数能够快速地指导算法找到最优解。

• EM Planner中的速度规划是次优的

        例如，如果速度限制是与位置相关的，那么对应于相同时间戳的速度约束将会随着优化过程速度轮廓的更新而不断变化，这使得问题是非凸的。