Apollo决策规划算法学习Chapter1 基本概念

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

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

---

---

前言

本文为第一章，主要讲解 Apollo决策规划算法的一些基本概念如凸优化、参考线、Frenet坐标系的概念。

---

一、凸优化与Frenet坐标系的概念

1.1 凸优化与非凸优化的概念

Notes：决策规划的含义（一辆无人车的无人驾驶系统主要由感知、定位、预测、决策规划、控制几个模块组成）：决策规划是无人车的核心算法模块，因为它就类似于人类驾驶员大脑决策在不同情况下怎么行驶，无人驾驶分为L0-L5，无人车的无人驾驶功能越强大，决策规划算法就越重要也就越难，决策规划虽然是一个模块，但是在它内部，可以细分为三个部分；第一部分是导航规划算法，导航规划算法是这个模块中最成熟的算法，常见的有A*、 RRT* 等，它的功能是搜索出一条起点到终点的最优路径（功能和手机导航相似），Apollo使用的是Hybrid A* 算法；第二部分是行为规划算法（也可以认为是决策算法），它的功能是决定车辆的行驶意图，比如遇到正前方行走的行人应该向右绕行还是减速避让，它也是决策规划中最难做的部分；第三部分是运动规划算法，它根据决策算法给出的行为意图搜索出一条具有详细路径点和速度信息同时满足各种约束的轨迹，运动规划生成的轨迹是决策规划模块的最终输出，输出后把这个轨迹给控制模块去跟踪；

1）细说五次多项式的特殊性；
五次多项式是规划算法中的常客，下面解释一下五次多项式的特殊性，在车辆运动规划中一个很重要的指标是舒适性，在物理中衡量舒适性的指标就是跃度（加速度的导数），因此在计算中我们想要得到一个关于t的函数f(t)，使得它的三阶导数（也就是跃度）在0-T时间内的的积分尽量小，那可能有些人会认为二次或者更低的多项式就可以了，但是这样考虑忽略了一个问题，真实情况是比较复杂的，真实情况往往是带有约束的，初始状态和终止状态对位置、速度、加速度都有限制，所以需要进行一下计算和推导，简要的说一下为什么是五次多项式，在求解有初始状态和终止状态约束的问题称为OBVP（Optimal Boundary Value Problem）问题（边界值最优），解OBVP问题的方法是，定义一个相关的哈密尔顿函数，通过庞特里亚金极小值原理求解出该问题的解为五次多项式。
2）凸优化与非凸优化；
（1）自动驾驶决策规划模块的目标是规划出一条满足约束的最优轨迹，最优指的是满足约束（轨迹连续性、无碰撞性、交通规则、车辆动力学）和各种指标（平滑性、舒适性、耗时少），我们使用代价函数评判衡量轨迹的质量；
（2）凸优化与非凸优化的数学概念不再说了，这里说凸优化与非凸优化只是为了说明自动驾驶的问题怎么由非凸优化问题转换为凸优化问题，下面来看凸优化具有的两个性质：第一是代价函数只有一个极值点（凸函数），第二是约束空间是完整的空间（凸空间），那么凸函数在凸空间的最小值问题就称为凸优化问题；
（3）接下来要说一说凸空间的概念，因为下面会用到这个概念，在一个多边形中，任意选取两个多边形内部的点如果这两点中间的点都属于多边形内部，那么此多边形称为凸多边形，凸空间就是凸多边形的引申，所以是类似的；
（4）来看一看自动驾驶车辆行驶过程中避障的约束空间如何：下面是一个示意图，可以看到明显不满足凸空间的性质，所以自动驾驶车辆的避障的约束空间是非凸的，那么非凸问题解决的一般思路就是转换为凸空间，我们下面看一看怎么转换为凸空间；

目前求解非凸问题没有完美的解决方案，主要的思路就是寻找非凸问题中的凸结构，我们使用启发式算法现随机在空间中采样一些离散的数值，取最小的作为迭代初值（这种启发式算法本质上是连续空间离散化后，离散约束空间的最优解），然后迭代求出最终解，这样做其实也存在一定的问题，如果采样点少，容易陷入局部最优解；如果采样过多的点，计算量太大，会造成维度灾难，因此采样点的数量的选择很重要，基本是一个如下的流程：

1.2 Frenet坐标系的概念

我们通常使用笛卡尔坐标系描述物体的位置，但笛卡尔坐标系对车辆来说并不是最佳选择，即使给出了车辆位置(x，y)，如果我们不知道道路在哪里，也很难知道车辆行驶了多远，也难以确定它是否偏离车道中心，笛卡尔坐标系转换为Frenet坐标系就可以解决这个问题，因为Frenet坐标系以道路为坐标轴，它的基向量也不是常向量，而是一直会变化的，它的纵轴为道路曲线的切线方向，横轴为道路曲线的法线方向，可以把从第一张图的形式转换为第二张图的形式：

二、参考线的概念

2.1 决策规划总体概览、参考线的用途

1）定位+导航：生成参考线；
2）静态障碍物投影到以参考线位坐标轴的Frenet坐标系中；
3）决策算法对障碍物做决策（往左绕、往右绕、减速避让）开辟最优凸空间；
4）规划算法在凸空间中搜索出最优的路径；
5）后处理，在规划轨迹中选一个点，坐标转换为笛卡尔坐标系，输出给控制去跟踪；

Notes：这个决策规划总体概览相对比较粗略，没有考虑动态避障；
6）大家是否想过导航的路径有问题呢，因为导航的路径是一个很长的路径（从起点到终点），而且过长的路径不利于坐标转换，同时障碍物的投影可能不唯一，这就带来很大问题；就算导航的路径很短也没有一些具体速度、路径的详细信息，无法直接给控制模块去跟踪，所以导航给出的路径无法直接用，必须要用到参考线，参考线是解决导航路径过长，路径不平滑的问题方法；
7）参考线的概念：在每个规划周期内，找到车在导航路径上的投影点，以投影点为坐标，往后取30m，往前取150m，（如果往后没有30m，就是刚开始从起点开始走的时候，那就往前多取一些）做平滑，平滑后的点的集合称为参考线。

2.3 参考线平滑算法Fem smoother

参考线平滑算法是对参考线进行平滑的算法，使参考线更加平滑；参考线平滑算法考虑了三个指标：参考线是否平滑、参考线和平滑之前的几何形状的差距（差距不能太大）、长度尽可能紧凑和均匀，下面有图解的方式解释这三个指标的作用



平滑算法用二次规划算法，因此我们要求出二次规划的矩阵，下面是计算过程：

2.4 参考线模块优化计算，提升速度的方法

回顾一下参考线生成的流程：

下面来说一说优化计算参考线生成算法，提升计算速度的方法：
1）减少规划频率，规划算法每100ms执行一次，控制算法每10ms执行一次；
2）充分利用上一个规划周期的结果；
3）快速找到匹配点的方法：常规做法是每一个规划周期做遍历，但是速度导航路径可能很长，遍历很费时；正确的做法是以上个周期的匹配点为起点开始做遍历，如果发现有一个点前面的点距离车的距离大于该点的距离，那么该点就是本周期的匹配点；

但是这样做做其实也存在一定的问题，因为这样做有一个条件：上个匹配点结果附近只有一个极小值点，如果没有这样的条件的话可能会陷入局部最小值，针对这个问题我们有一种方法来解决；

首先是，规划周期是100ms，那么车辆也只是行驶了几米，道路一般不可能出现这么扭曲的几何，其次就算出现了我们用一个变量继续l(i+1)>l(i)的次数，当这个变量记录的增加次数超过阈值（比如50），也就是连续检查了后面50个点也没有更小的点了，那就可以把l(i)当做是匹配点了。

2.5 轨迹拼接

每个规划周期都会生成参考线，但是肯定不可能上个周期生成的参考线和下个周期生成的能刚好拼接起来，很大几率存在重复的点，这时候就比较这个周期的和上个周期的，相同的保留着，剩下的不足的点就从这个周期生成的取一些拼接上去即可。

---

总结

以上就是今天要讲的内容，本文介绍了 Apollo决策规划算法的一些基本概念如凸优化、参考线、Frenet坐标系的概念，同时还介绍了参考线平滑算法Fem smoother。