机器人运动学正解逆解 ppt课件

C1(C23a44
S1S5C6
S1S5S6
S1C5
C2a33C2a2)
S1C (C 1S253SC465C6S23S46)
S2 3C45C6
S1(C23C45C6S23C46)
C1S5S6 S23C45C6C23C46
S1(C23S45)
C1C5 S2 3S45
S1(C23a44 S23C a442a33S2C a332a2S )2a2
S2
0
C2 0
0
S2a2
1 0
0
0
0
1
C3 S3 0 C3a3
A3
S3
0
C3 0
0
S3a3
1 0
0 0 0 1
C4 0 S4 C4a4
A4
S4
0
0 1
C4 0
S4a4
0
0 0 0 1
C5 0 S5 0
A5
S
5
0
0 1
C5 0
0
0
0
0
0
1
C6 S6 0 0
A6
S
2
arctan(C3a3 (C3a3
a2 )( pz S234a4 ) S3a3( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3( pz
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 ， 等 可 以 得 到
学习重点：1. 给关节指定参考坐标系 2. 制定D-H参数表 3. 利用参数表计算转移矩阵
2
精品资料
• 你怎么称呼老师？ • 如果老师最后没有总结一节课的重点的难点，你
是否会认为老师的教学方法需要改进？ • 你所经历的课堂，是讲座式还是讨论式？ • 教师的教鞭 • “不怕太阳晒，也不怕那风雨狂，只怕先生骂我
§1.4 机器人正向运动学 工业机器人的正向运动学是指已知各关节的类型、相邻
关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
1
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
pxC 1pyS1C23 a4 4C2a 33C2a2 pzS23 a4 4S2a 33S2a2
将上面两个方程两边平方相加，并利用和差化积公式得到
S2S23C2C23co3s
于是有： C3(pxC1pyS1C23a44)22a2a(3pzS23a44)2a22a32
32
已知 S3 1C32
于是可得到：
Zi轴：与Ai+1关节轴重合，指向任意
Xi轴： Zi和Zi-1构成的面的法线，
或连杆i两端轴线Ai 与Ai+1的公垂线(即： Zi和Zi-1的公垂线)
原点Oi： Ai与Ai+1关节轴线的交点，或Zi与Xi的交点
A3
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离
αi — 绕 xi 轴，由 zi-1 转向zi
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿
S1(C23C45C6S23C46) C1S5S6
S1(C23S45) C1C5
S1(C23a44
C2a33C2a2)
S23C45C6
S23C45C6C23C46
S23S45 S23a44S2a33S2a2
0
0
0
1
27
综上： 依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次变 换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐标 系中的空间描述，即
§1.5 机器人的逆运动学解
给定机器人终端位姿，求各关节变量，称求机器人运动学逆解。 让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步 骤。前面例子最后方程为：
nx ox ax px
RTH
n y
nz
oy oz
ay az
p
y
pz
0 0 0 1
RTHA1A2A3A4A5A6
C1(C23C45C6S23S46) C1(C23C45C6S23C46) C1(C23S45)
θ2
θ1
θ3
关节变量都是θ
θ5
θ4
θ6
35
§2.10 机器人的运动学编程
在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需
要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯消去 法等相对繁琐的算法。正确的算法是：
1
arctan(py px
)
3
arctanS3 C3
2arct((aC C33naa33 aa22))((ppzxC 1S2p3ay44S)1SC32a33(a4p4)xC 1S3ap3y(Sp1zC S22
S6
C6
0 0
0
0
0 1
234arctaCn1a(xaz S1ay )和234234180
C234S2（ 34C1aaxz S1ay)
33
接下来再一次利用式
pxC 1pyS 1C 23 a4 4C 2a 33C 2a2
pzS23 a4 4S2a 33S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
dn1
0
0
0
1
#
d
a
1
1
2
2
3
3
4
4
5
5
0
0
90
0
a2
0
0
a3
0
0
a4
-90
0
0
90
C1 0 S1 0
A1
S
1
0
0 1
C1 0
0 0
0
0
0
1
6
6
0
0
0
25
第四步：将参数代入A矩阵，可得到
C1 0 S1 0
A1
S1 0
0 1
C1 0
0 0
0
0
0
1
C2 S2 0 C2a2
A2
#
1
1
2
2
3
3
4
4
5
5
6
6
d
a
0
0
90
0
a2
0
0
a3
0
0
a4
-90
0
0
90
0
0
0
24
nTn1An1Ro(z,tn1)Tra(0n,0,sdn1)Tra(ann1s,0,0)Ro(xt,n1)
Cn1 Sn1Cn1 Sn1Sn1 an1Cn1
An1S0n1
Cn1Cn1 Sn1
Cn1Sn1 Cn1
an1Sn1
系原点的距离 θi — 绕 zi-1 轴，由 xi-1转向 xi
连杆0
z0 y0
d1 x0
O0
11
解：
12
13
14
15
例2、PUMA560运动学方程（六个自由度，全部是旋转关节） 关节变量都是θ
θ2
θ1
θ3
θ5
θ4 θ6
16
PUMA560机器人的连杆及关节编号
17
A1
O1 O0
A2
为右手坐标系，Yi轴：按右手定则
di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
系原点的距离
θi — 绕 zi-1 轴，由 xi-1转向 xi
A5
A4 A6
18
连杆 n θn
dn
an
αn
1 θ1 (900) 0
0 -900
2
θ2 (0) d2 a2
0
3 θ3 (-900) 0
a3 -900
4
θ4 (0) d4
5
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向，
转角 为关节变量。如果
关节是滑动的，Z轴为沿 直线运动的方向，连杆 长度d为关节变量。关节 n处Z轴下标为n-1。
6
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。
x3
连杆4
y3
O3
连杆3
A3
d3 A2
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标系2
x5
o3 , o4 , o5重 合 d4 d5 0
关节2 O1
z1
坐标系1
y1 连杆1
x1
d2
关节1 坐标系0
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 αi — 绕 xi 轴，由 zi-1 转向zi di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
6
0
C6 0
0 0 1 0
0 0 0 1
26
第5步 求出总变化矩阵
RTHA1A2A3A4A5A6
C1(C23C45C6S23S46) C1(C23C45C6S23C46) C1(C23S45)
S1S5C6
S1S5S6
S1C5
C1(C23a44
C2a33C2a2)
S1C (C 1S253SC645C6S23S46)
0
0
0
1
求逆运动学方程的解
30
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 nyS1
nz
nxS1 nyC1 0
oxC1 oyS1 oZ
oxS1 oyC1 0
axC1 ayS1 az
axS1 ayC1 0
PxC1 PyS1
pz
PxS1 PyC1
1
C234C5C6 S234S6 S234C5C6 C234S6
C1oy S1ox
C1ay S1ax
0
S234(C1nx S1ny)
S234(C1ox S1oy)
S234(C1ax S1ay)
S234(C1Px
S1Py)C234pz
C234nz
0
C234oz 0
C234az 0
S34a2 S4a3 1
C5C6 C5S6 S5 0 S5C6 S5S6 C5 0
Zn测量的距离;an+1表示关节偏移，an+1是从Zn到Zn+1沿Xn+1测量 的距离;角α表示关节扭转, αn+1是从Zn到Zn+1绕Xn+1旋转的角度。 通常情况下，只有θ和d是关节变量。
7
斯坦福机器人
斯坦福机器人开始的两个关节是旋转的， 第三个关节是滑动的，最后三个腕关节 全是旋转关节
8
9
S5C6
0
C234C5C6 S234C6 S234C5C6 C234C6
S5S6 0
C234S5 S234S5
C5 0
C234a4 C23a3 C2a2
S234a4
S23a3
S2a2
0
1
根据第3行第4列元素对应相等可得到
1a rc tp paxy)n和 (111 8 0
31
根据1，4元素和2，4元素，可得到：
例1：Stanford机器人运动学方程
10
• 为右手坐标系 • 原点Oi： Ai与Ai+1关节轴线的交点
A6
y6
z6
A5
连杆5
• zi轴：与Ai+1关节轴重合，指向任意
x6
O6
关节6
关节5 坐标系4
• xi轴： Zi和Zi-1构成的面的法线 • yi轴：按右手定则
坐标系5
d6 z4
A4 z3
关节4 坐标系3
S5 C23（4 C1ax S1ay ) S234az
C5 C1ay S1ax
5
arctanC234(C1ax S1ax
S1ay ) C1ay
S234az
34
最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：
6a
rcS t2a3 (C n 41nxS1ny)C 23 nz4 S23 (C 41oxS1oy)C 23 oz4
0
900
5
θ5 (0) 0
0 -900
19
6 θ6 (0) 0
0
0
20
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
21
第一步：根据D-H法建立坐标系的规则建立坐标系
22
第二步：将做好的坐标系简化为我们熟悉的线图形式
23
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
3
arctanS3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
C234(C1nx S1ny) S234nz
C234(C1ox S1oy) S234oz
C234(C1ax S1ay)S234ax
C234(C1px S1py)S234pz
C34a2 C4a3 a4
C1ny S1nx
0 T 1 ( q 1 ) 1 T 2 ( q 2 ) n - 1 T n n 0 o 0 a 0 1 p 0 R 0 n 0 P 1 n O
上式称为运动方程。
已知q1,q2,…,qn，求
，称为运动学正解；
已知
，求q1,q2,…,qn，称为运动学反解。
28
正解 反解
29
笨，没有学问无颜见爹娘 ……” • “太阳当空照，花儿对我笑，小鸟说早早早……”
4
背景简介：
1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出 了这一方法，后成为表示机器人以及对机器人建模的标准方法， 应用广泛。
总体思想：
首先给每个关节指定坐标系，然后确定从一个关节到下一个 关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化， 将所有变化结合起来，就确定了末端关节与基座之间的总变化， 从而建立运动学方程，进一步对其求解。
3a44) 3a44)
4 2 3423
5
arctCa2n3(4C1axS1ay)S2 S1axC1ay
3a4z
6a
rcS t2a3 (C n 41nxS1ny)C 23 nz4 S23 (C 41oxS1oy)C 23 oz4
36
§2.11 设计项目
利用本书中所介绍的四自由度机器人，结合本章所学的知识 进行四自由度机器人的正逆运动学分析。