路径规划中三次样条插值方法和python实现

路径规划中三次样条插值方法和python实现

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

附赠自动驾驶学习资料和量产经验：链接

1、Cubic Spline曲线定义

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

2、Cubic Spline曲线求解

已知:

a) n+1个数据点, i = 0, 1, …, n;

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

c) 节点达到二阶连续;

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

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

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

根据上述的公式可以得到4n-2个方程，然而有4n个未知数，所以还需要对边界做些约束，所以需要对两端点的微分加些限制。 选择不是唯一的，3种比较常用的限制如下。

a. 自由边界(Natural)

b. 固定边界(Clamped)

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

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

3、算法总结

4、举例

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

5、Python代码实现

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

#! /usr/bin/python
# -*- coding: utf-8 -*-
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()

拟合效果如下：