基于三次样条插值的路径生成方法

Introduction

无人车路径规划问题是指在具有障碍物的环境中，按照一定的评价标准，比如路径长度最短或能量消耗最少原则等，寻找一条从起始状态到目标状态的无碰撞路径。其中路径生成是路径规划问题中的一个重要问题，也是后续进行轨迹规划的基础。本文介绍基于三次样条插值的路径生成方法。

样条插值是一种工业设计中常用的、得到平滑曲线的一种插值方法，三次样条又是其中用的较为广泛的一种。低阶样条插值虽然计算简单、稳定性好、收敛性有保证且易在电子计算机上实现，但只能保证各小段曲线在连接处的连续性，不能保证整件曲线的光滑性。

1.介绍三次样条差值

三次样条插值（Cubic Spline Interpolation）简称 Spline 插值，是通过一系列值点构建一条光滑曲线的过程。假如有一系列的点集：

样条曲线$S(x)$ 是一个分段定义的公式。给定n+1个数据点，共有n个区间，三次样条方程满足以下条件：
1） 在每个分区$[x_i,x_{i+1}](i=0,1,2,...,n-1)$ ,$S(x)$都是一个三次多项式；
2） 满足 $S(x_i)=y_i(i=0,1,2,...,n)$；
3） $S(x)$ 的一阶导数和二阶导数在区间上都是连续的，即曲线整个是光滑的。

n个三次多项式可以表示为：

其中$a_i,b_i,c_i,d_i$ 是未知数，共4n个。

2.构造三次样条方程

1）根据插值连续性，可得

则


则

2）根据微分连续性，可得

又根据多项式表达式，微分方程表述为：



根据自由边界条件，在首端和尾端的二次导为0，故

整理上式

3.python构建方程

1）$x_i在递增$

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

class Spline:

    def __init__(self, x,y):

        self.x=x
        self.y=y
        #点集数
        self.n=len(x)
        #计算h值
        h=np.diff(x)

        self.a=[]
        self.b =[]
        self.c =[]
        self.d = []

        self.a=[i for i in y]
     #   print(self.a)
        A=self.A_calc(h)
     #   print(A)
        B=self.B_calc(h)
     #   print(B)

        self.c=np.linalg.solve(A, B)
    #    print(self.c)

        for i in range(self.n-1):
            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
            self.b.append((self.a[i + 1] - self.a[i]) / h[i] - h[i] * (self.c[i + 1] + 2.0 * self.c[i]) / 3.0)



    #    print(self.d)
    #    print(self.b)

    #计算公式的左侧矩阵
    def A_calc(self,h):
        A = np.zeros((self.n, self.n))
        A[0, 0] = 1.0
        for i in range(self.n-2):
            A[i+1,i+1] = 2.0 * (h[i] + h[i + 1])
            A[i+1,i]=h[i]
            A[i + 1, i+2] = h[i+1]
        A[self.n-1, self.n-1]=1.0

        return A

    #计算公式的右侧矩阵
    def B_calc(self,h):
        B=np.zeros((self.n,1))
        for i in range(self.n - 2):
            B[i+1]=3.0*((self.a[i+2]-self.a[i + 1])/ h[i + 1]-(self.a[i + 1] - self.a[i]) / h[i])
        B[0]=0
        B[self.n - 1]=0

        return B
    def calc_equation(self, tx):
        ti = bisect.bisect(self.x, tx) - 1
        dx = tx - self.x[ti]
        result = self.a[ti] + self.b[ti] * dx + self.c[ti] * dx ** 2.0 + self.d[ti] * dx ** 3.0

        return result

def main():

    x = [-8, -3, 0.0, 6, 8, 10, 16, 19]
    y = [4.5, 0.8, 9.6, 8.8, 6.5, 4.5, -3.0, -8]

    spline = Spline(x, y)
    rx = np.arange(-8, 19, 0.01)
    ry = [spline.calc_equation(i) for i in rx]

    plt.plot(x, y, "ob", label='Point set')
    plt.plot(rx, ry, "-r",label='Curve fitting')
    plt.axis("equal")
    plt.legend()
    plt.show()


if __name__ == '__main__':
		main()

结果图：

2）$x_i非递增$，实际中可能经常发生

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

class Spline:

    def __init__(self, x,y):

        self.x=x
        self.y=y
        #点集数
        self.n=len(x)
        #计算h值
        h=np.diff(x)

        self.a=[]
        self.b =[]
        self.c =[]
        self.d = []

        self.a=[i for i in y]
     #   print(self.a)
        A=self.A_calc(h)
     #   print(A)
        B=self.B_calc(h)
     #   print(B)

        self.c=np.linalg.solve(A, B)
    #    print(self.c)

        for i in range(self.n-1):
            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
            self.b.append((self.a[i + 1] - self.a[i]) / h[i] - h[i] * (self.c[i + 1] + 2.0 * self.c[i]) / 3.0)



    #    print(self.d)
    #    print(self.b)

    #计算公式的左侧矩阵
    def A_calc(self,h):
        A = np.zeros((self.n, self.n))
        A[0, 0] = 1.0
        for i in range(self.n-2):
            A[i+1,i+1] = 2.0 * (h[i] + h[i + 1])
            A[i+1,i]=h[i]
            A[i + 1, i+2] = h[i+1]
        A[self.n-1, self.n-1]=1.0

        return A

    #计算公式的右侧矩阵
    def B_calc(self,h):
        B=np.zeros((self.n,1))
        for i in range(self.n - 2):
            B[i+1]=3.0*((self.a[i+2]-self.a[i + 1])/ h[i + 1]-(self.a[i + 1] - self.a[i]) / h[i])
        B[0]=0
        B[self.n - 1]=0
        return B

   **##不一样的地方**
    def calc_equation(self,acc,tx):
        ti=bisect.bisect(self.x, tx) - 1
        dx=tx - self.x[acc]
        result = self.a[acc] + self.b[acc] * dx + self.c[acc] * dx ** 2.0 + self.d[acc] * dx ** 3.0
        return result
def main():

    x=[-8, -6, -4, -2, 0, 2, 4, 6, 8, 10,14,16,13,10,8,6,4,2,0,-2,-4,-6,-8]
    y=[0.8, 0.5, 0.1, 1.2, 1.9, 2.0, 3.0,  1.5,1.2,0.6,4.5,8.6,10.8,11.2,12.3,13.9,14.5,12.3,11.8,10.4,13.4,15.6,11.1]
    spline = Spline(x, y)
    rx=[]
    ry=[]
    res=[]
    ro=[]
 
    #判断xi和xi+1的大小
    for i in range(len(x)-1):
        if x[i]x[i+1]:
            ro=np.arange(x[i], x[i + 1], -0.01)
        account=i
        res=[spline.calc_equation(account,i) for i in ro]
        ry.extend(res)
        rx.extend(ro)
        if i==1:
            print(len(ry))
            print(len(rx))

    plt.plot(x, y, "ob", label='Point set')
    plt.plot(rx, ry, "-r",label='Curve fitting')
    plt.axis("equal")
    plt.legend()
    plt.show()


if __name__ == '__main__':
		main()

结果图：

分析：在$x_i$第一次非递增时，计算出的路径存在问题。