机器人动力学
Dynamics of Robotics
研究机器人的运动特性与力的关系。
有两类问题：
动力学正问题,已知机械手各关节的作用力或
力矩，求各关节的位移、速度、加速度、运动
轨迹；
动力学逆问题,已知机械手的运动轨迹，即各
关节的位移、速度和加速度，求各关节的驱动
力和力矩。
机器人动力学
Dynamics of Robotics
4.1 机器人刚体动力学
(复习加深理论力学内容 )
4.2 机械手动力学方程
(Lagrange法 )
4.3 机械手动力学方程实例
(二杆、三杆机械手 )
4.4 机器人的动态特性
4.5 机械手的稳态负荷
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
绝对运动,相对于定坐标系的运动
相对运动,相对于动坐标系的运动
牵连运动,动坐标相对于定坐标运动
绝对运动方程,在定坐标系中的运动方程
相对运动方程,在动坐标系中的运动方程
牵连运动方程,动坐标系在定坐标系中的运动方程
)()(')( ' trtrtr o??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
绝对运动速度,在定坐标系中的运动速度
相对运动速度,在动坐标系中的运动速度
牵连运动速度,动坐标系在定坐标系中的运动速度
绝对运动加速度,在定坐标系中的运动 加 速度
相对运动加速度,在动坐标系中的运动 加 速度
牵连运动加速度,动坐标系在定坐标系中的运动 加 速度
当牵连速度为平动时，
当牵连运动为定轴转动时，
er vvv ??
re aaa ??
rrekre vaaaaaa ??????? ?2
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
Lagrange方程
T:系统动能；
qi:广义坐标； Qi:对应于广义坐标的广义力
当主动力为势力时,方程变为：
L:Lagrange函数
j
jj
Q
q
T
q
T
dt
d ?
?
??
?
?
?
0?
?
??
?
?
?
?
?
?
?
?
?
?
jj q
L
q
L
dt
d
? VTL ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
当主动力中有非势力时,
Qj:为非势的广义力
当含有粘性阻尼时，方程变为：
,Φ,瑞利耗三散函数
j
jj
Q
q
L
q
L
dt
d ?
?
??
?
?
?
?
?
?
?
?
?
?
?
j
jjj
Q
qq
L
q
L
dt
d ?
?
???
?
??
?
?
?
?
?
?
?
?
?
?
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
例：图示为振动系统方程
1。动能
2。势能
)(21 222211 xmxmT ?? ??
? ?2122211 )(21 xxcxcV ???
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
3。耗散函数
4。拉格朗日函数
? ?2122211 )(21 xxx ??? ???? ??
? ?? ?2222122121222211 2
2
1)(
2
1 xcxxcxccxmxm
VTL
??????
??
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
对每个广义坐标写出拉格朗日方程
将上述结果代入，得
下面将 K,P,D,W等表示动能、势能、耗散函数、外力做的功
0
0
222
111
?
?
??
?
?
?
???
?
?
??
?
?
?
?
?
?
??
?
?
?
???
?
?
??
?
?
?
?
xx
L
x
L
dt
d
xx
L
x
L
dt
d
??
??
? ? ? ?
0
0
2212221222
221212212111
?????
???????
xcxcxxxm
xcxccxxxm
????
????
??
???
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
考虑重力时,
当 时,取 为广义坐标，有
01
2
01
0011
2
01
2
00
2
11
)(
2
1
)(
2
1
2
1
2
1
FxFxW
xxcD
gxMgxMxxkP
xMxMK
??
??
????
??
??
??
00 ?x 1x
gMFkxxcxM
F
x
P
x
D
x
K
x
K
dt
d
11111
1111
????
?
?
??
?
??
?
??
???
?
???
?
?
?
???
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
当, 都为广义坐标，有
从以上两个例子看出，
求取动力学方程的关键是求出各能量函数 K,P,D,W的广义坐
标表达式。
00 ?x 01,xx
? ? ? ?
? ? ? ? FgMxxkxxcxM
FgMxxkxxcxM
???????
??????
0010100
1010111
????
????
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
两杆机器人如图。
对连杆 1：
对连杆 2：
111111
2
1
2
11
2
111
co s
2
1
2
1
?
?
gdmghmP
dmvmK
???
?? ?
222
2
222,2
1 ymgPvmK ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
二杆动能和势能分别为：
))(s in (s in
))(c o s (c o s
)c o s (c o s
)s in (s in
212121112
212121112
212112
212112
2
2
2
2
2
2
??????
??????
???
???
????
????
??
????
????
????
???
??
ddy
ddx
ddy
ddx
yxv
? ? ? ?? ?
? ?21221122
21
2
1221
2
221
2
1
2
2
2
1
2
12
2
222
co sco s
co s22
2
1
2
1
???
?????????
????
??????
?
gdmgdmP
ddddm
vmK
????????
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
系统的总动能和势能及拉格朗日函数分别为：
分别求得
注意：这里只求显因变量的偏导数
PKL
PPP
KKK
??
??
??
21
21
222
111
???
???
??
??
?
?
?
?
?
?
?
?
?
?
?
?
L
dt
dLL
L
dt
dLL
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
代入拉格朗日方程
? ?? ? ? ?
? ? ? ?
21221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
???
?????
????
??
????
??
??????
?
?
?
?
?
?
gdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
???
????
?
? ?
? ?2122212212
2
2
2212212
2
22
22
2
s ins in
co s
????
???
??
???
???
?
?
?
?
?
?
gdmddm
dmddmdm
LL
dt
d
T
?
????
?
)64( ?
)74( ?
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
写成矩阵有：
惯性力 向心力 哥式力 重力
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
??
?
??
?
2
1
12
21
2 2 12 1 2
1 2 11 1 2
2
2
2
1
2 2 22 1 1
1 2 21 1 1
2
1
2221
1211
2
1
D
D
DD
DD
DD
DD
DD
DD
T
T
??
??
?
?
?
?
??
??
?
?
??
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
当考虑关节 摩擦阻尼 时
? ?? ? ? ?
? ? ? ?
21221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
???
?????
????
??
????
??
??????
?
?
?
?
?
?
gdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
???
????
?
)64( ?
? ?? ? ? ?
? ? ? ? 1121221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
????
?????
????
??
?
???
????
?
cgdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
?????
??
??????
?
?
?
?
?
?
)124( ?
iq
DccD
?
??
?
???,
2
1
2
1 2
22
2
11 ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
当考虑关节 摩擦阻尼 时
)74( ?
)134( ?
? ?
? ?2122212212
2
2
2212212
2
22
22
2
s ins in
co s
????
???
??
???
???
?
?
?
?
?
?
gdmddm
dmddmdm
LL
dt
d
T
?
????
?
? ?
? ? 222122212212
2
2
2212212
2
22
22
2
s ins in
co s
?????
???
??
??
????
?
cgdmddm
dmddmdm
LL
dt
d
T
????
???
?
?
?
?
?
?
Robotics 动力学
4.2 机械手动力学方程
本节将以右图为例，研究由 A矩阵描述的机械手的动力学方程建
立的一般步骤：
1。计算任一连杆上任意一点的速度；
2。计算各连杆的动能和机械手的总动能；
3。计算各连杆的位能和机械手的总位能；
4。建立机械手系统的拉格郎日函数；
5。对拉格郎日函数求导，得到动力学方程。
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
图中，连杆 3上 P点的位置为：
0rp为基坐标系中 P的位置矢量；
3rp为杆 3坐标系中 P的位置矢量（原点 O3）；
T3杆 3的位姿矩阵；（即 P点在上述两坐标
系中坐标之间的变换矩阵。）
对任一连杆 i上的一点，其位置为
pp rTr 330 ?
rTr ii?0
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
所以,P点的速度为
为何不对 r求导？
对任一连杆 i上的一点，其速度为
p
j
j
j
pppp
q
q
dt
d
dt
d
r
T
rTrTrv
3
3
1
3
3
3
3
3
00 )()(
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
rTrv i
i
j
j
j
i q
qdt
d
???
?
???
?
?
??? ?
? 1
0 ?
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
P点的加速度为
对任一连杆 i上的一点，其加速度为
pjk
k j j
p
j
j
j
pj
j j
p
j
j
j
p
j
j
j
ppp
qq
q
q
q
q
qdt
d
q
dt
d
q
q
qdt
d
dt
d
dt
d
r
T
r
T
r
T
r
T
r
T
rTva
3
3
1
3
1
33
3
1
3
3
3
1
33
3
1
3
3
3
1
33
3
00
)()(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
? ??
??
?
? ??
??
?
????
??
??
rTrTa ijk
i
k
i
j j
ii
i
j
j
j
i qq
qqq ???
?
???
?
?
??
???
?
???
?
?
?? ? ??
? ??
????
1 11
0
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
P点速度的平方为
对任一连杆 i上的一点，其速度平方为
? ? ? ? ? ?? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
? ?
??
? ?
??
3
1
3
1
3333
3
1
333
3
1
3
0020
j k
kj
k
T
T
pp
j
T
k
k
k
T
pp
j
j
j
T
ppp
qq
qq
T r a ce
q
q
q
q
T r a ce
T r a ce
??
??
T
rr
T
T
rr
T
vvv
? ? ? ?
???
?
???
?
?
??
?
?? ? ?
? ?
i
j
i
k
kj
k
T
iTii
j
i qq
qqT r a c e 1 1
20 ??TrrTv
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
杆 3上 P点质量为 dm的微元，其动能为：
对任一连杆 i上的质量为 dm点，其动能为
? ?
? ?
? ?? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
? ?
? ?
? ?
3
1
3
1
3333
3
1
3
1
3333
2
0
3
2
1
2
1
2
1
j k
kj
k
T
T
pp
j
j k
kj
k
T
T
pp
j
p
qq
q
dm
q
T r a c e
dmqq
qq
T r a c e
dmdK
??
??
T
rr
T
T
rr
T
v
? ?? ?
???
?
???
?
?
?
?
?? ? ?
? ?
i
j
i
k
kj
k
T
iTii
j
i
i qqqdmqT r a c edK
1 12
1 ??TrrT
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
杆 3的动能为：
记,并称之为连杆 3的 伪惯量矩阵,则
对任连杆 i，其动能为
? ?
?
?
?
?
?
?
?
?
?
?
???
?
???
?
?
??? ? ? ??
? ?
3
1
3
1
3
3
333
3
33 2
1
j k
kj
k
TT
pp
j
qqqdmqT r a cedKK ??TrrT
连杆连杆
???
?
???
?
?
?
?
??? ? ??
? ?
i
j
i
k
kj
k
T
i
i
j
i
i
ii qqqqT r a c edKK
1 12
1 ??TIT
连杆
??
3
33
3
连杆
dmI Tpp rr
???
?
???
?
?
?
?
??? ? ??
? ?
3
1
3
1
3
3
3
3
33 2
1
j k
kj
k
T
j
qqqqT r a c edKK ??TIT
连杆
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
伪惯量矩阵 I的一般形式为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
????
????
????
????
?
dmz d my d mx d m
z d mdmzz d myz d mx
y d mz d mydmyy d mx
x d mz d mxy d mxdmx
dmI
iii
iiiiii
iiiiii
iiiiii
i
Tii
i 2
2
2
连杆
rr
? ? ? ? ? ?
???
???
???
???
??????
??????
z d mmzy d mmyx d mmx
y z d mIIx z d mIIx y d mII
dmyxIdmzxIdmzyI
zyyzzxxzyxxy
zzyyxx
222222
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
则具有 n个连杆的机械手的连杆总动能为：
考虑传动装置的惯量，所有传动装置的总动能为：
系统的总动能为
? ? ??
? ? ?? ?
?
?
?
?
?
?
?
?
?
?
??? n
i
i
j
i
k
kj
k
T
i
i
j
i
n
i
i qqqqT r a c eKK
1 1 11 2
1 ??TIT
?
?
?
n
i
iaia qIK
1
2
2
1 ?
?? ? ?
?? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
???? n
i
iai
n
i
i
j
i
k
kj
k
T
i
i
j
i
at qIqqqqT r a c eKKK
1
2
1 1 1 2
1
2
1 ???TIT
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
位能,质量 m,高 h的物体，其位能为
连杆 i上位置 的质量 dm的微元，其位能为
连杆 i的总位能为
m g hP ?
ri
dmTdmdP iiTiTi rgrg ????
i
i
i
T
i
i
i
i
T
i
i
i
T
i
ii
Tm
dmTdmTdPP
rg
rgrg
??
????? ???
连杆连杆连杆
? ?1zyxT ggg?g
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
系统的总位能为
式中,为连杆 i的质量；
为连杆 i对其前端关节坐标系的重心位置。
??
??
???
n
i
i
i
i
T
i
n
i
i TmPP
11
rg
im
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
系统的拉格朗日函数为
?
???
,2,1
2
1
2
1
11
2
1 1 1
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??? ? ?
??? ? ?
n
mqIqq
qq
T r a c e
PKL
n
i
i
i
i
T
i
n
i
iai
n
i
i
j
i
k
kj
k
T
i
i
j
i
t
rTg
T
I
T
np
qIq
q
T
q
T
T r a c e
q
q
T
q
T
T r a c e
q
L
pap
n
i
i
j
j
p
T
i
i
j
i
n
i
i
k
k
k
T
i
i
p
i
p
,,2,1
2
1
2
1
1 1
1 1
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
? ?
? ?
? ?
I
I
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
np
qq
q
T
qq
T
T r a c e
qIq
q
T
q
T
T r a c e
q
L
dt
d
k
n
pi
i
j
j
i
k p
T
i
i
kj
i
pap
n
pi
i
k
k
p
T
i
i
k
i
p
,,2,1
2
1 1
2
1
?
??
????
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ? ?
? ?
? ? ?
? ?
I
I
?? ? ?
?? ? ? ?
??
???
?
???
?
?
?
??
??
?
? n
pi
i
i
p
iT
ik
n
pi
i
j
j
i
k k
T
i
i
jp
i
p q
Tmqq
q
T
qq
TT r a c e
q
L rgI ??
1 1
2
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
系统的动力学方程为
i
n
j
n
k
kjijkiai
n
j
jij
n
j
i
i
i
iT
jm
n
j
j
k
k
j
m i
T
j
j
mk
i
iai
n
ij
j
k
k
i
T
j
j
k
j
i
DqqDqqD
q
T
mqq
q
T
qq
T
T r a ce
qIq
q
T
q
T
T r a ceT
????
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ??
?? ? ?
? ?
? ??
?? ? ?
? ?
1 11
11 1 1
2
1
??????
??
????
I
rgI
I
)244( ?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
??
?
??
?
2
1
12
21
2 2 12 1 2
1 2 11 1 2
2
2
2
1
2 2 22 1 1
1 2 21 1 1
2
1
2221
1211
2
1
D
D
DD
DD
DD
DD
DD
DD
T
T
??
??
?
?
?
?
??
??
?
?
??
??
)104( ?
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
qq
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
1。惯量项的简化
利用
记微分旋转和平移为：
通过计算有：
为质心矢量,为与惯量相关的矩阵，具有如下形式。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
qq
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
i
T
p
i
p pT
q
T ??
?
?
ipi
T
ipi
T ddpp ?? ??
? ?? ??
?
??????? 6
,m a x jip
j
p
j
p
j
p
i
p
p
p
j
p
i
p
j
p
p
T
i
p
pij ddddmD ???? rk
)314( ?
pk
ppr
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
1。惯量项的简化
当 i=j时，有
? ?? ??
?
???????
6
2
222222 2
ip
p
i
p
p
p
i
p
i
p
p z ziz
p
p y yiy
p
p x xix
p
pii dddkkkmD ???? r
)334( ?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
222
222
222
p z zp y zp x z
p y zp y yp x y
p x zp x yp x x
p
kkk
kkk
kkk
k
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
2。重力项的简化
将 代入
i
T
p
i
p pT
q
T ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
qq
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
?
?
???
6
ip
p
p
i
p
p
T
pi TmD rg )364( ?
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
0100
2022
2022
1000
0100
1011
1011
2
2
2
1
2
1
1
1
sdcs
cdsc
TA
sdcs
cdsc
A
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
以 为基准，有
以 为基准，有
以 为基准，有
kjikjdid 10000 11111 ?????? ?
10T
21T
kjikjdid 10000 22222 ?????? ?
20T
kjikjddcidsd 1000)(2 22212122 ??????? ?
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
设所有惯性力矩为零,为零。
有
此结果与 4.1.2中的一致。
2211,rr
? ?
? ?22212212
2
2222
2212
2
22
2
12111 2
dddcmD
dmD
cddmdmdmmD
??
?
????
? ?
12
121
222
221211
sgdmD
sgdmsgdmmD
?
???
Robotics 动力学
4.4 机械手动态特性
动态特性指：工作精度、重复能力、稳定性、空间分辨率
4.4.1 概述
快速、准确、定位的能力；
过块：精度差，（惯性影响）
过慢：效率低
对工业机器人，采用中继点来减少惯性影响。
Robotics 动力学
4.4 机械手动态特性
4.4.2 稳定性
稳定性指系统、装置或工具运动过程中，无振荡问题。
对机械系统 主要有系统的自激振动；
对电子系统,主要指其自激振荡；
对机器人系统,除上述两者外，还有其机电耦合振荡。如机械
手的抖动。
Robotics 动力学
4.4 机械手动态特性
4.4.3 空间分辨率
空间分辨率是描述机器人工具末端运动所达到的最小运动
增量。
Robotics 动力学
4.4 机械手动态特性
4.4.4 精度
用下列三个因素的集合来描述精度
1）各控制部件的分辨率；
2）各机械部件的偏差；
3）最近到达点距目标位置的差；
Robotics 动力学
4.4 机械手动态特性
4.4.4 精度
示教时，精度只与机械偏差有关；
离线编程时，与分辨率、机械偏差、接近能力有关。
Robotics 动力学
4.4 机械手动态特性
4.4.5 重复性
指机器人重复到达某一位置的能力。
Robotics 动力学
4.4 机械手动态特性
4.4.5 重复性
指机器人重复到达某一位置的能力。
几个概念的区别：
1）空间分辨率描述机器人所能控制的末端工具的最小运动增量
2）精度指一定分辨率下，机器人对某目标的接近能力；
3）重复性描述机器人重复到达同一目标时实际的位置误差；
4）一般重复性比精度好。
Robotics 动力学
4.5 机械手的静态负荷
解决：
1。静力和力矩的表示方法；
2。不同坐标系间，静负荷的变换；
3。确定机械手静态关节力矩；
4。由关节力矩确定机械手所载物体的重量。
Robotics 动力学
4.5 机械手的静态负荷
4.5.1 静力和力矩的表示方法
广义力 F 例：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
m
m
m
f
f
f
F
? ?01 0001 50010
01 000
1 50010
???
???
???
F
m
f
kji
kji
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
采用虚位移原理，研究静力作用在不同坐标系中的描述问题。
已知：在 A坐标系中的作用力表示为：
表示虚位移的微分运动矢量为：
已知 C坐标系到 A坐标系的坐标变换；
求：在 C坐标系中的作用力 CF 。
F所做虚功为：
在 C坐标系中做的虚功为：
? ?Tzyxzyx mmmfff?F
? ?zyxzyx ddd ????D
DF TW ??
DF cTcW ??
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
即：
由微分变换的等价关系式（ 3-97），有
或记为,
DFDF cTcT ?
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
c
y
c
x
c
z
c
y
c
x
c
d
d
d
aaa
ooo
nnn
aaa
ooo
nnn
d
d
d
?
?
?
?
?
?
000
000
000
apapap
opopop
npnpnp
JDD ?C
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
从而有
经变换有
FJFJFFJDFDFDF cTTcTTccTcT ????,,
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
c
y
c
x
c
z
c
y
c
x
c
f
f
f
m
m
m
aaa
ooo
nnn
aaa
ooo
nnn
f
f
f
m
m
m
000
000
000
apapap
opopop
npnpnp
Robotics 动力学
4.5 机械手的静态负荷
4.5.3 关节力矩的确定
设作用于作用于机器人末端 T6杆上的力所做的虚功
各关节力 /力矩所做的虚功，
τ,广义力矩矢量（旋转、平移）；
dq:表示虚位移的微分变换。
两者相等：
由（ 3-111）
得
DF 66 TTTW ??
dqW T?? ?
dqTTTT ??DF 66
dqq )(JD ?
FJ 6TT??
Robotics 动力学
习题,P142,第 4.1题
Dynamics of Robotics
研究机器人的运动特性与力的关系。
有两类问题：
动力学正问题,已知机械手各关节的作用力或
力矩，求各关节的位移、速度、加速度、运动
轨迹；
动力学逆问题,已知机械手的运动轨迹，即各
关节的位移、速度和加速度，求各关节的驱动
力和力矩。
机器人动力学
Dynamics of Robotics
4.1 机器人刚体动力学
(复习加深理论力学内容 )
4.2 机械手动力学方程
(Lagrange法 )
4.3 机械手动力学方程实例
(二杆、三杆机械手 )
4.4 机器人的动态特性
4.5 机械手的稳态负荷
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
绝对运动,相对于定坐标系的运动
相对运动,相对于动坐标系的运动
牵连运动,动坐标相对于定坐标运动
绝对运动方程,在定坐标系中的运动方程
相对运动方程,在动坐标系中的运动方程
牵连运动方程,动坐标系在定坐标系中的运动方程
)()(')( ' trtrtr o??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
绝对运动速度,在定坐标系中的运动速度
相对运动速度,在动坐标系中的运动速度
牵连运动速度,动坐标系在定坐标系中的运动速度
绝对运动加速度,在定坐标系中的运动 加 速度
相对运动加速度,在动坐标系中的运动 加 速度
牵连运动加速度,动坐标系在定坐标系中的运动 加 速度
当牵连速度为平动时，
当牵连运动为定轴转动时，
er vvv ??
re aaa ??
rrekre vaaaaaa ??????? ?2
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
Lagrange方程
T:系统动能；
qi:广义坐标； Qi:对应于广义坐标的广义力
当主动力为势力时,方程变为：
L:Lagrange函数
j
jj
Q
q
T
q
T
dt
d ?
?
??
?
?
?
0?
?
??
?
?
?
?
?
?
?
?
?
?
jj q
L
q
L
dt
d
? VTL ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
当主动力中有非势力时,
Qj:为非势的广义力
当含有粘性阻尼时，方程变为：
,Φ,瑞利耗三散函数
j
jj
Q
q
L
q
L
dt
d ?
?
??
?
?
?
?
?
?
?
?
?
?
?
j
jjj
Q
L
q
L
dt
d ?
?
???
?
??
?
?
?
?
?
?
?
?
?
?
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
例：图示为振动系统方程
1。动能
2。势能
)(21 222211 xmxmT ?? ??
? ?2122211 )(21 xxcxcV ???
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
3。耗散函数
4。拉格朗日函数
? ?2122211 )(21 xxx ??? ???? ??
? ?? ?2222122121222211 2
2
1)(
2
1 xcxxcxccxmxm
VTL
??????
??
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.0 动力学基本定理
对每个广义坐标写出拉格朗日方程
将上述结果代入，得
下面将 K,P,D,W等表示动能、势能、耗散函数、外力做的功
0
0
222
111
?
?
??
?
?
?
???
?
?
??
?
?
?
?
?
?
??
?
?
?
???
?
?
??
?
?
?
?
xx
L
x
L
dt
d
xx
L
x
L
dt
d
??
??
? ? ? ?
0
0
2212221222
221212212111
?????
???????
xcxcxxxm
xcxccxxxm
????
????
??
???
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
考虑重力时,
当 时,取 为广义坐标，有
01
2
01
0011
2
01
2
00
2
11
)(
2
1
)(
2
1
2
1
2
1
FxFxW
xxcD
gxMgxMxxkP
xMxMK
??
??
????
??
??
??
00 ?x 1x
gMFkxxcxM
F
x
P
x
D
x
K
x
K
dt
d
11111
1111
????
?
?
??
?
??
?
??
???
?
???
?
?
?
???
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
当, 都为广义坐标，有
从以上两个例子看出，
求取动力学方程的关键是求出各能量函数 K,P,D,W的广义坐
标表达式。
00 ?x 01,xx
? ? ? ?
? ? ? ? FgMxxkxxcxM
FgMxxkxxcxM
???????
??????
0010100
1010111
????
????
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
两杆机器人如图。
对连杆 1：
对连杆 2：
111111
2
1
2
11
2
111
co s
2
1
2
1
?
?
gdmghmP
dmvmK
???
?? ?
222
2
222,2
1 ymgPvmK ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.1 机械手的动能与势能
二杆动能和势能分别为：
))(s in (s in
))(c o s (c o s
)c o s (c o s
)s in (s in
212121112
212121112
212112
212112
2
2
2
2
2
2
??????
??????
???
???
????
????
??
????
????
????
???
??
ddy
ddx
ddy
ddx
yxv
? ? ? ?? ?
? ?21221122
21
2
1221
2
221
2
1
2
2
2
1
2
12
2
222
co sco s
co s22
2
1
2
1
???
?????????
????
??????
?
gdmgdmP
ddddm
vmK
????????
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
系统的总动能和势能及拉格朗日函数分别为：
分别求得
注意：这里只求显因变量的偏导数
PKL
PPP
KKK
??
??
??
21
21
222
111
???
???
??
??
?
?
?
?
?
?
?
?
?
?
?
?
L
dt
dLL
L
dt
dLL
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
代入拉格朗日方程
? ?? ? ? ?
? ? ? ?
21221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
???
?????
????
??
????
??
??????
?
?
?
?
?
?
gdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
???
????
?
? ?
? ?2122212212
2
2
2212212
2
22
22
2
s ins in
co s
????
???
??
???
???
?
?
?
?
?
?
gdmddm
dmddmdm
LL
dt
d
T
?
????
?
)64( ?
)74( ?
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
写成矩阵有：
惯性力 向心力 哥式力 重力
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
??
?
??
?
2
1
12
21
2 2 12 1 2
1 2 11 1 2
2
2
2
1
2 2 22 1 1
1 2 21 1 1
2
1
2221
1211
2
1
D
D
DD
DD
DD
DD
DD
DD
T
T
??
??
?
?
?
?
??
??
?
?
??
??
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
当考虑关节 摩擦阻尼 时
? ?? ? ? ?
? ? ? ?
21221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
???
?????
????
??
????
??
??????
?
?
?
?
?
?
gdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
???
????
?
)64( ?
? ?? ? ? ?
? ? ? ? 1121221121
2
22212212212
22212
2
2212212
2
22
2
121
11
1
s i ns i n
s i ns i n2
c o sc o s2
????
?????
????
??
?
???
????
?
cgdmgdmm
ddmddm
ddmdmddmdmdmm
LL
dt
d
T
?????
??
??????
?
?
?
?
?
?
)124( ?
iq
DccD
?
??
?
???,
2
1
2
1 2
22
2
11 ??
Robotics 动力学
4.1 机器人刚体动力学
4.1.2 机械手动力学方程的求法
当考虑关节 摩擦阻尼 时
)74( ?
)134( ?
? ?
? ?2122212212
2
2
2212212
2
22
22
2
s ins in
co s
????
???
??
???
???
?
?
?
?
?
?
gdmddm
dmddmdm
LL
dt
d
T
?
????
?
? ?
? ? 222122212212
2
2
2212212
2
22
22
2
s ins in
co s
?????
???
??
??
????
?
cgdmddm
dmddmdm
LL
dt
d
T
????
???
?
?
?
?
?
?
Robotics 动力学
4.2 机械手动力学方程
本节将以右图为例，研究由 A矩阵描述的机械手的动力学方程建
立的一般步骤：
1。计算任一连杆上任意一点的速度；
2。计算各连杆的动能和机械手的总动能；
3。计算各连杆的位能和机械手的总位能；
4。建立机械手系统的拉格郎日函数；
5。对拉格郎日函数求导，得到动力学方程。
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
图中，连杆 3上 P点的位置为：
0rp为基坐标系中 P的位置矢量；
3rp为杆 3坐标系中 P的位置矢量（原点 O3）；
T3杆 3的位姿矩阵；（即 P点在上述两坐标
系中坐标之间的变换矩阵。）
对任一连杆 i上的一点，其位置为
pp rTr 330 ?
rTr ii?0
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
所以,P点的速度为
为何不对 r求导？
对任一连杆 i上的一点，其速度为
p
j
j
j
pppp
q
q
dt
d
dt
d
r
T
rTrTrv
3
3
1
3
3
3
3
3
00 )()(
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
rTrv i
i
j
j
j
i q
qdt
d
???
?
???
?
?
??? ?
? 1
0 ?
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
P点的加速度为
对任一连杆 i上的一点，其加速度为
pjk
k j j
p
j
j
j
pj
j j
p
j
j
j
p
j
j
j
ppp
q
q
q
q
qdt
d
q
dt
d
q
q
qdt
d
dt
d
dt
d
r
T
r
T
r
T
r
T
r
T
rTva
3
3
1
3
1
33
3
1
3
3
3
1
33
3
1
3
3
3
1
33
3
00
)()(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
? ??
??
?
? ??
??
?
????
??
??
rTrTa ijk
i
k
i
j j
ii
i
j
j
j
i qq
qqq ???
?
???
?
?
??
???
?
???
?
?
?? ? ??
? ??
????
1 11
0
Robotics 动力学
4.2 机械手动力学方程
4.2.1 速度的计算
P点速度的平方为
对任一连杆 i上的一点，其速度平方为
? ? ? ? ? ?? ?
? ?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
? ?
??
? ?
??
3
1
3
1
3333
3
1
333
3
1
3
0020
j k
kj
k
T
T
pp
j
T
k
k
k
T
pp
j
j
j
T
ppp
T r a ce
q
q
q
q
T r a ce
T r a ce
??
??
T
rr
T
T
rr
T
vvv
? ? ? ?
???
?
???
?
?
??
?
?? ? ?
? ?
i
j
i
k
kj
k
T
iTii
j
i qq
qqT r a c e 1 1
20 ??TrrTv
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
杆 3上 P点质量为 dm的微元，其动能为：
对任一连杆 i上的质量为 dm点，其动能为
? ?
? ?
? ?? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
? ?
? ?
? ?
3
1
3
1
3333
3
1
3
1
3333
2
0
3
2
1
2
1
2
1
j k
kj
k
T
T
pp
j
j k
kj
k
T
T
pp
j
p
q
dm
q
T r a c e
dmqq
T r a c e
dmdK
??
??
T
rr
T
T
rr
T
v
? ?? ?
???
?
???
?
?
?
?
?? ? ?
? ?
i
j
i
k
kj
k
T
iTii
j
i
i qqqdmqT r a c edK
1 12
1 ??TrrT
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
杆 3的动能为：
记,并称之为连杆 3的 伪惯量矩阵,则
对任连杆 i，其动能为
? ?
?
?
?
?
?
?
?
?
?
?
???
?
???
?
?
??? ? ? ??
? ?
3
1
3
1
3
3
333
3
33 2
1
j k
kj
k
TT
pp
j
qqqdmqT r a cedKK ??TrrT
连杆连杆
???
?
???
?
?
?
?
??? ? ??
? ?
i
j
i
k
kj
k
T
i
i
j
i
i
ii qqqqT r a c edKK
1 12
1 ??TIT
连杆
??
3
33
3
连杆
dmI Tpp rr
???
?
???
?
?
?
?
??? ? ??
? ?
3
1
3
1
3
3
3
3
33 2
1
j k
kj
k
T
j
qqqqT r a c edKK ??TIT
连杆
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
伪惯量矩阵 I的一般形式为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
????
????
????
????
?
dmz d my d mx d m
z d mdmzz d myz d mx
y d mz d mydmyy d mx
x d mz d mxy d mxdmx
dmI
iii
iiiiii
iiiiii
iiiiii
i
Tii
i 2
2
2
连杆
rr
? ? ? ? ? ?
???
???
???
???
??????
??????
z d mmzy d mmyx d mmx
y z d mIIx z d mIIx y d mII
dmyxIdmzxIdmzyI
zyyzzxxzyxxy
zzyyxx
222222
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
则具有 n个连杆的机械手的连杆总动能为：
考虑传动装置的惯量，所有传动装置的总动能为：
系统的总动能为
? ? ??
? ? ?? ?
?
?
?
?
?
?
?
?
?
?
??? n
i
i
j
i
k
kj
k
T
i
i
j
i
n
i
i qqqqT r a c eKK
1 1 11 2
1 ??TIT
?
?
?
n
i
iaia qIK
1
2
2
1 ?
?? ? ?
?? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
???? n
i
iai
n
i
i
j
i
k
kj
k
T
i
i
j
i
at qIqqqqT r a c eKKK
1
2
1 1 1 2
1
2
1 ???TIT
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
位能,质量 m,高 h的物体，其位能为
连杆 i上位置 的质量 dm的微元，其位能为
连杆 i的总位能为
m g hP ?
ri
dmTdmdP iiTiTi rgrg ????
i
i
i
T
i
i
i
i
T
i
i
i
T
i
ii
Tm
dmTdmTdPP
rg
rgrg
??
????? ???
连杆连杆连杆
? ?1zyxT ggg?g
Robotics 动力学
4.2 机械手动力学方程
4.2.2 动能和位能的计算
系统的总位能为
式中,为连杆 i的质量；
为连杆 i对其前端关节坐标系的重心位置。
??
??
???
n
i
i
i
i
T
i
n
i
i TmPP
11
rg
im
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
系统的拉格朗日函数为
?
???
,2,1
2
1
2
1
11
2
1 1 1
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??? ? ?
??? ? ?
n
mqIqq
T r a c e
PKL
n
i
i
i
i
T
i
n
i
iai
n
i
i
j
i
k
kj
k
T
i
i
j
i
t
rTg
T
I
T
np
qIq
q
T
q
T
T r a c e
q
q
T
q
T
T r a c e
q
L
pap
n
i
i
j
j
p
T
i
i
j
i
n
i
i
k
k
k
T
i
i
p
i
p
,,2,1
2
1
2
1
1 1
1 1
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
? ?
? ?
? ?
I
I
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
np
q
T
T
T r a c e
qIq
q
T
q
T
T r a c e
q
L
dt
d
k
n
pi
i
j
j
i
k p
T
i
i
kj
i
pap
n
pi
i
k
k
p
T
i
i
k
i
p
,,2,1
2
1 1
2
1
?
??
????
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ? ?
? ?
? ? ?
? ?
I
I
?? ? ?
?? ? ? ?
??
???
?
???
?
?
?
??
??
?
? n
pi
i
i
p
iT
ik
n
pi
i
j
j
i
k k
T
i
i
jp
i
p q
Tmqq
q
T
TT r a c e
q
L rgI ??
1 1
2
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
系统的动力学方程为
i
n
j
n
k
kjijkiai
n
j
jij
n
j
i
i
i
iT
jm
n
j
j
k
k
j
m i
T
j
j
mk
i
iai
n
ij
j
k
k
i
T
j
j
k
j
i
DqqDqqD
q
T
mqq
q
T
T
T r a ce
qIq
q
T
q
T
T r a ceT
????
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ??
?? ? ?
? ?
? ??
?? ? ?
? ?
1 11
11 1 1
2
1
??????
??
????
I
rgI
I
)244( ?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
?
?
?
?
?
?
??
?
??
??
??
?
??
?
2
1
12
21
2 2 12 1 2
1 2 11 1 2
2
2
2
1
2 2 22 1 1
1 2 21 1 1
2
1
2221
1211
2
1
D
D
DD
DD
DD
DD
DD
DD
T
T
??
??
?
?
?
?
??
??
?
?
??
??
)104( ?
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的推导
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
1。惯量项的简化
利用
记微分旋转和平移为：
通过计算有：
为质心矢量,为与惯量相关的矩阵，具有如下形式。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
i
T
p
i
p pT
q
T ??
?
?
ipi
T
ipi
T ddpp ?? ??
? ?? ??
?
??????? 6
,m a x jip
j
p
j
p
j
p
i
p
p
p
j
p
i
p
j
p
p
T
i
p
pij ddddmD ???? rk
)314( ?
pk
ppr
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
1。惯量项的简化
当 i=j时，有
? ?? ??
?
???????
6
2
222222 2
ip
p
i
p
p
p
i
p
i
p
p z ziz
p
p y yiy
p
p x xix
p
pii dddkkkmD ???? r
)334( ?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
222
222
222
p z zp y zp x z
p y zp y yp x y
p x zp x yp x x
p
kkk
kkk
kkk
k
Robotics 动力学
4.2 机械手动力学方程
4.2.3 动力学方程的简化
2。重力项的简化
将 代入
i
T
p
i
p pT
q
T ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
6
6
,,m a x
2
6
,m a x
ip
p
p
i
pT
pij
kjip i
T
p
p
kj
p
ijk
jip i
T
p
p
j
p
ij
q
T
mD
q
T
T
T r a c eD
q
T
q
T
T r a c eD
rg
I
I
?
?
???
6
ip
p
p
i
p
p
T
pi TmD rg )364( ?
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
0100
2022
2022
1000
0100
1011
1011
2
2
2
1
2
1
1
1
sdcs
cdsc
TA
sdcs
cdsc
A
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
以 为基准，有
以 为基准，有
以 为基准，有
kjikjdid 10000 11111 ?????? ?
10T
21T
kjikjdid 10000 22222 ?????? ?
20T
kjikjddcidsd 1000)(2 22212122 ??????? ?
Robotics 动力学
4.3 机械手动力学方程实例
4.3.1 二连杆机械手动力学方程
设所有惯性力矩为零,为零。
有
此结果与 4.1.2中的一致。
2211,rr
? ?
? ?22212212
2
2222
2212
2
22
2
12111 2
dddcmD
dmD
cddmdmdmmD
??
?
????
? ?
12
121
222
221211
sgdmD
sgdmsgdmmD
?
???
Robotics 动力学
4.4 机械手动态特性
动态特性指：工作精度、重复能力、稳定性、空间分辨率
4.4.1 概述
快速、准确、定位的能力；
过块：精度差，（惯性影响）
过慢：效率低
对工业机器人，采用中继点来减少惯性影响。
Robotics 动力学
4.4 机械手动态特性
4.4.2 稳定性
稳定性指系统、装置或工具运动过程中，无振荡问题。
对机械系统 主要有系统的自激振动；
对电子系统,主要指其自激振荡；
对机器人系统,除上述两者外，还有其机电耦合振荡。如机械
手的抖动。
Robotics 动力学
4.4 机械手动态特性
4.4.3 空间分辨率
空间分辨率是描述机器人工具末端运动所达到的最小运动
增量。
Robotics 动力学
4.4 机械手动态特性
4.4.4 精度
用下列三个因素的集合来描述精度
1）各控制部件的分辨率；
2）各机械部件的偏差；
3）最近到达点距目标位置的差；
Robotics 动力学
4.4 机械手动态特性
4.4.4 精度
示教时，精度只与机械偏差有关；
离线编程时，与分辨率、机械偏差、接近能力有关。
Robotics 动力学
4.4 机械手动态特性
4.4.5 重复性
指机器人重复到达某一位置的能力。
Robotics 动力学
4.4 机械手动态特性
4.4.5 重复性
指机器人重复到达某一位置的能力。
几个概念的区别：
1）空间分辨率描述机器人所能控制的末端工具的最小运动增量
2）精度指一定分辨率下，机器人对某目标的接近能力；
3）重复性描述机器人重复到达同一目标时实际的位置误差；
4）一般重复性比精度好。
Robotics 动力学
4.5 机械手的静态负荷
解决：
1。静力和力矩的表示方法；
2。不同坐标系间，静负荷的变换；
3。确定机械手静态关节力矩；
4。由关节力矩确定机械手所载物体的重量。
Robotics 动力学
4.5 机械手的静态负荷
4.5.1 静力和力矩的表示方法
广义力 F 例：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
m
m
m
f
f
f
F
? ?01 0001 50010
01 000
1 50010
???
???
???
F
m
f
kji
kji
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
采用虚位移原理，研究静力作用在不同坐标系中的描述问题。
已知：在 A坐标系中的作用力表示为：
表示虚位移的微分运动矢量为：
已知 C坐标系到 A坐标系的坐标变换；
求：在 C坐标系中的作用力 CF 。
F所做虚功为：
在 C坐标系中做的虚功为：
? ?Tzyxzyx mmmfff?F
? ?zyxzyx ddd ????D
DF TW ??
DF cTcW ??
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
即：
由微分变换的等价关系式（ 3-97），有
或记为,
DFDF cTcT ?
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
c
y
c
x
c
z
c
y
c
x
c
d
d
d
aaa
ooo
nnn
aaa
ooo
nnn
d
d
d
?
?
?
?
?
?
000
000
000
apapap
opopop
npnpnp
JDD ?C
Robotics 动力学
4.5 机械手的静态负荷
4.5.2 不同坐标系间静力的变换
从而有
经变换有
FJFJFFJDFDFDF cTTcTTccTcT ????,,
? ? ? ? ? ?
? ? ? ? ? ?
? ? ? ? ? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
c
y
c
x
c
z
c
y
c
x
c
f
f
f
m
m
m
aaa
ooo
nnn
aaa
ooo
nnn
f
f
f
m
m
m
000
000
000
apapap
opopop
npnpnp
Robotics 动力学
4.5 机械手的静态负荷
4.5.3 关节力矩的确定
设作用于作用于机器人末端 T6杆上的力所做的虚功
各关节力 /力矩所做的虚功，
τ,广义力矩矢量（旋转、平移）；
dq:表示虚位移的微分变换。
两者相等：
由（ 3-111）
得
DF 66 TTTW ??
dqW T?? ?
dqTTTT ??DF 66
dqq )(JD ?
FJ 6TT??
Robotics 动力学
习题,P142,第 4.1题
