三次样条曲线将稀疏点变成稠密点,是常用的一种规划方法。
原理讲解
源代码
#!/usr/bin/env python
#-*-coding:utf-8-*-
#本文档用于三次样条曲线规划
#程序员:陈永*
#版权:哈尔滨工业大学
#日期:2019.11.26
import numpy as np
#***三次样条曲线***#
#三弯构造法求取三次样条曲线参数M
def spline_param(t,f,v0,vn):
'''
本函数用于求取三次样条曲线参数M
input:t,f,时间系列、数值点系列
v0,vn,起点、结束点速度
output:m,h对应点的加速度,时间间隔系列
'''
n = len(t)
#求取时间间隔及平均值
h = np.zeros(n-1) #时间间隔
c = np.zeros(n-1) #平均速度
for i in range(n-1):
h[i] = t[i+1] - t[i]
c[i] = (f[i+1] - f[i])/h[i]
#平均加速度求取
d = np.zeros(n)
d[0] = 6.0*(c[0] - v0)/h[0]
a = np.zeros(n-2)
b = np.zeros(n-2)
for i in range(n-2):
a[i] = h[i]/(h[i] + h[i+1]) #对应λ
b[i] = 1-a[i] #对应
d[i+1] = 6*(c[i+1] - c[i])/(h[i+1] + h[i])
d[n-1] = 6.0*(vn - c[n-2])/h[n-2]
#时间矩阵计算
P = 2*np.eye(n)
P[0,1] = 1
P[n-1,n-2] = 1
for i in range(n-2):
P[i+1