機(jī)器人建模與控制課件：機(jī)器人微分運(yùn)動(dòng)學(xué)和靜力學(xué)

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

微分運(yùn)動(dòng)學(xué)與靜力學(xué)向量B

Q

的微分表示為如下同維向量若

B

Q是描述某個(gè)點(diǎn)的位置向量，該點(diǎn)關(guān)于{B}的速度是

B

VQ

像其他向量一樣，速度向量B

VQ

可在任意坐標(biāo)系中描述A

(

B

VQ

)

=

R

B

VQ

=

B

Q

=

R(t)

需要注意，

A

(

B

VQ

)不同于

AVQ

=

Q

t→0

t當(dāng)兩個(gè)上標(biāo)相同時(shí)，無(wú)需給出外層上標(biāo)，即B

(

B

VQ

)

=

B

VQti

lBAti

lBA

5.1

時(shí)變位姿的符號(hào)表示

P

=

PBORG

+

R

P

AAAAAAAAAABAABAA

BORG

(t

+

t)

+

R(t

+

t)

B

Q(t

+

t)

?

APBORG

(t)

?

R(t)

BQ(t)BABA5.1.1

線速度向量?位置向量的微分速度大小取決于兩個(gè)坐標(biāo)系：(1)進(jìn)行微分的局部坐標(biāo)系；(2)描述該速度向量的觀測(cè)坐標(biāo)系。B

d

B

B

Q(t

+

t)?BQ(t)Q

dt

t→0

tV

=Q

=

lim5.1.1

線速度向量?世界坐標(biāo)系的速度經(jīng)常討論的是一個(gè)坐標(biāo)系原點(diǎn)相對(duì)于世界坐標(biāo)系{U}的

速度，對(duì)于這種情況，定義一個(gè)縮寫符號(hào)oC

=U

VCORG式中的點(diǎn)為坐標(biāo)系

{C}的原點(diǎn)特別要注意下列符號(hào)的意思AoC

=RoC

=R

U

VCORG

CoC

=RoC

=R

U

VCORGUCUCUAUA5.1

時(shí)變位姿的符號(hào)表示

AVCORGX八X八BORGZ八A由理論力學(xué)知：剛體(其聯(lián)體坐標(biāo)系為{B}

)在參考坐標(biāo)系{A}中的任何運(yùn)動(dòng)都可以分解為

點(diǎn)APBORG

的運(yùn)動(dòng)和剛體繞

APBORG

的定點(diǎn)轉(zhuǎn)動(dòng)剛體的定點(diǎn)轉(zhuǎn)動(dòng)剛體繞體內(nèi)或其外延部分的一固定點(diǎn)旋轉(zhuǎn)定點(diǎn)轉(zhuǎn)動(dòng)不同于定軸轉(zhuǎn)動(dòng)(T

=

J9)5.1

時(shí)變位姿的符號(hào)表示5.1.2

角速度向量APY八BY八AAB5.1.2

角速度向量?jī)H考慮剛體(或{B})

的定點(diǎn)轉(zhuǎn)動(dòng)，令

APBORG

=，0{B}與{A}原點(diǎn)重合由理論力學(xué)知：在任一瞬間，

{B}在{A}中的定點(diǎn)轉(zhuǎn)動(dòng)X八BORGBX八Y八BAP在{A}中描述{B}的定點(diǎn)轉(zhuǎn)動(dòng)可用角速度向

量A

B

，A

B

的方向是瞬軸在{A}中的方向

A

B

的大小表示在{A}中{B}繞瞬軸的旋轉(zhuǎn)速

度可以看作是繞瞬時(shí)轉(zhuǎn)動(dòng)軸

(簡(jiǎn)稱瞬軸，

瞬軸上的每個(gè)點(diǎn)在該瞬時(shí)相對(duì)于{A}的速度為零)的轉(zhuǎn)動(dòng)5.1

時(shí)變位姿的符號(hào)表示瞬軸的位置可隨時(shí)間t變化,但原點(diǎn)始終在瞬軸上BBAA

B{B}X八Z八Z八Y八Y八AAB5.1.2

角速度向量像其他向量一樣,

角速度向量可以在任意坐標(biāo)系中描述C

(A

B

)=R

A

BAC經(jīng)常討論的是動(dòng)坐標(biāo)系(比如{C})

相對(duì)于世界坐標(biāo)系{U}的角速度，對(duì)于這種情況，定義一個(gè)縮寫符號(hào)C

C特別要注意下列符號(hào)的意思AoC

=RoC

=R

U

C

豐

A

C

CoC

=RoC

=R

U

CUCUCUAUA5.1

時(shí)變位姿的符號(hào)表示o

=

U

5.2.1

剛體純平移時(shí)的線速度變化坐標(biāo)系間僅有線速度(即純平移)時(shí)，點(diǎn)的速度變化坐標(biāo)系{B}固連在剛體上，描述

B

Q

相對(duì)于坐標(biāo)系{A}的運(yùn)動(dòng)。

{B}相對(duì)于{A}用位置向量

APBORG

和旋轉(zhuǎn)矩陣

R

來(lái)描述。若方位

R

不隨時(shí)間變化，則Q點(diǎn)相對(duì)于坐標(biāo)系{A}的運(yùn)動(dòng)是由于BABA坐標(biāo)系{A}中的Q點(diǎn)的線速度:AVQ

=AVBORG

+

R

B

VQ只適用于坐標(biāo)系{B}和坐標(biāo)系{A}的相對(duì)方位保持不變的情況。BA5.2

剛體的線速度與角速度或

B

Q

隨時(shí)間的變化引起的。BQBORGAPRRT

+RRT

=RRT

+(RRT

)T

=

0n定義

S

=RRT

,

由此有

S

+ST

=

0nS

是一個(gè)反對(duì)稱陣(skew-symmetric

matrix)

.

正交矩陣的微分與反對(duì)稱陣之間存在如下特性:S

=

RR

?1正交矩陣的導(dǎo)數(shù)性質(zhì)對(duì)任何

n

n

的正交矩陣

R

，有:求導(dǎo)，得到:5.2.2

剛體一般運(yùn)動(dòng)時(shí)的線速度變化5.2

剛體的線速度與角速度RRT

=

InQ

t→0

t=lim

+lim

t→0

t

t→

0

tddt=AVBORG

+R

B

Q

+R

B

VQBABAA

AQ(t

+t)?AQ(t)Q

t→0

tB

B

Q(t

+t)?BQ(t)Q

t→0

tA

BORG

(t

+

t)

+

R(t

+

t)

B

Q(t

+

t)

?

APBORG

(t)

?

R(t)

BQ(t)BABA?Q是空間中的動(dòng)點(diǎn)，{B}是動(dòng)坐標(biāo)系，求APBORG

(t

+

t)

?

APBORG

(t)

R(t

+

t)

B

Q(t

+

t)

?

R(t)

BQ(t)BABA5.2.2

剛體一般運(yùn)動(dòng)時(shí)的線速度變化5.2

剛體的線速度與角速度與AVQ

B

VQ

的關(guān)系A(chǔ)P

=APBORG

+

R

BPBA{A}=AVBORG

+R(t)B

Q(t)BA和V

=

limV

=

lim5.2.2

剛體一般運(yùn)動(dòng)時(shí)的線速度變化AR

=

lim

R(t

+

t)

?

R(t)AK是瞬軸的歸一化向量，即BABAB

t→0

t在時(shí)間間隔Δt中，通過(guò)繞瞬軸勻速旋轉(zhuǎn)φ角度，

姿態(tài)

R(t)變成姿態(tài)

R(t

+t)根據(jù)等效軸角方法，有

R(t

+t)=Rot(AK,Q)

R(t)BABABABA「kx

]「x

Q]AK

=

k

y

Q

角速度向量|Lkz

」||L

z

Q」|標(biāo)量Q表示旋轉(zhuǎn)速度則

R

=

lim

Rot(

K,Q)

?

I3

R(t)

t→0

tRot(KA

,Q)?I3

AQ→0

QAABABA5.2

剛體的線速度與角速度「x

]||

A|L

z

」|=lim

QB

R(t)|

y

|

=

BRot(AK,0)=I3lim

=limQ→0

Q

Q→0

Q

?

0「

kx

kx

vQ+cQRot(AK,Q)=kx

ky

vQ+kz

sQ|Lkx

kz

vQ?ky

sQvQ=1?cQ5.2.2

剛體一般運(yùn)動(dòng)時(shí)的線速度變化5.2

剛體的線速度與角速度「kx

kx

sQ?sQ=

|kx

ky

sQ+

kz

cQ

|Lkx

kz

sQ?ky

cQkx

kz

vQ+ky

sQ]ky

kz

vQ

?

kx

sQ

|

kz

kz

vQ+cQ

」|kx

kz

sQ+ky

cQ]ky

kz

sQ

?

kx

cQ

|

kz

kz

sQ?sQ

」|kx

ky

vQ?kz

sQky

ky

vQ+cQky

kz

vQ+kx

sQkx

ky

sQ?kz

cQky

ky

sQ?sQky

kz

sQ+kx

cQRot(

K,Q)

?

I3

Rot(

K,Q)

?

Rot(

K,

0)AAAAAAAAAAAAAAA=

Rot(

AK,Q)「0

|=

|

kz

|L?ky顯然于是ky

]?kx

|0

|?kz

0k|」已知Q=0Q=0|x||

的一

變化

A〉B

=

=

1

=

2222222223332321bbbb1b3b2213aaab2b1b3a1a3a2度度時(shí)的線速與角速動(dòng)度運(yùn)速般線體體2

剛剛?cè)鬗

=?M

T

，稱M為反對(duì)稱矩陣

角速度矩陣

S

是反對(duì)稱矩陣AVQ

=AVBORG

+R

B

Q

+R

B

VQ

=AVBORG

+S

R

B

Q

+R

B

VQBABABABABABA「0?kz

ky

]=

Q|

kz

0

?kx

||L?ky

kx

0

」|x對(duì)應(yīng)角速度向量

QB

=

AAzyQQ]

「0

，定義角速度矩陣

S

=Qz

」|

|L?Qy?Qz0QxBAAR「0

|=

|

Qz

|L?Qy?Qz0QxQ

]?Qx

R

=

S

R0

|BABABAAVQ

=AVBORG

+R

B

VQ

+A

QB

〉R

BQBABAR

=

Qlim

Rot(

K,Q)

?

I3

RQ→

0

QAABABAB

B

B

B

BAS

=

AR

AR

?1

=

AR

ARTSP

=A

QB

〉PBAQ

]|

?Qx

|

=0

|B

B

BAR

=

AS

AR||3〉3「Q」」yyB?理論力學(xué)證明：A

C

=A

B

+R

B

CBA?{A}

、{B}和{C}是動(dòng)坐標(biāo)系，A

、B

和A

的關(guān)系如何?5.2.3

運(yùn)動(dòng)坐標(biāo)系之間的角速度向量關(guān)系5.2

剛體的線速度與角速度B

C

C5.2.3

運(yùn)動(dòng)坐標(biāo)系之間的角速度向量關(guān)系{A}、

{B}和{C}是動(dòng)坐標(biāo)系，A

業(yè)B

、B

業(yè)C

和A

業(yè)C

的關(guān)系如何？R

=R

R

R

=R

R

+

R

RC

C

B

B

C

B

C

C=S

R

+R

B

業(yè)C

根B

XC

B

業(yè)C

根B

YC

B

業(yè)C

根B

ZC

=S

R

+R

B

業(yè)C

根R

B

XC

R

B

業(yè)C

根R

B

YC

R

B

業(yè)C

根R

B

ZC

=S

R

+R

B

業(yè)C

根AXC

R

B

業(yè)C

根AYC

R

B

業(yè)C

根AZC

=S

R

+A

rC

根AXC

A

rC

根AYC

A

rC

根AZC

=S

R

+W

R

=(S

+W)

RCACABACACACABACABABABABACABABABABABABABACABABACABACBBACBBACACBBACAyy

]W

=

|

yz|L?yyCA?yz0yx?yx

|0

|「0|」|「yx

]A

rC

=y

R

B

業(yè)C

|Lyz

」|5.2

剛體的線速度與角速度先乘除后加減，先點(diǎn)乘后叉乘AS

AR

=

AS

AR

BR

+

AR

B

S

BRA

業(yè)C

=A

業(yè)B

+R

B

業(yè)CBAS

=S

+

WCABACA在操作臂工作過(guò)程中基座靜止，所以一般將{0}作為世界坐標(biāo)系{U}對(duì)于連桿i

(其聯(lián)體坐標(biāo)系{i})

的速度，

有

=Ui

R

U

ViORG

=R

0

ViORGiOi

=Ui

ROi

=Ui

R

U

i

=R

0

i

=

i

1R

U

ViORG

=

i

R

0

ViORGi+1Oi

=

i

1ROi

=

i

1R

U

i

=

i

R

0

i顯然i+1oi

=

i+

R

ioii+1O

=

i+1R

iOi

i

ii10+1U+U+0+1U+0i0i5.3

機(jī)器人連桿間的速度傳遞

業(yè)C

=

業(yè)B

+

R

業(yè)C

BAA1aiaiBA1

轉(zhuǎn)動(dòng)機(jī)器操作臂是一個(gè)鏈?zhǔn)浇Y(jié)構(gòu)，每個(gè)連桿的運(yùn)動(dòng)都與它的相鄰桿有關(guān)，由于這種結(jié)構(gòu)的特點(diǎn)，我們可以依次計(jì)算各連桿的速度(線速度和角速度)當(dāng)關(guān)節(jié)i+1是旋轉(zhuǎn)關(guān)節(jié)時(shí)i

業(yè)

=

9

i

R

i+1Z八i+1i+1i+1

i+1i+1Z八i+1

=[001]T

是軸i+1在{i+1}中的表示9i+1

是旋轉(zhuǎn)關(guān)節(jié)i+1的關(guān)節(jié)轉(zhuǎn)速iO

=

i

ROi+10i+10i

0i+1i

i+1i+1Oi+1

=Oi

+R

i

業(yè)i+1

=Oi

+9i+1

R

i+

R

i+1Z八i+11ii0i0A對(duì)應(yīng){0}

B對(duì)應(yīng){i}

C對(duì)應(yīng){i+1}i+1Oi+1

=

i+

R

iOi

+9i+1

i+1Z八i+1i1=

i

RO

+

i

R9

0R

i

R

i+1Z八=iOi

+i+

R9i+1

i+1Z八i+11iiOiioii+1oi+15.3.1

轉(zhuǎn)動(dòng)型關(guān)節(jié)的速度傳遞對(duì)于旋轉(zhuǎn)關(guān)節(jié)i+1

，{0}是{A}

，{i}是{B}

，Q是{i+1}的原點(diǎn)B

Q

=i

Pi+1

是定常向量，因此

B

VQ

=0ioi+1

=Roi+1=

Roi

+

R

(Oi

R

i

Pi+1)

iOi

=ioi

+(ROi

)i

Pi+1=ioi

+iOi

i

Pi+10ii00i0i0i于是

0

Vi+1

=0

Vi

+0

i

R

i

Pi+1即

oi+1

=oi

+Oi

R

i

Pi+1i0i05.3

機(jī)器人連桿間的速度傳遞AVQ

=AVBORG

+R

B

VQ

+A

B

R

BQBABAi+1oi+1

=i+

R(ioi

+iOi

i

Pi+1)i1ioii+1oi+1i+1O

=

i+1R

iOi+1i

i5.3.2

平動(dòng)型關(guān)節(jié)的速度傳遞當(dāng)關(guān)節(jié)i+1是移動(dòng)關(guān)節(jié)時(shí)5.3

機(jī)器人連桿間的速度傳遞

ioi

+iOi

i

Pi+1)+di+1

i+1Z八i+1di+1

是移動(dòng)關(guān)節(jié)i+1的平移速度ioiiOi+1oi+1i向外迭代法若已知每個(gè)旋轉(zhuǎn)關(guān)節(jié)的9i

和9i

以及每個(gè)移動(dòng)關(guān)節(jié)的

di

和di

，從連桿0

的

0O0

=0，0o0

=0開始，依次應(yīng)用這些公式，

可以計(jì)算出最后一個(gè)連桿

的角速度NON

和線速度NoN

，進(jìn)一步，可得oN

=R

NoNN05.3

機(jī)器人連桿間的速度傳遞5.3.3

連桿速度計(jì)算的向外迭代法i+1oi+1

=i+

R(ioi

+iOi

i

Pi+1)+di+1

i+1Z八i+1i1i+1oi+1

=i+

R(ioi

+iOi

i

Pi+1)i1i+1Oi+1

=

i+

R

iOi

+9i+1

i+1Z八i+1i1i+1O

=

i+1R

iOi+1i

iN

N

N移動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)關(guān)節(jié)O

=

0R

NO5.3.3

連桿速度計(jì)算的向外迭代法例子:

一個(gè)具有兩個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)的操作臂.

計(jì)算操作臂末端的速度，將它表達(dá)

成關(guān)節(jié)速度的函數(shù)。給出兩種形式的解答，

一種是用坐標(biāo)系{3}來(lái)表示，另一種是用坐標(biāo)系{0}來(lái)表示「c1

?s1

00]|

|0T

=

|s1

c1

0

0

|1

|0

0

1

0

||||L000

1」||

|L000]|0

|1

|5.3

機(jī)器人連桿間的速度傳遞「c12

?

s12|R

=

R

21R

R

=

|s12

c1232103000l2

]||010||

0

0

1

」「1|2T

=

|

3

|0|

L0「c2

?s2|1T

|s2

c20l1

]||

1

0

||

01

」|2

=

|0

0|

0

010000」L05.3

機(jī)器人連桿間的速度傳遞5.3.3

連桿速度計(jì)算的向外迭代法「c2

s2|2o2

=R(1o1

+1O1

根1P2

)=|?s2

c2

|L0

012「0],1o1

=

01R(0o0

+0O0

根0P1)=0|L0」|s2

0]「0]「0

]c2

0

||0

|+92

2

2

=

|

0

|01」||L91」|

|L91

+92」|0]「0]「l1

]

「c2

s2|

|||

|

|0|{|0

|

根

|0

|}=

|

?s2

c2

1」|

|L91」||L0」||L0

0「c2R

1O1

+92

2

2

=?s2

|L

00]「0]

「l1s291

]|

|

|

|

|0

|

|l191

|

=

|l1c291

|1

||0

||

0

|1O1

=

01R

0O0

+911Z八1

=|0|L91」基坐標(biāo)系的速度為零:Frame

{1}

—{3}:i+1Oi+1

=

i+

R

iOi

+9i+1

i+1Z八i+1i1i+1oi+1

=i+

R(ioi

+iOi

根i

Pi+1)i1||||

|」L

」

L

」0O0

=0,0o0

=0「0]

||||轉(zhuǎn)動(dòng)關(guān)節(jié)5.3.3

連桿速度計(jì)算的向外迭代法「0]3O3

=

R

2O2

+93

3Z八3

=

2O2

=

0

|L91

+92」|「l1s291

]「0

]「l2

]

「

l1s291

]3D3

=

R(2D2

+

2O2

根

2P3

)

=

R(|l1c291

|+

|

0

|

根

|0

|)

=

|l1c291

+

l2

(91

+92

)

|

0

|L91

+92」||L0」|

0

「c12

?s12

0]「l1s291

]

「?l1s191

?l2s12

(91

+92

)]|||||

|0D3

=

|s12

c12

0

||l1c291

+

l2

(91

+92

)

|

=

|l1c191

+

l2c12

(91

+92

)

||L0

0

1」|

0

0

L|L|L|L|2323235.3

機(jī)器人連桿間的速度傳遞|

||||

|

|

|i+1Oi+1

=

i+

R

iOi

+9i+1

i+1Z八i+1i1i+1Di+1

=i+

R(iDi

+iOi

根i

Pi+1)i133330O

=

0R

3O

=

3O5.4.1

雅可比矩陣雅可比

Jacobian是多元函數(shù)的導(dǎo)數(shù)假設(shè)6個(gè)函數(shù)，每個(gè)函數(shù)都有6個(gè)獨(dú)立的變量：6y1

=

?f1

6x1

+

?f1

6x2

+

+

?f1

6x6

?x1

?x2

?x66y2

=

?f2

6x1

+

?f2

6x2

+

+

?f2

6x6

?x1

?x2

?x6y6

=f6

(x1

,x2

,x3

,x4

,x5

,x6

)可用偏微分表達(dá)計(jì)算

yi

的微分關(guān)于y1

=f1

(x1

,x2

,x3

,x4

,x5

,x6

)y2

=f2

(x1

,x2

,x3

,x4

,x5

,x6

)6y6

=

?f6

6x1

+

?f6

6x2

+

+

?f6

6x6

?x1

?x2

?x65.4

雅可比矩陣6Y

=6X

=J(X)6X?X

Y

=F(X)的微分的函數(shù)

:?Fxj

9.2

9.3

9.6

Jacobian矩陣「x.

]y.z.Oxy5偏導(dǎo)數(shù)矩陣J(X)稱作雅可比矩陣,是

xi

的函數(shù)。雅可比矩陣可看成是X中的速度向Y中速度的映射。J(X)

是一個(gè)時(shí)變的線性變換。5.4.1

雅可比矩陣

?F?X

雅可比Jacobian:Jacobian矩陣：

關(guān)節(jié)空間的微

分變化(速度

變化)與目標(biāo)

空間(速度變

化)之間的關(guān)

系「91

]

92

93

關(guān)節(jié)空間965|

|

|

|

||||

「x

]yz09L0」關(guān)節(jié)空間向目標(biāo)空間速度的傳動(dòng)比。5.4

雅可比矩陣6Y

=6X

=J(X)6XY

=J(X)X|

|

|

|

||||

LOz

」1「9.

]目標(biāo)空間運(yùn)動(dòng)學(xué)逆向正向O5.4.2

幾何雅可比矩陣注意機(jī)械臂末端相對(duì)基坐標(biāo)系的角速度

o=

()

并不是由基坐標(biāo)系下的末端姿態(tài)最小表示(如歐拉角)的求導(dǎo)得到的。幾何雅可比矩陣：操作臂的關(guān)節(jié)速度

與末端線速度和角速度

(v

o)T

之間的映射關(guān)系矩陣

J

()「v

]Lo」TTzoyoxo前述向外迭代法計(jì)算機(jī)械臂末端速度的算法本質(zhì)上是計(jì)算操作臂幾何雅可比矩陣的方法之一。5.4

雅可比矩陣|

|

=

J(

)

的向量積構(gòu)造法

B

AB

C

采用向量積法直接求出末端線速度和角速度，可以構(gòu)造幾何雅可比矩陣。BORG

A+BORGBORGBORGBORGVA=QVAAVQ

=

AVBORG

BQRBBARCB

RABA陣陣矩矩比比可可3

雅雅速度如下：若第i個(gè)關(guān)節(jié)為移動(dòng)關(guān)節(jié)，o

)

=

di

Z八iN若第i個(gè)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)o)

=9iZ八i

(PN

?Pi

)O(i)

=

9Z八N

i

iN(iN(i假設(shè)其他關(guān)節(jié)固定不動(dòng)，只有第i個(gè)關(guān)節(jié)運(yùn)動(dòng)，則由此運(yùn)動(dòng)產(chǎn)生的連桿N的線速度和角若機(jī)械臂末端執(zhí)行器固連在連桿N上，

轉(zhuǎn)軸單位向量

Z八i

已由正運(yùn)動(dòng)學(xué)求得。{A}={0}

，{B}={i}

，Q是{N}的原點(diǎn)而且每個(gè)連桿的坐標(biāo)系原點(diǎn)

Pi

和o

)

=oi

+R

i

VN

+Oi

R

i

Qi0i0N(iZ八N?1(2)(2)N

,

NO

)

=

Oi

+

R

i

Ni0N(iZ八1Z八NO(i)

=

0{0}={U}PN

?P2o

O2Z八Z八92PPPN231

5.4.3

雅可比矩陣的向量積構(gòu)造法

O

=

9

末端實(shí)際線速度和角速度就是各關(guān)節(jié)造成的線速度和角速度的總和)))))iiZ八Z八i)N(iN

NoN

=xo)，ON

=xON(i)

i=1

i=1以機(jī)械臂每個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)為例構(gòu)造雅可比矩陣定義笛卡爾速度向量

N

=

6和關(guān)節(jié)空間角速度向量LON

」N(i對(duì)于任意已知的操作臂位形，

關(guān)節(jié)速度和操作臂末端速度的關(guān)系是線性的，然而這種線性關(guān)系僅僅是瞬時(shí)的，因?yàn)樵谙乱豢?，雅可比矩?/p>

就會(huì)有微小的變化。雅可比矩陣是時(shí)變的。Z八N?1

根

(PN

?

PN

?1)

0

OZ八N?1

Z八N

」|向量積構(gòu)造法是計(jì)算幾何雅可比

矩陣的方法之一。則vN

=

|

1

N

1

2

N

2|L

Z八1

Z八2=J(O)O「9]|1

|O==N||L9N

」5.4

雅可比矩陣「Z八

根(P

?P)Z八

根(P

?P

)J(O)=6根N即為雅可比矩陣o)

=9iZ八i

根(PN

?Pi

)N(i「?l2s12

?l1s1

?l2s12

0]|

l2c12+

l1c1

l2c120

|「9

]「o3

]|000||

1

||Lo3」|

=

|

|L

111」「?l1s191

?l2s12

(91

+92

)]

「

0

]|

|

|

|o3

=

|l1c191

+

l2c12

(91

+92

)

|

o3=

|

0

|

所得結(jié)果與向外迭代法相同

0

|L91

+92」|L|93925.4

雅可比矩陣

J(O)

=

Z八1

根

(PN

?

P1)|L

Z八15.4.3

雅可比矩陣的向量積構(gòu)造法「0]由正運(yùn)動(dòng)學(xué)計(jì)算得

Z八1

=Z八2

=Z八3

=0|L1」|設(shè)

O=91

92

93

，6×3的雅可比表達(dá)為TT例子:

以兩連桿操作臂為例,

用雅可比求末端執(zhí)行器的速度「l2c12

+l1c1

]P3

=

|l2s12

+

l1s1

||L

0

」|Z八N?1

根(PN

?PN?1)Z八N?1Z八2

根(PN

?P2

)Z八「0]||P1

=

|0

||L0」|P2=

|l1s1|

|L

0

」|||「l1c1

]

||0

]|Z八N

」||

|25.4.4

參考坐標(biāo)系變換下的雅可比矩陣若關(guān)心{i}中的笛卡爾速度向量，則「ioN

]「R0]「oN

]

「R||=

||||

=

|0i0i可記變換后的雅可比為「

i

R

i

J()=|

0L

0即i

oN

=

i

J(

)

ii5.4

雅可比矩陣L

iON

」

L

0

R」LON

」L

00i0]|J()0]|J()LON

」R」0iR」0i

5.4.4

參考坐標(biāo)系變換下的雅可比矩陣由

|

3

3

|

=

|

0

3

|

|

3

|

和前一例子，有L

o3

」

L

0

0

R」Lo3」「c12

s12

0

?s12

c12

0「3o3

]|0

01L

3o3

」

|

0

0

0l2

+l1c20l2000001001001平面操作臂重視2維線速度且

93

三0，則2×2的雅可比表達(dá)為0]|0

|L

93

」||1」0

TT?l2s12l2

c1200015.4

雅可比矩陣「?l2s12

?l1s1l2

c12

+l1c1「3o

]「3R0

]「o]「ux

]「l1s2||=

|0

|「9

]

0

||1

|0

|

|92

|「9]

|1

||92

|

|L

93

」|0]「91

]|||l2

」L92

」「l1s2

00]|||||||L」Luy

」

Ll2

+

l1c2設(shè)

3o3

=

uxc?s120|

||

||

||

||

||

||

|00

1」|0

|L

0||=

|0]0

0sc0|

|

|

|

|

|

|000100000000001212uL12=y5.5.1

逆微分運(yùn)動(dòng)在機(jī)械臂關(guān)節(jié)角處于Θ時(shí)，利用此時(shí)的雅可比矩陣J(Θ)和關(guān)節(jié)角速度，可以計(jì)算末端執(zhí)行器的笛卡爾空間速度

vN

(包括線速度和角速度)，即vN

=J()

若已知末端執(zhí)行器笛卡爾空間速度vN

，產(chǎn)生

vN

的各關(guān)節(jié)角速度如下計(jì)算

=J?1()vN由于對(duì)于冗余機(jī)械臂和欠驅(qū)動(dòng)機(jī)械臂，雅可比矩陣不是方陣，需要考慮雅可比矩陣的偽逆(廣義逆)

。若矩陣A的維度為m

n

(m士n)，且A為滿秩，則A的偽逆(廣義逆)

A+為：m>n

時(shí)，

A

+為左逆矩陣，m<n

時(shí)，

A

+為右逆矩陣，5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性A+

=A

=(AT

A)

ATA+

=A

=AT

(AAT

)

?1?1?1?1?1?1?1?1t1lef?5.5.1

逆微分運(yùn)動(dòng)若A為m

n維矩陣，且A為滿秩，則線性方程組Ax=b的解(1)

m>n

時(shí)，方程組是過(guò)定的，通常方程組無(wú)解。此時(shí)，使得||

Ax-b

||2最小的x為方程的最小二乘解，由左偽逆計(jì)算x*

=A+

b

=A

b

=(AT

A)

AT

b(2)

m<n

時(shí)，方程組是欠定的，通常方程組可能存在無(wú)數(shù)個(gè)解。此時(shí)，所有解中使得x范數(shù)最小的x為方程的最小范數(shù)解，由右偽逆計(jì)算x*

=A+

b

=A

b

=AT

(AAT

)

b零空間(Null

Space)：若A為m

n維矩陣，

則A的零空間為線性方程組Ax=0的所有解集合，記為N(A)

N

(A)

=x

=n

:Ax

=0}(1)

m>n

時(shí)，若A為列滿秩，

A的零空間只有零向量(2)

m<n

時(shí)，若A為行滿秩，

A的零空間中的向量為x

=

(I

?

A+

A)x

=

(I

?

A

A)x

=

(I

?

AT

(AAT

)A)x其中，

x

為任意n維向量?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1t1lef?5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性5.5.1

逆微分運(yùn)動(dòng)在機(jī)械臂關(guān)節(jié)角處于Θ時(shí)，雅可比矩陣為J(Θ)，由末端執(zhí)行器的笛卡爾空

間速度

vN

求關(guān)節(jié)角速度的公式如下：(1)

無(wú)冗余：機(jī)械臂操作空間維度等于機(jī)械臂關(guān)節(jié)數(shù)，若雅可比矩陣滿秩，則

=J?1()vN(2)

冗余：機(jī)械臂操作空間維度小于機(jī)械臂關(guān)節(jié)數(shù)，對(duì)于末端執(zhí)行器的某一笛

卡爾空間速度

，通常會(huì)有無(wú)窮組對(duì)應(yīng)的關(guān)節(jié)速度，若雅可比矩陣是行滿秩

的，其中滿足關(guān)節(jié)速度范數(shù)最小的一個(gè)特解(最小范數(shù)解)，用右偽逆計(jì)

算

r

=

JT

(JJT

)

vN通解為

=

r

+

0

=

JT

(JJT

)vN

+

(I

?

JT

(JJT

)J

)0其中0遍歷所有的關(guān)節(jié)速度向量(3)

欠驅(qū)動(dòng)：機(jī)械臂操作空間維度大于機(jī)械臂關(guān)節(jié)數(shù)，對(duì)于末端執(zhí)行器的某一

笛卡爾空間速度

，可能沒有對(duì)應(yīng)的關(guān)節(jié)速度，這時(shí)，若雅可比矩陣是列滿

秩的，只能得到誤差范數(shù)最小的關(guān)節(jié)速度(最小二乘解)，用左偽逆計(jì)算

=(JT

J

)

JT

vN?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?15.5

逆微分運(yùn)動(dòng)學(xué)及奇異性5.5.2

奇異性對(duì)于6×6的J和某個(gè)Θ，若J(Θ)可逆，則對(duì)任何笛卡爾速度向量

vN

，由

=J?1()vN可以計(jì)算出產(chǎn)生

vN

的各關(guān)節(jié)轉(zhuǎn)速，這些轉(zhuǎn)速是產(chǎn)生vN

的唯一解大多數(shù)6×6的J都有使得其不可逆的Θ值，這些Θ值所對(duì)應(yīng)的位姿稱為機(jī)構(gòu)的奇異位形或簡(jiǎn)稱奇異狀態(tài)所有的操作臂在工作空間的邊界都存在奇異位形，并且大多數(shù)操作臂在它們的工作空間也有奇異位形5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性對(duì)于一般機(jī)械臂，

奇異位形為令雅可比矩陣J不滿秩的Θ值所構(gòu)成的位

形，此時(shí)

rank(J

(o))<min(m,n)。不同情況下的奇異點(diǎn)的判斷條件為(1)無(wú)冗余(

m=n

)：在此

Θ時(shí)J不可逆，即

det

(J(o))=0(2)冗余(

m<n

)：在此

Θ時(shí)J不行滿秩，即

rank

(J

(o))<

m(3)欠驅(qū)動(dòng)(

m>n

)：在此

Θ時(shí)J不列滿秩，即

rank

(J

(o))<n注意：對(duì)于平面機(jī)械臂，由于其末端姿態(tài)只有一個(gè)旋轉(zhuǎn)自由度，且旋轉(zhuǎn)軸一直垂直于平面，轉(zhuǎn)角大小為所有轉(zhuǎn)動(dòng)關(guān)節(jié)的轉(zhuǎn)角之和，所以判斷奇異性時(shí)，平面機(jī)械臂只需關(guān)心平面二維線速度部分的雅可比矩陣，即「vx

]v

v

q因此，對(duì)于平面機(jī)械臂，上述奇異位形的判斷條件需利用雅可比矩陣JvL|5.5.2

奇異性對(duì)于空間機(jī)械臂，總有

rank

(J

)共min

(6,n)5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性rank

(J

)共min

(2,n)這里n為機(jī)械臂關(guān)節(jié)數(shù)對(duì)于平面機(jī)械臂，總有5.5.2

奇異性奇異位形大致分為兩類:1)邊界奇異性：工作空間邊界的奇異位形。出現(xiàn)在操作臂完全展開或者收回使得末端執(zhí)

行器處于或非常接近空間邊界的情況2)內(nèi)點(diǎn)奇異性：工作空間內(nèi)部的奇異位形。出現(xiàn)在遠(yuǎn)離工作空間的邊界，通常是由于兩

個(gè)或兩個(gè)以上的關(guān)節(jié)軸線共線引起的當(dāng)操作臂處于奇異位形時(shí)，操作臂的末端在笛

卡爾空間中會(huì)失去一個(gè)或多個(gè)自由度，

即此時(shí)

無(wú)論選擇多大的關(guān)節(jié)速度，操作臂的末端在笛

卡爾空間的某個(gè)方向上(或某個(gè)子空間中)

都

不能運(yùn)動(dòng)。5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性5.5.2

奇異性例子:

對(duì)于兩自由度操作臂，末端執(zhí)行器沿著X八軸以1.0m/s的速度運(yùn)動(dòng)。當(dāng)操作臂遠(yuǎn)離奇異位形時(shí)，

關(guān)節(jié)速度都在允許范圍內(nèi)。但是當(dāng)

92

=

0

時(shí)，操作臂接近奇異位形，

此時(shí)關(guān)節(jié)速度趨向于無(wú)窮大首先計(jì)算坐標(biāo)系

{0}中雅可比矩陣的逆:0J

?1

2c12

l2s12

l1l2s2

L

?

l1c1

?

l2c12

?

l1s1

?

l2s12

」當(dāng)末端執(zhí)行器以

1m/s

的速度沿著

X八

方向運(yùn)動(dòng)時(shí)，按照操作臂位形的函數(shù)計(jì)算出關(guān)節(jié)速度:91

=

12

,

92

=

?

1

?

12l1s2

l2s2

l1s2當(dāng)操作臂伸展到接近

92

=

0

,兩個(gè)關(guān)節(jié)的速度趨向無(wú)窮大.5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性c

c

c

的可操作性問(wèn)題若機(jī)械臂處于某位形時(shí)關(guān)節(jié)向量為

Φ

，關(guān)節(jié)速度取為單位速度向量

Φe

，滿足

Φ

Φe

=1此時(shí)，機(jī)器人末端速度記為ve

，則滿足(1)機(jī)器人無(wú)冗余：

v

(J

(Φ

)J

(Φ

)T

)

ve

=1(2)機(jī)器人冗余：

v

(J

(Φ

)J

(Φ

))

ve

1當(dāng)機(jī)器人處于某位形時(shí)，限制關(guān)節(jié)速度為單位速度向量，機(jī)器人末端速度所構(gòu)成的空

間稱作該位形的可操作橢球體。TTTTTT?1?1?1TeT?1?1?1?1?1?1?1eTeT可操作度是衡量機(jī)器人位形與奇異位形距離的一種度量方式由于欠驅(qū)動(dòng)機(jī)器人的逆微分運(yùn)動(dòng)只有最小二乘解，一般只討論無(wú)冗余和冗余機(jī)器人Φe

=

Φr

+

Φ

=

JT

(JJT

)ve

+

(I

?

JT

(JJT

)J

)

Φf?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?15.5

逆微分運(yùn)動(dòng)學(xué)及奇異性5.5.3

可操作度Φe

=J

?1(Φ)ve5.5.3

可操作度假設(shè)機(jī)器人有N個(gè)關(guān)節(jié)，末端速度空間的維數(shù)為

m，要求

N

>m

，則

m

N

維雅可比矩陣

J

的奇異值分解為

J

=

UΣVT其中

Σ

是

m

N

維矩陣，

Σ

=diag(1

,2

,,m

)其主對(duì)角線外的元素均為零，主對(duì)角線上的每個(gè)元素為J

的奇異值

i

=入i

(JJT

)i

=1,,m

1

>2

>>m

>0U和V分別為m維和N維正交矩陣，且U由矩陣

JJT

的特征向量

ui

(

i

=1,,m

)張成，V由矩陣

JT

J

的特征向量

vi

(

i

=

1,,N

)張成。其中，

Σ?2

=diag

(

2

)，記

=

UT

ve

則v

(JJT

)ve

=α

ΣT

?2α

=

1上式是一個(gè)標(biāo)準(zhǔn)的橢球體方程，表明機(jī)器人此位形的可操作橢球體的軸由向量

i

ui

給出。?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1eTm?5.5

逆微分運(yùn)動(dòng)學(xué)及奇異性v

(J

(Φ

)J

(Φ

)T

)

ve

1?1?1?1?1?1?1?1eTv

(JJT

)ve

=(UT

ve

)Σ

?2

(UT

ve

)?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1?1T?1?1eT由此得到5.5.3

可操作度機(jī)器人關(guān)節(jié)速度取單位速度時(shí)：(1)可操作橢球體軸的長(zhǎng)度越大，在該軸方向上，所得到的末端速度越大，表明在