哈工大PUMA机器人大作业.docx
《哈工大PUMA机器人大作业.docx》由会员分享，可在线阅读，更多相关《哈工大PUMA机器人大作业.docx（15页珍藏版）》请在冰豆网上搜索。
哈工大PUMA机器人大作业
2017年秋季学期研究生课程考核
（读书报告、研究报告）
考
核科
目:
机器人技术
学
生所在
院：
机电学院
（
系
）
学生所在学科：
机械电子
学
生姓
名：
王
学
号：
17S
学
生类
别：
学硕
考
察结
果：
阅
卷
人：
PUMA机器人正逆运动学推导及运动空间解算
图1PUMA机器人模型
任务：
1.建立坐标系；
2.给出D-H参数表；
3.推导正、逆运动学；
4.编程得工作空间。
建立坐标系
根据PUMA机器人运动自由度,在各关节处建立坐标系如图2所示
图2PUMA机器人坐标系建立图
其中'Oo与7Oi原点交于一点,与7O5原点交于一点
D-H参数表
D-H参数表可根据坐标系设定而得出，见表1
1）诂为绕Zy轴，从Xi」到Xi的角度；
2）ai为绕Xi轴，从Zi」到乙的角度；
3）L为沿Xi轴，从Zid与Xi交点到Oi的距离；
4）①为沿Zij轴，从Zj二与Xj到0債的距离。
杆件i
h
«i
di
变量q（当前值）
变量范围
1
0
0
（90）
-160u160
2
a2
d2
日2（05
-225LI45
3
as
-90
0
日s（-90’）
-225LI45
4
0
d4
04（180）
7dLI350
5
0
90
0
05（0）
-10O100
6
0
03
de
06（0）
-266LJ266
表1PUMA机器人杆件参数表
三、正运动学推导
由坐标系及各杆件参数可得到6个连杆变换矩阵
节变量的函数
%二°Ti弓1T21HH亠4T5二5T二6
将上述变换矩阵逐个依次相乘可以得到°t6。
nxOxaxPx
0丁nyOyayPy
T6=
nzOzazPz
.0001一
等式联立，对比等式左右两侧元素可得：
nx=sin（c6）*（cos（c4）*sin（c1）-cos（c2+c3）*cos（c1）*sin（c4））+cos（c6）*（cos（c5）*（sin（c1）*sin（c4）+cos（c2+c3）*cos（c1）*cos（c4））+sin（c2+c3）*cos（c1）*sin（c5））；
ox=cos（c6）*（cos（c4）*sin（c1）-cos（c2+c3）*cos（c1）*sin（c4））-
sin（c6）*（cos（c5）*（sin（c1）*sin（c4）+cos（c2+c3）*cos（c1）*cos（c4））+
sin（c2+c3）*cos（c1）*sin（c5））;
ax=sin（c5）*（sin（c1）*sin（c4）+cos（c2+c3）*cos（c1）*cos（c4））-
sin（c2+c3）*cos（c1）*cos（c5）;
px=d6*（sin（c5）*（sin（c1）*sin（c4）+cos（c2+c3）*cos（c1）*cos（c4））-
sin（c2+c3）*cos（c1）*cos（c5））-d2*sin（c1）-d4*sin（c2+c3）*cos（c1）+a2*cos（c1）*cos（c2）+a3*cos（c1）*cos（c2）*cos（c3）-a3*cos（c1）*sin（c2）*sin（c3）;
ny=-sin（c6）*（cos（c1）*cos（c4）+cos（c2+c3）*sin（c1）*sin（c4））-cos（c6）*（cos（c5）*（cos（c1）*sin（c4）-cos（c2+c3）*cos（c4）*sin（c1））-sin（c2+c3）*sin（c1）*sin（c5））;
oy=sin（c6）*（cos（c5）*（cos（c1）*sin（c4）-cos（c2+c3）*cos（c4）*sin（c1））-
sin（c2+c3）*sin（c1）*sin（c5））-cos（c6）*（cos（c1）*cos（c4）+
cos（c2+c3）*sin（c1）*sin（c4））；
ay=-sin（c5）*（cos（c1）*sin（c4）-cos（c2+c3）*cos（c4）*sin（c1））-
sin（c2+c3）*cos（c5）*sin（c1）;
py=d2*cos（c1）-d6*（sin（c5）*（cos（c1）*sin（c4）-
cos（c2+c3）*cos（c4）*sin（c1））+sin（c2+c3）*cos（c5）*sin（c1））-
d4*sin（c2+c3）*sin（c1）+a2*cos（c2）*sin（c1）+
a3*cos（c2）*cos（c3）*sin（c1）-a3*sin（c1）*sin（c2）*sin（c3）;
nz=cos（c6）*（cos（c2+c3）*sin（c5）-sin（c2+c3）*cos（c4）*cos（c5））+sin（c2+
c3）*sin（c4）*sin（c6）;
oz=sin（c2+c3）*cos（c6）*sin（c4）-sin（c6）*（cos（c2+c3）*sin（c5）-
sin（c2+c3）*cos（c4）*cos（c5））;
az=-cos（c2+c3）*cos（c5）-sin（c2+c3）*cos（c4）*sin（c5）
pz=-d4*cos（c2+c3）-a3*sin（c2+c3）-d6*（cos（c2+c3）*cos（c5）+
sin（c2+c3）*cos（c4）*sin（c5））-a2*sin（c2）；
上述12个公式中,c1-c6分别对应k丸，将关节变量角度的当前值代入
以上公式得到：
0-10-d2
001a2+d4+d6
-100a3
.0001一
_COS日5
sin日5
0
01
-cos日6
sin日6
0
01
|0
0
1
0
5_—sin日6
cosB6
0
0
sin日5
—cos日5
0
0
6_0
0
1
-d6
.0
0
0
1_
-0
0
0
1_
五、工作空间的逆解运算
从上述的推导中我们已经得到了关节变量kL的推导公式。
根据逆解的
求解思想，在这里我们参照正解所求得的工作间，将机器的运动空间划分成了x=-1000:
n1:
1000,y=-1000:
n2:
1000,z=-1000:
n3:
1000,所组成的空间点集合，
其中n1,n2,n3代表步距，单位为mm。
-
nx
°x
ax
Pxl
ny
oy
ay
Py
nz
Oz
az
Pz
0
0
0
1一
在程序中通过循环，将此空间上的点的坐标代入>6中的Px，Py，Pz
0T6中n,o,a，所对应的9个参数，代表了末端坐标系（执行器）各坐标轴与零坐标系（固定坐标系）各坐标轴之间的夹角余弦值。
在这里为了简便运算，减少运算量，我们讨论固定姿态下机器人的工作空间。
令ox=-1，ay=1，nz=-1，其
余六个参数等于零，即当前PUMA机器人末端执行器相对于固定坐标系的姿态至此，求解关节变量的参数均已给出。
另外考虑到手腕环节（后三个坐标系）对工作空间的影响不大，为了简化
运算，在这里只将逆解求解到哥L屯,而不再讨论卞。
逆解源代码，请参照附录。
逆解求解的结果如图3L6。
六、工作空间的正解运算
机器人正解的末端执行器C06）相对于固定坐标系（^0。
）的转换矩阵0T6已经求出。
取'。
6上的原点06=（0,0,0,1）丁，°16*06就可得到'06上的原点在固定坐标系下的坐标，即工作空间中一个点。
通过将^6在对应的变量范围内变换便可以得到整个工作空间。
在这里为了减少运算量，同逆解，我们不讨论乙L入的变化，只讨论
在各自运动范围内的变化。
正解源代码及改进的高效率代码请参照附录
逆解正解的结果如图7LI10。
800*
图3PUMA机器人逆解空间（轴测图）
图4PUMA机器人逆解空间（XOY）
图5PUMA机器人逆解空间（XOZ）
图6PUMA机器人逆解空间（YOZ）
O
40
oO
2OC
40
■i
10DD
逆加远算.歩劲2
500
1000
-iuuu
图7PUMA机器人正解空间（轴测图）
-1000
dOOO-&00-€0&-400-20002004006M&0010X
图8PUMA机器人正解空间（XOY）
1000
eoo
&D0
4QQ
200
0
-200
-400
-800
-1000-SOO£D0dOO-2000200^000008001000
X
图9PUMA机器人正解空间（XOZ）
图10PUMA机器人正解空间（YOZ）
附录
a）逆解程序
clc;
clear;
a2=431.8;
a3=-20.32;
d2=149.09;
d4=433.07;
d6=56.25;
i=1;
j=1;
t1=[0,0]；
mt1=[0,0];
x=zeros（450000,1）;
y=zeros（450000,1）;
z=zeros（450000,1）;
forpx=-1000:
10:
1000
forpy=-1000:
10:
1000
forpz=-1000:
10:
1000
j=j+1;
m=1;
t=0;
k=pxA2+（py-d6）A2-d2A2;
ifk<0
m=0;
else
s1=atan2（py-d6,px）-atan2（d2,sqrt（k））;
s12=atan2（py-d6,px）-atan2（d2,-sqrt（k））;
end
ifm==1
ifs1<160*pi/180&&s1>-160*pi/180
t1
（1）=s1;
mt1
（1）=1;
end
ifs12<160*pi/180&&s12>-160*pi/180
t1
（2）=s12;
mt1
（2）=1;
end
ifmt1
（1）==0&&mt1
（2）==0
m=0;
end
end
ifm==1
forn=1:
2
ifmt1（n）==1
s1=sin（t1
（1））;
c1=cos（t1
（1））;
k=4*a2A2*（a3A2+d4A2）-（pzA2+（px*c1+py*s1-d6*s1）A2-a3A2-d4A2-a2A2）A2;
k0=pzA2+（px*c1+py*s1-d6*s1）A2-a2A2-a3A2-d4A2;
ifk<0
m=0;
else
s31=-atan2（d4,a3）-atan2（sqrt（k）,k0）;
s32=-atan2（d4,a3）+atan2（sqrt（k）,k0）;
if（s31<225*pi/180&&s31>-45*pi/180）…
||（s32<225*pi/180&&s32>-45*pi/180）
else
m=0;
end
ifm==1
k=a3A2+
升级会员 