PUMA560机器人运动学方程 PUMA560机器人运动学方程
连杆变换矩阵
c1 −s1 s c 1 0 T = 1 1 0 0 0 0
c4 −s4 0 0 3 T = 4 −s4 −c4 0 0
0 0 0 0 1 0 0 1
0 a3 1 d4 0 0 0 1
cα −sα 0 0 cβ sα cα 0 0 0 = 0 0 1 0−sβ 0 0 1 0 0 cαcβ cαsβsγ − sαcγ sαcβ sαsβsγ + cαcγ = −sβ cβsγ 0 0
2011年7月8日
南京航空航天大学机械电子工程系
2011年7月8日
南京航空航天大学机械电子工程系
24
运动学逆问题
封闭解 θ1 = q1 (nx , n y , nz ,L , px , p y , pz ) θ 2 = q2 (nx , n y , nz ,L , px , p y , pz ) θ = q (n , n , n ,L , p , p , p ) 3 x y z x y z 3 θ 4 = q4 (nx , n y , nz ,L , px , p y , pz ) θ = q (n , n , n ,L , p , p , p ) 5 x y z x y z 5 θ 6 = q6 (nx , n y , nz ,L , px , p y , pz ) 是否存在？ 是否存在？
0 1 0 cα i −1 = 0 sα i −1 0 0
− sθi cθi sθ cα cθi cα i −1 i i −1 i −1 i T = sθi sα i −1 cθi sα i −1 0 0
0 − s α i −1 cα i −1 0
ai −1 −di sα i −1 ii −1 R = di cα i −1 0 0 0 1
由给定的旋转变换矩阵求等价的ZYX欧拉角 欧拉角 由给定的旋转变换矩阵求等价的
cαcβ cαsβ sγ − sαcγ sαcβ sαsβ sγ + cαcγ −sβ cβ sγ 0 0 cαsβcγ + sαsγ sαsβcγ − cαsγ cβcγ 0 0 r r r 11 12 13 0 r21 r22 r23 = 0 r31 r32 r33 1 0 0 0 0 0 0 1
南京航空航天大学机械电子工程系 17
2011年7月8日
nx n o a p ny 0 6T = = n 0 0 0 1 z 0
ox oy oz 0
ax ay az 0
px py pz 1
s6
2011年7月8日
南京航空航天大学机械电子工程系
连杆变换
i −1 i
T = Rot ( x, α i −1 )Trans ( x, ai −1 ) Rot ( z ,θ i )Trans ( z , di )
0 − s α i −1 cα i −1 0 0 1 0 0 0 0 1 0 0 0 ai −1 cθi 1 0 0 sθi 0 1 0 0 0 0 1 0 − sθi cθi 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 1 0 0 0 1 di 0 0 1 0 0
zi−1
zi
D－H参数 － 参数
yi−1
xi−1
α i −1
ai −1
di
yi
xi
θi
连杆长度a ： zi-1沿着 i-1到zi的距离； 沿着x 的距离； 连杆 连杆长度 i-1： 连杆扭转角α 的转角； 参数 连杆扭转角 i-1： zi-1绕xi-1到zi的转角； 关节偏置d 关节 关节偏置 i： 参数 关节转角θi ： 关节转角 xi-1沿着zi到xi的距离； 的距离； 沿着 xi-1绕zi到xi的转角。 的转角。
反过来如果不满足上面两个条件，逆解有可能存在或不存在。 反过来如果不满足上面两个条件，逆解有可能存在或不存在。
现在绝大多数的机械手， 现在绝大多数的机械手，都满足上面的充 分条件之一，所以封闭式逆解都存在。 分条件之一，所以封闭式逆解都存在。
2011年7月8日 南京航空航天大学机械电子工程系 26
3.6 欧拉角的反变换法
α
r11 cα = cβ
px py pz 1
s6
2011年7月8日
南京航空航天大学机械电子工程系
23
运动学正解
nx = n = y nz = ox = o y = oz =
f1 (θ1 ,θ 2 ,L ,θ 6 ) f 2 (θ1 ,θ 2 ,L ,θ 6 ) f3 (θ1 ,θ 2 ,L ,θ 6 ) f 4 (θ1 ,θ 2 ,L ,θ 6 ) f5 (θ1 ,θ 2 ,L ,θ 6 ) f 6 (θ1 ,θ 2 ,L ,θ 6 )
南京航空航天大学机械电子工程系
12
z5
x3
z3
z4
z6
2011年7月8日
南京航空航天大学机械电子工程系
13
PUMA560机器人运动学方程 PUMA560机器人运动学方程
Unimation PUMA560机器人示意图与坐标系 PUMA560机器人示意图与坐标系
2011年7月8日 南京航空航天大学机械电子工程系 14
PUMA560机器人运动学方程 PUMA560机器人运动学方程
Unimation PUMA560机器人示意图与坐标系 PUMA560机器人示意图与坐标系
2011年7月8日 南京航空航天大学机械电子工程系 15
连杆参数
连杆参数：
初始值
不同的坐标系下D-H矩阵是不同的，关键是约定!!
2011年7月8日 南京航空航天大学机械电子工程系 16
18
0 θ1 = 90°, θ 2 = 0°, 0 当 时， 0 θ3 = −90°, θ 4 = θ5 = θ 6 = 0° 6T = 1 0
2011年7月8日 南京航空航天大学机械电子工程系
1 0 −d 2 0 1 a2 + d 4 a3 0 0 0 0 1
β
c β =r +r
2 2 11
2 21
− s β = r31
∴ β = A tan 2 − r31 , r + r
2 11
2011年7月8日
有两个解， 有两个解，通常取 −90° ≤ β ≤ 90° 的一个解
2 21
(
)
双变量反 A tan 2( y, x) 正切函数
28
南京航空航天大学机械电子工程系
ax = f 7 (θ1 ,θ 2 ,L ,θ 6 ) a = f (θ ,θ ,L ,θ ) 8 1 2 6 y az = f9 (θ1 ,θ 2 ,L ,θ 6 ) px = f10 (θ1 ,θ 2 ,L ,θ 6 ) p y = f11 (θ1 ,θ 2 ,L ,θ 6 ) pz = f12 (θ1 ,θ 2 ,L ,θ 6 )
2011年7月8日 南京航空航天大学机械电子工程系 25
运动学反解的封闭解
6自由度机械手封闭解形式运动学逆解存在 自由度机械手封闭解形式运动学逆解存在 的两个充分条件： 的两个充分条件：
满足其中之一条件即可
1. 任意相邻三个关节轴线相交于一点。 任意相邻三个关节轴线相交于一点。 2. 任意相邻三个关节轴线相互平行。 任意相邻三个关节轴线相互平行。
ห้องสมุดไป่ตู้27
由给定的旋转变换矩阵求等价的ZYX欧拉角 欧拉角 由给定的旋转变换矩阵求等价的
cαcβ cαsβ sγ − sαcγ sαcβ sαsβ sγ + cαcγ −sβ cβ sγ 0 0 cαsβcγ + sαsγ sαsβcγ − cαsγ cβcγ 0 0 r r r 11 12 13 0 r21 r22 r23 = 0 r31 r32 r33 1 0 0 0 0 0 0 1
19
与图示的所表示的情况一致。 与图示的所表示的情况一致。
机器人运动学
正解
n o a p 0 1 n −1 0 0 0 1 =1 T (q1 ) 2 T (q2 )Ln T (qn )
反解
运动学方程
o
n
p
a
2011年7月8日
南京航空航天大学机械电子工程系
20
2011年7月8日
南京航空航天大学机械电子工程系
1 1 2
对转动关节 对移动关节
n −1 n
0 n
T = T (q ) T (q2 )L T (qn )
0 1
直角坐标系下描述的位置和姿态
关节空间 关节矢量
q = (q1 , q2 ,L , qn )
2011年7月8日
南京航空航天大学机械电子工程系
7
PUMA560机器人运动学方程 PUMA560机器人运动学方程
21
运动学逆问题
1. 2. 3. 4. 5. 反变换法（变量分离法） 数值法 几何法 旋量代数 四元素法
2011年7月8日
南京航空航天大学机械电子工程系
22
nx n n o a p y 0 6T = = n 0 0 0 1 z 0
ox oy oz 0
ax ay az 0
i −1
pi 0 1
机器人运动学
0 n
n o a p T = T TL T = 0 0 0 1
0 1 1 2 n −1 n
运动学方程
o n
p
a
手爪在基 座坐标系 下的描述 位姿） （位姿）
2011年7月8日