Apollo进阶课程㉔丨Apollo 规划技术详解——Motion Planning Environment

原文链接：进阶课程㉔丨Apollo 规划技术详解——Motion Planning Environment 

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

运动规划根据环境的变化在算法和处理方法上有很大的不同。

上周阿波君为大家详细介绍了「进阶课程㉓ Apollo 规划技术详解——Motion Planning with Environment」。其中详细讲解了如何建立 Vehicle Model，另外还介绍了曲线坐标系 SL 、SL 坐标系到 XY 坐标系的投影、XY 坐标系到 SL 坐标系的投影等。

本周阿波君将继续与大家分享 Apollo 规划技术详解——Motion Planning Environment的相关课程。下面，我们一起进入进阶课程第24期。

目录

1.多项式

2.Bezier Spline

---

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

1.多项式

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

                                                                                                        龙格现象

---

2.Bezier Spline

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

                                                                                                n 阶 Bezier Spline 曲线表达形式

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

                                                                                                一维平滑度定义

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

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

                                                                                                        二维样条

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

还有一种方式叫做螺旋曲线，它通过一个极坐标形式定义，比如说沿着一条曲线，如果一个点 S 的曲率是知道的，假设它的原点在 （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 更好处理，对一些极端情况处理更好。