机器人技术课程作业——PUMA机器人如上图所示的PUMA机器人，要求实现右图所示的运动，求解：①建立坐标系；②给出D-H参数表；③推导正运动学、逆运动学；④编程得出工作空间。

解：①建立坐标系a、建立原始坐标系b 、坐标系简化② 给出D-H 参数表a 、PUMA 机器人的杆件参数1d 0.6604m =，2d 0.14909m =，4d 0.43307m =，6d 0.05625m = 2a 0.4318m =，3a 0.02032m =b 、D-H 参数表关节i i θ i α i L i d运动范围 1 90 0 0 0-160o ~160o2 0 -90 02d 0.14909m = -225o ~45o3 -90 0 2a 0.4318m =-45o ~225o4 0 -90 3a 0.02032m = 4d 0.43307m = -110 o ~170 o5 0 90 0 0-100 o ~100 o6-906d 0.05625m = -266 o ~266 o③ 推导正运动学、逆运动学 a 、正运动学推导由式111111111100001ii i i i i i i i i i i i i i i i i i c s a s c c c s d s T s s c s c d c θθθαθαααθαθααα-----------⎡⎤⎢⎥--⎢⎥=⎢⎥⎢⎥⎣⎦可得： 111101000000100001c s s c T -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦222122200001000001c s d T s c -⎡⎤⎢⎥⎢⎥=⎢⎥--⎢⎥⎣⎦33233230000010001c s a s c T -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦44343444000100001c s a d T s c -⎡⎤⎢⎥⎢⎥=⎢⎥--⎢⎥⎣⎦ 554555000010000001c s T s c -⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦ 665666000010000001c s T s c -⎡⎤⎢⎥⎢⎥=⎢⎥--⎢⎥⎣⎦由00123456123456T T T T T T T =，得机械手变换矩阵：601x x x x yy y y z z z z n o a p n o a p T n o a p ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦23654164123651654164123654164123651654164123654642365()()()x y z n c c c c c s s c s c s c c c s s s c s n c c c c s s s s s c s s c c s c s c c n s c c c s s c c s =--++=----=--- 23654164123651654164123654164123651645164123654642365()()()x y z o c s c c c c s c s s s c s c s s c c s o c s c c s c s s s s s s s s c c c c c o s s c c s s c c s =-++-+=-+++-=++ 2354123515412354123515412352354x y z a c s c c s c c s s s a c s c s s c s s s c a c c s s c =---=--+=-+ 3231221423121323122142312142332322x y z p a c c a c c d s c d s p a c s a c s d s s d c p d c a s a s =+--=+-+=---b 、逆运动学推导 1)．求1θ用逆变换011T -左乘方程00123456123456T T T T T T T =两边：10123451623456T T T T T T T -=11111600000010000101xx x x yy y y zz z z c s n o a p s c n o a p T n o a p ⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦得112x y s p c p d -+=三角代换 cos x p ρφ=，sin y p ρφ=式中，ρ=tan 2(,)x y a p p φ=得到1θ的解12a tan 2(,)a tan 2(,y x p p d θ=-2)．求3θ矩阵方程两端的元素（1，4）和（2，4）分别对应相等113232242342332322x y z c p s p a c a c d s p d c a s a s +=+-⎧⎨-=++⎩ 平方和为：4333d s a c k += 其中 2222222242322x y z p p p d d a a k a ++----=解得：334a tan 2(,)a tan 2(,a d k θ=- 3)．求2θ在矩阵方程00123456123456T T T T T T T =两边左乘逆变换013T -。

1034536456T T T T T -=123123232312312323233611200101xx x x yy y y zz z z c c s c s a c n o a p c s s s c a s n o a p T s c d n o a p --⎡⎤⎡⎤⎢⎥⎢⎥---⎢⎥⎢⎥=⎢⎥⎢⎥--⎢⎥⎢⎥⎣⎦⎣⎦方程两边的元素（1，4）和（3，4）分别对应相等，得123123233231231232323400x y z x y z c c p s c p s p a a c c s p s s p c p a s d +---=⎧⎨++-+=⎩ 联立，得23s 和23c()()()()()()()()2341123323221123311234232211x y z x y z x y z x y z a s d c p s p p a c a s p c p s p a c a c p s p p a s d c p c p s p ⎧-+-+=⎪⎪++⎪⎨+++-⎪=⎪++⎪⎩23s 和23c 表达式的分母相等，且为正，于是()()()()()()23232341123323311234a tan 2,x y z x y z a s d c p s p p a c a a c a c p s p p a s d θθθ⎡⎤=+=-+-++++-⎣⎦根据解1θ和3θ的四种可能组合，可以得到相应的四种可能值23θ，于是可得到2θ的四种可能解:2233θθθ=-，式中2θ取与3θ相对应的值。

4．求4θ令两边元素（1，3）和（2，3）分别对应相等，则可得12312323451145x y z x y c c a s c a s a c s s a c a s s +-=-⎧⎨-+=⎩只要50s ≠，便可求出4θ()41112312323a tan 2,x y x y z s a c a c c a s c a s a θ=-+-+当50s =时，机械手处于奇异形位。

5．求5θ10454656T T T T -=14231414231423434224434123144123142343422443461231232332400101xx x x y y y y z z z z c c c s s s c c c s s c c c a d s c a n o a p s c c s c s s c c c s s c s a d c s a n o a p T c s s s c s a d n o a p +---+-⎡⎤⎡⎤⎢⎥⎢⎥-+--++⎢⎥⎢⎥=⎢⎥⎢⎥---+⎢⎥⎢⎥⎣⎦⎣⎦根据矩阵两边元素（1，3）和（2，3）分别对应相等，可得()()2341423141423145123231235z x y x y z a s c a c c c s s a s c c c s s a c s a s s a c c -+--=⎧⎪⎨---=⎪⎩()()()523414231414231412323123a tan 2,z x y x y z a s c a c c c s s a s c c c s a c s a s s a c θ=-+-----6．求6θ105566T T T -=根据矩阵两边元素（2，1）和（1，1）分别对应相等，可得()()()()()142314412314234614523154152341523152341552345236x y z x y z n c s c s c n s s c c c n s s s n c c c c s c s c s s n c s c c s s s s c c n s c c s s c ---++=⎧⎪⎨+-+---+=⎪⎩从而求得：()666atan 2,s c θ=④ 编程得出工作空间 a 、已知条件可以将连杆6的原点做为机器人的动点，连杆6原点相对于坐标系0就是06T 的x p 、y p 、z p ，已知：3231221423121323122142312142332322x y z p a c c a c c d s c d s p a c s a c s d s s d c p d c a s a s =+--=+-+=--- b 、Matlab 程序clc,clearlength2=431.8;length3=20.32; d2=149.09;d4=433.07; a=pi/180;for a1=-160*a:20*a:160*a for a2=-225*a:20*a:45*a a3=-45*a:20*a:225*afor k=1:length(a3)px(k)=cos(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*s in(a2+a3(k)))-d2*sin(a1)py(k)=sin(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*s in(a2+a3(k)))+d2*cos(a1)pz(k)=-a3(k)*sin(a2+a3(k))-length2*sin(a2)-d4*cos(a2+a3(k ))endplot3(px,py,pz),title('机器人的工作空间'),xlabel('xmm'),ylabel('y mm'),zlabel('z mm')hold ongrid onendendc、机器人工作空间。