基于自然正交補的冗余驅動并聯機構動力學建模*

王耀軍,張海峰

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

0 引 言

在多體系統的動力學建模中,需要一種合適的方法來求解系統的運動方程解,這是設計和控制機械系統的關鍵環節之一[1,2]。

機構的動力學模型建立起各關節驅動力（矩）與各關節速度、加速度的相互關系,是機構的設計、控制、仿真等算法設計的基礎。其中,正動力學模型解決機構受力后的運動軌跡問題,而逆動力學模型是在給定軌跡下求解驅動力（矩）。

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

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

在研究冗余驅動并聯機構時,驅動冗余給建模帶來了新的挑戰[15],導致驅動力求解不唯一,可能產生對抗內力。因此,機構具有對驅動力進行分配和優化的內在要求[16]。

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

為了滿足并聯機構的應用和發展需求,需要一種形式簡潔、計算高效的適用于冗余驅動機構的動力學建模方法。

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

1 機構運動學模型

1.1 機構描述

3RRR冗余驅動并聯機器人如圖1所示。

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

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

機器人各參數值如表1所示。

表1 3RRR機器人參數表

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

1.2 機構位移求解

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

桿長為:

‖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內,根據內角的余弦定理,可知:

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

（4）

其中:

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

（5）

再令:

（6）

最后得到機構的逆運動學關系:

（7）

進一步可知被動關節的角度:

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

（8）

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

b1isinqai+b2icosqai+b3i=0

（9）

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

（10）

（11）

（12）

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

（13）

1.3 機構速度求解

將式（13）求導得到:

（14）

其中:

（15）

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

進一步求解得:

（16）

當機構不處于奇異狀態,則系統的雅可比矩陣可表示為:

（17）

該例中各參數取值如下:

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

2 機構動力學模型

2.1 剛體自然正交補建模

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

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

（18）

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

（19）

引入對子（dyad）概念,用一個6×6陣列來表示一個剛體的慣性特征,即質量和慣性矩,在式（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的質心的慣性張量;mi—剛體的質量。

給出剛體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 系統自然正交補建模

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

與單個剛體相對應,和系統有關的全局變量如下:

（32）

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

未約束的系統動力學方程為:

（33）

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

Kt=0

（34）

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

可得系統運動螺旋為:

（35）

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

KT=O

（36）

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

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

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

（37）

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

考慮到:

（38）

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

（39）

（40）

可知:

（41）

式中:τ—系統的二維廣義驅動力;γ—二維廣義重力;δ—二維廣義耗散力。

從而得到所求的系統數學模型為:

（42）

該結論說明,用于串聯機器人的控制律和方法同樣可用于控制并聯機構[20]。但機械系統難免存在磨損及尺寸偏差[21],冗余驅動可能產生非常大的內力[22],必須在控制律設計時充分考慮。

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

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

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

（43）

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

運動螺旋的前3個元素表示剛體的角速度,已知機構旋轉矩陣Q可得到剛體的角速度矩陣:

（44）

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

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

（45）

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

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

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

令各桿件質心離轉動關節的距離為lci,相鄰桿件AiBi和BiO的夾角為qδi,該夾角的關系為:

qδi=qbi-qai

（46）

最后得到系統各桿件的運動螺旋:

（47）

（48）

（49）

（50）

（51）

（52）

（53）

2.4 運動螺旋映射關系

求解如下:

（54）

由式（54）來推導系統6個剛體桿的運動螺旋映射關系,結果如下:

（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 最小化最大驅動力優化

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

優化問題表示為:

（67）

式中:τa—驅動力（矩）。

受等式約束為:

ATτa=τ

（68）

和不等式約束為:

（69）

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

（70）

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

受不等式約束:

（71）

和等式約束:

（72）

以及不等式約束:

（73）

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

4 系統仿真

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

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

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

（74）

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

筆者采用圓軌跡跟蹤仿真,驅動關節的位置、速度、加速度如圖3所示。

由圖3可得:機構各支鏈角位移變換連續平緩;各支鏈速度變化連續,第一支鏈變化最顯著;該支鏈運動狀態變化明顯。

最小化最大驅動力優化的驅動器輸出力矩如圖4所示。

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

用于比對的歐幾里得范數法求解驅動力結果,如圖5所示。

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

5 結束語

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

研究結果表明:

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

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

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

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