1三次多项式轨迹规划2五次多项式轨迹规划34-3-4次多项式轨迹规划基于Matlab的多项式轨迹规划4源码汇总三次多项式角度位置：约束条件：多项式系数：1三次多项式轨迹规划q0=30,q1=75，v0=v1=0时：限定最大速度为8时：三次多项式轨迹规划五次多项式角度位置：约束条件：2五次多项式轨迹规划q0=30,q1=75，v0=v1=0，a0=a1=0时：限定最大速度为8时：第一段初始位置（起始点）：第一段初始速度：第一段初始加速度：中间点1位置：第一段的终端本地时间：34-3-4轨迹规划第二段3次多项式终端本地时间：第三段4次多项式终端本地时间：第三段终端位置（终止点）：第三段终端速度（终止点速度）：第三段终端加速度（终止点加速度）：或%三次多项式源码clcclear%轨迹定义条件%时间t0=0;t1=6;v_max=8;%定义最大速度%位置和速度（a）q0=30;q1=75;v0=0;v1=0;%利用公式（1-22）求系数h=q1-q0;%最大位置差T=t1-t0;%最大时间差a0=q0;%第一个参数a1=v0;%第二个参数a2=(3*h-(2*v0+v1)*T)/(T*T);%第三个参数a3=(-2*h+(v0+v1)*T)/(T*T*T);%第四个参数%轨迹生成t=t0:0.1:t1;4源码汇总%位置q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3);v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2);%速度fori=1:1:length(v)if(v(i)>v_max)v(i)=v_max;endend%加速度acc=2*a2+6*a3*power((t-t0),1);%绘图subplot(3,2,1)plot(t,q,'r');ylabel('position')gridonsubplot(3,2,3)plot(t,v,'b');ylabel('velocity')gridonsubplot(3,2,5)plot(t,acc,'g');xlabel('(a)');ylabel('acceleration')%五次多项式源码clcclear%轨迹定义条件%时间t0=0;t1=10;%位置和速度（a）q0=0;q1=150;v0=0;v1=0;v_max=8;%定义最大速度acc0=0;%加速度acc1=0;%利用公式（1-25）求系数h=q1-q0;T=t1-t0;a0=q0;a1=v0;a2=1.0/2*acc0;a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)*(T*T));a4=1.0/(2*T*T*T*T)*(-30*h+(14*v1+16*v0)*T+(3*acc0-2*acc1)*(T*T));a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)*(T*T));%轨迹生成t=t0:0.1:t1;%位置q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...a4*power(t-t0,4)+a5*power(t-t0,5);%速度v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...5*a5*power(t-t0,4);%fori=1:1:length(v)%if(v(i)>v_max)%v(i)=v_max;%end%end%加速度acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);%绘图figure(1),subplot(3,1,1)%plot(t,q,'r');t1=1:1:t1;i=1;num=1;fori=1:i+10:length(q)q1(num)=q(i);num=num+1;endplot(t1,q1,'ro');%axis([0,8,0,11])ylabel('position')gridonsubplot(3,1,2)plot(t,v,'b');%axis([0,8,-1,2.5])ylabel('velocity')gridonsubplot(3,1,3)plot(t,acc,'k');xlabel('(a)');ylabel('acceleration')gridon%四三四次多项式轨迹规划clcclearToDeg=180/pi;ToRad=pi/180;%转化为弧度制%定义初始角度Th1=30;%30Th1_1=0;Th1_2=0;Th2=50;%50Th3=90;%90Th4=70;%70Th4_1=0;Th4_2=0;%定义初始本地起始时间Tao1_i=0;Tao2_i=0;Tao3_i=0;%定义初始本地终端时间Tao1_f=2;Tao2_f=4;Tao3_f=2;%定义全局起始、终止时间t_i=0;t_f=Tao1_f+Tao2_f+Tao3_f-(Tao1_i+Tao2_i+Tao3_i);TH=[Th1Th1_1Th1_2Th2Th200Th3Th300Th4Th4_1Th4_2]';%C=[a0a1a2a3a4b0b1b2b3c0c1c2c3c4]';M1=[10000000000000];M2=[01000000000000];M3=[00200000000000];M4=[1Tao1_fTao1_f^2Tao1_f^3Tao1_f^4000000000];M5=[00000100000000];M6=[012*Tao1_f3*Tao1_f^24*Tao1_f^30-10000000];M7=[0026*Tao1_f12*Tao1_f^200-2000000];M8=[000001Tao2_fTao2_f^2Tao2_f^300000];M9=[00000000010000];M10=[00000012*Tao2_f3*Tao2_f^20-1000];M11=[000000026*Tao2_f00-200];M12=[0000000001Tao3_fTao3_f^2Tao3_f^3Tao3_f^4];M13=[000000000012*Tao3_f3*Tao3_f^24*Tao3_f^3];M14=[0000000000026*Tao3_f12*Tao3_f^2];M=[M1;M2;M3;M4;M5;M6;M7;M8;M9;M10;M11;M12;M13;M14];C=inv(M)*TH;a0=C(1);a1=C(2);a2=C(3);a3=C(4);a4=C(5);b0=C(6);b1=C(7);b2=C(8);b3=C(9);c0=C(10);c1=C(11);c2=C(12);c3=C(13);c4=C(14);%轨迹生成%轨迹定义条件%t_array=[t_i,t_i+Tao1_f-Tao1_i,t_i+Tao1_f-Tao1_i+Tao2_f-Tao2_i,t_f];%q_array=[Th1,Th2,Th3,Th4];%位置%v_array=[0,-10,10,0];%速度t1=Tao1_i:0.1:Tao1_f;t2=Tao2_i+0.1:0.1:Tao2_f-0.1;%要考虑函数重合t3=Tao3_i:0.1:Tao3_f;t=t_i:0.1:t_f;%位置q1=a0+a1*power(t1,1)+a2*power(t1,2)+a3*power(t1,3)+a4*power(t1,4);q2=b0+b1*power(t2,1)+b2*power(t2,2)+b3*power(t2,3);q3=c0+c1*power(t3,1)+c2*power(t3,2)+c3*power(t3,3)+c4*power(t3,4);q=[q1q2q3];%速度v1=a1+2*a2*power(t1,1)+3*a3*power(t1,2)+4*a4*power(t1,3);v2=b1+2*b2*power(t2,1)+3*b3*power(t2,2);v3=c1+2*c2*power(t3,1)+3*c3*power(t3,2)+4*c4*power(t3,3);v=[v1v2v3];%加速度acc1=2*a2+6*a3*power(t1,1)+12*a4*power(t1,2);acc2=2*b2+6*b3*power(t2,1);acc3=2*c2+6*c3*power(t3,1)+12*c4*power(t3,2);acc=[acc1acc2acc3];%绘图figureplot(t,q,'r','LineWidth',2);%绘制角度位置holdonplot(t,v,'b','LineWidth',2);%绘制速度holdonplot(t,acc,'k','LineWidth',2);%绘制加速度xlabel('t')axis([0,8,-40,120]);legend('position','velocity','acceleration')%可依次设置成你想要的名字gridon
