Apollo进阶课程㉒丨Apollo规划技术详解——Motion Planning with Autonomous Driving
原创： 阿波君 Apollo开发者社区 6月12日
自动驾驶车辆的规划决策模块负责生成车辆的行驶行为，是体现车辆智慧水平的关键。规划决策模块中的运动规划环节负责生成车辆的局部运动轨迹，是决定车辆行驶质量的直接因素。
在大多数情况下，运动规划问题的精确解决方案在计算上是难以处理的。 因此，数值近似方法通常在实践中使用。在最流行的数值方法中，变分方法将问题视为函数空间中的非线性优化，图形搜索方法构建车辆状态空间的图形离散化并使用图形搜索测量方法搜索最短路径，此外基于树的增量方法，从车辆的初始状态逐步构造可到达状态的树，然后选择这种树的最佳分支。
上周阿波君为大家详细介绍了「进阶课程㉑Apollo规划技术详解——Basic Motion Planning and Overview」。
通过介绍了基本的运动规划和概述，其中详细的介绍了如何构建汽车的运动规划问题，其中包括路径查找问题、路径优化，可以通过A-Star算法、D-Star算法对路径搜索进行优化，通过曲线平滑的方式来优化折线。除此之外，还简单的介绍了无人驾驶的硬件系统和系统软件。
本周阿波君将与大家分享Apollo规划技术详解——Motion Planning with Autonomous Driving。下面，我们一起进入进阶课程第22期。
目录
1 - 运动规划基本方法
2 - 运动规划框架
2-1 RRT（基于快速扩展随机树算法）
2 - 2 Lattice网格方法
2 - 3 Polynomial方法
2 - 4 Functional Optimization方法
1 - 运动规划基本方法
本节主要介绍运动规划的一些基本方法，重点从robotics的角度阐释。这些方法主要有RRT、Lattice、Spira、Polynomial、Functional Optimization等，如下图所示。
运动规划方法
前面讲到决策规划问题，都是从质点模型出发考虑。质点模型将运动轨迹当成一个点，这个点和无人车是不一样的。假设把一个无人车看成一个点，这个点和另一个点不相撞，在数学定义上是点和点没有交集，但是在实际生活中一个车和一个车可是会相撞。下面介绍解决这些问题的一些方法：Configuration Space （构造空间），也就是说能够控制什么变量。对于刚体而言，不仅是XY坐标，还要有heading信息才能研究跟障碍物之间的关系。对于无人车来说有更多的变量。其复杂性主要体现在两个方面，一个是Space Dimensionality（空间维度），另一个Geometric Complexity（几何复杂性）。例如bounding box跟bounding box之间怎么相交，一个多面体跟一个多面体之间怎么检测出路径，以避免跟另一个障碍物相交。
构造空间方法
规划问题中涉及到一些约束条件，Constraints（约束）大概分为三类：一个是Local Constraint，例如避免和障碍物碰撞。第二是Differential Constraint，比如边界曲率。最后是Global Constraint。比如最短路径。
2 - 运动规划框架
运动规划是在连续空间的一种优化，对于连续空间过程的优化往往比较难。因此，通常先将连续空间问题离散化表示，然后寻找对应的解决方案。如下图所示，可以使用图搜索的方法对离散空间问题进行求解。
那么，如何去离散化连续空间呢？有种方法叫做Roadmap，这个方法使用简单的连通图表示配置空间，类似于城市如何用地铁图表示。其中Visibility Graph是一种常用的Roadmap方法，如下图所示。Visibility Graph将起始节点，所有障碍物的顶点和目标节点相互连接来构建路线图。我们会发现，从红点到绿点的最短路径一定会通过靠近障碍物边界的折线。
图搜索方法
除了Roadmap之外，还有Cell decomposition（网格分解方法）和Potential field（势场法）等路径规划方法。Cell decomposition将整个空间分割成一个个cell，通过cell的连接图表示自由空间的连接属性。Potential field就是直接用微分方法处理。
路径规划方法
一种常用的抽象连续空间的方法叫做PRM。它在整个配置空间随机采样一些点，如果点在障碍物上则去掉，然后将这些点连接起来，如下图所示的紫色点。从点s到g的最短路径就可以利用A-Star算法进行求解。但是该方法要求是对全局感知，而无人车是一个部分感知的应用场景，因此有RRT的改进方法。
PRM 方法
2-1 RRT（基于快速扩展随机树算法）
如下图所示，它构造一个根结点为起始点的配置空间树，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。如果叶子节点和目标节点之间的连接被障碍物阻挡，则需要重新采样。
基于快速扩展随机树算法
通过这种方式离散化的线是不适合无人车行驶的，因为这些线的curvature不连续，甚至curvature都没有。针对这一问题MIT提出使用平滑曲线进行连接的方法，如下图所示。但是该方法得到的路径可能还是不够平滑，另外对动态障碍物的处理也存在问题。
平滑曲线进行连接的方法
2 - 2 Lattice网格方法
针对上述问题，就有人提出了Lattice网格方法。
Lattice网络方法
如上图所示，最原始的Lattice网格方法非常简单，它在XY世界坐标系中，以1米为单位进行网格划分，然后用无人车可以行进的、曲率连续的曲线将起始点和目标点连接起来。但是这种方法还是不能满足需求。对于道路来说，这种抽象方式并不合适。Lattice Sampling撒点不能在规则化坐标系下去撒点，因为道路并不是一个完全XY的坐标系。因此提出了在sl坐标系下进行离散的方法Lattice in Frenet Frame。
2 - 3 Polynomial方法
当抽象出这些点之后，如何使用平化曲线连接呢？ 有一种使用螺旋曲线的方法，即Splines方法。此外，还可以使用路径-速度迭代优化的方法对Lattice方法进行优化，也就是Polynomial方法。它将问题降维，分成了path 和 speed两个维度逐渐优化，这是一种iterative的处理方式。
Polynomial方法
2 - 4 Functional Optimization方法
还可以用Functional Optimization方法对运动规划进行处理，对整个问题建模，设计相应的代价函数。二次规划就是其中一种常用的方法。
Functional Optimization方法