三次多项式轨迹规划

 Trajectory Planning for Automatic Machines and Robots 2.1.4小节三次多项式轨迹规划

%三次多项式的轨迹规划
%支持中间路径点
%需要填入的参数为起点和终点的位置   起点和终点的速度 起点和终点的时间 一共6个参数 第七个参数决定是否需要画图 1表示画图 0反之
%plot_choice 决定是否要画出函数的图像
function [p,sd,sdd,t]=cubic_trajectory(i_pos,f_pos,i_vel,f_vel,i_t,f_t,plot_choice)
		h=f_pos-i_pos;
		T=f_t-i_t;
		a0=i_pos;
		a1=i_vel;
		a2=(3*h-(2*i_vel+f_vel)*T)/(T^2);
		a3=(-2*h+(i_vel+f_vel)*T)/(T^3);

		%开始画图
        t=i_t:0.1:(f_t);
        len=length(t);
        p=zeros(1,len);
        sd=zeros(1,len);
        sdd=zeros(1,len);
        for i=1:len
        p(i)=a0+a1*(t(i)-i_t)+a2*(t(i)-i_t)^2+a3*(t(i)-i_t)^3;
		sd(i)=a1 + a2*(2*t(i) - 2*i_t) + 3*a3*(t(i) - i_t)^2;
        sdd(i)=2*a2 + 3*a3*(2*t(i) - 2*i_t);
        end 
        if  plot_choice > 0
		%位置图像
		subplot(221);
		plot( t,p);

		%速度图像
		subplot(222);
		plot(t,sd);

		%加速度图像
		subplot(223);
		plot(t,sdd);
		end
end
%例子
% [p,sd,sdd,t]=cubic_trajectory(0,10,0,0,0,8,1)
		

图像：