机器人学作业.docx

1、机器人学作业MOTOMANs HP6 robot位姿矩阵的计算姿态矩阵的正解：一、 机构简图及坐标建立二、 两步法求变换矩阵对六个坐标系应用公式： t1_0 = cos(thet1), -sin(thet1), 0, 0 sin(thet1), cos(thet1), 0, 0 0, 0, 1, 0 0, 0, 0, 1t2_1 = sin(thet2), cos(thet2), 0, 150 0, 0, 1, 0 cos(thet2), -sin(thet2), 0, 0 0, 0, 0, 1t3_2 = cos(thet3), -sin(thet3), 0, 570 sin(thet3),

2、 cos(thet3), 0, 0 0, 0, 1, 0 0, 0, 0, 1t4_3 = cos(thet4), -sin(thet4), 0, 155 0, 0, 1, 640 -sin(thet4), -cos(thet4), 0, 0 0, 0, 0, 1t5_4 = cos(thet5), -sin(thet5), 0, 0 0, 0, -1, 0 sin(thet5), cos(thet5), 0, 0 0, 0, 0, 1t6_5 = cos(thet6), -sin(thet6), 0, 0 0, 0, 1, 0 -sin(thet6), -cos(thet6), 0, 0 0

3、, 0, 0, 1将以上六个变换矩阵相乘，即可得t6_0=t1_0*t2_1*t3_2*t4_3*t5_4*t6_5其中第四列元素:Px，Py，Pz分别为：Px=155*cos(thet1)*sin(thet2)*cos(thet3)+155*cos(thet1)*cos(thet2)*sin(thet3)-640*cos(thet1)*sin(thet2)*sin(thet3)+640*cos(thet1)*cos(thet2)*cos(thet3)+570*cos(thet1)*sin(thet2)+150*cos(thet1)Py=155*sin(thet1)*sin(thet2)*co

4、s(thet3)+155*sin(thet1)*cos(thet2)*sin(thet3)-640*sin(thet1)*sin(thet2)*sin(thet3)+640*sin(thet1)*cos(thet2)*cos(thet3)+570*sin(thet1)*sin(thet2)+150*sin(thet1)Pz=155*cos(thet2)*cos(thet3)-155*sin(thet2)*sin(thet3)-640*cos(thet2)*sin(thet3)-640*sin(thet2)*cos(thet3 )+570*cos(thet2)三、 matlab计算程序%求末端位

5、姿变换矩阵%其中thet1 thet2 thet3 thet4 thet5 thet6为各关节转角%故采用符号计算Rotz矩阵，再使用subs替换其中的符号变量.%达到了既可代入符号，又可代入数值的目的%研究生课程,工业机器人%北京科技大学机械工程学院clc;syms thet syms thet1 thet2 thet3 thet4 thet5 thet6;thet1=0;thet2=0.5*pi;thet3=0.5*pi;thet4=0;thet5=0;thet6=0;rotz=cos(thet) -sin(thet) 0 0;sin(thet) cos(thet) 0 0; %计算旋转矩

6、阵 0 0 1 0;0 0 0 1;T100=eye(4,4); %未简化155mm的拐角T210=0 1 0 150;0 0 1 0;1 0 0 0;0 0 0 1;T320=1 0 0 570;0 1 0 0;0 0 1 0;0 0 0 1;T430=1 0 0 155;0 0 1 640;0 -1 0 0;0 0 0 1;T540=1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1;T650=1 0 0 0;0 0 1 0;0 -1 0 0;0 0 0 1;Tg0=1 0 0 155;0 1 0 0;0 0 1 0;0 0 0 1;T10=subs(T100*rotz,th

7、et,thet1)T21=subs(T210*rotz,thet,thet2)T32=subs(T320*rotz,thet,thet3)Tg=subs(Tg0*rotz,thet,thet3); %添加额外坐标系，用于计算155拐点坐标T43=subs(T430*rotz,thet,thet4)T54=subs(T540*rotz,thet,thet5)T65=subs(T650*rotz,thet,thet6)T60=T10*T21*T32*T43*T54*T65disp(末端x坐标为: num2str(T60(1,4)disp(末端y坐标为: num2str(T60(2,4)disp(末

8、端z坐标为: num2str(T60(3,4) p1=T10*T21 %保存每个杆末端坐标p2=T10*T21*T32pg=T10*T21*T32*Tgp3=T10*T21*T32*T43 %以下分别绘制各杆x=0 p1(1,4);y=0 p1(2,4);z=0 p1(3,4);plot3(x,y,z,r,LineWidth,8);grid on;hold on;x=p1(1,4) p2(1,4);y=p1(2,4) p2(2,4);z=p1(3,4) p2(3,4);plot3(x,y,z,b,LineWidth,4);x=p2(1,4) pg(1,4);y=p2(2,4) pg(2,4);

9、z=p2(3,4) pg(3,4);plot3(x,y,z,k,LineWidth,2);x=pg(1,4) p3(1,4);y=pg(2,4) p3(2,4);z=pg(3,4) p3(3,4);plot3(x,y,z,k,LineWidth,2);plot3(0,0,0,d);运算结果：T10 = 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1T21 = 1.0000 0.0000 0 150.0000 0 0 1.0000 0 0.0000 -1.0000 0 0 0 0 0 1.0000T32 = 0.0000 -1.0000 0 570.0000 1.0000 0.0

10、000 0 0 0 0 1.0000 0 0 0 0 1.0000T43 = 1 0 0 155 0 0 1 640 0 -1 0 0 0 0 0 1T54 = 1 0 0 0 0 0 -1 0 0 1 0 0 0 0 0 1T65 = 1 0 0 0 0 0 1 0 0 -1 0 0 0 0 0 1T60 = 0.0000 0 -1.0000 80.0000 0 -1.0000 0 0 -1.0000 0 -0.0000 -155.0000 0 0 0 1.0000末端x坐标为:80末端y坐标为:0末端z坐标为:-155p1 = 1.0000 0.0000 0 150.0000 0 0 1.

11、0000 0 0.0000 -1.0000 0 0 0 0 0 1.0000p2 = 0.0000 -1.0000 0 720.0000 0 0 1.0000 0 -1.0000 -0.0000 0 0.0000 0 0 0 1.0000pg = -1.0000 -0.0000 0 720.0000 0 0 1.0000 0 -0.0000 1.0000 0 -155.0000 0 0 0 1.0000p3 = 0.0000 0 -1.0000 80.0000 0 -1.0000 0 0 -1.0000 0 -0.0000 -155.0000 0 0 0 1.0000四、 典型机构位置a) t

12、het1=0;thet2=0;thet3=0;thet4=0;thet5=0;thet6=0;末端x坐标为:790 末端y坐标为:0 末端z坐标为:725b) thet1=pi*0.25;thet2=0;thet3=0.25*pi;thet4=0;thet5=0;thet6=0;末端x坐标为:503.566 末端y坐标为:503.566 末端z坐标为:227.0532姿态矩阵逆解一、 前三关节转角的推导a) 关节1转角的推导因为HP6机器人所有关节均处于同一截面，由几何关系： thet1=atan2(py,px)b) 关节3转角的推导由先前建立的六个位姿矩阵，使其t6_1=t2_1*t3_2*

13、t4_3*t5_4*t6_5提取其中的1,4 3,4元素，可得等式： 将以上两式平方相加，又由，可得：由于其中thet1为已知，上式可简化为的形式，可应用solve求解c=pz2+(sin(thet1)*py+cos(thet1)*px-150)2-758525thet3=atan2(-32/24716625*c+31/98866500*(-c2+563539050000)(1/2),31/98866500*c+32/24716625*(-c2+563539050000)(1/2)c) 关节2转角的推导至此计算（b）方程组中thet3已求出，原方程组可化简为：由于页面宽度有限，其中sin(1)

14、代表sin(thet1)等等 方程组中只有sin(2)及cos(2)两个未知数，可得到sin(2)准确解，再由反正弦求解thet2。 至此，决定机器人末端P点位置的三个转角变量thet1、thet2，thet3均已求出，具体计算参见下节逆解程序。二、 位姿逆解matlab程序%两步法变换矩阵的逆解%需要分别带入pz,py,pz%程序中的a、b、c为临时计算变量%研究生课程,工业机器人%北京科技大学机械工程学院clc;clear all;px=80;py=0;pz= -155;%输入末端坐标thet1=atan2(py,px) c=pz2+(sin(thet1)*py+cos(thet1)*px

15、-150)2-758525; %thet3利用坐标元素相等，平方和相加得到thet3=atan2(-32/24716625*c+31/98866500*(-c2+563539050000)(1/2),31/98866500*c+32/24716625*(-c2+563539050000)(1/2)a=sin(thet1)*py+cos(thet1)*px-150; %开始利用方程组求解thet2b=pz; C=a b;A=155*cos(thet3)-640*sin(thet3)+570 155*sin(thet3)+640*cos(thet3); -155*sin(thet3)-640*co

16、s(thet3) 155*cos(thet3)-640*sin(thet3)+570;B=AC; thet2=asin(B(1)/B(2) %由thet2的正切值反求弧度disp(弧度：thet1=,num2str(thet1) thet2=,num2str(thet2) thet3=,num2str(thet3)disp(角度：thet1=,num2str(thet1*180/pi) thet2=,num2str(thet2*180/pi) thet3=,num2str(thet3*180/pi)将前面正解计算出的坐标带回验证，得计算结果： thet1 = 0thet3 =1.5708thet2 =-1.5708 +37.8070i弧度：thet1=0 thet2=-1.5708+37.807i thet3=1.5708角度：thet1=0 thet2=-90+2166.1789i thet3=90逆解中的thet1、thet2、thet3与正解中的Px、Py、Pz对应，程序验证通过。