哈工大PUMA機器人大作業(yè)

1、2017 年 秋 季學(xué)期研究生課程考核（讀書報告、研究報告）考核科目: 機器人技術(shù)學(xué)生所在院（系）: 機電學(xué)院學(xué)生所在學(xué)科: 機械電子學(xué) 生 姓 名: 王學(xué) 號: 17S學(xué) 生 類 別: 學(xué)碩考察結(jié)果: 閱卷人:PUMA 機器人正逆運動學(xué)推導(dǎo)及運動空間解算圖1 PUMA 機器人模型任務(wù):1. 建立坐標系；2. 給出D-H參數(shù)表；3. 推導(dǎo)正、逆運動學(xué)；4. 編程得工作空間。一、 建立坐標系根據(jù)PUMA 機器人運動自由度，在各關(guān)節(jié)處建立坐標系如圖2所示。圖2 PUMA 機器人坐標系建立圖其中與原點交于一點, 與原點交于一點。二、 D-H參數(shù)表D-H參數(shù)表可根據(jù)坐標系設(shè)定而得出，見表1。1) 為繞

2、軸，從到的角度；2) 為繞軸，從到的角度；3) 為沿軸，從與交點到的距離；4) 為沿軸，從與到的距離。桿件變量（當(dāng)前值）變量范圍1002304050060表1 PUMA 機器人桿件參數(shù)表三、 正運動學(xué)推導(dǎo)由坐標系及各桿件參數(shù)可得到6個連桿變換矩陣。 根據(jù)各連桿變換矩陣相乘，可以得到PUMA 機械手變換矩陣，其矩陣為關(guān)節(jié)變量的函數(shù)。將上述變換矩陣逐個依次相乘可以得到。等式聯(lián)立，對比等式左右兩側(cè)元素可得：nx=sin(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4)+ cos(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c

3、3)*cos(c1)*cos(c4)+ sin(c2+c3)*cos(c1)*sin(c5)；ox=cos(c6)*(cos(c4)*sin(c1)-cos(c2+c3)*cos(c1)*sin(c4)- sin(c6)*(cos(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)+ sin(c2+c3)*cos(c1)*sin(c5)；ax=sin(c5)*(sin(c1)*sin(c4)+cos(c2+c3)*cos(c1)*cos(c4)- sin(c2+c3)*cos(c1)*cos(c5)；px=d6*(sin(c5)*(sin(c1)*s

4、in(c4)+cos(c2+c3)*cos(c1)*cos(c4)- sin(c2+c3)*cos(c1)*cos(c5)-d2*sin(c1)-d4*sin(c2+c3)*cos(c1)+ a2*cos(c1)*cos(c2)+a3*cos(c1)*cos(c2)*cos(c3)- a3*cos(c1)*sin(c2)*sin(c3)；ny=-sin(c6)*(cos(c1)*cos(c4)+cos(c2+c3)*sin(c1)*sin(c4)- cos(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*

5、sin(c1)*sin(c5)；oy=sin(c6)*(cos(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*sin(c1)*sin(c5)-cos(c6)*(cos(c1)*cos(c4)+ cos(c2+c3)*sin(c1)*sin(c4)；ay=-sin(c5)*(cos(c1)*sin(c4)-cos(c2+c3)*cos(c4)*sin(c1)- sin(c2+c3)*cos(c5)*sin(c1)；py=d2*cos(c1)-d6*(sin(c5)*(cos(c1)*sin(c4)- cos(c2+c3)*

6、cos(c4)*sin(c1)+sin(c2+c3)*cos(c5)*sin(c1)- d4*sin(c2+c3)*sin(c1)+a2*cos(c2)*sin(c1)+ a3*cos(c2)*cos(c3)*sin(c1)-a3*sin(c1)*sin(c2)*sin(c3)；nz=cos(c6)*(cos(c2+c3)*sin(c5)-sin(c2+c3)*cos(c4)*cos(c5)+ sin(c2 + c3)*sin(c4)*sin(c6)；oz=sin(c2+c3)*cos(c6)*sin(c4)-sin(c6)*(cos(c2+c3)*sin(c5)- sin(c2+c3)*co

7、s(c4)*cos(c5)；az=-cos(c2+c3)*cos(c5)-sin(c2+c3)*cos(c4)*sin(c5)；pz=-d4*cos(c2 + c3)- a3*sin(c2+c3)-d6*(cos(c2+c3)*cos(c5)+ sin(c2+c3)*cos(c4)*sin(c5)-a2*sin(c2)；上述12個公式中，c1-c6分別對應(yīng)，將關(guān)節(jié)變量角度的當(dāng)前值代入以上公式得到： 計算結(jié)果與當(dāng)前位置姿態(tài)一致，推導(dǎo)結(jié)果無誤。四、 逆運動學(xué)推導(dǎo)逆運動學(xué)推導(dǎo)為末端位置姿態(tài)n,o,a,p確定，求解關(guān)節(jié)變量。 五、 工作空間的逆解運算從上述的推導(dǎo)中我們已經(jīng)得到了關(guān)節(jié)變量的推導(dǎo)公式。根據(jù)

8、逆解的求解思想，在這里我們參照正解所求得的工作間，將機器的運動空間劃分成了x=-1000:n1:1000, y=-1000:n2:1000, z=-1000:n3:1000,所組成的空間點集合，其中n1,n2,n3代表步距，單位為mm。在程序中通過循環(huán)，將此空間上的點的坐標一一代入中的，。中n,o,a，所對應(yīng)的9個參數(shù)，代表了末端坐標系（執(zhí)行器）各坐標軸與零坐標系（固定坐標系）各坐標軸之間的夾角余弦值。在這里為了簡便運算，減少運算量，我們討論固定姿態(tài)下機器人的工作空間。令，其余六個參數(shù)等于零，即當(dāng)前PUMA機器人末端執(zhí)行器相對于固定坐標系的姿態(tài)。至此，求解關(guān)節(jié)變量的參數(shù)均已給出。另外考慮到手腕

9、環(huán)節(jié)（后三個坐標系）對工作空間的影響不大，為了簡化運算，在這里只將逆解求解到，而不再討論。逆解源代碼，請參照附錄。逆解求解的結(jié)果如圖。六、 工作空間的正解運算機器人正解的末端執(zhí)行器（）相對于固定坐標系（）的轉(zhuǎn)換矩陣已經(jīng)求出。取上的原點,就可得到上的原點在固定坐標系下的坐標，即工作空間中一個點。通過將在對應(yīng)的變量范圍內(nèi)變換便可以得到整個工作空間。在這里為了減少運算量，同逆解，我們不討論的變化，只討論在各自運動范圍內(nèi)的變化。正解源代碼及改進的高效率代碼請參照附錄。逆解正解的結(jié)果如圖。圖3 PUMA 機器人逆解空間（軸測圖）圖4 PUMA 機器人逆解空間（XOY）圖5 PUMA 機器人逆解空間（XO

10、Z）圖6 PUMA 機器人逆解空間（YOZ）圖7 PUMA 機器人正解空間（軸測圖）圖8 PUMA 機器人正解空間（XOY） 圖9 PUMA 機器人正解空間（XOZ）圖10 PUMA 機器人正解空間（YOZ） 附錄a) 逆解程序clc;clear;a2=431.8;a3=-20.32;d2=149.09;d4=433.07;d6=56.25;i=1;j=1;t1=0,0;mt1=0,0;x=zeros(450000,1);y=zeros(450000,1);z=zeros(450000,1);for px=-1000:10:1000 for py=-1000:10:1000 for pz=-1

11、000:10:1000 j=j+1; m=1; t=0; k=px2+(py-d6)2-d22; if k<0 m=0; else s1=atan2(py-d6,px)-atan2(d2,sqrt(k); s12=atan2(py-d6,px)-atan2(d2,-sqrt(k); end if m=1 if s1<160*pi/180 && s1>-160*pi/180 t1(1)=s1; mt1(1)=1; end if s12<160*pi/180 && s12>-160*pi/180 t1(2)=s12; mt1(2)=1;

12、 end if mt1(1)=0&&mt1(2)=0 m=0; end end if m=1 for n=1:2 if mt1(n)=1 s1=sin(t1(1); c1=cos(t1(1); k=4*a22*(a32+d42)-(pz2+(px*c1+py*s1-d6*s1)2-a32-d42-a22)2; k0=pz2+(px*c1+py*s1-d6*s1)2-a22-a32-d42; if k<0 m=0; else s31=-atan2(d4,a3)-atan2(sqrt(k),k0); s32=-atan2(d4,a3)+atan2(sqrt(k),k0); i

13、f (s31<225*pi/180 && s31>-45*pi/180)|(s32<225*pi/180 && s32>-45*pi/180) else m=0; end if m=1 k=a32+d42-a22-pz2-(px*c1+py*s1-d6*s1)2; k0=4*a22*(a32+d42)-(a32+d42-a22-pz2-(px*c1+py*s1-d6*s1)2)2; k1=d6*s1-px*c1-py*s1; if k0>=0 t21=-atan2(k,sqrt(k0)-atan2(k1,pz); t22=atan2

14、(k,sqrt(k0)-atan2(k1,pz); if (t21<45*pi/180 && t21>-225*pi/180)|(t22<45*pi/180 && t22>-225*pi/180) t=1; end end end end end end end if t=1 x(i)=px; y(i)=py; z(i)=pz; i=i+1; end end endendscatter3(x(1:(i-1),y(1:(i-1),z(1:(i-1),2,'.');xlabel('X/mm')ylabel(&#

15、39;Y/mm')zlabel('Z/mm')title('逆解運算，步距10');b) 正解程序clear;clc;syms c1 c2 c3 c4 c5 c6;a2=431.8;d2=149.09;a3=20.32;d4=433.07;d6=56.25;A1=cos(c1),0,-sin(c1),0;. sin(c1),0,cos(c1),0;. 0,-1,0,0;0,0,0,1;A2=cos(c2),-sin(c2),0,a2*cos(c2);. sin(c2),cos(c2),0,a2*sin(c2);. 0,0,1,d2;0,0,0,1;A3=

16、cos(c3),0,-sin(c3),a3*cos(c3);. sin(c3),0,cos(c3),a3*sin(c3);. 0,-1,0,0;0,0,0,1;A4=cos(c4),0,-sin(c4),0;. sin(c4),0,cos(c4),0;. 0,-1,0,d4;. 0,0,0,1;A5=cos(c5),0,sin(c5),0;. sin(c5),0,-cos(c5),0;. 0,1,0,0;0,0,0,1;A6=cos(c6),-sin(c6),0,0;. sin(c6),cos(c6),0,0;. 0,0,1,d6;. 0,0,0,1;c4=pi;c5=0;c6=0;A4=ev

17、al(A4);A5=eval(A5);A6=eval(A6);T36=A4*A5*A6;x=zeros(26000,1);y=zeros(26000,1);z=zeros(26000,1);i=0;for c1=(-160:10:160)*pi/180 T01=eval(A1); for c2=(-225:10:45)*pi/180 T12=eval(A2); T02=T01*T12; for c3=(-225:10:45)*pi/180 T23=eval(A3); T03=T02*T23; T06=T03*T36; Object=T06*0;0;0;1; i=i+1; x(i)=Object

18、(1); y(i)=Object(2); z(i)=Object(3); end endendplot3(x(1:i),y(1:i),z(1:i),'.');fprintf(1,'n循環(huán)次數(shù)為%dn',i);grid on;xlabel('X');ylabel('Y');zlabel('Z');title('正解運算，步距10');c) 改進的正解程序clear;clc;syms c1 c2 c3;x=zeros(3000000,1);y=zeros(3000000,1);z=zeros(3000000,1);i=0;for c1=(-160:2:160)*pi/180 for c2=(-225:2:45)*pi/180 for