机械臂规划----五次多项式

规划中，常用到三次、五次多项式式，本文将介绍五次多项式规划。

原理讲解

源代码

注：这是我早期写的代码，区分单变量和多变量，最好的办法是将多变量和单变量整合为统一函数，对参数判断进行区分。

#!/usr/bin/env python
#-*-coding:utf-8-*-
#本文档用于五次多项式规划
#程序员：陈永*
#版权：哈尔滨工业大学
#日期：2019.11.26
import numpy as np
#5次多项式插值求取求取一段时间的离散数值点,单变量
def interp5rdPoly1(q0,qv0,qa0,qf,qvf,qaf,tf,dt):
	'''
		本函数用5项插值将离散数据连续化,单变量
		input:q0,qv0,qa0起点位置速度加速度
			  qf,qvf,qaf终点位置速度加速度
			  tf终点时间，dt时间间隔
		output:[qq,qv,qa] 对应时刻位置速度加速度
	'''
	k = np.floor(tf/dt).astype(int) + 1
	#求取5次多项式插值系数

	a0 = q0
	a1 = qv0
	a2 = qa0/2.0
	a3 = (20*(qf - q0) - (8*qvf + 12*qv0)*tf + (qaf - 3*qa0)*tf*tf)/(2*pow(tf,3))
	a4 = (-30*(qf - q0) + (14*qvf + 16*qv0)*tf - (2*qaf - 3*qa0)*tf*tf)/(2*pow(tf,4))
	a5 = (12*(qf - q0) - (6*qvf + 6*qv0