機器臂運動軌跡規(guī)劃位姿1分析〔由最初始狀態(tài)到折疊狀態(tài)，圖中粉色線表示〕關(guān)節(jié)i關(guān)節(jié)變量初值關(guān)節(jié)變量值100°0θ10°-28°22450°0θ2-90°28°30-90°20400°0°一、位姿1分析1、運動學(xué)正解，求齊次變換矩陣〔Matlab編程〕symsa1%各關(guān)節(jié)變量變化量a1=—28*pi/180;a2=28*pi/180;a3=0*pi/180;%各z軸間夾角b1=0;b2=0;b3=-pi/2;%求齊次變換矩陣由公式=[cos(a)-sin(a)0csin(a)*cos(b)cos(a)*cos(b)-sin(b)-d*sin(b);sin(a)*sin(b)cos(a)*sin(b)cos(b)d*cos(b);0001]=**=*=*由此可求出各其次變換矩陣=[0.88290.469500-0.46950.882900001.0000000]=[0.8829-0.469500.46950.882900001.00000000]=[1.000000000.00001.00000-1.00000.0000000]=[1.00000000.00001.00000-1.00000.0000000]=[1.000000;01.00000;001.00000;000]=[0.8829-0.0000-0.4695;0.46950.00000.8829;0;00000]2、求雅克比矩陣由公式：z1=[T10(1,3);T10(2,3);T10(3,3)];z2=[T20(1,3);T20(2,3);T20(3,3)];z3=[T30(1,3);T30(2,3);T30(3,3)];p1=[T31(1,4);T31(2,4);T31(3,4)];p2=[T32(1,4);T32(2,4);T32(3,4)];r1=[T10(1,1)T10(1,2)T10(1,3);T10(2,1)T10(2,2)T10(2,3);T10(3,1)T10(3,2)T10(3,3)];r2=[T20(1,1)T20(1,2)T20(1,3);T20(2,1)T20(2,2)T20(2,3);T20(3,1)T20(3,2)T20(3,3)];p11=cross(z1,r1*p1);p22=cross(z2,r2*p2);j=[p11(1,1)p22(1,1)p11(2,1)p22(2,1)p11(3,1)p22(3,1)z1(1,1)z2(1,1)z1(2,1)z2(2,1)z1(3,1)z2(3,1)]J=[216.32220000000]3、運動學(xué)反解由程序：x=216.3222;y=88.9795;f1=x-T30(1,4);f2=y-T30(2,4);[a1,a2]=solve(f1,f2,a1,a2);a11=vpa(a1/pi*180,6);a22=vpa(a2/pi*180,6);可得出：a11=a22=4、操作速度分析symsq1q2v1v2v=[v1;v2;0;0;0;0];q=[q1;q2];je=j*q;f3=v(1,1)-je(1,1);f4=v(2,1)-je(2,1);[q1,q2]=solve(f3,f4,q1,q2)q11=vpa(q1,3)；q22=vpa(q2,3)；可得結(jié)果為：q11=0.462e-2*v2q22=-0.490e-2*v1-.202e-2*v25、各關(guān)節(jié)軌跡規(guī)劃Matlab程序：%利用三次多項式對關(guān)節(jié)角1進行軌跡規(guī)劃%設(shè)三次多項式為%f(t)=a0+a1*t+a2*t^2+a3*t^3%v(t)=diff(f(t),t)=a1+2*a2*t+3*a3*t^2軌跡上的關(guān)節(jié)速度%a(t)=diff(f(t),t,2)=2*a2+6*a3*t軌跡上的關(guān)節(jié)加速度%由在起始點和終止點的關(guān)節(jié)速度要求，規(guī)定%v(0)=0v(tf)=0,分別得出a1=a0=0%運動時間tf設(shè)為3s,關(guān)節(jié)角1終值為bf,初值為0symsa2abf=-28*pi/180;a1=0;a0=0;tf=3;f(tf)=a0+a1*tf+a2*tf^2+a3*tf^3-bf;v(tf)=a1+2*a2*tf+3*a3*tf^2;[a2,a3]=solve(f(tf),v(tf),a2,a3)%繪制關(guān)節(jié)角隨時間的變化的三次曲線t=0:0.01:tf;f3=a0+a1*t+a2*t.^2+a3*t.^3plot(t,f3)gridxlabel('時間t/s')ylabel('關(guān)節(jié)變量值/°')title('位姿1時關(guān)節(jié)角1軌跡規(guī)劃曲線')subplot(1,3,2)v=a1+2*a2*t+3*a3*t.^2plot(t,v)gridxlabel('時間t/s')ylabel('關(guān)節(jié)角1的關(guān)節(jié)速度°/s')title('位姿1時關(guān)節(jié)角1速度軌跡曲線')subplot(1,3,3)a=2*a2+6*a3*tplot(t,a)gridxlabel('時間t/s')ylabel('關(guān)節(jié)角1的加速度°/s^2')title('位姿1時關(guān)節(jié)角1加速度軌跡曲線')關(guān)節(jié)角1三次多項式差值曲線：關(guān)節(jié)角初值為0°，終值為-28°。關(guān)節(jié)角2三次多項式差值曲線：關(guān)節(jié)角初值為-90°，終值為28°。Figure.2二、位姿2分析〔由折疊狀態(tài)到抓球狀態(tài)，以折疊狀態(tài)為初始位姿，圖中藍色線表示〕關(guān)節(jié)i關(guān)節(jié)變量初值關(guān)節(jié)變量值100°0θ1-28°-28°22450°0θ228°-62°30-90°20400°0°運動學(xué)正解求齊次變換矩陣由上述公式可求出位姿2時的各齊次變換矩陣為%各關(guān)節(jié)變量變化量a1=—28*pi/180;a2=〔-62〕*pi/180;a3=0*pi/180;%各z軸間夾角b1=0;b2=0;b3=-pi/2;%求齊次變換矩陣=[0.88290.469500-0.46950.882900001.00000000]=[0.46950.88290-0.88290.469500001.00000000]=[1.000000000.00001.00000000]=[0.00000.00001.0000-1.00000.00000.00000000]=[0.00001.00000-1.00000.00000001.0000000]=[0.46950.00000.88290000]2、求雅克比矩陣由上述可求出位姿2時的雅克比可求的雅克比矩陣為：J=[420.32220000001.0000]3、運動學(xué)反解求前述的齊次變換矩陣，可得，X=,Y=當(dāng)a1,a2為變量時，求出的表達式x=420.3222;y=—115.0205;f1=x-T30(1,4);f2=y-T30(2,4);[a1,a2]=solve(f1,f2,a1,a2)a11=vpa(a1/pi*180,6)a22=vpa(a2/pi*180,6)求出結(jié)果為a11=a22=其中〔-28.001，-61.9997〕為最優(yōu)解4、操作速度分析設(shè)末端執(zhí)行器的速度為v=[v1;v2;0;0;0;0]，需要根據(jù)v的內(nèi)容來求解關(guān)節(jié)的速度q=[q1;q2]Matlab程序symsq1q2q3q4q5q6v1v2v3w1w2w3v=[v1;v2;0;0