Python实现三次样条插值进行无人车路径生成

三次样条插值算法

假设目前有$n+1$个路径点，它们分别是：$(x_0,y_0)$，$(x_1,y_1)$，$(x_2,y_2)$，$\dots$ ，$(x_n,y_n)$，求解每一段样条曲线的系数$(a_i,b_i,c_i,d_i)$，有如下方法：

1. 计算点与点之间的步长：
   $$h_i=x{}_i{+}_1-x_i，(i=0,1,\cdots ,n+1)$$

2. 将路径点和端点条件（如果满足自由边界三次样条中的端点条件，即$S″=0$）代入如下矩阵方程中：
   

3. 解矩阵方程，求得二次微分值$m_i$。

4. 计算每一段的三次样条曲线系数：
   $$a_i=y_i$$
   $$b_i=\frac{y_{i+1}-y_i}{h_i}-\frac{h_i}{2}{m}_i-\frac{h_i}{6}(m_{i+1}-m_i)$$
   $$c_i=\frac{m_i}{2}$$
   $$d_i=\frac{m_{i+1}-m_i}{6h_i}$$

5. 那么在每一个子区间 $x_i\le x\le x_{i+1}$内，其对应的样条函函数表达式为：
   $$f_i(x)=a_i+b_i(x-x_i)+c_i(x-x_i{)}^2+d(x-x_i{)}^3$$

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

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

import numpy as np
import bisect
from matplotlib import pyplot as plt


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

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 = 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()

生成路径的结果：

其中，绿色圆点为需要拟合的点集，红线为计算出来的三次样条曲线。

参考：
1.无人驾驶汽车系统入门（二十）——基于自由边界三次样条插值的无人车路径生成
2.基于三次样条插值的路径生成方法