第四章 位置运动学

0 0 1 0
s1 - c1 0 0
0 c2 s 0 A2 2 0 0 0 1
- s2 c2 0 0
0 0 1 0
c2a2 c3 s2a 2 s A 3 3 0 0 1 0
- s3 c3 0 0
0 0 1 0
c3a 3 s3a 3 0 1
c4 s 4 A4 0 0
0 0 -1 0
- s4 c4 0 0
c4a4 c5 s s4a4 A 5 5 0 0 1 0
0 0 1 0
s5 - c5 0 0
c6 0 s 0 6 A6 0 0 0 1
cos q i sin q i 0 0
- sin q i cos i cos q i cos i sin i 0
sin q i sin i - cos q i sin i cos i 0
a i cos q i a i sin q i di 1
2012-9-11
8
第四章 位置运动学
4.1.3 以移动副连接的两杆件的 D-H参数的确定
若杆件以移动副相连接时，则连杆的坐标系的建立 与参数的规定同回转副连接的杆件的规定相类似，但是 连杆的长度已经没有意义，故可以令其为零。可得齐次 矩阵为
cos q i sin q i 0 0
q i —— x i - 1和 x i 的平行线绕轴 z i - 1的转角，称为关节角；
di
——O i -1 和 H i 之间的距离，称为横距；
a i ——公共法线的距离，称为杆件长度；
i —— z i - 1 轴在 O i 点处平行线与 z i 轴绕 x i 轴按右手法则
定义的夹角，称扭转角；
2012-9-11
14
第四章 位置运动学
6关节机械手的端部对基座的关系 0 T 6 可用下列矩阵表示
n x ny 0 T6 nz 0
ox oy oz 0
ax ay az 0
Px Py Pz 1
2012-9-11
15
第四章 位置运动学 变换Z－－机械手与参考坐标系的相对关系， 变换E－－机械手与其端部工具的关系， 变换X－－此工具端部对参考坐标系的位置和方向
机 构 运 动 简 图
坐 标 系 设 置
STANFORD机器人操作机
2012-9-11 22
第四章 位置运动学
斯坦福机器人关节变量及D-H参数
Ai
i
A1
-90o
A2
90o
A3
A4
-90o
A5
A6
0o
90o
0o
ai
di
0
0
0
0
d3
0
0
0
0
0
0
d2
q2
qi
q1
0
q4
q5
q6
2012-9-11
23
第四章 位置运动学
关节空间
q1 - q 6
笛卡尔空间
B
TH
逆向变换
2012-9-11
2
第四章 位置运动学
本章的主要内容
4.1 D-H参数的确定 4.2 从关节变量到手部位姿 ——运动学正问题 4.3 从手部位姿到关节变量 ——运动学逆问题 习题
2012-9-11
3
第四章 位置运动学
4.1 D-H参数的确定
具有n个关节自由度的机器人系统，其齐次矩阵可表示为
- s6 c6 0 0
0 0 1 0
0 0 0 1
2012-9-11
19
第四章 位置运动学
nx n y A A1 A 2 A3 A 4 A5 A6 nz 0
n x c 1 ( c 234 c 5 c 6 - s 234 s 6 ) - s 1 s 5 s 6
12
2012-9-11
第四章 位置运动学
4.2.2 运动学方程求解实例
6关节机器人手爪坐标系
n
a XH ZH P
YH
o
Z0 Y0
X0
6关节机器人手爪坐标系
2012-9-11 13
第四章 位置运动学
机器人的最后一个构件（手部）由三个自由度来 确定位置，三个自由度确定其方向； 我们将描述其位置和方向的坐标系原点定在两个 手指的中间，用一个向量P描述这个原点；三个 向量 n , o 和 a 描述机器人的姿态； 当手部处于初始位置和姿态时，向量 Z H 指向手接 近物体的方向，其单位向量 a 称为接近向量；向 量 Y H 的单位向量 o 称为方向向量；向量 X H 的单 位向量 n 称为法线向量。
2012-9-11
11
第四章 位置运动学
简化转动关节
条件： 杆件均为 直杆，即 为公共法 线，与 x 轴重合。 故 d=0
cq i sq i Ai 0 0
- sq i c i cq i c i s i 0
sq i s i - cq i s i c i 0
a i cq i a i sq i 0 1
2012-9-11
- sin q i cos i cos q i cos i sin i 0
sin q i sin i - cos q i sin i cos i 0
0 0 di 1
9
第四章 位置运动学
4.2 从关节变量到手部位姿
——运动学正问题
4.2.1 三种简化情况的齐次变换矩阵
2012-9-11 17
第四章 位置运动学
6关节操作机齐次矩阵中的D-H参数
Ai A1 90° A2 0° A3 0° A4 -90° A5 90° A6 0°
αi
ai
θi
0
θ1
a2
θ2
a3
θ3
a4
θ4
0
θ5
0
θ6
对于简化转动关节 d i =0
2012-9-11 18
第四章 位置运动学
c1 s 1 A1 0 0
Px c 1 ( c 234 a 4 c 23 a 3 c 2 a 2 )
P y s 1 ( c 234 a 4 c 23 a 3 c 2 a 2 )
Pz s 234 a 4 s 23 a 3 s 2 a 2
20
o z - s 234 c 5 s 6 - c 234 c 6
cos q sin q 0 0
- sin q cos q 0 0
- sin q cos q 0 0
0 0 1 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
r 0 0 1
r cos q r sin q 0 1
H i -1 O i -1
i i
i -1 i -1
i
i
i -1
i -1
i -1
i -1
i -1
i -1
i
i
2012-9-11
6
第四章 位置运动学
上述两个坐标系之间的关节变量是可变关节角 q i （或 可变杆长 a i ）。另外还有三个参数 i 、d i 、及 a i（或q i ） 在机器人手的结构设计时定为常数，这四个参数即为D-H参 数。具体定义如下：
0 0 d3 1
0 0 0 1
2012-9-11
24
第四章 位置运动学
nx n y A A1 A 2 A3 A 4 A5 A6 nz 0 ox oy oz 0 ax ay az 0 Px Py Pz 1
a x c 1 ( c 23 c 4 s 5 s 23 c 5 ) - s 1 s 4 s 5
X Z T6 E
0
Z
A1
A2 A3 A4 A5 A6
5T 2T 6 3T 6 4T 6 6 6
E X
1T 0T 6
从图中可求得
2012-9-11
0
T6 Z
-1
XE
-1
16
第四章 位置运动学
具有6个简化转动关节的操作机 6个简化转动关节的操作机由转动坐标臂和手腕组成。
由6个简化转动关节组成的操作机
0 0 1 0
0 0 1 0
s2 - c2 0 0
s5 - c5 0 0
1 0 0 0 A d2 3 0 1 0
c6 0 0 s A 6 6 0 0 1 0
0 1 1 0
- s6 c6 0 0
0 0 0 0
0 0 1 0
2012-9-11
7
第四章 位置运动学
4.1.2 变换矩阵的确立
a 若已知四个参数 q i、 i 、 i 及 d i 就完全确定了连杆i - 1 和连 杆 i 之间的相对关系。对此，我们建立 i - 1 和 i 坐标系之间 的变换关系。对于旋转关节可以确定以下的齐次矩阵
Ai Rot ( z i -1 , q i )Trans ( z i - 1 , d i )Trans ( x i , a i ) Rot ( x i , i )
2012-9-11
第四章 位置运动学
斯坦福机器人的求解
Z3 Z5 Z6
斯坦福机器人由球面 坐标臂和手腕组成。 由于各关节轴线彼此 正交，可以将各杆件 坐标系的 X 轴都安 排在同一方向。 暂不计终端操作装置 的位移。
X0 X1 X3-6 Z0 Z1
Z4
d3 Z2
X
X2 d2
2012-9-11
21
第四章 位置运动学
只具有伸缩臂的操作机
1 0 A 0 0 0 1 0 0 0 0 1 0 0 0 d 1
关节变量是伸缩臂的长度d
2012-9-11 10
第四章 位置运动学
q -r
操作机（平行轴情况）
A Rot ( z 0 , q )Trans ( x1 , r )
cos q sin q 0 0
n y s 1 ( c 234 c 5 c 6 - s 234 s 6 ) c 1 s 5 c 6
ox oy oz 0
ax ay az 0
Px Py Pz 1
a x c 1 c 234 s 5 s 1 c 5
a y s 1 c 234 s 5 - c 1 c 5
c1 s 1 A1 0 0
c4 s 4 A4 0 0
0 0 -1 0
0 0 -1 0
- s1 c1 0 0
- s4 c4 0 0
0 c2 0 s A s 5 A 0 5 0 1 0
2012-9-11
5
第四章 位置运动学
i-1和i 关节各坐标轴的定义
——关节轴i-1和i的公共法线； H O ——关节轴i和i+1的公共法线； 的交点； O i - 1 ——关节轴i和 H O Oi ——关节轴i+1和 H O 的交点； z i -1 轴——沿着关节i的轴线方向； x i - 1 轴——沿着 H O 的延长线方向； y i - 1 轴——使 O x y z 构成右手坐标系。 z i 轴——沿着关节i+1的轴线方向； x i 轴——沿着 H O 的延长线方向； y i 轴——使 O x y z 构成右手坐标系。 i i i i
《机器人原理与应用》
第四章 位置运动学
授课教师：王斐
东北大学人工智能与机器人研究所
2012-9-11
1
第四章 位置运动学
运动学处理运动的几何学以及与时间有关的量，而不考虑引 起运动的力。 位置运动学则只处理运动的几何学，而不考虑运动的时间。 机器人的位置运动学存在有两类问题：
－－根据关节变量求手部位姿的位置运动学正问题； －－根据手部位姿求关节变量的位置运动学逆问题，又称为手臂解。 正向变换
A A1 A 2 A i A n
为建立运动学方程，要讨论相邻连杆运动关系，为此引入机器 人学中的重要参数—Denavit-Hartenberg参数，简称为D-H参数。
2012-9-11 4
第四章 位置运动学
4.1.1 以回转副连接的两杆件的 D-H参数的确定
定义: 在杆件i-1前端的坐标系 O i -1 X i -1Y i -1 Z i -1 视为基础坐标系B， 在杆件i前端的坐标系 O i X i Y i Z i 视为运动坐标系H。
a z s 234 s 5
n z s 234 c 5 c 6 - c 234 s 6
o x - c 1 ( c 234 c 5 s 6 s 234 c 6 ) s 1 s 5 s 6
o y - s 1 ( c 234 c 5 s 6 s 234 c 6 ) - c 1 s 5 s 6