第 4 章机器人动力学

第 4 章机器人动力学

机器人动力学研究内容正问题：已知作用在机器人机构上的力和力矩，求机器人机构各关节的位移、速度、加速度反问题：已知机 ` 器人机构各关节的位移、速度和加速度，求作用在各关节上的驱动力或驱动力矩

4.1 引言

机器人动力学研究方法动力学模型：根据机构的结构和运动学、动力学关系建立起来的一组动力学方程数学工具：矢量方法、张量方法、旋量方法及矩阵方法等力学原理：能量守恒定理、达朗贝尔原理、虚功原理、拉格朗日方程、动量矩定理、哈密尔顿原理、牛顿－欧拉方程、凯恩方程等


牛顿—欧拉方程刚体随质心的平动可以用牛顿方程，绕质心的转动可以用欧拉方程分别建立其力学模型。

4.2 牛顿—欧拉方程法

牛顿方程和欧拉方程是建立在牛顿第二定律基础之上的，即通过力、力矩，动量和动量矩等物理量来描述刚体的动力学性能。


如图 4-1 所示，设刚体的质量为，刚体坐标系下的惯量张量由六个量组成，可用 3×3 对称矩阵表示为

4.2.1 惯量张量

图 4.1

C

x

y

z X

Y

Z

O

r

pc

P m

x’

y’

z’

mCxyz

CI

x xy xz

C yx y yz

zx zy z

I I I

I I I I

I I I

2 2 2 2( ) ( )x i i im y z y z dm I

2 2 2 2( ) ( )y i i im z x z x dm I

2 2 2 2( ) ( )z i i im x y x y dm I

xy yx i i i i im x y x y dm I I

yz zy i i i i im y z y z dm I I

zx xz i i i i im z x z x dm I I

式中


已知相对于某一坐标系的惯量张量，可根据平行轴定理及相似变换求得相对于另一坐标系的惯量张量。若两坐标系之间有平行移动关系

4.2.1 惯量张量

2 2( )X x c cm X Y I I

2 2( )Y y c cm X Z I I

2 2( )Z z c cm Y Z I I

XY xy c cmX Y I I

YZ yz c cmY Z I I

XZ xz c cmX Z I I

若两坐标系之间有旋转变换关系 '

TC CI RI R

主惯量

0xy yz xz I I I

0 0

0 0

0 0

x

C y

z

I

I I

I


如图 4-1 所示，假设刚体的质量为，质心在 C 点，质心处的位置矢量用表示，质心处的加速度用表示，绕质心的角速度用表示，则绕质心的角加速度为，根据牛顿方程可得作用在刚体质心 C 处的力为

4.2.2 牛顿—欧拉方程

mF c

C CM = I ε + ω× I ω

根据欧拉方程作用在刚体上的力矩为

图 4.1

C

x

y

z X

Y

Z

O

r

pc

P m

x’

y’

z’

mc c

ω

以上两式合称为牛顿—欧拉方程。

ε

欧拉方程式可由动量矩定理推得，设 P 点为刚体上的任意一点，其质量为，相对于质心 C 的矢径为，速度为，刚体相对于质心 C 的动量矩为


4.2.2 牛顿—欧拉方程

由于

即

im ir

i i v ω r

( )c i i i i i im m H r v r ω r

( ) ( ) ( )i i i i i i r ω r r r ω r ω r

2

( ) ( )

( )

c i i i i i

i i i i i

m

m m

H r r ω r ω r

r ω r ω r

所以

( )

( )

( )

c x x xy y xz z

yx x y y yz z

zx x zy y z z

I I I i

I I I j

I I I k

H

写在矩阵形式为

x xx xy xz

y C yx y yz y

zx zy zz z

H

H

H

I I I

H I ω I I I

I I I

根据动量矩定理

1 2 3

yx z

C C

dHdH dHdi j k

dt dt dt dt

di dj dkH H H

dt dt dt

HM

I ε + ω× I ω


4.2.3 作用力和力矩

1,i if

, 1i i f

, 1i i m

1,i im

ifim

imiI

iC

iL

ilih

图 4.2 构件受力图

iL如图 4.2所示，将第 i 个构件作为

隔离体进行分析，作用在其上的外力有：

1,i if 1iL iL—— 构件作用在构件上的力

1,i im 1iL iL—— 构件作用在构件上的力矩

1,i if 1iL iL—— 构件作用在构件上的力

1,i im 1iL iL—— 构件作用在构件上的力矩

if iL iC—— 作用在第 i 个构件上的外力化简到质心处的合力，即外力的主矢

im iL iC—— 作用在第 i 个构件上的外力矩化简到质心处的合力矩，即外力的主矩


4.2.3 作用力和力矩上述力和力矩包括了运动副中的约束反力、驱动力、摩擦力等引起的作用力和作用力矩。作用在第 i 个构件上的所有力（包括约束反力、驱动力、摩擦力、重力、外力等）化简到质心的总的合力为

1, , 1i i i i i i F f f f

按牛顿方程有

i i imF c

所以

1, , 1i i i i i i im f f f c

相对于质心的总的合力矩为 1, 1,

, 1 , 1

i i i i i i

i i i i i i

M = m f l

m f h m

按欧拉方程有

i Ci i i Ci i M = I ε + ω × I ω

xi xyi xzi

Ci yxi yi yzi

zxi zyi zi

I I I

I I I I

I I I

为构件相对于其质心的惯性张量

式中


4.2.4 应用牛顿—欧拉方程法建立机器人机构动力学方程

1C

2m

X

Y

O

1

2

1l

2l

1h

2h

A

B2L

1L1m

2C

图 4.3 平面两自由度机器人机构

1L 1C 1m

1 11{0 0 }Tmm

1 1{0 0 }ω

1 1{0 0 }Tε

2L 2C 2m

2 22{0 0 }Tmm

2 2{0 0 }ω

2 2{0 0 }Tε

例 4.1 如图 4.3 所示的平面两自由度机器人机构，连杆质心为，质量为，驱动力矩为，角速度为，角加速度为。连杆质心为，质量为，驱动力矩为角速度为，角加速度为。



O A 1 2

1L

选取关节和关节处的转角，为系统的广义坐标，可以写出连杆的牛顿—欧拉方程为

0,1 1,2 1 1 1m f f f c

0,1 0,1 1 1,2 1,2 1 1 1C m f l m f h I ε

2L连杆的牛顿—欧拉方程为

1,2 2 2 2m f f c

1,2 1,2 2 2 2C m f l I ε

式中1 1m gf 2 2m gf

0,1 1m m 1,2 2m m

由以上几式可解得

2 2 2 2 2 2 2( )C m m g m I ε c l

1 1 1

1 1 1 2 2 2 1

2 2 2 1 2

( )

( )

C

m m g m m g

m m g

m I ε

c c l

c h m

考虑到

1 1

1 1 1

sin

cos

0

l

l

c

1 1 2 1 2

2 1 1 2 1 2

sin sin( )

cos cos( )

0

L l

L l

c



所以

1 1 1

1 1 1 1

cos

sin

0

l

l

c

21 1 1 1 1

21 1 1 1 1 1

( sin cos )

( cos sin )

0

l

l

c

1 1 1 2 1 2 1 2

2 1 1 1 2 1 2 1 2

cos ( )cos( )

sin ( )sin( )

0

L l

L l

c

2 21 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2

2 22 1 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2

sin ( ) sin( ) cos ( )cos( )

cos ( ) cos( ) sin ( )sin( )

0

L l L l

L l L l

c



综合以上几式，并考虑到

1 1 1

1 1 1 1

( )sin

( )cos

0

L l

L l

h

可解得

2 2 211 1 2 2 1 2 2 1 1 2 1 2 2 1

2 22 2 2 2 1 2 2 2 2 1 2 2 2 2 1 2 1 2 2

2 2 1 2 1 2 1 1

( 2 cos )

( cos ) sin 2 sin

sin( ) ( ) sin

z z

z

m m L l m l m L m l

m l m L l m L l m L l

m gl m m gl

I I

I

2 222 2 2 2 2 1 2 2 1 2 2 2 2

22 1 2 1 2 2 2 1 2

( cos ) ( )

sin sin( )

z zm m l m L l m l

m L l m gl

I I



以上两式进一步写成

式中

211 11 1 12 2 122 2 112 1 2 1

222 21 1 22 2 211 2 2

m D D D D D

m D D D D

2 2 211 1 2 2 1 2 2 1 1 2 1 2 22 cosz zD m L l m l m L m l I I

212 2 2 2 2 1 2 2coszD m l m L l I

122 2 1 2 2sinD m L l

112 2 1 2 22 sinD m L l

1 2 2 1 2 1 2 1 1sin( ) ( ) sinD m gl m m gl 2

21 2 2 2 2 1 2 2coszD m l m L l I2

22 2 2 2zD m l I

211 2 1 2 2sinD m L l

2 2 2 1 2sin( )D m gl


4.3 拉格朗日方程法拉格朗日方程是基于能量项对系统变量及时间的微分而建立的。对于简单系统拉格朗日方程法相较于牛顿—欧拉方程法更显复杂，然而随着系统复杂程度的增加，拉格朗日方程法建立系统运动微分方程变得相对简单。

系统拉格朗日方程为

ii i

d L LQ

dt q q

1,2,...i n

式中 n

iq ——广义坐标

iq ——广义速度

iQ

LkE pE—— 拉格朗日函数为系统的动能和位能之差 k pL E E

—— 系统的广义坐标数

—— 作用在第 i个广义坐标上的广义力或广义力矩


4.3.2 速度分析

机器人系统一般具有结构复杂的连杆机构，在利用拉格朗日方程法建立其动力学方程之前，需要求得机器人机构各连杆上某一点的位移、速度、加速度等物理量，进而求得机构的动能和位能，从而导出机器人机构的速度、动能、位能和动力学方程的一般表达式。

O

io

1io

Pr

piL

X

Y

Z

ix

iy

iz

图 4.4机器人机构

引入用 4×4 变换矩阵表示第个构件的刚体坐标系相对于基坐标系的齐次线性变换矩阵。如图 4.4 所示，若坐标系中任意一点 P 的位置矢量为，在基坐标系中，同一点 P 的位置矢量为，则有

iT i

iL i i i io x y z

OXYZ

r OXYZ

p

ip T r


4.3.2 速度分析 OXYZ点 P 在坐标系下的速度为

( )i i

d d

dt dt p

v T r T r

iT

iT iT

齐次线性变换矩阵的元素是可导的，用表示的元素对时间一次求导建立起来的速度矩阵

1

ii i

i jj j

dq

dt q

T TT

1

( )i

ii j

j j

d dq

dt dt q

Tpv T r r

2

2

1

2

1 1 1

( )i

ii

jj j

i i ii i

j k jj k jj k j

d d d

dt dtdt

dq

dt q

q q qq q q

p va T r

Tr

T Tr r

速度

加速度

速度的平方2

1 1

1 1

T

Ti i

Ti ij k

j kj k

Ti i

Ti ij k

j k j k

Trace

Trace q qq q

Trace q qq q

v v v

v v

T Tr r

T Tr r

n

式中： Trace 为矩阵的迹的运算符号，对于阶方阵来说，矩阵的迹就是其主对角线上的各元素之和。


4.3.3 动能

iL dm设构件上任意一点 P 的质量为，则该点的动能为

2

1 1

1

2

1

2

ki

Ti i

Ti ij k

j k j k

dE dm

Trace dm q qq q

v

T Tr r

iL构件的动能等于构件上的所有点的动能积分，即

iL

1 1

1 1

1 1

1

2

1

2

1

2

i i

i

Ti i

Ti iki ki j k

j k j kL L

Ti i

Ti ij k

j k j kL

Ti i

i ii j k

j k j k

E dE Trace dm q qq q

Trace dm q qq q

Trace q qq q

T Tr r

T Tr r

T TH

iH式中的积分称为连杆的伪惯量矩阵，记为

i

xxi xyi xzi xi

yxi yyi yzi yiTi

zxi zyi zzi ziL

xi yi zi i

I I I s

I I I sdm

I I I s

s s s m

H r r

其中 2

1

i

xxi

L

I r dm 22

i

yyi

L

I r dm 23

i

zzi

L

I r dm

1 2

i

xyi yxi

L

I I r r dm 1 3

i

xzi zxi

L

I I r r dm

2 3

i

yzi zyi

L

I I r r dm

1

i

x i

L

s r dm 2

i

y i

L

s r dm 3

i

z i

L

s r dm


4.3.3 动能

xxI yyI zzI xI yI zI 相对于平面的转动惯量、、与相对于轴的转动惯量、、之间存在一定的变换关系

2 2 2 2 2 2 21 2 3 1 3 1 2

1 1 1 1( ) ( ) ( ) ( )

2 2 2 2i i i i

xxi x y z

L L L L

I r dm r r dm r r dm r r dm I I I

2 2 2 2 2 2 22 2 3 1 3 1 2

1 1 1 1( ) ( ) ( ) ( )

2 2 2 2i i i i

yyi x y z

L L L L


2 2 2 2 2 2 23 2 3 1 3 1 2

1 1 1 1( ) ( ) 1 ( ) ( )

2 2 2 2i i i i

zzi x y z

L L L L


根据以上三式可以由相对于轴的转动惯量求得相对于平面的转动惯量


4.3.3 动能

整个机构所有构件的动能为

1 1 1 1

1 1 1

1

2

1

2

Tn n i i

i ik ki i j k

i i j k j k

Tn i ii i

i j ki j k j k

E E Trace q qq q

Trace q qq q

T TH

T TH

此外，在机器人机构系统中还存在驱动各构件运动的驱动和传动元件，如驱动电机或液压马达的转子，减速器的齿轮等，驱动电机和传动装置的的动能可以根据其广义速度写出

21

2k ai a i iE q I

a iI i

iq

式中：是第个驱动电机转子或传动装置在广义坐标上的等效转动惯量，若是移动运动副，则为等效质量。为广义坐标。机构所有驱动电机及传动装置的总动能为

2

1 1

1

2

n n

k a k ai a i ii i

E E q

I

机构总的动能为

1 1 1

2

1

1

2

1

2

kt k k a

Tn i ii i

i j ki j k j k

n

a i ii

E E E

Trace q qq q

q

T TH

I


4.3.4 位能

iL dm若构件上任意一点 P 的质量为，则该点的位能为

T Tpi idE dm dm g p g T r

式中 g为重力加速度矢量

1 2 3 0T

g g gg

iL构件位能为

i i i

T T Tp i pi i i i i i

L L L

E dE dm dm m g T r g T r g T r

im iL ir iL式中，为构件的质量，为构件质心在其刚体坐标系中的矢径，系统总的位能为

1 1

n nT

P p i i i ii i

E E m

g T r


4.3.5 动力学方程拉格朗日函数

1 1 1

2

1 1

1

2

1

2

kt p

Tn i ii i

i j ki j k j k

n nT

a i i i i ii i

L E E

Trace q qq q

q m

T TH

I g T r

pq式中， n 为机构的构件数。求拉格朗日函数 L 对的偏导数得

1 1

1 1

1

2

1

2

Tn ii i

i ki kp p k

Tn ii i

i j a p pi j j p

LTrace q

q q q

Trace q qq q

T TH

T TH I

1,2,3......p n iHT

i iH H式中：，考虑到为对称矩阵，即，所以有

TT T

i i i ii i

p k p k

TTi ii

k p

Ti i

ik p

Trace Traceq q q q

Traceq q

Traceq q

T T T TH H

T TH

T TH

所以

1 1

Tn ii i

i k a p pi kp k p

LTrace q q

q q q

T T

H I

iT ip i / 0i pq T

坐标转换矩阵仅与前个广义坐标有关，所以当时，所以有

1

Tn ii i

i k a p pi p kp k p

LTrace q q

q q q

T T

H I


4.3.5 动力学方程

1

1

2

1 1

2

Tn ii i

i k a p pi p kp k p

Tn ii i

i k a p pi p k k p

Tn i ii i

i j ki p j k j k p

Ti i

ij p k

d L dTrace q q

dt q dt q q

Trace q qq q

Trace q qq q q

Traceq q q

T TH I

T TH I

T TH

T TH

1 1

n i i

j ki p j k

q q

于是

Lpq再求拉格朗日函数对的偏导数得

2

1 1

2

1 1

1

2

1

2

Tn i ii i

i j ki p j kp j p k

Tn i ii i

i j ki p j k k p j

nT i

i ii p p

LTrace q q

q q q q

Trace q qq q q

mq

T TH

T TH

Tg r

上式进一步写成2

1 1

Tn i ii i

i j ki p j kp j p k

nT i

i ii p p

LTrace q q

q q q q

mq

T TH

Tg r


4.3.5 动力学方程

1

2

1 1

Tn ii i

i k a p pi p ki i k p

Tn i ii i

i j ki p j k j k p

nT i

i ii p p

d L LTrace q q

dt q q q q

Trace q qq q q

mq

T TH I

T TH

Tg r

所以

p i i j j

m

iQ

交换式中的下标，将换成，换成，换成，设作用在各广义坐标上的广义力为，则得

1

2

1 1

Tjnj j

i j k ai ij i k k i

Tj jnj j

j k mj i k m k m i

njT

j ij i i

Trace q qq q

Trace q qq q q

mq

T TQ H I

T TH

Tg r

, , ,1 1 1

n n n

i i j k ai i i j k k m ij j k

Q D q q D q q D

I

上式进一步写成

式中

,max( , )

Tnp p

i j pp i j j i

D Traceq q

T TH

2

, ,max( , , )

Tnp p

i j k pp i j k j k i

D Traceq q q

T T

H

npT

i p pp i i

D mq

T

g r


4.3.6 应用拉格朗日方程法建立机器人机构动力学方程

1m

XY

O

1

2

1l

2lA

B

图 4.5 平面两自由度机器人机构

例 4.2 如图 4.5 所示的平面两自由度机器人机构系统，集中质量、分别位于连杆 1 和连杆 2 的末端 A 、B 处，连杆长度分别为、，推导该机构动力学方程。

1m 2m

1l 2l

该机构由两个连杆机构组成，系统的动能为两连杆动能之和，即

1 2k k kE E E

连杆 1 的动能为2 2

1 1 1 1

1

2kE m l


4.3.6 应用拉格朗日方程法建立机器人机构动力学方程为了计算连杆 2 的动能，需要首先计算B 点的运动速度，为此列出 B 点的位置方程为

2 1 1 2 1 2

2 1 1 2 1 2

sin sin( )

cos cos( )

x l l

y l l

上式求导得 B 点速度

2 1 1 1 2 1 2 1 2

2 1 1 1 2 1 2 1 2

cos ( )cos( )

sin ( )sin( )

x l l

y l l

B 点速度的平方为

2 2 2 2 2 22 1 1 2 1 2 1 2

21 2 1 1 2 1 1 2 1 1 2

2 2 2 2 2 21 1 2 1 2 1 2 1 2 1 1 2 2

( 2 )

2 ( ) cos cos( ) sin sin( )

( 2 ) 2 ( )cos

v l l

l l

l l l l

连杆 2 的动能为

2 22 2 1 1

2 2 22 2 1 2 1 2

22 1 2 1 1 2 2

1

21

( 2 )2

( )cos

kE m l

m l

m l l

系统的总动能为

1 2

2 21 2 1 1

2 2 22 2 1 2 1 2

22 1 2 1 1 2 2

1( )

2

1( 2 )

2

( )cos

k k kE E E

m m l

m l

m l l


4.3.6 应用拉格朗日方程法建立机器人机构动力学方程连杆 1 所位能为

1 1 1 1cospE m gl

连杆 2 所位能为

2 2 1 1 2 2 1 2cos cos( )pE m gl m gl

系统的总位能为 1 2

1 2 1 1 2 2 1 2( ) cos cos( )

p p pE E E

m m gl m gl

拉格朗日函数

2 2 2 2 21 2 1 1 2 2 1 2 1 2

22 1 2 1 1 2 2 1 2 1 1

2 2 1 2

1 1( ) ( 2 )

2 2

( )cos ( ) cos

cos( )

k pL E E

m m l m l

m l l m m gl

m gl

1对于第一个广义坐标

2 21 2 1 1 2 2 1 2 2 1 2 1 2 2

1

( ) ( ) (2 )cosL

m m l m l m l l

2 21 2 1 1 2 2 1 2

1

22 1 2 1 2 2 2 1 2 1 2 2 2

2 21 2 1 2 2 2 1 2 2 1

22 2 2 1 2 2 2

22 1 2 2 1 2 2 1 2 2 2

( ) ( )

(2 )cos (2 )sin

( ) 2 cos

cos

2 sin sin

d Lm m l m l

dt

m l l m l l

m m l m l m l l

m l m l l

m l l m l l

1 2 1 1 2 2 1 21

( ) sin sin( )L

m m gl m gl


4.3.6 应用拉格朗日方程法建立机器人机构动力学方程 2对于第二个广义坐标

22 2 1 2 2 1 2 1 2

2

( ) cosL

m l m l l

22 2 1 2 2 1 2 2 1 2 1 2 1 2

2

( ) cos sind L

m l m l l m l ldt

22 1 2 1 1 2 2 2 2 1 2

2

( )sin sin( )L

m l l m gl

相对于第一个广义坐标的动力学方程为 2 2 2

1 1 2 1 2 2 2 1 2 2 1 2 2 2 1 2 2 2 2 1 2 2 1 2

22 1 2 2 2 1 2 1 1 2 2 1 2

( ) 2 cos cos 2 sin

sin ( ) sin sin( )

Q m m l m l m l l m l m l l m l l

m l l m m gl m gl

相对于第二个广义坐标的动力学方程为 2 2

2 2 2 1 2 2 1 2 2 1 2 1 2 1 2 2 1 2 1 1 2 2 2 2 1 2

2 2 22 2 2 1 2 2 1 2 2 2 2 1 2 1 2 2 1 2 1 2 2 2 2 1 2

( ) cos sin ( )sin sin( )

( cos ) sin sin sin( )

Q m l m l l m l l m l l m gl

m l m l l m l m l l m l l m gl


4.3.6 应用拉格朗日方程法建立机器人机构动力学方程以上两式写成矩阵形式为

21 11 12 122 1112 1211 1 1 2

22 21 22 211 22 2 2 1

0

0 0 0

Q D D D DD D

Q D D D D

式中

2 211 1 2 1 2 2 2 1 2 2( ) 2 cosD m m l m l m l l

212 2 2 2 1 2 2cosD m l m l l

221 2 2 2 1 2 2cosD m l m l l

222 2 2D m l

122 2 1 2 2sinD m l l

211 2 1 2 2sinD m l l

112 2 1 2 2sinD m l l

121 2 1 2 2sinD m l l

1 1 2 1 1 2 2 1 2( ) sin sin( )D m m gl m gl

2 2 2 1 2sin( )D m gl


4.4 凯恩方程法凯恩（ Kane ）方程是构建机器人机构动力学模型的

另一种常用方法，既适合于完整系统，也适合于非完整系统。凯恩（ Kane ）方程以广义速率代替广义坐标作为系统的独立变量，应用达朗贝尔原理及虚位移原理建立系统的动力学方程。

牛顿—欧拉方程、拉格朗日方程和凯恩方程在本质上是等效的，但对于机器人机构这样多自由度的复杂系统，凯恩方程可以有效减少计算步骤，提高计算效率。


4.4.1 广义速率、偏速度和偏角速度 iq

iq

1 2[ ... ]Tnu u uu

对于具有 n 个自由度的质点系，可以通过 n 个广义坐标（ i=1 ， 2 ，…，n ）来描述其相对于基础坐标系 oxyz 的运动， n 个广义坐标对应的广义速度（ i=1, 2, …, n ）。引入 n 个变量，使得

1

n

r r i i ri

u q

r i r其中、为 t 的函数

（ r=1 ， 2 ，…， n ）

1 2( , ,..., , )r i r i nq q q t

1 2( , ,..., , )r r nq q q t

于是

1

n

i i r r ir

q u b

1 2, , ..., nu u u

iq

iu

iq

定义为系统相对于基础坐标系的广义速率。

由于（ i=1 ， 2 ，…， n ）是独立的，所以广义速率（ i=1 ， 2 ，…， n ）也是独立的，构成了质点系的 n 个独立完整变量，质点系中任一点 P 相对于基础坐标系的速度都可以用这 n 个广义速率唯一地表示。设系统中任一质点的径矢为 r ， r为广义坐标及 t 的函数

1 2( , , ... , , )nr q q q tr

该点的速度

1

n

ii i

qq t

r rv r


4.4.1 广义速率、偏速度和偏角速度

1 1

1 1 1

n n

i r r ii ri

n n n

i r r ir i ii i

u bq t

u bq q t

r rv

r r r

1

n

r r tr

u

v v v

1

1

n

r i ri i

n

t ii i

q

bq t

rv

r rv

1 2( , ,..., , )r r nv v q q q tru

iq

0tv

定义为基础坐标系下相对于广义速率的第 r 偏速度。它是广义坐标和时间 t 的矢量函数。对于定常系统，，偏速度只是广义坐标的矢量函数。

将代入上式有iq

即

式中

ω 若系统中任一刚体的角速度为，在刚体坐标系中可以表示为

1e 2e 3e

1 2 3 式中：、、刚体坐标系下各坐标轴上的单位矢量；、、各坐标轴上的分量，有

1 1 2 2 3 3 ω e e e

32 13 1 1 2 2 3

dd d

dt dt dt

ee eω e e e e e e


4.4.1 广义速率、偏速度和偏角速度 1 2( , ,..., , )j j nq q q te e iq式中：为以及

t 的函数，有

1

nj j j

ii i

dq

dt q t

e e e

j=1, 2, 3

所以

32 13 1 1 2 2 3

1

32 13 1 1 2 2 3

n

ii i i i

i i i

qq q q

q q q

ee eω e e e e e e

ee ee e e e e e

上式可以写成

1

n

r r tr

u

ω

式中

32 13 1 1 2 2 3

1

n

r i ri i i iq q q

ee e

e e e e e e

32 13 1 1 2 2 3

1

32 13 1 1 2 2 3

n

ti i i iq q q

t t t

ee ee e e e e e

ee ee e e e e e

1 2( , ,..., , )r r nq q q t ru

iq

0t

定义为基础坐标系下相对于广义速率的第 r 偏角速度。一般来说，偏角速度是广义坐标和时间 t的矢量函数。对于定常系统，，偏角速度只是广义坐标的矢量函数。


4.4.2 凯恩方程

1P

2P3P

C

1F

3F

1r

2r

3r S

图 4.6 质点系 S

iP im

ir iF

如图 4.6 所示，设质点系 S 有 n 个自由度，为上的任意一质点，其质量为，相对于基础坐标系的矢径为，作用力为。根据达朗倍尔原理和虚位移原理可以得到质点系 S 的动力学普遍方程为

1

( ) 0n

i i i ii

m

F r r

ruiP

若系统相对于基础坐标系的广义速率为（ r=1, 2,…, n ），系统中点的速度，可写成广义速率的线性组合的形式，即

1

ni

i ri r tir

du

dt

rv v v

1 1

n n

i ri r ti ri r tir r

d u dt dt d dt

r v v v v

于是


4.4.2 凯恩方程上式中，得的变分r rd u dt ir

1

n

i ri rr

r v

1 1

( ) 0n n

i i i ri ri r

m

F r v

1 1 1

0n n n

i ri i i ri rr i i

m

F v r v

由此可得

即

ru系统对应于的广义主动力定义为

* *

1 1

n n

r i i ri i rii i

m

F r v F v

1

n

r i rii

F F v

ru系统对应于的广义惯性力定义为

*i i imF r iP式中为质点的惯性力，有

*

1

( ) 0n

r r rr

F F

r由于变分为相互独立的，所以

* 0r r F F r =1， 2 …，， n

上式即为质点系的凯恩动力学方程。凯恩方程表明系统的广义主动力与广义惯性力之和应等于零。


4.4.3 广义主动力

ir iF

在刚体上任取一点，不失一般性，取刚体的质心到各质点 C 的径矢为，力系向点 C 简化，得主矢 R和主矩 L

1

n

ii

R F1

n

i ii

L r F

ri r r i v v ω r

rv rω式中为刚体质心 C 点处的第 r 个偏速度，为刚体的第 r 个偏角速度。于是刚体上的所有作用力所对应的广义主动力为

1 1

( )n n

r i ri i r r ii i

F F v F v ω r

r =1， 2 …，， n

上式简化得

1 1

1 1

( )

( )

n n

r i r i r ii i

n n

i r i i ri i

r r

F F v F ω r

F v r F ω

R v L ω

引入广义主动力可以消除理想约束的约束反力。运动副中的约束反力在不计及接触处的摩擦力的情况下，即是理想约束。这种约束的约束反力，总是成对出现的，约束反力的合效应为零，所以理想约束力所对应的广义主动力为零。在计算广义主动力时，可将理想约束力排除，这也是引进广义主动力的概念的主要优点。

由于


4.4.4 广义惯性力

ω ε ωim

ir

crv

rω

设刚体的质心为，绕质心转动的角速度矢量为，角加速度矢量为，刚体上任意质点质量为（ i=1 ， 2 ，…， m ），各质点的径矢为（ i=1 ， 2 ，…， m ），质心的加速度为，为刚体质心 C 点处的第 r个偏速度，为刚体的第 r 个偏角速度。刚体上任意质点的偏速度为

ri r r i v v ω r

于是广义惯性力为 *

1 1

1 1

1 1

* *

( )

( )

( )

n n

r i i ri i i r r ii i

n n

i i r i i r ii i

n n

i i r i i i ri i

r r

m m

m m

m m

F rv r v ω r

rv r ω r

rv r r ω

R v Lω

*R *L式中、为各质点的惯性力向 C 点简化，得惯性力系的主矢和主矩

*

1

n

i ii

m

R r *

1

( )n

i i ii

m

L r r

1

n

ii

m m

1

0n

i ii

m

r考虑到、，且

( )i i i r c ε r ω ω r i=1， 2 …，， m

* mR c

*

1 1

1

( ) ( )

[ ( )]

n n

i i i i ii i

n

i i ii

C C

m m

m

L r c r ε r

r ω ω r

I ε ω× I ω

所以有

CI式中为刚体坐标系下的惯量张量


4.4.5 应用凯恩方法建立机器人机构的动力学方程

2i

2k

2j1i 1k

1j

1q

2q

2m

1m

1R

2R

l

1C2C

1L

2L

图 4.7 2R两自由度机械手

应用凯恩方法建立动力学方程的过程可分为运动分析、求广义惯性力、求广义主动力及建立动力学方程的四大步骤。下面以图 4.7 所示的 2R 两自由度机械手为例说明具体的分析过程。

1L 2L

1C 2C 1m

2m 1R 2R 1q 2q

1L

1i 1j 1k 2L

2i 2j 2k

例 4.4 如图 4.7 所示，该机构由两个构件和组成，其质心分别为和，，集中质量为和，、为两个旋转关节，、为广义坐标，固定在构件上的刚体坐标系用单位矢量、、表示，固定在构件上的刚体坐标系用单位矢量、、表示，则两个坐标系之间的变换关系为

2 2

2 2 2 1 1 1

2 2

cos 0 sin

[ ] [ ] 0 1 0

sin 0 cos

q q

i j k i j k

q q



r ru q

(1) 选择广义速率，广义速率的选取原则是使方程能够得到简化，一般在机器人动力学中往往把构件间的相对速度作为广义速率，即，这样得到的动力学方程的形式较简单，所以本例中取

1 1u q 2 2u q

1L(2) 运动分析，构件的角速度为1 1 1 1 1q i u i

角加速度为1 1 1 1u i

1L 1u

11 2u

12

构件对第一个广义速率的偏角速度为、对第二个广义速率的偏角速度为，有

11 1i 12 0

2L构件的角速度为2 1 1 2 1

1 2 2 2 2 1 2 2cos sin

q i q j

u i q u j u k q

角加速度为2 2 1 2 1 2 2 2

2 2 1 2 1 2 2 2

( cos sin )

( sin cos )

u q u u q i

u j u q u u q k

2L 1u

21 2u

22

构件对第一个广义速率的偏角速度为、对第二个广义速率的偏角速度为，有

21 2 2 2 2cos sini q k q 22 2j

1L构件质心速度为1 0v

所以偏速度为

11 0v 12 0v



2L构件质心速度为

所以偏速度为

2 1 1 1 1v j l u k l

21 1v k l 22 0v

(3) 求广义惯性力

1L构件的惯性力主矢为

*1 1 1 0m a R

惯性张量为 1

1 1

1

0 0

0 0

0 0

x

C y

z

I

I

I

I

惯性力主矩为 *1 1 1 1 1 1

1 1 1 1 1

1 1

1 1

1 1 1

0 0 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0

C C

x x

y y

z z

x

I u u I u

I I

I I

I u i

L I ε ω × I ω

1u对第一个广义速率的广义惯性力为* * *

11 1 11 1 11 1 1 1 1 1 1x xI u i i I u F R v Lω

* * *12 1 12 1 12 0 F R v Lω

2u对第一个广义速率的广义惯性力为

2L构件的惯性力主矢为 * 22 2 2 2 1 1 1 1( )m a m u k l u j l R



惯性张量为 2

1 2

2

0 0

0 0

0 0

x

C y

z

I

I

I

I



惯性力主矩为 *2 2 2 2 2 2

2 1 2 1 2 2

2 2

2 1 2 1 2 2

1 2 2 1 2

2 2 2

1 2 2 1 2

2 1 2 2 2

0 0 cos sin

0 0

0 0 sin cos

cos 0 0 cos

0 0

sin 0 0 sin

[ cos (

C C

x

y

z

x

y

z

x x y z

I u q u u q

I u

I u q u u q

u q I u q

u I u

u q I u q

I u q I I I

L I ε ω × I ω

2 1 2 2 2

22 2 2 2 1 2 2 2

2 2 2 1 2 2 2 1 2 2

) sin ]

[ ( ) sin cos ]

[( ) cos sin ]

y x z

x y z z

u u q i

I u I I u q q j

I I I u u q I u q k

* * *21 2 21 2 21

2 2 22 2 2 2 2 1

2 2 1 2 2

( cos sin )

( ) sin 2

x z

x z

I q I q m l u

I I u u q

F R v L ω

* * *22 2 22 2 22

22 2 2 2 1 2 2( ) sin cosy x zI u I I u q q

F R v L ω

所以广义惯性力为* * *

1 11 21

2 2 21 2 2 2 2 2 1

2 2 1 2 2

( cos sin )

( ) sin 2

x x z

x z

I I q I q m l u

I I u u q

F F F

* * *2 12 22

22 2 2 2 1 2 2( ) sin cosy x zI u I I u q q

F F F


4.4.5 应用凯恩方法建立机器人机构的动力学方程 1L 2L

(4) 求广义主动力构件和构件的重力分别为

1 1 1G m gi 1 2 1G m gi

1R 2R

1 11 1M m i 2 21 1M m j 1L

若作用在关节和关节上的驱动力矩分别为和，则得构件主矢和主矩分别为

1 1 1m giR 1 11 1m iL

1L 2u构件对第二个广义速率的广义主动力为

1u构件对第一个广义速率的广义主动力为

1L 构件对第二个广义速率的广义主动力为

2L 2u

2L构件对第一个广义速率的广义主动力为

1u

11 1 11 1 11 11 1 1 11m i i m F R v L ω

12 1 12 1 12 0 F R v L ω

构件主矢和主矩分别为 2L

2 2 1m giR 2 21 2m jL

21 2 21 2 21

2 1 1 21 2 2 2 2 2( cos sin )

0

m gi k l m j i q k q

F R v L ω

22 2 22 2 22 21 1 2 21(m j j m F R v L ω

所以广义主动力为1 11 21 11m F F F

2 12 22 21m F F F


4.4.5 应用凯恩方法建立机器人机构的动力学方程 (5) 凯恩方程

1u对第一个广义速率有，所以*1 1 0 F F

221 2 2 2 2 1 2 2( ) sin cos 0y x zm I u I I u q q

2 2 211 1 2 2 2 2 2 1 2 2 1 2 2( cos sin ) ( ) sin 2 0x x z x zm I I q I q m l u I I u u q

2u对第二个广义速率有，所以*2 2 0 F F

最后得到的动力学方程为

1 1

2 22 2 2

11 1 2 2 2 2 2 1 2 2 1 2 22

21 2 2 2 2 1 2 2

( cos sin ) ( ) sin 2 0

( ) sin cos 0x x z x z

y x z

q u

q u

m I I q I q m l u I I u u q

m I u I I u q q

第 4 章 机器人动力学

Documents

第 4 章机器人动力学