机器人正逆运动学 课件

0 0

0
0
0
1
C6 S6 0 0
A6


S
6
0
C6 0
0 0 1 0

0
0 0 1
第5步 求出总变化矩阵
RTH A1 A2 A3 A4 A5 A6
C1(C234C5C6 S234 S6 ) S1S5C6

S1(C234C5C6 C1S5 S6

0T1(q1)1T2 (q2 )n-1Tn

n 0
o 0
a 0
p
1



0
Rn 0
0
PnO 1

上式称为运动方程。
已知q1,q2,…,qn，求
，称为运动学正解；
已知
，求q1,q2,…,qn，称为运动学反解。
正解 反解
§1.5 机器人的逆运动学解
给定机器人终端位姿，求各关节变量，称求机器人运动学逆解。 让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步 骤。前面例子最后方程为：
axC1 a y S1 az
ax S1 a yC1 0
C234C5C6 S234 S6

S234C5C
6

C234 S6

S5C6

0
C234C5C6 S234C6 S234C5C6 C234C6
S5 S6 0
PxC1 Py S1
pz

Px
S1
的距离;an+1表示关节偏移，an+1是从Zn到Zn+1沿Xn+1测量的距离;角α 表示关节扭转, αn+1是从Zn到Zn+1绕Xn+1旋转的角度。 通常情况下， 只有θ和d是关节变量。
斯坦福机器人
斯坦福机器人开始的两个关节是旋转的， 第三个关节是滑动的，最后三个腕关节 全是旋转关节
例1：Stanford机器人运动学方程

1 0

0
0
0
1

C3 S3 0 C3a3
A3


S3
0
C3 0
0
S
3a3

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


S5
0
0 1
C5 0
§1.4 机器人正向运动学
工业机器人的正向运动学是指已知各关节的类型、相邻 关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向， 转角 为关节变量。如 果关节是滑动的，Z轴为 沿直线运动的方向，连 杆长度d为关节变量。关 节n处Z轴下标为n-1。
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。
an α n 0 -900
a2
0
a3 -900
0 900
0 -900
00
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
第一步：根据D-H法建立坐标系的规则建立坐标系
第二步：将做好的坐标系简化为我们熟悉的线图形式
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
系原点的距离 θi — 绕 zi-1 轴，由 xi-1转向 xi
连杆0
z0 y0
d1 x0
O0
解：
例2、PUMA560运动学方程（六个自由度，全部是旋转关节） 关节变量都是θ
θ2
θ1
θ3
θ5
θ4 θ6
PUMA560机器人的连杆及关节编号
A1
O1 O0
A2
为右手坐标系，Yi轴：按右手定则
S n1
0

0
Sn1C n1 C n1C n1
S n1
0
S n1 S n1 Cn1S n1
C n1
0
an1C n1
a
n1
S
n1

dn1

1
#

d
a

1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
将上面两个方程两边平方相加，并利用和差化积公式得到
S2 S23 C2C23 cos3
于是有：
C3

(
pxC1

py S1
C234a4 )2 ( pz 2a2a3

S234a4 )2

a22

a32
已知 S3 1 C32
于是可得到：
3

arctan
S3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 等， 可 以 得 到
S5 C23（4 C1ax S1a y ) S234az
C5 C1a y S1ax
5

arctan C234 (C1ax S1a x
0
C1 (C234a4

C23a3 C2a2 )

S1(C234a4

C23a3 C2a2 )

S234a4 S23a3 S2a2
1

综上： 依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次
变换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐
标系中的空间描述，即
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量
#

d
a

1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
0
90
6
6
0
0
0
nTn1 An1 Rot(z,n1 ) Trans (0,0, dn1 ) Trans (an1 ,0,0) Rot ( x,n1 )
C n1
An1


学习重点：1. 给关节指定参考坐标系 2. 制定D-H参数表 3. 利用参数表计算转移矩阵
背景简介：
1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出 了这一方法，后成为表示机器人以及对机器人建模的标准方法， 应用广泛。
总体思想：
首先给每个关节指定坐标系，然后确定从一个关节到下一个 关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化， 将所有变化结合起来，就确定了末端关节与基座之间的总变化， 从而建立运动学方程，进一步对其求解。
0
90
C1 0 S1 0
A1


S1
0
0 1
C1 0
0 0

0
0
0
1
6
6
0
0
0
第四步：将参数代入A矩阵，可得到
C1 0 S1 0
A1


S1
0
0 1
C1 0
0 0

0
0
0
1
C2 S2 0 C2a2
A2


S2
0
C2 0
0
S
2a2
C234 (C1nx S1ny ) S234nz
C1ny S1nx S234 (C1nx S1ny )
C
234nz

0

C234 (C1ox S1oy )
S234oz C1oy S1ox
S234 (C1ox S1oy )
C 234oz 0
nx ox ax px
RTH

n y

nz
oy oz
ay az
p
y

pz

0 0 0 1
RTH A1 A2 A3 A4 A5 A6
C1(C234C5C6 S234 S6 )
S1S5C6

S1(C234C5C6


C1
S5
S6

S234 S6
)

S234C5C6

0
C1(C234C5C6 S234C6 )
S1S5 S6 S1(C234C5C6 S234C6 )
C1S5S6 S234C5C6 C234C6
0
C1(C234 S5 )
S1C5 S1(C234 S5 )
C1C5 S234 S5
0
C1(C234a4

C23a3 C2a2 )
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

S1(C234a4

C23a3 C2a2 S234a4 S23a3
) S2a2

1

求逆运动学方程的解
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 ny S1

nz

nx
S1

nyC1
0
oxC1 oy S1 oZ
ox S1 oyC1 0
)

C 234
pz

S34a2 S4a3 1

C5C6 C5S6 S5 0

S5C
6
S5S6
C5
0

S6
C6
0 0
0
0
0 1
234

arctan(
az C1ax
S1a y
)和 234

234

180
C234

S23（4 C1a x az
• 为右手坐标系 • 原点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
x3
连杆4
y3
O3
连杆3
A3
d3 A2
C234 (C1ax
S1a y ) S234ax C1a y S1a x S234 (C 1ax S1a y )
C 234a z 0
C234 (C1 px S1 py ) S234 pz

C34a2 C4a3 a4

0


S234 (C1Px

S1 Py
di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
系原点的距离
θi — 绕 zi-1 轴，由 xi-1转向 xi
A5
A4 A6
连杆 n θ n
dn
1 θ 1 (900) 0
2 θ 2 (0) d2 3 θ 3 (-900) 0
4 θ 4 (0) d4
5 θ 5 (0) 0
6 θ 6 (0) 0

S234 S6
)

S234C5C6

0
C1 (C234C5C6 S234C6 )
S1S5 S6 S1 (C234C5C6 S234C6 )
C1S5S6 S234C5C6 C234C6
0
C1(C234 S5 )
S1C5 S1(C234 S5 )
C1C5 S234 S5
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标Fra Baidu bibliotek2
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 坐标

PyC1

1
C234 S5 S234 S5
C5 0
C234a4 C23a3 C2a2
S234a4

S23a3

S2a2

0

1

根据第3行第4列元素对应相等可得到
1

arctan(
py px
)和1
1
180
根据1，4元素和2，4元素，可得到：
pxC1 py S1 C234a4 C23a3 C2a2 pz S234a4 S23a3 S2a2

S1a y
)
接下来再一次利用式
pxC1 p y S1 C234a4 C23a3 C2a2
pz S234a4 S23a3 S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
2

arctan (C3a3 (C3a3

a2 )( pz S234a4 ) S3a3 ( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3 ( pz