三次样条插值_基于自由边界三次样条插值的自动驾驶路径生成

前面我们提到，轨迹即包含时间这一维度的路径，而无人车的动作规划问题实际上就是要根据初始配置和目标配置生成一序列的动作，一种简单的思路即生成从当前位置x到目标位置y的轨迹，然后通过pure pursuit方法来完成循迹，其中，轨迹生成是非常重要的一步，在本节我们介绍一种基于三次样条插值的路径生成方法，后面我们将结合Moritz Werling提出的基于Frenet坐标系的最佳轨迹生成来完成一个简单的动作规划实例。 路 径生成，顾名思义就是在给定起点和终点的情况下生成一个最好的路径，有别于我们前面提到的例如A*和Hybird A*这类路径搜索算法，在路径生成问题中，起点和终点均已知。 生成的路径的一个基本要求即能够通过车辆的制动完成该路径，一个简化的处理方法即保证生成的路径的连续性和平滑性，在本节我们介绍使用三次样条函数生成路径的方法，并且使用Python实现一个简单的三次样条插值路径生成。

三次样条插值

在讲解基于样条插值的路径生成之前，首先我们了解一下什么是样条(Spline)，样条插值最初是用于函数拟合，那么什么是函数拟合呢？给定如下图所示的点：

那么我们要使用怎样的函数去拟合它呢？一种简单粗暴的方法就是不光滑的直线来将离散的点相连，即我们通常所说的线性拟合，如下图所示：

显然，线性拟合会存在一个问题，拟合出来的函数不够“光滑”，为了让线条更加光滑，我们可以使用二次线条来连接每一个点： 我们再试试三次线条来连接：

看起来三次多项式的拟合想过更加好，但是以上的拟合结果都是使用N次的线条将点简单连接起来，下面我们使用三次样条插值来实现拟合这几个点：

对比3次线条连接方法，三次样条插值方法拟合出来的曲线更加符合实际，3次线条连接方法在一些很明显不应该弯曲的段(如第一个点和第二个点之间)也有一个曲线，对比之下三次样条插值就更好一些，由此我们可以引出三次样条的一些性质：

• 三次样条曲线在衔接点处是连续光滑的

• 三次样条的一阶导数和二阶导数是连续的

• 自由边界三次样条(Nature Cubic Spline)的边界二阶导数也是连续的

• 单个点并不会影响到整个插值曲线

下图是在衔接点的连续性区别：

第一个即在衔接点不连续，第二个在衔接点连续，但是不光滑(即在衔接点处一阶导数不连续)，第三个即在衔接点一阶导数连续。那么如何计算样条曲线呢？假定有三个点需要拟合： S 1=( x 1, y 1)S1=(x1,y1),  S 2=( x 2, y 2)S2=(x2,y2),  S 3=( x 3, y 3)S3=(x3,y3) 。那么我们可以使用一个三次函数来拟合 ( S 1, S 2)(S1,S2) 两个点， 用另一个三次函数来拟合 ( S 2, S 3)(S2,S3) 两个点，通过代数计算来确定两段3次样条的多项式系数( a,b,c,d,e,f,g,h)，在实际样条参数求解过程中，在确定一段样条的多项式系数以后(即a，b，c，d确定)，后面样条的求解就更简单了。