自动驾驶运动规划(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代码实现
三阶样条曲线拟合代码如下：
代码语言：javascript复制#! /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使用上述代码将点集拟合成曲线：
代码语言：javascript复制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()拟合效果如下：
参考链接
https://zhuanlan.zhihu.com/p/62860859
https://www.cnblogs.com/xpvincent/archive/2013/01/26/2878092.html
完整代码链接：https://github.com/AtsushiSakai/CubicSpline/blob/master/PyCubicSpline/py_cubic_spline.py
