機(jī)器人技術(shù)大作業(yè)

1、大作業(yè)：PUMA機(jī)器人1. 坐標(biāo)系建立利用D-H參數(shù)法建立坐標(biāo)系:2. D-H參數(shù)表關(guān)節(jié)0（。）1運(yùn)動(dòng)范圍190-9000-16016020032d2-225 4539090-a30-45 22540-900心-110170509000-1001006000-2662663. 正運(yùn)動(dòng)學(xué)推導(dǎo)31變換矩陣求取A = RotCZj.pTrans(0,0a)Trans(l門0,0) Rot(xpa,)=clA =51o0一1-51cl0c2s20ac2% =s2c20a2s200120001000c4 0一$40c5 0550540 c4 04 As5 0 -c5 00-10r/40 10 00 0

2、0 10 0 0 10 0 0 1本文中 d = cos 0, si = sin 0,c30s3% =s30-c3-a3s301000001c6-s60 o54s6c60 0A1 =001久_000 1注：為書寫方面,又由雹刊.1舛丸.5石 利用Matlab進(jìn)行符號(hào)運(yùn)算,運(yùn)行程序列各D-H變換矩陣如下：PUMAzhengyundongxue （程序詳見附錄）得:nxsxaxPXnyQpynz.szazPZ0001其中nx = - c6*(s5*(c 1 *c2*s3 + c 1 *c3*s2) + c5*(s 1 *s4 - c4*(c 1 *c2*c3 - cl*s2*s3) -s6*(c4

3、*sl + s4*(cl*c2*c3 - cl*s2*s3)ny = s6*(cl*c4 - s4*(c2*c3*sl - sl*s2*s3) - c6*(s5*(c2*sl*s3 + c3*sl*s2)-c5*(cl*s4 + c4*(c2*c3*sl - sl*s2*s3)nz = s4*s6*(c2*s3 + c3*s2) - c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2) sx = s6*(s5*(c 1 *c2*s3 + c 1 *c3*s2) + c5*(s 1 *s4 - c4*(c 1 *c2*c3 - c 1 *s2*s3) -c

4、6*(c4*sl + s4*(c 1 *c2*c3 - c 1 *s2*s3)sy = s6*(s5*(c2*s 1 *s3 + c3*sl*s2) - c5*(cl*s4 + c4*(c2*c3*sl - sl*s2*s3) + c6*(c 1 *c4 - s4*(c2*c3*s 1 - s 1 *s2*s3)sz = s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2) + c6*s4*(c2*s3 + c3*s2)ax = c5*(cl *c2*s3 + cl*c3*s2) - s5*(sl*s4 - c4*(c 1 *c2*c3 - cl*s2*

5、s3) ay = c5*(c2*s 1 *s3 + c3*s 1 *s2) + s5*(c 1 *s4 + c4*(c2*c3*s 1 - si*s2*s3) az. = c5*(c2*c3 - s2*s3) c4*s5*(c2*s3 + c3*s2)px = -d2*sl+d4*(c 1 *c2*s3 + c 1 *c3*s2) -d6*s5*(s 1 *s4-c4*(c 1 *c2*c3 -c 1 *s2*s3)+ a?*cl*c2 + c5*d6*(cl*c2*s3 + cl*c3*s2) +a3*cl*s2*s3 -a3*cl*c2*c3py = cl*d2 + d4*(c2*sl*s

6、3 + c3*sl*s2) + d6*s5*(cl*s4 + c4*(c2*c3*sl - sl*s2*s3) + a2*c2*sl + c5*d6*(c2*sl*s3 + c3*sl*s2) +a?*sl*s2*s3 - a3*c2*c3*slpz = d4*(c2*c3 - s2*s3) - a,*s2 + d6*(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2) + a3*c2*s3 +a3*c3*s232變換矩陣驗(yàn)證3. 2.1初步驗(yàn)證根據(jù)DH參數(shù)表中4的初始值，將其帶入人后得到末端執(zhí)行器的坐標(biāo)為px= -d2pyp+di+cLpz=a3與初始設(shè)定一

7、致，因此初步判斷變換矩陣正確。3. 2. 2使用matlab機(jī)器人工具箱詳細(xì)驗(yàn)證關(guān)節(jié)0, ()乞(。)A(m)心(1D)運(yùn)動(dòng)范圍190-9000-160160。200a2=0. 4318d2=0. 14909-225 4539090-a：二-0. 02032()-45 22540-90()d產(chǎn)0. 43307-110170。509000-100100。6000d6=0. 05625-266266利用mat lab機(jī)器人工具箱按照以上DH參數(shù)，編寫并運(yùn)行程序PLMAysnzheng （程序詳 見附錄）得到如下圖，與PUMA560機(jī)器人一致，表明DH參數(shù)法建模正確。4. 逆運(yùn)動(dòng)學(xué)推導(dǎo)反變換法求逆

8、%人丸人孰人，且有nxsxaxPxsyQpynzszazPZ0001(1) 求 q由31 = %47；=覽丸人人61得nxsxlaxpxnysy時(shí)pynzszaz,陽0001Z7X*cl + /7y*51-nzny c- nx s0szsy * cl - sx*sl0axc + ay s畑aycl-ax s0pxcl + py* s-pz，py * cl - px s1nx = s4* s6* (c2 * c3 - $2 * $3) c6* (s5 * (c2 * $3+c3 水 s2) c4*c5* (c2 * c3 _ s2 * s3) ny1 = c6 * (s5 (c2 * c3 -

9、s2 * j3) + c4*c5* (c2 *j3 + c3* s2)+$4 * s6 * (c2 *s3+c3* s2) nz1 = -c4*s6 + c5*c6*s4sx = -s6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3) - c6*s4*(c2*c3 - s2*s3) sy9 = s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2) - c6*s4*(c2*s3 + c3*s2) sz = c4*c6 + c5*s4*s6ax = c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 -

10、 s2*s3) ay = c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3) az = s4*s5px=a2*c2 + d4*(c2*s3 + c3*s2) - d6*(c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3) -a3*c2*c3 + a3*s2*s3/?y, = a2*s2 - d4*(c2*c3 - s2*s3) + d6*(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2) -a3*c2*s3 - a3*c3*s2pz = 2 d6*s4*s5由矩陣對應(yīng)元素均相等,81(3,3

11、) =T61(3,3), 81(3,4) =T61(3,4)得av*cl - ax s = s4*s5py * cl=- d6*s4*s5得到:py *d- px*s = d2 -t/6 * ( *cl - or* $1)整理有cl (py + cl6 *ay)-(px + cl6 *ax)s = d?三角變換px + d6 * ax = rsin ppy + 6 * ay =廠 cos /其中r =* ax + PXY + (py + d6 * ay)2z px + d. * ax 、“ =arctanf)py + r/6 ax得到 =arccos(-)-/zr(2) 求q由 B3=%-,.

12、，VA_，-X =人XX =T63 得711b2bl 3b!4c4*c5*c6 + s4*s6-c6*s4 + c4*c5*s6c4*s5-d6*c4*s5b21b22b23b24-c4*s6 + c5*c6*s4c4*c6 + c5*s4*s6s4*s5-d6*s4*s5b3b32b33b34-c6*s5 s5*s6c5d4 -d6*c5Mlb42M344.0001 _其中B3個(gè)元素如下：bl 1 = ny *51* c23 nz * $23 + cl* nx * c23bl2 = sx*c* c23 - sz * s23 + sy * si * c23bl3 = ax *cl* c23 -

13、 a 乙 * s23 + ay *51* c23bl4 = py*si*c23- pz*s23 + cl* px*c23 + a3-a2 *c3b21= ny*cl - nx*slb22= sy*cl -sx *slb23= ay*cl - ax*slb24= py*cl - px*sl-d2b31 =* c23 + ny * si * $23 + nx * cl * s23b32 = 5Z* c23 + sx * cl * s23 + sy *51* s23 b33 = az * c23 + ar*cl* s23 + ay * si * s23 Z?34 = pz* c23 + py * s

14、i * s23 + px * cl * $23a2 * s3Ml=0 b42 = 0M3 = 0Z?44 = I注：為書寫方便.本文中e23 = cos(&2+Q),s23 = sin(q + q)。由矩陣元素對應(yīng)相等，B3(l,4) =-仇*33(1,3), 33(3,4) =心一6 *33(3,3)得a3 -(pz + az*d6)*s23 + d6*(ax*cl + ay*sl) + px*cl + py*sl*c23 = a2*c3-d4 + d6*(ax*cl + ay*sl) + px*cl + py*sl*s23+(pz + az*d6)*c23 = a2*s3 兩式平方求和，整

15、理得到：(2*J4 *E-2*3*F)*c23+(2*3*E+2*i/4 *F)*s23=G其中E = /?z + z*J6 F=J6 * ax*c 1 + ay * 51) + px*c + py*s , G=E2+F2+a/+d-a進(jìn)行三角變換2*/4 * E- 2* F = psincp2*3 *E + 2*= pcos(p其中p = yj(2d4 E-2* a3 *F)2 + (2*r/3 *E + 2*J4 *F)2 z2*J4*E-2*F、(p = arctanf丿2*3*E + 2*6；4*F從而得到婦+a =arclan(G/)_(p由 33(1,4) =* 33(1,3),

16、33(3,4) = d4 一 d6 * 33(3,3)得a3 -E*s23 + F*c23 = a2*c3-d4 + F*s23+E*c23 = a?*s3解得仇十(魚竺竺巴a,-E*s23 + F*c23(3) 求&a + F*s23+E*c23 as-E*s23 + F*c23 0. =(a+&J-= arctanf G/72-G2 )-(p-arctanf（4）求q由 83(3,3) = T 6(3,3)得c5 = az c23 + ax * cl * $23 + ay *51* s23解得&5 = arccos(oz * c23 + ax *cl* s23 + ay * $1 * $2

17、3)(5) 求q由 53(1,4) = T 63(1,4), 33(2,4) = T 63(2,4)得-d6 *c4*s5 = py *51* c23 p乙 * s23 + cl* px * c23 + a3 一 a2 * c3-d6*s4*s5 = cl* py - pxs-d2s5工0時(shí)，解得八zV*cl-/ZV*51-f/n04 = arctanf:=)py*sl*c23-pz*s23+px*cl*c23+a3-a2c3(6) 求由 83(3,1) = T63(3,1), 33(3,2)=T63(3,2)得-c6*s5 = sz* c23 4- sx * cl *523 + sy * $

18、1 * s23v-s5*s6 = nz * c23 + ny *51* s23 + nx *cl* c23s5工0時(shí)，解得_, sz*c23+sx*cl*S23+sv*$l*s23ft = arctanf:)nz*c23+ny *s*s23+nx * cl * c235. 工作空間求取末端操作手的坐標(biāo)為(px, py, pz),根據(jù)正運(yùn)動(dòng)學(xué)推導(dǎo)結(jié)果，用Mat lab編程求取其工作空間，根據(jù)參考文獻(xiàn)獲得PUMA560機(jī)器人各關(guān)節(jié)長度為：d2 = 149.09mm, 2 =431.8mm,a3 = 2Q.32mmd4 =433.07mm,d6 = 56.25mm編寫并運(yùn)行程序PUMXgongzuo

19、kongjian (程序詳見附錄)，得到：PUMA機(jī)器人的工作空間1000-1000 -1000變換角度:PUMA機(jī)器人的工作空間500-5001000500-10001000-5001000 -10001000俯視:綜上，PUMA560運(yùn)動(dòng)的逆解可能存在8種。但是，由于結(jié)構(gòu)的限制，例如各 關(guān)節(jié)變量不能在全部360。圍運(yùn)動(dòng)，有著特定的圍，有些解不能實(shí)現(xiàn)。這種多解 情況下，應(yīng)選取其中最滿意的一組解，以滿足機(jī)器人的工作要求。附錄:(1) PUMAzhengyundongxue 程序 clearsyms a2 a3 d2 d4 d6 cl c2Al=clA2=c2A3=c3A4=c4A5=c5A6=

20、c6c3 c4 c5 c6 si s2 s3 s4 s5 s6；0 -si 0；si 0 cl 0；0-s2 0 a2*c2；s2 c2 0-1 0 0；0 0 0 1；d2；0 0 0 1； 0 0；0 0 0 1；0 1；1;a2*s2；0 00 s3 -a3*c3；s3 0 -c3 -a3*s3；00 -s4 0； s4 0 c40 s5 0；s5 0 -c5-s6 0 0；s6 c6 00；0 -1 0 d4；00；0 10；0 00 0；0 01 d6；0 0 0 1；T60=A 1 *A2*A3M4*A5*A6T60=subs(T60,cl c2 c3c6 si s2 s3 s4

21、s5 s6t 0 1 0 1 1 1 1 0 1 0 0 0)c4 c5(2) PUMAyanzheng 程序clearLl=Link(-pi/2 000 0.standard)L2=Link(0 0.4318 0 0.14909 0 / standard*)L3=Link(pi/2 -0.02032 pi/2 0 0 / standard*)L4=Link(-pi/2 0 0 0. 43307 0/standard1)L5=Link(pi/2 0 0 0 0t * standard*)L6=Link(0 0 0 0.05625 0.standard1) r=robot(Ll L2 L3 L4

22、 L5 L6 ) drivebot(r)(3) PUMAgongzuokongjian 程序clear82=431. 8； a3=20. 32； d2=149. 09；(14=433. 07；d6=56. 25；cl=-0.5*pi；c3=0. 5*pi；c4=-0. 5*pi；c5=0. 5*pi；for bl=-160. *pi/180：32. *pi/180：160. *pi/180for b2=-225. 1/180:54. *pi/180：45. *pi/180for b3=-45. *pi/180：54. *pi/180：225.和i/180for b4=-U0. *pi/180：

23、 140.和i/180： 170.和i/180for b5=-100. *pi/180：100.和i/180：100. *pi/180x=d l*(cos(b3)*s in(c3)(cos(b1)*si n(b2) + cos(b2)*cos(c1)*sin(bl) +.sin(b3)*sin(c3)(cos(bl)*cos(b2) - cos(cI)*sin(b1)*sin(b2) + cos(c3)*s i n(b1)*s i n(c1) + d6*(cos(c5)*(cos(c4)(cos(b3)*s i n(c3)(cos(b1)*s i n(b2) + cos(b2)*cos(cl)

24、*sin(bl) + sin(b3)*sin(c3)(cos(bl)*cos(b2) - cos(c1)in(b1)*sin(b2) + cos(c3)*sin(bl)*sin(cl) + cos(b4)*si n(c4)(cos(b3)*cos(c3)*(cos(bl)*s in(b2) + cos(b2)*cos(c1)*sin(b1) + cos(c3)*sin(b3)*(cos(bl)cos(b2) 一cos (c 1) *sin (b 1) *sin(b2) - sin(bl)*sin(cl)*sin(c3) +sin(b4)*sin (c4) * (cos (b3) (cos (b

25、l) *cos (b2) - cos (cl)*sin(bl)*sin(b2)- sin (b3) (cos (b 1) *s in (b2) + cos(b2) *cos(cl)*sin(bl) + sin(b5)*sin(c5)(cos(b4)(cos(b3)*(cos(b1)*cos(b2) 一 cos(cl)*sin(bl)*sin(b2) 一 sin(b3)*(cos(bl)*sin(b2) + cos(b2)*cos(cl)*sin(bl) - sin(b4)*(cos(b3)*cos(c3)*(cos(bl)*sin(b2) +cos(b2)*cos(cl)*sin(bl) +c

26、os(c3)*sin(b3)*(cos(bl)*cos(b2) - cos(c1)*sin(b1)*sin(b2) sin(bl)*sin(cl)*sin(c3) +cos (b5) *s i n (c5)* (cos (b4)水cos (c4)*(cos (b3) *cos (c3) * (cos (bl)*sin (b2) +cos(b2)*cos(cl)*s in(b1) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) 一cos (c 1) *sin(b 1) *sin(b2) - sin(bl)*sin(cl)*sin(c3) -sin(c4)* (cos (b3

27、) *s i n (c3) (cos (bl)*sin (b2) + cos (b2) *cos (cl)*sin(bl) +sin(b3)*sin(c3)*(cos(bl)*cos(b2)-cos (c 1) *s i n (b 1) *s i n (b2) + cos(c3) *sin(bl)*sin(cl) +cos (c4) *si n (b4) * (cos (b3) (cos (bl) *cos (b2) - cos (cl)*sin(bl)*sin(b2)- sin (b3) (cos (b 1) *s in (b2) + cos(b2) *cos(c1)*sin(b1) + a

28、2*cos(bl)*cos(b2) + d2*sin(bl)sin(cl)- a3*cos(b3)(cos(b1)*cos(b2) - cos(cl)sin(bl)sin(b2) + a3*sin(b3)(cos(b1)*sin(b2) + cos(b2)*cos(cl)*sin(bl) - a2*cos(cl)*sin(bl)*sin(b2)y=d4*(cos(b3)水sin(c3)*(sin(bl)*sin(b2) - cos(bl)*cos(b2)*cos(cl) +sin(b3)*sin(c3)*(cos(b2)*sin(bl) +cos(b1)*cos(c1)*sin(b2) - c

29、os(bl)cos(c3)*sin(cl)+ d6*(cos(c5)*(cos(c4)(cos(b3)*sin(c3) (sin(bl)*sin(b2) - cos(b1)*cos(b2)*cos(cl) + s i n (b3) *s i n (c3) (cos (b2) *s i n (b 1) + cos (b 1) *cos (c 1) *s i n (b2)-cos (bl)*cos (c3)*sin(cl) + cos (b4) *sin (c4) * (cos (b3) *cos (c3) *(sin (bl) sin (b2) - cos(bl)*cos(b2)*cos(cl)

30、 + cos(c3)*sin(b3)(cos(b2)*sin(bl) + cos (b 1) *cos (c 1) *s i n (b2) + cos (bl)*sin(cl)*sin(c3) +sin(b4)*sin (c4) * (cos (b3) (cos (b2) *s i n (b 1) + cos (bl) *cos (cl) *sin (b2) - sin(b3) (sin(b 1)*sin(b2)-cos(b1)*cos(b2)*cos(cl) +cos (b5) *s i n (c5) * (cos (b4)水cos (c4) (cos (b3) *cos (c3) *(si

31、n(bl)*sin (b2) -cos(bl)*cos(b2)*cos(cl) + cos(c3)*s i n(b3)*(cos(b2)*s i n(b1) + cos(b1)*cos(c1)*sin(b2) + cos(bl)*sin(cl)*sin(c3) -sin(c4)*(cos(b3)*sin(c3)*(sin(bl)*sin(b2) - cos (bl)水cos(b2)*cos (cl) +sin(b3)*sin(c3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*s i n(b2) - cos(b1)*cos(c3)*si n(c1) +cos(c4)*

32、sin(b4)*(cos(b3)*(cos(b2)*sin(b1) + cos(bl)*cos(c1)sin(b2)- sin(b3)*(sin(bl)*sin(b2)-cos(bl)*cos(b2)*cos(cl) + sin(b5)*sin(c5)*(cos(b4)(cos(b3)*(cos(b2)*sin(bl) + cos(bl)*cos(cl)*sin(b2) 一 sin(b3)(sin(bl)*sin(b2)-cos(b1) *cos(b2) *cos(cl) - sin(b4)(cos(b3)*cos(c3)(s in(b1)*s in(b2) - cos(bl)*cos (b2

33、)*cos(cl) + cos (c3) *sin (b3) (cos (b2) *sin (b 1) + cos (bl) *cos (cl) *sin (b2) +cos(b 1)*sin(c 1)*sin(c3) + a2*cos(b2)*sin(b 1) - d2*cos(bl)*sin(cl) - a3*cos (b3) (cos (b2) *s i n (b 1)十 cos (bl) *cos (c 1) *s i n (b2) + a3*sin(b3)*(sin(bl)*sin(b2) cos (bl)*cos (b2)*cos(cl) + a2*cos(bl)*cos(cl)*

34、sin(b2)z=dl*(cos(cl)*cos(c3) - cos(b2)*cos(b3)*sin(cl)*sin(c3) + sin(b2)*sin(b3)*sin(cl)*sin(c3) + d2*cos(cl) +d6*(cos(c5)*(cos(c4)*(cos(c1)*cos(c3) cos(b2)*cos(b3)*s i n(c1)*s i n(c3) + sin(b2)*sin(b3)*sin(cI)*sin (c3) + sin(b4)*sin(c4)*(cos(b2)*sin(b3)*sin(cl) + cos(b3)*sin(b2)*sin(cl) 一cos(b4)*si

35、n(c4)*(cos(cl)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(cl) -cos(c3)*s j n(b2)*s i n(b3)*s i n(c1) + sin(b5)sin(c5)(cos(b4)*(cos(b2)*sin(b3)*sin (cl)十cos (b3) *s i n (b2) *s i n (c 1) + sin (b4) *(cos (cl) *s in (c3) +cos(b2)*cos(b3)*cos(c3)*sin(cl) - cos(c3)*sin(b2)*sin(b3)*sin(c1) -cos (b5) *s in (c5

36、) * (sin (c4) (cos (cl) *cos (c3) 一cos(b2)*cos(b3)*s i n(c1) *s i n(c3) + sin(b2)*sin(b3)sin(cl)*sin(c3) +cos (b4)*cos (c4)*(cos(cl)*sin(c3) + cos (b2) *cos (b3) *cos (c3) *sin (cl) -cos (c3) *s i n (b2) *s i n (b3) *s i n (c 1) 一cos(c4)*si n(b4)(cos(b2)*sin(b3)*sin (cl) + cos(b3)*s i n(b2)*s i n(c1) + a2*sin(b2)*sin(cl) - a3*cos(b2)*sin(b3)*sin(c1) - a3*cos(b3)*sin(b2)*sin(cl) plot3(x,y.z,*)hold on grid