自动驾驶时空联合规划

1. 前言

自动驾驶中安全性是首要解决的，其中安全性要求轨迹与所有的障碍进行碰撞检测，其中关于动态障碍物的碰撞检测定义为：同一时间占据同一空间位置。因此只用路径或者速度曲线都无法准确检测碰撞，业内一般采用时空解耦的规划方法，即确定路径曲线之后，用速度曲线检查碰撞。

附赠自动驾驶最全的学习资料和量产经验：链接

2. 时空解耦规划

2.1 概述

时空解耦正是用迭代法先在假定固定速度的前提下求解路径，然后假定路径已经固定，求解速度。然而，迭代法成立有个前提，那就是需要迭代至收敛，因为只有在不动点上，我们才能讨论路径和速度两者共同决定的轨迹的好坏。我们可以考虑在一个规划周期内迭代多次求解路径-速度，但受限于算力，这么做不太现实。基于上一个规划周期的速度曲线解算这个周期的路径这种方法，可以理解为跨多个规划周期的迭代，也是可行的，但由于不同规划周期之间感知、预测等输入在持续变化，这样的迭代经常会遇到突跃、不收敛等问题。这些问题的根源都在于，路径和速度，也就是运动规划中的两个自由度，有不可忽视的相互影响和相互依赖。

2.2 EM Planner

EM Planner做为由百度阿波罗提出的运动规划问题求解器，其采用横纵向解耦的方法进行运动规划，先规划path在此基础上进行speed的规划求解；

EM Planner主要思路：

1. 将STL三维规划问题降维，分解成两个二维的SL与ST规划问题，上一帧的轨迹（Trajectory）优化当前帧的SL，SL优化ST，不断迭代。

2. 利用上一帧的规划轨迹以及当前帧的预测，对当前帧进行路径规划（SL）再进行速度规划（ST），其中路径规划与速度规划都是采用DP+QP的方法，最后将路径与速度结合生成轨迹送往控制模块。

3. 避障问题的二次规划是非凸的，cost function有多个极小值点，利用动态规划在离散空间中得到粗解；

4. 利用二次规划在凸空间上优化出最终解；

5. 连接两点曲线为五次多项式

EM Planner主要流程

step1：根据上一帧的规划轨迹以及当前帧的预测，将自车与障碍物交互区域投影到SL图上；

step2：SL图上lattice采样，采样点之间用五次多项式连接，计算各个连接点曲线的cost，采用DP迭代优化求解最优路径；

step3：根据DP求解的路径以及约束空间确定的凸空间，根据object函数利用QP求解出平滑且接近DP最优路径的最终路径；

step4：根据求解出来的最优路径，结合目标预测，将目标投影到ST图上

step5：直接在ST图上进行稠密的采点生成gridmap,map点之间直接用直线相连并计算每一段直线的cost，利用DP求出相应的粗解

step6：根据DP求解的速度曲线以及障碍物区域确定凸空间，根据object函数用QP求解出贴近粗解且平滑的最终速度曲线；

step7：路径和速度合成轨迹，传输给控制并用作下一周期规划条件

EM Planner小结

EM Planner的迭代过程存在一定的弊端，当预测的时间或者范围比较大，且动态障碍物比较稠密时，多轮迭代可能不收敛；
对问题进行降维处理，三维规划问题转化为两个二维规划问题，显著降低运算复杂度；

2.3 时空解耦规划缺点

无论是先规划路径后规划速度，还是先规划速度后规划路径，二者共同影响的因素只能留到最后，而先规划的部分很难考虑到这些因素。例如：常见的先路径后速度的框架下，我们在规划的时候无法知道最终的速度曲线，等到速度规划完成了，回头可能发现这样的速度下，早已经规划好的路径其实并不是最优的，甚至可能是很差的。

3. 时空联合规划

时空联合规划即同时求解路径与速度曲线，其将路径和速度曲线同时作为优化问题的变量，最终得到的为二者的最优组合，而不是在某个给定路径下的最优速度，亦或者是某个给定速度曲线下的最优路径。

3.1 时空联合决策

3.2 时空联合优化

3.3 时空联合规划可视化

时空联合规划中将横纵向自由度的整个轨迹当做一个整体考虑，可视化需要直接反映轨迹在时间和空间中的形状。

可视化方式：将高度Z这个维度作为时间维度，将轨迹可视化从低（现在）到高（未来）逐渐上升的曲线。

可视化结果分析

在此种可视化处理方式中，轨迹的倾斜程度反映速度，例如完全竖直即表示静止不动；

• 两个物体的时空轨迹如果经过同一个空间位置（交叉路口等），让行一方的轨迹会在上面（时间较大）；

• 两个轨迹近距离交错而过，对应着碰撞的危险情况，在水平方向（x/y）上接近表示两个物体在空间距离上的接近；

• 在垂直方向（z）上的接近表示两个物体在短时间内相继经过同一空间位置，如果速度快的话会比较危险；

在时空联合规划中，轨迹解算的约束条件为：避免碰撞危险等不希望发生的情况来设定约束条件；

3.4 时空联合规划初始解

数值优化效果的重要影响因素之一为初始解的质量，规划问题本身是一个非凸问题的优化，存在很多局部极值点，而基于梯度的数值优化方法，由于其根据函数的局部特征逐步收敛的工作原理，很难跳出初始解所选定的局部范围，即初始解决定了数值优化过程中会探索到哪些局部极值。

初始解的求解过程：在诸多局部极小值中找到最优解的过程，该过程对应“决策”过程；

如何获取可供选择的局部极小值？

拓扑学（topology）给我们提供了分析这个问题的工具。在轨迹空间中，有很多可行的轨迹是相似的，一个轨迹可以经过连续变换转化成另一个轨迹，而在变换过程中始终保持可行（不会跨过任何一个障碍物）；直观地讲，对这些轨迹的探索可能都会被同一个初始解覆盖到。如果我们把轨迹看作是空间位置对时间的连续函数，那么描述这种轨迹“相似性”的数学概念就是同伦（homotopy），相似轨迹的分组就是同伦类（homotopy classes），而对于每一个同伦类中的所有轨迹，我们只需要生成一个与之同伦的初始解，就可以覆盖了。枚举所有同伦类可以通过推广维诺图（generalized Voronoi diagram）等算法来实现。

Note
枚举所有同伦类可以通过推广维诺图等算法来实现，不过该领域在学术界还在活跃地研究中，在时空中如何高效地处理同伦类仍有待探索，而且有学者质疑同伦类可能并不是离散、定性地描述轨迹分类（也就是决策）的理想形式。时空联合规划能力的最大限度的发挥，离不开该领域的突破。

3.5 时空联合规划的优势：

在很多复杂的互动场景中，更符合人类司机驾驶的处理方式，即同时动用横向（方向盘）和纵向（刹车/油门）控制，通过二者的配合达到安全舒适的驾驶行为。时空联合规划具有较强的协调路径和速度的能力。