Apollo进阶课程[[5]——规划技术1

basic motion planning and overview

什么是规划问题？

规划问题本质上是一个搜索问题，即对一个给定的函数，寻找最优解。相对于无人车而言，规划问题就是给定现在的状态，找到无人车移动的最优解。通常最优解目标函数F(x)定义。

从内容考虑，规划问题涉及三个领域，不同的领域对问题的理解不同。

• 机器人领域：规划就是如何产生轨迹以完成目标，涉及RRT， A*，D* lite，Lattice Planning等。
• 控制理论领域：规划理解为到达目标状态的动态系统，涉及MPC， LQR等
• 人工智能领域：是生成状态到动作的映射，涉及reinforcement learning，end-to-end imitation learning等。

如何构建规划：

从最简单问题出发，把运动规划抽象成一个path finding problem（路径查找问题），不关心车的速度，只关心车怎样走，周围环境是不变的。将其抽象成无数的网格，从绿色的点到红色的点的路径规划就是一个path finding problem 。若要寻找最短路径，可通过BFS或DFS。
也可以通过启发式方式对搜索问题*（最关心的是能不能以最快的速度找都最优解）* 进行优化。
BFS没有利用起点和终点之间的信息，BFS和DFS都属于无信息搜索策略，问题就在于不知道目标在哪里。
使用BFS搜索：

A* 算法是大概知道红点在右边，定义一个启发式函数，该函数猜测距离目标还有多远，通过这种方法先搜索一些比较近的点，然后从这个点出发逐渐扩大搜索圈。
使用A* 搜索：

A* 花费时间更短，因为它有信息支持，现在的一些路径搜索算法本质上都是从A* 算法出发，需要知道目标函数的样子。

A* 是否能用在规划模块？
eg.红色的点是小车，红色和绿色块是障碍物，对于无人车来说是看不见的，传感器系统只能部分观察环境。

A* 算法不能直接用在规划模块上，A* 算法本身要求对整个环境全知。而自动驾驶对周围环境是部分观察的。
搜索分为部分观察和全知观察。对于部分观察环境可以用贪心算法，它是一个增量搜索，就是在看见的情况下选择最好的。
利用D* 算法对部分观察的数据进行控制规划。它利用当前能够看到的信息进行增量规划，D* 的特点是处理在看到的有限范围的条件下，如何到达预定地点的搜索问题方法。这种增量搜索很难通过一步步的迭代达到全局最优解。在现实生活中，人类开车是很少做90度直角转弯的，D* 得出的折线并没有考虑无人车运动过程中的运动模型和动力学模型。更进一步，可以通过平滑性曲线的方式来优化折线，换成一些较为平滑的曲线来完成。

环境本身是动态过程，对于规划非常重要的一个问题是如何处理动态障碍物。
运动规划问题就是让自动驾驶车辆能够安全平稳到达终点，本质是一个三维规划问题，即 XY 坐标加上时间维度，叫做 3D Trajectory Optimization Problem（轨迹优化问题）。从车辆动力学模型来说，维度需要进一步上升，因为涉及到车头的方向，车的转向角、加速度等问题。

无人驾驶车系统

运动规划可以获得两部分信息

• 动态信息：包括从认知获得的信息，就是从感知模块和定位模块获得信息。
• 静态信息——高精地图。

规划模块能获得的信息：车辆状态、交通灯信息、障碍以及障碍轨迹、导航、高精地图。
规划就是在这样的部分可见信息中给无人车找到一条轨迹。
轨迹：不仅仅是一条路径，而是随着时间路径该怎么走，它包含两方面，一是路径信息，二是速度配置文件，需要保证速度和路径变化都是平滑的。

概览

Motion Planning with Autonomous Driving

运动规划基本方法

重点从robotics的角度阐释。

决策规划问题都是从最简单的离散化的空间，从质点模型出发考虑。
质点模型将运动轨迹当成一个点，这个点和无人车是不一样的。假设把一个无人车看成一个点，这个点和另一个点不相撞，在数学定义上是点和点没有交集，但是在实际生活中一个车和一个车可是会相撞。

对于刚体来说，不仅是XY坐标，还要有heading信息才能研究跟障碍物之间的关系。
构造空间（Configuration Space）：能够控制什么变量。
对于无人车来说有更多的变量。其复杂性主要体现在两个方面：

• 空间维度（Space Dimensionality）
• 几何复杂性（Geometric Complexity）。
  例如bounding box跟bounding box之间怎么相交，一个多面体跟一个多面体之间怎么检测出路径，以避免跟另一个障碍物相交。
  约束（Constraints）大概分为三类：
• Local Constraint，如避免和障碍物碰撞。
• Differential Constraint，如边界曲率。
• Global Constraint。如最短路径。

如何解决连续空间过程的优化？
通常先将连续空间问题离散化表示，然后寻找对应的解决方案。

如何去离散化连续空间？
Roadmap：使用简单的连通图表示配置空间。

• Visibility Graph是一种常用的Roadmap方法。Visibility Graph将起始节点，所有障碍物的顶点和目标节点相互连接来构建路线图。从红点到绿点的最短路径一定会通过靠近障碍物边界的折线。
  
• Cell decomposition（网格分解方法）：将整个空间分割成一个个cell，通过cell的连接图表示自由空间的连接属性。
• Potential field（势场法）：直接用微分方法处理。

PRM

一种常用的抽象连续空间方法。它在整个配置空间随机采样一些点，如果点在障碍物上则去掉，然后将这些点连接起来，如下图所示的紫色点。从点s到g的最短路径就可以利用A* 算法进行求解。但是该方法要求是对全局感知，而无人车是一个部分感知的应用场景，因此有RRT的改进方法。

RRT（基于快速扩展随机树算法）

它构造一个根结点为起始点的配置空间树，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。如果叶子节点和目标节点之间的连接被障碍物阻挡，则需要重新采样。

通过这种方式离散化的线的curvature不连续，甚至curvature都没有。不够平滑。

Lattice网格方法

最原始的Lattice网格方法非常简单，它在XY世界坐标系中，以1米为单位进行网格划分，然后用无人车可以行进的、曲率连续的曲线将起始点和目标点连接起来。

存在问题：

• 这种连续抽象方式对于道路是不够的，不能走。

解决的问题：先撒点，然后用平滑线连接，限制它能采样的点都是在已经存在的点。由此可以简化计算。可以将重复计算的路径记录下来（使用了动态规划）进行复用。
Lattice Sampling撒点不能在规则化坐标系下去撒点，因为道路并不是一个完全XY的坐标系。因此提出了在sl坐标系下进行离散的方法Lattice in Frenet Frame。
在时间维度空间维度离散化后使用动态规划处理。

另一种常用抽样方式：

从车辆的运动出发，将车辆运动分为沿着道路走的横向运动和沿着道路走的纵向运动。在这种空间上随机采样一些点，生成一些候选路径，从中选取最优路径。

Polynomial方法

当抽象出点之后，如何使用平化曲线连接？
使用螺旋曲线（Splines）。此外，还可以使用路径-速度迭代优化的方法对Lattice方法进行优化，也就是Polynomial方法。它将问题降维，分成了path （XY）和 speed（ST）两个维度逐渐优化，这是一种iterative的处理方式。处理速度快，但不一定会达到最优解。

iterative处理方式：举例，一个二维优化问题，把它转化成在一个维度上优化完以后，在另一个维度上再进行一个优化。

Functional Optimization方法

Functional Optimization方法对运动规划进行处理，对整个问题建模，设计相应的代价函数。二次规划Quadratic Programming就是其中一种常用的方法，更进一步是凸优化空间convex optimization。

在凸空间的一个凸的优化是有唯一的最优解，要么在边界上要么在中心。这时候用二次规划的方式可以以非常快的速度找到最优解。
二次规划存在的问题：必须要求目标函数是凸的，并且搜索空间也是凸的。
Bspline：先生成一些离散的点，先优化再平滑。
龙格现象：用一个高阶的多项式polynomial去差值一个函数时，对它的控制的误差是无法保证的。

总结