机器人技术大作业

1、大作业：PUMA机器人1. 坐标系建立利用D-H参数法建立坐标系：2D-H参数表()()190-9000-160°160°200a2d2-225°45°3 9090-a30-45°225°40-900-110°170°509000-100°100°6000-266°266°3. 正运动学推导3.1变换矩阵求取列各D-H变换矩阵如下： 注：为书写方面，本文中又由，利用Matlab进行符号运算，运行程序PUMAzhengyundongxue（程序详见附录）得：其中3.2变换矩阵验

2、证3.2.1初步验证根据DH参数表中i的初始值，将其带入后得到末端执行器的坐标为px= -d2py=a2+d4+d6pz=a3与初始设定一致，因此初步判断变换矩阵正确。使用matlab机器人工具箱详细验证()()(m)(m)190-9000-160°160°200a2=0.4318d2=0.14909-225°45°3 9090-a3=-0.020320-45°225°40-900d4=0.43307-110°170°509000-100°100°6000d6=0.05625-266°

3、266°利用matlab机器人工具箱按照以上DH参数，编写并运行程序PUMAyanzheng（程序详见附录）得到如下图，与PUMA560机器人一致，表明DH参数法建模正确。 4. 逆运动学推导反变换法求逆，且有（1）求由 得 由矩阵对应元素均相等，得 得到：整理有三角变换其中得到（2）求由得其中B3个元素如下：注：为书写方便，本文中。由矩阵元素对应相等， 得 两式平方求和，整理得到：其中，进行三角变换其中从而得到由，得解得（3）求（4）求由得解得（5）求由，得时，解得（6）求由，得时，解得5. 工作空间求取末端操作手的坐标为，根据正运动学推导结果，用Matlab编程求取其工作空间，根

4、据参考文献获得PUMA560机器人各关节长度为：编写并运行程序PUMAgongzuokongjian（程序详见附录），得到：变换角度:俯视：综上，PUMA560运动的逆解可能存在8种。但是，由于结构的限制，例如各关节变量不能在全部360°范围内运动，有着特定的范围，有些解不能实现。这种多解情况下，应选取其中最满意的一组解，以满足机器人的工作要求。附录：（1） PUMAzhengyundongxue程序clearsyms a2 a3 d2 d4 d6 c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6;A1=c1 0 -s1 0;s1 0 c1 0;0 -1 0 0

5、;0 0 0 1;A2=c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 d2;0 0 0 1;A3=c3 0 s3 -a3*c3;s3 0 -c3 -a3*s3;0 1 0 0;0 0 0 1;A4=c4 0 -s4 0;s4 0 c4 0;0 -1 0 d4;0 0 0 1;A5=c5 0 s5 0;s5 0 -c5 0;0 1 0 0;0 0 0 1;A6=c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1;T60=A1*A2*A3*A4*A5*A6T60=subs(T60,c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6

6、,0 1 0 1 1 1 1 0 1 0 0 0)（2） PUMAyanzheng程序clearL1=Link(-pi/2 0 0 0 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,'standard')L5=Link(pi/2 0 0 0 0,'standard')L6=Link(0 0 0 0.05625 0,&#

7、39;standard')r=robot(L1 L2 L3 L4 L5 L6 )drivebot(r)（3） PUMAgongzuokongjian程序cleara2=431.8;a3=20.32;d2=149.09;d4=433.07;d6=56.25;c1=-0.5*pi;c3=0.5*pi;c4=-0.5*pi;c5=0.5*pi;for b1=-160.*pi/180:32.*pi/180:160.*pi/180for b2=-225.*pi/180:54.*pi/180:45.*pi/180for b3=-45.*pi/180:54.*pi/180:225.*pi/180fo

8、r b4=-110.*pi/180:140.*pi/180:170.*pi/180for b5=-100.*pi/180:100.*pi/180:100.*pi/180 x=d4*(cos(b3)*sin(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1) +. sin(b3)*sin(c3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) + cos(c3)*sin(b1)*sin(c1) + . d6*(cos(c5)*(cos(c4)*(cos(b3)*sin(c3)*(cos(b1)*sin(b2) + c

9、os(b2)*cos(c1)*sin(b1) + sin(b3)*sin(c3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) + . cos(c3)*sin(b1)*sin(c1) + cos(b4)*sin(c4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - . cos(c1)*sin(b1)*sin(b2) - sin(b1)*sin(c1)*sin(c3) + sin(b4)*sin(c4)*(co

10、s(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) - sin(b3)*(cos(b1)*sin(b2) + . cos(b2)*cos(c1)*sin(b1) + sin(b5)*sin(c5)*(cos(b4)*(cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) - sin(b3)*(cos(b1)*sin(b2) + . cos(b2)*cos(c1)*sin(b1) - sin(b4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*

11、sin(b1) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) - . sin(b1)*sin(c1)*sin(c3) + cos(b5)*sin(c5)*(cos(b4)*cos(c4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - . cos(c1)*sin(b1)*sin(b2) - sin(b1)*sin(c1)*sin(c3) - sin(c4)*(cos(b3)*

12、sin(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1) + sin(b3)*sin(c3)*(cos(b1)*cos(b2) - . cos(c1)*sin(b1)*sin(b2) + cos(c3)*sin(b1)*sin(c1) + cos(c4)*sin(b4)*(cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) - sin(b3)*(cos(b1)*sin(b2) + . cos(b2)*cos(c1)*sin(b1) + a2*cos(b1)*cos(b2) + d2*sin(b1)*s

13、in(c1) - a3*cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2) + a3*sin(b3)*(cos(b1)*sin(b2) + . cos(b2)*cos(c1)*sin(b1) - a2*cos(c1)*sin(b1)*sin(b2) y=d4*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + sin(b3)*sin(c3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) - cos(b1)*cos(c3)*sin(c1

14、) + . d6*(cos(c5)*(cos(c4)*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + sin(b3)*sin(c3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) - . cos(b1)*cos(c3)*sin(c1) + cos(b4)*sin(c4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + cos(c3)*sin(b3)*(cos(b2)*sin(b1) + . cos(b1)

15、*cos(c1)*sin(b2) + cos(b1)*sin(c1)*sin(c3) + sin(b4)*sin(c4)*(cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) - sin(b3)*(sin(b1)*sin(b2) - . cos(b1)*cos(b2)*cos(c1) + cos(b5)*sin(c5)*(cos(b4)*cos(c4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + cos(c3)*sin(b3)*(cos(b2)*sin(b1) +

16、. cos(b1)*cos(c1)*sin(b2) + cos(b1)*sin(c1)*sin(c3) - sin(c4)*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + sin(b3)*sin(c3)*(cos(b2)*sin(b1) + . cos(b1)*cos(c1)*sin(b2) - cos(b1)*cos(c3)*sin(c1) + cos(c4)*sin(b4)*(cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) - sin(b3)*(sin(b1)*

17、sin(b2) - . cos(b1)*cos(b2)*cos(c1) + sin(b5)*sin(c5)*(cos(b4)*(cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) - sin(b3)*(sin(b1)*sin(b2) - . cos(b1)*cos(b2)*cos(c1) - sin(b4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1) + cos(c3)*sin(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)

18、 + . cos(b1)*sin(c1)*sin(c3) + a2*cos(b2)*sin(b1) - d2*cos(b1)*sin(c1) - a3*cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2) + a3*sin(b3)*(sin(b1)*sin(b2) - . cos(b1)*cos(b2)*cos(c1) + a2*cos(b1)*cos(c1)*sin(b2) z=d4*(cos(c1)*cos(c3) - cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(

19、c3) + d2*cos(c1) + d6*(cos(c5)*(cos(c4)*(cos(c1)*cos(c3) - . cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(c3) + sin(b4)*sin(c4)*(cos(b2)*sin(b3)*sin(c1) + cos(b3)*sin(b2)*sin(c1) - . cos(b4)*sin(c4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) - cos(c3)*sin(b2)*sin(b3)*sin(c1)

20、 + sin(b5)*sin(c5)*(cos(b4)*(cos(b2)*sin(b3)*sin(c1) + . cos(b3)*sin(b2)*sin(c1) + sin(b4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) - cos(c3)*sin(b2)*sin(b3)*sin(c1) - cos(b5)*sin(c5)*(sin(c4)*(cos(c1)*cos(c3) - . cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(c3) + cos(b4)*

21、cos(c4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) - cos(c3)*sin(b2)*sin(b3)*sin(c1) - . cos(c4)*sin(b4)*(cos(b2)*sin(b3)*sin(c1) + cos(b3)*sin(b2)*sin(c1) + a2*sin(b2)*sin(c1) - a3*cos(b2)*sin(b3)*sin(c1) - a3*cos(b3)*sin(b2)*sin(c1)plot3(x,y,z,'*')hold ongrid ontitle('PUMA»

22、;úÆ÷È˵Ť×÷¿Õ¼ä')xlabel('x');ylabel('y');zlabel('z');endendendendend（4） PUMA560niyundongxueclearsyms theta1 theta2 theta3 theta4 theta5 theta6 c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6 a2 a3 d2 d4 d6 nx ny nz sx sy sz ax ay az px py pz;c1=cos(theta1);c2=cos(theta2);c3=cos(theta3);c4=cos(theta4);c5=cos(theta5);c6=cos(theta6);s1=sin(theta1);s2=sin(theta2);s3=sin(theta3);s4=sin(theta4);s5=sin(theta5);s6=sin(theta6);A1=c1 0 -s1 0;s1