王耀軍,張海峰

（1.浙江機(jī)電職業(yè)技術(shù)學(xué)院 自動(dòng)化學(xué)院,浙江 杭州 310053;2.浙江理工大學(xué) 機(jī)械與自動(dòng)控制學(xué)院,浙江 杭州 310018）

0 引 言

在多體系統(tǒng)的動(dòng)力學(xué)建模中,需要一種合適的方法來(lái)求解系統(tǒng)的運(yùn)動(dòng)方程解,這是設(shè)計(jì)和控制機(jī)械系統(tǒng)的關(guān)鍵環(huán)節(jié)之一[1,2]。

機(jī)構(gòu)的動(dòng)力學(xué)模型建立起各關(guān)節(jié)驅(qū)動(dòng)力（矩）與各關(guān)節(jié)速度、加速度的相互關(guān)系,是機(jī)構(gòu)的設(shè)計(jì)、控制、仿真等算法設(shè)計(jì)的基礎(chǔ)。其中,正動(dòng)力學(xué)模型解決機(jī)構(gòu)受力后的運(yùn)動(dòng)軌跡問(wèn)題,而逆動(dòng)力學(xué)模型是在給定軌跡下求解驅(qū)動(dòng)力（矩）。

有學(xué)者采用經(jīng)典動(dòng)力學(xué)建模方法對(duì)此進(jìn)行研究。比如,田波等人[3]基于運(yùn)動(dòng)和力關(guān)系分析用牛頓-歐拉法（Newton-Euler,NE）建立了機(jī)械臂動(dòng)力學(xué)模型,考慮了機(jī)構(gòu)各剛體的相互作用力,全面揭示了系統(tǒng)剛體的動(dòng)力學(xué)特性;但該方法方程眾多、計(jì)算量大。KHALIL W等人[4]基于系統(tǒng)能量平衡分析用歐拉-拉格朗日法（Euler-Lagrange,EL）,建立了并聯(lián)機(jī)構(gòu)動(dòng)力學(xué)模型,采用EL法雖簡(jiǎn)化了系統(tǒng)方程;但該方法存在計(jì)算量較大和實(shí)時(shí)性問(wèn)題[5]。

另外,還有如倪仕全等人[6]基于虛功原理、CHENG Hui等人[7]應(yīng)用了d’Alembert原理、MILLER K[8]采用了Hamilton原理和KANE TR等人[9]提出的凱恩方程法;以及其他一些方法,如李群理論[10]、虛擬彈簧法[11]、螺旋理論[12]、遞歸矩陣法[13]和廣義動(dòng)量法[14]等。

在研究冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)時(shí),驅(qū)動(dòng)冗余給建模帶來(lái)了新的挑戰(zhàn)[15],導(dǎo)致驅(qū)動(dòng)力求解不唯一,可能產(chǎn)生對(duì)抗內(nèi)力。因此,機(jī)構(gòu)具有對(duì)驅(qū)動(dòng)力進(jìn)行分配和優(yōu)化的內(nèi)在要求[16]。

很多學(xué)者基于傳統(tǒng)方法,研究了機(jī)構(gòu)的動(dòng)力學(xué)建模,但存在模型變量多、方程數(shù)量大和計(jì)算效率問(wèn)題。為此,ANGELES J[17]提出了,根據(jù)螺旋理論,系統(tǒng)約束螺旋可采用與系統(tǒng)各連桿運(yùn)動(dòng)螺旋成線性關(guān)系的約束矩陣的正交補(bǔ)來(lái)消除（該約束的消除方法基于運(yùn)動(dòng)與約束的自然對(duì)應(yīng)關(guān)系,故稱(chēng)自然正交補(bǔ)）,該方法具有系統(tǒng)高效的優(yōu)點(diǎn)。但ANGELES J未討論自然正交補(bǔ)在冗余驅(qū)動(dòng)系統(tǒng)中的應(yīng)用。

為了滿(mǎn)足并聯(lián)機(jī)構(gòu)的應(yīng)用和發(fā)展需求,需要一種形式簡(jiǎn)潔、計(jì)算高效的適用于冗余驅(qū)動(dòng)機(jī)構(gòu)的動(dòng)力學(xué)建模方法。

筆者以冗余驅(qū)動(dòng)3RRR平面機(jī)構(gòu)[18]為研究對(duì)象,基于閉環(huán)矢量法推導(dǎo)機(jī)構(gòu)的位移、速度關(guān)系,再基于螺旋理論,應(yīng)用自然正交補(bǔ)方法,建立機(jī)構(gòu)的逆動(dòng)力學(xué)方程;以最小化最大驅(qū)動(dòng)力為目標(biāo),采用無(wú)窮大范數(shù)法求解驅(qū)動(dòng)力靜不定問(wèn)題,并通過(guò)圓軌跡跟蹤進(jìn)行仿真,研究冗余驅(qū)動(dòng)機(jī)構(gòu)的動(dòng)力學(xué)特性。

1 機(jī)構(gòu)運(yùn)動(dòng)學(xué)模型

1.1 機(jī)構(gòu)描述

3RRR冗余驅(qū)動(dòng)并聯(lián)機(jī)器人如圖1所示。

由圖1可知：該機(jī)構(gòu)由1個(gè)基礎(chǔ)平臺(tái)（base platform,BP）、3個(gè)相同的RR分支（各分支由對(duì)應(yīng)的第1個(gè)R關(guān)節(jié)驅(qū)動(dòng)）、及1個(gè)動(dòng)平臺(tái)（mobile platform,MP）組成。第i個(gè)分支,當(dāng)i=1,2,3時(shí),是AiBiO。3個(gè)驅(qū)動(dòng)關(guān)節(jié)位于點(diǎn)Ai,3個(gè)被動(dòng)關(guān)節(jié)位于點(diǎn)Bi,機(jī)構(gòu)的末端執(zhí)行器匯聚到O點(diǎn),即機(jī)構(gòu)的動(dòng)平臺(tái)。

筆者建立參考坐標(biāo)系XY,其中A1、A2點(diǎn)分別在X軸和Y軸上。設(shè)6根桿子的長(zhǎng)都為l,將編號(hào)1—6分別指定連桿A1B1、A2B2、A3B3、B1O、B2O、B3O。

機(jī)器人各參數(shù)值如表1所示。

表1 3RRR機(jī)器人參數(shù)表

由表1可知:該機(jī)構(gòu)三支鏈均布;動(dòng)平臺(tái)可在X和Y組成的平面內(nèi)移動(dòng),該機(jī)構(gòu)具有XY平面內(nèi)的兩個(gè)移動(dòng)自由度。

1.2 機(jī)構(gòu)位移求解

已知組成機(jī)構(gòu)的六桿長(zhǎng)度皆為l,Ai和Bi的各點(diǎn)坐標(biāo)分別為:xAi=[xAi,yAi]T和xBi=[xBi,yBi]T。動(dòng)平臺(tái)O點(diǎn)坐標(biāo)為x=[x,y]T。當(dāng)i=1,2,3,主動(dòng)關(guān)節(jié)角度qai和被動(dòng)關(guān)節(jié)角度qbi可經(jīng)計(jì)算得到。

桿長(zhǎng)為:

‖x-xBi‖=l,‖xAi-xBi‖=l

（1）

且:

xBi=[xAi+lcosqai,yAi+lsinqai]T

（2）

由式（1,2）可求運(yùn)動(dòng)學(xué)正解:

x=[‖xB1‖2（yB2-yB3）+‖xB2‖2（yB3-yB1）+‖xB3‖2（yB1-yB2）]/2[xB1（yB2-yB3）+xB2（yB3-yB1）+xB3（yB1-yB2）]

y=[‖xB1‖2（xB3-xB2）+‖xB2‖2（xB1-xB3）+‖xB3‖2（xB2-xB1）]/2[xB1（yB2-yB3）+xB2（yB3-yB1）+xB3（yB1-yB2）]

（3）

任一分支組成的三角形AiBiO內(nèi),根據(jù)內(nèi)角的余弦定理,可知:

‖BiO‖2=‖AiBi‖2+‖AiO‖2-
2‖AiBi‖‖AiO‖cos（qai-ψi）

（4）

其中:

ψi=atan2（y-yAi,x-xAi）

（5）

再令:

（6）

最后得到機(jī)構(gòu)的逆運(yùn)動(dòng)學(xué)關(guān)系:

（7）

進(jìn)一步可知被動(dòng)關(guān)節(jié)的角度:

qbi=atan2（y-yBi,x-xBi）

（8）

再令|AiAj|=a,由式（1）可知:

b1isinqai+b2icosqai+b3i=0

（9）

式（9）中間變量定義如下:

（10）

（11）

（12）

由式（10～12）可推導(dǎo)運(yùn)動(dòng)學(xué)逆解的另一種表達(dá)式:

（13）

1.3 機(jī)構(gòu)速度求解

將式（13）求導(dǎo)得到:

（14）

其中:

（15）

式中:J—系統(tǒng)雅可比矩陣;Jq11,Jq22,Jq33—正雅可比矩陣各對(duì)角元素;Kx11,Kx12,Kx21,Kx22,Kx31,Kx32—逆雅可比矩陣各元素。

進(jìn)一步求解得:

（16）

當(dāng)機(jī)構(gòu)不處于奇異狀態(tài),則系統(tǒng)的雅可比矩陣可表示為:

（17）

該例中各參數(shù)取值如下:

Ai點(diǎn)坐標(biāo)分別為A1（0,0.25）、A2（0.433,0）、A3（0.433,0.5）,所有連桿長(zhǎng)度l=0.244 m。任兩個(gè)Ai點(diǎn)距離a=0.5 m。

2 機(jī)構(gòu)動(dòng)力學(xué)模型

2.1 剛體自然正交補(bǔ)建模

針對(duì)該3RRR機(jī)構(gòu),首先，筆者采用牛頓-歐拉方程來(lái)構(gòu)造不含約束的耦合方程。機(jī)構(gòu)由6個(gè)剛體組成。有3個(gè)冗余驅(qū)動(dòng)的連桿,冗余度（degree-of-redundancy,DOR）為1。

定義剛體i的六維運(yùn)動(dòng)螺旋ti和作用在該剛體上的六維力螺旋wi如下:

（18）

接著,筆者將作用于剛體i的力螺旋分解成兩部分:

（19）

引入對(duì)子（dyad）概念,用一個(gè)6×6陣列來(lái)表示一個(gè)剛體的慣性特征,即質(zhì)量和慣性矩,在式（20）中用Mi來(lái)表示。

剛體i的6×6角速度對(duì)子Wi和慣性對(duì)子Mi定義為:

（20）

式中:Ωi—向量ωi的3×3叉乘矩陣（cross product matrix,CPM）。

任何一個(gè)向量v∈3的叉乘矩陣V∈3×3定義為:V=（?v×x/?x）,?x∈3。O,1—3×3零矩陣和單位矩陣;ICi—3×3作用在剛體i的質(zhì)心的慣性張量;mi—?jiǎng)傮w的質(zhì)量。

給出剛體i的牛頓-歐拉方程為:

（21）

上述方程可表示為一種更加緊湊的形式[17]:

（22）

即:

（23）

（24）

式中:Ti—?jiǎng)傮wi的6×2運(yùn)動(dòng)映射矩陣。

（25）

將式（24）代入式（25）,可得

（26）

（27）

（28）

式（24）兩邊同時(shí)對(duì)時(shí)間求導(dǎo):

（29）

最后,將式（28,29）代入式（27）得到:

（30）

式中:Ii—?jiǎng)傮wi的6×6慣性對(duì)子;Ci—同一個(gè)剛體的6×6科氏力和離心力對(duì)子。

即:

（31）

2.2 系統(tǒng)自然正交補(bǔ)建模

接下來(lái),筆者推導(dǎo)系統(tǒng)不含約束力螺旋的數(shù)學(xué)模型。通過(guò)系統(tǒng)運(yùn)動(dòng)螺旋和系統(tǒng)約束力螺旋的對(duì)應(yīng)作用關(guān)系來(lái)消除約束力螺旋。其關(guān)系將在下文進(jìn)一步定義。

與單個(gè)剛體相對(duì)應(yīng),和系統(tǒng)有關(guān)的全局變量如下:

（32）

式中:t—系統(tǒng)運(yùn)動(dòng)螺旋;wC—系統(tǒng)約束力螺旋;wA—系統(tǒng)驅(qū)動(dòng)力螺旋;wG—系統(tǒng)重力螺旋;wD—系統(tǒng)耗散力螺旋;M—36×36塊對(duì)角矩陣表示的系統(tǒng)質(zhì)量;W—36×36塊對(duì)角矩陣表示的系統(tǒng)角速度;T—36×2系統(tǒng)運(yùn)動(dòng)映射矩陣。

未約束的系統(tǒng)動(dòng)力學(xué)方程為:

（33）

顯而易見(jiàn),系統(tǒng)的運(yùn)動(dòng)約束關(guān)系可以通過(guò)一個(gè)六維系統(tǒng)運(yùn)動(dòng)螺旋t的線性齊次方程來(lái)表示:

Kt=0

（34）

式中:K—36×36矩陣,其秩為34。

可得系統(tǒng)運(yùn)動(dòng)螺旋為:

（35）

將式（35）代入式（34）,得到:

KT=O

（36）

式中:O—36×2零矩陣。

稱(chēng)K為矩陣T的自然正交補(bǔ)矩陣。

此外,式（35）兩邊同時(shí)對(duì)時(shí)間求導(dǎo),可得到系統(tǒng)運(yùn)動(dòng)螺旋t隨時(shí)間的變化率:

（37）

接著,將式（37）代入式（33）,隨后在新式兩邊同時(shí)前乘TT。

考慮到:

（38）

系統(tǒng)的牛頓-歐拉方程在廣義坐標(biāo)上表示,而前面的式（27）表示為笛卡爾變量,因此,系統(tǒng)各剛體的運(yùn)動(dòng)螺旋和力螺旋表示為:

（39）

（40）

可知:

（41）

式中:τ—系統(tǒng)的二維廣義驅(qū)動(dòng)力;γ—二維廣義重力;δ—二維廣義耗散力。

從而得到所求的系統(tǒng)數(shù)學(xué)模型為:

（42）

該結(jié)論說(shuō)明,用于串聯(lián)機(jī)器人的控制律和方法同樣可用于控制并聯(lián)機(jī)構(gòu)[20]。但機(jī)械系統(tǒng)難免存在磨損及尺寸偏差[21],冗余驅(qū)動(dòng)可能產(chǎn)生非常大的內(nèi)力[22],必須在控制律設(shè)計(jì)時(shí)充分考慮。

筆者提出的自然正交補(bǔ)建模步驟如圖2所示。

2.3 機(jī)器人連桿的運(yùn)動(dòng)螺旋系

兩個(gè)通過(guò)轉(zhuǎn)動(dòng)副或移動(dòng)副連接的連桿其相對(duì)位姿可分別用運(yùn)動(dòng)螺旋tr和tp來(lái)表示,即

（43）

式中:tr—轉(zhuǎn)動(dòng)副運(yùn)動(dòng)螺旋;e1—轉(zhuǎn)動(dòng)副旋轉(zhuǎn)軸;θ—旋轉(zhuǎn)角度;s—從旋轉(zhuǎn)軸上某點(diǎn)O指向連桿2的質(zhì)心的向量;tp—移動(dòng)副運(yùn)動(dòng)螺旋;b—移動(dòng)副移動(dòng)距離;e2—移動(dòng)方向的單位向量。

運(yùn)動(dòng)螺旋的前3個(gè)元素表示剛體的角速度,已知機(jī)構(gòu)旋轉(zhuǎn)矩陣Q可得到剛體的角速度矩陣:

（44）

式中:ωij—角速度矩陣各元素。

運(yùn)動(dòng)螺旋的后3個(gè)元素表示剛體質(zhì)心的速度,直接利用式（43）計(jì)算給出。因此,由j個(gè)連桿串聯(lián)而成的構(gòu)件,其末端連桿的運(yùn)動(dòng)螺旋t可以通過(guò)每一對(duì)相連連桿之間的相對(duì)運(yùn)動(dòng)螺旋的組合來(lái)表示:

（45）

式中:ti,i-1—連桿i相對(duì)于其上一級(jí)連桿i-1的相對(duì)運(yùn)動(dòng)。

該3RRR平面冗余驅(qū)動(dòng)并聯(lián)機(jī)器人由3個(gè)相同尺寸的分支鏈組成,每個(gè)分支是一個(gè)平面二自由度串聯(lián)機(jī)構(gòu)。所以,3個(gè)分支運(yùn)動(dòng)鏈組成的并聯(lián)機(jī)構(gòu),其運(yùn)動(dòng)可表示為各分支鏈允許的運(yùn)動(dòng)集,即在閉鏈約束條件下允許的動(dòng)平臺(tái)運(yùn)動(dòng)。

由式（43，45）可求得系統(tǒng)各桿件的運(yùn)動(dòng)螺旋。

令各桿件質(zhì)心離轉(zhuǎn)動(dòng)關(guān)節(jié)的距離為lci,相鄰桿件AiBi和BiO的夾角為qδi,該夾角的關(guān)系為:

qδi=qbi-qai

（46）

最后得到系統(tǒng)各桿件的運(yùn)動(dòng)螺旋:

（47）

（48）

（49）

（50）

（51）

（52）

（53）

2.4 運(yùn)動(dòng)螺旋映射關(guān)系

求解如下:

（54）

由式（54）來(lái)推導(dǎo)系統(tǒng)6個(gè)剛體桿的運(yùn)動(dòng)螺旋映射關(guān)系,結(jié)果如下:

（55）

（56）

（57）

（58）

（59）

（60）

當(dāng)i=1,2,3時(shí),過(guò)程變量的表達(dá)式為:

（61）

（62）

（63）

（64）

n3i=[-（y-yAi-lsinqai）+（l+xAicosqai+yAisinqai-xcosqai-ysinqai）ln1i]/（x-xAi-lcosqai）2

（65）

n4i=[（x-xAi-lcosqai）+（l+xAicosqai+yAisinqai-xcosqai-ysinqai）ln2i]/（x-xAi-lcosqai）2

（66）

3 最小化最大驅(qū)動(dòng)力優(yōu)化

最小化最大驅(qū)動(dòng)力法優(yōu)化是冗余驅(qū)動(dòng)力（矩）優(yōu)化中用以解決驅(qū)動(dòng)器物理極限的方法,即無(wú)窮大范數(shù)解。與歐幾里得范數(shù)解[23]相比,無(wú)窮大范數(shù)求解最小化驅(qū)動(dòng)力的最大元素,物理極限意義相對(duì)前者更明確,具有低力矩絕對(duì)值、驅(qū)動(dòng)力均勻分布的優(yōu)點(diǎn)。

優(yōu)化問(wèn)題表示為:

（67）

式中:τa—驅(qū)動(dòng)力（矩）。

受等式約束為:

ATτa=τ

（68）

和不等式約束為:

（69）

為了提高計(jì)算效率,筆者引入一種新標(biāo)量s≥0來(lái)替換‖τa‖∞的值,因此,初始問(wèn)題轉(zhuǎn)變成以如下形式存在:

（70）

其中:c=[0,0,0,1]T。

受不等式約束:

（71）

和等式約束:

（72）

以及不等式約束:

（73）

式中:06,03—6和3維零陳列。

4 系統(tǒng)仿真

根據(jù)GB—T12642—2001《工業(yè)機(jī)器人性能規(guī)范》,測(cè)試軌跡的形狀應(yīng)為直線或圓。因此,系統(tǒng)仿真采用經(jīng)典圓軌跡來(lái)描述該平面機(jī)器人動(dòng)平臺(tái)的運(yùn)動(dòng)。

跟蹤的圓軌跡,相對(duì)于固定坐標(biāo)系,可給出:

（x-0.216 5）2+（y-0.25）2=0.072

（74）

即圓心點(diǎn)（0.216 5,0.25）,半徑r=0.07 m。運(yùn)行過(guò)程采用三段（加速、滑行、減速）梯形速度軌跡。從而得到驅(qū)動(dòng)關(guān)節(jié)變量和廣義坐標(biāo)變量的相對(duì)關(guān)系解析解。相應(yīng)的廣義坐標(biāo)位置和速度、加速度,及作用到動(dòng)平臺(tái)點(diǎn)O的力可分別計(jì)算求解。

筆者采用圓軌跡跟蹤仿真,驅(qū)動(dòng)關(guān)節(jié)的位置、速度、加速度如圖3所示。

由圖3可得:機(jī)構(gòu)各支鏈角位移變換連續(xù)平緩;各支鏈速度變化連續(xù),第一支鏈變化最顯著;該支鏈運(yùn)動(dòng)狀態(tài)變化明顯。

最小化最大驅(qū)動(dòng)力優(yōu)化的驅(qū)動(dòng)器輸出力矩如圖4所示。

由圖4可得:無(wú)窮大范數(shù)法其最大驅(qū)動(dòng)力矩在1.2 Nm左右,其中第二支鏈驅(qū)動(dòng)在0.85 s附近有一個(gè)突變,是由于無(wú)窮大范數(shù)求解引起。

用于比對(duì)的歐幾里得范數(shù)法求解驅(qū)動(dòng)力結(jié)果,如圖5所示。

采用歐幾里得范數(shù),最大驅(qū)動(dòng)力矩在1.5 Nm,超過(guò)無(wú)窮大范數(shù)法25%。

5 結(jié)束語(yǔ)

該研究采用了閉環(huán)矢量法推導(dǎo)冗余驅(qū)動(dòng)3RRR并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)方程,應(yīng)用了螺旋理論建立逆動(dòng)力學(xué)方程,采用了無(wú)窮大范數(shù)法,解決了冗余系統(tǒng)的驅(qū)動(dòng)力求解不唯一問(wèn)題,并通過(guò)圓軌跡跟蹤進(jìn)行了仿真。

研究結(jié)果表明:

（1）基于螺旋理論將自然正交補(bǔ)方法應(yīng)用到冗余驅(qū)動(dòng)系統(tǒng)的動(dòng)力學(xué)建模,并以典型的平面冗余驅(qū)動(dòng)3RRR并聯(lián)機(jī)構(gòu)為例,給出了推導(dǎo)步驟和過(guò)程。得益于動(dòng)力學(xué)公式中約束力螺旋的消除,該自然正交補(bǔ)建模具有系統(tǒng)高效、中間變量少等優(yōu)點(diǎn);

（2）在機(jī)構(gòu)的逆動(dòng)力學(xué)方程基礎(chǔ)上,采用無(wú)窮大范數(shù)法來(lái)最小化各驅(qū)動(dòng)器驅(qū)動(dòng)力矩的最大極值,最大驅(qū)動(dòng)器功率由1.5 Nm降低到1.2 Nm,減小20%,大大降低了驅(qū)動(dòng)器功率要求;

（3）該自然正交補(bǔ)方法不僅適用于平面冗余驅(qū)動(dòng)機(jī)構(gòu),也適用于空間冗余驅(qū)動(dòng)機(jī)構(gòu),針對(duì)超冗余機(jī)構(gòu)也有效。

針對(duì)冗余驅(qū)動(dòng)的復(fù)雜多鏈過(guò)約束并聯(lián)機(jī)構(gòu),在后續(xù)的研究中,筆者將搭建3RRR原理樣機(jī),開(kāi)展其魯棒控制律設(shè)計(jì),以進(jìn)一步研究機(jī)構(gòu)的離線和在線動(dòng)力學(xué)參數(shù)辨識(shí)方法。