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);endend%例子% [p,sd,sdd,t]=cubic_trajectory(0,10,0,0,0,8,1)图像：