機(jī)器人建模與控制課件：機(jī)器人動力學(xué)

機(jī)器人建模與控制

機(jī)器人動力學(xué)7.1.1

線速度和角速度的傳遞?點(diǎn)Q以線速度BVQ相對于坐標(biāo)系{B}運(yùn)動?{B}的原點(diǎn)以線速度AVBoRG相對于坐標(biāo)系{A}運(yùn)動?{B}以角速度A貝B繞坐標(biāo)系{A}運(yùn)動?線速度的傳遞關(guān)系為：AVQ

=AVBoRG

+RBVQ+A貝B

×

RBQBABA?坐標(biāo)系

C

以角速度B貝C繞坐標(biāo)系B運(yùn)動?B

以角速度A貝B繞坐標(biāo)系

A

運(yùn)動?坐標(biāo)系

C

繞坐標(biāo)系{A}運(yùn)動的角速度為：

A貝C

=A貝B

+RB貝CBA注意：

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

速度和加速度的傳遞7.1.2

線加速度的傳遞?線加速度的傳遞可通過對線速度傳遞關(guān)系式的求導(dǎo)獲得?特殊情況：坐標(biāo)系{A}的原點(diǎn)和坐標(biāo)系{B}的原點(diǎn)重合，有：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求導(dǎo)7.1.2

線加速度的傳遞?一般情況：如果坐標(biāo)系{A}的原點(diǎn)和坐標(biāo)系{B}的原點(diǎn)不重合?需加上{B}原點(diǎn)的線加速度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

角加速度的傳遞?類似的，角加速度的傳遞關(guān)系可以通過對角速度傳遞關(guān)系式求導(dǎo)得到

dt

B

RB

C

=RB

?C

+A

B

×RB

CBABABA7.1

速度和加速度的傳遞A

C

=A

B

+RB

CBA求導(dǎo)?考慮多個質(zhì)點(diǎn)連接形成剛體?設(shè)質(zhì)點(diǎn)i的質(zhì)量為mi

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

有：UVi

=UVBORG

+UQB

×

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

=UV?BORG

+UQ?B

×RBPi

+UQB

×UQB

×

RBPi

?注意：由于是剛體，

BP?i

=0?作用在質(zhì)點(diǎn)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ì)點(diǎn)i上的力矩：U

BNi

=RBPi

×U

fi

=mi

RBPi

×BUBU

UQ?B

×

RBPi

BUUV?BORG

+UQ?B

×RBPi

+UQB

×BU7.2

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

法結(jié)合律，不能將

后面的括號去掉?作用在整個剛體上的總力矩：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

在坐標(biāo)系{C}中的表示：C

CN

=

CN

=

RU

CN=R

?mi

RCPi

×UQ?C

×RCPi

+?mi

RCPi

×UQC

×UQC

×

RCPi

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

?imiBPi

=

07.2

剛體的慣性張量與歐拉方程?為強(qiáng)調(diào)聯(lián)體坐標(biāo)系原點(diǎ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ì)心坐標(biāo)系在慣性坐標(biāo)系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ì)心坐標(biāo)系在慣性坐

標(biāo)系U中的角速度7.2

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

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

tensor)

，或旋轉(zhuǎn)慣性矩陣(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的乘積替代點(diǎn)質(zhì)量，用積分運(yùn)算替代求和運(yùn)算，可得：?非對角元素Ixy

、Ixz和Iyz稱為慣性積(mass

product

of

inertia)7.2

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

CI

=:?Ixy?Ixz?IxyIyy?Iyz?Ixz?IyzIzzBBBB?例7-1:考慮如圖中的質(zhì)量為m，長度為l，寬度為w，高度為?的長方體連桿。連桿的質(zhì)量是均勻分布的。建立如圖所示的(原點(diǎn))位于長方體連桿質(zhì)心的聯(lián)體坐標(biāo)系{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ì)心坐標(biāo)系中?例7-2:考慮如圖中的質(zhì)量為m，長度為l，寬度為w，高度為?的長方體連桿。連桿的質(zhì)量是均勻

分布的。建立如圖所示的(原點(diǎn))位于長方體連桿一頂點(diǎn)的聯(lián)體坐標(biāo)系{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?

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

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

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

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

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

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

1到連

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

節(jié))關(guān)節(jié)力?注意：

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

牛頓-歐拉迭代動力學(xué)方程?考慮一般的連桿坐標(biāo)系{i}及其相鄰連桿坐標(biāo)系{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

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

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

=An?B

+RBn?C

+AnB

×

RBnC

?令A(yù)={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

牛頓-歐拉迭代動力學(xué)方程7.3.1

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

i+

Ri+1

i+1?求導(dǎo)得：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?對于平動型關(guān)節(jié)，因為e?i+1=0，

i+1

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

i+1

=

i+

Ri仙?

ii1i1i1i11i1i0i10101ii01ii01i1i7.3

牛頓-歐拉迭代動力學(xué)方程7.3.1

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

=AV?BORG

+RBV?Q+2AnB

×RBVQ+An?B

×RBQ+AnB

×AnB

×

RBQ?令A(yù)={0}，B={i}，Q為{i+1}原點(diǎn)：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

牛頓-歐拉迭代動力學(xué)方程7.3.1

向外迭代：速度和加速度的計算?對于平動型關(guān)節(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?對于轉(zhuǎn)動型關(guān)節(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

牛頓-歐拉迭代動力學(xué)方程?選取

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

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

AnB

×

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

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

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

Fi=

mU?

CiCiNi

=CiICi仙?

i

+

Ci仙i

×

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

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

相同，則上式中Ci仙?

i

=i仙?

i

，Ci仙i

=i仙i7.3

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

Fi?

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

{i}

ni+1ni

fi

Ni?作用于{i}原點(diǎn)的力向量fi表示連桿i

?

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

?

1施

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

?

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

示在坐標(biāo)系{i}下?由于連桿i對連桿i

+

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

+

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

的，用i+1fi+1在坐標(biāo)系{i

+

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

=

ifi

?

i+

Ri+1fi+11i7.3.2

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

fi+17.3

牛頓-歐拉迭代動力學(xué)方程{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?在坐標(biāo)系{i}中表示，除了力矩iNi，連桿i還受到連桿i

?

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

牛頓-歐拉迭代動力學(xué)方程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)遞推直至機(jī)器人基座1i1i1i1i1i1i7.3

牛頓-歐拉迭代動力學(xué)方程7.3.2

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

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

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

Ti

=

in

i?

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

Ti

=

if

iiTiiTi?注意：

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

0U?

0

=

G

，這里G與重力

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

牛頓-歐拉迭代動力學(xué)方程7.3.2

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

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

=l1

1

=0

07.3

牛頓-歐拉迭代動力學(xué)方程2000C2I2

=000000000C1I1

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

=l2

2

=11l200 m21mlll27.3.2

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

=0,3n3

=0機(jī)器人基座保持不動00仙0

=0

,0仙?

0

=07.3

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

?Si+1

Ci+1

0001Ci+1

?Si+1i+

R

=

Si+1

Ci+11i00

R

=1連桿間的相對轉(zhuǎn)動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

牛頓-歐拉迭代動力學(xué)方程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

為簡化表達(dá)式，這里用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

牛頓-歐拉迭代動力學(xué)方程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

牛頓-歐拉迭代動力學(xué)方程=

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

牛頓-歐拉迭代動力學(xué)方程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=?牛頓-歐拉方法是基于動力學(xué)方程以及作用在連桿之間約束力和力

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

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

機(jī)器人動力學(xué)方程的拉格朗日方法?機(jī)器人的動力學(xué)方程可表示為：

?

=

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

、末端執(zhí)行1

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

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

：系統(tǒng)動能?u小：系統(tǒng)勢能?一個機(jī)械結(jié)構(gòu)系統(tǒng)的動能和勢能的差值稱為拉格朗日函數(shù)，表示為：7.4

機(jī)器人動力學(xué)方程的拉格朗日方法?小,小?

=k小,小?

?

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

矩向量F

=0?機(jī)器人的動力學(xué)方程可進(jìn)一步表示為：

?

+=T

?

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

機(jī)器人動力學(xué)方程的拉格朗日方法

?

=

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

Nb

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

=k小,小?

?

u小新=T?

B小?

JT?

小Fi?i仙i

=R

仙i

=

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

nk=?kii00i7.4.1

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

iUCi

+仙RCiIi

RT仙ii0i0iTCTki

=miU

iUCi

+i仙CiIii仙iiTCT7.4

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

UCi

=J小?,仙i

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

J

)和

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

J

J+J

RCiIi

RTJOii0i0OiPiPiN

T

Ti=1?因為機(jī)器人的總動能非負(fù)，且僅在巾?

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

=?i=1ki

小,小?

=2

小?TM

小小?N

17.4

機(jī)器人動力學(xué)方程的拉格朗日方法ki

=miU

iUCi

+仙RCiIi

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

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

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

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

小NN7.4

機(jī)器人動力學(xué)方程的拉格朗日方法=?

?mi

g0T

0PCiui

=?mi

g0T0PCii=1i=1Np?1

T

m11

p?2

m21k

=

小?TM

小

小?

=

i1?

?

p?N

mN1?mij

=mij

小是矩陣M小

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

完整的拉格朗日動力學(xué)方程?由前述推得的機(jī)器人的總動能方程和總勢能方程可得到完整的機(jī)器人動力學(xué)方程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

機(jī)器人動力學(xué)方程的拉格朗日方法

?

+=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

機(jī)器人動力學(xué)方程的拉格朗日方法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

機(jī)器人動力學(xué)方程的拉格朗日方法

?

+=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

機(jī)器人動力學(xué)方程的拉格朗日方法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符號，

拉格朗日動力學(xué)方程可寫成更簡潔的形式：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

機(jī)器人動力學(xué)方程的拉格朗日方法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

機(jī)器人動力學(xué)方程的拉格朗日方法M小小?

+C