第六章 机器人动力学

第六章机器人操作臂动力学

动力学研究的是物体的运动和受力之间的关系。操作臂动力学有两个问题需要解决。

①动力学正问题：根据关节运动力矩或力，计算操作臂的运动（关节位移，速

度和加速度）

②动力学逆问题：已知轨迹运动对应的关节位移，速度和加速度，求出所需要

的关节力矩或力。

机器人操作臂是个复杂的动力学系统，由多个连杆和多个关节组成，具有多个输入和多个输出，存在着错综复杂的耦合关系和严重的非线性。因此，对于机器人动力学的研究，引起了十分广泛的重视。所采用的方法很多，①有拉格朗日方法，②牛顿-欧拉方法，③高斯法，④凯恩方法，⑤旋量对偶数方法等等。在此重点介绍牛顿-欧拉方法，它是基于运动坐标和达朗贝尔原理来建立相应的运动方程。

研究机器人动力学的目的是多方面的，动力学正问题与操作臂仿真有关，逆问题是为实时控制的需要，利用动力学模型，实现最优控制，以期达到良好的动态性能和最优指标。

机器人动力学模型主要用于机器人的设计和离线编程。在设计中需根据连杆质量，运动学和动力学参数，传动机构特征和负载大小进行动态仿真，从而决定机器人的结构参数和传动方案，验算设计方案的合理性和可行性，以及结构优化程度。在离线编程时，为了估计机器人高速运动引起的动载荷和路径偏差，要进行路径控制仿真和动态模型的仿真。这些都必须以机器人动态模型为基础。

为了建立机器人动力学方程，在此首先讨论机器人运动的瞬时状态，对其进行速度分析和加速度分析，研究连杆的静力平衡，然后利用朗贝尔原理，将静力学平衡条件用于动力学。

§6-1连杆的速度和加速度

点的速度表示一般要涉及到两个坐标系：

要指明速度是相对于哪个坐标系的运动所造成的。

① 要指明在哪个坐标系中描述这一速度。

连杆I 相对于参考系{o 的速度用w i 和v i 表示; w i 是连杆坐标系{i}的角速度矢量，v i 是{i}的原点线速度矢量。如果把两个向量在{i}中描述，即为

i

w i 和i

v i

。

为了描述刚体在不同坐标系中的运动，设有两坐标系：参考系{A}和运动坐

标系{B}.{B}相对于{A}的位置矢量为0B A P ,旋转矩阵为R A

B 。

任一点P 在两坐标系中的描述P A 和P B 之间的关系为：

P A

=0B A P +R A

B P B

将上式两边对时间求导得：=P A 0

B A P +P R P R B A B B A B + P A

和P B 分别表示点P 的速度相对于{A}和{B}的运动造成的，记为V A 和P

B V ; 0B A

P 是{B}的原点相对{A}的运动速度，记为0B A V ； R A

B

表示旋转矩阵的R A B

导数。 一 旋转矩阵的导数R

利用导数和微分的定义，即

()()()()t t R t t R t t R t R t t ??=?-?+=→?→?lim lim 0

0 由选择变换的公式可知，()t t R ?+总是可以看成为()t R 在时间间隔t ?内绕某轴K 转动微分角度δθ而得到的，即

()()()t R k R t t R δθ,=?+

()()()()()()()t R k t R I k R t R t t K t R δθδθ,],[A =-=-?+=?

式中，I 是3*3的单位矩阵，()δθ,k A 称微分旋转算子。利用选择变换通式，并用微量δθ代替θ，由于1cos ,sin ==δθγθδθ可得：

()()???

?

??????---=-=A 000,,δθδθδθδθδθδθδθδθx y x z y z k k k k k k I k R k

把上式两端除以t ?，并取极限，定义为角速度算子矩阵：

()????

?

?????---=??????????---=000000x

y

x z

y z x y x z y

z w w w w w w k k k k k k w S θθθθθθ 相应的角度矢量W 为：

角速度算子矩阵()w S 和角速度矢量W 是角速度的两种描述，在任意矢径P 处引起的线速度p V 可表示为

()P W p w S V p ?=?= 有伤吗可得旋转矩阵的导数;

()()R W R w S t R

?== 二． 刚体的速度和加速度

根据公式 ()()()

R W S R R w S t R A B

B A A B =?= 则;()

P R W S V R V V B A

B B A P B A B B A P A ?++=0

将上式两端求导，得加速度P A V 和P

B V 之间的关系： ()()()()

P R W S W S P R W S V R W S V R V V B A B

B A B A B A B B A P B A B B A P B A B B A P A

++++= 20 上面两式分别表示质点P 在不同坐标系中运动速度和加速度的转换公式，根

据不同的情况，该公式还可简化：

()a {A}固定不动，刚体与{B}固接

const P B

= 0==P

B P B V V 则 ()

P R W S V V B A B B A B A P A

+=0

()()()

P R W S W S P R W S V V B A B

B A B A B A B B A B A P A

++= 0 ()b 若运动坐标系{B}相对于参考系{A}移动，即R A B 固定不变；

θ

θ

θθ I w w w k k k W z y x z y x =??????????=??????????=

P

B A B B A P A

P B

A B B A P A B A B A V R V V V R V V W W +=+===000

()c 若{B}相对于{A}纯滚动；

const P B A

=0 00

0==B A B A V V 则： ()

P R W S V R V B A B B A P B A B P A

+=

()()()()

P R W S W S P R W S V R W S V R V B A B

B A B A B A B B A P B A B B A P B A B P A

+++= 2 式中B A W 表示坐标系{B}相对{A}转动的角速度矢量，因为角速度矢量是自由矢量。若已知{C}相对{B}的转动角速度为C B W ，则{C}相对{A}的转动角速度和角加速度矢量为：

C

B

A B B A C B A B B A C A C

B A B B A

C A

W R W S W R W W W R W W )(++=+= 三．旋转关节的连杆运动的传递。

连杆运动通常是用连杆坐标系的原点和加速度，以及连杆坐标系的角速度和角加速度来表示的。i v 和i w 分别表示连杆坐标系{}i 相对参考系{}0的线速度和角速度，而i i v 和i i w 在坐标系{}i 中表示的。11++i i v 和11++i i w 表示在连杆坐标系{}1+i 中的线速度和角速度。

连杆i+1相对连杆i 转动的角速度是绕关节i+1运动引起的。

i i

V

i i w

1

1

++i i V 1

1

++i i w

??

??

?

?????=+++++11

11100i i i i i Z θθ 1+i θ 是关节角速度，1

1++i i Z 是{i+1}的Z 轴单位向量 因为：同一坐标系中的向量可相加，因此，连杆i+1的角速度等于连杆i 的角速

度加上连杆i+1的角速度加上连杆i+1绕关节i+1旋转的角速度，在[i]中的表示为：

1+i i

w =

i i

w

+1

1

+?

+i i i k θ

11++i i z

将此式两端左乘旋转矩阵R i i 1

+，则得到相对连杆本身坐标系{i+1}的表示：

11

++i i w

=i i

i i w R 1

++1

+?

i θ

11

++i i z

①

坐标系{i+1}原点的线速度等于坐标系{i}原点的线速度加上连杆i 转动速度而产生的分量。1+i i

p 表示{i+1}的原点在坐标系{i}中的位置矢量，是不变

的，因此：

1i

i i i i i v v w +=+×1i i p +

此式两端都左乘1

i i

R +；得到相对于{i+1}中的表示：

1

111()i i i i i i i i i i v R v w x p ++++=

+ ②

由①和②式可以得到从一连杆向下一连杆的角加速度和线加速度 传递公式：

11

11i i i i

i i

i i

i i w R w R w +?

?

+++=

+

×1

+?

i θ

1

11111

i i i i i i Z Z θ+++++++ 1

1i i v ++=1i i R +11()i i i i i i

i i i i i i v w p w w p ++??+?+????

四．移动关节的连杆运动传递

当关节i+1是移动关节时，连杆i+1相对坐标系{i+1}的Z 轴移动，没有转

动，R i

i 1

+是常数矩阵，相应的运动“传递”关系为：

1

11i i i i i

i w R w +++=

1

111111

()i i i i i i i i

i i i i i v R v w p d Z +++++++=

+?+ 1

1

1i i i i i i w R w

+++=

1

1

111[()]i i i i i i i i i i i i i i i i v R v

w p w w p +++++=+?+?? 11111111

i i i i i i i i Z w d Z d Z +++++++++?+ 五．质心的速度和加速度

i

i i i ci i i ci v v w p =+?

()i

i i i i i i ci i i ci i i ci v

v w p w w p =+?+?? 式中，质心坐标系的{i c

}与连接杆i 固接；坐标原点位于连杆i 的质心，

坐标方向与{i}同向。上式中不含关节运动，因此，不论关节i+1是转动还是移动都同样适用。

上面是计算操作臂各连接杆运动的传递的公式，利用这些公式既可依次从基座开始传递各连杆的

i

i w ，i i v ，i i w

和i i v 。作为递推的初始（基座）

0w w

==

， 0v v

==

，这样递推得到的连杆速度、角速度、加速度和角加速度都是相对连杆本身坐标系表示的。如果将这些速度和角速度相对于基坐标系{0}表示，则需左乘旋转矩阵0

i R ,即：

i i i i w R w =

i i i i v R v = (i=1,2,……，n)

六．雅克比矩阵的速度递推法

逐次运用上面所介绍的递推公式，即可求出末端连杆的角速度

n

n w 和线速

度n

n v ，根据上式，利用n R 左乘，则p 得到这些速度在基座标系中的表示=n

w 和

n v

，简写为n w ，n v 。构造末端连杆的笛卡尔广义速度：

n n n n v v x

w ??==????

它与关节速度q

之间的关系就是由雅克比组成的线性映射： ()n n x

J q q = 【例】 用递推法求出平面2R 机械手末端连杆的速度和角速度，雅克比。

{3}固接在手臂末端，既有30θ=，3

0θ=

各连杆变换为：

1111

01

cos sin 00sin cos 0000100

1T θθθ

θ-????-??=??????

沿Z 轴旋转1

θ

22122

12cos sin 0sin cos 000010000

1l T θθθ

θ-????-??=??????

沿Z 轴旋转2θ，1X 轴移动1

l

22310001000

0100

1l T ??????=??????

2X 轴移动2l

适用式：11

1111i i i i i

i i i w Rw Z θ++++++=+

1111()i i i i i i i i i i v R v w p ++++=

+?

依次算出各连接杆的速度和角速度。基座标系{0}固定又动

00w =

00v =

其余各连杆的速度和角速度为“

1

1100w θ??

??=??????

11000v ????=??????

2

21200w θθ????=??

??+??

121

12

2

2

22

21211000000

100l s l c s v s c l c θθθ????

????????????????=--?=

????????????????????????????

??

3

232w w =

21

21

3

31

2

1121

0000

1

0000

100

00l l s v l c θθθθ??

??????????????????=+?????????????????????+???????

?

??

2211

12

121212122220()0l s l s l c l l c l l θθθθθθ????????=++=??????+????????

??

12

3122

20()l s J q l c l l ??∴=?

?+??

为了求出相对于基座标系{0}表示的

3w 和03v ，计算旋转变换：

12

120012

312312

1200001c s R R R R s c -??

??==??????

033331200w R w θθ??

??==??

??+??

1112121200333311121212()()0l s l s v R v l c l c θθθθθθ??--+??==++??????

末端连杆的卡迪尔广义速度为：

311212212311212

212331

33323300000011x y z x y z v l s l s l s v l c l c l c v v v w w w w θθ??---??

???

?+????

?????

?????===???

??????????

??????????

????

?

?? 于是机械手的雅克比即为

11212212112122120

00()000011l s l s l s l c l c l c J q ---????+????=????????

?

?

可知与前面求得的结果是一样的。

§6-2 连杆静力学分析

在对操作壁进行静力学分析时，首先考虑其中的一个连杆i ，建立连杆i 的力和力矩平衡方程。

图为连杆i 和连杆i+1的受力情况。

i f 和i n 分别表示连杆i-1作用在连杆i 上的力和力矩。 i m g 为连杆i 的重量，作用在质心上，质心的位置矢量为ci i r 。

当连杆处于平衡状态时，其上所受的合力和力矩为零，因此得到力和力矩平衡方程式（在{i}中的表示）：

11111

11i i i i i i i i i i i i i i i i f R f n R n p f +++++++?=?=+??111100i i i i i i i i i i i i i i i i ci i f f m g n n p f r m g ++++?-+=?--?+?=? 在此，暂时忽略连杆本身的自重，从末端连杆逐次向基座（连杆0）反向递推各连杆所受的力和力矩：

1111i i i i i i i i

i i i i f f n n p f ++++?=?=+??

利用旋转变换矩阵R i i 1

+，将上式右端的力和力矩写成在自身坐标系中的表

示：

1111111i

i i i i i i i i i i

i i i i i f R f n R n p f +++++++?=?=+??

接下来的任务是计算每个关节的驱动力或力矩。其实各个连杆所承受的力向量和力矩向量中，某些分量由操作臂本身的连杆结构所平衡，一部分量则为各关节的驱动力或驱动力矩所平衡。

对于转动关节，关节驱动力矩平衡力矩的Z 分量：

i i T i i i z n ?=τ

对于移动关节，关节驱动力平衡力的Z 分量：

i i T i i i Z f ?=τ

类似于速度雅克比的递推算法，可以得到力雅克比的递推算法。下面举例说明：

[例]:平面2R 机械手，令f 3

是水平左作用在机械手的末端执行器（即坐标{3}的圆点上）的外力。

那么各关节的表达式反向递推如下：

111i

i i i i i f R f +++= 1111i i i i i i i i i i n R n p f ++++=+?

223

23310001000100x x y y f f f R f f f ??????

??????===??????

????????????

223

22233322220x y y f o n R n p f l x f o l f ??

??????=+?=??=?

??????????? 2

2221

121222

2

2200100x x y y x y c s f c f s f f R f s c f s f c f o

o

--????????????===+??????

???????????? 2

2221

1211122212

211222121220000000

10x y x y y x y y c s c f s f n R n p f s c l x s f c f l f l s f l c f l f ???

?--????????????=+?=+?+=???

?????????????++????????

11112212121220

000()1T

T

i i x

y x y y n z l s f l l c f

l s f l c f l f τ????

????=?==++??

??????++????

2222T

i i y

n z l f τ=?=

写成矩阵形成：

111222220x y f l s l l c f l ττ+??????=????????????11111111i

i i i i i i i i i i

i i i i i f R f n R n p f ++++++++?

=?=+?? 于是得到在{3}中表示的力雅克比为：

112223

2()0T l S l l c J q l +??

=??

??

将旋转矩阵0

3

T

R 右乘()3T J q ，得到在{0}表示的力雅克比：

()()11212112120

303212212T

T T

l s l s l c l c J

q J q R l s l c --+??==??-??

§6-3 牛顿—欧拉递推运动学方程

操作臂动力学的研究方法很多，现利用运动（速度和加速度）递推和力的递推来建立操作臂运动学方程，讨论运动学问题的求解方法。 一. 牛顿—欧拉方程

如果将操作臂的连杆堪称刚体，它的质心加速度c v ?

、总质量m 与产生这一加速度的作用力f 之间的关系满足牛顿第二运动定律：

c f mv ?

=

当刚体绕过质心的轴线旋转时，角速度ω、角加速度?

ω、惯性张量C

I ，与

作用力矩n 之间满足欧拉方程：

()

C

C n I I ωωω?

=+?

二． 惯性张量

令{C}是以刚体的质心C 为原点规定的一个坐标系，相对于该坐标系{C},惯

性张量C

I 定义为33?的对称矩阵：

xx xy xz c

xy

yy yz xz yz

zz I I I I I I I I I I ??

--??=--????--?

?

式中，对角线元素是刚体绕三坐标轴x,y,z 的质量惯性矩： ()22xx v

I y z dv

ρ=+??? ()2

2

yy v

I x z

dv

ρ=+???

()22zz v

I x y dv

ρ=+???

其余元素为惯性积：

xy v

I xy dv

ρ=???

yz v

I yz dv

ρ=???

zx v

I zx dv

ρ=???

ρ为密度；dv 是微分体元，其位置由矢量(),,T

c

p x y z =确定。

惯性张量C

I 表示刚体质量分布的特征，其值与选取的参考坐标有关，如果

所选的坐标系{c}的方位使各惯性积

xy

I ，

yz

I 和zx I 均为零，惯性张量变成对角型，

则此坐标的各轴称为惯性主轴，相应的质量惯性矩称为主惯性矩。

令坐标系{A}与质心坐标系{C}平行，根据平行轴定理，刚体相对两平行坐标系的惯性矩和惯性积存在以下关系：

A

C xy xy c c

I I mx y =+

()

22A

C zz zz C C I I m x y =++

式中c x ，c y ，c z 是质心，C 在{}

A 中的三个分量，其他惯性矩和惯性

积的转换公式类似。

三． 动力学逆问题的递推算法。

动力学逆问题是根据关节位移、速度和加速度θθθ???

?? ??

?

求所需的关节力

矩或力τ。整个算法由两部分组成：

① 首先向外递推计算各连杆的速度和加速度，由牛顿—欧拉公式算出各连杆的惯性力和力矩。

② 向内递推计算各连杆相互作用的力和力矩，以关节驱动力或力矩。 向外递推（i ：0—>n-1）

1111+++++?i i i i i i i

Z R θω (对于转动关节i+1) i i

i i R ω?+1 (对于移动关节i+1)

i i i i

R ω ?+1+

1111111+++++++?+??i i i i i i i i i i Z Z X R θθω （转动关节） i i

i i R ω

?+1 （移动关节）

=

++11

i i ω=++11i i ω

R i i

1+[]

)(11++++i i i i i i i i i i i i P X X P X ωωων （转动关节）

R i i

1

+[]

)(11++++i i

i i i i i i

i

i

i

i

P X X P X ωωων

+

111111112+++++++++?i i i i i i i i Z d Z d X ω （移动关节） )(11111111111111

++++++++++++++++=ci i i i i i ci i i i i i ci i X X X γωωγωνν 11111

+++++=ci i i ci i m f ν

(

)

1111

11111111

+++++++++++++=i i i ci i i i i i ci ci i I X I n ωωω

向内递推（i ：n —>1）

ci i

i i i i i i

f f R f +=+++111 1111111

++++++++++=i i i i i i ci i ci i ci i i i i i i i f R X P f X n n R n γ

i i

T

i i Z n ? （转动关节） i i

T i i Z f ? （移动关节）

[例] 平面2R 机械手，假定两个连杆的质量集中在连杆末端，分别为m 1和m 2。机械手的运动参数和动力学参数为：

1111X l P c =，2222

X l P c = 两连杆质心矢径

11

I c =0 22I c =0 相对质心的惯性张量为0

f 03= n 03= 末端执行器自由

00=ω 00=ω 基座固定

000

Y g V

= 考虑重力的作用

连杆间的旋转变换矩阵为：

??????????-=+++++100

0011

111

i i i i i i c s s c R ??

???

?????-=+++++1000011111

i i i i i i c s s c R

向外递推计算各连杆的速度和加速度

=++11

i i ν

=

i τ

??????????=?=11111100θθω Z ??????????=?=11111100θθω Z ??

???

?????=????????????????????-=0001000011111111

gc gs g c s s c υ

??????????+-=??????????????????????????????????????????+?????????????????????+??????????=00000000000011121

11111111111θθθθθυ l gc l gs l l gc gs c ??????????+-==011121

11111111θθν l gc l gs m m f c c

??

???

?????=00011

c n

对于连杆2：

??????????+=212200θθω ??

???

?????+=2122

00θθω

??????????+++-=???????

???+-???????

???-=0010

0002112211122

1122111211121112222

22

c l s l gc s l c l gs l gc l gs c s s c θθθθθθυ

(

)

(

)??????????+++-+????

??

?????

?+-+?????????

?

+=0000

2112211122

112211122

2

1221222

c l s l gc s l c l gs l l c θθθθθθθθυ

??????????+++++-+-=0)()

(2122112211122212211221112222θθθθθθθθ l c l s l gc l s l c l gs m f c ??

????????=00022

c n

向内递推计算 对于连杆2：

2222

c f f =

??

???

?????++++=)(002122212222122121221222

θθθθ l m gc l m s l l m c l l m n

对于连杆1：

????

??????++-+??????????+++-++--??????????-=00)()(10

001121111

121111222122212121

2121222212221212121222

2

2

11

gc m l m gs m l m gc m l m s l m c l m gs m l m c l m s l m c s s c f θθθθθθθθθθ

??????????++++=1222212212212221221211

)(00gc l m s l l m l m c l l m n θθθθ ++??

???

?????+11121100gc m l m θ ??

???

?????++-++=1122212212212212121211

)()(00gc l m s l l m c l l m l m n θθθθθ

最后，将11

n 和22n 的Z 轴分量列出，即得到两关节力矩：

22221212121212212212221)()2()(θθθθθθτ s l l m l m m c l l m l m -+++++=

11211222212212)(2gc l m m gc l m gs l l m ++++θθ

122221221221222122122)(gc l m s l l m l m c l l m ++++=θθθθτ

此为关节驱动力矩作为关节位移、速度和加速度的显函数表达式，即为平面2R 机械手动力学方程的封闭形式。

§6-4 关节空间和操作空间动力学

一 关节空间动力学方程

将上面得到的平面2R 机械手的动力学方程封闭形式写成矩阵形式：

?

?

????=21τττ

?

?????=21θθq ??????+++++=2

22221222212

2212122212211)()()2()(l m c l l l m c l l l m c l l l l m l m q D

()??????--=2122122

12212222212,2θθθθ s l l m s l l m s l l m h q q

??????++=122211211222)()(gc l m gc l m m gc l m G q 则：

()q q q q G q

h q D ++= ),()(τ

此为操作臂在关节空间中的动力学方程封闭形式的一般结构式，它反映了关节力矩与关节变量、速度和加速度之间的函数关系。对于n 个关节操作臂，D )(q 是

n n ?的正定对称矩阵，是q 的函数，称为操作臂的惯性矩阵；()q q h ,是1?n 的离心

力和哥氏力向量；

)

(q G 是1?n 的重力矢量，与机械手的形位q 有关。

D )(q 表示惯性力（矩）。D )(q 中的主对角元素表示各连杆本身的有效性，代表给定关节上的力矩与产生的角加速度之间的关系；而非主对角元素代表连杆之间的耦合惯性，即为某连杆的加速运动对另一关节产生耦合作用力矩的度量。

()

q q h ,包括两部分：共一与关节速度平方成正比，表示该关节速度产生的离心

力。其二与两关节速度的乘积成正比，称为哥氏力。

因而可将该项()q q h ,分成两部分，即；

()[][]2)()(,,q c q q

B h q q q q +=

)(q B 称为哥氏力的系数矩阵，为2/)1(-?n n n 阶矩阵。 )

(q C 是n n ?阶的离心力系数矩阵。

[]q q ,是2/)1(-n n 维的关节速度积矢量：[]q q ,=[]n

n q q q q q q

13

12

1,,-

[][]222

2

1

2

,,n

q q q q =是n 维关节速度平方矢量。

动力学方程可写成另一种形式——形位空间方程：

[]()q q q q G q C q q B q

D +++=2)()()(, τ

对于平面2R 机械手：

[][][][]T T q q q 2221221,,,θθθθ ==

???

???-=022212)

(s l l m B q ???

???-=0022122212)(s l l m s l l m C q

二 操作空间动力学方程

与关节可将动力学方程相对应，在笛卡尔操作空间中，操作力F 与末端加速

度X 方程表示为：之间的关系可用动力学

F=

()()

q q q P u x V ++ ,q ）（ …………①

）

（q V ：操作空间中的惯性矩阵 ()q q u ,：操作空间的离心力和哥氏力向量

()

q P ： 操作空间中的重力向量

x: 操作臂末端位姿向量

F ：广义操作力向量，它与关节力向量τ之间的关系为F J q T

)(=τ操作空间与

关节空间之间的速度与加速度的关系为：

q J x

q )(=

()q q a q J q J q J x

r q q q ,)()()(+=+=

将式①两端左乘)(q T

J ：

()()()

q q T q T q q T q T P J q q u J x V J F J )()()()(,++=

()()()()()

q q T q T r q q T q q q T q T P J q q u J q q a V J q J V J F J )()()()()()(,,+++=

比较关节空间与操作空间动力学方程可以看出两者之间存在以下关系：

)

()()(),()(),()()(),()

()()()(q P q J q G q q u q J q q a q V q J q

q h q J q V q J q D T T r T T =+==

如果雅可比)(q J 的逆存在，则与上述关系式相对应的有：

??

?

??=-==----)()()(),()()()(),()

()()()(1q G q J q P q q a q V q h q J q q u q J q D q J q V T r T T

【例】写出平面2R 机械手的操作空间动力学方程。

???????

???

?????????

????+=??????????+++--+-=???

?

?

?????-+=??

??

??-=??

????--=

????

??+=-1221222112121221212212

211212222222212221222122222

212213

212

2122211

322

21213)()2()(00)(00)(01)(0)(gc m gs m s gc m q P s l m s l m s c l m c l m l m l m l m c l m u s l s m m q V s l c l q J s l l c l l s l l q J l l c l s l q J θθθθθθθθθθ

当?=02s 或?180时，机械手处于奇异状态，操作空间动力学方程中的某些项趋于无限大。例如：操作空间中的有效惯性沿径向的分量变为无穷大，这一特定方向称为奇异方向。机械手不能沿此方向运动，但在垂直于这一方向的子空间内，一般还是可以的。

三、操作运动—关节力矩方程。

机器人动力学最终是研究其关节输入力矩与其输出的操作运动之间的关系。

[][][])()(,)()()()(),()()(2q G q q C q q q B x

q V q J q p q q u x

q V q J x x T T +++=++= ττ 一般情况下，)()(q B q B x ≠ )()(q C q C x ≠

§6-5拉格朗日动力学

前面推导的操作臂动力学方程的封闭形式是基于牛顿第二定律和欧拉方程给出的，并利用达朗贝尔原理，将静力平衡条件用于动力学问题。平衡方程中既

考虑外界驱动力，又计及产生加速度的惯性力。而拉格朗日动力学描述则基于系统能量的概念。对于任何机械系统，拉格朗日函数L 定义为系统总的动能k E 与总的势能

p

E 之差，即：

)(),(),(q E q q E q

q L p k -=

[]n q q q q ,,,21 = 是表示动能和势能的广义坐标。 []n q q q q ,,,21= 是相应的广义速度。 连杆i 具有的动能ki E i 为：

i

i i ci T i i ci T

ci i ki w I w V V m E 2121+=

操作臂所具有的动能是n 个连杆的动能之和，即：

∑==n

i ki

k E E 1

有前面的速度分析可知，ci V 和i i

w 是操作臂关节变量q 和关节速度q

的函数，故此操作臂的动能是关节变量和关节速度的标量函数，记为),(q q E k

，实际上，操作臂的动能为：

q q D q q

q E T

k )(21),(=

)(q D 是n n ?阶的操作臂惯性矩阵。显然，操作臂的动能k E 是惯性矩阵的二

次型，由于动能k E 为正，因而)(q D 是正定的矩阵。

连杆i 具有的势能

pi

E 为：

ci

T i pi p g m E 00-=

g 0

是13?的重力加速度向量。

ci p 0

是连杆i 质心的位置向量。

操作臂所具有的势能为各连杆的势能之和，即：

∑==n

i pi

p E E 1