華科機(jī)器人原理及控制技術(shù)第4章-逆運(yùn)動(dòng)學(xué)方程

第四章 逆運(yùn)動(dòng)學(xué)方程Chapter

Ⅳ

Inverse

Kinematic

Equations引言逆運(yùn)動(dòng)學(xué)方程的解4.3

斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解歐拉變換的逆運(yùn)動(dòng)學(xué)解RPY變換的逆運(yùn)動(dòng)學(xué)解球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解本章小結(jié)4.1 引言

(Introduction)θn

dnan－連桿長(zhǎng)度;dn－相鄰兩連桿的距離;θnαn－連桿扭轉(zhuǎn)角;θn－相鄰兩連桿的夾角。

dn逆運(yùn)動(dòng)學(xué)方程解的步驟如下：（1）根據(jù)機(jī)械手關(guān)節(jié)坐標(biāo)設(shè)置確（2）根據(jù)任務(wù)確定機(jī)械手的位姿T6（3）由T6和An（n＝1，2，…，6）和式（4.1）求出相應(yīng)的關(guān)節(jié)變量θn或dn4.2

逆運(yùn)動(dòng)學(xué)方程的解（Solving

inverse

kinematic

equations）θn

dn4.3

斯坦福機(jī)械手的逆運(yùn)動(dòng)學(xué)解（

Inverse

solution

of

Stanford

manipulator）在第三章

推導(dǎo)出

Stanford

Manipulator

的運(yùn)動(dòng)方程和各關(guān)節(jié)齊次變換式。下面應(yīng)用式（4.2）～（4.6）進(jìn)行求解：ww2w.由式（4.21）、（4.22）和（4.18）可得到第一個(gè)關(guān)節(jié)變量θ1的值（4.23）

根據(jù)同樣的方法，利用式（4.9）和式（4.13）矩陣元素相等建立的相關(guān)的方程組，可得到其它各關(guān)節(jié)變量如下：22d2

x

r

d211

py

1

tanp

tanzp2

S1

py

tan1C

p1

x（4.24）d3

S2

C1px

S

p

C

p1

y（4.25）2

1

x

1

y

2

zC

C

a

S

a

S

a

S1ax

C1ay14

tan（4.26）S C

a2 1

x4

2

1

x1

y2

z

4

1

x1

y

S

a

C

a1

y

2

z1

C

C

C

a

S

a

S

a

S

S

a

C

a

5

tan（4.27）5

4

2 1

x1

y

2

z

4 1

x

1

y

5

2 1

x

1

y

2

z1

x1

y2

z

S

o

4C

S

o

C

o

S4

C2

C1ox

S1oy

1

C

C

C

Co

S

o

S

o

S

S

o

C

o

S

S

C

o

S

o

C

o

6

tan（4.28）注意：4.4

歐拉變換的逆運(yùn)動(dòng)學(xué)解（Inverse

solution

of

Euler

Angles

）這里需要 的是，在 采用式（4.43）~式（4.45）來(lái)計(jì)算θ、φ、Ψ時(shí)都是采用反余弦函數(shù)，而且式（4.43）和式（4.45）的分母為sinθ，這會(huì)帶來(lái)如下問(wèn)題：

1）由于絕對(duì)值相同的正負(fù)角度的余弦相等，如cosθ＝cos（-θ），因此不能確定反余弦的結(jié)果是在那個(gè)象限；2）當(dāng)sinθ接近于0時(shí)，由式（4.43）和式（4.45）所求出的角度φ和Ψ是不精確的；3）當(dāng)θ＝0或±180o時(shí)，式（4.43）和式（4.45）無(wú)數(shù)值解。為此，

須尋求更為合理的求解方法。由三角函數(shù)的知識(shí)

知道，反正切函數(shù)θ＝tan－1（x

/y）所在的象限空間可由自變量的分子和分母的符號(hào)確定（如圖4.1所示），因此如果

得到歐拉角的正切表達(dá)式，就不難確定歐拉角所在的象限。為此， 采用本章第二節(jié)的方法，用Rot

(

z,

?)－1左乘式（4.31）有Rot－1(z,?)

T

＝

Rot

(y,θ)

Rot

(z,

ψ)

（4.46）yx

y＋

＋x

y＋

－xx

y－

－x

y—

＋圖4.1

正切函數(shù)所在象限θ即（4.47）將上式寫成如下形式（4.48）式中（4.49）（4.50）（4.51）

1001

1

00

0cos

0

sin

sin

cos

00

0sin

cos

sin

sin

0

0

sin

cos

00

10

cosy

yyy

sin

cosoz

az

pz

0

00nzp

o

a0npx

cos

cosox

axsin

00nx

100010000

cos

0sin

sin

00sinsin

cos

sin

cosf13

(

p)

f13

(n)f12

(

p)f12

(a)f13

(a)

f

(n)

f

(o)12

12f13

(o)f11

(

p)

cos

cosf11

(a)

f11

(n)

f11

(o)f11

cos

x

sin

yf12

sin

x

cos

yf13

z同樣，上面三個(gè)式子中的x、y、z分別表示n、o、a、p矢量的各個(gè)分量，如f12

(a)

sin

ax

cos

ay（4.52）

sin

cosorald.c)nw

ww.4.5

RPY變換的逆運(yùn)動(dòng)學(xué)解（Inverse

solution

of

RPY）4.6

球坐標(biāo)變換的逆運(yùn)動(dòng)學(xué)解（Inverse

solution

of

Spherical

Coordinates

）第三章介紹的球坐標(biāo)變換的表達(dá)式如下T

=

Sph

(α,

β,

γ)=

Rot

(

z,

α)

Rot

(y,

β)

Trans(

0,

0,

γ)（4.84）用Rot－1(z,α)左乘上式得到Rot－1(

z,

α)

T

=Rot

(

y,

β)Trans

(

0,

0,

γ)（4.85）（4.86）（4.87）

由上式第2行元素相等有10

cos