PUMA560 机器人运动学论文
姓名:
学
号:20080084005
8
专业及班级:机械二班
山东大学威海分校2012年上学期工业机器人学作业论文
目录
摘要 (2)
关键词 (2)
1.引言 (2)
2．PUMA560 机器人数学模型的建立 (2)
2.1.确定 D-H 坐标系幵获取 D-参数 (2)
2.2 建立运动学方程 (3)
3. 位姿的正﹑逆解 (3)
3.1 正解 (3)
3.2 逆解 (3)
3.2.1 求θ1，θ2，θ3 (3)
3.2.2 求θ4，θ5，θ6 (4)
4. PUMA560 雅克比矩阵 (5)
4.1 矢量积法 (6)
4.2 微分变换法 (6)
5． Matlab 编程对其正﹑逆解和雅克比矩阵的求解 (7)
5.1 正解﹑逆解 (7)
5.2 雅克比矩阵 (7)
6. 总结 (7)
参考文献 (7)
附件： (8)
正解程序 (8)
逆解程序 (8)
雅克比矩阵的矢量法程序 (10)
雅克比矩阵的微分变换法程序 (11)
第1页6/1/2012作者：李安洲
摘要：采用D-H坐标系对机器人Puma560建立个关节的坐标系幵获取D-H参数，幵对其运动建立数学模型用matlab编程对其求位姿正逆解及雅克比矩阵，catia对Puma560建模三维模型。
关键词：Puma560正逆解；雅克比矩阵；Matlab
1. 引言
机器人运动学包括正向运动学，即给定机器人各关节变量，计算机器人末端的位置姿态；逆向运动学即已知机器人末端的位置姿态，计算机器人对应位姿的全部关节变量。一般正向运动学的解是唯一和容易获得的，而逆向运动学往往有多个解而且分析更为复杂。机器人逆运动分析是运动规划不控制中的重要问题，但由于机器人逆运动问题的复杂性和多样性，无法建立通用的解析算法。机构逆运动学问题实际上是一个非线性超越方程组的求解问题，其中包括解的存在性、唯一性及求解的方法等一系复杂问题。本文主要通过最基本分析方法对Puma560 机器人的运动进行分析，包括运动的位置和姿态的正、逆解，及其运动的雅克比矩阵的求解。
2．PUMA560 机器人数学模型的建立
PUMA 机器人操作臂可以看作一个开式运动链，它由一系列连杆通过转动关节串联而成，关节的相对转动导致连杆的运动。为了研究机器人各连杆的运动，建立如图1所示坐标系根据Denavil和Hartenberg提出的齐次变换矩阵法，建立机器人的运动学方程。
2.1.确定 D-H 坐标系并获取 D-参数
PUMA560 的 6 个关节全为转动关节:
Z i坐标轴:沿着i+1关节的运动轴;
X i坐标轴:沿着Z i和 Z i-1的公法线,指向离开 Z i-1轴的
方向; Y i坐标轴:按右手直角坐标系法则制定;
连杆长度a i;Z i和Z i-1两轴心线的公法线长度;
连杆扭角a i:Z i和Z i-1两轴心线的夹角;两连杆
距离d i:X i和X i-1两坐标轴的公法线距离;两
杆夹角 i:X i和X i-1两坐标轴的夹角;
图 1
第2页6/1/2012作者：李安洲
D-H 参数表
连杆 i 变量θi
αi -1
a
i -1
d i 变量范围 1 θ1 (90 ?) 90? 0 0 -160?~160? 2 θ2 (0?) 0? 0 149.09 -225? ~ 45? 3 θ3 (-90?) -90? 431.8 0 -45?~225? 4 θ4 (0?) 0? 20.32 433.07 -110?~170? 5 θ5 (0?) 0? 0 0 -100?~100?
6
θ6 (0?)
0?
-226?~226?
2.2 建立运动学方程
PUMA560 机器人具有六个自由度，而且六个关节均为旋转关节，前三个关节主要影响 末端执行器的位置，后三个关节决定末端执行器的姿态，将机器人位置结构和姿态结构末端 执行器的位置矢量和姿态转换矩阵幵通过齐次变换得到：
两杆间的位姿矩阵
i -1
T = Rot ( x , α i -1 )Trans ( a -1 , 0, 0) Rot ( z , θ )Trans (0, 0, d )
i
i
i i
? c θi
-s θi 0 a i -1
?
?
s θ c α
i -1
c θ c α
i -1 -s α
i -1
-s α d ?
= ?
i
i
c α
i -1
i ?
( )
2.1
? i
i -1 i i -1
i -1
i -1 i
?
1
?
?
其中： s θi = sin θi ,c θi = cos θi ,s αi -1 = sin αi -1 , c αi -1 = cos αi -1 末端执行器位姿矩阵即 PUMA560 机器人的运动学方程：
T i = 0T 1 (θ1 ) 1T 2 (θ2 ) 2T 3 (θ3 )...... i -1
T i (θi ) (2.2)
3. 位姿的正﹑逆解
3.1 正 解
已知各关节的变量θi ，求末端执行器的位姿矩阵即正解。把θi 带入（2.1）求得各两连 杆间的位姿矩阵 0
T 1 ,1
T 2 , 2
T 3 , 3
T 4 , 4
T 5 ,5
T 6 ，再由（2.2）即可求得末端执行器的位姿矩阵：
= 0 1 2 3 4 5
(θ6 ) ( 3.1
T 6 T 1 (θ1 ) T 2 (θ2 ) T 3 (θ3 ) T 4 (θ4 ) T 5 (θ5 ) T 6 )
第3页6/1/2012作者：李安洲
山东大学威海分校 2012 年上学期工业机器人学作业论文
3.2 逆 解
将 PUMA 560 的运动方程（3.1）写为:
?n x o x a
x
?n
y o y a y ? T 6 = ?n z
o
z
a
z
?
? 0
0 0
p x ?
?
(3.2)
p
y ?
= 0
1
2
3
4
5
(θ6 ) p
z
? T 1 (θ1 ) T 2 (θ2 ) T 3 (θ3 ) T 4 (θ4 ) T 5 (θ5 ) T 6
1 ?
?
若末端连杆的位姿已经给定,即 n,o,a 和 p 为已知,则求关节变量 θ1 , θ 2 , , θ6 的值称为运动 反解.
3.2.1 求 θ1 ，θ2 ，θ3
T 1-1 (θ1 ) 0T 6 = 1T 2 (θ2 ) 2T 3 (θ3 ) 3T 4 (θ4 ) 4T 5 (θ5 ) 5
T 6 (θ6 )
? c 1
s
1
0 0? ?n x
o
x
a x p x ?
?
-s
c 0
? ?
n o a
p ? ( )
?
1
1
? ? y
y y y
? 1 = 3.3
? 0 0
1 0? ?n z
o
z
a z p z ? T 6
?
? ?
?
? 0 0 0 1 ? ? 0 0 0 1 ?
?
sin(? -θ ) = d / ρ; cos(? -θ ) = ± 1 - ( d / ρ)2 1 2
1
2 ?
? ?
?
d ? d ? 2
? 2 2 ? ?
? -θ1 = atan2
, ± 1-
?
?
? ρ ? ρ ? ? ?
?
? ? θ1 = atan2( p y , p x ) - atan2( d 2 , ± p x 2 + p y 2
- d 22
?
?
式中,正、负号对应于θ1 的两个可能解. 再令矩阵方程(3.3)两端的元素(1,4)和(3,4)分别对应相等,则得两方程:
c p x + s p y = a c -
d s 23 + a c ?
1
1
3 23
4 2 2 ?
- p = a s + d c + a s
?
z 23 23 2
?
3 4 2
?
式(3.68)不式(3.71)的平方和为:
a 3 c 3 - d 4 s 3 = k
θ3 = atan2(a 3 , d 4 ) - atan2(k ,
±a 32 + d 42 - k 2
)
式中,正、负号对应于θ3 的两个可能解.
T 3-1 (θ1 , θ2 ,θ3 ) 0T 6 = 3T 4 (θ4 ) 4T 5 (θ5 ) 5
T 6 (θ6 ) ? c 1c 23
s 1c
23
-s
23
-a 2 c 3 ? ?n x o
x a x p x ?
?
-c s
-s s
-c a s ? ?n o a p ? = T 6 ?1 23
1 23
23
2 3 ? ?
y
y y y ?
3
? -s 1
c 1
0 -d 2 ? ?n z o z a z p z ?
?
? ?
?
? 0 0 0
1
? ? 0 0 0
1 ?
(3.4)
(3.5)
(3.6)
(3.7)
(3.8)
第4页6/1/2012作者：李安洲