考慮風(fēng)擾的空中機(jī)械臂系統(tǒng)力位混合控制

彭定亮,郭宇飛,李慧子,郝志強(qiáng),王志剛

(1.武漢科技大學(xué)冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室,湖北 武漢,430081;2.中國(guó)艦船研究設(shè)計(jì)中心,湖北 武漢,430064)

空中機(jī)械臂系統(tǒng)主要由旋翼無(wú)人機(jī)和作業(yè)機(jī)械臂構(gòu)成,通常應(yīng)用于基礎(chǔ)設(shè)備檢測(cè)[1-2]、工程輔助[3-4]、生活服務(wù)[5-7]等領(lǐng)域。在實(shí)施相關(guān)作業(yè)時(shí),機(jī)械臂系統(tǒng)會(huì)與外部環(huán)境發(fā)生物理接觸,因此需對(duì)其接觸力和位置進(jìn)行精確、有效的控制。機(jī)械臂系統(tǒng)與外部環(huán)境的物理交互方式可分為準(zhǔn)靜態(tài)接觸和動(dòng)態(tài)接觸[8]。當(dāng)機(jī)械臂系統(tǒng)相對(duì)所接觸環(huán)境或物體幾乎靜止時(shí)為準(zhǔn)靜態(tài)接觸,如物體的抓取[9]和擺放[10],當(dāng)機(jī)械臂系統(tǒng)與所接觸環(huán)境或物體存在相對(duì)運(yùn)動(dòng)時(shí)為動(dòng)態(tài)接觸,如橋梁探傷[1]。

當(dāng)前,與空中機(jī)械臂系統(tǒng)力位控制相關(guān)的研究大多針對(duì)機(jī)械臂系統(tǒng)與外界環(huán)境的準(zhǔn)靜態(tài)接觸。如Fumagalli等[11]將阻抗控制方法[12]應(yīng)用于空中機(jī)械臂系統(tǒng)的接觸式作業(yè)控制,借助分級(jí)控制結(jié)構(gòu),將整個(gè)系統(tǒng)分為環(huán)境系統(tǒng)、無(wú)人機(jī)系統(tǒng)和機(jī)械臂系統(tǒng)等3個(gè)子系統(tǒng)并分別設(shè)計(jì)控制器,研究結(jié)果表明,使用該方法既能保證空中機(jī)械臂系統(tǒng)的自由飛行穩(wěn)定性,也能保證其在作業(yè)過(guò)程中具有較理想的平穩(wěn)特性。Forte 等[13]利用阻抗控制方法,將空中機(jī)械臂系統(tǒng)分為橫向、縱向及機(jī)械臂等3個(gè)控制閉環(huán)并分別設(shè)計(jì)控制策略,同樣獲得了較好的控制效果。相比之下,與空中機(jī)械臂系統(tǒng)動(dòng)態(tài)控制相關(guān)的研究較少。孟祥冬等[14]針對(duì)空中機(jī)械臂系統(tǒng)的動(dòng)態(tài)控制,設(shè)計(jì)了力位混合控制框架,將整個(gè)系統(tǒng)分為2個(gè)閉環(huán)子系統(tǒng)并對(duì)子系統(tǒng)分別進(jìn)行接觸力控制和位置控制,在保證空中機(jī)械臂系統(tǒng)平穩(wěn)移動(dòng)的同時(shí)也能有效控制其接觸力。不過(guò),需要指出的是,在已有的研究報(bào)道中,無(wú)論空中機(jī)械臂系統(tǒng)進(jìn)行準(zhǔn)靜態(tài)接觸或動(dòng)態(tài)接觸作業(yè),均未考慮不確定風(fēng)擾因素對(duì)機(jī)械臂系統(tǒng)整體作業(yè)的影響,代波等[15]和Escareo 等[16]涉及風(fēng)擾因素的研究也僅針對(duì)空中機(jī)械臂系統(tǒng)中的旋翼無(wú)人機(jī)部分。為了提升空中機(jī)械臂系統(tǒng)在不確定風(fēng)擾下實(shí)施接觸作業(yè)的控制精度、控制效率和穩(wěn)定性,本文采用力位混合控制框架[17],將空中機(jī)械臂整體系統(tǒng)作業(yè)空間分為2個(gè)正交子空間并借助比例-微分(PD)加接觸力前饋控制方法以及一種固定時(shí)間收斂的快速終端滑模控制(FTSMC)方法分別進(jìn)行接觸力控制和位置控制,通過(guò)仿真模擬,與采用一般滑?？刂?SMC)方法的空中機(jī)械臂力位混合控制器[14]控制效果進(jìn)行對(duì)比,以期為風(fēng)擾條件下的空中機(jī)械臂系統(tǒng)力位混合控制研究提供參考。

1 空中作業(yè)無(wú)人機(jī)的動(dòng)力學(xué)模型

1.1 風(fēng)擾模型

自然風(fēng)具有突發(fā)性、持續(xù)性、周期性和不確定性等多種特點(diǎn),模擬難度極大。粗略建立隨機(jī)模型來(lái)模擬自然風(fēng),其風(fēng)速v由基本風(fēng)速v1、漸變風(fēng)速v2和隨機(jī)風(fēng)速v3組成[18]。3種風(fēng)速分別為:

v1=a

(1)

(2)

v3=c·rand(-1,1)sin(2πt+α)

(3)

式(1)～式(3)中,a∈;b∈,t0、t1、t2為時(shí)間間隔;c∈,rand(-1,1)為-1到1的隨機(jī)數(shù)。則自然風(fēng)風(fēng)速v為:

v=v1+v2+v3

(4)

根據(jù)伯努利流體方程,標(biāo)準(zhǔn)風(fēng)壓Pw為:

(5)

式中,r為空氣重度,其值為0.01225 kN/m3。

空中機(jī)械臂系統(tǒng)在實(shí)際風(fēng)力干擾下的工況極其復(fù)雜,此處忽略風(fēng)擾因素對(duì)機(jī)械臂控制的影響,僅考慮無(wú)人機(jī)x,y方向上的二維風(fēng),則風(fēng)擾作用在無(wú)人機(jī)x、y方向上產(chǎn)生的力Fx和Fy以及力矩Mx和My分別為:

(6)

(7)

式(6)～式(7)中,S為無(wú)人機(jī)的受風(fēng)面積,φ和θ分別為無(wú)人機(jī)環(huán)繞x、y軸時(shí)的偏轉(zhuǎn)角。此時(shí)擾動(dòng)力矩陣為:

τext=[FxFy0MxMy0 0 0]T

(8)

1.2 風(fēng)擾下的動(dòng)力學(xué)模型

圖1 空中機(jī)械臂系統(tǒng)示意圖

(9)

式中,τ表示系統(tǒng)的驅(qū)動(dòng)力矩,JT表示雅克比矩陣的轉(zhuǎn)置,F表示外界作用于機(jī)械臂末端的力,τp表示位置控制系統(tǒng)的驅(qū)動(dòng)力矩,τf表示力控制系統(tǒng)的驅(qū)動(dòng)力矩。本文在考慮風(fēng)擾對(duì)空中機(jī)械臂系統(tǒng)控制的影響時(shí),將自然風(fēng)以擾動(dòng)力矩陣(式(8))的形式耦合到系統(tǒng)的動(dòng)力學(xué)模型(式(9))中,即風(fēng)擾因素通過(guò)力和力矩作用于系統(tǒng),這種處理雖然忽略了自然風(fēng)對(duì)機(jī)械臂控制的擾動(dòng),但因無(wú)人機(jī)與機(jī)械臂之間存在耦合,風(fēng)擾因素仍會(huì)間接影響機(jī)械臂的控制。

2 力位混合控制框架

2.1 力位混合控制方案

圖2 力位混合控制框架

在對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行力位混合控制的過(guò)程中,欲使系統(tǒng)按照期望的軌跡并獲得相應(yīng)的末端力,需將機(jī)械臂部分的關(guān)節(jié)角位置與工作中的直角坐標(biāo)相互轉(zhuǎn)換。圖3所示為機(jī)體坐標(biāo)系下的機(jī)械臂結(jié)構(gòu)圖,機(jī)械臂在該坐標(biāo)系下的位置(z1,x1)可表示為:

圖3 機(jī)體坐標(biāo)系下的機(jī)械臂結(jié)構(gòu)

(10)

又有:

(11)

(12)

(13)

由此可得:

(14)

2.2 力控制器設(shè)計(jì)

機(jī)械臂系統(tǒng)力控制采用傳統(tǒng)的PD加接觸力前饋控制方法。根據(jù)胡克定律,機(jī)械臂系統(tǒng)與外部環(huán)境接觸時(shí)的測(cè)量力F為:

(15)

式中,K為環(huán)境剛度?；趫D3,可以求出該機(jī)械臂的雅克比矩陣J為:

(16)

將機(jī)械臂末端所受實(shí)際力F與相應(yīng)期望力Fd進(jìn)行比較,從而實(shí)現(xiàn)力的閉環(huán)控制。借助機(jī)械臂轉(zhuǎn)置雅可比矩陣JT,可將機(jī)械臂工作空間中的力轉(zhuǎn)化為各關(guān)節(jié)上的驅(qū)動(dòng)力。故驅(qū)動(dòng)力力矩τf=JTF。

2.3 位置控制器設(shè)計(jì)

2.3.1 固定時(shí)間收斂系統(tǒng)

引理1[21]考慮一類(lèi)非線(xiàn)性系統(tǒng):

(17)

式中,signa1y=|y|a1sign(y),b1>0,b2>0,a1>1,0

(18)

該式中的控制系統(tǒng)收斂時(shí)間上界Tmax可通過(guò)選擇不同的參數(shù)a1、a2、b1、b2來(lái)確定,并不需要依賴(lài)系統(tǒng)初始狀態(tài)。Zuo等[22]根據(jù)這種非線(xiàn)性系統(tǒng)設(shè)計(jì)的固定時(shí)間收斂的滑?？刂品椒ㄈ〉昧肆己玫膽?yīng)用效果,但在收斂的速度上還有進(jìn)一步改善的空間。姚來(lái)鵬等[23]設(shè)計(jì)了一種固定時(shí)間收斂的終端滑?？刂破鞑?yīng)用于彈藥傳輸機(jī)械臂中,其研究結(jié)果表明,該方法對(duì)不確定干擾具有強(qiáng)魯棒性,能夠?qū)ν饨鐢_動(dòng)下的彈藥傳輸機(jī)械臂進(jìn)行準(zhǔn)確定位控制。

2.3.2 基于固定時(shí)間的快速終端滑?？刂破髟O(shè)計(jì)

(19)

s=e2+Asignk1(e1)+Bsignk2(e1)

(20)

式中,A、B為對(duì)稱(chēng)矩陣,k1>1,0

-α1sign(s)α2|s|β1sign(s)-α3|s|β2sign(s)]

(21)

式中,α1、α2、α3均大于0,0<β1<1,β2>1。通過(guò)分析可知,式(20)滑模面具有收斂特性。當(dāng)系統(tǒng)狀態(tài)距平衡點(diǎn)較遠(yuǎn)時(shí),Asignk1(e1)起主導(dǎo)作用,保證系統(tǒng)快速收斂;當(dāng)系統(tǒng)接近平衡點(diǎn)時(shí),Bsignk2(e1)起主導(dǎo)作用,保證系統(tǒng)在固定時(shí)間內(nèi)穩(wěn)定,使系統(tǒng)在整個(gè)過(guò)程中保持較快的收斂速度。

基于引理1給出的固定時(shí)間Ts,對(duì)式(9)所示的系統(tǒng)動(dòng)力學(xué)方程采用式(20)所示的滑模面以及式(21)所示的控制律,則系統(tǒng)誤差在固定時(shí)間內(nèi)收斂至平衡點(diǎn),收斂時(shí)間T滿(mǎn)足:

T

(22)

Bk2diag(|e1|k2-1)e2]

=sT[-α1sign(s)-α2|s|β1sign(s)-

α3|s|β2sign(s)]

≤sT[-α1|s|-α2|s|β1-α3|s|β2]

≤-α2|s|β1+1-α3|s|β2+1

(23)

(24)

由上分析證明可知,系統(tǒng)狀態(tài)可從任意位置到達(dá)滑模面s→0。然后沿著滑模面做趨近運(yùn)動(dòng)至平衡點(diǎn)的一個(gè)極小鄰域內(nèi)震蕩,理想的滑動(dòng)模態(tài)滿(mǎn)足:

(25)

考慮Lyapunov函數(shù)為:

(26)

對(duì)其求導(dǎo)得:

(27)

系統(tǒng)狀態(tài)在固定時(shí)間t2內(nèi)收斂,即:

(28)

綜合可得,收斂時(shí)間為:T=t1+t2≤Tmax=T1+T2。

對(duì)本研究采用非奇異固定時(shí)間終端滑模面的優(yōu)越性進(jìn)行驗(yàn)證。選取一般滑模面s1和傳統(tǒng)非奇異終端滑模面s2進(jìn)行仿真對(duì)比。一般滑模面為:

s1=σ1e1+e2

(29)

式中,σ1∈。傳統(tǒng)終端滑模面為:

s2=σ2e1+σ3signr(e2)

(30)

式中,σ2、σ3、r∈。為了便于對(duì)仿真結(jié)果進(jìn)行對(duì)比,相關(guān)參數(shù)的取值依次為:σ1=σ2=σ3=1;A=diag([1 1 1 1 1 1 1 1]);k1=3/5;B=diag([20 20 20 20 20 20 20 20]);k2=5/3。則本研究所采用的具有固定時(shí)間收斂的快速終端滑模面可表示為:

s=e2+Asignk1(e1)+Bsignk2(e1)

(31)

選用不同的滑模面,分別在初始狀態(tài)x(0)為0.1和10時(shí)進(jìn)行仿真模擬,結(jié)果如圖4所示。由圖4可見(jiàn),無(wú)論系統(tǒng)初始狀態(tài)是在平衡點(diǎn)附近還是遠(yuǎn)離平衡點(diǎn)處,與采用滑模面s1和s2相比,采用固定時(shí)間收斂的快速終端滑模面s明顯提高了系統(tǒng)狀態(tài)的收斂速度,該仿真結(jié)果充分證實(shí)了采用滑模面s的優(yōu)越性。

(a)x(0)=0.1

3 機(jī)械臂系統(tǒng)力位混合控制仿真分析

表1 空中機(jī)械臂系統(tǒng)主要參數(shù)

3.1 不考慮外界干擾時(shí)的仿真結(jié)果

不考慮外界干擾即τext→0。此時(shí),將位置控制系統(tǒng)中期望位置與實(shí)際位置的差值作為反饋信號(hào)。根據(jù)圖1所示的空中機(jī)械臂系統(tǒng)結(jié)構(gòu)及仿真設(shè)定的期望值可知,對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行位置控制時(shí),要求在旋翼無(wú)人機(jī)向 軸方向移動(dòng)的同時(shí),由機(jī)械臂末端來(lái)完成軌跡跟蹤和接觸力控制。圖5和圖6所示分別為無(wú)外界干擾條件下無(wú)人機(jī)的位置、姿態(tài)以及機(jī)械臂末端軌跡的仿真模擬結(jié)果。通過(guò)分析圖5中無(wú)人機(jī)在X、Y、Z方向上的位置和姿態(tài)角變化可知,采用FTSMC方法對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行位置控制時(shí),無(wú)論在控制精度還是響應(yīng)速度上均較采用SMC方法時(shí)有明顯提高,并且圖6的仿真結(jié)果同時(shí)表明,所采用的FTSMC方法在機(jī)械臂末端軌跡跟蹤方面也有更好的表現(xiàn)。

(a)位置變化

在對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行接觸力控制時(shí),選擇PD控制器和接觸力前饋控制來(lái)實(shí)施對(duì)期望力的控制,該控制器將實(shí)際接觸力與相應(yīng)期望力的差值作為反饋信號(hào)。雖然位置控制系統(tǒng)和力控制系統(tǒng)分開(kāi)控制,但整個(gè)空中機(jī)械臂系統(tǒng)是耦合關(guān)系,相互會(huì)產(chǎn)生影響。無(wú)外界干擾條件下,機(jī)械臂系統(tǒng)接觸力控制的仿真模擬結(jié)果如圖7所示。由圖7可見(jiàn),隨著系統(tǒng)位置控制精度的提升以及響應(yīng)時(shí)間的延長(zhǎng),系統(tǒng)接觸力的控制效果也相應(yīng)提高。

3.2 考慮外界風(fēng)擾的仿真結(jié)果

對(duì)式(4)所描述的隨機(jī)自然風(fēng)進(jìn)行模擬,結(jié)果如圖8所示。

在風(fēng)擾條件下,分別采用FTSMC控制器和SMC控制器對(duì)空中機(jī)械臂系統(tǒng)進(jìn)行位置控制,旋翼無(wú)人機(jī)的位置、姿態(tài)以及機(jī)械臂末端軌跡的仿真模擬結(jié)果分別如圖9和圖10所示。由圖9和圖10可見(jiàn),無(wú)論對(duì)于無(wú)人機(jī)的運(yùn)動(dòng)軌跡還是機(jī)械臂末端執(zhí)行器的運(yùn)動(dòng)軌跡,采用FTSMC控制器在控制精度和響應(yīng)速度上均比采用SMC控制器時(shí)有明顯提升,并且采用FTSMC控制器對(duì)外界風(fēng)擾的抗干擾能力也更強(qiáng)。

(a)位置變化

圖10 風(fēng)擾下的機(jī)械臂末端軌跡

4 結(jié)論

(1)應(yīng)用力位混合控制框架將機(jī)械臂系統(tǒng)位置控制和接觸力控制分解成相互正交的兩部分,并分別借助傳統(tǒng)的PD加接觸力前饋控制和FTSMC進(jìn)行接觸力控制和位置控制。

(2)應(yīng)用力位混合控制器能夠有效抑制風(fēng)擾的影響,在風(fēng)擾下實(shí)現(xiàn)了對(duì)空中機(jī)械臂系統(tǒng)的力及位置進(jìn)行同時(shí)控制。

(3)與SMC控制器相比,FTSMC控制器在控制速度/控制精度和抗干擾能力等方面均有明顯提升,具有良好的魯棒性。