机器人轨迹规划matlab仿真代码

%example2.11, %七次多项式轨迹规划 %关节1 clc clear close('all') %轨迹定义条件 t0=0;t1=8; q0=0;q1=120; v0=0;v1=0; acc0=0;acc1=0; j0=0;j1=0; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v1)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度 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)+... +6*a6*power(t-t0,5)+7*a7*power(t-t0,6); %加速度 acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+... +5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5); %加加速度 j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+... +4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4); %绘图 figure(1) subplot(4,2,1) plot(t,q,'k'); ylabel('位置(deg)') grid on subplot(4,2,3) plot(t,v,'k'); ylabel('速度(deg/s)') grid on subplot(4,2,5) plot(t,acc,'k'); ylabel('加速度(deg/s^2)') grid on subplot(4,2,7) plot(t,j,'k'); xlabel('关节1/时间(s)'); ylabel('冲击(deg/s^3)') grid on %轨迹定义条件 t0=0;t1=8; q0=0;q1=-140; v0=0;v1=0; acc0=0;acc1=0; j0=-2;j1=2; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度 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)+... +6*a6*power(t-t0,5)+7*a7*power(t-t0,6); %加速度 acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+... +5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5); %加加速度 j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+... +4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4); %绘图 subplot(4,2,2) plot(t,q,'k'); hold on ylabel('位置(deg)') grid on subplot(4,2,4) plot(t,v,'k'); hold on ylabel('速度(deg/s)') grid on subplot(4,2,6) plot(t,acc,'k'); ylabel('加速度(deg/s^2)') grid on subplot(4,2,8) plot(t,j,'k'); xlabel('关节2/时间(s)'); ylabel('冲击(deg/s^3)') grid on %七次多项式轨迹规划 %关节3 clc clear close('all') %轨迹定义条件 t0=0;t1=8; q0=0;q1=125; v0=0;v1=0; acc0=0;acc1=0; j0=0;j1=0; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度 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)+... +6*a6*power(t-t0,5)+7*a7*power(t-t0,6); %加速度 acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+... +5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5); %加加速度 j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+... +4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4); %绘图 figure(2) subplot(4,2,1) plot(t,q,'k'); hold on % axis([0,8,-1,12]) ylabel('位置(deg)') grid on subplot(4,2,3) plot(t,v,'k'); hold on % axis([0,8,-1,3]) ylabel('速度(deg/s)') grid on subplot(4,2,5) plot(t,acc,'k'); % axis([0,8,-1.5,1.5]) ylabel('加速度(deg/s^2)') grid on subplot(4,2,7) plot(t,j,'k'); % axis([0,8,-1.1,1]) xlabel('关节3/时间(s)'); ylabel('冲击(deg/s^3)') grid on %轨迹定义条件 t0=0;t1=8; q0=0;q1=-120; v0=0;v1=0; acc0=0;acc1=0; j0=-2;j1=2; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度 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)+... +6*a6*power(t-t0,5)+7*a7*power(t-t0,6); %加速度 acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+... +5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5); %加加速度 j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+... +4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4); %绘图 subplot(4,2,2) plot(t,q,'k'); hold on ylabel('位置(deg)') grid on subplot(4,2,4) plot(t,v,'k'); hold on ylabel('速度(deg/s)') grid on subplot(4,2,6) plot(t,acc,'k'); ylabel('加速度(deg/s^2)') grid on subplot(4,2,8) plot(t,j,'k'); xlabel('关节4/时间(s)'); ylabel('冲击(deg/s^3)') grid on %七次多项式轨迹规划 %关节5 clc clear close('all') %轨迹定义条件 t0=0;t1=8; q0=0;q1=-200; v0=0;v1=0; acc0=0;acc1=0; j0=0;j1=0; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度 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)+... +6*a6*power(t-t0,5)+7*a7*power(t-t0,6); %加速度 acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+... +5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5); %加加速度 j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+... +4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4); %绘图 figure(3) subplot(4,2,1) plot(t,q,'k'); hold on % axis([0,8,-1,12]) ylabel('位置(deg)') grid on subplot(4,2,3) plot(t,v,'k'); hold on % axis([0,8,-1,3]) ylabel('速度(deg/s)') grid on subplot(4,2,5) plot(t,acc,'k'); % axis([0,8,-1.5,1.5]) ylabel('加速度(deg/s^2)') grid on subplot(4,2,7) plot(t,j,'k'); % axis([0,8,-1.1,1]) xlabel('关节5/时间(s)'); ylabel('冲击(deg/s^3)') grid on %轨迹定义条件 t0=0;t1=8; q0=0;q1=-310; v0=0;v1=0; acc0=0;acc1=0; j0=-2;j1=2; %利用（1-26）式求系数 h=q1-q0; T=t1-t0; a0=q0; a1=v0; a2=acc0/2; a3=j0/6; a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0)); a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1)); a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1)); a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1)); %计算轨迹 t=linspace(t0,t1,100); %位置 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)+a6*power(t-t0,6)+a7*power(t-t0,7); %速度