機(jī)械臂軌跡規(guī)劃

《機(jī)械臂軌跡規(guī)劃》由會員上傳分享，免費(fèi)在線閱讀，更多相關(guān)內(nèi)容在工程資料-天天文庫。

1、機(jī)器臂運(yùn)動軌跡規(guī)劃位姿1分析（rh最初始狀態(tài)到折疊狀態(tài)，圖中粉色線表示）al=—28*pi/180;a2=28*pi/180;a3=0*pi/180;%各乙軸間夾角j—1丁山公式丿=[cos(a)-sin(a)sin(a)*cos(b)cos(a)*cos(b)sin(a)*sin(b)cos(a)*sin(b)00關(guān)節(jié)i%%40關(guān)節(jié)變量初值關(guān)節(jié)變量值100°0910°-28°22450°092?90。28°30-90°20400°0°一、位姿1分析1、運(yùn)動學(xué)正解,求齊次變換矩陣（Matbb編程）symsala2a3b1b2b3%各關(guān)節(jié)變量變化雖bl=O;b2=0;b3=-

2、pi/2;%求齊次變換矩陣0c;-sin(b)-d*sin(b);cos(b)d*cos(b);01IllI此可求出各其次變換矩陣0丁/=[0.88290.4695-0.4695000.88290000001.0000001.0000]0245.00000.46950.88290001.00000003/=[1.000000紅=[0.8829-0.46950001.0000]0000丁3/=[1.0000-L00000.00000.0000001.0000]00216.32220.00001.0000204.00000.00001.000088.979500=[1.0000

3、-L00000.00000.000001.0000]0216.3222;-115.0205;0；1.0000]1.00000001.000()0[0.8829-0.0000-0.4695149.2278;0.46950.00000.8829180.1213;0-1.00000.00000.0000;0001.000012、求雅克比矩陣rh公式:zl=[T10(l,3)；T10(2,3);T10(3,3)];z2=[T20(l,3)；T20(2,3)；T20(3,3)]；z3=[T30(l,3)；T30(2,3)；T30(3,3)]；p1=[T31(1,4);T31(2,4)

4、;T31(3,4)];p2=[T32(l,4);T32(2,4);T32(3,4)];rl=[T10(l,l)T10(l,2)T10(l,3)；T10(2,l)T10(2,2)T10(2,3)；T10(3,1)T10(3,2)T10(3,3)];r2=[T20(l,l)T20(l,2)T20(l,3);T20(2,l)T20(2,2)T20(2,3)；T2O(3,1)T2O(3,2)T20(3,3)];pll=cross(zl,rl*pl);p22=cross(z2,r2*p2);j=[pl1(1,1)p22(l,l)pl1(2,1)p22(2,l)pl1(3,1)p22(

5、3,l)zl(l,l)z2(l,l)zl(2,l)z2(2,l)zl(3,l)z2(3,l)]J=[-88.9795-204.0000216.322200000001.00001.0000]3、運(yùn)動學(xué)反解由程序：x=216.3222;y=88.9795;fl=x-T30(l,4);f2=y-T30(2,4);[a1,a2]=solve(fl,f2,a1,a2);al1=vpa(a1/pi*180,6);a22=vpa(a2/pi*l80,6);可得出:al1=-28.000172.7177a22=27.9999152.0004、操作速度分析symsqlq2vlv2v=[vl

6、;v2;0;0;0;0];q=[ql;q2];je=j*q；n=v(l,l)-je(l,l);f4=v(2,l)-je(2,l);[ql,q2]=solve(f3,f4,ql,q2)qll=vpa(ql,3);q22=vpa(q2,3);可得結(jié)果為：ql1=0.462e-2*v2q22=-0.490e-2*vl-.202e-2*v25、各關(guān)節(jié)軌跡規(guī)劃Matlab程序：%利用三次多項(xiàng)式對關(guān)節(jié)角1進(jìn)行軌跡規(guī)劃%設(shè)三次多項(xiàng)式為%f(t)=aO+a1*t+a2*tA2+a3*tA3%v(t)=diff(f(t),t)=a1+2*a2*a3*tA2軌跡上的關(guān)節(jié)速度%a(t)=diff

7、(f(t),t,2)=2*a2+6*a3*t軌跡上的關(guān)節(jié)加速度%由在起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度要求，規(guī)定%v(0)=0v(tf)=O,分別得出al=aO=O%運(yùn)動時間tf設(shè)為3s,關(guān)節(jié)角1終值為bf,初值為0symsa2a3tft;bf=-28*pi/180;a1=0;a0=0;tf=3;f(tf)=a0+a1*tf+a2*tfA2+a3*tfA3-bf;v(tf)=a1+2*a2*tf+3*a3*tfA2;fa2,a3]=solve(f(tf),v(tf),a2,a3)%繪制關(guān)節(jié)角隨時間的變化的三次曲線t=0:0.