机器人学作业.docx
《机器人学作业.docx》由会员分享，可在线阅读，更多相关《机器人学作业.docx（11页珍藏版）》请在冰点文库上搜索。
机器人学作业
MOTOMAN'sHP6robot位姿矩阵的计算
姿态矩阵的正解：
一、机构简图及坐标建立
二、两步法求变换矩阵
对六个坐标系应用公式：
t1_0=
[cos（thet1）,-sin（thet1）,0,0]
[sin（thet1）,cos（thet1）,0,0]
[0,0,1,0]
[0,0,0,1]
t2_1=
[sin（thet2）,cos（thet2）,0,150]
[0,0,1,0]
[cos（thet2）,-sin（thet2）,0,0]
[0,0,0,1]
t3_2=
[cos（thet3）,-sin（thet3）,0,570]
[sin（thet3）,cos（thet3）,0,0]
[0,0,1,0]
[0,0,0,1]
t4_3=
[cos（thet4）,-sin（thet4）,0,155]
[0,0,1,640]
[-sin（thet4）,-cos（thet4）,0,0]
[0,0,0,1]
t5_4=
[cos（thet5）,-sin（thet5）,0,0]
[0,0,-1,0]
[sin（thet5）,cos（thet5）,0,0]
[0,0,0,1]
t6_5=
[cos（thet6）,-sin（thet6）,0,0]
[0,0,1,0]
[-sin（thet6）,-cos（thet6）,0,0]
[0,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）*cos（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计算程序
%求末端位姿变换矩阵
%其中thet1thet2thet3thet4thet5thet6为各关节转角
%故采用符号计算Rotz矩阵，再使用subs替换其中的符号变量.
%达到了既可代入符号，又可代入数值的目的
%研究生课程,工业机器人
%北京科技大学机械工程学院
clc;
symsthet
symsthet1thet2thet3thet4thet5thet6;
thet1=0;thet2=0.5*pi;thet3=0.5*pi;thet4=0;thet5=0;thet6=0;
rotz=[cos（thet）-sin（thet）00;sin（thet）cos（thet）00;%计算旋转矩阵
0010;0001];
T100=eye（4,4）;%未简化155mm的拐角
T210=[010150;0010;1000;0001];
T320=[100570;0100;0010;0001];
T430=[100155;001640;0-100;0001];
T540=[1000;00-10;0100;0001];
T650=[1000;0010;0-100;0001];
Tg0=[100155;0100;0010;0001];
T10=subs（T100*rotz,thet,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*T65
disp（['末端x坐标为:
'num2str（T60（1,4））]）
disp（['末端y坐标为:
'num2str（T60（2,4））]）
disp（['末端z坐标为:
'num2str（T60（3,4））]）
p1=T10*T21%保存每个杆末端坐标
p2=T10*T21*T32
pg=T10*T21*T32*Tg
p3=T10*T21*T32*T43%以下分别绘制各杆
x=[0p1（1,4）];y=[0p1（2,4）];z=[0p1（3,4）];plot3（x,y,z,'r','LineWidth',8）;gridon;holdon;
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）];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=
1000
0100
0010
0001
T21=
1.00000.00000150.0000
001.00000
0.0000-1.000000
0001.0000
T32=
0.0000-1.00000570.0000
1.00000.000000
001.00000
0001.0000
T43=
100155
001640
0-100
0001
T54=
1000
00-10
0100
0001
T65=
1000
0010
0-100
0001
T60=
0.00000-1.000080.0000
0-1.000000
-1.00000-0.0000-155.0000
0001.0000
末端x坐标为:
80
末端y坐标为:
0
末端z坐标为:
-155
p1=
1.00000.00000150.0000
001.00000
0.0000-1.000000
0001.0000
p2=
0.0000-1.00000720.0000
001.00000
-1.0000-0.000000.0000
0001.0000
pg=
-1.0000-0.00000720.0000
001.00000
-0.00001.00000-155.0000
0001.0000
p3=
0.00000-1.000080.0000
0-1.000000
-1.00000-0.0000-155.0000
0001.0000
四、典型机构位置
a）thet1=0;thet2=0;thet3=0;thet4=0;thet5=0;thet6=0;
末端x坐标为:
790末端y坐标为:
0末端z坐标为:
725
b）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*t4_3*t5_4*t6_5
提取其中的[1,4][3,4]元素，可得等式：
将以上两式平方相加，又由
，可得：
由于其中thet1为已知，上式可简化为
的形式，可应用solve求解
c=pz^2+（sin（thet1）*py+cos（thet1）*px-150）^2-758525
thet3=atan2（-32/24716625*c+31/98866500*（-c^2+563539050000）^（1/2）,31/98866500*c+32/24716625*（-c^2+563539050000）^（1/2））
c）关节2转角的推导
至此计算（b）方程组中thet3已求出，原方程组可化简为：
由于页面宽度有限，其中sin
（1）代表sin（thet1）等等
方程组中只有sin
（2）及cos
（2）两个未知数，可得到sin
（2）准确解，再由反正弦求解thet2。
至此，决定机器人末端P点位置的三个转角变量thet1、thet2，thet3均已求出，具体计算参见下节逆解程序。
二、位姿逆解matlab程序
%两步法变换矩阵的逆解
%需要分别带入pz,py,pz
%程序中的a、b、c为临时计算变量
%研究生课程,工业机器人
%北京科技大学机械工程学院
clc;clearall;
px=80;py=0;pz=-155;%输入末端坐标
thet1=atan2（py,px）
c=pz^2+（sin（thet1）*py+cos（thet1）*px-150）^2-758525;%thet3利用坐标元素相等，平方和相加得到
thet3=atan2（-32/24716625*c+31/98866500*（-c^2+563539050000）^（1/2）,31/98866500*c+32/24716625*（-c^2+563539050000）^（1/2））
a=sin（thet1）*py+cos（thet1）*px-150;%开始利用方程组求解thet2
b=pz;C=[ab]';
A=[155*cos（thet3）-640*sin（thet3）+570155*sin（thet3）+640*cos（thet3）;
-155*sin（thet3）-640*cos（thet3）155*cos（thet3）-640*sin（thet3）+570];
B=A\C;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=0
thet3=1.5708
thet2=-1.5708+37.8070i
弧度：
thet1=0thet2=-1.5708+37.807ithet3=1.5708
角度：
thet1=0thet2=-90+2166.1789ithet3=90
>>
逆解中的thet1、thet2、thet3与正解中的Px、Py、Pz对应，程序验证通过。
升级会员 