Apollo决策规划算法学习Chapter2 路径规划算法

Apollo决策规划算法学习系列目录

第一章 Apollo决策规划算法基本概念
第二章 Apollo决策规划之路径规划算法
第三章 Apollo决策规划之速度规划算法

---

---

前言

本文为第二章，主要讲解 Apollo决策规划算法中的路径决策算法，EM planner的路径决策算法是通过动态规划和二次规划实现的，下面来分两部分细讲路径决策算法中的动态规划和二次规划算法。

---

一、路径决策算法：动态规划

Notes

1.1 离散空间找粗解：动态规划

1）在Apollo决策规划算法学习的第一章中，我们已经讲过无人车怎样进行避障，下面我们针对第一章说的找粗解进行详细的讲解；第一章已经说明了怎么生成参考线，假设我们现在已经有了参考线的信息，然后就需要把车辆投影到参考线上，并以投影点为坐标原点，建立Frenet坐标系，同时把障碍物也投影到参考线上，生成SL图；
2）大家是不是认为规划的起点都是通过车辆的位置投影到参考线上，然后得到投影点在SL坐标系下的坐标，以此点为路径规划的起点就可以了，这么认为其实不太正确，因为这样得到的规划轨迹在控制看来就是如下图所示，是离散的；

所以只有起点的时候可以用上面的方法来找规划的起点，其他位置正确的做法是判断当前车的位置与上个周期规划的结果是否差别很大，如果差别很大那就把车辆的位置投影到参考线上，然后得到投影点在SL坐标系下的坐标，以此点为路径规划的起点就可以了；如果差别不大，就需要把当前的位置投影到上个周期规划的轨迹上，再投影到参考线上的点作为规划起点；这样得到的轨迹就是如下图所示；

下面以图解的方式来看一看无人车通过动态规划得到避障的粗解的过程：

1.2 控制接口，轨迹拼接

1）规划出来的轨迹输入到控制模块后需要有反馈看车辆是否按照规划的轨迹行驶，没有反馈的话，规划做错了也不知道，因此规划模块的输出给控制需要涉及到规划和控制的接口问题；一般来说控制的周期为10ms，规划的周期为100ms，这意味着需要在10个周期内跟踪同一个规划进行控制，这样控制效果不是太好，接下来我们讲一下关于这个问题的解决方案；
2）上面已经提出来一个问题，那么规划模块的输出只有位置、速度、加速度、航向角等信息，但是却没有时间信息，我们在规划输出的信息中添加时间信息，轨迹上的每个点都带有时间戳，这样就可以搜索当前的时间对应的轨迹了，规划发出一条带时间的轨迹给控制模块就可以了；
3）目前还有问题就是规划需要100ms的时间，那么规划的起点是本周期规划模块开始规划轨迹的时间T还是T+100ms呢；还有另外一个问题就是为什么本周期开始规划的起点不是当前车辆所在的位置呢？
下面来解决这两个问题：首先说一下规划的起点是指本周期开始规划的位置，规划的起点应该是T+100ms，原因是规划需要100ms的时间，这100ms内跟踪的是上个周期的轨迹，那么即使你从时间T所在的位置作为规划起点，但是规划完成是在T+100ms，你从T+100ms才开始跟踪这次规划的轨迹，会出现如下图所示的情况：

T时刻作为规划起点，会造成规划的轨迹的不连续性，T+100ms作为起点得到的轨迹如下图所示：

这里我们也相当于解释了车辆当前定位的点为什么不能作为规划的起点，因为执行本周期规划的轨迹是在100ms以后，你在这里作为规划起点会造成几个周期之间规划轨迹的不连续性。

1.3 轻决策与重决策

1）首先来看一下什么是轻决策和重决策：

2）说详细一点，重决策就是给予人给定的规则（综合与障碍物的距离、相对速度、周围环境等信息）给出决策，它是一种场景与决策的映射关系，是人为事先设计好的；
缺点：场景太多了，无法完全覆盖；同时人给出的决策所开辟的凸空间未必满足约束；
优点：计算量小；在感知受限的情况下仍然能做决策，因为结合了专家的智慧；
3）轻决策就是一种离散空间，通过设计代价函数，使用动态规划算法求解离散空间的最优路径的方法，它不需要先验的规则；
优点：无需人为规则就可以处理复杂场景；通过设计代价函数，可以使找到的粗解满足约束，使二次规划求解成功的几率大大增加；
缺点：复杂场景计算量大；依赖预测；对周围环境感知、定位要求高；
总的来说目前L2、L3级别的自动驾驶都是使用重决策方法，L4级别的自动驾驶使用轻决策方法。

1.4 路径二次规划算法

1）二次规划是非线性规划中的一类特殊数学规划问题，在很多在非线性优化问题都有应用，二次规划的形式上一节已经说过了，我们来看一看怎么得到二次规划的等式约束和不等式约束；

2）得到了等式约束和不等式约束后，还有二次规划的目标函数呢，路径规划中目标函数就是损失函数，定义如下，二次规划计算出结果后，然后我们就完成了路径规划。

二、路径规划总结与疑难解答

2.1 靠近障碍物时，二次规划无解

1）在进行路径规划的时候.会出现靠近障碍物的时候，可能会出现二次规划无解的情况，这是因为规划的起点约束可能与碰撞约束冲突造成的，在离障碍物很远的时候轨迹拼接的起点与避障的凸空间边界离得比较远，看下图的图解可能比较清晰；

大家可能会问为什么规划的路径贴着边界，这是由二次规划的性质决定的，带约束的二次规划的最小值只会在极小值点或者边界点取得，我们有两种解决方案：1.增大障碍物的边界，不管规划的起点冲突问题，因为把障碍物边界放大后自然就不会冲突了；2.增大Wcentre的权重，使极小值点更靠近道路中心线（这种方法的缺点是Wcentre调大的幅度小不起作用，幅度大路径不平滑，因此这个权重的调整需要合适）；

2.2 无人车方向盘大幅抖动，车辆抖动

1）这个问题还是由于规划的路径往边界靠拢造成的，边界不平滑不满足车辆动力学，我们可以通过增加约束的方法解决，同时带来的问题是约束的增加导致二次规划的求解变慢，针对这个问题我们提出解决方案:

2.3 速度规划对路径规划的影响

1）路径规划不仅要处理静态障碍物，还要拿上一帧的轨迹去判断与动态障碍物的交互，下图图解的方式更易懂一点；

---

总结

以上就是今天要讲的内容，本文介绍了Apollo决策规划算法中的路径决策算法，路径决策算法是通过动态规划和二次规划实现的。