假设目前有 n + 1 n+1 n+1个路径点,它们分别是: ( x 0 , y 0 ) (x_0,y_0) (x0,y0), ( x 1 , y 1 ) (x_1,y_1) (x1,y1), ( x 2 , y 2 ) (x_2,y_2) (x2,y2), … \ldots … , ( x n , y n ) (x_n,y_n) (xn,yn),求解每一段样条曲线的系数 ( a i , b i , c i , d i ) (a_i,b_i,c_i,d_i) (ai,bi,ci,di),有如下方法:
计算点与点之间的步长:
h i = x i + 1 − x i , ( i = 0 , 1 , ⋯ , n + 1 ) h_i=x{_i+_1}\ −\ x_i,(i=0,\ 1,\ \cdots,\ n+1) hi=xi+1 − xi,(i=0, 1, ⋯, n+1)
解矩阵方程,求得二次微分值 m i m_i mi。
计算每一段的三次样条曲线系数:
a i = y i a_i=y_i ai=yi
b i = y i + 1 − y i h i − h i 2 m i − h i 6 ( m i + 1 − m 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) bi=hiyi+1−yi−2himi−6hi(mi+1−mi)
c i = m i 2 c_i=\frac{m_i}{2} ci=2mi
d i = m i + 1 − m i 6 h i d_i=\frac{m_{i+1}−m_i}{6h_i} di=6himi+1−mi
那么在每一个子区间 x i ≤ x ≤ x i + 1 x_i≤x≤x_{i+1} xi≤x≤xi+1内,其对应的样条函函数表达式为:
f i ( x ) = a i + b i ( x − x i ) + c i ( x − x i ) 2 + d ( x − x i ) 3 f_i(x)=a_i+b_i(x−x_i)+c_i(x−x_i)^2+d(x−x_i)^3 fi(x)=ai+bi(x−xi)+ci(x−xi)2+d(x−xi)3
下面使用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.基于三次样条插值的路径生成方法