Apollo进阶课程[5]——motion planning with environment

运动规划的环境变化

Vehicle Model的建立

对于汽车而言，质点模型是远远不够的。
前轮转向的车，前后位置的变化是不一样的，那么怎么去描述这种不一样呢？
首先从刚体角度考虑，二维平面里的刚体涉及到XY和$\theta$，也就是以车后轴中心作为XY坐标原点时车身的朝向heading。因为无人车运动模型还多了一个转向的变量，多了一个自由度，刚体模型也不够。
自行车模型：

• 将四轮抽象成两个轮子，车辆在垂直方向的运动被忽略掉，前轮中心和后轮中心的运动方向和自行车一样。
• 特点：前进方向和前轮方向一致。后轮是沿着切向方向运动的。前轮和后轮都围绕一个中心点转动，后轮转向半径与前轮转向半径不一样，并且后轮的转向半径$(1/k)$与方向盘转动角度$w$满足以下关系，其中L为前轮中心和后轮中心的距离：

假设刚体的后轴中心是沿着一条平滑的轨迹运行，该轨迹对应的曲率$k$表示调整方向盘的度数*(如果为正，表示向左转，反之则向右转)*。因此，自行车运动模型可以用$X，Y，\theta ，k$还有速度$v$来表示。

沿着这样的轨迹运动时，如何去估计障碍物的距离呢？
解决这个问题，先了解一下曲线坐标系以及与世界坐标系之间的转换关系。

曲线坐标系SL

SL坐标系（frenet frame）以道路中心线为参考，S表示道路中心线的方向，L表示与道路中心线垂直的方向。在结构化道路上行驶的时候，SL坐标系比XY坐标系更加贴合实际需求。
SL坐标系到XY坐标系的投影：
在给定SL坐标系时，每一个点的S坐标本身对应一个$（x_r,y_r）$坐标，根据该点的横向偏移距离，可以求出给定点在世界坐标系中的XY位置。

$theta$是参考线的方向，也就是切线方向。如果XY与S方向平行的话，轨迹的曲率还满足图中所示的关系。

XY坐标系到SL坐标系的投影：
SL坐标到XY坐标一一对应。反过来不是，在半径不超过曲率的情况下是一一对应的。
因为SL坐标系并不是唯一的，XY会在曲线上产生很多投影，投影点是经过XY坐标，且垂直于曲线的线段与曲线的交点。假设如图，XY就有两个投影点。通常情况下会增加一些限制，例如投影距离不能超曲率值。需要注意的是，掉头的时候还是需要特殊处理的。

如何找到投影点
首先是曲线的表示问题。计算机无法记录一条连续的曲线，需要将其离散化。

世界坐标系上的点如何投影到这条曲线上得到SL坐标？

研究障碍物与障碍物之间的关系

XY转化为SL坐标后就要研究障碍物与障碍物之间的关系或者车与障碍物之间的关系。
如何判断两个动态障碍物有相交？
首先将障碍物本身抽象成一个凸的多边形，然后用一个方框（简化计算）处理，判断两个方框的相交。
超平面分割定理：如果两个凸的多边形不相交，一定有超平面可以把他们分割出来。

Motion Planning with Environment2

在自动驾驶中，我们将环境抽象成 SL 坐标系，在此坐标系下的曲线光滑度是有要求的。首先，曲线本身要平滑。其次，其曲率也要满足平滑的特性。因此需要对轨迹线进行平滑处理。
但是不能先生成一条线，然后再进行平滑，能对 Curvature 进行一个后期的平滑。因为 XY 坐标本身与 Curvature 是有联系的，不能单独平滑曲率，也不能单独平滑 X 或者 Y。

多项式

首先，可以在轨迹上以等距离的方式随机选择一些点，然后用高阶多项插值的方式来近似表示轨迹，对多项式进行优化。
龙格现象：高阶多项式不能用于平滑，因为高阶的多项式抖动太大，没有办法控制幅度。

Bezier Spline

Bezier Spline 曲线是由一系列控制点定义的，例如P_0到P_n，其中n代表曲线的阶数。通过对控制点做平滑，得到平滑的曲线，例如二阶平滑保证曲线的曲率平滑。
但是这种方法的缺点是，除了起始点和终点，其它控制点不能保证会被得到的曲线经过。

1阶、2阶、3阶Bezier Spline曲线的表示形式

生成一条光滑的曲线，涉及到两方面，一方面是目标（干什么，满足什么样的约束条件），另一方面是工具（选择的函数）。
平滑最原始的抽象就是最短路径，但是路径最短还不能保证平滑性，因此会对其不同阶导数进行 Minimize 求解，保证导数空间的连续，这就是 Smoothing Spline 最初的思想。

Smoothing Spline的性质：

• 在给定边界的条件下，它是一个多项式，可以找到最优解
• 它的边界约束只考虑了起点和终点，如果中间有障碍物就不是最优解。这种情况下可以使用 Piecewise Polynomial（分段多项式）来处理。
• 一节节分段多项式的误差是由使用的多少节的多项式来控制的。

建立平滑坐标系

Piecewise Polynomial 是一维的函数，描述二维曲线是不够的，这时候就有一个 Spline 2D。
假设把曲线分成 N 截，每节曲线段的 X 坐标是一个多项式，Y 坐标也是一个多项式。

eg.用 5 阶多项式来表示 X 和Y，称之为 Quintic Spline（五次样条），每一节都是这样的函数。
这种表示的目标函数具有旋转不变性。
怎么让曲线足够平滑？
让它在 X 坐标上的变化率，也就是三阶导的平方是最小的，Y 上的变化率三阶导也是最小的，代价函数就是这两个变化率的和。代价函数的求解就是一个二次规划问题。

用一节一节的线段来保证曲线是光滑的，在线段内部用一个二维的 Polynomial 表示，在内部是 N 阶可导的。
但是如何保证节点处是平滑的？
端点约束条件：需要保证 X 和 Y 方向的倒数是相等的，一般要求到三阶导都是相等的，包括它的 X，Y 点的值也完全相等，此时就能保证三阶导连续。

路径平滑的前提是参考系平滑

螺旋曲线

它通过一个极坐标形式定义，比如说沿着一条曲线，如果一个点 S 的曲率$k$是知道的，假设它的原点在 （0，0）的位置，可以唯一定义出一条经过 S 的曲线，也就是 Spiral Path 。那么可以让 Spiral Path 满足起点、终点约束条件生成一条螺旋曲线。

Spiral Path 和 Spline 2D 的区别：

• 任何的曲线在足够密的时候都可以用Piecewise Spiral path 或者是 Piecewise Polynomial 表示。
• 它们的出发点不一样，Polynomial 计算很快很简单，Spline 2D 是一个凸空间里面生成一个 Spline 曲线。Spiral Path 是从 Configuration Space 出发。理论上来讲，螺旋曲线生成的线是要比 Spline 更好处理，对一些极端情况处理更好。