机械臂规划----三次样条曲线

三次样条曲线将稀疏点变成稠密点，是常用的一种规划方法。

原理讲解

源代码

#!/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