基于自然正交補的冗余驅(qū)動并聯(lián)機構(gòu)動力學建模*

王耀軍,張海峰

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

0 引 言

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

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

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

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

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

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

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

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

1 機構(gòu)運動學模型

1.1 機構(gòu)描述

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

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

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

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

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

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

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

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

桿長為:

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

（1）

且:

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

（2）

由式（1,2）可求運動學正解:

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）

最后得到機構(gòu)的逆運動學關(guān)系:

（7）

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

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

（8）

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

b1isinqai+b2icosqai+b3i=0

（9）

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

（10）

（11）

（12）

由式（10～12）可推導運動學逆解的另一種表達式:

（13）

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

將式（13）求導得到:

（14）

其中:

（15）

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

進一步求解得:

（16）

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

（17）

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

Ai點坐標分別為A1（0,0.25）、A2（0.433,0）、A3（0.433,0.5）,所有連桿長度l=0.244 m。任兩個Ai點距離a=0.5 m。

2 機構(gòu)動力學模型

2.1 剛體自然正交補建模

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

定義剛體i的六維運動螺旋ti和作用在該剛體上的六維力螺旋wi如下:

（18）

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

（19）

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

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

（20）

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

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

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

（21）

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

（22）

即:

（23）

（24）

式中:Ti—剛體i的6×2運動映射矩陣。

（25）

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

（26）

（27）

（28）

式（24）兩邊同時對時間求導:

（29）

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

（30）

式中:Ii—剛體i的6×6慣性對子;Ci—同一個剛體的6×6科氏力和離心力對子。

即:

（31）

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

接下來,筆者推導系統(tǒng)不含約束力螺旋的數(shù)學模型。通過系統(tǒng)運動螺旋和系統(tǒng)約束力螺旋的對應作用關(guān)系來消除約束力螺旋。其關(guān)系將在下文進一步定義。

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

（32）

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

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

（33）

顯而易見,系統(tǒng)的運動約束關(guān)系可以通過一個六維系統(tǒng)運動螺旋t的線性齊次方程來表示:

Kt=0

（34）

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

可得系統(tǒng)運動螺旋為:

（35）

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

KT=O

（36）

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

稱K為矩陣T的自然正交補矩陣。

此外,式（35）兩邊同時對時間求導,可得到系統(tǒng)運動螺旋t隨時間的變化率:

（37）

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

考慮到:

（38）

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

（39）

（40）

可知:

（41）

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

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

（42）

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

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

2.3 機器人連桿的運動螺旋系

兩個通過轉(zhuǎn)動副或移動副連接的連桿其相對位姿可分別用運動螺旋tr和tp來表示,即

（43）

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

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

（44）

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

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

（45）

式中:ti,i-1—連桿i相對于其上一級連桿i-1的相對運動。

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

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

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

qδi=qbi-qai

（46）

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

（47）

（48）

（49）

（50）

（51）

（52）

（53）

2.4 運動螺旋映射關(guān)系

求解如下:

（54）

由式（54）來推導系統(tǒng)6個剛體桿的運動螺旋映射關(guān)系,結(jié)果如下:

（55）

（56）

（57）

（58）

（59）

（60）

當i=1,2,3時,過程變量的表達式為:

（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ū)動力優(yōu)化

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

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

（67）

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

受等式約束為:

ATτa=τ

（68）

和不等式約束為:

（69）

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

（70）

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

受不等式約束:

（71）

和等式約束:

（72）

以及不等式約束:

（73）

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

4 系統(tǒng)仿真

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

跟蹤的圓軌跡,相對于固定坐標系,可給出:

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

（74）

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

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

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

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

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

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

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

5 結(jié)束語

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

研究結(jié)果表明:

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

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

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

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