【算法几何】基于自由边界三次样条插值的无人车路径生成

文章来源：https://blog.csdn.net/adamshan/article/details/80696881

前面我们提到，轨迹即包含时间这一维度的路径，而无人车的动作规划问题实际上就是要根据初始配置和目标配置生成一序列的动作，一种简单的思路即生成从当前位置x到目标位置y的轨迹，然后通过pure pursuit方法来完成循迹，其中，轨迹生成是非常重要的一步，在本节我们介绍一种基于三次样条插值的路径生成方法，后面我们将结合Moritz Werling提出的基于Frenet坐标系的最佳轨迹生成来完成一个简单的动作规划实例。

路径生成，顾名思义就是在给定起点和终点的情况下生成一个最好的路径，有别于我们前面提到的例如A*和Hybird A*这类路径搜索算法，在路径生成问题中，起点和终点均已知。生成的路径的一个基本要求即能够通过车辆的制动完成该路径，一个简化的处理方法即保证生成的路径的连续性和平滑性，在本节我们介绍使用三次样条函数生成路径的方法，并且使用Python实现一个简单的三次样条插值路径生成。

三次样条插值

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

这里写图片描述

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

这里写图片描述

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

这里写图片描述

我们再试试三次线条来连接：

这里写图片描述

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

这里写图片描述

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

三次样条曲线在衔接点处是连续光滑的
三次样条的一阶导数和二阶导数是连续的
自由边界三次样条（Nature Cubic Spline）的边界二阶导数也是连续的
单个点并不会影响到整个插值曲线
下图是在衔接点的连续性区别：

这里写图片描述

第一个即在衔接点不连续，第二个在衔接点连续，但是不光滑（即在衔接点处一阶导数不连续），第三个即在衔接点一阶导数连续。那么如何计算样条曲线呢？假定有三个点需要拟合：

S1=(x1,y1)S1=(x1,y1),

S2=(x2,y2)S2=(x2,y2),

S3=(x3,y3)S3=(x3,y3) 。那么我们可以使用一个三次函数来拟合

(S1,S2)(S1,S2) 两个点，用另一个三次函数来拟合

(S2,S3)(S2,S3) 两个点，这两个三次函数分别记做：

y=ax^3+bx^2+cx+d

和

y=ex^3+fx^2+dx+h

有这两个函数可得如下两个个方程：

y1=ax1^3+bx1^2+cx1+d

y3=ex3^3+fx3^2+dx3+h

由于两条曲线都经过S2 , 故可得：

ax2^3+bx2^2+cx2+d=ex2^3+fx2^2+dx2+h

由于样条曲线在衔接点处的导数也连续，即两个三次函数在

S2处的一阶导数也相等，可得：

6ax2+2b = 6ex2+2f

如果是自由边界三次样条，那么要求在起点和终点的二阶导数也是连续的，即：

6ax1+2b = 0

6ex2+2f = 0

结合以上6个方程组以及给定的点集，通过代数计算来确定两段3次样条的多项式系数（ a,b,c,d,e,f,g,h），在实际样条参数求解过程中，在确定一段样条的多项式系数以后（即a，b，c，d确定），后面样条的求解就更简单了。那么如何编程实现呢？下面我们使用Python实践在给定一组2维点集 (S1, S2, S3,...Si) 的情况下，使用3次样条生成曲线来拟合，而这个拟合过程，实际上就是一种简单的路径生成。

### 三次样条插值算法
人在计算三次样条的系数我们使用的是代数方法，即使用已有的方程组不断的代入求解即可，在实际的算法实现中通常不是通过代数解方程来求解，下面我们给出三次样条插值的计算机算法，考虑到该算法的推导过程涉及到数值分析等基础，且和无人驾驶的主题相去甚远，故不讨论该算法的推导，感兴趣的同学可以自行扩展阅读。

假定目前有n+1个路径点，它们分别是: ，求解每一段样条曲线的系数: (，有如下算法：

计算点与点之间的步长：
将路径点和端点条件（如果是自由边界三次样条中端点条件即S''=0）代入如下矩阵方程中：
解矩阵方程，求得二次微分值mi
计算每一段的三次样条曲线系数：

5. 那么在每一个子区间 xi <= x <= xi+1内，其对应的样条函函数表达式为：

下面我们使用Python来实现该算法。

使用Python实现自由边界三次样条插值进行路径生成

首先我们新建一个Python文件 cubic_spline.py , 在文件中定义我们的自由边界三次样条类Spline：

# coding=utf-8
import numpy as np
import bisect


class Spline:
    """
    三次样条类
    """

    def __init__(self, x, y):
        self.a, self.b, self.c, self.d = [], [], [], []

        self.x = x
        self.y = y

        self.nx = len(x)  # dimension of x
        h = np.diff(x)

        # calc coefficient c
        self.a = [iy for iy in y]

        # calc coefficient c
        A = self.__calc_A(h)
        B = self.__calc_B(h)
        self.m = np.linalg.solve(A, B)
        self.c = self.m / 2.0

        # calc spline coefficient b and d
        for i in range(self.nx - 1):
            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
            tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
            self.b.append(tb)

    def calc(self, t):
        """
        计算位置
        当t超过边界，返回None
        """

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index(t)
        dx = t - self.x[i]
        result = self.a[i] + self.b[i] * dx + \
            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

        return result

    def __search_index(self, x):
        return bisect.bisect(self.x, x) - 1

    def __calc_A(self, h):
        """
        计算算法第二步中的等号左侧的矩阵表达式A
        """
        A = np.zeros((self.nx, self.nx))
        A[0, 0] = 1.0
        for i in range(self.nx - 1):
            if i != (self.nx - 2):
                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
            A[i + 1, i] = h[i]
            A[i, i + 1] = h[i]

        A[0, 1] = 0.0
        A[self.nx - 1, self.nx - 2] = 0.0
        A[self.nx - 1, self.nx - 1] = 1.0
        return A

    def __calc_B(self, h):
        """
        计算算法第二步中的等号右侧的矩阵表达式B
        """
        B = np.zeros(self.nx)
        for i in range(self.nx - 2):
            B[i + 1] = 6.0 * (self.a[i + 2] - self.a[i + 1]) / h[i + 1] - 6.0 * (self.a[i + 1] - self.a[i]) / h[i]
        return B

其中方法 __calc_A 和 __calc_B 分别用于构建上述算法第二步中的方程左右矩阵，由于 m 为对对角矩阵，这里我们直接使用Numpy中的linalg.solve 求解 m 。下面我们新建一个 test.py 文件来执行测试代码：

import cubic_spline
import numpy as np
import matplotlib.pyplot as plt

def main():
    x = [-4., -2, 0.0, 2, 4, 6, 10]
    y = [1.2, 0.6, 0.0, 1.5, 3.8, 5.0, 3.0]

    spline = cubic_spline.Spline(x, y)
    rx = np.arange(-4.0, 10, 0.01)
    ry = [spline.calc(i) for i in rx]

    plt.plot(x, y, "og")
    plt.plot(rx, ry, "-r")
    plt.grid(True)
    plt.axis("equal")
    plt.show()


if __name__ == '__main__':
    main()

生成路径的结果：

这里写图片描述

其中，绿色圆点为需要拟合的点集，红线为计算出来的三次样条曲线。以上，我们使用Python实验了基于Spline的路径生成，在实际中，需要使用C++来完成这些工作，我们通常不需要自己实现样条插值，有很多成熟的Spline开源代码，我们可以使用：http://kluge.in-chemnitz.de/opensource/spline/ 提供的代码迅速的实现C++的三次样条插值。