基于三次样条插值的路径规划算法

前言

        当已知某些点而不知道具体的方程时，我们通常有两种做法：拟合或者插值。拟合不要求方程通过所有的已知点，讲究神似，就是整体趋势一致。插值则是形似，每个已知点都必会穿过，但是高阶会出现龙格库塔现象，所以最常见的为三次样条插值。

1.样条函数的基本原理

        已知n+1个数据节点，分成n个区间(也就是下图中的interval)，在每个区间构造一个三次函数，共n段三次函数，S0（x）~ Sn-1(x)。

假设我们每一段三次函数都用如下的方程来定义：

那么n个区间对应的三次函数S(x)的数学表达式如下：

        每段三次函数S(x)都有a,b,c,d四个系数（即:未知数)，上述n段三次函数，共有4n个系数（未知数）。要想求解这4n个未知数，共需要4n个方程。这就好比，我要求解y=ax+b中的两个系数（未知数）a,b，我们至少要知道两个点p1(x1,y1)和p2(x2,y2)，联立两个方程y1=ax1+b和y2=ax2+b，才能求出系数a,b一样。

下面，我们根据三次样条函数对每一段三次函数在断点处的约束(期望)，生成求解4n个系数a,b,c,d所需的所有方程。

条件1：n段三次函数必须穿过所有已知节点

        已知n+1个数据节点，要让n段函数三次函数穿过所有的数据点，总共可以生成n+1个方程。其中，前n个方程由i=0~n-1给出，第n+1个方程，由i=n单独给出，则：

当i = 0~n-1时：

当i = n时：

        前n个方程所表达的实际意义是，S0~Sn-1段函数必定会经过自己所在区间的起点，例如，点(x0,y0) for S0(x)。而，最后一个方程所描述的是Sn-1段函数，也就是最后一段函数必定会过自己所在区间的终点(xn,yn)。简而言之就是，前面n-1段函数保证过起点，而最后一段函数，既过起点也过终点。

条件2：在所有节点(除了第一节点和最后一个节点)处0阶连续(保证数据不间断，无跳变),即，前一段方程在节点处的函数值和后一段方程在相同节点处的函数值相等。

共得到n-1个方程，因为n段三次函数之间共有n-1个衔接点，其中，根据条件1可推导出：

条件3：在所有节点（除了第一节点和最后一个节点）处1阶连续（保证节点处有相同的斜率,原函数曲线上没有剧烈的跳变）

共得到n-1个方程，n段三次函数之间共有n-1的衔接点。

条件4：在所有节点（除了第一节点和最后一个节点）处2阶连续（保证节点处有相同的曲率，即，相同的弯曲程度）

共得到n-1个方程，n段三次函数之间共有n-1的衔接点。

综上，第一个约束条件得到了n+1个方程，第二，三，四个约束条件分别得到n-1个方程，共4n-2个方程，还差2个方程。（最后缺少的这两个方程，会在后面给出）

2.求解方程组

先分别求出函数S(x)的一阶导数和二阶导数

根据条件1可得：

根据条件2可得：

根据条件3可得：

根据条件4可得：

由公式（1）

可得：

由公式（2）

可得：

所以，该曲线的四个系数可表达为：

 

3.端点条件(最后增加两个约束条件，使得方程组数目正好是4n）

（1）自由边界(Natural)

        Natural样条是柔软又有弹性的木杆经过所有数据点后形成的曲线，让端点的斜率自由的在某一位置保持平衡，使得曲线的摇摆最小。

自由边界的两个附加边界条件为：

所以，将公式（6）

写成矩阵A*B = C的形式，为：

将矩阵可化简为：

 

因此：

使用追赶法求解矩阵A，令A= L*D

已知：

可得：

令D*B = E ， 所以，L*E = C

因为D*B = E

所以，可得：

因为m = y'' ,所以

已知：

所以：

 

matlab代码链接如下：

【免费】基于追赶法计算三次样条插值路径规划资源-CSDN文库

运行结果如下：

        由上图可知，三次样条插值曲线会经过所有的点，并且得到的是一条光滑，连续的曲线，使用追赶法算出的结果同matlab自带的插值函数基本一样，使用追赶法可避免对矩阵的一系列运算，更高效的得到计算结果，在三次样条插值曲线上的点处得到的曲率为局部样条函数的曲率，并不能表示整条曲线的全局曲率，所以使用三次样条插值算法应视情况而定。