三次曲线和五次曲线函数

%三次曲线规划函数:

%x0,x1分别为规划起始位置和终止时刻位置,v0,v1分别为规划起始速度和终止时刻速度

%T为规划时长,t为求解时刻

function y=triple(x0,x1,v0,v1,T,t)
a=(2*x0-2*x1+v0*T+v1*T)/T^3;
b=(-3*x0+3*x1-2*v0*T-v1*T)/T^2;
c=v0;
d=x0;
y=a*t^3+b*t^2+c*t+d;
end



%五次曲线规划函数:

%x0,x1分别为规划起始位置和终止时刻位置,v0,v1分别为规划起始速度和终止时刻速度

%a0,a1分别为规划起始加速度和终止时刻加速度,T为规划时长,t为求解时刻

function y=five(x0,x1,v0,v1,a0,a1,T,t)

k0=x0;
k1=v0;
k2=a0/2;
k3=(20*x1-20*x0-8*v1*T-12*v0*T-3*a0*T^2+a1*T^2)/(2*T^3);
k4=(30*x0-30*x1+14*v1*T+16*v0*T+3*a0*T^2-2*a1*T^2)/(2*T^4);
k5=(12*x1-12*x0-6*v1*T-6*v0*T-a0*T^2+a1*T^2)/(2*T^5);
y=k5*t^5+k4*t^4+k3*t^3+k2*t^2+k1*t+k0;

end

其中,y=five(0, 10,0,0,0,0,10,t)的结果如下图:




展开阅读全文

没有更多推荐了,返回首页