第3章 机械手运动学(4)

0 0 0 1
0 a2 0 0 1 0 0 1 0 0 1 0 0 0 0 1
PUMA560的手臂变换矩阵： 的手臂变换矩阵： 的手臂变换矩阵
0 6
T = T( )T( )T( )T( )T( )T( 6 ) θ θ θ θ θ θ
0 1 1 1 2 2 2 3 3 3 4 4 4 5 5 5 6
南京航空航天大学机械电子工程系
12
z5
x3
z3
z4
z6
2011年7月8日
南京航空航天大学机械电子工程系
13
PUMA560机器人运动学方程 PUMA560机器人运动学方程
Unimation PUMA560机器人示意图与坐标系 PUMA560机器人示意图与坐标系
2011年7月8日 南京航空航天大学机械电子工程系 14
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 )
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
由给定的旋转变换矩阵求等价的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
反过来如果不满足上面两个条件，逆解有可能存在或不存在。 反过来如果不满足上面两个条件，逆解有可能存在或不存在。
现在绝大多数的机械手， 现在绝大多数的机械手，都满足上面的充 分条件之一，所以封闭式逆解都存在。 分条件之一，所以封闭式逆解都存在。
2011年7月8日 南京航空航天大学机械电子工程系 26
3.6 欧拉角的反变换法
PUMA560机器人示意图
2011年7月8日 南京航空航天大学机械电子工程系 8
初始状态
2011年7月8日 南京航空航天大学机械电子工程系 9
2011年7月8日
南京航空航天大学机械电子工程系
10
z3
z4
2011年7月8日
南京航空航天大学机械电子工程系
11
z4 z 4 x3
z3 z3
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机器人运动学方程
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
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
c2 −s2 0 0 1 T = 2 −s2 −c2 0 0
0 0 1 d2 0 0 0 1
c3 −s3 s c 3 2 3 3T = 0 0 0 0
c6 −s6 0 0 5 T = 6 −s6 −c6 0 0
c5 −s5 0 0 0 −1 4 5T = s5 c5 0 0 0 0
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日
南京航空航天大学机械电子工程系
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
南京航空航天大学机械电子工程系 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日
南京航空航天大学机械电子工程系
β
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
南京航空航天大学机械电子工程系
机器人学
Robotics
南京航空航天大学机电学院 刘凯 liukai@nuaa.edu.cn
第三章 机械手运动学(3) 机械手运动学(3) 重点回顾
2011年7月8日
南京航空航天大学机械电子工程系
2
zi−1
连杆 坐标 系的 确定
zi
yi−1
xi−1
yi
xi
zi-1坐标轴：沿着 关节的运动轴； 坐标轴：沿着i-1关节的运动轴 关节的运动轴； xi-1坐标轴：沿着 i-1和zi的公法线，指向下一个关 坐标轴：沿着z 的公法线， 节的方向； 节的方向； yi-1坐标轴：按右手直角坐标系法则确定。 坐标轴：按右手直角坐标系法则确定。
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日
南京航空航天大学机械电子工程系
6
用 qi 描述关节变量
θ i qi = di
α
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 )
2011年7月8日 南京航空航天大学机械电子工程系 25
运动学反解的封闭解
6自由度机械手封闭解形式运动学逆解存在 自由度机械手封闭解形式运动学逆解存在 的两个充分条件： 的两个充分条件：
满足其中之一条件即可
1. 任意相邻三个关节轴线相交于一点。 任意相邻三个关节轴线相交于一点。 2. 任意相邻三个关节轴线相互平行。 任意相邻三个关节轴线相互平行。
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
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 ) 是否存在？ 是否存在？
连杆变换
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
ZYX欧拉角变换的逆运动学解 ZYX欧拉角变换的逆运动学解
A B
Rzyx (α,β,γ ) = Rot(z,α)Rot( y, β)Rot(x,γ ) 0 sβ 0 1 1 0 0 0 0 cβ 0 0 0 0 1 0 cαsβcγ + sαsγ sαsβcγ − cαsγ cβcγ 0 0 cγ sγ 0 0 0 0 1 0 −sγ cγ 0 0 0 0 1
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的转角。 的转角。
PUMA560机器人运动学方程 PUMA560机器人运动学方程
Unimation PUMA560机器人示意图与坐标系 PUMA560机器人示意图与坐标系
2011年7月8日 南京航空航天大学机械电子工程系 15
连杆参数
连杆参数：源自文库
初始值
不同的坐标系下D-H矩阵是不同的，关键是约定!!
2011年7月8日 南京航空航天大学机械电子工程系 16
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日
南京航空航天大学机械电子工程系