ppt机器人正逆运动学(PPT60页)

换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐标
系中的空间描述，即
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 机器人的逆运动学解
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 坐标
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向， 转角 为关节变量。如果 关节是滑动的，Z轴为沿 直线运动的方向，连杆 长度d为关节变量。关节 n处Z轴下标为n-1。
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。
2
arctan (C3a3 (C3a3
a2 )( pz S234a4 ) S3a3 ( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3 ( pz
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 等， 可 以 得 到
C1C5 S234 S5
0
C1(C234a4
C23a3 C2a2 )
S1(C234a4
C23a3 C2a2 S234a4 S23a3
) S
2a2
1
求逆运动学方程的解
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 ny S1
nz
n
x
S1
nyC1
0
oxC1 oy S1 oZ
C n1
0
an1C n1
an1
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
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
将上面两个方程两边平方相加，并利用和差化积公式得到
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的逆，可得到
• 为右手坐标系 • 原点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
S1ny ) S1oy )
C234nz C234oz
θ2
θ1
θ3
关节变量都是θ
θ5
θ4
θ6
§2.10 机器人的运动学编程
在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需
要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯消去
法等相对繁琐的算法。正确的算法是：
1
arctan(
py px
给定机器人终端位姿，求各关节变量，称求机器人运动学逆解。 让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步 骤。前面例子最后方程为：
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 )
A2
S
2
0
C2 0
0
S2
a
2
1 0
0
0
0
1
C3 S3 0 C3a3
A3
S
3
0
C3 0
0
S
3a3
1 0
0
0
0
1
C4
A4
S
4
0
0
0 S4 0 C4 1 0 00
C4a4
S4
a4
0
1
C5 0 S5 0
A5
S5
0
0 1
C5 0
0 0
0
0
0
1
C6 S6 0 0
A6
S6
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿
系原点的距离 θ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轴：按右手定则
az C1ax
S1a y
)和 234
234
180
C234
S23（4 C1a x az
S1a y
)
接下来再一次利用式
pxC1 p y S1 C234a4 C23a3 C2a2
pz S234a4 S23a3 S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
C 234a z 0
C234 (C1 px S1 py ) S234 pz
C34a2 C4a3 a4
0
S234 (C1Px
S1 Py
)
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(
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
S
n1
0
0
Sn1C n1 C n1C n1
S n1
0
S n1 S n1 Cn1S n1
S5 C23（4 C1ax S1a y ) S234az
C5 C1a y S1ax
5
arctan C234 (C1ax S1a x
S1a y ) C1a y
S234az
最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：
6
arctan
S234 (C1nx S234 (C1ox
§1.4 机器人正向运动学
工业机器人的正向运动学是指已知各关节的类型、相邻 关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
学习重点：1. 给关节指定参考坐标系 2. 制定D-H参数表 3. 利用参数表计算转移矩阵
背景简介：
1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出 了这一方法，后成为表示机器人以及对机器人建模的标准方法， 应用广泛。
总体思想：
首先给每个关节指定坐标系，然后确定从一个关节到下一个 关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化， 将所有变化结合起来，就确定了末端关节与基座之间的总变化， 从而建立运动学方程，进一步对其求解。
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
S1a y ) C1a y
S 234 a z
6
arctan
S234 (C1nx S234 (C1ox
S1ny S1o y
) )
C234nz C234oz
§2.11 设计项目
αn
0 -900
a2
0
a3 -900
0 900
0 -900
00
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
第一步：根据D-H法建立坐标系的规则建立坐标系
第二步：将做好的坐标系简化为我们熟悉的线图形式
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
#
d
a
1
1
Zn测量的距离;an+1表示关节偏移，an+1是从Zn到Zn+1沿Xn+1测量 的距离;角α表示关节扭转, αn+1是从Zn到Zn+1绕Xn+1旋转的角度。 通常情况下，只有θ和d是关节变量。
斯坦福机器人
斯坦福机器人开始的两个关节是旋转的， 第三个关节是滑动的，最后三个腕关节 全是旋转关节
例1：Stanford机器人运动学方程
ox S1 oyC1 0
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
Py
C1
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
C1S5S6 S234C5C6 C234C6
0
C1(C234 S5 )
S1C5 S1(C234 S5 )
C1C5 S234 S5
0
C1 (C234a4
C23a3 C2a2 )
S1(C234a4
C23a3 C2a2 )
S234a4 S23a3 S2a2
1
综上：
依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次变
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
S234 S6
)
S234C5C6
0
C1 (C234C5C6 S234C6 )
S1S5 S6 S1 (C234C5C6 S234C6 )
C234 (C1nx S1ny ) S234nz
C1ny S1nx S234 (C1nx S1ny )
C
234nz
0
C234 (C1ox S1oy )
S234oz C1oy S1ox
S234 (C1ox S1oy )
C 234oz 0
C234 (C1ax
S1a y ) S234ax C1a y S1a x S234 (C 1ax S1a y )
)
3
arctan
S3 C3
2
arctan
(C3a3 (C3a3
a2 )( a2 )(
pz S234a4 ) S3a3 ( pxC1 py S1 pxC1 py S1 C234a4 ) S3a3 ( pz
C234a4 ) S234a4 )
4 234 2 3
5
arctan
C234 (C1a x S1a x
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
an
S1S5C6
S1(C234C5C6 S234 S6
C1
S5 S6 S234C
5C
6
)
0
C1(C234C5C6 S234C6 )
S1S5 S6 S1(C234C5C6 S234C6 )
C1S5S6 S234C5C6 C234C6
0
C1(C234 S5 )
S1C5 S1(C234 S5 )