Apllo进阶课程18Apollo规划技术详解——MotionPlanningEnvironment

Apollo 规划技术详解——Motion Planning Environment

自动驾驶汽车核心技术包括环境感知、行为决策、运动规划与控制等方面。其中，行为决策系统、运动规划与控制系统作为无人驾驶汽车的“大脑”，决定了其在不同交通驾驶场景中行驶的合理性与安全性。

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

多项式

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

Bezier Spline

Bezier Spline 曲线是由一系列控制点定义的。如下图所示，分别给出 1 阶、2 阶、3 阶 Bezier Spline 曲线的表示形式。通过对它们做平滑，得到平滑的曲线，例如二阶平滑保证曲线的曲率平滑。但是这种方法的缺点是，除了起始点和终点，其它控制点不能保证会被得到的曲线经过。

生成一条光滑的曲线，涉及到两方面，一方面是目标，另一方面是工具。怎么定义平滑呢？最简单的方法就是最短路径，但是路径最短还不能保证平滑性，因此会对其不同阶导数进行 Minimize 求解，保证导数空间的连续，这就是 Smoothing Spline 最初的思想。那么，问题的目标就明确了，定义一个函数，能够最小化它的类似三阶导平滑性。

Smoothing Spline 具有一些特殊的性质，在给定边界的条件下，它是一个多项式，可以找到最优解。但是它的 Boundary Constraint 只考虑了起点和终点，如果中间有障碍物就不是最优解。这种情况下可以使用 Piecewise Polynomial（分段多项式）来处理。

Spline 20

一个 Piecewise Polynomial 是一维的函数，描述二维曲线是不够的，这时候就有一个 Spline 2D，假设我们把曲线分成 N 截，每节曲线段它的 X 坐标是一个 Polynomial ，Y 坐标也是一个 Polynomial 。如下图所示，用 5 阶多项式来表示 X 和Y，称之为 Quintic Spline（五次样条），每一节都是这样的函数。这种表示有一个很好的特性，就是目标函数具有旋转不变性。怎么让曲线足够平滑？我们让它在 X 坐标上的变化率，也就是三阶导的平方是最小的，Y 上的变化率三阶导也是最小的，代价函数就是这两个变化率的和。代价函数的求解就是一个二次规划问题，我把这种 Loss Function 定义成这种形式是因为平方的积分能够给计算带来便利。

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

Spiral Path

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

Spline 20 与 Spiral Path 区别

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