第4章操作臂的雅可比

1第4章機(jī)器人的雅可比公式

4.1雅可比矩陣的定義

4.2機(jī)器人的微分運(yùn)動(dòng)和廣義速度

4.3雅可比矩陣的構(gòu)造

4.4機(jī)器人雅可比矩陣計(jì)算實(shí)例

4.5力雅可比

4.6奇異性和靈巧度

4.7剛度和變形

4.8誤差標(biāo)定和補(bǔ)償

4.9小結(jié)2

前面我們建立了操作臂的運(yùn)動(dòng)學(xué)方程，實(shí)際上是建立了機(jī)器人操作臂在操作與關(guān)節(jié)空間中的位移關(guān)系。通過求解運(yùn)動(dòng)方程的反解，建立了兩個(gè)空間的映射關(guān)系。本章在位移分析的基礎(chǔ)上，進(jìn)行速度分析，研究操作空間與關(guān)節(jié)空間的速度間的線性映射關(guān)系–––雅可比矩陣

雅可比(Jacobian)。雅可比不僅表示了兩個(gè)空間之間的速度線性映射關(guān)系，也表示兩空間的力的傳遞關(guān)系。為進(jìn)行速度分析，要利用到微分運(yùn)動(dòng)的概念。3

機(jī)器人的操作與控制，常涉及到機(jī)械手位姿的微小變化。這些變化可由描述機(jī)械手位姿的T

的微小變化來表示。在數(shù)學(xué)上，這種微小變化可用微分變化來表達(dá)。機(jī)械手運(yùn)動(dòng)過程中的微分關(guān)系是很重要的。如用攝像機(jī)觀察機(jī)械手的末端執(zhí)行裝置時(shí)，需把一個(gè)坐標(biāo)系的微分變化變換為對(duì)另一坐標(biāo)系的微分變化，如把攝像機(jī)的坐標(biāo)系建立在T6上。應(yīng)用微分變化的另一情況是當(dāng)已知對(duì)T6的微分變化時(shí)，要求出對(duì)各關(guān)節(jié)坐標(biāo)的相應(yīng)變化。微分關(guān)系對(duì)于研究機(jī)械手的動(dòng)力學(xué)問題，也是十分重要的。4§4.1雅可比矩陣的定義操作臂的雅可比矩陣定義為其操作速度與關(guān)節(jié)速度的線性變換，可看成是從關(guān)節(jié)空間向操作空間運(yùn)動(dòng)速度的傳動(dòng)比。操作臂的運(yùn)動(dòng)方程X＝X(q)(4.1)代表操作空間x與關(guān)節(jié)空間

q

之間的位移關(guān)系。將式(4.1)兩邊對(duì)時(shí)間

t

求導(dǎo)，即得出

q

與

X

之間的微分關(guān)系

X＝J(q)q(4.2)式中，X

稱為末端在操作空間的廣義速度，簡(jiǎn)稱操作速度，q

為關(guān)節(jié)速度；J(q)是6×n的偏導(dǎo)數(shù)矩陣操作臂的雅可比矩陣。它的第

i

行第

j

列元素為5Jij(q)＝Xi(q)/qj，

i＝1,2,…,6；j＝1,2,…,n(4.3)式(4.2)表示，對(duì)給定的qRn，雅可比J(q)是從關(guān)節(jié)空間速度q

向操作空間速度x

映射的線性交換。6

例4.1

對(duì)圖示平面2R機(jī)械手，其運(yùn)動(dòng)學(xué)方程如下：

x

＝l1c1＋l2c12；y

＝l1s1＋l2s12

讓運(yùn)動(dòng)學(xué)方程兩端分別對(duì)時(shí)間t求導(dǎo)，

x＝–l1s11

–l2s12(1＋2)

y＝l1c11

＋l2c12(1＋2)寫成矩陣的形式：

X＝J(q)q式中，X＝{xy}，q＝{1

2}，

J(q)＝(4.4)式(4.4)中的J(q)是2×2的方陣。7當(dāng)2＝0(或180)時(shí)。J(q)＝0，矩陣的秩為1，處于奇異狀態(tài)。從幾何上看，機(jī)械手完全伸直(2＝0)，或完全縮回(2＝180)時(shí)，機(jī)械手末端喪失了徑向自由度，僅能沿切向運(yùn)動(dòng)。在奇異形位時(shí)，機(jī)械手在操作空間的自由度將減少。

對(duì)關(guān)節(jié)空間的某些形位q，J(q)的秩減少，這些形位稱為操作臂的奇異形位?？捎肑(q)判別奇異形位：J(q)＝l1l2s28滿秩的，關(guān)節(jié)速度就可解出q＝J–1(q)X

對(duì)平面2R機(jī)械手，J–1(q)可由式(4.4)求得J–1(q)＝(4.5)

例4.2

如圖4–1，為實(shí)現(xiàn)平面2R機(jī)械手末端沿x0軸以1m/s的速度運(yùn)動(dòng)，求相應(yīng)的關(guān)節(jié)速度q＝{1,2}。由式(4.2)可見，只要J(q)是圖4–1平面2R機(jī)械手的速度反解

9∴得到對(duì)應(yīng)于末端速度X＝{1,0}的關(guān)節(jié)速度反解為1＝c12/l1s2；2＝–c1/l2s2－c12/l2s2

當(dāng)20(或180)時(shí)。機(jī)械手奇異形位，相應(yīng)的q。當(dāng)操作臂有較多自由度時(shí)，用上述定義計(jì)算雅可比較為復(fù)雜，但有兩種構(gòu)造性方法(矢量積法和微分變換法)相對(duì)較為簡(jiǎn)單。下面介紹利用操作空間與關(guān)節(jié)空間的微分運(yùn)動(dòng)關(guān)系構(gòu)造雅可比矩陣的微分變換法和矢量叉積構(gòu)造法。10

4.2機(jī)器人的微分運(yùn)動(dòng)和廣義速度若已知一個(gè)變換的元素是某個(gè)變量的函數(shù)，那么對(duì)這個(gè)變量的微分變換就是其元素為原變換元素的導(dǎo)數(shù)。本節(jié)推導(dǎo)出一種方法，使得對(duì){T}的微分變換等價(jià)于對(duì)基系的變換。此方法可推廣至任何兩個(gè)坐標(biāo)系，使它們的微分運(yùn)動(dòng)能聯(lián)系起來。機(jī)械手的變換包括平移變換、旋轉(zhuǎn)變換、比例變換和投影變換等。這里的討論限于平移和旋轉(zhuǎn)變換。∴可把其導(dǎo)數(shù)項(xiàng)表示為微分平移和微分旋轉(zhuǎn)。111.微分平移和微分旋轉(zhuǎn)（補(bǔ)充內(nèi)容）用給定的坐標(biāo)系(也可用基系)來表示微分平移和旋轉(zhuǎn)。已知{T}，可表示T＋dT為(相對(duì)基系)T＋dT＝Trans(dx,dy,dz)·Rot(f,d)·T式中，Trans(dx,dy,dz)是基系中微分平移變換；Rot(

f,d)是基系中繞矢量

f

的微分旋轉(zhuǎn)變換。由上式得dT為dT＝[Trans(dx,dy,dz)·Rot(f,d)－I]·T(B.1)同樣，可給出對(duì){T}的微分平移和旋轉(zhuǎn)的微分變換T＋dT＝T·Trans(Tdx,Tdy,Tdz)·Rot(

f,dT)式中，Trans(Tdx,

Tdy,

Tdz)為對(duì){T}的微分平移變換；Rot(

f,dT)是繞{T}中矢量

f

的微分旋轉(zhuǎn)。這時(shí)有12dT＝T·[Trans(Tdx,Tdy,Tdz)·Rot(

f,dT)

－I](B.2)式中(B.1)和(B.2)都有相似項(xiàng)Trans(dx,

dy,

dz)·Rot(

f,

d)－I。當(dāng)微分運(yùn)動(dòng)是相對(duì)基系時(shí)，記為；當(dāng)運(yùn)動(dòng)是相對(duì){T}時(shí)，記為T。即，當(dāng)對(duì)基系有微分變化時(shí)，dT＝·T；而當(dāng)對(duì){T}有微分變化時(shí)，dT＝T·T。表示微分平移和{T}的齊次變換分別為

Trans(dx,dy,dz)＝T＝(B.3)

13上式中Trans的變量是由微分變化dx

i＋dyj＋dz

k表示的微分矢量d。利用前面的通用旋轉(zhuǎn)變換式(參考式(2.45))：

Rot(f,)＝

對(duì)微分變化d，其相應(yīng)的正、余弦函數(shù)和正交函數(shù)為代入到上式，可把微分旋轉(zhuǎn)齊次變換表示為Rot(f,d)＝(B.4)14將式(B.3)和(B.4)代入＝Trans(dx,dy,dz)·Rot(

f,d)－I，可得＝化簡(jiǎn)得

＝(B.5)繞矢量

f

的微分旋轉(zhuǎn)d等價(jià)于分別繞三個(gè)軸x、y和z的微分旋轉(zhuǎn)x,y和z，即

fxd＝x，fyd＝y(tǒng)，fzd＝z。代入上式得15＝(B.6)類似，可得T為T＝(B.7)

于是，可把看成是由微分平移矢量d

和微分旋轉(zhuǎn)矢量

構(gòu)成的，且有：d＝dxi＋dyj＋dzk(4.6)＝xi＋yj＋zk(4.7)四.微分旋轉(zhuǎn)的無序性當(dāng)θ→0時(shí)，有sinθ→dθ，cosθ→1．若令δx=dθx，δy=dθy，δz=dθz，則繞三個(gè)坐標(biāo)軸的微分旋轉(zhuǎn)矩陣分別為略去高階無窮小量?jī)烧呓Y(jié)果相同，可見這里左乘與右乘等效。同理可得結(jié)論：

微分旋轉(zhuǎn)其結(jié)果與轉(zhuǎn)動(dòng)次序無關(guān)，這是與有限轉(zhuǎn)動(dòng)（一般旋轉(zhuǎn)）的一個(gè)重要區(qū)別。若Rot（δx，δy，δz）和Rot（δx‘，δy’，δz‘）表示兩個(gè)不同的微分旋轉(zhuǎn)，則兩次連續(xù)轉(zhuǎn)動(dòng)的結(jié)果為：上式表明：任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的代數(shù)和，即微分旋轉(zhuǎn)是可加的。kxdθ=δx，

kydθ=δy

，

kzdθ=δz所以有由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與等效，有即20用列矢量D來包含上述兩矢量，并稱為剛體或坐標(biāo)系的微分運(yùn)動(dòng)矢量D＝{dx

dy

dz

x

y

z}或D＝{d

}

(4.8)同理，相對(duì){T}有下列各式

Td＝Tdxi＋Tdyj＋Tdzk

T

＝Txi＋Tyj＋Tzk(B.8)TD＝{Tdx

Tdy

Tdz

Tx

Ty

Tz}或TD＝{Td

T

}(B.9)21

例4.3

已知{A}和對(duì)基系的微分平移與微分旋轉(zhuǎn)為

A＝，d＝1

i＋0

j＋0.5k，＝0

i＋0.1

j＋0k試求微分變換dA。解：首先據(jù)式(B.6)可得下式

＝在按照dT＝·T，有：dA＝·A，即

dA＝{A}的這一微分變化如圖B–1。圖B–122

2．微分運(yùn)動(dòng)的等價(jià)變換要求機(jī)械手的雅可比矩陣，需要把一個(gè)坐標(biāo)系內(nèi)的位姿的微小變化，變換為另一坐標(biāo)系內(nèi)的等效表達(dá)式。∵dT＝·T和dT＝T·T，當(dāng)兩坐標(biāo)系等價(jià)時(shí)，·T＝T·T，變換后得T

–1·T＝T(B.10)由式(B.3)和(B.6)有·T＝＝

(B.11)23(B.11)式與下式等價(jià)

·T＝用T

–1左乘上式得

T

–1·T＝＝24＝應(yīng)用三矢量相乘的兩個(gè)性質(zhì)a·(b×c)＝b·(c×a)及a·(a×c)＝0，由式(B.10)可把上式寫為

T＝化簡(jiǎn)得T＝(B.12)25

∵T已被式(B.7)所定義，∴令式(B.7)與(B.12)各元分別相等，并利用三矢量相乘的性質(zhì)：a·(b×c)＝c·(a×b)等，可求得下列各式Tdx＝·(p×n)+d·n＝n·[(×p)+d]Tdy＝·(p×o)+d·o＝o·[(×p)+d]

(4.11)Tdz＝·(p×a)+d·a＝a·[(×p)＋d]Tx＝·n＝n·

；Ty＝·o＝o·

Tz＝·a＝a·(4.12)式中，n,o,a和p分別為微分坐標(biāo)變換

T

的列矢量。用上兩式，能夠十分方便地把對(duì)基系的微分變化變換為對(duì){T}的微分變化。從上列兩式可得微分運(yùn)動(dòng)TD和D的關(guān)系如下：26(4.13)上式可簡(jiǎn)寫為

(4.14)式中，R是旋轉(zhuǎn)矩陣，R＝(4.15)

對(duì)任何3維矢量

p＝[px,py,pz]T，其反對(duì)稱矩陣S(p)定義為27S(p)＝(4.16)它具有以下性質(zhì)(1)S(p)＝p×，S(p)＝p×；(2)T·S(p)＝–(p×)，

T·S(p)＝–(p×)(3)–RT·S(p)＝28

若定義剛體或坐標(biāo)系的廣義速度是由線速度和角速度組成的

6

維列矢量，即：V＝{v

}＝{d

}(4.9)則由式(4.14)可得相應(yīng)的廣義速度V的坐標(biāo)變換為(4.17)任意兩坐標(biāo)系間的廣義速度的坐標(biāo)變換為(4.18)29

例4.4

已知{A}對(duì)基系的微分平移d

和旋轉(zhuǎn)

，同例4.3。試求對(duì){A}的等價(jià)微分平移和微分旋轉(zhuǎn)。解：因?yàn)閚＝0

i＋1j＋0

k

；o＝0

i＋0

j＋1

ka＝1

i＋0

j＋0

k

；p＝10

i＋5j＋0

k以及

×p＝＝0

i＋0

j–1

k加上d后有：×p＋d＝1

i＋0

j–0.5

k。又據(jù)式(4.11)和(4.12)可求得等價(jià)微分平移和微分旋轉(zhuǎn)為Ad＝0

i–0.5

j＋1

k

A＝0.1

i＋0

j＋0

k

30由式dT＝T·T，計(jì)算dA＝A·A計(jì)算，以檢驗(yàn)所得微分運(yùn)動(dòng)是否正確。據(jù)式(B.7)有

A＝dA＝＝所得結(jié)果與例4.3一致。可見所求得的對(duì)A的微分平移和微分旋轉(zhuǎn)是正確無誤的。31

3.變換式中的微分關(guān)系式(4.11)～(4.13)可用于變換任何兩坐標(biāo)系間的微分運(yùn)動(dòng)。由式(4.11)和(4.12)，可根據(jù)坐標(biāo)變換T

和微分變換求T。若要從T的各微分矢量來求微分矢量，可從式(B.10)左乘T和右乘T–1，得：

＝T

·T·T–1或者變換為＝(T–1)–1·T·T–1(B.13)

設(shè){A}和{B}，{B}相對(duì)于{A}定義。則{A}、{B}都可以用來表示微分運(yùn)動(dòng)。左圖表示了這一情況。32＝A·B

·B·B–1·A–1或變換為

＝(B–1·A–1)–1·

B·(B–1·A–1)(B.14)此式表明了{(lán)B}內(nèi)與基系內(nèi)的微分運(yùn)動(dòng)間的關(guān)系，它具有式(B.13)的一般形式；而(B–1·A–1)則對(duì)應(yīng)式(B.10)和(B.11)中的T。

圖B–2表明了{(lán)A}、{B}間的微分關(guān)系，且有·A·B＝A·B·B對(duì)求解得33

同樣，由圖B–2還可得：A·A·B＝A·B·B或

A·B＝B·B對(duì)A求解得A＝B·B·B–1

A＝(B–1)–1·B·B–1(B.15)此式表示{A}內(nèi)與{B}內(nèi)的微分運(yùn)動(dòng)的關(guān)系。其中，B–1對(duì)應(yīng)于式(B.10)中的T，但T已不是坐標(biāo)系矩陣，而是微分坐標(biāo)變換矩陣。它可從圖B–2直接求得，即從已知的微分變化變換圖箭頭起，回溯到待求的等價(jià)微分變化止，所經(jīng)過的路徑。34

對(duì)上述第一種情況，從B之箭頭至之箭頭間所經(jīng)路徑即為B–1·A–1；而對(duì)于第二種情況，從B至A，所經(jīng)路徑為B–1。35

例4.5

有一攝像機(jī)裝在機(jī)械手的連桿5上。連接由下式確定：

C＝機(jī)械手的連桿6的位置描述如下

A6＝被觀察的目標(biāo)物體為CO。要把機(jī)械手的末端引向目標(biāo)物體，需要知道的坐標(biāo)系{C}內(nèi)的微分變化為：Cd＝–1

i＋0

j＋0k，C＝0

i＋0

j＋0.1

k試求在坐標(biāo)系{T6}內(nèi)所需要的微分變化。36解：上述情況可由圖B–3(a)及下列方程描述T5·A6·E·X＝T5·C·O式中，T5描述連桿5與基系的關(guān)系；A6是以{5}描述連桿6；E為描述物體對(duì)末端的未知變換；O是用攝像機(jī)坐標(biāo)系描述物體。變換圖見圖B–3(b)。T5CT＝T6＝T5A6圖B–3例4.5的位姿及微分變換圖37

從圖可得相對(duì)T6至C的微分坐標(biāo)變換T為：

T＝C–1T5–1·T5A6＝C–1A6∵

C–1＝∴可得此微分坐標(biāo)變換

T＝又因?yàn)椤羛＝＝0

i＋0.2

j＋0

k最后據(jù)式(4.11)和(4.12)可得坐標(biāo)系T6內(nèi)的微分變化如下T6d＝–0.2

i＋0

j＋1k，T6＝0

i＋0.1

j＋0

k38

4.3機(jī)器人的雅可比矩陣的構(gòu)造法上面分析了機(jī)械手的微分運(yùn)動(dòng)。下面將研究機(jī)器人操作空間速度與關(guān)節(jié)空間速度之間的線性映射關(guān)系，即雅可比矩陣。

1．J(q)矩陣的分解剛體或坐標(biāo)系的廣義速度

X

是由線速度

v

和角速度

組成的6維列矢量V＝

X

＝{v|}＝{d|}(4.9)J(q)陣既是從關(guān)節(jié)→操作空間的速度傳遞的線性關(guān)系，又是微分運(yùn)動(dòng)轉(zhuǎn)換的線性關(guān)系，由式(4.9)和式(4.2)有：39V

＝J(q)q

(4.19)D

＝s13dki2＝X·t＝J(q)q·t即D＝J(q)·dq(4.20)對(duì)n關(guān)節(jié)的機(jī)器人，J(q)＝J(q)6×n，每列代表相應(yīng)的關(guān)節(jié)速度

qi對(duì)手爪線速度和角速度的變換。J(q)可分塊為{v|}＝{q1

q2…

qn}(4.21)由上式可把v

和

表示成

qi的線性函數(shù)v＝Jl1q1＋Jl2q2＋…＋Jlnqn＝Ja1q1＋Ja2q2＋…＋Janqn

(4.22)式中，Jli和Jai分別表示關(guān)節(jié)

i

的單位關(guān)節(jié)速度引起手爪的線速度和角速度。40

2．J(q)矩陣的求法式(4.8)、(B.9)、(4.13)、(4.14)和(4.20)等是計(jì)算J(q)的基本公式，但這些公式計(jì)算J(q)的過程較復(fù)雜。下面介紹兩種直接構(gòu)造J(q)的方法。

(1)矢量積法：求機(jī)器人J(q)的矢量積法是建立在運(yùn)動(dòng)坐標(biāo)系的概念上的。圖4–2是關(guān)節(jié)速度的傳遞情況，末端手爪{T}的v和

與qi有關(guān)。圖4–2

關(guān)節(jié)速度的傳遞41

對(duì)移動(dòng)關(guān)節(jié)i，有

{v}＝{zi0}qi；

Ji＝{zi0}(4.23)

對(duì)轉(zhuǎn)動(dòng)關(guān)節(jié)i，有

{v}＝{zi×ipn0

zi}qi

；

Ji＝{zi×ipn0

zi}＝{zi×(i0R·ipn)zi}(4.24)式中，ipn0表示{T}的原點(diǎn)相對(duì){i}的位矢在{0}中的表示，即ipn0＝i0R·ipn(4.25)而zi是{i}的z軸單位矢量在{0}中的表示。42

(2)微分變換法(前面已介紹過推導(dǎo)方法)

①旋轉(zhuǎn)關(guān)節(jié)i，連桿

i

相對(duì)

i–1繞{

i

}的

zi軸作微分轉(zhuǎn)動(dòng)di

，其微分運(yùn)動(dòng)矢量為d

＝{000}di

，＝{001}di

(*)由式(4.13)可得手爪系{T}相應(yīng)的微分運(yùn)動(dòng)矢量為

{Tdx

Tdy

Tdz

Tx

Ty

Tz}＝{(p×n)z(p×o)z(p×a)z

nz

oz

az}di(4.26)

∴可得J(q)的第

i

列如下：TJli＝{(p×n)z(p×o)z(p×a)z}TJai＝{nz

oz

az}

(4.28a)43

②移動(dòng)關(guān)節(jié)i，連桿i

相對(duì)i–1沿zi軸作微分移動(dòng)ddi，其微分運(yùn)動(dòng)矢量為d

＝{001}ddi

，＝{000}di

(**)

同樣，由式(4.13)可得手爪的微分運(yùn)動(dòng)矢量為

{Tdx

Tdy

Tdz

Tx

Ty

Tz}＝{nz

oz

az

000}ddi

(4.27)則

TJli＝{nz

oz

az}；TJai＝{000}

(4.28b)式中，n,o,a和

p

是

iTT的4個(gè)列矢量。上述求TJq的方法是構(gòu)造性的，只要知道i–1Ti，就可自動(dòng)生成J(q)

，而不需求解方程等手續(xù)。步驟如下：44

①

計(jì)算i–1Ti

：0T1，1T2，…，n–1Tn(,nTT)。

②

計(jì)算iTn

(或iTT)(見圖4–3)：

n–1Tn，n–2Tn＝n–2Tn–1n–1Tn，…，i–1Tn＝i–1TiiTn，…，

0Tn＝0T11Tn(，0TT＝0T11TT)

③

計(jì)算TJ(q)的各列元素，第

i

列TJi由iTn決定。由式(4.28)計(jì)算TJli和TJai。從上述兩種構(gòu)造法得出的J(q)和JT(q)間存在有關(guān)系：圖4–3

Tji和iTn之間的關(guān)系JT(q)＝J(q)或

J(q)＝JT(q)(4.29)0231方法一：微分變換法方法一：矢量積的方法對(duì)轉(zhuǎn)動(dòng)關(guān)節(jié)i，有

{v}＝{zi×ipn0

zi}qi

；

Ji＝{zi×ipn0

zi}＝{zi×(i0R·ipn)zi}(4.24)式中，ipn0表示{T}的原點(diǎn)相對(duì){i}的位矢在{0}中的表示，即ipn0＝i0R·ipn(4.25)而zi是{i}的z軸單位矢量在{0}中的表示。i=15354i=2554.4

機(jī)器人雅可比矩陣計(jì)算實(shí)例下面舉例說明計(jì)算具體機(jī)器人微分運(yùn)動(dòng)和雅可比矩陣的方法。先計(jì)算PUMA560機(jī)器人的Jacobian矩陣，再計(jì)算西博特奇(Cybotech)V–80機(jī)器人的Jacobian矩陣。1.PUMA560機(jī)器人的雅可比矩陣

PUMA560有6個(gè)旋轉(zhuǎn)關(guān)節(jié)，其雅可比矩陣有6列。由式(4.28)可計(jì)算各列元素。現(xiàn)分別用兩種方法計(jì)算。56

(1)微分變換法求TJ(q)

TJ(q)的第1列TJ1(q)對(duì)應(yīng)著1T6，式(3.63)給出了1T6的各元素，由式(4.26)得

TJ1(q)＝{TJ1x

TJ1y

TJ1z

–s23(c4c5c6–s4s6)–c23s5c6

s23(c4c5c6＋s4c6)＋c23s5s6

s23c4s5–c23s5}式中，TJ1x＝–d2[c23(c4c5c6–s4s6)–s23s5c6]–(a2c2＋a3c23–d4s23)(s4c5c6＋c4s6)TJ1y＝–d2[–c23(c4c5c6＋s4c6)＋s23s5s6]

＋(a2c2＋a3c23–d4s23)(s4c5s6–c4c6)

TJ1z＝d2(c23c4c5–s23c5)＋(a2c2＋a3c23–d4s23)s4s5教材中是式(3.14)57同理，利用變換矩陣2T6得出TJ(q)的第2列

TJ2(q)＝{TJ2x

TJ2y

TJ2z

–s4c5c6–c4s6

s4c5s6–c4c6

s4s5}式中，TJ2x＝a3s5c6–d4(c4c5c6–s4s6)

＋a2[s3(c4c5c6–s4s6)＋c3s5c6]TJ2y＝–a3s5s6–d4(–c4c5c6–s4c6)

＋a2[s3(–c4c5c6–s4s6)＋c3s5s6]

TJ2z＝a3c6＋d4c4s5＋a2(–s3c4s5＋c3c6)同樣可得，

TJ3(q)＝{–d4(c4c5c6–s4s6)＋a3s5c6–d4(c4c5c6＋s4c6)–a3s5s6d4c4s5＋a3c6

–s4c5c6–c4s6

s4c5s6–c4c6

s4s5}

TJ4(q)＝{0

0

0

s5c6

–s5c6

c5}；

TJ5(q)＝{0

0

0

–s6–

c6

0}；

TJ6(q)＝{0

0

0

0

0

1}58

(2)矢量積法求J(q)

對(duì)都是轉(zhuǎn)動(dòng)關(guān)節(jié)的PUMA560，其J(q)矩陣為

J(q)＝由圖3–7及各連桿的0T1,1T2,,5T6，可計(jì)算出各中間項(xiàng)，再求J(q)的各列，即J1(q),J2(q),,J6(q)J(q)。利用第3章的公式，可計(jì)算上式中各量，過程如下：

①

10R，21R，32R，43R，54R和65R；

②

z1，z2，z3，z4，z5和z6；

③

1p6，2p6，3p6，4p6，5p6和6p6；用第3章的公式

④

1p60，2p60，3p60，4p60，5p60和6p60；用式(4.25)

⑤

J(q)中的各列：J1(q),J2(q),,J6(q)。用式(4.24)

教材中是p40的式(3.09)59

2．V–80機(jī)器人的雅可比矩陣圖B–4是法國(guó)西博特奇公司生產(chǎn)的V–80工業(yè)機(jī)器人的外形圖和停止位置。在停止位置：懸臂與基系的x軸平行，末端夾手垂直向上。連桿和關(guān)節(jié)參數(shù)見表4–1，其6個(gè)關(guān)節(jié)都是旋轉(zhuǎn)運(yùn)動(dòng)的。

在建立V–80操作機(jī)器人的雅可比矩陣時(shí)，應(yīng)用了圖4–3的變換圖。圖B–4

V–80機(jī)械手外形圖及停止位置6061

J(q)陣的第6列把關(guān)節(jié)6的運(yùn)動(dòng)變換為T6坐標(biāo)，而關(guān)節(jié)6也作用于{5}。通過跟蹤變換圖，從{5}至T6坐標(biāo)，可得微分坐標(biāo)變換矩陣5T6。據(jù)(*)式可得

T6d6＝0

i＋0

j＋0k，T66＝0

i＋0

j＋1

k這兩矢量構(gòu)成了J(q)陣的第6列。對(duì)第5列，其微分變換為5T6。將其元素代入(*)式，得T6d5＝0

i＋0

j＋0k，T65＝s6

i＋c6

j＋0

k

對(duì)第4列，采用矩陣4T6。則有

T6d4＝0

i＋0

j＋0k，T64＝–s5c6

i＋s5s6

j＋c5k62

對(duì)V–80機(jī)械手J(q)陣的第3列，其微分坐標(biāo)變換矩陣為3T6。∵是旋轉(zhuǎn)關(guān)節(jié)，∴用(*)式求本列各元素

T6d3x＝–nxpy＋nypx＝–[c3(c4c5c6–s4s6)–s3s5c6](a3s3–d4c3)

＋[s3(c4c5c6–s4s6)＋c3s5c6](a3c3＋d4s3)

T6d3y＝–oxpy＋oypx＝–[–c3(c4c5s6＋s4c6)＋s3s5s6](a3s3–d4c3)

＋[–s3(c4c5s6＋s4c6)–c3s5s6](a3c3＋d4s3)

T6d3z＝–axpy＋aypx

＝–(c3c4c5＋s3c5)(a3s3–d4c3)＋(s3c4c5–c3c5)(a3c3＋d4s3)化簡(jiǎn)得：

T6d3x＝d4(c4c5c6–s4s6)＋a3s5c6

T6d3y＝–d4(c4c5c6＋s4c6)–a3s5s6

T6d3z＝d4c4s5–a3c563

第3列后三個(gè)元素為：T63x＝nz＝s4c5c6＋c4s6

T63y＝oz＝–s4c5c6＋c4c6T63z＝az＝s4s6

用同樣方法可以得到本機(jī)械手J(q)陣的第2列和第1列，結(jié)果如下：

T6/2＝{(a2s3＋d4)(c4c5c6–s4s6)＋(a2c3＋a3)s5c6┇

–(a2s3＋d4)(c4c5c6＋s4c6)–(a2c3＋a3)s5s6

┇

d4c4s5–a2c3c6＋a2s3c4s5–

a3c5┇

s4c5c6＋c4s6┇–s4c5s6＋c4c6┇s4s5}64

T6/1＝{(s4c5c6＋c4s6)(a2c2＋d4s23)┇–(s4c5c6–c4s6)(a2c2＋d4s23)┇s4s5(a2c2＋d4s23)

┇–s23(c4c5c6–s4s6)＋c23s5s6

┇s23(c4c5c6＋s4s6)–c23s5s6┇–s23c4c5＋c23c5}

于是用{6}所表示的V–80機(jī)械手的TJ(q)如下65§4.5力雅可比機(jī)器人與外界環(huán)境相互作用時(shí)，在接觸處要產(chǎn)生力

f

和力矩n，統(tǒng)稱為末端廣義(操作)力矢量。記為F＝{

f

n}(4.30)如：手臂提取重物時(shí)承受的外力和力矩；手爪抓物體的作用力和力矩；步行機(jī)構(gòu)與地面的作用力和力矩。在靜止?fàn)顟B(tài)，F(xiàn)應(yīng)與各關(guān)節(jié)的驅(qū)動(dòng)力(或力矩)相平衡。n個(gè)關(guān)節(jié)的驅(qū)動(dòng)力(或力矩)組成的n維矢量

＝[1,2,…,n]T(4.31)稱為關(guān)節(jié)力矢量。令各關(guān)節(jié)的虛位移為qi，末端執(zhí)行器相應(yīng)的虛位移為D。用虛功原理就可導(dǎo)出

～F

的關(guān)系。66

各關(guān)節(jié)所作虛功的總和W1＝Tq＝1niqi應(yīng)與末端執(zhí)行器所作虛功的W2＝F*TD＝f

Td＋nT

相等(總虛功為零)，即

T·q＝FT·D

(4.32)

將式(4.20)代入上式(4.32)可得出

＝JT(q)F

(4.33)式中，JT(q)稱為操作臂的力雅可比，表示在靜平衡時(shí)，F(xiàn)向

映射的線性關(guān)系。可以看出：力雅可比＝運(yùn)動(dòng)雅可比的轉(zhuǎn)置，即操作臂的靜力傳遞關(guān)系與速度有關(guān)。式(4.19)和式(4.33)具有對(duì)偶性。圖4–4畫出了關(guān)節(jié)與操作空間的速度及靜力映射的線性關(guān)系。

DT＝qT·JT(q)

qT·

＝DT·FqT·

＝qT·JT(q)·F67

圖中，n是關(guān)節(jié)數(shù)，m是操作空間維數(shù)。J(q)＝J(q)m×n。對(duì)給定的q，J(q)的值域空間R(J(q))代表關(guān)節(jié)運(yùn)動(dòng)能夠產(chǎn)生的全部操作速度的集合。當(dāng)J(q)退化時(shí)，操作臂處于奇異形位。圖4–4速度和靜力的線性映射

另一方面J(q)的零空間N(J(q))表示不產(chǎn)生操作速度的關(guān)節(jié)速度的集合。若N(J(q))不只含有0，則對(duì)給定的操作速度，關(guān)節(jié)速度的反解有無限多。

68

靜力映射是從m維操作空間向n維關(guān)節(jié)空間的映射，其零空間N(JT(q))代表不需要任何關(guān)節(jié)驅(qū)動(dòng)力(矩)，就能承受的所有操作力的集合。即：末端操作力完全由機(jī)構(gòu)本身承受。

值域空間R(JT(q))則是F

能平衡的所有

矢量的集合。

69

由線性代數(shù)可知，零空間N(J(q))是值空間R(JT(q))在n維關(guān)節(jié)空間的正交補(bǔ)，即對(duì)非零的qN(J(q))，則有qR(JT(q))，反之亦然。

物理含義：在不產(chǎn)生操作速度的那些關(guān)節(jié)速度方向上，關(guān)節(jié)力矩不能被操作力所平衡。為使操作臂保持靜止不動(dòng)，在零空間N(J(q))的關(guān)節(jié)力矢量必須為零。

70

在m維操作空間中存在著相似的對(duì)偶關(guān)系，R(J(q))是N(JT(q))在操作空間的正交補(bǔ)。∴不能由關(guān)節(jié)運(yùn)動(dòng)產(chǎn)生的那些操作運(yùn)動(dòng)的方向恰恰是不需要

來平衡的F

的方向。反之，若外力作用的方向是末端執(zhí)行器能夠運(yùn)動(dòng)的方向，則外力就由

來平衡。當(dāng)J(q)退化時(shí)，操作臂處于奇異形位，零空間N(JT(q))不只包含0，因而外力可能承受在操作臂機(jī)構(gòu)本身上。

利用瞬時(shí)運(yùn)動(dòng)和靜力的對(duì)偶關(guān)系、可從瞬時(shí)運(yùn)動(dòng)關(guān)系導(dǎo)出相應(yīng)的靜力關(guān)系。由式(4.18)可導(dǎo)出{A}和{B}間廣義操作力的坐標(biāo)變換關(guān)系若J是關(guān)節(jié)空間向操作空間的映射（微分運(yùn)動(dòng)矢量），則把操作空間的廣義力矢量映射到關(guān)節(jié)空間的關(guān)節(jié)力矢量。關(guān)節(jié)空間操作空間雅可比J力雅可比JT若已知?jiǎng)t有{T}{0}{0}{T}{B}{A}{A}{B}JTJ根據(jù)前面導(dǎo)出的兩坐標(biāo)系{A}和{B}之間廣義速度的坐標(biāo)變換關(guān)系，可以導(dǎo)出{A}和{B}之間廣義操作力的坐標(biāo)變換關(guān)系。解：由前面的推導(dǎo)知例:如圖3-18所示的平面2R機(jī)械手，手爪端點(diǎn)與外界接觸，手爪作用于外界環(huán)境的力為，若關(guān)節(jié)無摩擦力存在，求力的等效關(guān)節(jié)力矩。所以得：圖3-18關(guān)節(jié)力和操作力關(guān)系y0x0例：如圖所示的機(jī)械手夾扳手?jǐn)Q螺絲，在腕部（{Os}）裝有力/力矩傳感器，若已測(cè)出傳感器上的力和力矩，求這時(shí)作用在螺釘上的力和力矩。()解：根據(jù)圖示的相應(yīng)位姿關(guān)系得因此可得兩坐標(biāo)系的微分運(yùn)動(dòng)關(guān)系和靜力傳遞關(guān)系為：{S}{T}{S}{T}微分運(yùn)動(dòng)關(guān)系時(shí)：靜力傳遞關(guān)系時(shí)：77§4.6奇異性和靈巧度*

一、J(q)的奇異性操作臂J(q)依賴于形位q，與奇異形位q對(duì)應(yīng)的J(q)6×n是不滿秩的，此時(shí)有

Rank(J(q))＜min(6,n)(4.33)對(duì)應(yīng)操作空間中的點(diǎn)X＝X(q)為工作空間的奇異點(diǎn)。在奇異形位處，操作臂喪失操作自由度。機(jī)器人的奇異形位有兩類：78(1)邊界奇異形位。平面2R機(jī)械手(例4.1)當(dāng)s2＝0時(shí)，2＝0和2＝180處于工作空間的邊界，只有一個(gè)操作自由度。這種奇異點(diǎn)容易避免,不至于帶來很大麻煩。

(2)內(nèi)部奇異形位。由兩(或多)個(gè)關(guān)節(jié)軸線重合造成的，操作臂各關(guān)節(jié)運(yùn)動(dòng)相抵消，不產(chǎn)生操作運(yùn)動(dòng)。如PUMA560機(jī)械手：在3＝–90附近，手臂伸直，處于邊界奇異狀態(tài)；當(dāng)5＝0時(shí)，關(guān)節(jié)4和6的軸線重合，喪失了一個(gè)自由度，處于內(nèi)部奇異狀態(tài)。

79

二、速度反解機(jī)器人在工作時(shí)，所需手爪的獨(dú)立運(yùn)動(dòng)參數(shù)的數(shù)目m(≤6)隨任務(wù)的性質(zhì)而異。如弧焊、噴漆等有對(duì)稱軸線，獨(dú)立參數(shù)是5個(gè)；帶球形測(cè)頭的機(jī)器人有3個(gè)獨(dú)立參數(shù)；用于圓柱和端銑刀加工的都是4個(gè)獨(dú)立參數(shù)；平面作業(yè)的機(jī)器人需要3個(gè)獨(dú)立參數(shù)。獨(dú)立參數(shù)數(shù)目＝操作空間維數(shù)m。

(1)當(dāng)m＜n，且J(q)是滿秩時(shí)，機(jī)器人具有冗余自由度，冗余度定義為dim(N(J))；

(2)當(dāng)m＝n，且J(q)是滿秩時(shí)，稱為滿自由度；

(3)當(dāng)m＞n，機(jī)器人是欠自由度的。80

對(duì)滿自由度的機(jī)器人，J(q)是方陣，通常由X

可反解相應(yīng)的q。只是在奇異形位時(shí)，J–1(q)不存在，速度反解可能不存在。并且，在奇異點(diǎn)附近，J(q)矩陣是病態(tài)的、反解的q

。操作臂的運(yùn)動(dòng)和動(dòng)態(tài)性能變壞。實(shí)際上，若J(q)是滿秩方陣時(shí)，操作臂運(yùn)動(dòng)方程(4.2)的速度反解為

q＝J–1(q)·X(4.36)

對(duì)冗余度機(jī)器人，其J(q)的列數(shù)多于行數(shù)，即n＞m。當(dāng)J(q)是滿秩時(shí)，冗余度為

dim(N(J))＝n－m＞0(4.37)81由于運(yùn)動(dòng)方程(4.2)的速度反解不唯一，解集合所包含的任意參數(shù)的數(shù)目等于冗余度dim(N(J))。其通解可表示為q＝qs＋kqa

式中，qs是方程(4.2)的特解；qaN(J(q))是J(q)零空間的任意矢量，k是任意常數(shù)。

冗余度機(jī)器人對(duì)避免碰撞，避開奇異狀態(tài)，增加操作臂的靈巧性，改善動(dòng)態(tài)性能會(huì)帶來好處。

82

三、雅可比矩陣的奇異值分解*(略講)

操作臂雅可比的奇異性定性地描述了操作臂的運(yùn)動(dòng)靈巧性和運(yùn)動(dòng)性能。為了定量分析操作臂的靈巧性和速度反解的精度，提出了許多度量指標(biāo)。所有這些指標(biāo)在概念上都與雅可比的奇異值有關(guān)。根據(jù)矩陣的奇異值分解理論，對(duì)操作臂在任意形位的雅可比J(q)進(jìn)行奇異值分解，即J(q)＝UV

式中，URmn；VRnn，為正交矩陣。而

＝式中，1≥2≥…≥m≥0為J的奇異值。

83

可以證明，Rank()＝Rank(J)＝r。當(dāng)r＜m時(shí)，的形式為

＝式中，1是最大奇異值，r是最小奇異值。

84

四、靈巧性度量指標(biāo)

1．條件數(shù)

Salibury和Craig利用J(q)的條件數(shù)作為評(píng)定Stanford/JPL手爪理想尺度最優(yōu)化的準(zhǔn)則。條件數(shù)的定義為k(J)＝||J(q)||·||J–(q)||，當(dāng)m＝n，且非奇異時(shí)(4.42)||J(q)||·||J＋(q)||，當(dāng)m＜n時(shí)式中，||·||代表任意矩陣范數(shù)，通常取Euclidc范數(shù)。可以證明，條件數(shù)與奇異值的關(guān)系為k(J)＝1/r1和r的含義同上，上式定義的條件數(shù)k(J)對(duì)非方陣情況也適用。顯然，矩陣的條件數(shù)取值范圍是1≤k≤當(dāng)k＝1時(shí)，操作臂所具有的形位稱為各向同性。，一般在設(shè)計(jì)機(jī)器人機(jī)械結(jié)構(gòu)時(shí)，應(yīng)盡量使最小條件數(shù)為1，這時(shí)靈巧性最高，各奇異值相等：1＝2＝…＝r

85

2．最小奇異值雅可比陣J(q)的最小奇異值可用來作為控制所需關(guān)節(jié)速度上限的指標(biāo)。即||q||＝(1/r)||X||

在奇異形位附近，r0，對(duì)給定的||X||，所對(duì)應(yīng)的關(guān)節(jié)速度||q||非常大。最小奇異值越大，操作臂終端對(duì)關(guān)節(jié)運(yùn)動(dòng)的響應(yīng)越快。

3．運(yùn)動(dòng)靈巧性指標(biāo)

Angeles和Rojas于1987年提出把最小條件數(shù)km的倒數(shù)定義為度量操作臂靈巧性的指標(biāo)，即D＝(1/km)×100％式中，km＝mink(J)。實(shí)際上，條件數(shù)k(J)只與中間的關(guān)節(jié)變量qi(i＝2,3,…,5)有關(guān)，∴記k＝k(q2,q3,q4,q5)。

864．可操作性(可操作度)Yoshikawa將J(q)與其轉(zhuǎn)置之積的行列式定義為可操作性的度量指標(biāo)，即w＝(det[J(q)JT(q)])1/2利用J(q)的奇異值，可得另一表達(dá)式：w＝12…m

顯然，當(dāng)m＝n時(shí)，w＝|J(q)|；當(dāng)J(q)處于奇異形位時(shí)，Rank(J)＝＜m，w＝0，因此，操作臂的可操作性為0。

上述度量指標(biāo)從不同的角度表示操作臂的靈巧性和反解的精確性。用可操作性可直接判別奇異形位。當(dāng)w＝0時(shí)，處在奇異狀態(tài)；w＞0時(shí)，處在非奇異狀態(tài)。由于矩陣行列式的值并不能代表矩陣求逆運(yùn)算的穩(wěn)定性，因此用它作為可操作性指標(biāo)，衡量操作臂的靈巧性，有一定的缺陷。87

如2×2的對(duì)角矩陣，主對(duì)角元素是1和1010，其行列式的值很大，但其求逆的計(jì)算精度很差；反之，若主對(duì)角元素都為10–10時(shí)，雖然其行列式值很小，但求逆的計(jì)算精度很高。∴用|J(q)|作為靈巧性的評(píng)定指標(biāo)是有缺陷的。同樣，用J(q)與其轉(zhuǎn)置之積的行列式來作為靈巧性指標(biāo)也是有問題的。用J(q)的條件數(shù)作為度量指標(biāo)比較合理，例如前面兩個(gè)矩陣的條件數(shù)分別為1010和1，定量地表示矩陣求逆的數(shù)值穩(wěn)定性。基于條件數(shù)，Angcles和Cajun推導(dǎo)出6軸各向同性操作臂DIFSTRO的運(yùn)動(dòng)學(xué)參數(shù)。在表4–2中列出該操作臂在各向同性形位時(shí)的參數(shù)值，相應(yīng)的雅可比值為

J＝88關(guān)節(jié)iaidii

i111900211–909031190–90411–909051190–90611–90180表4–2DIFSTRO的連桿參數(shù)

可以驗(yàn)證，其條件數(shù)k(J)＝1。實(shí)際上J–1＝JT/2；||J||＝21/2；||J–1||＝1/21/2因此，操作臂處在各向同性的形位。89§4.7剛度和變形操作臂末端在外力的作用下，會(huì)產(chǎn)生變形。變形的大小和方向與操作臂的剛度及作用力矢量有關(guān)。因此，操作臂的剛度是影響它的動(dòng)態(tài)特性和定位精度的主要因素。本課程將在第7章討論以剛度本身作為控制目標(biāo)的順應(yīng)控制和阻抗控制，使機(jī)器人完成各種細(xì)微操作，實(shí)現(xiàn)自動(dòng)裝配和仿形跟蹤。本節(jié)主要討論操作臂的剛度和變形。90

產(chǎn)生變形的部位有連桿本身、支承和關(guān)節(jié)驅(qū)動(dòng)裝置。對(duì)細(xì)長(zhǎng)臂，如：航天飛機(jī)的操作臂，長(zhǎng)達(dá)20多米，連桿產(chǎn)生的變形是末端變形的主要部分。但工業(yè)機(jī)器人的變形要源于傳動(dòng)、減速裝置和伺服系統(tǒng)。

PUMA760的模態(tài)分析表明，由第1、2階振型圖可看出，最薄弱的環(huán)節(jié)發(fā)生在第2、3關(guān)節(jié)。在傳遞驅(qū)動(dòng)力(矩)時(shí)，每個(gè)元件都產(chǎn)生變形，驅(qū)動(dòng)電機(jī)本身由于反饋系統(tǒng)的增益有限而具有的剛度也是有限的。整個(gè)驅(qū)動(dòng)系統(tǒng)(含傳動(dòng)減速機(jī)構(gòu))的剛度用一彈簧系數(shù)Kqi表示，則有：91i＝Kqi·dqi，i＝1,2,…,n式中，i是關(guān)節(jié)力(矩)

靜態(tài)誤差力(矩)；dqi是qi由于i產(chǎn)生的附加變形。上式可寫成矩陣形式

＝Kq·dq式中，Kq＝diag(Kq1,Kq2,

…,Kqn)。再由定義：D＝J(q)dq和＝JT(q)F，則有D＝J(q)dq＝J(q)Kq–1＝J(q)Kq–1JT(q)F＝C(q)·F(4.53)式中，Kq–1為關(guān)節(jié)柔度，它是存在的，∵各關(guān)節(jié)的剛度Kqi嚴(yán)格為正；C(q)m×m為操作臂末端柔度矩陣，用以表示操作空間F～D的線性關(guān)系。

92

∵C(q)和J(q)有關(guān)，∴也隨操作臂的形位q而變化。J(q)滿秩時(shí)，C–1(q)存在，C–1(q)稱為操作臂的操作剛度矩陣。當(dāng)J(q)退化時(shí)，零空間N(JT(q))包含非零向量(m維)，N(JT(q))中的任何操作力F

映射為零關(guān)節(jié)力(處)，亦不產(chǎn)生關(guān)節(jié)變形。這意味著沿這些方向的操作剛度為無限大。為說明C(q)的物理特征，對(duì)C