机器人技术陶建国哈尔滨工业大学机电学院
2005,2.
2
第六章 机器人静力学和动力学静力学和动力学分析，是机器人操作机设计和动态性能分析的基础。特别是动力学分析，它还是机器人控制器设计、
动态仿真的基础。
机器人静力学研究机器人静止或缓慢运动式，作用在机器人上的力和力矩问题。特别是当手端与环境接触时，各关节力（矩）与接触力的关系。
机器人动力学研究机器人运动与关节驱动力（矩）间的动态关系。描述这种动态关系的微分方程称为动力学模型。由于机器人结构的复杂性，其动力学模型也常常很复杂，因此很难实现基于机器人动力学模型的实时控制。然而高质量的控制应当基于被控对象的动态特性，因此，如何合理简化机器人动力学模型，使其适合于实时控制的要求，一直是机器人动力学研究者追求的目标。
3
6.1 机器人静力学一、杆件之间的静力传递在操作机中，任取两连杆,。设在杆 上的 点作用有力矩 和力 ；在杆 上作用有自重力 〔 过质心 ）； 和 分别为由 到 和 的向径。
iL 1iL? 1iL? 1i
O?
1iM? 1iF? iL iG
iC ir Cir iO 1i
O? iC
1iF?
1iM?
4
按静力学方法,把这些力,力矩简化到 的固联坐标系
,可得：
iL
i i i io x y z?
1
11
iii
i i i i C i i
F F G
M M r F r G
1 0
110
110
11 1 1 0
ii i i
ii i i
ii i i i i i i
iii i i i C i i
F R F R G
M R M r R F r R G
或式中 ( 为杆 的质量 )。0
iiG m g im iL
求出 和 在 轴上的分量，就得到了关节力和扭矩，
它们就是在忽略摩擦之后，驱动器为使操作机保持静力平衡所应提供的关节力或关节力矩，记作,其大小为
iF iM iz
i?
i
ikF
ikM
5
1
11
1 1 1
0
i i i
i i i
i i i i
i i i i i
F R F
M r R R M
当忽略杆件自重时,上式可简记为,
iG
若以 表示不计重力的关节力或力矩值,对于转动关节则有,
0i?
,0 ()j
n
iCi i ji
ji
k r G
jC
式中 ——是自 到杆 的质心 的向径 。
,jiCr iO jL
6
例 1 求两杆操作机的静关节力矩 (坐标系与结构尺寸如图 )。
解：
设已知
7
8
9
二、操作机的静力平衡设有操作机如图所示，每个关节都作用有关节力矩 (广义驱动力，指向 的正向 )，在末端执行器的参考点 处将产生力 和力矩 。由于,是操作机作用于外界对象的力和力矩，为了和输入关节力矩 一起进行运算，
故应取负值。
iz eP
i?
eF eM eF eM
i?
10
1,,,,Tin
,,,,,Te x e y e z e x e y e zQ F F F M M M
1,,,,Tinq q q q
,,,,,Te e e x y zp x y z
利用虚功原理建立静力平衡方程，令于是，操作机的总虚功是：
根据虚功原理，若系统处于平衡，则总虚功 (虚功之和 )为 0，
即
TTW q Q p
0TTq Q p
11
式中 J —— 是速度分析时引出的雅可比矩阵，其元素为相应的偏速度。
由机器人运动微分关系可知，,则有p J q
0TTJ Q q
因为 是独立坐标，则,所以有
0qiq
TJQ
上式是针对操作机的关节力和执行器参考点 间所产生的力和力矩之间的关系式。
eP
该式表明关节空间和直角坐标空间广义力可以借助于雅可比矩阵 J 进行变换。这种变换关系，也可推广到任两杆间固联直角坐标系中的广义力变幻，这时应将关节空间与直角坐标空间的雅可比矩阵，换作直角坐标空间的雅可比矩阵。
12
例 2 如图，操作机的手爪正在持板手扭某一哩栓，手爪上 方联接一测力传感器可测六维力向量 (力和力矩 )。试确定测力传感器和扭动板手时力和力矩的关系。
13
解：
设在测力传感器上置坐标系 Sf ( )，在螺栓上置坐标系 S ( ) 。在图示瞬间，两坐标系彼此平行。因为刚体的无限小位移 (平移和转动 )可表示为六维向量，故对二者的微位移可分别表示为：
由于两坐标系的坐标轴平行，于是可以得到：
fO uvw?
O xyz?
,,,,,x y zq x y z
,,,,,u v wp u v w
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
zy
zx
yx
u x
v y
zw
u xrr
v yrr
w zrr
p J q
14
前式也可以从前图直观求得。
设 为相应于 的广义力向量,为相应于 的广义力向量，则可得,
Q q P p
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
x u
y v
z w T
x zy u
zxy v
yx wz
F F
F F
F F
Q J P
M rr M
rrM M
rr MM
上式也可直接用虚功原理求得。
15
一,研究目的：
1,合理地确定各驱动单元 （ 以下称关节 ） 的电机功率 。
2,解决对伺服驱动系统的控制问题 （ 力控制 ）
在机器人处于不同位置图形 （ 位形 ） 时,各关节的有效惯量及耦合量都会发生变化 （ 时变的 ）,因此,加于各关节的驱动力也应是时变的,可由动力学方程给以确定 。
6-2 机器人动力学概述二,机器人动力学研究的问题可分为两类：
1,给定机器人的驱动力 （ 矩 ）,用动力学方程求解机器人 （ 关节 ） 的运动参数或动力学效应 （ 即已知,求 和,
称为动力学正问题 。 ） 。
2,给定机器人的运动要求,求应加于机器人上的驱动力
（ 矩 ） （ 即已知 和,求,称为动力学逆问题 ） 。
,
,
16
三,动力学研究方法：
1,拉格朗日方程法：通过动,势能变化与广义力的关系,建立机器人的动力学方程 。 代表人物 R.P.Paul,J.J.Uicker、
J.M.Hollerbach等 。 计算量 O(n4),经优化 O(n3),递推 O(n)。
2,牛顿 —欧拉方程法：用构件质心的平动和相对质心的转动表示机器人构件的运动,利用动静法建立基于牛顿 —欧拉方程的动力学方程 。 代表人物 Orin,Luh(陆养生 )等 。 计算量 O(n)。
3,高斯原理法,利用力学中的高斯最小约束原理,把机器人动力学问题化成极值问题求解,代表人物波波夫 (苏 ),用以解决第二类问题 。 计算量 O(n3)。
4,凯恩方程法：引入偏速度概念,应用矢量分析建立动力学方程 。 该方法在求构件的速度,加速度及关节驱动力时,只进行一次由基础到末杆的推导,即可求出关节驱动力,其间不必求关节的约束力,具有完整的结构,也适用于闭链机器人 。 计算量 O(n!)。
17
系统的动能和势能可在任何坐标系（极坐标系、圆柱坐标系等）中表示,不是一定在直角坐标系中 。
动力学方程为：
广义力 广义速度 广义坐标
（力或力矩）（ 或 ） （ 或 ）
i
ii
d L L
d t q q?
v? d
6.3 二杆机器人的拉格朗日方程应用质点系的 拉格朗日 方程来处理杆系的问题。
定义,L=K-P
L— Lagrange函数； K—系统动能之和； P—系统势能之和 。
6.3.1 刚体系统拉格朗日方程
18
设二杆机器人臂杆长度分别为,质量分别集中在端点为,坐标系选取如图 。2,1 dd
2,1 mm
以下分别计算方程中各项：
一、动能和势能
2
2
1 mvK? mghp?
对质点,
1m
2 2 2
1 1 1 1 1 1 1 1 1
1 1 1()
2 2 2k m v m d m d
势能：
动能：
1 1 1 1c o s ( )p m g d
（ 负号与坐标系建立有关 ）
对质点,
2m 先写出直角坐标表达式：
)c o s ()c o s (
)s i n ()s i n (
212112
212112
ddy
ddx
6.3.2 刚体系统拉格朗日方程
1?
1?
19
对 求导得速度分量：x
)2121)(2c o s(212)2221221(222121222222
)21)(21si n (21)1si n (12
)21)(21c o s(21)1c o s(12
ddddyxv
ddy
ddx
动能：
)2121)(2c o s (212)2221221(22222121212212 ddmdmdmK
)21c o s (22)1c o s (122 gdmgdmP
势能：
二,Lagrange函数
1 2 1 2( ) ( )L K P k k p p
2 2 2 2 2 2
1 2 1 1 2 2 1 1 2 2 2 1 2 2 1 1 2
11( ) ( 2 ) c os( ) ( )
22m m d m d m d d
1 2 1 1 2 2 1 2( ) ( ) c o s ( )m m g d s m g d
),,,( 2121L?
20
三、动力学方程先求第一个关节上的力矩
2221212212222212222121211 )c o s()c o s(2)( ddmddmdmdmdmmL
222122221221222221211 )]c o s([)]c o s(2)[( ddmdmddmdmdmmLdtd
222212212212 )s i n ()s i n (2 ddmddm
)si n ()si n ()( 212211211 gdmgdmmL
2 2 21 2 1 2 2 2 1 2 2 1 2 2 2 1 2 2 2[ ( ) 2 c o s ( ) ] [ c o s ( ) ]m m d m d m d d m d m d d
LL
dt
d )(
11?
22 1 2 2 1 2 2 1 2 2 2 1 2 1 1 2 2 1 22 s i n ( ) s i n ( ) ( ) s i n ( ) s i n ( )m d d m d d m m g d m g d
—— （ 1）
1
1 1 1 1
( ( ) ( ) )d L L d L Ldt q q dt1?
21
同理，对 和 微分，可求得第二关节力矩2 2?
12212222212222 )c o s( ddmdmdmL
21221212212222212222 )si n ()c o s( ddmddmdmdmLdtd
)si n ())(si n ( 2122212122122 gdmddmL
222 )(
LL
dt
d?
)s i n ()s i n ()]c o s ([ 2122212212222212212222 gdmddmdmddmdm
以上是两杆机器人动力学模型。
—— （ 2）
22
系数 D 的物理意义：
—关节 的有效惯量（等效转动惯量的概念）。由关节处的加速度 引起的关节 处的力矩为 （ ）
iiD
ii
i
iiiDJMi?
i
—关节 和 之间的耦合惯量 。 由关节 或 的加速度
（ 或 ）所引起的关节 和 处的力矩为 或
ijD i j j
i j j
i
iijD jiji
—向心力项系数。表示关节 处的速度作用在关节 处的向心力（ ）
ijjD ji
2jijjD
—向心力项系数。表示关节 处的速度作用在本身关节处的向心力（ ）
iiiD i
2iiiiD
四,动力学方程中各系数的物理意义将前面结果重新写成简单的形式,
221 1 1 1 1 2 2 1 1 1 1 1 2 2 2 1 1 2 2 1D D D D D D
222 2 1 1 2 2 2 2 1 1 1 2 2 2 2 2 1 2 1 2 2 2 1 2 1 2D D D D D D D
23
—哥氏力项系数。 两项组合为关节 与处的速度作用在关节 处的哥氏力，哥氏力是由于牵连运动是转动造成的。
ijkD jkijkkjijk DD
j k
i
—关节 处的重力项 。 重力项只与 大小、长度 以及机构的结构图形（ ）有关。
iD i m d
21,
比较二杆机器人例中的系数与一般表达式中的系数得到有效惯量系数：
221 1 1 2 1 2 2 2 1 2 2[ ( ) 2 c o s ( ) ]D m m d m d m d d
22 2 2 2D m d?
耦合惯量系数：
21 2 2 1 2 2 2 1 2 2c o s ( )D D m d m d d
24
向心力项系数：
0
)si n (
)si n (
222
2212211
2212122
111
D
ddmD
ddmD
DD
哥氏力项系数：
0
)s i n (2
221212
2212121112
DD
ddmDD?
重力项：
1 1 2 1 1 2 2 1 2( ) sin ( ) sin ( )D m m g d m g d
2 2 2 1 2s in ( )D m g d
25
6.4 机器人的拉格朗日方程 的一般表达形式从上节容易看出 Lagrange方程是一个二阶耦合、非线性和微分方程，为简化计算，未虑及传动链中的摩擦。以下方程的推导，也是不考虑传动链带来的摩擦影响，只考虑杆件本身，然后再加入关节处驱动装置（如电机、码盘等）的影响。
推导分五步进行：
一、计算任意任意杆件上任意点的速度；
二、计算动能 ；
三、计算势能 ；
四、形成 Lagrange函数；
五、建立动力学方程 。
21
2i i ik m v?
i i ip m g h?
()i
ii
d L LF
d t q q
26
其速度为：
一、点的速度
r
由于整个系统的动能都是在基础系中考虑的，故需求系统各质点在基础坐标系中的速度 。
r
对于杆 坐标系中的一点,它在基础坐标系中的位置为i
ir
iir T r
式中 —变换矩阵
iT
1
()
i
i
iii
j
j
j
d T r Tdr
r q r
d t d t q
速度平方为：
22( ) ( ) ( )r r r T r a c e r r
式中 —矩阵的迹，即矩阵主对角元素之和。()T race
27
2( ) ( )r Tr a c e r r
11
()
ii
i i Tii
jk
jk
jk
TTT r a c e q r q r
qq
11
()
ii
TT
iiii
jk
jk
jk
TTT r a c e r r q q
qq
二、动能位于杆 上 处质量为 的质点的动能是：i ir dm
11
1 ()
2
ii
TT
iiii
i j k
jk
jk
TTd k T r a c e r r q q d m
qq
11
1 ()
2
ii
TT
iiii
jk
jk
jk
TTT r a c e r d m r q q
qq
28
则杆 的动能（在基础坐标系中）为：i
11..
1 ()
2
ii
TT
iiii
i i j k
jk
jkli n k i li n k i
TTk dk T rac e r r dm q q
qq
令式中 称为连杆 的伪惯量矩阵。i
.
Tii
i
lin k i
J r r d m
11
1
2
ii
T
ii
i i j k
jk
jk
TTk T r a c e J q q
qq
则得到杆 的动能为：i
对于杆 上任意一点的 （在杆 坐标系中）可以表示为：i ir i
(,,,1 )i i i ir x y z?
29
2
.,,,
2
.,,,
2
.
.,,,
..
i i i i i i
l i nk i l i nk i l i nk i l i nk i
i i i i i i
T
l i nk i l i nk i l i nk i l i nk iii
i
i i i i i i
l i nk i
l i nk i l i nk i l i nk i l i nk i
ii
l i nk i l i nk i
x dm x y dm x zdm x dm
x y dm y dm y zdm y dm
J r r dm
x zdm y zdm z dm zdm
x dm y dm
..
i
l i nk i l i nk i
zdm dm
根据理论力学中惯性矩、惯性积和静矩的定义，引入下列记号：
2 2 2 2 2 2( ),( ),( )
x x y y zzI y z dm I x z dm I x y dm
对坐标轴的惯性矩：
则有：
30
对坐标轴的惯性积：
,,x y y z x zI x y dm I y zd m I x zd m
对坐标轴的静矩：
,,x y zI x dm I y dm I zdm
质量之和：
m dm
于是：
2 2 2 2 2 2 21 1 1( ) ( ) ( )
2 2 2x d m y z d m x z d m x y d m
2
x x y y z zI I I
x
z
y
r
31
同理：
22,
22
x x y y zz x x y y zzI I I I I Iy dm z dm
2
2
2
i x x i y y i zz
i x y i x z i x
i x x i y y i zz
i x y i y z i y
i
i x x i y y i zz
i x z i y z i z
i x i y i z i
I I I
I I I
I I I
I I I
J
I I I
I I I
I I I m
于是 能够表达为：
iJ
机器人臂杆总的动能是：
1 1 1 1
1
2
n n i i
T
ii
i i j k
jk
i i j k
TTK k T r a c e J q q
qq
32
如果考虑到关节处驱动装置的动能：
2
.
1
2 im ot or i a ik J q?
调换求迹与求和运算顺序，并加入关节处驱动装置的动能，
得到机器人总的动能为：
2
1 1 1 1
11 ()
22 i
n i i n
T
ii
i j k a i
jk
i j k i
TTK T r a c e J q q J q
qq
2
.
1
2 im o t o r i a ik m q?
（对于移动关节,）
式中 为关节 处驱动装置的转动惯量。
aiJ i
三、势能设杆 的质心在再其自身坐标系的位置向量为,则它在基础坐标系中的位置向量 为
i i Cr
Cr
iC i Cr T r (,,,1 )Ti i i iC C C Cr x y z
33
设重力加速度 在基础坐标系中的齐次分量为：
T T ii i C i i Cp m g r m g T r
于是机器人的总势能为：
g
(,,,1 )Tx y zg g g g?
则杆在基础坐标系中的势能为：
（一般认为基础坐标系的 z 轴取向上方）
11
nn
Ti
i i i C
ii
P p m g T r
34
先求拉格朗日方程中的各项：
11
1 ()
2
ni
T
ii
ik
p p k
ik
TTL T r ace J q
q q q
四、拉格朗日函数
1 1 1
1 ()
2
n i i
T
ii
i j k
jk
i j k
TTL K P T r a c e J q q
qq
2
11
1
2
nn
Ti
ai i i i C
ii
J q m g T r
五、动力学方程
11
1 ()
2 p
ni
T
ii
i j a p
jp
ij
TTT r a c e J q I q
qq
（ 1 )
35
由于 是对称矩阵，则有：
合并 (a)式中前两项，得到：
iJ
( ) ( )
TT
Ti i i i
ii
p k p k
T T T TTr a c e J Tr a c e J
q q q q
()
T
Tii
i
kp
TTT ra c e J
qq
()
T
ii
i
kp
TTTr a c e J
qq
11
()
p
ni
T
ii
i k a p
p k p
ik
TTL T r ace J q I q
q q q
（ 1’)
当 时,中不包含 以后关节变量，即：ip?
iT
p
0,i
p
T
q
()ip?
于是可得：
1
()
p
ni
T
ii
i k a p
p k p
i p k
TTL T r a c e J q I q
q q q
36
2
11
()
n i i
T
ii
i k m
k m p
i p k m
TTT r a c e J q q
q q q
（ 2 )
1
( ) ( )
p
ni
T
ii
i k a p
p k p
i p k
TTdL T r a c e J q I q
d t q q q
2
11
()
n i i
T
ii
i k m
p m k
i p k m
TTT r a c e J q q
q q q
1
( ) ( )
p
ni
T
ii
i k a p
p k p
i p k
TTdL T r a c e J q I q
d t q q q
交换其中的部分哑元，得到：
2
11
2 ( )
n i i
T
ii
i k m
k m p
i p k m
TTT r a c e J q q
q q q
37
2
11
1 ()
2
n i i
T
ii
i j k
p j p k
i p j k
TTL T r a c e J q q
q q q q
2
11
1 ()
2
n i i
T
ii
i j k
k p j
i p j k
TTT r a c e J q q
q q q
n
Ti i
iC
p
ip
Tm g r
q
2
11
()
n i i
T
ii
i j k
j p k
i p j k
TTT r a c e J q q
q q q
n
Ti i
iC
p
ip
Tm g r
q
（ 3 )
38
将以上各项带入拉格朗日公式，并用 和 分别代替上式中的哑元 和,得到,i jip
1
()
i
jn
T
jj
i j k a i
ki
j i k
TT
T r a c e J q I q
qq
2
11
()
jjn
T
jj
j k m
k m i
j i k m
TT
T r a c e J q q
q q q
n
jTi
jC
i
ji
T
m g r
q
上式为拉格朗日方程的最后形式。这些方程与求和的次序无关，因此可将上式写为简化形式：
（ 5 )
1 1 1
i
n n n
i i j j a i i j k j k i
j j k
D q I q D q q D?
（ 4 )
39
式中：
m a x (,)
()
n T
pp
i j p
jj
p i j
TT
D T r a c e J
qq
()
n
pTp
i p C
i
pi
T
D m g r
q
2
m a x (,,)
()
n T
pp
i j k p
j k i
p i j k
TT
D T r a c e J
q q q
以上的动力学方程 (5)中系数 D的意义与上节所列相同，
即分别为有效惯量项系数 ( ),耦合惯量项系数（ ），
向心力项系数（ ），哥氏力项系数（ ），重力项等。
ij? ij?
kj? kj?
ij?
40
动力学方程中的惯量项和重力项在机器人控制重特别重要，
将直接系统的稳定性和定位精度。只有当机器人高速运动时，
向心力项和哥氏力项才是重要的。传动装置的惯量值往往较大，对系统动态特性的影响也不可忽略。
在机器人动力学问题的讨论中，拉格朗日动力学方程常写作更简化的一般形式：
( ) ( ( ) ) ( ) ( ( ),( ) ) ( ( ) )t D q t q t h q t q t c q t
式中：
12( ) ( ( ),( ),,( ),) Tnt t t t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
( ( ) ),( ( ),( ) ),( ( ) )D q t h q t q t c q t的意义见（ 5 )式。
（ 6)
41
乘法次数,4 3 21 2 8 / 3 5 1 2 / 3 8 4 4 / 3 7 6 / 3n n n n
6.5 机器人的 牛顿 —欧拉方程机器人的拉格朗日动力学模型为非线性二阶常微分方程,
利用这些方程,由已知的每一轨迹设定点的关节位置,速度和加速度,可以计算各关节的标称力矩,但拉格朗日方程利用 4× 4齐次变换矩阵,使得计算效率太低 。
4 3 29 8 / 3 7 8 1 / 6 6 3 7 / 3 1 0 7 / 6n n n n加法次数：
为了实现实时控制,曾用过略去哥氏力和向心力的简化模型,但当操作机快速运动时,哥氏力和向心力在计算关节力矩中是相当重要的 。 因而这种简化只能用于机器人的低速运动,在典型的制造业环境中,这是不合乎要求的 。 此外,这种简化所引起的关节力矩误差,不能用反馈控制校正 。
牛顿 —欧位法采用迭代形式方程,计算速度快,可用于实时控制,因而成为一种常用的建模方法 。
42
寻求转动坐标系和固定惯性坐标系之间必要的数学关系,再推广到运动坐标系 (转动和平移 )和惯性坐标系之间的关系。
如图,惯性坐标系 O-XYZ和转动坐标系 O-X*Y* Z*的原点重合于 O点。而 OX*,OY*,OZ*轴 相对 OX,OY,OZ轴旋转。设和 分别为这两个坐标系沿主轴的单位矢量。转动坐标系中点 P 可用它在任一坐标系中的分量来表示：
6.5.1 转动坐标系
(,,)i j k (,,)i j k
r x i y j zk
或
r x i y j z k
在惯性坐标系中的运动：
d r d x d y d zi j k x i y j z k
d t d t d t d t
Y
X
Z
r
Y*
X*
Z*
O
P
在转动坐标系中的运动：
d r d x d y d zi j k x i y j z k
d t d t d t d t
43
在惯性坐标系中的运动：
d r d x d y d z d i d j d ki j k x y z
d t d t d t d t d t d t d t
d r d i d j d kx y z
d t d t d t d t
（ 7 )
需要解决转动坐标系坐标轴在惯性坐标系中的导数问题。我们假定，转动坐标系绕着过原点 O的某轴 OQ以角速度 旋转。
方向沿 OQ轴，指向转动坐标系右旋方向。则可以证明转动坐标系中的任意固定矢量 在惯性坐标系中的导数为：
s
X
Z
Y
s
Y*
X*
Z*
O
Q
ds s
dt
44
于是由 （ 6 )式可得：
( ) ( ) ( )d r d r x i y j z kd t d t
d r d r r
d t d t?
这是建立转动坐标系两种时间导数之间关系的基本方程。
2
2 ()
d r d d r d r d r
d t d t d t d tdt
2
2 ()
d r d r d r drr
d t d t d tdt
22
22 2 ( )
d r d r d r drr
d t d td t d t
方程 （ 9 )又被称为哥氏定理。
（ 8)
（ 9 )
45
6.5.2 运动坐标系如图,运动坐标系 O-X*Y* Z*相对于惯性坐标系 O-XYZ转动和平移。质量为 M 的质点 P分别以 和 确定相对于惯性坐标系和运动坐标系的原点的位置。原点 O* 相对于原点 O的位置以矢量 表示。则有：
r r?
h
r r h
( ) ( )kd r d r d hv t v vd t d t d t
() d r d hv t rd t d t?
22
22
()( ) ( )
k
d v t d r d ha t a a
dt d t d t
22
2 ( )d r d r d d hrrd t d td t d t
r
YX
Z
O
P
Y*
X*
Z* O*
r*
h
（ 10 )
（ 11 )
46
6.5.3 杆件运动学根据前述运动坐标系的概念，推导一组数学方程，描述机器人的运动杆件相对于基础坐标系的运动学关系。
令 和 分别为坐标系 相对于基础坐标系 的线速度和角速度。令 和 分别为杆件 i
坐标系 相对于基础坐标系 和杆件 i -1
坐标系 的角速度。则杆件 i 坐标系 相对于基础坐标系的线速度 和角速度 分别是：
1iv? 1i 1 1 1(,,)i i ix y z
0 0 0(,,)x y z i? i
(,,)i i ix y z 0 0 0(,,)x y z
1 1 1(,,)i i ix y z
iv
i?
坐标系 是基础坐标系，而坐标系和 分别固联于杆件 i -1和杆件 i 上，原点分别为
Oi-1和 Oi 。原点 Oi 相对于原点 O 和原点 Oi-1 的位置分别用位置矢量 和 表示。原点 Oi-1相对于基础坐标系原点 O
的位置用位置矢量 表示。
0 0 0(,,)x y z
(,,)i i ix y z
ip ip?
1ip
1 1 1(,,)i i ix y z
47
11
i
i i i i
dpv p v
dt?
1i i i
式中 d*(.)／ dt 表示在 运动坐标系 的时间导数。
1 1 1(,,)i i ix y z
（ 12 )
（ 13 )
48
2
1 1 1 1 12 2 ( )
ii
i i i i i i i i
d p d pv p p v
dtdt
1i i i
1
ii
i i i
dd
d t d t
11
i
i i i i
d
dt
（ 14 )
（ 15 )
为坐标系 相对于 的角加速度,(,,)
i i ix y z 1 1 1(,,)i i ix y zi
根据机器人杆件坐标系建立的步骤和参数的定义，杆件 i 在杆件 i –1 坐标系中的运动是沿 方向的平移或绕 转动。
因此，
1iz? 1iz?
式中 是杆件 i 相对于杆件 i -1 坐标系的角速度值。q
i
1iizq?
0
若杆件 i 转动若杆件 i 平移 （ 16 )
49
（ 18 )
类似地 （ 17 )
id
dt
1iizq?
0
若杆件 i 转动若杆件 i 平移由式 （ 13 )和 式 （ 16 )有：
11i i izq
i
1i
若杆件 i 转动若杆件 i 平移
1 1 1 1()i i i i i iz q z q
i
1i
若杆件 i 转动若杆件 i 平移 （ 19 )
2
2
idp
dt
1iizq?
()i i i i id ppdt
若杆件 i 转动若杆件 i 平移由式 （ 15 ),式 （ 16 )和 式 （ 17 )有：
idp
dt
1iizq?
iip
若杆件 i 转动若杆件 i 平移
（ 20 )
（ 21 )
由式 （ 15 ),式 （ 16 )和 式 （ 17 )有：
50
( ) ( ) ( )a b c b a c a b c
（ 22 )
iv?
1i i ipv
11i i i i iz q p v
若杆件 i 转动若杆件 i 平移由式 （ 12 ),式 （ 13 )和 式 （ 20 )有：
利用矢量叉乘积的恒等式
( ) ( ) ( )a b c b a c c a b
并根据式 （ 14 ),式 （ 15 )和 式 （ 19 )有：
iv?
1()i i i i i ip p v
1 1 12 ( ) ( )i i i i i i i i iz q p z q p v
—（ 23 )
若杆件 i 转动若杆件 i 平移
51
刚体的运动可分解为随质心的移动和绕质心的转动。借助于杆件运动学知识，我们把达朗贝尔原理用于每个杆件，描述机器人各杆件的运动。达朗贝尔原理可应用于任意瞬时，它实质上是牛顿第二运动定律的一种变型，可表示为：
6.5.4 牛顿 ——欧拉法基本运动方程
i i iF m v?
牛顿定理,
欧拉方程,()
i i i i i iN I I
式中,— 杆 i 质量；
— 杆 i上所有外力合力；
— 杆 i上所有外力对质心的合力矩；
— 杆 i 绕其质心惯性矩阵 。
im
iF
iN
iI
()ii
i
d m vF
dt?
()ii
i
dIN
dt
52
1,,1i i i i i iF f f m g
1,,1,,1 1,1,( ) ( )i i i i i i c i i i i c i i iN N N r f r f
根据力（矩）平衡原理，在质心处有：
则有
1,,1 0i i i i i i if f m g m v
1,,1,,1 1,1,( ) ( ) ( ) 0i i i i i c i i i i c i i i i i i i iN N r f r f I I
（ 24)
方程 （ 24 )即为牛顿 ——欧拉法的基本方程。
,1iif?
,1iiN?
1,iif? 1,iiN?
img
iz
1iz?
,iiCr
io
iy
ix
1iy?
1ix?
1io?
53
上面推导的牛顿 ——欧拉法 （ 也简称 N-E法 ） 方程式含关节联接的约束力 （ 矩 ）,没有显示地表示输入 —输出关系,不适合进行动力学分析和控制器设计 。 如果变换成由一组完备且独立的位置变量 （ 质心位置变量通常不是相互独立的 ） 和输入力来描述,这些变量都显式地出现在动力学方程中,即得到 显式的输入 ——输出形式表示的动力学方程,称为封闭形式的动力学方程 （ 拉格朗日方程即是封闭的 ） 。
6.5.5 递推形式的牛顿 ——欧拉方程关节变量 是一组完备且独立的变量,关节力 （ 矩 ） 是一组从约束力 （ 矩 ） 中分解出来的独立的输入,所以用 和来描述方程,可以得到封闭形式的动力学方程 。
q?
q?
54
根据 N-E法的基本方程,利用质心运动变量与关节变量及关节运动变量之间的关系以及约束力与关节力矩之间的关系,消去中间变量,可以得到封闭形式的动力学方程 。 但显然不如用拉格朗日法简单,特别是当机器人自由度较多时,更是如此 。
因此,对于 N-E法,常用的不是它的封闭形式方程,而是它的递推形式方程 。 方程 （ 24） 可直接写成如下递推形式：
1,,1i i i i i i if f m v m g
1,,1,,1 1,1,( ) 0i i i i i c i i i i c i i i i i i i iN N r f r f I I
（ 25)
而关节力（矩）可写成如下形式：
1 1,
1 1,
Ti i i i i
i T
i i i i i
z N b q
z f b q
对于旋转关节对于移动关节
（ 26)
式中,为沿关节轴线 的单位矢量,
为关节的粘滞阻尼系数 。
1iz
ib
1?iz
55
递推形式的 N-E法方程 与封闭形式方程比较,计算量从减少到,
乘法次数,117n– 24，
加法次数,103n- 21，
从而大大加快了计算速度 。 自由度越多,递推形式的优势越明显 。 对于典型 n=6 的情形,递推形式的计算效率几乎提高 10倍 。
因此,常用于实时计算 。
递推形式 方程 的特点是其计算从机器人操作机的一个杆到另一杆逐个顺序进行的,它充分利用了操作机的串联链特性,常用于求解动力学逆问题 （ 即已知,求 ） 。
求解的大致过程为：根据运动和力的不同传递方向,进行运动量的向前迭代和力学量的向后迭代 。
具体步骤如下：
4()On
()On
,,?
56
n
1n?2
1
0
n
,1nnf
,1nnN
动力学计算运动学计算
2 2 2 21,1 1 1 1 1 1
3331 2 2
,,,,,,,,,,
,,,,
cccc
c n c n n n
v v w wq q q v v w w v v w w
qqqq q q
1,确定计算 N-E方程所需的所有运动量,包括每个杆件的
（ ） 由杆 1 杆 n：
1,,1,,1 1,1,1,2,1 0,1()n n n n n c i n n n c i n i n n n n n n n n nN N r f r f I I N N N
1,,1 1,2,1 0,1n n n n n n c n n n n nf f m g m v f f f
i?
2,将上述运动量代入 N-E方程,确定关节力 （ 矩 ） 。 计算顺序与运动量计算相反,由 杆 n 杆 1：
iciici wvwv,,,
57
前述递推运动方程的明显缺点是所有惯性矩阵和物理几何参数 （ 如 ） 等,都是以基础坐系为参照的,因此,
当机器人运动时,它们也随着变化 。 Luh等人改进了上述 N-E
方程,将所有杆件的速度,加速度,惯性矩阵,质心位置,
力和力矩等,都表示在各杆的自身坐标系中,从而使计算更加简单 。
这种改进的最重要的成果是,计算关节驱动力矩的时间不仅与机器人关节数成线性比例,而且与机器人构型无关 。 这就有可能在关节变量空间实现机器人的实时控制算法 。
6.5.6 在杆件自身坐标系中的递推方程
iiicii zrr?,,1,,?
设 是 3× 3旋转矩阵,它把矢量由坐标系 变换到坐标系 中 。
1i iR? ),,(
iii zyx
1 1 1(,,)i i ix y z
58
这样,可不计算相对基础坐标系的 和等,而是直接计算在杆件自身坐标系中的和 等 。
于是有关运动量的递推公式变为：
1,,,,,,i i i i c i i iv v v f 1,iiN?
1,,,,,,i i i i i ii i i i c i i iv v v f
1,i iiN?
,,()i i i i i i ic i i i i c i i i i c iv v r r
1
1 1
1 1
()i i ii i i i i
i ii
ii
R q z
R
若杆件 i 转动若杆件 i 平移
1
1 11
1 1
[ ( ) ]i i i i ii i i i i i i i
i ii
ii
R q z q z
R
若杆件 i 转动若杆件 i 平移
1 1 1 1 1 1
1,1 1 1,1
1 1 1 1 1 1
1 1 1,1 1 1
[]
( ) 2 ( )
i i i i i i i
i i i i i i i i i
i i i i i i i i i
i i i i i i i i i i i i
R v r r
v R v q z r q R z
1 1 11 1,1()i i ii i i ir
若杆件 i 转动若杆件 i 平移
59
11,1,1i i i ii i i i i i c if R f m v
1 1 11,1,1 1,1 1,,( ) ( ) ( )i i i i i i i ii i i i i i i i i i i c i i c iN R N r f r r m v
1
1,1 1
1
1,1 1
()
()
i T i i
i i i i
i i T i i
i i i i
N R z
f R z
关节间约束力公式变为：
()i i ii i i i iII
因此，概括地说，高效的牛顿一欧拉运动方程是一组正向和反向的递推方程，每一杆件的动力学和运动学参数都是以其自身坐标系为参照的。
60
6.6 机器人的 凯恩方程法简介凯恩 (Kane)方法是用来建立机器人机构动力学模型的一种普遍方法，其基本思想是以广义速率代替广义坐标作为系统的独立变量，它用达朗倍尔原理及虚位移原理建立动力学方程。凯恩方法既适合于完整系统，也适合于非完整系统。
6.6.1 广义速率和偏速度及偏角速度一、广义速率一个具有 n 个自由度的完整系统，相对于惯性坐标系的运动一般通过 n 个独立的广义坐标来描述 。 n 个广义速度 也是独立的，故可用
n 个广义速度的线性组合，即 n 个广义速率 (或称准速度 )
来描述系统的运动，即
0 0 0(,,)x y z ( 1,2,,)iq i n
( 1,2,,)iq i n
( 1,2,,)iu i n
61
1
( 1,2,,)
n
i ri i r
i
u a q a r n
式中,为 及 t 的函数；而由 组成的系数矩阵应为非奇异阵。则有,ri raa
iq ria
二、偏速度系统中任意质点 p 的径矢为 为广义坐标 及 t 的函数，
,rr iq
12(,,,,)pnr r q q q t则该点的速度为
1 1 1 1
()
n n n n
p p i i r r i
i i i
i r i i
r r r r rv r q u b
q t q q t
1
( 1,2,,)
n
i i r i i
r
q u b i n?
（ 27)
（ 28)
62
令
1
()
n
p r r t
r
v v r u v
其中
1
()
n
r ir
i
i
rvr
q
1
()
n
ti
i
i
rrv r b
qt
称广义速率 前的系数矢量 为 p点相对于惯性坐标系的第 r 偏速度。一般说来它是广义坐标 和时间 t 的函数。对于定常系统,偏速度只是广义坐标的矢量函数。
()rvrru
iq
0tv?
（ 29)
（ 31)
（ 30)
三、偏角速度
1 1 2 2 3 3
1
()
n
r r t
r
e e e e u
63
从式 (30) 可见，广义速率的取法不同，系统内同一质点及同一刚体的偏速度也可不同。故可通过一定的技巧来选取广义速率，使得到的偏速度和偏角速度具有最简单的形式。最理想的是使较多的偏速度和偏角速度等于零，这样有利于简化动力学方程。
一般选择与系统的运动密切相关的量，如选取系统中刚体的角速度分量或质点的速度分量为广义速率，以使刚体的角速度和质点的速度具有最简单的表达形式。作为特例，如果取广义速度为广义速率，即
iiuq?
12
12
n
n
d r r r r rv q q q
d t q q q t
可见对于广义速度 的偏速度为 。
iq
i
r
q
64
6.6.2 凯恩动力学方程四、刚体各点偏速度
( ) ( )rrp C r pv r v r r
由达朗倍尔原理和虚位移原理推得的系统的动力学普遍方程：
( ) 0p p p p
p
F m r r
系统中 p 点的速度，对于具有 n 个自由度的系统，可写成广义速率的线性组合,即
1
()
rt
n
p
p rp r tp
r
dr
v v r u v
dt
（ 32)
（ 33)
65
11
( ) ( )
r t r t
nn
p rp r tp rp r tp
rr
dr v r u dt v dt v r d v dt?
式中 。如果 可积分，则 为另一种定义的广义坐标。 rrd u dt
ru
r?
（ 34)
从式 (34) 可得到 的变分,即
pr pr?
1
()
r
n
p rp r
r
r v r
将上式代入式 (33) 得
1
( ) ( ) 0
r
n
p p p rp r
pr
F m r v r
66
改变求和的形式得
1
( ) ( ) 0
rr
n
p rp p p rp r
r p p
F v r m r v r
（ 34)
令
()
rr p rp
p
F F v r
()
rr p p rp
p
F m r v r
称为系统对应于 的广义主动力
ru
称为系统对应于 的广义惯性力
ru
67
这样，式（ 34）成为：
1
0
n
r r r
r
FF
由于 为独立的变分，所以有
r
0 1,2,,rrF F r n
（ 35)
上式称为凯恩动力学方程，意为广义上动力与广义惯性力之相等于零。
凯恩方程法 可以得到封闭形式的动力学方程。
68
6.7 弹性机器人 动力学简介
6.7.1 机器人系统的弹性问题通常，机器人机构的手臀、驱动、传动元件被假设为刚性的，故系统的建模及控制方案设计都是以这样一个刚性假设为前提的。结构刚性越好越不易振动，机器人精度越高，经过近似处理的控制方法也越精确、可行。
随着工业的发展，对机器人性能要求越来越高。如：要求机器人重量轻以降低能耗；运行速度快，以提高劳动生产率；
定位精度高，以适应更多的精密作业要求。另外，对于如太空机器人所处的特殊环境要求具有超长手臂的机器人，其结构已不再是刚性结构，而必须计入曾被忽略的系统弹性或非线性因素，这便使系统设计、建模及控制变得更为复杂。
69
关于具有弹性的机器入系统的建模与控制问题的研究历史不长，始于 70年代初,研究具有弹性附件的飞机动力学模型的动力学分析问题，计入分布质量和柔性效应的机器人机构在低速运动下的动力学，对具有分布质量和弹性影响的二连杆机械手提出反馈控制方法。近年来，关于这方面的研究发展很快，大多以研究单弹性臂机器人的建模与控制入手。
本节将以单弹性臂机器人机构为例，介绍弹性手臂机器人的建模方法。
机器人系统的弹性主要来源于两个方面，①由于臂本身的弹性而引起的变形及运行过程中的振动。②由驱动系统的弹性及非线性因素而引起的振动。
在机器人驱动系统中，存在着轴与轴承之间的库仑摩擦，
传动装置中的齿轮间隙及谐波齿轮减速器的弹性等因素。
6.7.2 机器人机构的弹性及非线性问题
70
一、建模方法。
弹性手臂机器人是一分布参数系统，其运动特性由一组非线性偏微分方程描述。在系统仿真和应用设计时必须作有限维的近似，并在不影响系统稳定性性态的前提下化为可实际控制的系统状态方程。
解决弹性手臂机器人的建模问题的各种建模方法，在不同程度上部有一定的近似性。准确性及可行性是衡量一种方法好坏的标准，就现有文献资料大致可归纳为以下几种：
（ 1） 拉格朗日方程结合模态展开法；
（ 2） 拉格朗日方程结合有限元素法；
（ 3） 牛顿一欧拉方程结合模态展开法；
（ 4） 牛顿一欧拉方程结合有限元素法；
（ 5） 哈密顿原理结合模态分析法。
6.7.3 主要研究内容
71
二、控制方程现在应用于机器人控制中的最佳控制法、极点配置法均以系统不产生振荡为前提，而弹性手臂机器人系统对其控制器的要求就是要能控制其弹性振动。因而有效可行的控制方法的研究很有必要。
对控制方案的要求：
（ 1） 必须同时控制刚体运动及弹性振动而控制输出量仅作用于关节处；
（ 2） 避免溢漏现象；
（ 3） 控制方案所要求的传感器数量应尽可能少，且安装简单、方便。
三、带有弹性构件的新型机械结构。
72
设 T1为驱动部分动能,T2为手臂部分动能,T3为负载动能，则
6.7.4 单弹性臂机构的建模
73
()i
ii
d L LF
d t q q
2005,2.
2
第六章 机器人静力学和动力学静力学和动力学分析，是机器人操作机设计和动态性能分析的基础。特别是动力学分析，它还是机器人控制器设计、
动态仿真的基础。
机器人静力学研究机器人静止或缓慢运动式，作用在机器人上的力和力矩问题。特别是当手端与环境接触时，各关节力（矩）与接触力的关系。
机器人动力学研究机器人运动与关节驱动力（矩）间的动态关系。描述这种动态关系的微分方程称为动力学模型。由于机器人结构的复杂性，其动力学模型也常常很复杂，因此很难实现基于机器人动力学模型的实时控制。然而高质量的控制应当基于被控对象的动态特性，因此，如何合理简化机器人动力学模型，使其适合于实时控制的要求，一直是机器人动力学研究者追求的目标。
3
6.1 机器人静力学一、杆件之间的静力传递在操作机中，任取两连杆,。设在杆 上的 点作用有力矩 和力 ；在杆 上作用有自重力 〔 过质心 ）； 和 分别为由 到 和 的向径。
iL 1iL? 1iL? 1i
O?
1iM? 1iF? iL iG
iC ir Cir iO 1i
O? iC
1iF?
1iM?
4
按静力学方法,把这些力,力矩简化到 的固联坐标系
,可得：
iL
i i i io x y z?
1
11
iii
i i i i C i i
F F G
M M r F r G
1 0
110
110
11 1 1 0
ii i i
ii i i
ii i i i i i i
iii i i i C i i
F R F R G
M R M r R F r R G
或式中 ( 为杆 的质量 )。0
iiG m g im iL
求出 和 在 轴上的分量，就得到了关节力和扭矩，
它们就是在忽略摩擦之后，驱动器为使操作机保持静力平衡所应提供的关节力或关节力矩，记作,其大小为
iF iM iz
i?
i
ikF
ikM
5
1
11
1 1 1
0
i i i
i i i
i i i i
i i i i i
F R F
M r R R M
当忽略杆件自重时,上式可简记为,
iG
若以 表示不计重力的关节力或力矩值,对于转动关节则有,
0i?
,0 ()j
n
iCi i ji
ji
k r G
jC
式中 ——是自 到杆 的质心 的向径 。
,jiCr iO jL
6
例 1 求两杆操作机的静关节力矩 (坐标系与结构尺寸如图 )。
解：
设已知
7
8
9
二、操作机的静力平衡设有操作机如图所示，每个关节都作用有关节力矩 (广义驱动力，指向 的正向 )，在末端执行器的参考点 处将产生力 和力矩 。由于,是操作机作用于外界对象的力和力矩，为了和输入关节力矩 一起进行运算，
故应取负值。
iz eP
i?
eF eM eF eM
i?
10
1,,,,Tin
,,,,,Te x e y e z e x e y e zQ F F F M M M
1,,,,Tinq q q q
,,,,,Te e e x y zp x y z
利用虚功原理建立静力平衡方程，令于是，操作机的总虚功是：
根据虚功原理，若系统处于平衡，则总虚功 (虚功之和 )为 0，
即
TTW q Q p
0TTq Q p
11
式中 J —— 是速度分析时引出的雅可比矩阵，其元素为相应的偏速度。
由机器人运动微分关系可知，,则有p J q
0TTJ Q q
因为 是独立坐标，则,所以有
0qiq
TJQ
上式是针对操作机的关节力和执行器参考点 间所产生的力和力矩之间的关系式。
eP
该式表明关节空间和直角坐标空间广义力可以借助于雅可比矩阵 J 进行变换。这种变换关系，也可推广到任两杆间固联直角坐标系中的广义力变幻，这时应将关节空间与直角坐标空间的雅可比矩阵，换作直角坐标空间的雅可比矩阵。
12
例 2 如图，操作机的手爪正在持板手扭某一哩栓，手爪上 方联接一测力传感器可测六维力向量 (力和力矩 )。试确定测力传感器和扭动板手时力和力矩的关系。
13
解：
设在测力传感器上置坐标系 Sf ( )，在螺栓上置坐标系 S ( ) 。在图示瞬间，两坐标系彼此平行。因为刚体的无限小位移 (平移和转动 )可表示为六维向量，故对二者的微位移可分别表示为：
由于两坐标系的坐标轴平行，于是可以得到：
fO uvw?
O xyz?
,,,,,x y zq x y z
,,,,,u v wp u v w
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
zy
zx
yx
u x
v y
zw
u xrr
v yrr
w zrr
p J q
14
前式也可以从前图直观求得。
设 为相应于 的广义力向量,为相应于 的广义力向量，则可得,
Q q P p
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
x u
y v
z w T
x zy u
zxy v
yx wz
F F
F F
F F
Q J P
M rr M
rrM M
rr MM
上式也可直接用虚功原理求得。
15
一,研究目的：
1,合理地确定各驱动单元 （ 以下称关节 ） 的电机功率 。
2,解决对伺服驱动系统的控制问题 （ 力控制 ）
在机器人处于不同位置图形 （ 位形 ） 时,各关节的有效惯量及耦合量都会发生变化 （ 时变的 ）,因此,加于各关节的驱动力也应是时变的,可由动力学方程给以确定 。
6-2 机器人动力学概述二,机器人动力学研究的问题可分为两类：
1,给定机器人的驱动力 （ 矩 ）,用动力学方程求解机器人 （ 关节 ） 的运动参数或动力学效应 （ 即已知,求 和,
称为动力学正问题 。 ） 。
2,给定机器人的运动要求,求应加于机器人上的驱动力
（ 矩 ） （ 即已知 和,求,称为动力学逆问题 ） 。
,
,
16
三,动力学研究方法：
1,拉格朗日方程法：通过动,势能变化与广义力的关系,建立机器人的动力学方程 。 代表人物 R.P.Paul,J.J.Uicker、
J.M.Hollerbach等 。 计算量 O(n4),经优化 O(n3),递推 O(n)。
2,牛顿 —欧拉方程法：用构件质心的平动和相对质心的转动表示机器人构件的运动,利用动静法建立基于牛顿 —欧拉方程的动力学方程 。 代表人物 Orin,Luh(陆养生 )等 。 计算量 O(n)。
3,高斯原理法,利用力学中的高斯最小约束原理,把机器人动力学问题化成极值问题求解,代表人物波波夫 (苏 ),用以解决第二类问题 。 计算量 O(n3)。
4,凯恩方程法：引入偏速度概念,应用矢量分析建立动力学方程 。 该方法在求构件的速度,加速度及关节驱动力时,只进行一次由基础到末杆的推导,即可求出关节驱动力,其间不必求关节的约束力,具有完整的结构,也适用于闭链机器人 。 计算量 O(n!)。
17
系统的动能和势能可在任何坐标系（极坐标系、圆柱坐标系等）中表示,不是一定在直角坐标系中 。
动力学方程为：
广义力 广义速度 广义坐标
（力或力矩）（ 或 ） （ 或 ）
i
ii
d L L
d t q q?
v? d
6.3 二杆机器人的拉格朗日方程应用质点系的 拉格朗日 方程来处理杆系的问题。
定义,L=K-P
L— Lagrange函数； K—系统动能之和； P—系统势能之和 。
6.3.1 刚体系统拉格朗日方程
18
设二杆机器人臂杆长度分别为,质量分别集中在端点为,坐标系选取如图 。2,1 dd
2,1 mm
以下分别计算方程中各项：
一、动能和势能
2
2
1 mvK? mghp?
对质点,
1m
2 2 2
1 1 1 1 1 1 1 1 1
1 1 1()
2 2 2k m v m d m d
势能：
动能：
1 1 1 1c o s ( )p m g d
（ 负号与坐标系建立有关 ）
对质点,
2m 先写出直角坐标表达式：
)c o s ()c o s (
)s i n ()s i n (
212112
212112
ddy
ddx
6.3.2 刚体系统拉格朗日方程
1?
1?
19
对 求导得速度分量：x
)2121)(2c o s(212)2221221(222121222222
)21)(21si n (21)1si n (12
)21)(21c o s(21)1c o s(12
ddddyxv
ddy
ddx
动能：
)2121)(2c o s (212)2221221(22222121212212 ddmdmdmK
)21c o s (22)1c o s (122 gdmgdmP
势能：
二,Lagrange函数
1 2 1 2( ) ( )L K P k k p p
2 2 2 2 2 2
1 2 1 1 2 2 1 1 2 2 2 1 2 2 1 1 2
11( ) ( 2 ) c os( ) ( )
22m m d m d m d d
1 2 1 1 2 2 1 2( ) ( ) c o s ( )m m g d s m g d
),,,( 2121L?
20
三、动力学方程先求第一个关节上的力矩
2221212212222212222121211 )c o s()c o s(2)( ddmddmdmdmdmmL
222122221221222221211 )]c o s([)]c o s(2)[( ddmdmddmdmdmmLdtd
222212212212 )s i n ()s i n (2 ddmddm
)si n ()si n ()( 212211211 gdmgdmmL
2 2 21 2 1 2 2 2 1 2 2 1 2 2 2 1 2 2 2[ ( ) 2 c o s ( ) ] [ c o s ( ) ]m m d m d m d d m d m d d
LL
dt
d )(
11?
22 1 2 2 1 2 2 1 2 2 2 1 2 1 1 2 2 1 22 s i n ( ) s i n ( ) ( ) s i n ( ) s i n ( )m d d m d d m m g d m g d
—— （ 1）
1
1 1 1 1
( ( ) ( ) )d L L d L Ldt q q dt1?
21
同理，对 和 微分，可求得第二关节力矩2 2?
12212222212222 )c o s( ddmdmdmL
21221212212222212222 )si n ()c o s( ddmddmdmdmLdtd
)si n ())(si n ( 2122212122122 gdmddmL
222 )(
LL
dt
d?
)s i n ()s i n ()]c o s ([ 2122212212222212212222 gdmddmdmddmdm
以上是两杆机器人动力学模型。
—— （ 2）
22
系数 D 的物理意义：
—关节 的有效惯量（等效转动惯量的概念）。由关节处的加速度 引起的关节 处的力矩为 （ ）
iiD
ii
i
iiiDJMi?
i
—关节 和 之间的耦合惯量 。 由关节 或 的加速度
（ 或 ）所引起的关节 和 处的力矩为 或
ijD i j j
i j j
i
iijD jiji
—向心力项系数。表示关节 处的速度作用在关节 处的向心力（ ）
ijjD ji
2jijjD
—向心力项系数。表示关节 处的速度作用在本身关节处的向心力（ ）
iiiD i
2iiiiD
四,动力学方程中各系数的物理意义将前面结果重新写成简单的形式,
221 1 1 1 1 2 2 1 1 1 1 1 2 2 2 1 1 2 2 1D D D D D D
222 2 1 1 2 2 2 2 1 1 1 2 2 2 2 2 1 2 1 2 2 2 1 2 1 2D D D D D D D
23
—哥氏力项系数。 两项组合为关节 与处的速度作用在关节 处的哥氏力，哥氏力是由于牵连运动是转动造成的。
ijkD jkijkkjijk DD
j k
i
—关节 处的重力项 。 重力项只与 大小、长度 以及机构的结构图形（ ）有关。
iD i m d
21,
比较二杆机器人例中的系数与一般表达式中的系数得到有效惯量系数：
221 1 1 2 1 2 2 2 1 2 2[ ( ) 2 c o s ( ) ]D m m d m d m d d
22 2 2 2D m d?
耦合惯量系数：
21 2 2 1 2 2 2 1 2 2c o s ( )D D m d m d d
24
向心力项系数：
0
)si n (
)si n (
222
2212211
2212122
111
D
ddmD
ddmD
DD
哥氏力项系数：
0
)s i n (2
221212
2212121112
DD
ddmDD?
重力项：
1 1 2 1 1 2 2 1 2( ) sin ( ) sin ( )D m m g d m g d
2 2 2 1 2s in ( )D m g d
25
6.4 机器人的拉格朗日方程 的一般表达形式从上节容易看出 Lagrange方程是一个二阶耦合、非线性和微分方程，为简化计算，未虑及传动链中的摩擦。以下方程的推导，也是不考虑传动链带来的摩擦影响，只考虑杆件本身，然后再加入关节处驱动装置（如电机、码盘等）的影响。
推导分五步进行：
一、计算任意任意杆件上任意点的速度；
二、计算动能 ；
三、计算势能 ；
四、形成 Lagrange函数；
五、建立动力学方程 。
21
2i i ik m v?
i i ip m g h?
()i
ii
d L LF
d t q q
26
其速度为：
一、点的速度
r
由于整个系统的动能都是在基础系中考虑的，故需求系统各质点在基础坐标系中的速度 。
r
对于杆 坐标系中的一点,它在基础坐标系中的位置为i
ir
iir T r
式中 —变换矩阵
iT
1
()
i
i
iii
j
j
j
d T r Tdr
r q r
d t d t q
速度平方为：
22( ) ( ) ( )r r r T r a c e r r
式中 —矩阵的迹，即矩阵主对角元素之和。()T race
27
2( ) ( )r Tr a c e r r
11
()
ii
i i Tii
jk
jk
jk
TTT r a c e q r q r
11
()
ii
TT
iiii
jk
jk
jk
TTT r a c e r r q q
二、动能位于杆 上 处质量为 的质点的动能是：i ir dm
11
1 ()
2
ii
TT
iiii
i j k
jk
jk
TTd k T r a c e r r q q d m
11
1 ()
2
ii
TT
iiii
jk
jk
jk
TTT r a c e r d m r q q
28
则杆 的动能（在基础坐标系中）为：i
11..
1 ()
2
ii
TT
iiii
i i j k
jk
jkli n k i li n k i
TTk dk T rac e r r dm q q
令式中 称为连杆 的伪惯量矩阵。i
.
Tii
i
lin k i
J r r d m
11
1
2
ii
T
ii
i i j k
jk
jk
TTk T r a c e J q q
则得到杆 的动能为：i
对于杆 上任意一点的 （在杆 坐标系中）可以表示为：i ir i
(,,,1 )i i i ir x y z?
29
2
.,,,
2
.,,,
2
.
.,,,
..
i i i i i i
l i nk i l i nk i l i nk i l i nk i
i i i i i i
T
l i nk i l i nk i l i nk i l i nk iii
i
i i i i i i
l i nk i
l i nk i l i nk i l i nk i l i nk i
ii
l i nk i l i nk i
x dm x y dm x zdm x dm
x y dm y dm y zdm y dm
J r r dm
x zdm y zdm z dm zdm
x dm y dm
..
i
l i nk i l i nk i
zdm dm
根据理论力学中惯性矩、惯性积和静矩的定义，引入下列记号：
2 2 2 2 2 2( ),( ),( )
x x y y zzI y z dm I x z dm I x y dm
对坐标轴的惯性矩：
则有：
30
对坐标轴的惯性积：
,,x y y z x zI x y dm I y zd m I x zd m
对坐标轴的静矩：
,,x y zI x dm I y dm I zdm
质量之和：
m dm
于是：
2 2 2 2 2 2 21 1 1( ) ( ) ( )
2 2 2x d m y z d m x z d m x y d m
2
x x y y z zI I I
x
z
y
r
31
同理：
22,
22
x x y y zz x x y y zzI I I I I Iy dm z dm
2
2
2
i x x i y y i zz
i x y i x z i x
i x x i y y i zz
i x y i y z i y
i
i x x i y y i zz
i x z i y z i z
i x i y i z i
I I I
I I I
I I I
I I I
J
I I I
I I I
I I I m
于是 能够表达为：
iJ
机器人臂杆总的动能是：
1 1 1 1
1
2
n n i i
T
ii
i i j k
jk
i i j k
TTK k T r a c e J q q
32
如果考虑到关节处驱动装置的动能：
2
.
1
2 im ot or i a ik J q?
调换求迹与求和运算顺序，并加入关节处驱动装置的动能，
得到机器人总的动能为：
2
1 1 1 1
11 ()
22 i
n i i n
T
ii
i j k a i
jk
i j k i
TTK T r a c e J q q J q
2
.
1
2 im o t o r i a ik m q?
（对于移动关节,）
式中 为关节 处驱动装置的转动惯量。
aiJ i
三、势能设杆 的质心在再其自身坐标系的位置向量为,则它在基础坐标系中的位置向量 为
i i Cr
Cr
iC i Cr T r (,,,1 )Ti i i iC C C Cr x y z
33
设重力加速度 在基础坐标系中的齐次分量为：
T T ii i C i i Cp m g r m g T r
于是机器人的总势能为：
g
(,,,1 )Tx y zg g g g?
则杆在基础坐标系中的势能为：
（一般认为基础坐标系的 z 轴取向上方）
11
nn
Ti
i i i C
ii
P p m g T r
34
先求拉格朗日方程中的各项：
11
1 ()
2
ni
T
ii
ik
p p k
ik
TTL T r ace J q
q q q
四、拉格朗日函数
1 1 1
1 ()
2
n i i
T
ii
i j k
jk
i j k
TTL K P T r a c e J q q
2
11
1
2
nn
Ti
ai i i i C
ii
J q m g T r
五、动力学方程
11
1 ()
2 p
ni
T
ii
i j a p
jp
ij
TTT r a c e J q I q
（ 1 )
35
由于 是对称矩阵，则有：
合并 (a)式中前两项，得到：
iJ
( ) ( )
TT
Ti i i i
ii
p k p k
T T T TTr a c e J Tr a c e J
q q q q
()
T
Tii
i
kp
TTT ra c e J
()
T
ii
i
kp
TTTr a c e J
11
()
p
ni
T
ii
i k a p
p k p
ik
TTL T r ace J q I q
q q q
（ 1’)
当 时,中不包含 以后关节变量，即：ip?
iT
p
0,i
p
T
q
()ip?
于是可得：
1
()
p
ni
T
ii
i k a p
p k p
i p k
TTL T r a c e J q I q
q q q
36
2
11
()
n i i
T
ii
i k m
k m p
i p k m
TTT r a c e J q q
q q q
（ 2 )
1
( ) ( )
p
ni
T
ii
i k a p
p k p
i p k
TTdL T r a c e J q I q
d t q q q
2
11
()
n i i
T
ii
i k m
p m k
i p k m
TTT r a c e J q q
q q q
1
( ) ( )
p
ni
T
ii
i k a p
p k p
i p k
TTdL T r a c e J q I q
d t q q q
交换其中的部分哑元，得到：
2
11
2 ( )
n i i
T
ii
i k m
k m p
i p k m
TTT r a c e J q q
q q q
37
2
11
1 ()
2
n i i
T
ii
i j k
p j p k
i p j k
TTL T r a c e J q q
q q q q
2
11
1 ()
2
n i i
T
ii
i j k
k p j
i p j k
TTT r a c e J q q
q q q
n
Ti i
iC
p
ip
Tm g r
q
2
11
()
n i i
T
ii
i j k
j p k
i p j k
TTT r a c e J q q
q q q
n
Ti i
iC
p
ip
Tm g r
q
（ 3 )
38
将以上各项带入拉格朗日公式，并用 和 分别代替上式中的哑元 和,得到,i jip
1
()
i
jn
T
jj
i j k a i
ki
j i k
TT
T r a c e J q I q
2
11
()
jjn
T
jj
j k m
k m i
j i k m
TT
T r a c e J q q
q q q
n
jTi
jC
i
ji
T
m g r
q
上式为拉格朗日方程的最后形式。这些方程与求和的次序无关，因此可将上式写为简化形式：
（ 5 )
1 1 1
i
n n n
i i j j a i i j k j k i
j j k
D q I q D q q D?
（ 4 )
39
式中：
m a x (,)
()
n T
pp
i j p
jj
p i j
TT
D T r a c e J
()
n
pTp
i p C
i
pi
T
D m g r
q
2
m a x (,,)
()
n T
pp
i j k p
j k i
p i j k
TT
D T r a c e J
q q q
以上的动力学方程 (5)中系数 D的意义与上节所列相同，
即分别为有效惯量项系数 ( ),耦合惯量项系数（ ），
向心力项系数（ ），哥氏力项系数（ ），重力项等。
ij? ij?
kj? kj?
ij?
40
动力学方程中的惯量项和重力项在机器人控制重特别重要，
将直接系统的稳定性和定位精度。只有当机器人高速运动时，
向心力项和哥氏力项才是重要的。传动装置的惯量值往往较大，对系统动态特性的影响也不可忽略。
在机器人动力学问题的讨论中，拉格朗日动力学方程常写作更简化的一般形式：
( ) ( ( ) ) ( ) ( ( ),( ) ) ( ( ) )t D q t q t h q t q t c q t
式中：
12( ) ( ( ),( ),,( ),) Tnt t t t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
12( ) ( ( ),( ),,( ),) Tnq t q t q t q t
( ( ) ),( ( ),( ) ),( ( ) )D q t h q t q t c q t的意义见（ 5 )式。
（ 6)
41
乘法次数,4 3 21 2 8 / 3 5 1 2 / 3 8 4 4 / 3 7 6 / 3n n n n
6.5 机器人的 牛顿 —欧拉方程机器人的拉格朗日动力学模型为非线性二阶常微分方程,
利用这些方程,由已知的每一轨迹设定点的关节位置,速度和加速度,可以计算各关节的标称力矩,但拉格朗日方程利用 4× 4齐次变换矩阵,使得计算效率太低 。
4 3 29 8 / 3 7 8 1 / 6 6 3 7 / 3 1 0 7 / 6n n n n加法次数：
为了实现实时控制,曾用过略去哥氏力和向心力的简化模型,但当操作机快速运动时,哥氏力和向心力在计算关节力矩中是相当重要的 。 因而这种简化只能用于机器人的低速运动,在典型的制造业环境中,这是不合乎要求的 。 此外,这种简化所引起的关节力矩误差,不能用反馈控制校正 。
牛顿 —欧位法采用迭代形式方程,计算速度快,可用于实时控制,因而成为一种常用的建模方法 。
42
寻求转动坐标系和固定惯性坐标系之间必要的数学关系,再推广到运动坐标系 (转动和平移 )和惯性坐标系之间的关系。
如图,惯性坐标系 O-XYZ和转动坐标系 O-X*Y* Z*的原点重合于 O点。而 OX*,OY*,OZ*轴 相对 OX,OY,OZ轴旋转。设和 分别为这两个坐标系沿主轴的单位矢量。转动坐标系中点 P 可用它在任一坐标系中的分量来表示：
6.5.1 转动坐标系
(,,)i j k (,,)i j k
r x i y j zk
或
r x i y j z k
在惯性坐标系中的运动：
d r d x d y d zi j k x i y j z k
d t d t d t d t
Y
X
Z
r
Y*
X*
Z*
O
P
在转动坐标系中的运动：
d r d x d y d zi j k x i y j z k
d t d t d t d t
43
在惯性坐标系中的运动：
d r d x d y d z d i d j d ki j k x y z
d t d t d t d t d t d t d t
d r d i d j d kx y z
d t d t d t d t
（ 7 )
需要解决转动坐标系坐标轴在惯性坐标系中的导数问题。我们假定，转动坐标系绕着过原点 O的某轴 OQ以角速度 旋转。
方向沿 OQ轴，指向转动坐标系右旋方向。则可以证明转动坐标系中的任意固定矢量 在惯性坐标系中的导数为：
s
X
Z
Y
s
Y*
X*
Z*
O
Q
ds s
dt
44
于是由 （ 6 )式可得：
( ) ( ) ( )d r d r x i y j z kd t d t
d r d r r
d t d t?
这是建立转动坐标系两种时间导数之间关系的基本方程。
2
2 ()
d r d d r d r d r
d t d t d t d tdt
2
2 ()
d r d r d r drr
d t d t d tdt
22
22 2 ( )
d r d r d r drr
d t d td t d t
方程 （ 9 )又被称为哥氏定理。
（ 8)
（ 9 )
45
6.5.2 运动坐标系如图,运动坐标系 O-X*Y* Z*相对于惯性坐标系 O-XYZ转动和平移。质量为 M 的质点 P分别以 和 确定相对于惯性坐标系和运动坐标系的原点的位置。原点 O* 相对于原点 O的位置以矢量 表示。则有：
r r?
h
r r h
( ) ( )kd r d r d hv t v vd t d t d t
() d r d hv t rd t d t?
22
22
()( ) ( )
k
d v t d r d ha t a a
dt d t d t
22
2 ( )d r d r d d hrrd t d td t d t
r
YX
Z
O
P
Y*
X*
Z* O*
r*
h
（ 10 )
（ 11 )
46
6.5.3 杆件运动学根据前述运动坐标系的概念，推导一组数学方程，描述机器人的运动杆件相对于基础坐标系的运动学关系。
令 和 分别为坐标系 相对于基础坐标系 的线速度和角速度。令 和 分别为杆件 i
坐标系 相对于基础坐标系 和杆件 i -1
坐标系 的角速度。则杆件 i 坐标系 相对于基础坐标系的线速度 和角速度 分别是：
1iv? 1i 1 1 1(,,)i i ix y z
0 0 0(,,)x y z i? i
(,,)i i ix y z 0 0 0(,,)x y z
1 1 1(,,)i i ix y z
iv
i?
坐标系 是基础坐标系，而坐标系和 分别固联于杆件 i -1和杆件 i 上，原点分别为
Oi-1和 Oi 。原点 Oi 相对于原点 O 和原点 Oi-1 的位置分别用位置矢量 和 表示。原点 Oi-1相对于基础坐标系原点 O
的位置用位置矢量 表示。
0 0 0(,,)x y z
(,,)i i ix y z
ip ip?
1ip
1 1 1(,,)i i ix y z
47
11
i
i i i i
dpv p v
dt?
1i i i
式中 d*(.)／ dt 表示在 运动坐标系 的时间导数。
1 1 1(,,)i i ix y z
（ 12 )
（ 13 )
48
2
1 1 1 1 12 2 ( )
ii
i i i i i i i i
d p d pv p p v
dtdt
1i i i
1
ii
i i i
dd
d t d t
11
i
i i i i
d
dt
（ 14 )
（ 15 )
为坐标系 相对于 的角加速度,(,,)
i i ix y z 1 1 1(,,)i i ix y zi
根据机器人杆件坐标系建立的步骤和参数的定义，杆件 i 在杆件 i –1 坐标系中的运动是沿 方向的平移或绕 转动。
因此，
1iz? 1iz?
式中 是杆件 i 相对于杆件 i -1 坐标系的角速度值。q
i
1iizq?
0
若杆件 i 转动若杆件 i 平移 （ 16 )
49
（ 18 )
类似地 （ 17 )
id
dt
1iizq?
0
若杆件 i 转动若杆件 i 平移由式 （ 13 )和 式 （ 16 )有：
11i i izq
i
1i
若杆件 i 转动若杆件 i 平移
1 1 1 1()i i i i i iz q z q
i
1i
若杆件 i 转动若杆件 i 平移 （ 19 )
2
2
idp
dt
1iizq?
()i i i i id ppdt
若杆件 i 转动若杆件 i 平移由式 （ 15 ),式 （ 16 )和 式 （ 17 )有：
idp
dt
1iizq?
iip
若杆件 i 转动若杆件 i 平移
（ 20 )
（ 21 )
由式 （ 15 ),式 （ 16 )和 式 （ 17 )有：
50
( ) ( ) ( )a b c b a c a b c
（ 22 )
iv?
1i i ipv
11i i i i iz q p v
若杆件 i 转动若杆件 i 平移由式 （ 12 ),式 （ 13 )和 式 （ 20 )有：
利用矢量叉乘积的恒等式
( ) ( ) ( )a b c b a c c a b
并根据式 （ 14 ),式 （ 15 )和 式 （ 19 )有：
iv?
1()i i i i i ip p v
1 1 12 ( ) ( )i i i i i i i i iz q p z q p v
—（ 23 )
若杆件 i 转动若杆件 i 平移
51
刚体的运动可分解为随质心的移动和绕质心的转动。借助于杆件运动学知识，我们把达朗贝尔原理用于每个杆件，描述机器人各杆件的运动。达朗贝尔原理可应用于任意瞬时，它实质上是牛顿第二运动定律的一种变型，可表示为：
6.5.4 牛顿 ——欧拉法基本运动方程
i i iF m v?
牛顿定理,
欧拉方程,()
i i i i i iN I I
式中,— 杆 i 质量；
— 杆 i上所有外力合力；
— 杆 i上所有外力对质心的合力矩；
— 杆 i 绕其质心惯性矩阵 。
im
iF
iN
iI
()ii
i
d m vF
dt?
()ii
i
dIN
dt
52
1,,1i i i i i iF f f m g
1,,1,,1 1,1,( ) ( )i i i i i i c i i i i c i i iN N N r f r f
根据力（矩）平衡原理，在质心处有：
则有
1,,1 0i i i i i i if f m g m v
1,,1,,1 1,1,( ) ( ) ( ) 0i i i i i c i i i i c i i i i i i i iN N r f r f I I
（ 24)
方程 （ 24 )即为牛顿 ——欧拉法的基本方程。
,1iif?
,1iiN?
1,iif? 1,iiN?
img
iz
1iz?
,iiCr
io
iy
ix
1iy?
1ix?
1io?
53
上面推导的牛顿 ——欧拉法 （ 也简称 N-E法 ） 方程式含关节联接的约束力 （ 矩 ）,没有显示地表示输入 —输出关系,不适合进行动力学分析和控制器设计 。 如果变换成由一组完备且独立的位置变量 （ 质心位置变量通常不是相互独立的 ） 和输入力来描述,这些变量都显式地出现在动力学方程中,即得到 显式的输入 ——输出形式表示的动力学方程,称为封闭形式的动力学方程 （ 拉格朗日方程即是封闭的 ） 。
6.5.5 递推形式的牛顿 ——欧拉方程关节变量 是一组完备且独立的变量,关节力 （ 矩 ） 是一组从约束力 （ 矩 ） 中分解出来的独立的输入,所以用 和来描述方程,可以得到封闭形式的动力学方程 。
q?
q?
54
根据 N-E法的基本方程,利用质心运动变量与关节变量及关节运动变量之间的关系以及约束力与关节力矩之间的关系,消去中间变量,可以得到封闭形式的动力学方程 。 但显然不如用拉格朗日法简单,特别是当机器人自由度较多时,更是如此 。
因此,对于 N-E法,常用的不是它的封闭形式方程,而是它的递推形式方程 。 方程 （ 24） 可直接写成如下递推形式：
1,,1i i i i i i if f m v m g
1,,1,,1 1,1,( ) 0i i i i i c i i i i c i i i i i i i iN N r f r f I I
（ 25)
而关节力（矩）可写成如下形式：
1 1,
1 1,
Ti i i i i
i T
i i i i i
z N b q
z f b q
对于旋转关节对于移动关节
（ 26)
式中,为沿关节轴线 的单位矢量,
为关节的粘滞阻尼系数 。
1iz
ib
1?iz
55
递推形式的 N-E法方程 与封闭形式方程比较,计算量从减少到,
乘法次数,117n– 24，
加法次数,103n- 21，
从而大大加快了计算速度 。 自由度越多,递推形式的优势越明显 。 对于典型 n=6 的情形,递推形式的计算效率几乎提高 10倍 。
因此,常用于实时计算 。
递推形式 方程 的特点是其计算从机器人操作机的一个杆到另一杆逐个顺序进行的,它充分利用了操作机的串联链特性,常用于求解动力学逆问题 （ 即已知,求 ） 。
求解的大致过程为：根据运动和力的不同传递方向,进行运动量的向前迭代和力学量的向后迭代 。
具体步骤如下：
4()On
()On
,,?
56
n
1n?2
1
0
n
,1nnf
,1nnN
动力学计算运动学计算
2 2 2 21,1 1 1 1 1 1
3331 2 2
,,,,,,,,,,
,,,,
cccc
c n c n n n
v v w wq q q v v w w v v w w
qqqq q q
1,确定计算 N-E方程所需的所有运动量,包括每个杆件的
（ ） 由杆 1 杆 n：
1,,1,,1 1,1,1,2,1 0,1()n n n n n c i n n n c i n i n n n n n n n n nN N r f r f I I N N N
1,,1 1,2,1 0,1n n n n n n c n n n n nf f m g m v f f f
i?
2,将上述运动量代入 N-E方程,确定关节力 （ 矩 ） 。 计算顺序与运动量计算相反,由 杆 n 杆 1：
iciici wvwv,,,
57
前述递推运动方程的明显缺点是所有惯性矩阵和物理几何参数 （ 如 ） 等,都是以基础坐系为参照的,因此,
当机器人运动时,它们也随着变化 。 Luh等人改进了上述 N-E
方程,将所有杆件的速度,加速度,惯性矩阵,质心位置,
力和力矩等,都表示在各杆的自身坐标系中,从而使计算更加简单 。
这种改进的最重要的成果是,计算关节驱动力矩的时间不仅与机器人关节数成线性比例,而且与机器人构型无关 。 这就有可能在关节变量空间实现机器人的实时控制算法 。
6.5.6 在杆件自身坐标系中的递推方程
iiicii zrr?,,1,,?
设 是 3× 3旋转矩阵,它把矢量由坐标系 变换到坐标系 中 。
1i iR? ),,(
iii zyx
1 1 1(,,)i i ix y z
58
这样,可不计算相对基础坐标系的 和等,而是直接计算在杆件自身坐标系中的和 等 。
于是有关运动量的递推公式变为：
1,,,,,,i i i i c i i iv v v f 1,iiN?
1,,,,,,i i i i i ii i i i c i i iv v v f
1,i iiN?
,,()i i i i i i ic i i i i c i i i i c iv v r r
1
1 1
1 1
()i i ii i i i i
i ii
ii
R q z
R
若杆件 i 转动若杆件 i 平移
1
1 11
1 1
[ ( ) ]i i i i ii i i i i i i i
i ii
ii
R q z q z
R
若杆件 i 转动若杆件 i 平移
1 1 1 1 1 1
1,1 1 1,1
1 1 1 1 1 1
1 1 1,1 1 1
[]
( ) 2 ( )
i i i i i i i
i i i i i i i i i
i i i i i i i i i
i i i i i i i i i i i i
R v r r
v R v q z r q R z
1 1 11 1,1()i i ii i i ir
若杆件 i 转动若杆件 i 平移
59
11,1,1i i i ii i i i i i c if R f m v
1 1 11,1,1 1,1 1,,( ) ( ) ( )i i i i i i i ii i i i i i i i i i i c i i c iN R N r f r r m v
1
1,1 1
1
1,1 1
()
()
i T i i
i i i i
i i T i i
i i i i
N R z
f R z
关节间约束力公式变为：
()i i ii i i i iII
因此，概括地说，高效的牛顿一欧拉运动方程是一组正向和反向的递推方程，每一杆件的动力学和运动学参数都是以其自身坐标系为参照的。
60
6.6 机器人的 凯恩方程法简介凯恩 (Kane)方法是用来建立机器人机构动力学模型的一种普遍方法，其基本思想是以广义速率代替广义坐标作为系统的独立变量，它用达朗倍尔原理及虚位移原理建立动力学方程。凯恩方法既适合于完整系统，也适合于非完整系统。
6.6.1 广义速率和偏速度及偏角速度一、广义速率一个具有 n 个自由度的完整系统，相对于惯性坐标系的运动一般通过 n 个独立的广义坐标来描述 。 n 个广义速度 也是独立的，故可用
n 个广义速度的线性组合，即 n 个广义速率 (或称准速度 )
来描述系统的运动，即
0 0 0(,,)x y z ( 1,2,,)iq i n
( 1,2,,)iq i n
( 1,2,,)iu i n
61
1
( 1,2,,)
n
i ri i r
i
u a q a r n
式中,为 及 t 的函数；而由 组成的系数矩阵应为非奇异阵。则有,ri raa
iq ria
二、偏速度系统中任意质点 p 的径矢为 为广义坐标 及 t 的函数，
,rr iq
12(,,,,)pnr r q q q t则该点的速度为
1 1 1 1
()
n n n n
p p i i r r i
i i i
i r i i
r r r r rv r q u b
q t q q t
1
( 1,2,,)
n
i i r i i
r
q u b i n?
（ 27)
（ 28)
62
令
1
()
n
p r r t
r
v v r u v
其中
1
()
n
r ir
i
i
rvr
q
1
()
n
ti
i
i
rrv r b
qt
称广义速率 前的系数矢量 为 p点相对于惯性坐标系的第 r 偏速度。一般说来它是广义坐标 和时间 t 的函数。对于定常系统,偏速度只是广义坐标的矢量函数。
()rvrru
iq
0tv?
（ 29)
（ 31)
（ 30)
三、偏角速度
1 1 2 2 3 3
1
()
n
r r t
r
e e e e u
63
从式 (30) 可见，广义速率的取法不同，系统内同一质点及同一刚体的偏速度也可不同。故可通过一定的技巧来选取广义速率，使得到的偏速度和偏角速度具有最简单的形式。最理想的是使较多的偏速度和偏角速度等于零，这样有利于简化动力学方程。
一般选择与系统的运动密切相关的量，如选取系统中刚体的角速度分量或质点的速度分量为广义速率，以使刚体的角速度和质点的速度具有最简单的表达形式。作为特例，如果取广义速度为广义速率，即
iiuq?
12
12
n
n
d r r r r rv q q q
d t q q q t
可见对于广义速度 的偏速度为 。
iq
i
r
q
64
6.6.2 凯恩动力学方程四、刚体各点偏速度
( ) ( )rrp C r pv r v r r
由达朗倍尔原理和虚位移原理推得的系统的动力学普遍方程：
( ) 0p p p p
p
F m r r
系统中 p 点的速度，对于具有 n 个自由度的系统，可写成广义速率的线性组合,即
1
()
rt
n
p
p rp r tp
r
dr
v v r u v
dt
（ 32)
（ 33)
65
11
( ) ( )
r t r t
nn
p rp r tp rp r tp
rr
dr v r u dt v dt v r d v dt?
式中 。如果 可积分，则 为另一种定义的广义坐标。 rrd u dt
ru
r?
（ 34)
从式 (34) 可得到 的变分,即
pr pr?
1
()
r
n
p rp r
r
r v r
将上式代入式 (33) 得
1
( ) ( ) 0
r
n
p p p rp r
pr
F m r v r
66
改变求和的形式得
1
( ) ( ) 0
rr
n
p rp p p rp r
r p p
F v r m r v r
（ 34)
令
()
rr p rp
p
F F v r
()
rr p p rp
p
F m r v r
称为系统对应于 的广义主动力
ru
称为系统对应于 的广义惯性力
ru
67
这样，式（ 34）成为：
1
0
n
r r r
r
FF
由于 为独立的变分，所以有
r
0 1,2,,rrF F r n
（ 35)
上式称为凯恩动力学方程，意为广义上动力与广义惯性力之相等于零。
凯恩方程法 可以得到封闭形式的动力学方程。
68
6.7 弹性机器人 动力学简介
6.7.1 机器人系统的弹性问题通常，机器人机构的手臀、驱动、传动元件被假设为刚性的，故系统的建模及控制方案设计都是以这样一个刚性假设为前提的。结构刚性越好越不易振动，机器人精度越高，经过近似处理的控制方法也越精确、可行。
随着工业的发展，对机器人性能要求越来越高。如：要求机器人重量轻以降低能耗；运行速度快，以提高劳动生产率；
定位精度高，以适应更多的精密作业要求。另外，对于如太空机器人所处的特殊环境要求具有超长手臂的机器人，其结构已不再是刚性结构，而必须计入曾被忽略的系统弹性或非线性因素，这便使系统设计、建模及控制变得更为复杂。
69
关于具有弹性的机器入系统的建模与控制问题的研究历史不长，始于 70年代初,研究具有弹性附件的飞机动力学模型的动力学分析问题，计入分布质量和柔性效应的机器人机构在低速运动下的动力学，对具有分布质量和弹性影响的二连杆机械手提出反馈控制方法。近年来，关于这方面的研究发展很快，大多以研究单弹性臂机器人的建模与控制入手。
本节将以单弹性臂机器人机构为例，介绍弹性手臂机器人的建模方法。
机器人系统的弹性主要来源于两个方面，①由于臂本身的弹性而引起的变形及运行过程中的振动。②由驱动系统的弹性及非线性因素而引起的振动。
在机器人驱动系统中，存在着轴与轴承之间的库仑摩擦，
传动装置中的齿轮间隙及谐波齿轮减速器的弹性等因素。
6.7.2 机器人机构的弹性及非线性问题
70
一、建模方法。
弹性手臂机器人是一分布参数系统，其运动特性由一组非线性偏微分方程描述。在系统仿真和应用设计时必须作有限维的近似，并在不影响系统稳定性性态的前提下化为可实际控制的系统状态方程。
解决弹性手臂机器人的建模问题的各种建模方法，在不同程度上部有一定的近似性。准确性及可行性是衡量一种方法好坏的标准，就现有文献资料大致可归纳为以下几种：
（ 1） 拉格朗日方程结合模态展开法；
（ 2） 拉格朗日方程结合有限元素法；
（ 3） 牛顿一欧拉方程结合模态展开法；
（ 4） 牛顿一欧拉方程结合有限元素法；
（ 5） 哈密顿原理结合模态分析法。
6.7.3 主要研究内容
71
二、控制方程现在应用于机器人控制中的最佳控制法、极点配置法均以系统不产生振荡为前提，而弹性手臂机器人系统对其控制器的要求就是要能控制其弹性振动。因而有效可行的控制方法的研究很有必要。
对控制方案的要求：
（ 1） 必须同时控制刚体运动及弹性振动而控制输出量仅作用于关节处；
（ 2） 避免溢漏现象；
（ 3） 控制方案所要求的传感器数量应尽可能少，且安装简单、方便。
三、带有弹性构件的新型机械结构。
72
设 T1为驱动部分动能,T2为手臂部分动能,T3为负载动能，则
6.7.4 单弹性臂机构的建模
73
()i
ii
d L LF
d t q q
