百度Apollo自动驾驶专题讲座笔记之运动规划模块

在百度技术学院有Apollo的技术专题课程，对各个模块都有一个入门级的课程，对于了解各个模块间的相互作用关系有很大的作用，很适合对自动驾驶领域感兴趣的人的入门课程。感谢百度Apollo开放了这么好的课程。
百度技术学院连接：
Apollo自动驾驶 - 百度技术学院 http://bit.baidu.com/Subject/index/id/16.html
这是看完决策规划模块的视频做的一部分学习笔记，在此与大家分享，因水平有限，难免出现诸多错误，如有问题希望各位批评指正。
对自动驾驶水平认知有限，相关笔记看着可能会有点乱，在此也希望自己对各个模块有更深入的了解后及时进行更新。

运动规划模块

主讲人：范昊阳
包括运动规划基础概述、自动驾驶运动规划、环境下运动规划、运动规划内优化、Apollo emplanner开发环节、强化学习与数据驱动方法七个部分

第一部分：运动规划基础概述

1、什么是规划问题？

首先明确规划问题的本质是一个搜索问题 （给一个目标函数找一个最优解），运动规划 motion planning
搜索问题可以认为是找到对应状态（input：车辆当前位置、感知障碍物位置速度、地图信息等）和结果（运动规划路径、油门、转向、刹车）映射关系。 state action mapping 状态空间的映射关系

2、在各个领域大家一般是用什么算法解决规划问题？
机器人学领域：
RRT算法（Rapidly-exploring Randomized Trees），A算法，D lite算法，Lattice Planning算法等
控制理论领域：
MPC算法、LQR算法等
人工智能领域：
强化学习，端到端训练等

3、做运动规划，从最简单的角度入手，是一个什么问题呢？
决策规划问题可以简化为一个Path Finding Problem

首先想象一下：从绿色点到红色的点怎么样寻找最短路径，这是一个搜索问题，我们可以想到一些方法：
深度优先搜索：Depth-First-Search
广度优先搜索：Breadth-First-Search
上面这两种搜索都是 non-informative search

考虑到搜索算法计算的时间
深度优先搜索算法计算这么一个东西需要6.4ms的时间，在无人车的规划计算中需要程序的计算时间尽可能的短，所以这种耗时耗力的搜索算法不能够直接运用的无人车的运动规划中。
A算法作为对比需要2.4ms 因为A算法带了information，所以可以节省很大的搜索量，提高了运算时间。

考虑到无人车感知的范围
A算法时有information作为参考进行搜索的，那么自动驾驶汽车的运动规划中就使用A算法可以不可以呢？
答案是不可以，因为 A算法要求自车环境有全部的了解 fully observed missionplan，然后进行全局的路径搜索
实际的自动驾驶汽车的感知范围是有限的（当前一般自动驾驶汽车的有效感知范围大约是前后100米，左右10米差不多）
无人车本身是一个局部感知环境下的路径搜索问题，partially observed problem，这个时候我们就要考虑使用 incremental search 渐进式搜索，可以考虑D算法，在看到有限的范围达到局部的最优解。
那么达到局部的最优解对于无人车的运动规划来说是否够用了呢？规划模块其实是解决局部最优基本上就够用了。

考虑到车辆的运动学模型和动力学模型
D*算法规划出来的路径时走折线，可以看做是把空间离散化以后的路径最优解（为什么走直线，是因为把环境离散成了一个个的小方格）
车辆本身还有运动学模型 + 动力学模型，不可能按照上述的折线去走，所以实际的工程化问题要解决路径的平滑问题。
怎么样使用平滑的线去连接路径

进行路径的曲线平滑以后我们距离无人车的运动规划问题还有多远？
1、环境本身是一个动态的过程，怎么样去处理有限视野下的动态障碍物
2、曲线虽然平滑了，但是车辆一定能够按照指定的曲线运动吗？不一定，还需要假如车辆的动力学模型（比如不可能一下把方向盘打到一定的角度、车辆有最小转弯半径的限制等）
3、怎么样让无人车去理解交通规则，怎么样在模型里面去融入交通规则。
4、决策规划的计算时间百度要求是100ms，复杂工况下200ms到300ms的要求（人的反应时间是 400到500ms之间），有限时间内算法寻找最优解 C++语言更偏底层，所以计算速度相对Java或者python要快很多
（一个合格规划算法，必须满足几个条件。首先，必须能够使自动驾驶汽车到达目的地；其次，必须符合交规；第三，能够避免碰撞；最后，也需要能保证一定的舒适性。）

无人车的决策规划问题,其实可以抽象成如何安全地舒适地驾驶无人车到达终点的问题。

决策规划在自动驾驶整体的位置

规划模块的输入信息：感知信息、定位信息、车辆状态信息、交通信号灯信息、高精地图的静态信息

整个课程的结构：
知道我们做运动规划的方法
理解自车所处的环境
找到问题的最优解
建立自动驾驶运动规划模块百度apollo里的 case by case的学习
深度学习在决策规划中的应用

思想：知道为了解决什么问题然后再想方法不是为了用某种方法而用某种方法。

第二部分：自动驾驶运动规划的实现的基本方法

从研究的角度来看决策运动规划问题的基本解决方法：
机器人学角度：
RRT算法（ rapidly exploring random tree）快速探索随机树方法。RRT算法简介 - 魂淡林的博客 - CSDN博客 https://blog.csdn.net/u013528298/article/details/80546175
像树枝一样去探索，碰到障碍物就停止，去快速搜寻到达某一个点的路径
Lattice算法
在一个环境下先计算出来车辆所有可能的路径曲线，然后根据碰撞检测、交通规则等判断计算可行的路径，
然后在可行的路径中选择cost最小的一条路径（轨迹规划所需要满足的四点要求，分别是到达目的、符合交规，避免碰撞、平稳舒适。针对这四点要求，我们设计了六个cost，cost越高就表示越不满足要求。）六个cost分别是：任务到达cost 、横向偏移cost 、碰撞cost 、纵向冲击度 cost 、横向加速度cost 、向心加速度cost

从测试的角度分为：限制检测和碰撞检测。
限制检测考察的内容有轨迹的加速度、加加速度和曲率。碰撞检测是把自动驾驶汽车的轨迹和其他障碍物的预测轨迹进行比对，观察是否有轨迹重叠。

百度用的其实是在sl坐标系下的Lattice算法
【Baidu Apollo】6.2 Lattice Planner规划算法 - 笑扬轩逸的博客 - CSDN博客 https://blog.csdn.net/yuxuan20062007/article/details/82330165

做运动规划的方法：
Dijkstra’s Algorithm 迪杰斯特拉算法，从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题；FunctionOptimization算法：函数优化算法；Lattices算法；
A*算法；RRT算法；Line&Circle算法；Clothoid算法（回旋曲线）； Polynomial算法（多项式拟合算法）；Bezier算法（贝塞尔曲线）；Spline算法（样条曲线）

第一步：
先把车辆考虑成质点模型，对空间进行离散化（把连续的空间离散成网格），然后根据可通行的区域进行路径的搜索。

第二步：
考虑环境的约束问题，对生成的路径进行选择或者处理
local constriants：局部约束问题，比如要考虑进行障碍物的避障。
Differential constraints: 微分约束问题，要考虑曲线的曲率大小是否合适，曲率是否连续。
Global constrains: 全局约束问题，要考虑两点之间的最短路径问题。

二维空间中在有障碍物约束的情况下最短路径的问题：（举例子：Dubin’s path，杜宾斯曲线，最短，但是曲率不连续）

对空间进行离散化的一些方法： roadmap 路线图方法离散化； Cell decomposition 网格分解离散化； Potential field 势能场离散化方法

路径规划的一些方法：传统的机器人相关路径搜索算法，改进算法和数据驱动的路径搜索算法。

连续空间进行离散化相当于在图像上进行撒点然后紫色的点就是可通行的点把这些点连起来就解决了路径的问题，点撒的约密集，得到的路径也就相对越平滑。
分为在配置空间内随机采样进行离散化和最邻近链接的算法（在感知中也用来分割障碍物或者环境语义信息，在这可以判断可通行区域的边界）

路径搜索，在这些点中找到一条最优路径出来，相当于网格中去做 A star算法。
前面的无论是A star算法还是 Dijkstra 算法都要求有一个全局的感知视野，然后进行全局感知下的路径搜索问题。
但是在真实的环境下，无人车感知到的都是局部环境，因此需要在局部感知条件下进行渐进式搜索。
局部离散化空间求路径最优问题，在一个局部的环境下进行渐进式搜索，RRT算法（ rapidly exploring random tree）快速探索随机树方法

第三步：考虑路径的平滑性
然后折线替换成为平滑的曲线在非结构道路上可以随机去生成路径。
平滑的定义：高阶导数平滑。怎么样才能使一个曲线足够平滑呢
第一点：最原始的平滑抽象就是减少线段和线段之间的长度，减少min length。（比如采集vectormap的路点是，要保证车辆本身的速度够慢，同时采集转弯时缓慢的转动方向盘，从采集阶段就要控制整个曲线的平滑性）
第二点：保证路径的导数是连续的
第三点：保证路径的二阶导数是连续的
根据微积分里的泰勒展开，随着阶数的增加，误差项被控制在尾数项上。越是高阶展开，整个曲线就越平滑。

xy坐标系下的 Lattice算法，相当于是非机构化道路下的路径搜索问题
SL坐标系下的Lattice算法，相当于在结构化道路下的路径搜多问题，更加适合真实的无人车的路径搜索

Lattice算法在纵向（s方向）和横向（l方向）的路径搜索分解

Lattice算法规划路径，同时对路径点进行相应的速度规划。

在Lattice算法中加入障碍物，那么就演化成凸空间中的路径搜索问题。
凸空间的凸优化问题凸空间中牛顿法，二次迭代收敛牛顿法比binary search还要快目标函数和search space都要是凸的
凸的定义： x和y都术语某个集合，那么x+y/2也就属于这个集合。
遇到障碍物的二次规划问题，既要满足约束条件（比如实际过程中，对每条曲线的起点和终点的曲率要做一系列的约束），又要足够平滑。
生成的曲线要有旋转不变性，生成的曲线不随坐标系的变化而变化。

第三节课：真实环境下的路径规划问题

在真实环境下的路径规划问题要考虑下面这些条件：
车辆的运动模型从质点模型到自行车模型
生成带速度的路径时要考虑车辆下层能不能控制的住
要保证行驶的路径的平滑性和速度的平滑性
要考虑障碍物的投影关系和障碍物的bounding box
SL坐标系和xy坐标系对应得坐标转换映射关系

bicycle model 车辆运动学的自行车模型（等同于横向控制里面常用的圆弧模型），不是刚体模型，因为前轮的位置变化和后轮的位置变化是不一样的。

描述自行车模型的运动轨迹

SL坐标系和XY坐标系下的转换

sl坐标又叫 Frenet坐标系

从SL坐标系转换为xy坐标系，每一个点都是可以唯一对应的；而从xy坐标系转换为sl坐标系没有唯一确定性。
特别是掉头这种路段很难进行处理

真实环境下的运动规划问题

贝塞尔曲线（三阶导数也是连续的）

第四节课：运动规划内优化

优化问题有三点：
environment 环境怎么去抽象
objective function 目标函数怎么样去抽象，最好能定义的更加明确一点
constrain，约束怎么去抽象，对路网的一些抽象，对动力学约束的一些抽象，交通规则的抽象问题
路径规划问题就是在这些约束条件下面，去解一个最优解的问题。随着环境的实时变化，是一个动态规划的问题。

决策规划假如需要往前提取100米的路，那么每10cm一个点的话也有1000个点，还要计算很多可能的路径，在计算延时的限制下，整体对算法的性能要求还是很高的。这还没考虑平滑性的指标，整个时间复杂度很高
piecewise linear 分段线性 resolution 分辨率 quadratic programming 二次规划问题

人开车的过程，在考虑避让一个障碍物的时候，近似二分法 binary search去做搜索，凭经验去试。
牛顿法：看变化趋势有多快，凸的问题，看导数的变化是怎么样的，用二分法去搜索导数的值逼近一个凸的问题，导数指数级收敛。
二阶导数的变化，就知道导数的变化率了，牛顿法的思想是用泰勒展开去逼近它。二次规划问题，本质核心都是泰勒展开和牛顿法
实际到二阶导已经够用了，因为涉及到二维和三维问题，求导本身耗费的时间也会比一维要高很多。

直接去逼近就是二分法，用导数进行逼近就是牛顿法，然后用泰勒展开的二次项去逼近它就更快

qp函数就是 quadratic programming 二次规划问题

拉格朗日方法 + 松弛变量

numeric optimation 数值优化问题

qp函数的问题：连续波浪形的曲线是不能够保证的，有很多极大值或者极小值。这种情况动态规划和二次规划都解决不了
牛顿法要求，导数严格单调递增，这个时候要用 divide and conquer 分而治之，分段法，把波浪形分开。
Apollo中的EM planner中用到的非常重要的一个思想就是：组合问题的优化：divide and conquer

启发式搜索（连续空间里的优化问题）其实和人开车的过程是相似的先知道大概的往左往右等等
撒点时先撒稀疏一点对全局有个大概的了解

决策：离散空间的优化问题：比如避障解决的是要不要避障或者从哪边避障比较好，怎么样去加减速，是不是要让行动态规划，对整个空间有一个粗浅的认识，然后把这个粗浅的解作为一个启发，用于下层的搜索。启发式问题不一定能搜索到最优解
**规划：连续空间的优化问题：**具体的连续的路径

qpOases active set solver

先进行计算，然后看哪些条件没有满足，然后把不满足条件的去掉，满足条件的就加进来。通过这样一些规则来求解得到一个最优解。
BFGS方法：BFGS算法 - u012507032的博客 - CSDN博客 https://blog.csdn.net/u012507032/article/details/77923960

第五部分百度Apollode EM planner的开发

硬约束：交通规则 障碍物实际情况，交通规则就不可以商量 hard constrains
软约束：决策做出的行为信息，绕不过去可以让行、可以follow，这些约束是可以进行商量的

基于速度优势的自由换道

在换道场景的运动规划过程中，planer可能会生成本车道一条 trajectory 和一条换道的tarjectory，然后根据环境的实时变化，通过 reference line decider模块确定哪一条路径更加合适。生成的trajectory包括三个维度的坐标系，分别是S、 L、和T ，对应纵向位置、横向位置和时间，无人车需要知道自车在什么时间出现在什么样的一个位置。

解决高维空间的优化问题有两种方法：每一种方法都有自己的优势和劣势。
1、对空间进行离散化，从configuration space出发，生成三维空间中的trajectory，找三维问题中的最优解。
2、降成二维度空间中的问题去解决，先干一件事，再干一件事。说白了就是pathplan 和 speedplan，不断地用path speed来回赋值切换的过程，来收敛到一个最优解。这个就是EM算法解决三维问题的核心。
缺点：本质上来讲是一个贪心算法，只能收敛到局部最优对于无人车来说算够用了。
Iterative Path Speed Solving 路径和速度不断迭代求解
**E step：**expectation 根据当前的车速和障碍车的车速估计一下相遇的位置或者避障的位置
M step： maximization 自车减速，减速变化以后刚才的估计值就又发生了变化
在Estep和Mstep两步之中，不断地进行迭代收敛。

第六节：强化学习和数据驱动的决策规划方法

规划问题：最外层是基于规则 rulebased，然后是优化器 optimization方式最后是data driven的方式