機器人建模與控制課件：機器人動力學

機器人建模與控制

機器人動力學7.1.1

線速度和角速度的傳遞?點Q以線速度BVQ相對于坐標系{B}運動?{B}的原點以線速度AVBoRG相對于坐標系{A}運動?{B}以角速度A貝B繞坐標系{A}運動?線速度的傳遞關系為：AVQ

=AVBoRG

+RBVQ+A貝B

×

RBQBABA?坐標系

C

以角速度B貝C繞坐標系B運動?B

以角速度A貝B繞坐標系

A

運動?坐標系

C

繞坐標系{A}運動的角速度為：

A貝C

=A貝B

+RB貝CBA注意：

需要用到角速度A貝B7.1

速度和加速度的傳遞7.1.2

線加速度的傳遞?線加速度的傳遞可通過對線速度傳遞關系式的求導獲得?特殊情況：坐標系{A}的原點和坐標系{B}的原點重合，有：AV?Q

=

RBV?Q+

AnB

×

RBVQ+

An?B

×

RBQ

+

AnB

×

RBVQ+

AnB

×

RBQ

=

RBV?Q+

2AnB

×

RBVQ+

An?B

×

RBQ

+

AnB

×

AnB

×

RBQ

BABABABABABABABABAdt?同理有：RBVQ

=

RBV?Q+

AnB

×

RBVQBABABAAV?Q

=

RBVQ

+

An?B

×

RBQ

+

AnB

×

RBQ

BABABA?注意到：AV

RBQ

=

RBVQ+

AnB

×

RBQBABA7.1

速度和加速度的傳遞AVQ

=

RBVQ+

AnB

×

RBQBABA求導7.1.2

線加速度的傳遞?一般情況：如果坐標系{A}的原點和坐標系{B}的原點不重合?需加上{B}原點的線加速度AV?Q

=

AV?BORG

+

RBV?Q+

2AnB

×

RBVQ+

An?B

×

RBQ

+

AnB

×?如果矢量B

Q保持不動?即：

BVQ

=

0,

BV?Q

=

0BABABA7.1

速度和加速度的傳遞AV?Q

=

AV?BORG

+

An?B

×

RBQ

+

AnB

×BA

AnB

×

RBQ

BA

AnB

×

RBQ

BAA

?C

=

A

?B

+

d

ARB

C

A

?C

=

A

?B

+

RB

?C

+

A

B

×

RB

CBABA7.1.3

角加速度的傳遞?類似的，角加速度的傳遞關系可以通過對角速度傳遞關系式求導得到

dt

B

RB

C

=RB

?C

+A

B

×RB

CBABABA7.1

速度和加速度的傳遞A

C

=A

B

+RB

CBA求導?考慮多個質(zhì)點連接形成剛體?設質(zhì)點i的質(zhì)量為mi

，則剛體的總質(zhì)量m=σimi?考慮該剛體的聯(lián)體坐標系{B}。在慣性坐標系{U}中，

有：UVi

=UVBORG

+UQB

×

RBPi?UVi表示質(zhì)點i在{U}中的速度?對速度求導獲得其加速度：UV?i

=UV?BORG

+UQ?B

×RBPi

+UQB

×UQB

×

RBPi

?注意：由于是剛體，

BP?i

=0?作用在質(zhì)點i上的力：U

fi

=mi

UV?i

=mi

UV?BORG

+UQ?B

×RBPi

+UQB

×UQB

×RBPi

BUBUBUBUBU7.2

剛體的慣性張量與歐拉方程=?mi

RBPi

×

UV?BORG

+?mi

RBPi

×+?mi

RBPi

×UQB

×UQB

×

RBPi

BUBUBUBUU

BN

=?iU

BNi

=?i

mi

RBPi

×UV?BORG

+UQ?B

×RBPi

+UQB

×BUBU?作用在質(zhì)點i上的力矩：U

BNi

=RBPi

×U

fi

=mi

RBPi

×BUBU

UQ?B

×

RBPi

BUUV?BORG

+UQ?B

×RBPi

+UQB

×BU7.2

剛體的慣性張量與歐拉方程注意叉乘不滿足乘

法結合律，不能將

后面的括號去掉?作用在整個剛體上的總力矩：i

i UQB

×

RBPi

BU UQB

×

RBPi

BUiσimi

RBPi

×UV?BORG

=0?總力矩可簡化為：U

CN

=?imi

RCPi

×UQ?C

×RCPi

+?i

mi

RCPi

×UQC

×UQC

×

RCPi

?計算U

CN

在坐標系{C}中的表示：C

CN

=

CN

=

RU

CN=R

?mi

RCPi

×UQ?C

×RCPi

+?mi

RCPi

×UQC

×UQC

×

RCPi

CUCUCUCUUCUCCUCUCUCUBU?如果將聯(lián)體坐標系{B}的原點選在剛體質(zhì)心上，則有：

?imiBPi

=

07.2

剛體的慣性張量與歐拉方程?為強調(diào)聯(lián)體坐標系原點在剛體質(zhì)心上這一情況，下面用{C}替代{B}i

i=?mi

CPi

×R

UQ?C

×RCPi

=?mi

CPi

×RUΩ?C

×R

RCPi

i

i=?i

?mi

CP

CP

C

UQ?C

=?i

?mi

CP

2C仙?

C?遵循之前符號規(guī)則，這里用仙?

C

=UQ?C

，表示該

(聯(lián)體)

質(zhì)心坐標系在慣性坐標系U中的角加速

度i^i^i^CUUCUCCUUCi

i=?mi

CPi

×C

UQ?C

×CPi

=??mi

CPi

×CPi

×

C

UQ?C

7.2

剛體的慣性張量與歐拉方程?imi

RCPi

×UQ?C

×

RCPiCUCU=?mi

R

RCPi

×CUUCR

UQ?C

×

RCPiCUUC?CN的第一項RUCiR

?mi

RCPi

×UQC

×UQC

×

RCPi

=?mi

R

RCPi

×R

UQC

×UQC

×

RCPi

=?mi

CPi

×RUQC

×R

UQC

×

RCPi

=?mi

CPi

×C

UQC

×RUQC

×R

RCPi

=?mi

CPi

×C

UQC

×C

UQC

×

CPi

=??mi

C

UQC

×CPi

×CPi

×C

UQC=??mi

C仙CP

2

C仙Ci^C^CUUCUCCUUCUCCUUCCUUCCUCUUC?遵循之前符號規(guī)則，這里

用仙C

=UQC，表示該(聯(lián)

體)質(zhì)心坐標系在慣性坐

標系U中的角速度7.2

剛體的慣性張量與歐拉方程?CN的第二項iiiiiii?該式為旋轉剛體的歐拉方程?歐拉方程描述了作用在剛體上的力矩CN與剛體旋轉角速度C仙C和角加速度C仙?

C之間的關系?CI稱為剛體的慣性張量(inertia

tensor)

，或旋轉慣性矩陣(rotational

inertia

matrix)CN

=

??mi

CP

2C仙?

C

+??mi

C仙CP

2C仙Ci∧C^i∧7.2

剛體的慣性張量與歐拉方程CN

=CIC仙?

C

+C仙C

×CIC仙C記CI

=

σi

?mi

CP

2i∧i

i?

?mixizi?

?miyizi?mi?可以看出，

CI是一個3×3矩陣?如將CPi完整記為

xi,yi,zi

T

，CI矩陣各元素為：?記為：CI

=:7.2

剛體的慣性張量與歐拉方程Ixx

?Ixy

?Ixz?

?mixiyi?

?mixizi?

?mixiyi?

?miyizi?IxyIyy?Iyz?Ixz?IyzIzz?mi?mix+

yi2i2y+

zi2i2x+

zi2i2CI

=Ixx

=?y2

+z2

p

x,y,z

dVBIyy

=?x2

+z2

p

x,y,zdVIzz

=?x2

+y2

p

x,y,zdVIxy

=?xypx,y,zdVIxz

=?xzpx,y,z

dVB?

慣性張量是一個對稱矩陣

Iyz

=

?

yzp

x,y,z

dV?慣性張量中的對角元素Ixx

、Iyy和Izz稱為慣性矩(mass

moments

of

inertia)?考慮質(zhì)量連續(xù)分布的剛體?用密度函數(shù)p

x,y,z

和微分單元體dV的乘積替代點質(zhì)量，用積分運算替代求和運算，可得：?非對角元素Ixy

、Ixz和Iyz稱為慣性積(mass

product

of

inertia)7.2

剛體的慣性張量與歐拉方程Ixx

CI

=:?Ixy?Ixz?IxyIyy?Iyz?Ixz?IyzIzzBBBB?例7-1:考慮如圖中的質(zhì)量為m，長度為l，寬度為w，高度為?的長方體連桿。連桿的質(zhì)量是均勻分布的。建立如圖所示的(原點)位于長方體連桿質(zhì)心的聯(lián)體坐標系{C}。計算該連桿在{C}下的

慣性張量。解：

該連桿的密度p=。?

l

?

l=?

?

w

y2

+z2

p

dydz=wp

?

?

d

+z2y

dz?

?=wp

?

+z2l

dz=wp

?

d

+

=wp

+

=l2

+?2

類似的，可計算得：Iyy

=?2

+w2

,Izz

=w2

+

l2

l(1)

慣性矩：Ixx

=

2l2l2?2?7.2

剛體的慣性張量與歐拉方程?

12

?

12

3

12

122?2?w

2w22l32l3z

z3l

l3

??3l?

?

?

?

32l2?2l2?2222

y3y2

+z2

pdxdydzwhX(3)

該連桿在{C}下的慣性張量： l2

+?2

0

?2

+w2

000 w2

+l2

?

l

w=p

d

dydz

=0類似的，可計算得：Iyz

=

0,

Iyz

=

02w22l22?2l7.2

剛體的慣性張量與歐拉方程w?xypdx

dydzw2Ixy

=

2l2l2?2?(2)

慣性積：CI

=00?

2whX解：

該連桿的密度p=。(1)

慣性矩：Ixx

=???y2

+z2

pdxdydz=??w

y2

+z2

p

dydz=wp

??d

+z2ydz?

l3

?

l3z

z3l333類似的，可計算得：?注意：

慣性張量也可以定義在非質(zhì)心坐標系中?例7-2:考慮如圖中的質(zhì)量為m，長度為l，寬度為w，高度為?的長方體連桿。連桿的質(zhì)量是均勻

分布的。建立如圖所示的(原點)位于長方體連桿一頂點的聯(lián)體坐標系{B}。計算該連桿在{B}下

的慣性張量。7.2

剛體的慣性張量與歐拉方程=wp

?+z2l

dz=wp

?d

+0

0=

wp

+

=

l2

+

?2

Iyy

=?2

+w2

,Izz

=w2

+

l2

000?

l

?

l

y300

00X?l

wwh3l(2)

慣性積：Ixy

=???xypdx

dydz=p

???d

dydz=p??dydz=p??ddz=p

?dz=

p

=

wl類似的，可計算得：Iyz

=l?,Ixz

=?w(3)

該連桿在{B}下的慣性張量：m

m

m l2

+?2

?

wl

?

?wBI

=

?

wl

?2

+

w2

?

l??

?w

?

l?

w2

+

l2

7.2

剛體的慣性張量與歐拉方程000?

l

w2y

?

l

w2y2

?

w2l2000?

l

w

x2y224

43440000

0X?l

wwhl?

利用牛頓-歐拉法求解動力學方程分兩個階段：?向外迭代：

從(虛擬)的連桿0開始，依次計算連桿1到N聯(lián)體坐標系得速度(線速度和角

速度)以及加速度(線加速度和角加速度)

，同時利用連桿i聯(lián)體坐標系得速度和加速度

計算連桿i質(zhì)心的加速度，并利用牛頓方程和歐拉方程求取作用在連桿上的力和力矩?向內(nèi)迭代：

從連桿N開始，根據(jù)力平衡方程和力矩平衡方程，依次計算出連桿N?

1到連

桿1上的力，同時計算出產(chǎn)生這些力和力矩所需的(轉動型關節(jié))關節(jié)力矩或(平動型關

節(jié))關節(jié)力?注意：

下面牛頓-歐拉迭代動力學方程的討論中忽略了摩擦的影響，即假設各關節(jié)均無摩擦7.3

牛頓-歐拉迭代動力學方程?考慮一般的連桿坐標系{i}及其相鄰連桿坐標系{i+1}，已知：i仙i+1

=i仙i

+i+

Re?i+1i+1

i+1iUi+1

=iUi

+i仙i

×iOi+1?

左乘i+

R

：i+1仙i+1

=

i+

Ri仙i

+

e?i+1i+1

i+1i+1Ui+1

=i+

R

iUi

+i仙i

×iOi+1

i1i1i11i7.3.1

向外迭代：速度和加速度的計算(1)連桿的速度傳遞:i+1Ui+1{i+1}i+1i+1ii7.3

牛頓-歐拉迭代動力學方程ii+1i+1PCi+1i+1UCi+1iUi{i}7.3.1

向外迭代：速度和加速度的計算(2)連桿的角加速度傳遞:?角加速度傳遞公式：An?C

=An?B

+RBn?C

+AnB

×

RBnC

?令A={0}，B={i}，

C={i+1}：仙?

i+1

=仙?

i+Rin?i+1

+仙i

×Rini+1

?ini+1=e?i+1

i+

Ri+1

i+1?注意：

ini+1和

i仙i+1具有不同的物理意義1ii0i0BABA7.3

牛頓-歐拉迭代動力學方程7.3.1

向外迭代：速度和加速度的計算ini+1=e?i+1

i+

Ri+1

i+1?求導得：in?i+1=i+1

i+

Ri+1

i+1?代入仙?

i+1式：仙?

i+1

=仙?

i

+R

i+1

i+

Ri+1

i+1

+仙i

×Re?i+1

i+

Ri+1

i+1

=仙?

i

+i+1

i+

Ri+1

i+1

+仙i

×e?i+1

i+

Ri+1

i+1?兩邊乘上

R：i仙?

i+1

=i仙?

i

+i+1

i+

Ri+1

i+1

+i仙i

×e?i+1

i+

Ri+1

i+1?為得到{i}到{i+1}的遞歸式?兩邊再乘上i+R：i+1仙?

i+1

=

i+

Ri仙?

i

+

i+1i+1

i+1

+

i+

Ri仙i

×e?i+1i+1

i+1?對于平動型關節(jié)，因為e?i+1=0，

i+1

=0，上式可簡化為：i+1仙?

i+1

=

i+

Ri仙?

ii1i1i1i11i1i0i10101ii01ii01i1i7.3

牛頓-歐拉迭代動力學方程7.3.1

向外迭代：速度和加速度的計算(3)連桿的線加速度傳遞：?線加速度傳遞公式AV?Q

=AV?BORG

+RBV?Q+2AnB

×RBVQ+An?B

×RBQ+AnB

×AnB

×

RBQ?令A={0}，B={i}，Q為{i+1}原點：U?i+1

=U?i+RiV?i+1

+2仙i

×RiVi+1+仙?

i×RiOi+1+仙i

×仙i

×RiOi+1

?兩邊同乘上

R：iU?i+1

=iU?i+iV?i+1+2i仙i×iVi+1+i仙?

i

×iOi+1

+i仙i

×i仙i×iOi+1

?兩邊再乘上i+

R，可得到{i}到{i+1}的遞歸式：i+1U?i+1

=i+

RiU?i+i仙?

i

×iOi+1

+i仙i

×i仙i×iOi+1+i+

R

iV?i+1+2i仙i×iVi+1

i1i1i10ii0i0i0i0BABABABA7.3

牛頓-歐拉迭代動力學方程7.3.1

向外迭代：速度和加速度的計算?對于平動型關節(jié)：i+

RiVi+1

=

i+1Vi+1=

d?i+1i+1

RiV?i+1=

i+1V?i+1=

i+1i+1

i+1

，i+1仙i+1

=

i+

Ri仙i?i+1U?i+1式可表示為：i1i1i+1U?i+1

=i+

RiU?i+i仙?

i

×iOi+1

+i仙i

×i仙i

×iOi+1+i+1i+1

i+1

+2i+1仙i+1

×d?i+1i+1

i+1i1?對于轉動型關節(jié)：iVi+1=0，iV?i+1=0?i+1U?i+1式可簡化為：i+1U?i+1

=i+

RiU?i+i仙?

i

×iOi+1

+i仙i

×i仙i×iOi+1

i17.3

牛頓-歐拉迭代動力學方程?選取

A=0，

B={i}，Q為連桿i質(zhì)心，表示為Ci?因為BVQ

=iVCi

=0，BVQ

=iV?Ci

=0，有：U?

Ci

=U?i+仙?

i×RiPCi

+仙i

×仙i

×RiPCi

?兩邊同乘上

R：iU?

Ci

=iU?i+i仙?

i

×iPCi

+i仙i

×i仙i

×iPCi

0ii0i07.3.1

向外迭代：速度和加速度的計算(4)連桿質(zhì)心的線加速度傳遞：i

i?

為了計算作用在連桿質(zhì)心上的力，還需

要計算連桿質(zhì)心的加速度?線加速度傳遞公式：AV?Q

=AV?BORG

+RBV?Q+2AnB

×RBVQ+An?B

×RBQ+AnB

×BABABA7.3

牛頓-歐拉迭代動力學方程i+1Ui+1{i+1}i+1i+1

AnB

×

RBQBAiUi{i}i+1PCi+1i+1UCi+1ii+1?由前面的計算，我們可以從連桿0開始，

向外迭代計算連桿1至連桿n的質(zhì)心坐標系的線加速

度、角速度和角加速度?接著利用牛頓-歐拉公式，可計算作用在連桿質(zhì)心上的慣性力和力矩:

Fi=

mU?

CiCiNi

=CiICi仙?

i

+

Ci仙i

×

CiICi仙i?坐標系{Ci}的原點位于連桿質(zhì)心，

可以選取坐標系{Ci}的各坐標軸方向與原連桿坐標系{i}方向

相同，則上式中Ci仙?

i

=i仙?

i

，Ci仙i

=i仙i7.3

牛頓-歐拉迭代動力學方程ni+1fi+1{i+1}NinifiFi{i}?考慮連桿i，先考慮其力平衡方程

Fi?

右圖為典型連桿在無重力狀態(tài)下的受力情況

{i}

ni+1ni

fi

Ni?作用于{i}原點的力向量fi表示連桿i

?

1施加在連桿i上的力，力矩向量ni表示連桿i

?

1施

加在連桿i上的力矩?除了慣性力iFi，連桿i還受到連桿i

?

1施加在連桿i上的ifi，這里左上標表示這兩個力表

示在坐標系{i}下?由于連桿i對連桿i

+

1有一作用力fi+1，所以連桿i

+

1對連桿i有一反作用力?fi+1，同樣

的，用i+1fi+1在坐標系{i

+

1}下表示fi+1?將所有作用于連桿i上的力向量相加，得到力平衡方程：iFi

=

ifi

?

i+

Ri+1fi+11i7.3.2

向內(nèi)迭代：力和力矩的計算

fi+17.3

牛頓-歐拉迭代動力學方程{i

+

1}?由于連桿i對連桿i

+

1有一作用力矩ini+1，所以連桿i

+

1對連桿i有一反作用力矩?

i+1ni+1

，

這里ini+1

=

i+

Ri+1ni+1?存在ifi和?ifi+1在連桿i質(zhì)心處產(chǎn)生的力矩?將所有力矩向量相加，得到力矩平衡方程：1i?在坐標系{i}中表示，除了力矩iNi，連桿i還受到連桿i

?

1施加在連桿i上的力矩ini7.3

牛頓-歐拉迭代動力學方程7.3.2

向內(nèi)迭代：力和力矩的計算?下面考慮作用在連桿i質(zhì)心處的力矩平衡方程

iOi+1

?

iPCi

iNi

=

ini

+?ifi+1?iPCi?

ini+1×ifi

+ni+1fi+1{i

+

1}×NinifiFi{i}7.3.2

向內(nèi)迭代：力和力矩的計算?將力平衡方程與力矩平衡方程整理為連桿i

+

1到連桿i的的迭代形式：?力平衡方程ifi

=

i+

Ri+1fi+1

+iFi?力矩平衡方程ini

=

iNi

+

i+

Ri+1ni+1

+iPCi

×ifi

+

iOi+1

?

iPCi

×ifi+1?由于ifi

=

ifi+1

+iFi

，有：ini

=

iNi

+

i+

Ri+1ni+1

+

iPCi

×

ifi+1

+iFi

+

iOi+1

?

iPCi

×ifi+1=

iNi

+

i+

Ri+1ni+1

+iPCi

×iFi

+iOi+1

×ifi+1=

iNi

+

i+

Ri+1ni+1

+iPCi

×iFi+iOi+1

×

i+

Ri+1fi+1?用上述方程對連桿依次求解，從連桿N開始向內(nèi)遞推直至機器人基座1i1i1i1i1i1i7.3

牛頓-歐拉迭代動力學方程7.3.2

向內(nèi)迭代：力和力矩的計算?對于轉動型關節(jié)i

，為產(chǎn)生力矩ni

，所需的關節(jié)力矩：

Ti

=

in

i?

對于平動型關節(jié)i，為產(chǎn)生力fi，所需的關節(jié)力：

Ti

=

if

iiTiiTi?注意：

上面的討論并沒有談及重力?這是因為我們可以考慮慣性系中連桿坐標系{0}以加速度G運動，即

0U?

0

=

G

，這里G與重力

矢量大小相等，方向相反，其產(chǎn)生的效果就與重力作用的效果是一樣的7.3

牛頓-歐拉迭代動力學方程7.3.2

向內(nèi)迭代：力和力矩的計算?例7-3

：計算如圖所示平面機器人的動力學方程，假設每個連桿的質(zhì)量都集中在連桿末端。解：連桿質(zhì)心的位置矢量11PC1

=l1

1

=0

07.3

牛頓-歐拉迭代動力學方程2000C2I2

=000000000C1I1

=000000連桿質(zhì)心的慣性張量2PC2

=l2

2

=11l200 m21mlll27.3.2

向內(nèi)迭代：力和力矩的計算無力作用于末端執(zhí)行器上，即：03f3

=0,3n3

=0機器人基座保持不動00仙0

=0

,0仙?

0

=07.3

牛頓-歐拉迭代動力學方程2Ci+1Si+10

?Si+1

Ci+1

0001Ci+1

?Si+1i+

R

=

Si+1

Ci+11i00

R

=1連桿間的相對轉動110U?

0

=g

0

=考慮重力0g000000000 m21mll2?向外迭代：連桿0到連桿1

(i

=0)

:1仙1

=

R0仙0

+

e?1

1

1

=

R

m22m1l101010

0

0

=0

1

e?11仙?

1

=R0仙?

0

+R0仙0

×e?1

1

1

+11

1=

R0101017.3

牛頓-歐拉迭代動力學方程7.3.2

向內(nèi)迭代：力和力矩的計算?m1l1e?

+m1gS1

m1l1

1

+m1gC11200

×

e?11

1

+

10?l1e?

+gS1

l1

1

+gC10120仙?

0

×0O1+0仙0

×1N1=C1I11仙?

1

+1仙1

×C1I1

1仙1

=1U?

C1

=1仙?

1

×1PC1

+1仙1

×0000000000

0

0

=0

1

11仙1

×1PC1

+1U?

1

=00+

R0010仙0

×0O1+0U?

0

=00

+e?101F1=m1

1U?

C1

=式中，C1I1

=gS1gC101U?

1

=R010000l217.3.2

向內(nèi)迭代：力和力矩的計算?向外迭代：連桿1到連桿2

(i=1)

:m2

2仙2

=R1仙1

+e?22

2

=0

=0

為簡化表達式，這里用e?12

=e?1

+e?21202仙?

2

=R1仙?

1

+R1仙1

×e?22

2

+22

2

=0122U?

2

=

R

1仙?

1

×

1O2

+

1仙1×

1仙1×

1O2

+

1U?

1

=

l1

1C2

+

l1e?

S2

+

gC12121212122m2l1

1S2

?

m2l1e?

C2

?

m2l2e?

2

+m2gS12

m2l1

1C2

+m2l1e?

S2

+m2l2

12

+m2gC12121212?l2e?

2

l2

120+l1

1S2

?

l1e?

C2

+gS12

l1

1C2

+l1e?

S2

+gC1201212127.3

牛頓-歐拉迭代動力學方程l1

1S2

?

l1e?

C2

+gS1201200e?1

+e?2

e?122N2=C2I22仙?

2

+2仙2

×C2I22仙2

=2U?

C2

=2仙?

2

×2PC2

+2仙2

×000000000

2仙2

×2PC2

式中，C2I2

=2F2=m22U?

C+2U?

2

=000 m1210=ll217.3.2

向內(nèi)迭代：力和力矩的計算?向內(nèi)迭代：連桿3到連桿2

(i

=

2)

:2f2

=

R3f3

+

2F2

=

m2

l1

1C2

+

m2

l1

e?

S2

+

m2

l2

12

+

m2gC122n2

=

2N2

+

R3n3

+

2PC2

×

2F2

+

2O3

×

R3f30=

0m2

l1

l2

1C2

+

m2

l1

l2

e?

S2

+

m2

l

12

+

m2

l2gC12221232321232=

0m2

l1

l2

1C2

+

m2

l1

l2

e?

S2

+

m2

l

12

+

m2

l2gC122212T2

=

2n

2

2T0

02T7.3

牛頓-歐拉迭代動力學方程=

m2

l1

l2

1C2

+

m2

l1

l2

e?

S2

+

m2

l

12

+

m2

l2gC122212m2

l1

1S2

?

m2

l1

e?

C2

?

m2

l2

e?

2

+

m2gS1212120107.3.2

向內(nèi)迭代：力和力矩的計算?向內(nèi)迭代：連桿2到連桿1

(i

=

1)

:1f1

=

R2f2

+

1F1

=

m12

l1

1

?

m2

l2S2e?

2

+

m2

l2C2

12

+

m12gC1

1221T1

=

1n

1

1C2

+

m2

l1

l2

e?

?

e?

2

S2

+

m2

l

12

+

m2

l2gC12

+

m12

l

1

+

m12

l1gC1122212121T00S2

+

m2

l

12

+

m2

l2gC12

+

m12

l

1

+

m12

l1gC112227.3

牛頓-歐拉迭代動力學方程0為簡化表示m12

=

m1

+

m21n1

=

1N1

+

R2n2

+

1PC1

×

1F1

+

1O2

×

R2f22121?m12

l1

e?

?

m2

l2C2e?

2

?

m2

l2S2

12

+

m12gS11212

e?

?

e?

2

1212

1

+

12

1

+

12

C2

+

m2

l1

l2=

m2

l1

l2m2

l1

l2=?牛頓-歐拉方法是基于動力學方程以及作用在連桿之間約束力和力

矩的分析之上的?拉格朗日力學是基于能量項對系統(tǒng)變量及時間微分的動力學方法?對于一個機器人來說，這兩種方法得到的運動方程是相同的?運用拉格朗日力學比運用牛頓力學更繁瑣，

但隨著系統(tǒng)復雜程度的增加，前者將變得相對簡單7.4

機器人動力學方程的拉格朗日方法?機器人的動力學方程可表示為：

?

=

新?新是非保守力/力矩向量?它包括關節(jié)力/力矩向量T=[T…T]T、摩擦力/力矩向量B小?

、末端執(zhí)行1

N器與環(huán)境接觸而引起的關節(jié)負荷力/力矩向量JT

小F小?aa小??aa?關節(jié)向量巾為廣義坐標?k小,小?

：系統(tǒng)動能?u?。合到y(tǒng)勢能?一個機械結構系統(tǒng)的動能和勢能的差值稱為拉格朗日函數(shù)，表示為：7.4

機器人動力學方程的拉格朗日方法?小,小?

=k小,小?

?

u小?本章中假設機器人末端執(zhí)行器與環(huán)境不接觸，因此末端執(zhí)行器與環(huán)境的接觸力/力

矩向量F

=0?機器人的動力學方程可進一步表示為：

?

+=T

?

B小?小uaa小kaa小?kaa7.4

機器人動力學方程的拉格朗日方法

?

=

新小?aa小??aa式中，B=diag(b,…,b)，1

Nb

為折算到關節(jié)i的粘滯摩擦參數(shù)?小,小?

=k小,小?

?

u小新=T?

B小?

JT?

小Fi?i仙i

=R

仙i

=

RT仙i?整個操作臂的動能是各個連桿動能之和:

nk=?kii00i7.4.1

動能的計算?考慮N連桿機器人，其中ki表示連桿i的動能?連桿i的動能：=miU

iUCi

+仙RCiIi

RT仙ii0i0iTCTki

=miU

iUCi

+i仙CiIii仙iiTCT7.4

機器人動力學方程的拉格朗日方法i=1?運用前面微分運動學部分引入的雅可比矩陣，可由關節(jié)變量計算UCi以及仙i：

UCi

=J小?,仙i

=J小??將各連桿的動能相加，并注意J

J

)和

R都依賴于Φ，就得到機器人的總動能：i0O(iOiPi?對稱矩陣M小稱為慣性矩陣：M小=?mi

J

J+J

RCiIi

RTJOii0i0OiPiPiN

T

Ti=1?因為機器人的總動能非負，且僅在巾?

=0時總動能為零，所以慣性矩陣還是一個正定矩陣k小,小?

=?i=1ki

小,小?

=2

小?TM

小小?N

17.4

機器人動力學方程的拉格朗日方法ki

=miU

iUCi

+仙RCiIi

RT仙ii0i0iTCT?操作臂的總勢能：nu=?

uii=1?將各連桿勢能相加，并注意

0PCi依賴于Φ，就得到機器人的總勢能：7.4.2

勢能的計算?0g表示世界坐標系中的重力加速度向量?例如，如果以y軸為豎直向上方向，則0g=0,?g,0T?PCi是連桿質(zhì)心的位置矢量?連桿i的勢能：u小=?ui

小NN7.4

機器人動力學方程的拉格朗日方法=?

?mi

g0T

0PCiui

=?mi

g0T0PCii=1i=1Np?1

T

m11

p?2

m21k

=

小?TM

小

小?

=

i1?

?

p?N

mN1?mij

=mij

小是矩陣M小

第i行第j列元素?利用展開后的總動能表達式，可計算得到：ii7.4.3

完整的拉格朗日動力學方程?由前述推得的機器人的總動能方程和總勢能方程可得到完整的機器人動力學方程m11m21?mj1?N1m12m22?mj2?N2?m1i

?m1N?m2i

?m2N?????

mji

?

mjN?????mNi

?

mNN?m1j

?m1N?m2j

?m2N?????mij

?

miN???

??mNj

?

mNN?m1j

?m1N?m2j

?m2N?????mij

?

miN???

??mNj

?

mNN7.4

機器人動力學方程的拉格朗日方法

?

+=T

?

B小?小uaa小kaa小?kaa①

=

p?1

T

p?2

?

p?j

?

p?Np?1p?2?p?j?p?Nm11m21?mi1?N1m12m22?mi2?N2m12m22?mi2?N2p?1

p?2

?

p?j

?

p?N00?1?000?1?0+

12mmmmmT?m1j

?m1N?m2j

?m2N?????mij

?

miN???

??mNj

?

mNN=?1

?=1

p?kp?j

+

?

1

mijp?jkN7.4

機器人動力學方程的拉格朗日方法d

mi1

mijdt

miNmi2mi2mi2ddt

?

+=T

?

B小?小uaa小kaa小?kaamij

=?=1

p?kkN00

?

1

?

0p?1

p?2

?

p?j

?

p?Nmi1

T

mi2

?

mij?miNmi1

mi2

?

mij

?

miNp?1p?2?p?j?p?Np?1p?2?p?j?p?Np?1p?2?p?j?p?Nd

ak

ddta??i=

dt?M小是對稱矩陣，有：m11m21?m12m22?=

?

mN1?

mN2mi2mi1+=TTTam11apiam21api?amj1api?amN1apiam12apiam22api?amj2api?amN2api??????am1kapiam2kapi?amjkapi?amNkapi??????am1Napiam2Napi?amjNapi?amNNapi=?1

?=1

p?kp?j③=?

?1

mj

0gT

=gi

小

kN?=1

k

p?k

?=1

k

p?k??=1

p?k??=1

k

p?kkNkNkNkN7.4

機器人動力學方程的拉格朗日方法

?

+=T

?

B小?小uaa小kaa小?kaap?1p?2?p?j?p?Np?1p?2?p?j?p?Np?1p?2?p?k?p?N

=

1=

2

②

TT?則有：?1

?=1

?

p?kp?,

=?1

?=1

+?

p?kp?,

=?1

?=1

Ck,ip?kp?,1

ami,

amik

am,kkNkNkN

=?=1

?=1

p?kp?,

+

?1

mi,p?,

=

?

1

?

=1

p?kp?,kNkN,N7.4

機器人動力學方程的拉格朗日方法Ck,i

=

2

apk

+

ap,

?

api

稱為

(第一類)

Christoffel符號?1

?=1

p?kp?,

=?1

?=1kNkN

?

+=T

?

B小?小uaa小kaa小?kaa?

1

mi,p?,+?

1

?

=1kNapi

=

?

?,=1

m,

0gT

api

=

gi

小

p?kp?,+gi

小=Ti?

bip?i,i=1,2,?,Nami,

1

am,kapk

2

apiami,

amikapk

ap,

au

N

a0PCj?可證：p?kp?,+?Ck,i

=

2

apk

+

ap,

?

api?因為M小是對稱矩陣，有：?可以發(fā)現(xiàn)：C,ki

=

2

ap,

+

apk

?

apim,k=

mk,C,ki=

Ck,i?利用(第一類)

Christoffel符號，

拉格朗日動力學方程可寫成更簡潔的形式：N

N

Nmi,p?,

+?

p?kp?,

+gi

小=Ti

?

bip?i,i=1,2,?,N?1

mi,p?,+?1

?=1

Ck,ip?kp?,+giΦ=Ti?

bip?i,i=1,2,?,NkN7.4

機器人動力學方程的拉格朗日方法1

amik

ami,

amk,1

ami,

amik

am,km12m22?mi2?N2??????m1,

?

m2,

?

?

?mi,?

??mN,??kCk11p?k

?kCk12p?k??kCk1ip?k??kCk1Np?k?kCk21p?k

?kCk22p?k??kCk2ip?k??kCk2Np?k??Ck,1p?kk????Ck,ip?kk???????kCkN1p?k

?kCkN2p?k??kCkNip?k??kCkNNp?kp?1

p?2

?

p?,

?

p?Ng1

g2?gi?

N=T1T2?Ti?TN?

1

mi,p?,+?

1

?

=1

Ck,ip?kp?,+

gi將i=1,2,?,N所有等式寫成如下的矩陣形式:kNp?1

p?2

?

p?,

?

p?N+10?0?00b2?0?0??????0?0???bi

???0?0

0

?

0?

bNp?1

p?2

?

p?i?

p?N+?矩陣C的第(i,j)項元素被定義為:Ci,

=?Ck,ip?k7.4

機器人動力學方程的拉格朗日方法M小小?

+C