Transcript 20150101201558069

机器人技术
陶建国
哈尔滨工业大学机电学院
2005. 2.

第六章 机器人静力学和动力学
静力学和动力学分析，是机器人操作机设计和动态性能分
析的基础。特别是动力学分析，它还是机器人控制器设计、
动态仿真的基础。
机器人静力学研究机器人静止或缓慢运动式，作用在机器
人上的力和力矩问题。特别是当手端与环境接触时，各关节
力（矩）与接触力的关系。
机器人动力学研究机器人运动与关节驱动力（矩）间的动
态关系。描述这种动态关系的微分方程称为动力学模型。由
于机器人结构的复杂性，其动力学模型也常常很复杂，因此
很难实现基于机器人动力学模型的实时控制。然而高质量的
控制应当基于被控对象的动态特性，因此，如何合理简化机
器人动力学模型，使其适合于实时控制的要求，一直是机器
人动力学研究者追求的目标。
2
6.1 机器人静力学
一、杆件之间的静力传递
在操作机中，任取两连杆 Li，Li 1 。设在杆 Li 1上的 Oi 1 点
作用有力矩 M i 1和力 F i 1；在杆 Li 上作用有自重力 G〔过质
i
心 Ci ）；ri 和 rCi 分别为由 Oi 到 Oi 1 和 Ci 的向径。
F i 1
M i 1
3
按静力学方法，把这些力、力矩简化到 Li 的固联坐标系
oi  xi yi zi ，可得：
Fi  Fi 1  G i
M i  M i 1  r i  F i 1  r Ci  G i
或
Fi  R F
i
i
i 1
i 1
i 1
 R0i Gi 0
i 1
i 1
i 1
i 1
 R M  ri  R F  rCi i  R0i Gi 0
0
式中 Gi   mi g ( mi 为杆 Li 的质量)。
M
i
i
i
i 1
i
i
i 1
求出 F i 和 M i 在 zi 轴上的分量，就得到了关节力和扭矩，
它们就是在忽略摩擦之后，驱动器为使操作机保持静力平
衡所应提供的关节力或关节力矩，记作  ，其大小为
i
i 
k Fi
kM i
4
当忽略杆件自重时 G i ，上式可简记为 ：
 F ii   R ii 1


 M ii   r i  R ii 1

 
i 1


0
F i 1 


i
i


R i 1   M i 1 
若以  i 0 表示不计重力的关节力或力矩值，对于转动关节
则有 ：
n
 i   i 0  ki  (r i ,C  G j )
j
j i
式中 r i ,C j ——是自 Oi 到杆 L j 的质心 C j 的向径。
5
例1 求两杆操作机的静关节力矩(坐标系与结构尺寸如图)。
解：
设已知
6
7
8
二、操作机的静力平衡
设有操作机如图所示，每个关节都作用有关节力矩  i (广
义驱动力，指向 zi 的正向)，在末端执行器的参考点 Pe 处
将产生力 F e 和力矩 M e 。由于 F e 、M e 是操作机作用于外
界对象的力和力矩，为了和输入关节力矩  i 一起进行运算，
故应取负值。
9
利用虚功原理建立静力平衡方程，令
   1 , , i , , n 
T
Q   Fex , Fey , Fez , M ex , M ey , M ez 
T
 q   q1 , ,  qi , ,  qn 
T
 p   xe ,  ye ,  ze ,  x ,  y ,  z 
T
于是，操作机的总虚功是：
T
T
W    q  Q  p
根据虚功原理，若系统处于平衡，则总虚功(虚功之和)为0，
即
T
T
  q Q  p  0
10
由机器人运动微分关系可知， p  J  q ，则有
T
  J T Q   q  0


因为 qi 是独立坐标，则  q  0 ，所以有
  JT Q
式中 J ——是速度分析时引出的雅可比矩阵，其元素为相应
的偏速度。
上式是针对操作机的关节力和执行器参考点 Pe 间所产生的
力和力矩之间的关系式。
该式表明关节空间和直角坐标空间广义力可以借助于雅可
比矩阵 J 进行变换。这种变换关系，也可推广到任两杆间固
联直角坐标系中的广义力变幻，这时应将关节空间与直角坐
标空间的雅可比矩阵，换作直角坐标空间的雅可比矩阵。
11
例2 如图，操作机的手爪正在持板手扭某一哩栓，手爪上 方
联接一测力传感器可测六维力向量(力和力矩)。试确定测力传
感器和扭动板手时力和力矩的关系。
12
解：
设在测力传感器上置坐标系 Sf ( O f  uvw )，在螺栓上置坐
标系 S ( O  xyz ) 。在图示瞬间，两坐标系彼此平行。因为刚
体的无限小位移(平移和转动)可表示为六维向量，故对二者的
微位移可分别表示为：
 q   x,  y,  z ,  x ,  y ,  z 
 p   u,  v,  w, u , v ,  w 
由于两坐标系的坐标轴平行，于是可以得到：
  u  1 0 0
 v  

 0 1 0
  w  0 0 1
p

u   0 0 0
   0 0 0
 v 
 w   0 0 0
0
rz
 rz
0
ry
 rx
1
0
0
1
0
0
ry    x 
rx    y 
0 z 
  J q

0   x 
0   y 


1   z 
13
前式也可以从前图直观求得。
设 Q 为相应于 q 的广义力向量， P 为相应于 p 的广义
力向量，则可得：
 Fx   1
F  
 y   0
 Fz   0
Q

M x   0
M   r
 y  z
 M z   ry
0
0
1
0
0
1
rz
ry
0
rx
rx
0
0   Fu 
0 0 0   Fv 


0 0 0   Fw 
T

J P
1 0 0  M u 
0 1 0  M v 


0 0 1   M w 
0
0
上式也可直接用虚功原理求得。
14
6-2 机器人动力学概述
一、研究目的：
1、合理地确定各驱动单元（以下称关节）的电机功率。
2、解决对伺服驱动系统的控制问题（力控制）
在机器人处于不同位置图形（位形）时，各关节的有
效惯量及耦合量都会发生变化（时变的），因此，加于各
关节的驱动力也应是时变的，可由动力学方程给以确定。
二、机器人动力学研究的问题可分为两类：
1、给定机器人的驱动力（矩），用动力学方程求解机器
人（关节）的运动参数或动力学效应（即已知  , 求  , 和，
称为动力学正问题。）。
2、给定机器人的运动要求，求应加于机器人上的驱动力
（矩）（即已知  , 和 ，求  , 称为动力学逆问题 ）。
15
三、动力学研究方法：
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!)。
16
6.3 二杆机器人的拉格朗日方程
6.3.1 刚体系统拉格朗日方程
应用质点系的拉格朗日方程来处理杆系的问题。
定义：L=K-P
L—Lagrange函数；K—系统动能之和；P—系统势能之和。
 系统的动能和势能可在任何坐标系（极坐标系、圆柱坐
标系等）中表示 ，不是一定在直角坐标系中。
动力学方程为：
d L L
i 

dt qi qi
广义力
广义速度
广义坐标
（力或力矩）（  或 v ） （  或 d ）
17
6.3.2 刚体系统拉格朗日方程
设二杆机器人臂杆长度分别为 m1, m2 ，质量
分别集中在端点为 d1, d2 ，坐标系选取如图。
以下分别计算方程中各项：
一、动能和势能
1
p  mgh
K  mv 2
2
对质点 m1 ：
1
1
1
2
2 2
k

m
v

m
(
d


)

m
d
动能： 1 2 1 1 2 1 1 1
1 1 1
2
势能： p1  m1 g  d1 cos(1 )
（负号与坐标系建立有关）
1
1
对质点 m2 ： 先写出直角坐标表达式：
x2  d1 sin(1)  d 2 sin( 1 2 )
y2  d1 cos(1)  d 2 cos(1   2 )
18
对 x 求导得速度分量：
x2  d1 cos(1)1  d 2 cos( 1 2 )(1  2 )
y 2  d1 sin(1)1  d 2 sin(1   2 )(1  2 )
v22  x22  y22  d1212  d 22 (12  212  22 )  2d1d 2 cos( 2 )(12  12 )
动能：
1
1
K 2  m2d1212  m22d 22 (12  212  22 )  m2d1d 2 cos( 2 )(12  12 )
2
2
势能：
P2  m2 gd1 cos(1)  m2 gd2 cos(1   2 )
二、Lagrange函数
L  K  P  (k1  k2 )  ( p1  p2 )
1
1
 (m1  m2 )d1212  m2 d22 (12  21 2   22 )  m2 d1d2 cos( 2 )(12  1 2 )
2
2
(m1  m2 ) g  d1 s(1 )  m2 gd2 cos(1   2 )
 L(1, 2 ,1,2 )
19
三、动力学方程
先求第一个关节上的力矩  1
( 1 
d L
L d L
L
( )
 (
)
)
dt q1 q1 dt 1 1
L
 (m1  m2 )d1212  m2d 221  m2d 222  2m2d1d 2 cos(2 )1  m2d1d 2 cos( 2 )2
1
d L
 [(m1  m2 )d12  m2d 22  2m2d1d 2 cos( 2 )]1  [m2 d 22  m2d1d 2 cos( 2 )]2
dt 1
 2m2d1d 2 sin( 2 )12  m2d1d 2 sin( 2 )22
L
 (m1  m2 ) gd1 sin(1)  m2 gd 2 sin(1   2 )
1
1 
d L
L
(
)
dt 1 
 [(m1  m2 )d12  m2 d22  2m2 d1d2 cos(2 )]1  [m2 d22  m2 d1d2 cos(2 )]2
2m2 d1d2 sin(2 )12  m2 d1d2 sin(2 )22  (m1  m2 ) g  d1 sin(1 )  m 2 g  d2 sin(1  2 )
——（1）
20
同理，对 2 和  2 微分，可求得第二关节力矩
L
 m2d 221  m2d 222  m2d1d 2 cos( 2 )1
2
d L
 m2d 221  m2d 222  m2d1d 2 cos( 2 )1  m2d1d 2 sin( 2 )12
dt 2
L
 m2d1d 2 sin( 2 )(12  1 2 )  m2 gd 2 sin(1   2 )
 2
2 
d L
L
(
)
dt 2
 2
 [m2d 22  m2d1d2 cos( 2 )]1  m2d222  m2d1d 2 sin(2 )12  m2 gd 2 sin(1   2 )
以上是两杆机器人动力学模型。
——（2）
21
四、动力学方程中各系数的物理意义
将前面结果重新写成简单的形式 ：
1  D111  D122  D11112  D12222  D11221  D
 2  D211  D222  D21112  D22222  D21212  D 22121  D2
系数 D 的物理意义：
Dii —关节 i 的有效惯量（等效转动惯量的概念）。由关节 i
处的加速度 i 引起的关节 i 处的力矩为 Diii （ M i  J ）
Dij —关节 i 和 j 之间的耦合惯量 。由关节 i 或 j 的加速度
（ i 或  j）所引起的关节 i 和 j 处的力矩为 Diji 或  ijj
Dijj —向心力项系数。表示关节 i 处的速度作用在关节 j 处的
向心力（ Dijj2j ）
Diii —向心力项系数。表示关节 i 处的速度作用在本身关节处
的向心力（ Diiii2 ）
22
Dijk —哥氏力项系数。 Dijk jk  Dijkk j 两项组合为关节 i 与
j 处的速度作用在关节 k 处的哥氏力，哥氏力是由于
牵连运动是转动造成的。
Di —关节 i 处的重力项 。重力项只与 m 大小、长度 d 以
及机构的结构图形（1, 2）有关。
比较二杆机器人例中的系数与一般表达式中的系数得到
有效惯量系数：
D11  [(m1  m2 )d12  m2 d22  2m2 d1d2 cos(2 )]
D22  m2 d22
耦合惯量系数：
D12  D21  m2 d22  m2 d1d2 cos(2 )
23
向心力项系数：
D111  D
D122   m2d1d 2 sin( 2 )
D211  m2 d1d 2 sin( 2 )
D222  0
哥氏力项系数：
D112  D121  2m2d1d 2 sin( 2 )
D212  D221  0
重力项：
D1  (m1  m2 ) g  d1 sin(1 )  m2 g  d 2 sin(1   2 )
D2  m2 g  d 2 sin(1   2 )
24
6.4 机器人的拉格朗日方程的一般表达形式
从上节容易看出Lagrange方程是一个二阶耦合、非线性和微
分方程，为简化计算，未虑及传动链中的摩擦。以下方程的推
导，也是不考虑传动链带来的摩擦影响，只考虑杆件本身，然
后再加入关节处驱动装置（如电机、码盘等）的影响。
推导分五步进行：
一、计算任意任意杆件上任意点的速度；
1
k

mi vi 2 ；
二、计算动能 i
2
三、计算势能 pi  mi ghi ；
四、形成Lagrange函数；
五、建立动力学方程 Fi 
d L
L
(
)
dt qi
qi
。
25
一、点的速度
r
由于整个系统的动能都是在基础系中考虑的，故需求系统
各质点在基础坐标系中的速度 r 。
对于杆 i 坐标系中的一点
i
r ，它在基础坐标系中的位置为
r  Ti  i r
式中 Ti —变换矩阵
其速度为：

d (Ti  r ) 
dr
r 



dt
dt

速度平方为：
i

i
j 1

Ti
 q j   ir

q j

(r )2  (r  r )2  Trace(r  r )
式中 Trace( ) —矩阵的迹，即矩阵主对角元素之和。
26
(r )2  Trace(r  r )

 Trace 




 Trace 


i
j 1
Ti
 q j  ir 
q j

i
j 1
i
k 1

i
k 1

Ti
(
 qk  i r )T 

qk

Ti
T i
 ( ir  ir )
q j
qk
T
T

q j qk 


二、动能
位于杆 i 上 i r 处质量为 dm 的质点的动能是：

1
dki  Trace 

2


1
 Trace 

2



i
i
j 1
k 1
i
i
j 1
k 1

T
T
Ti

T
i
 ( ir  ir )
q j qk  dm

q j
qk


T
T
Ti
T i
 ( i r  dm  i r )
q j qk 

q j
qk

27
则杆 i 的动能（在基础坐标系中）为：

1

ki 
dki  Trace 
2

link .i


令式中 J i 

i

i
i
j 1
k 1
T

i
r  i r dm)
T i
qk
T
link .i


q j qk 


称为连杆 i 的伪惯量矩阵。
r  r dm
i
Ti
(
q j
T
link .i
则得到杆 i 的动能为：

1
ki  Trace 

2


i
i
j 1
k 1

Ti
T T i
 Ji 
q j qk 

q j
qk

对于杆 i 上任意一点的 i r（在杆 i 坐标系中）可以表示为：
i
r  ( i x, i y, i z,1)
28
则有：
Ji 

link .i





i 2
x dm


 link .i

i

x  i ydm

T
 link .i
i
r  i r dm  

i
i
x

zdm


 link .i

i
xdm


 link .i




i
x  ydm
i
link .i
x  zdm
i
y  i zdm
i
link .i
i
y 2 dm
link .i
i




i
link .i
y  i zdm
link .i
link .i
i
z 2 dm
link .i
i
ydm
i
zdm
link .i





xdm 

link .i


i
ydm 


link .i


i
zdm 

link .i


dm 

link .i

i
根据理论力学中惯性矩、惯性积和静矩的定义，引入下列记号：
对坐标轴的惯性矩：
I xx 

( y 2  z 2 )dm, I yy 

( x 2  z 2 )dm, I zz 

( x 2  y 2 )dm
29
对坐标轴的惯性积：
I xy 

xydm, I yz 

yzdm, I xz 

z
xzdm
对坐标轴的静矩：
Ix 



ydm, I z 
m

dm
1
2

( y 2  z 2 )dm 
于是：
x 2 dm  

y
x
xdm, I y 
质量之和：
r
 I xx  I yy  I zz
2
1
2


zdm
( x 2  z 2 )dm 
1
2

( x 2  y 2 )dm
30
同理：

y dm 
2
I xx  I yy  I zz
2
,

z dm 
2
I xx  I yy  I zz
2
于是 J i 能够表达为：
  I ixx  I iyy  I izz

2


I ixy
Ji  


I ixz


I ix

I ixy
I ixz
I ixx  I iyy  I izz
2
I iyz
I ixx  I iyy  I izz
I iyz
2
I iz
I iy

I ix 


I iy 


I iz 

mi 
机器人臂杆总的动能是：

n
K 
i 1

n
1
ki 
2
i 1

Trace 



i
i
j 1
k 1

Ti
T T i
 Ji 
q j qk 

q j
qk

31
如果考虑到关节处驱动装置的动能：
1
kmotor .i  J ai qi2
2
1
（对于移动关节：kmotor .i  mai qi2 ）
2
式中 J ai 为关节 i 处驱动装置的转动惯量。
调换求迹与求和运算顺序，并加入关节处驱动装置的动能，
得到机器人总的动能为：

n
K 
1
2
i
i
Trace(
i 1
j 1
k 1
Ti
T i
1
 Ji 
)q j qk 
q j
qk
2
T

n
J ai q i2
i 1
三、势能
设杆 i 的质心在再其自身坐标系的位置向量为 i rC ，则它在
基础坐标系中的位置向量 rC 为
T
i
i
i
i
i


( rC   xC , yC , zC ,1 )
rC  Ti  rC
32
设重力加速度
g
在基础坐标系中的齐次分量为：
g  ( g x , g y , g z ,1)T
则杆在基础坐标系中的势能为：
pi  mi g T  rC  mi g T  Ti  i rC
（一般认为基础坐标系的 z 轴取向上方）
于是机器人的总势能为：

n
P
i 1

n
pi  
mi g T  Ti  i rC
i 1
33
四、拉格朗日函数
1
L  K P 
2

 
n
i
i
i 1
j 1
k 1
n
1

2
Ti
T T i
Trace(
 Ji 
)q j qk
q j
qk
n
J ai qi2 
i 1
mi g T  Ti  i rC
i 1
五、动力学方程
先求拉格朗日方程中的各项：
L
1

q p
2


1

2
n
i
i 1
n
k 1
i
i 1
j 1
Ti
TiT
Trace(
 Ji 
)qk
q p
qk
Ti
TiT
Trace(
 Ji 
)q j  I a p q p
q j
q p
（1 )
34
由于 J i 是对称矩阵，则有：
Ti
TiT
Ti
TiT T
Trace(
 Ji 
)  Trace(
 Ji 
)
q p
qk
q p
qk
T
Ti

T
 Trace(
 J iT  i )
qk
q p
Ti
TiT
 Trace(
 Ji 
)
qk
q p
合并(a)式中前两项，得到：
L

q p

n
i
i 1
k 1
Ti
TiT
Trace(
 Ji 
)qk  I a p q p
qk
q p
T
当 i  p 时，Ti 中不包含 p 以后关节变量，即： i  0, (i  p)
q p
n
i
于是可得：
Ti
TiT
L

Trace(
 Ji 
)qk  I a p q p （1’)
q p

i p
k 1
qk
q p
35
d L
(
)
dt q p



n
i
i p
k 1
Ti
TiT
Trace(
 Ji 
)qk  I a p q p
qk
q p
n
i
i
i p
k 1
m 1
n
i
i
i p
k 1
m 1


 2Ti
TiT
Trace(
 Ji 
)qk qm
qk qm
q p
 2Ti
TiT
Trace(
 Ji 
)qk qm
q p qm
qk
交换其中的部分哑元，得到：
d L
(
)
dt q p


n
i
i p
n
k 1
i
Ti
TiT
Trace(
 Ji 
)qk  I a p q p
qk
q p
i
2
i p
k 1
m 1
 2Ti
TiT
Trace(
 Ji 
)qk qm （2 )
qk qm
q p
36
L
1

q p
2



n
i
i
i p
j 1
k 1
1

2
n
i
i
i p
n
j 1
k 1

mi g T 
i p

 2Ti
TiT
Trace(
 Ji 
)q j qk
q j q p
qk
Ti i
 rC
q p


n
i
i
i p
j 1
k 1
n

 2Ti
TiT
Trace(
 Ji 
)q j qk
q j q p
qk
Ti i
mi g 
 rC
q p
T
i p
 2Ti
TiT
Trace(
 Ji 
)q j qk
qk q p
q j
（3 )
37
将以上各项带入拉格朗日公式，并用 i 和 j 分别代替上式中
的哑元 p 和 i ，得到：



j
n
i 
Trace(
j i
n
k 1
j
qk
Jj 
j

Trace(
j i
n

T j
k 1
m 1
mj g 
T
j i
T j
qi
T j T
qi
 2T j
qk qm
)qk  I ai qi
Jj 
T j T
qi
)qk qm
 i rC
（4 )
上式为拉格朗日方程的最后形式。这些方程与求和的次序无
关，因此可将上式写为简化形式：

n
i 
j 1

n
Dij q j  I ai qi 
j 1
n
k 1
Dijk q j qk  Di
（5 )
38
式中：

n
Dij 
Trace(
p  max( i , j )

Tp
q j
n
Dijk 
Trace(
p  max( i , j , k )

n
Di 
p i
(m p g 
T
Jp 
TpT
 2Tp
q j qk
Tp
qi
q j
)
Jp 
TpT
qi
)
 p rC )
 以上的动力学方程 (5)中系数 D的意义与上节所列相同，
i j
i  j ），
即分别为有效惯量项系数( i  j ),耦合惯量项系数（
向心力项系数（ k  j ），哥氏力项系数（ k  j ），重力项
等。
39
 动力学方程中的惯量项和重力项在机器人控制重特别重要，
将直接系统的稳定性和定位精度。只有当机器人高速运动时，
向心力项和哥氏力项才是重要的。传动装置的惯量值往往较
大，对系统动态特性的影响也不可忽略。
在机器人动力学问题的讨论中，拉格朗日动力学方程常写
作更简化的一般形式：
 (t )  D(q(t ))q(t )  h(q(t ), q(t ))  c(q(t )) （6)
式中：
 (t )  (1 (t ), 2 (t ), , n (t ), )T
q(t )  (q1 (t ), q2 (t ), , qn (t ), )T
q(t )  (q1 (t ), q2 (t ), , qn (t ), )T
q(t )  (q1 (t ), q2 (t ), , qn (t ), )T
D(q(t )), h(q(t ), q(t )), c(q(t )) 的意义见（5 )式。
40
6.5 机器人的牛顿—欧拉方程
机器人的拉格朗日动力学模型为非线性二阶常微分方程，
利用这些方程，由已知的每一轨迹设定点的关节位置、速度
和加速度，可以计算各关节的标称力矩，但拉格朗日方程利
用4×4齐次变换矩阵，使得计算效率太低。
乘法次数：128 / 3 n4   512 / 3 n3  844 / 3 n2   76 / 3 n
加法次数： 98 / 3 n4   781/ 6  n3   637 / 3 n2  107 / 6  n
为了实现实时控制，曾用过略去哥氏力和向心力的简化模
型，但当操作机快速运动时，哥氏力和向心力在计算关节力
矩中是相当重要的。因而这种简化只能用于机器人的低速运
动，在典型的制造业环境中，这是不合乎要求的。此外，这
种简化所引起的关节力矩误差，不能用反馈控制校正。
牛顿—欧位法采用迭代形式方程，计算速度快，可用于实
时控制，因而成为一种常用的建模方法。
41
6.5.1 转动坐标系
寻求转动坐标系和固定惯性坐标系之间必要的数学关系, 再
推广到运动坐标系(转动和平移)和惯性坐标系之间的关系。
如图, 惯性坐标系O-XYZ和转动坐标系O-X*Y* Z* 的原点重合
于O点。而OX*、OY*、OZ*轴相对OX、OY、OZ轴旋转。设
(i , j , k ) 和 (i  , j  , k  ) 分别为这两个坐标系沿主轴的单位矢
量。转动坐标系中点 P 可用它在任一坐标系中的分量来表示：
r  xi  yj  zk 或 r  xi   y j   zk 
在惯性坐标系中的运动：
Z*
dr dx
dy
dz

i
j
k  xi  yj  zk
dt
dt
dt
dt
X
在转动坐标系中的运动：
d  r dx  dy  dz 

i 
j 
k  x i   y  j   z  k 
dt
dt
dt
dt
Y*
Z
P
r
O
Y
X*
42
在惯性坐标系中的运动：



dr dx  dy  dz 
 di
 dj
 dk

i 
j 
k x
y
z
dt
dt
dt
dt
dt
dt
dt



d r
 di
 dj
 dk

x
y
z
（7 )
dt
dt
dt
dt
需要解决转动坐标系坐标轴在惯性坐标系中的导数问题。我
们假定，转动坐标系绕着过原点O的某轴OQ以角速度  旋转。
方向沿OQ轴，指向转动坐标系右旋方向。则可以证明转动坐
标系中的任意固定矢量 s 在惯性坐标系中的导数为：
Z
ds
s
dt
Y*
s
Z*
X
O
Q
Y
X*
43
于是由（6 )式可得：
dr d  r

 x (  i  )  y (  j  )  z (  k  )
dt
dt
dr
d r

  r
（8)
dt
dt
这是建立转动坐标系两种时间导数之间关系的基本方程。
d 2r
d d r
dr d

(
)




r
2
dt dt
dt
dt
dt
d 2 r
d r
d r
d







(



r
)

r
2
dt
dt
dt
dt
d 2r
d 2 r
d r
d

 2 
   (  r ) 
r
（9 )
2
2
dt
dt
dt
dt
方程（9 )又被称为哥氏定理。
44
6.5.2 运动坐标系
如图,运动坐标系O-X*Y* Z* 相对于惯性坐标系O-XYZ转动和平
移。质量为M 的质点P 分别以 r 和 r  确定相对于惯性坐标系
和运动坐标系的原点的位置。原点O* 相对于原点O的位置以矢
*
Y
量 h 表示。则有：
r  r  h
Z*
O*

*
P
dr dr
dh
X

r*
v(t ) 


( v  vk )
r
dt
dt
dt
h
Z
 
d r
dh

v(t ) 
  r 
（10 )
dt
dt
Y
X O
dv(t ) d 2 r  d 2 h

a(t ) 


(

a
 ak )
2
2
dt
dt
dt
d 2 r 
d r 
d
d 2h



 2 
   (  r ) 
 r  2 （11 )
2
dt
dt
dt
dt
45
6.5.3 杆件运动学
根据前述运动坐标系的概念，推导一组数学方程，描述机
器人的运动杆件相对于基础坐标系的运动学关系。
坐标系 ( x0 , y0 , z0 ) 是基础坐标系，而坐标系 ( xi 1 , yi 1 , zi 1 )
和 ( xi , yi , zi ) 分别固联于杆件 i -1和杆件 i 上，原点分别为
Oi-1和 Oi 。原点 Oi 相对于原点 O 和原点 Oi-1 的位置分别用
位置矢量 pi 和 pi 表示。原点 Oi-1相对于基础坐标系原点 O
的位置用位置矢量 pi 1 表示。
令 vi 1和 i 1 分别为坐标系 ( xi 1 , yi 1 , zi 1 ) 相对于基础坐标
系 ( x0 , y0 , z0 ) 的线速度和角速度。令  i 和  i 分别为杆件i
坐标系 ( xi , yi , zi ) 相对于基础坐标系 ( x0 , y0 , z0 ) 和杆件 i -1
坐标系 ( xi 1 , yi 1 , zi 1 ) 的角速度。则杆件 i 坐标系 相对于基
础坐标系的线速度 vi 和角速度  i 分别是：
46
d  pi
vi 
 i 1  pi  vi 1
dt
i  i 1  i
（12 )
（13 )
式中 d*(.)／dt 表示在运动坐标系 ( xi 1 , yi 1 , zi 1 ) 的时间导数。
47
 
d 2 pi
d
pi


vi 



p

2





(


p
i 1
i
i 1
i 1
i 1
i )  vi 1 （14 )
2
dt
dt
i  i 1  i
i 为坐标系 ( xi , yi , zi ) 相对于 ( xi 1 , yi 1 , zi 1 ) 的角加速度:



d

d

i
i
 i 

 i 1  i 
dt
dt
d i 
i  i 1 
 i 1  i 
dt
（15 )
根据机器人杆件坐标系建立的步骤和参数的定义，杆件 i 在
杆件 i –1 坐标系中的运动是沿 zi 1 方向的平移或绕 zi 1 转动。
因此，
zi 1qi 若杆件 i 转动

i 
（16 )
0
若杆件 i 平移
式中 q 是杆件 i 相对于杆件 i -1 坐标系的角速度值。
48
类似地
d i 
z q
 i 1 i 若杆件 i 转动
0
dt
若杆件 i 平移
（17 )
由式（13 )和式（16 )有：
i 1  zi 1qi 若杆件 i 转动
i  
i 1
若杆件 i 平移
（18 )
由式（15 ) 、式（16 )和式（17 )有：
i 1  zi 1qi  i 1  ( zi 1qi )
i  
i 1
若杆件 i 转动
若杆件 i 平移
（19 )
由式（15 ) 、式（16 )和式（17 )有：
i  pi 若杆件 i 转动
d  pi
（20 )
 z q
若杆件 i 平移
i 1 i
dt
 
d
i
2 
 pi  i  (i  pi ) 若杆件 i 转动
d pi
（21 )

dt
2
若杆件 i 平移
dt
zi 1qi
49
由式（12 ) 、式（13 )和式（20 )有：
i  pi  vi 1
vi 
zi 1qi  i  pi  vi 1
若杆件 i 转动
若杆件 i 平移
（22 )
利用矢量叉乘积的恒等式
(a  b )  c  b (a  c )  a(b  c )
a  (b  c )  b (a  c )  c (a  b )
并根据式（14 ) 、式（15 )和式（19 )有：
vi 
i  pi  i  (i  pi )  vi 1
若杆件 i 转动
zi 1q  i  pi  2i  ( zi 1q)  i  (i  pi )  vi 1
若杆件 i 平移
—（23 )
50
6.5.4 牛顿——欧拉法基本运动方程
刚体的运动可分解为随质心的移动和绕质心的转动。借助于
杆件运动学知识，我们把达朗贝尔原理用于每个杆件，描述机
器人各杆件的运动。达朗贝尔原理可应用于任意瞬时，它实质
上是牛顿第二运动定律的一种变型，可表示为：
d  mi vi 
( Fi 
)
牛顿定理 :
Fi  mi vi
dt
欧拉方程 :
Ni  Iii  i  ( Iii )
d  I ii 
( Ni 
)
dt
式中：mi — 杆i 质量；
Fi — 杆i上所有外力合力；
Ni — 杆i上所有外力对质心的合力矩；
I i — 杆i 绕其质心惯性矩阵。
51
zi
zi 1
f i ,i 1
yi 1
f i 1,i
N i ,i 1
yi
N i 1,i
oi
oi 1
ri ,Ci
xi 1
xi
mi g
根据力（矩）平衡原理，在质心处有：
Fi  fi 1,i  fi ,i 1  mi g
则有
Ni  Ni 1,i  Ni ,i 1  (ri ,ci )  ( fi ,i 1 )  ri 1,ci  fi 1.i
fi 1,i  fi ,i 1  mi g  mi vi  0
Ni 1,i  Ni ,i 1  (ri ,ci )  ( fi ,i 1 )  ri 1,ci  fi 1.i  Iii  i  ( Iii )  0
方程（24 )即为牛顿——欧拉法的基本方程。
（24)
52
6.5.5 递推形式的牛顿——欧拉方程
上面推导的牛顿——欧拉法（也简称N-E法）方程式含关节
联接的约束力（矩），没有显示地表示输入—输出关系，不适
合进行动力学分析和控制器设计。如果变换成由一组完备且独
立的位置变量（质心位置变量通常不是相互独立的）和输入力
来描述，这些变量都显式地出现在动力学方程中，即得到显式
的输入——输出形式表示的动力学方程，称为封闭形式的动力
学方程（拉格朗日方程即是封闭的）。
关节变量 q 是一组完备且独立的变量，关节力（矩） 是
一组从约束力（矩）中分解出来的独立的输入，所以用 q 和 
来描述方程，可以得到封闭形式的动力学方程。
53
根据N-E法的基本方程，利用质心运动变量与关节变量及关
节运动变量之间的关系以及约束力与关节力矩之间的关系，消
去中间变量，可以得到封闭形式的动力学方程。但显然不如用
拉格朗日法简单，特别是当机器人自由度较多时，更是如此。
因此，对于N-E法，常用的不是它的封闭形式方程，而是它
的递推形式方程。方程（24）可直接写成如下递推形式：
fi 1,i  fi ,i 1  mi vi  mi g
Ni 1,i  Ni ,i 1  ri ,ci  fi ,i 1  ri 1,ci  fi 1.i  Iii  i  ( Iii )  0
而关节力（矩）可写成如下形式：
T

ˆ
z
对于旋转关节
 i 1  Ni 1,i  bi qi
i   T
对于移动关节

 zi 1  fi 1,i  bi qi
式中，zˆi 1为沿关节轴线 z i 1 的单位矢量，
bi 为关节的粘滞阻尼系数。
（25)
（26)
54
递推形式的N-E法方程与封闭形式方程比较，计算量从 O(n4 )
减少到 O(n) ：
乘法次数：117n – 24，
加法次数：103n - 21，
从而大大加快了计算速度。自由度越多，递推形式的优势越明
显。对于典型 n=6 的情形，递推形式的计算效率几乎提高10倍。
因此，常用于实时计算。
递推形式方程的特点是其计算从机器人操作机的一个杆到另
一杆逐个顺序进行的，它充分利用了操作机的串联链特性，常
用于求解动力学逆问题（即已知  ,,，求  ）。
求解的大致过程为：根据运动和力的不同传递方向，进行运
动量的向前迭代和力学量的向后迭代。
具体步骤如下：
55
1．确定计算N-E方程所需的所有运动量，包括每个杆件的
（ vci , wi , vci , w i）由杆1
杆n：
q1, q1 , q1  vc1 , vc1 , w1 , w1  vc 2 , vc 2 , w2 , w2 


q3 , q3 , q3 
q1 , q2 , q2 
 vcn , vcn , wn , wn
2. 将上述运动量代入N-E方程，确定关节力（矩）。计算顺
序与运动量计算相反，由杆n
杆1：
f n 1,n  f n,n 1  mn g  mn vcn  f n 1,n  f n 2,n 1 
 f 0,1
Nn1,n  Nn,n1  rn,ci  f n, n1  rn1,ci  f n1.i  I nn  n  ( I nn )  Nn 1,n  Nn 2,n 1 
 N0,1
1
n 1
2
nn
运动学计算
0
i
动力学
计算
 N n, n 1
 f n , n 1
56
6.5.6 在杆件自身坐标系中的递推方程
前述递推运动方程的明显缺点是所有惯性矩阵和物理几何
参数（如 ri ,ci , ri ,i 1 , zˆi ）等，都是以基础坐系为参照的，因此，
当机器人运动时，它们也随着变化。Luh等人改进了上述N-E
方程，将所有杆件的速度、加速度、惯性矩阵、质心位置、
力和力矩等，都表示在各杆的自身坐标系中，从而使计算更
加简单。
这种改进的最重要的成果是，计算关节驱动力矩的时间不
仅与机器人关节数成线性比例，而且与机器人构型无关。这
就有可能在关节变量空间实现机器人的实时控制算法。
设 i 1Ri 是3×3旋转矩阵，它把矢量由坐标系 ( xi , yi , z i ) 变换
到坐标系 ( xi 1 , yi 1 , zi 1 ) 中。
57
这样，可不计算相对基础坐标系的 i , i , vi , vi , vci , fi 1,i 和 N i 1,i
等，而是直接计算在杆件自身坐标系中的 ii , ii , i vi , i vi , i vci , i fi 1,i
i
和 Ni 1,i 等。
于是有关运动量的递推公式变为：
i 1
i
i

若杆件 i 转动
ˆi )
R
(


q

i 1
i
i
i 1 z
i 1  
i 1 i
若杆件 i 平移

i R i
i 1
i
i
i
i

ˆ
ˆi )] 若杆件 i 转动
R
[


q

z



(
q

i 1
i
i
i 1
i
i
i 1 z
i 1  
i 1
i
若杆件 i 平移
R

i

i

i 1
若杆件 i 转动

Ri i vi  i 1i 1  i 1ri.i 1  i 1i 1  [ i 1i 1  i 1ri ,i 1 ]
 i 1 i
i 1
vi 1   Ri ( vi  qi 1  i zˆi )  i 1i 1  i 1ri ,i 1  2  i 1i 1  (qi 1  i 1Ri  i zˆi )
若杆件 i 平移
i 1
i 1
i 1

 i 1  ( i 1  ri ,i 1 )

i
vci  i vi  ii  i ri ,ci  ii  ( ii  i ri ,ci )
58
关节间约束力公式变为：
i
i
fi 1,i 
i
i 1
R i 1 fi ,i 1  mi  i vci
Ni 1,i  i Ri 1 ( i 1Ni ,i 1  i 1ri 1  i 1 fi ,i 1 )  ( i ri 1,i  i ri ,ci )  (mi  i vci )
 Ii  ii  ii  ( Ii  ii )
i T
i
i 1

N
(
R

zˆi 1 )
 i 1,i i 1
i   i T i
i 1
f
(
R

zˆi 1 )

 i 1,i i 1
因此，概括地说，高效的牛顿一欧拉运动方程是一组正向和
反向的递推方程，每一杆件的动力学和运动学参数都是以其自
身坐标系为参照的。
59
6.6 机器人的凯恩方程法简介
凯恩(Kane)方法是用来建立机器人机构动力学模型的一种普
遍方法，其基本思想是以广义速率代替广义坐标作为系统的独
立变量，它用达朗倍尔原理及虚位移原理建立动力学方程。凯
恩方法既适合于完整系统，也适合于非完整系统。
6.6.1 广义速率和偏速度及偏角速度
一、广义速率
一个具有n 个自由度的完整系统，相对于惯性坐标系
( x0 , y0 , z0 ) 的运动一般通过 n 个独立的广义坐标 qi (i  1, 2, , n)
来描述 。 n 个广义速度 qi (i  1, 2, , n) 也是独立的，故可用
n 个广义速度的线性组合，即n 个广义速率(或称准速度)
ui (i  1, 2, , n) 来描述系统的运动，即
60

n
ui 
ari qi  ar
(r  1, 2, , n)
（27)
i 1
式中：ari , ar 为 qi 及 t 的函数；而由 ari 组成的系数矩阵应为非
奇异阵。则有

n
qi 
 ir ui  bi
(i  1, 2, , n)
（28)
r 1
二、偏速度
系统中任意质点 p 的径矢为
r , r 为广义坐标
qi 及t 的函数，
rp  r (q1 , q2 , , qn , t )
则该点的速度为

n
v p  rp 
i 1
r
r
qi 

qi
t

n
n
(
r 1
i 1
r
 ir )ur 
qi

n
i 1
r
r
bi 
qi
t
61
令

n
vp 
vr (r )  ur  vt
（29)
r 1
其中


n
vr (r ) 
i 1
n
vt (r ) 
i 1
r
 ir
qi
（30)
r
r
bi 
qi
t
（31)
称广义速率 ur 前的系数矢量 vr (r ) 为 p点相对于惯性坐标系
的第 r 偏速度。一般说来它是广义坐标 qi 和时间t 的函数。对
于定常系统 vt  0 ，偏速度只是广义坐标的矢量函数。
三、偏角速度
  1e1  2 e2  3e3 

n
 r (e )  u r   t
r 1
62
从式 (30) 可见，广义速率的取法不同，系统内同一质点及
同一刚体的偏速度也可不同。故可通过一定的技巧来选取广
义速率，使得到的偏速度和偏角速度具有最简单的形式。最
理想的是使较多的偏速度和偏角速度等于零，这样有利于简
化动力学方程。
一般选择与系统的运动密切相关的量，如选取系统中刚体
的角速度分量或质点的速度分量为广义速率，以使刚体的角
速度和质点的速度具有最简单的表达形式。作为特例，如果
取广义速度为广义速率，即
ui  qi
dr
r
r
r
r
v

q1 
q2   
qn 
dt q1
q2
qn
t
可见对于广义速度 qi
r
的偏速度为
qi
。
63
四、刚体各点偏速度
v pr (r )  vCr (r )  r  rp
（32)
6.6.2 凯恩动力学方程
由达朗倍尔原理和虚位移原理推得的系统的动力学普遍方程：

( Fp  m p rp )   rp  0
（33)
p
系统中 p 点的速度，对于具有n 个自由度的系统，可写成
广义速率的线性组合， 即

n
drp
dt
 vp 
v rpr(r )  ur  vtpt
r 1
64

n
drp 

n
v rpr(r )  ur  dt  vtpt dt 
r 1
v rpr(r )  d r  vtpt dt （34)
r 1
式中 d r  ur  dt 。如果 ur 可积分，则  r 为另一种定义
的广义坐标。
从式 (34) 可得到 rp 的变分

 rp ，即
n
 rp 
v rpr(r )   r
r 1
将上式代入式 (33) 得

p




n
r 1

( Fp  m p rp )  v rpr(r )    r  0


65
改变求和的形式得





n
r 1
令
Fp  v rpr(r ) 
p

Fr  



p

称为系统对应于 ur
p

m p rp  v rpr(r )    r  0


（34)

Fp  v rpr(r ) 


的广义主动力

Fr   
m p rp  v rpr(r )
p
称为系统对应于 ur
的广义惯性力
66
这样，式（34）成为：

n
 Fr  F r    r  0


r 1
由于  r 为独立的变分，所以有
Fr  F r  0
r  1, 2, , n
（35)
上式称为凯恩动力学方程，意为广义上动力与广义惯性
力之相等于零。
凯恩方程法可以得到封闭形式的动力学方程。
67
6.7 弹性机器人动力学简介
6.7.1 机器人系统的弹性问题
通常，机器人机构的手臀、驱动、传动元件被假设为刚性
的，故系统的建模及控制方案设计都是以这样一个刚性假设
为前提的。结构刚性越好越不易振动，机器人精度越高，经
过近似处理的控制方法也越精确、可行。
随着工业的发展，对机器人性能要求越来越高。如：要求
机器人重量轻以降低能耗；运行速度快，以提高劳动生产率；
定位精度高，以适应更多的精密作业要求。另外，对于如太
空机器人所处的特殊环境要求具有超长手臂的机器人，其结
构已不再是刚性结构，而必须计入曾被忽略的系统弹性或非
线性因素，这便使系统设计、建模及控制变得更为复杂。
68
关于具有弹性的机器入系统的建模与控制问题的研究历史
不长，始于70年代初， 研究具有弹性附件的飞机动力学模
型的动力学分析问题，计入分布质量和柔性效应的机器人机
构在低速运动下的动力学，对具有分布质量和弹性影响的二
连杆机械手提出反馈控制方法。近年来，关于这方面的研究
发展很快，大多以研究单弹性臂机器人的建模与控制入手。
本节将以单弹性臂机器人机构为例，介绍弹性手臂机器人
的建模方法。
6.7.2 机器人机构的弹性及非线性问题
机器人系统的弹性主要来源于两个方面，①由于臂本身的
弹性而引起的变形及运行过程中的振动。②由驱动系统的弹
性及非线性因素而引起的振动。
在机器人驱动系统中，存在着轴与轴承之间的库仑摩擦，
传动装置中的齿轮间隙及谐波齿轮减速器的弹性等因素。 69
6.7.3 主要研究内容
一、建模方法。
弹性手臂机器人是一分布参数系统，其运动特性由一组非
线性偏微分方程描述。在系统仿真和应用设计时必须作有限
维的近似，并在不影响系统稳定性性态的前提下化为可实际
控制的系统状态方程。
解决弹性手臂机器人的建模问题的各种建模方法，在不同
程度上部有一定的近似性。准确性及可行性是衡量一种方法
好坏的标准，就现有文献资料大致可归纳为以下几种：
（1） 拉格朗日方程结合模态展开法；
（2） 拉格朗日方程结合有限元素法；
（3） 牛顿一欧拉方程结合模态展开法；
（4） 牛顿一欧拉方程结合有限元素法；
（5） 哈密顿原理结合模态分析法。
70
二、控制方程
现在应用于机器人控制中的最佳控制法、极点配置法均以
系统不产生振荡为前提，而弹性手臂机器人系统对其控制器
的要求就是要能控制其弹性振动。因而有效可行的控制方法
的研究很有必要。
对控制方案的要求：
（1） 必须同时控制刚体运动及弹性振动而控制输出量仅
作用于关节处；
（2） 避免溢漏现象；
（3） 控制方案所要求的传感器数量应尽可能少，且安装
简单、方便。
三、带有弹性构件的新型机械结构。
71
6.7.4 单弹性臂机构的建模
设T1为驱动部分动能、 T2为手臂部分动能、 T3为
负载动能，则
72
d L
L
Fi 
(
)
dt qi
qi
73