自动驾驶路径规划技术-三次样条插值(Cubic Spline Interpolation)曲线及Python代码实现

YoungTimes

发布于 2022-04-28 15:39:25

1.4K0

发布于 2022-04-28 15:39:25

自动驾驶运动规划(Motion Planning)是无人驾驶汽车的核心模块之一，它的主要任务之一就是如何生成舒适的、碰撞避免的行驶路径和舒适的运动速度。生成行驶路径最经典方法之一就是是Sampling-Based Planner算法；基于采样的规划器可以规划出可行的轨迹，但这种轨迹往往是折线，为了保证车辆行驶过程中给乘客良好舒适的体验，需要对规划的轨迹进行平滑。Cubic Spline就是一种常用的插值平滑算法，通过一系列的控制点得到一条连续平滑的轨迹。

1、Cubic Spline曲线定义

假定有以下n+1个节点：

无人驾驶路径规划技术(1)-Cubic Spline曲线

2、Cubic Spline曲线求解

已知:

a) n+1个数据点

, i = 0, 1, …, n;

b) 每一分段都是三次多项式函数曲线;

c) 节点达到二阶连续;

d) 左右两端点处特性（自然边界，固定边界，非节点边界）

根据已知点求出每段样条曲线方程中的系数，即可得到曲线方程。

曲线求解过程的推导的过程如下：

1）根据插值和连续性的定义：

2）根据微分连续性的定义:

3）样条曲线的微分式：

根据上述的公式可以得到4n-2个方程，然而有4n个未知数，所以还需要对边界做些约束，所以需要对两端点

和

的微分加些限制。选择不是唯一的，3种比较常用的限制如下。

a. 自由边界(Natural)

b. 固定边界(Clamped)

将上述两个公式代入方程组，新的方程组左侧为：

c. 非节点边界(Not-A-Knot)

指定样条曲线的三次微分相等，即：

新的方程组系数矩阵可写为：

下图可以看出不同的端点边界对样条曲线的影响：

无人驾驶路径规划技术(1)-Cubic Spline曲线

3、算法总结

假设有n+1个数据节点：

，曲线插值的步骤如下：

a) 计算步长：

，其中i = 0, 1, ..., n-1;

b) 将数据节点和指定的首尾断点条件代入矩阵方程；

c) 解矩阵方程，求得二次微分方程

，该矩阵为三对角矩阵；常见解法为高斯消元法，可以对系数矩阵进行LU分解，分解为单位下三角矩阵和上三角矩阵。即：

d) 计算样条曲线的系数：

其中i=0，1，...，n-1

4、举例

以y=sin(x)为例, x步长为1，x取值范围是[0,10]。对它使用三次样条插值，插值前后对比如下：

5、Python代码实现

三阶样条曲线拟合代码如下：

#! /usr/bin/python

u"""
Cubic Spline library

author Atsushi Sakai

license: MIT
"""
import math
import numpy as np

class Spline:

    u"""
    Cubic Spline class
    usage:
        spline=Spline(x,y)
        rx=np.arange(0,4,0.1)
        ry=[spline.calc(i) for i in rx]
    """

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

        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.c = np.linalg.solve(A, B)
        #  print(self.c1)

        # 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):
        u"""
        Calc position
        if t is outside of the input x, return 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):
        u"""
        search data segment index
        """

        for i in range(self.nx):
            if self.x[i] - x > 0:
                return i - 1

    def __calc__A(self, h):
        u"""
        calc matrix A for spline coefficient c
        """
        A = np.zeros((self.nx, self.nx))
        A[0, 0] = 1.0
        for i in range(self.nx - 1):
            if i is not 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):
        u"""
        calc matrix B for spline coefficient c
        """
        B = np.zeros(self.nx)
        for i in range(self.nx - 2):
            B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]

        return B

使用上述代码将点集拟合成曲线：

def test1():
    import matplotlib.pyplot as plt
    # input
    x = [-2.5, 0.0, 2.5, 5.0, 7.5]
    y = [0.7, -6, 5, 6.5, 0.0]

    # 3d spline interporation
    spline = Spline(x, y)
    rx = np.arange(-2.5, 7.5, 0.01)
    ry = [spline.calc(i) for i in rx]

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


if __name__ == '__main__':
    test1()

拟合效果如下：