大作业：PUMA 机器人1. 坐标系建立利用D-H 参数法建立坐标系：2．D-H 参数表关节i θ(︒) i α(︒) i l i d运动范围1 90 -90 0 0 -160°~160°2 0 0 a 2 d 2-225°~45° 3 90 90 -a 3 0 -45°~225° 4 0 -90 0 4d-110°~170° 5 0 90 0 0-100°~100° 66d-266°~266°3. 正运动学推导3.1变换矩阵求取1i-1i 11Rot(,) Trans(0,0,d ) Trans(l ,0,0) Rot(,) = 001i i i i i i i i i i i i i i i i i i i i i i c s c s s l c c c c c s l s A z x s c d θθαθαθθθαθαθθααα----⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦列各D-H 变换矩阵如下：11010101001000001-⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦c s s c A 22122220222020010001c s a c s c a s A d -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 33233033303301000001c s a c s c a s A -⎡⎤⎢⎥--⎢⎥=⎢⎥⎢⎥⎣⎦344404040400100001-⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦c s s c A d 455050505001000001c s s c A ⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦ 5666-6066000010001c s s c A d ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 注：为书写方面，本文中cos ,sin i i ci si θθ==又由00123456123456T A A A A A A =⋅⋅⋅⋅⋅，利用Matlab 进行符号运算，运行程序PUMAzhengyundongxue （程序详见附录）得：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=100060pz az sz nz py ay sy ny px axsx nx T其中- c6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - s6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3)) - c6*(s5*(c2*s1*s3 + c3*s1*s2) nx ny == - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3)))s4*s6*(c2*s3 + c3*s2) - c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2))nz ⎧⎪⎪⎪⎨⎪⎪=⎪⎩s6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - c6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(s5*(c2*s1*s3 + c3*s1*s2) - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))) sx sy ==+ c6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3))s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) + c6*s4*(c2*s3 + c3*s2)sz ⎧⎪⎪⎪⎨⎪⎪=⎪⎩c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)ax ay az =⎧⎪=⎨⎪=⎩24623346- d *s1+d *(c1*c2*s3 + c1*c3*s2) - d *s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) + a *c1*c2 + c5*d6*(c1*c2*s3 + c1*c3*s2) +a *c1*s2*s3 -a *c1*c2*c3c1*d2 + d *(c2*s1*s3 + c3*s1*s2) + d *s5*(c px py ==263342631*s4 + c4*(c2*c3*s1 - s1*s2*s3)) + a *c2*s1 + c5*d *(c2*s1*s3 + c3*s1*s2) +a *s1*s2*s3 - a *c2*c3*s1d *(c2*c3 - s2*s3) - a *s2 + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)) + a pz =3*c2*s3 +a *c3*s2⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩ 3.2变换矩阵验证 3.2.1初步验证根据DH 参数表中θi 的初始值，将其带入6T 后得到末端执行器的坐标为px= -d 2py=a 2+d 4+d 6 pz=a 3与初始设定一致，因此初步判断变换矩阵正确。

3.2.2使用matlab 机器人工具箱详细验证4 0 -90 0 d4=0.43307 -110°~170°5 0 90 0 0 -100°~100°6 0 0 0 d6=0.05625 -266°~266°利用matlab机器人工具箱按照以上DH参数，编写并运行程序PUMAyanzheng（程序详见附录）得到如下图，与PUMA560机器人一致，表明DH参数法建模正确。

4. 逆运动学推导反变换法求逆00123456123456T A A A A A A=⋅⋅⋅⋅⋅，且有⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=16pzazsznzpyaysynypxaxsxnxT（1）求1θ由010123451623456161-=⋅=⋅⋅⋅⋅=B A T A A A A A T得11111111111111110001''''''''''''0001nx*c ny*s sx*c sy*s ax*c ay*s px*c py*s -nz -sz -az -pz ny*c - nx*s sy*c - sx*s ay*c - ax*s py*c - px*s nx sx ax px ny sy ay py nz sz az pz ++++⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦4*6*(2*32*3)6*(5*(2*33*2)4*5*(2*32*3))6*(5*(2*32*3)4*5*('''c4*s6 2*33*2))4*6*(2*33*+ c5*c6*s42)--+---++=⎧⎪=⎨++⎪=-⎩s s c c s s c s c s c s c c c c s s c s c c s s c c c s c s s s c s c s nx ny nz's6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - c6*s4*(c2*c3 - s2*s3)'s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2)'c4*c6 + c5*s4*s6=-⎧⎪=⎨⎪=⎩sx sy sz 'c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)'c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3)'s4*s5ax ay az =⎧⎪=⎨⎪=⎩24633246px'=a *c2 + d *(c2*s3 + c3*s2) - d *(c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)) - a *c2*c3 + a *s2*s3'a *s2 - d *(c2*c3 - s2*s3) + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2))py =3326- a *c2*s3 - a *c3*s2'd - d *s4*s5pz ⎧⎪⎪⎪⎪⎨⎪⎪=⎪⎪⎩由矩阵对应元素均相等，1(3,3)T61(3,3)B =，1(3,4)T61(3,4)B =得2611s4*s511d - d *s4*s5ay*c - ax*s py*c - px*s =⎧⎨=⎩得到：()261111-=-py*c px*s d d *ay*c - ax*s整理有66211+-+=c *(py d *ay)(px d *ax)*s d三角变换66sin cos μμ+=⎧⎨+=⎩px d *ax r py d *ay r其中66*arctan *μ⎧=⎪⎨+=⎪+⎩r px d ax ()py d ax 得到21arccos μ=±-d θ()r（2）求3θ由21110103453216456B3=63A A A T A A A T ---==得664611121314 c4*c5*c6 + s4*s6- c6*s4 + c4*c5*s6c4*s5-d *c4*s521222324-c4*s6 + c5*c6*s4c4*c6 + c5*s4*s6s4*s5-d *s4*s531323334-c6*s5-s5*s6c5d d *c5414243440001b b b b b b b b b b b b b b b b ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦⎣⎦其中B3个元素如下：32111c2323123121232312313123231231412323123*3b ny*s * - nz*s +c *nx*c b sx*c *c - sz*s + sy*s *c b ax*c *c - az*s + ay*s *c b py*s *c - pz*s + c *px*c a a c =⎧⎪=⎪⎨=⎪⎪=+-⎩ 2b21= ny*c1 - nx*s1b22= sy*c1 -sx *s1b23= ay*c1 - ax*s1b24= py*c1 - px*s1-d ⎧⎪⎪⎨⎪⎪⎩23123123123322312312333231231233423123123*3b nz*c + ny*s *s + nx*c *s b sz*c + sx*c *s + sy*s *s b az*c + ax*c *s + ay*s *s b pz*c + py*s *s + px*c *s a s =⎧⎪=⎪⎨=⎪⎪=-⎩ 410420430441b b b b =⎧⎪=⎪⎨=⎪⎪=⎩注：为书写方便，本文中232323cos(),23sin()c s θθθθ=+=+。