第 4 章 机器人动力学

42
第 4 第 第第第第第第 第第第第第第第第第第 第第第 第第第第第第第第第第第第第第第第第 第第第第第第第第第第第第第 第第第 第第第 :,、、 第第第 第第第 ` 第第第第第第第第第第 第第第第第第第 第第第第第第第第第第第第第第第第第 、, 4.1 第第 第第第第第第第第第第 第第第第第 第第第第第第第第第第第第 第第第第第第第第第第第 第第第第第第 :、一 第第第第 第第第第第 第第第第第 第第第第第第第第第第 :、、 第第第第 第第第第第第第 第第第第第第第 第第第第第 第第第第第第第 第第第第第第 第第第第第第第 :、、、、、、 第第 第第第第第 第第第第第 -、

Upload: kaemon

Post on 04-Jan-2016

201 views

Category:

Documents


2 download

DESCRIPTION

4.1 引言. 第 4 章 机器人动力学. 机器人动力学研究内容 正问题:已知作用在机器人机构上的力和力矩,求机器人机构各关节的位移、速度、加速度 反问题:已知机 ` 器人机构各关节的位移、速度和加速度,求作用在各关节上的驱动力或驱动力矩. 机器人动力学研究方法 动力学模型:根据机构的结构和运动学、动力学关系建立起来的一组动力学方程 数学工具:矢量方法、张量方法、旋量方法及矩阵方法等 力学原理:能量守恒定理、达朗贝尔原理、虚功原理、拉格朗日方程、动量矩定理、哈密尔顿原理、牛顿-欧拉方程、凯恩方程等. 4.2 牛顿 — 欧拉方程法. 第 4 章 机器人动力学. - PowerPoint PPT Presentation

TRANSCRIPT

Page 1: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

4.1 引言

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

Page 2: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

4.2 牛顿—欧拉方程法

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

Page 3: 第 4 章 机器人动力学

第 4 章 机器人动力学

如图 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

式中

Page 4: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

Page 5: 第 4 章 机器人动力学

第 4 章 机器人动力学

如图 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

ω

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

ε

Page 6: 第 4 章 机器人动力学

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

第 4 章 机器人动力学

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 ω

Page 7: 第 4 章 机器人动力学

第 4 章 机器人动力学

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 个构件 上的外力矩化简到质心 处的合力矩,即外力的主矩

Page 8: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

式中

Page 9: 第 4 章 机器人动力学

第 4 章 机器人动力学

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 所示的平面两自由度机器人机构,连杆 质心为 ,质量为 ,驱动力矩为 ,角速度为 , 角加速度为 。连杆 质心为 ,质量为 ,驱动力矩为 角速度为 , 角加速度为 。

Page 10: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

Page 11: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

所以

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

Page 12: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

综合以上几式,并考虑到

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

Page 13: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

以上两式进一步写成

式中

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

Page 14: 第 4 章 机器人动力学

第 4 章 机器人动力学

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个广义坐标上的广义力或广义力矩

Page 15: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 16: 第 4 章 机器人动力学

第 4 章 机器人动力学

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 为矩阵的迹的运算符号,对于阶方阵来说,矩阵的迹就是其主对角线上的各元素之和。

Page 17: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 18: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

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

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

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

Page 19: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 20: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 21: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 22: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 23: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 24: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 25: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 26: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 27: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 28: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 29: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

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

Page 30: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 31: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 32: 第 4 章 机器人动力学

第 4 章 机器人动力学

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的矢量函数。对于定常系统, ,偏角速度只是广义坐标的矢量函数。

Page 33: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

于是

Page 34: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

Page 35: 第 4 章 机器人动力学

第 4 章 机器人动力学

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 ω

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

由于

Page 36: 第 4 章 机器人动力学

第 4 章 机器人动力学

4.4.4 广义惯性力

ω ε ωim

ir

crv

设刚体的质心为,绕质心转动的角速度矢量为 ,角加速度矢量为 ,刚体上任意质点质量为 ( 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式中 为刚体坐标系下的惯量张量

Page 37: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 38: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

Page 39: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

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

Page 40: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

惯性张量为 2

1 2

2

0 0

0 0

0 0

x

C y

z

I

I

I

I

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

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

惯性力主矩为 *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

Page 41: 第 4 章 机器人动力学

第 4 章 机器人动力学

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

Page 42: 第 4 章 机器人动力学

第 4 章 机器人动力学

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