机器人技术课程作业——PUMA机器人如上图所示的PUMA机器人，要求实现右图所示的运动，求解：①建立坐标系；②给出D-H参数表；③推导正运动学、逆运动学；④编程得出工作空间。
解：①建立坐标系a、建立原始坐标系b、坐标系简化②给出D-H参数表a、PUMA机器人的杆件参数d0.6604m，1 d 0.14909m，2d 0.43307m，4d 0.05625m6a 0.4318m，a3 0.02032m2b、D-H参数表关节i i i Lid i 运动范围1 90 0 0 0 -160o~160oo o2 0 -90 0 d2 0.14909m -225~453 -90 0 a2 0.4318m 0 -45o ~225o4 0 -90 a3 0.02032m d4 0.43307m -110o ~170o5 0 90 0 0 -100 o ~100o6 0 -90 0 d6 0.05625m -266 o ~266o③推导正运动学、逆运动学a、正运动学推导c s0ai i i1由式i1Ti s c c c s d si i1i i1i1i i1s s c s c d ci i1i i1i1i i1可得：0001c s 1100c s2200c s0a3320 T1s c001100101T2001d2s c22002T3s c00330010 000100010001c s0a443c s5500c s66003 T4001d4s c44004T50010s c55005T60010s c6600 000100010001由0012345T T T T T T T，得机械手变换矩阵：6123456n o a px x x x0 T6n o a p y y y yn o a p z z z z0001n c(c c c c s s c)s c s c c c s s s c s x236541641236516541641 n c(c c c s s s s)s c s s c c s c s c c y236541641236516541641n s(c c c s s)c c sz23654642365o c(s c c c c s c)s s s c s c s s c c s x236541641236516541641 o c(s c c s c s s)s s s s s s c c c c c y236541641236516451641o s(s c c s s)c c sz23654642365a c s c c s c c s s sx235412351541a c s c s s c s s s cy235412351541a c c s s cz2352354p a c c a c c d s c d s x3231221423121 p a c s a c s d s s d c y3231221423121p d c a s a sz42332322b、逆运动学推导1)．求1用逆变换01T左乘方程10012345T T T T T T T两边：612345601012345T T T T T T T1623456c s00n o a p11x x x xs c00n o a p11y y y y1T60010n o a pz z z z00010001得s p c p d1x1y2三角代换p cos，sinp xy式中，22p p，tan2(,)a p px y x y得到1的解2221a tan2(p y,p x)a tan2(d2,p x p y d2)2)．求3矩阵方程两端的元素（1，4）和（2，4）分别对应相等c p s p a c a cd s1x1y32322423p d c a s a sz42332322平方和为：d s a c k4333其中k2222222 p p p d d a a x y z24232a2解得：222 3a tan2(a3,d4)a tan2(k,d4a3k)3)．求2在矩阵方程0012345T T T T T T T两边左乘逆变换612345601 T。
3010345T T T T T36456c c sc s a c n o a p1231232323x x x xc s s s c a s n o a p1231232323y y y y3T6s c d n o a p1102z z z z00010001方程两边的元素（1，4）和（3，4）分别对应相等，得c c p s c p s p a a c123x123y23z323c s p s s p c p a s d123x123y23z234联立，得s和c2323a s d c p s p p a c a2341x1y z233s2322p c p s px1y1za c a c p s p p a s d2331x1y z234c2322p c p s px1y1zs和c23表达式的分母相等，且为正，于是232323a tan2a2s3d4c1p x s1p y p z a2c3a3,a2c3a3c1p x s1p y p z a2s3d4根据解1和3的四种可能组合，可以得到相应的四种可能值23，于是可得到2的四种可能解:2233，式中2取与3相对应的值。
4．求4令两边元素（1，3）和（2，3）分别对应相等，则可得c c a sc a s a c s123x123y23z45s a c a s s1x1y45只要s，便可求出4504a tan2s1a x c1a y,c1c23a x s1c23a y s23a z 当s50时，机械手处于奇异形位。
5．求501045T T T T4656c c c s s s c c c s s c c c ad s c a n o a p1 4 23 1 4 1 4 23 1 4 23 4 3 42 2 4 43 x x x xs c c sc s s c c c s s c s a d c s a n o a p4 1 23 1 4 4 1 23 1 4 23 4 3 4 2 2 4 4 3 y y y y 4T6c s s s c s ad n o a p1 23 1 23 23 32 4 z z z z0 0 0 1 0 0 0 1根据矩阵两边元素（1，3）和（2，3）分别对应相等，可得a s c a c c c s s a s c c c s sz 23 4 x 1 4 23 1 4 y 1 4 23 1 4 5a c s a s s a c cx 1 23 y 23 1 z 23 55 a tan 2 a z s23c4 a x c1c4 c23 s1s4 a y s1c4c23 c1s4 , a x c1 s23 a y s23 s1 a z c236．求60 1 0 5T T T5 6 6根据矩阵两边元素（2，1）和（1，1）分别对应相等，可得n c s c s c n s s c c c n s s sx 1 4 23 1 4 y 4 1 23 1 4 z 23 4 6n 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 x 1 4 5 23 1 5 4 1 5 23 y 4 1 5 23 1 5 23 4 1 5 z 5 23 4 5 23 6 从而求得：6atan2 s6,c6④编程得出工作空间a、已知条件可以将连杆 6 的原点做为机器人的动点，连杆 6 原点相对于坐标系0 就是0T 的p x 、p y 、p z ，已知：6p a c c a c c d s c d sx 3 23 1 2 2 1 4 23 1 2 1p a c s a c s d s s d cy 3 23 1 2 2 1 4 23 1 2 1p d c a s a sz 4 23 3 23 2 2b、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*a****** for k=1:length(a3)px(k)=cos(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*sin(a2+a3(k)))-d2*sin(a1)py(k)=sin(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*sin(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、机器人工作空间。