基于干擾觀測器的水下機(jī)械手反演控制方法

陳 偉，劉金龍，王偉然

(江蘇科技大學(xué)電子與信息學(xué)院，江蘇 鎮(zhèn)江 212003)

1 引言

水下機(jī)器人-機(jī)械手系統(tǒng)(Underwater Vehicle Manipulator System，UVMS)是進(jìn)行水下作業(yè)任務(wù)必不可少的裝備，在國防建設(shè)、水下工程作業(yè)、水下救援、海洋勘探與開發(fā)中起到了重要作用[1]。目前水下機(jī)器人已開始由信息型向自主作業(yè)型發(fā)展，且要求其有大范圍、長時(shí)間探測及精細(xì)化作業(yè)功能，建立以UVMS為核心的長期綜合立體化無人探測與作業(yè)系統(tǒng)，實(shí)現(xiàn)機(jī)械手抗干擾控制的需求就顯得極為迫切[2]。

但是，水下機(jī)器人的運(yùn)動(dòng)以及洋流會(huì)不可避免地干擾末端執(zhí)行器的操作，這也就使得復(fù)雜的水下機(jī)器人系統(tǒng)與機(jī)械手之間的協(xié)同工作成為當(dāng)前研究的熱點(diǎn)[3]。然而，水下機(jī)器人機(jī)械手系統(tǒng)是一個(gè)典型的非線性系統(tǒng)，由于水下作業(yè)過程中系統(tǒng)存在著時(shí)變不確定性，傳統(tǒng)的控制方法已不能滿足控制要求，其中主要的難點(diǎn)是機(jī)械手系統(tǒng)的高度非線性、強(qiáng)耦合和不確定特性，以及在水下環(huán)境中難以估計(jì)或測量時(shí)變外部干擾[4]。為了解決機(jī)械手軌跡跟蹤控制這一問題，國內(nèi)外學(xué)者進(jìn)行了研究并提出了各種控制方案。PID控制器由于結(jié)構(gòu)簡單、實(shí)用而被廣泛應(yīng)用于水下機(jī)械手控制系統(tǒng)，例如參數(shù)不確定水下機(jī)械手PID控制[5]、基于任務(wù)空間軌跡的魯棒非線性PID控制[6]等。終端滑?？刂圃趥鹘y(tǒng)滑模面中加入了非線性項(xiàng)，從理論上保證了系統(tǒng)在有限時(shí)間內(nèi)收斂，也被用于水下機(jī)械手控制：如結(jié)合時(shí)延估計(jì)的終端滑?？刂芠7]、自適應(yīng)非奇異積分終端滑?？刂芠8]等。另外，自適應(yīng)控制方法針對UVMS數(shù)學(xué)模型結(jié)構(gòu)已知但存在參數(shù)變化的情況，通過實(shí)時(shí)調(diào)節(jié)參數(shù)可以實(shí)現(xiàn)較精準(zhǔn)的機(jī)械手控制：如基于參考模型的自適應(yīng)控制[9]、自適應(yīng)抗擾控制方法等[10]。

上述文獻(xiàn)中，大都只研究了沒有擾動(dòng)或部分?jǐn)_動(dòng)情況下較為理想的數(shù)學(xué)模型和控制方法，與實(shí)際物理模型和環(huán)境仍有一定的差距。因此在處理干擾問題上無法實(shí)現(xiàn)對干擾的精準(zhǔn)預(yù)測。針對該問題，本文通過引入非線性干擾觀測器來對海流等外界不確定干擾進(jìn)行實(shí)時(shí)預(yù)估，并在控制輸入端進(jìn)行相應(yīng)的動(dòng)態(tài)補(bǔ)償。此外，通過反演算法設(shè)計(jì)水下機(jī)械手軌跡跟蹤控制器，最終對控制律的設(shè)計(jì)使得關(guān)節(jié)跟蹤誤差趨向于零，從而實(shí)現(xiàn)機(jī)械手精確跟蹤期望運(yùn)動(dòng)軌跡。

2 水下機(jī)械手?jǐn)?shù)學(xué)模型

水下機(jī)械手屬于水下空間運(yùn)動(dòng)體，是結(jié)構(gòu)復(fù)雜的多連桿系統(tǒng)，由于其復(fù)雜的結(jié)構(gòu)，從而制約著控制器精確的軌跡跟蹤能力。為了研究各個(gè)連桿的特性以及控制方法，需要建立機(jī)械臂的空間數(shù)學(xué)模型，為機(jī)械手的控制算法研究奠定基礎(chǔ)。

水下機(jī)械手是復(fù)雜的非線性系統(tǒng)，根據(jù)空間姿態(tài)與機(jī)械手各關(guān)節(jié)角度之間的關(guān)系，目前都一般采用其次變換的方法，對其進(jìn)行運(yùn)動(dòng)學(xué)分析建模[11]。圖1為雙關(guān)節(jié)水下機(jī)械手的簡化示意圖。

圖1 水下機(jī)械手簡化示意圖

水下機(jī)械手軌跡規(guī)劃可以轉(zhuǎn)化為末端執(zhí)行器姿態(tài)和位置的軌跡規(guī)劃問題。D-H建模方法是機(jī)械手運(yùn)動(dòng)學(xué)建模方法之一，其優(yōu)勢在于僅用四個(gè)參數(shù)來描述兩個(gè)相鄰關(guān)節(jié)桿之間的相對位置關(guān)系。在多連桿串聯(lián)系統(tǒng)中，該方法在每個(gè)連桿上建立坐標(biāo)系，通過齊次坐標(biāo)變換的方法來建立相鄰關(guān)節(jié)之間的關(guān)系。表1為本文所研究的水下機(jī)械手D-H參數(shù)。

表1 機(jī)械手D-H參數(shù)

表1中，αi-1是關(guān)節(jié)扭轉(zhuǎn)角度，ai-1是連桿的長度，di是連桿的偏距，θi是關(guān)節(jié)旋轉(zhuǎn)角度。

這里，采用Lagrange方程構(gòu)建水下機(jī)械臂系統(tǒng)數(shù)學(xué)模型。將參數(shù)代入Lagrange方程后，可得水下機(jī)械手的動(dòng)力學(xué)方程為

(1)

由式(1)，可得到水下機(jī)械手動(dòng)力學(xué)方程為

(2)

在實(shí)際應(yīng)用中，雖然水下機(jī)械手部分參數(shù)為已知，但在其工作時(shí)，由于受到外界的擾動(dòng)的影響，此時(shí)會(huì)對數(shù)學(xué)模型產(chǎn)生偏差。因此考慮到外部干擾，完整的機(jī)械臂動(dòng)力學(xué)方程可以表示為

(3)

3 非線性控制器設(shè)計(jì)

3.1 非線性干擾觀測器設(shè)計(jì)

由于水下機(jī)器人機(jī)械手系統(tǒng)工作環(huán)境比較復(fù)雜，而非線性干擾觀測器能夠?qū)⑼獠扛蓴_等效到控制輸入端，即觀測出等效干擾。在控制中引入等效的補(bǔ)償，從而實(shí)現(xiàn)對外部干擾的完全控制，提高系統(tǒng)的抗干擾性能[12]。

根據(jù)慣性坐標(biāo)系下機(jī)械手運(yùn)動(dòng)學(xué)方程，考慮到非線性干擾觀測器的基本構(gòu)成，有

(4)

(5)

對于雙關(guān)節(jié)機(jī)械臂，p(q)為待設(shè)計(jì)的非線性函數(shù)，即為

(6)

(7)

進(jìn)一步推導(dǎo)可得

(8)

(9)

(10)

(11)

3.2 反演控制器設(shè)計(jì)

反演控制算法是一種非線性系統(tǒng)控制方法。與傳統(tǒng)非線性系統(tǒng)反饋控制設(shè)計(jì)方法不同，該方法是一種級(jí)聯(lián)設(shè)計(jì)方法，通過引入虛擬控制量，將復(fù)雜的非線性系統(tǒng)分解成多個(gè)簡單的子系統(tǒng)，對每個(gè)子系統(tǒng)選擇適當(dāng)?shù)睦钛牌罩Z夫函數(shù)來保證前一個(gè)子系統(tǒng)的穩(wěn)定性，然后逐步導(dǎo)出最終系統(tǒng)的控制律及參數(shù)自適應(yīng)律，實(shí)現(xiàn)對系統(tǒng)的有效控制和全局調(diào)節(jié)，使系統(tǒng)達(dá)到期望的性能指標(biāo)。在處理一些線性和某些非線性系統(tǒng)時(shí)，該方法在提高過渡過程質(zhì)量方面具有很大的潛力[13]。

e1=q2-qd

(12)

對上述式(12)進(jìn)行求導(dǎo)可得

(13)

根據(jù)反演控制算法的設(shè)計(jì)步驟，定義李雅普諾夫函數(shù)

(14)

對式(14)求導(dǎo)可得

(15)

定義中間虛擬控制變量e2

(16)

其中，c1∈R3×3為對稱正定的常參數(shù)矩陣。將式(16)帶入式(15)，可得

(17)

(18)

對其求導(dǎo)可得

(19)

(20)

最終選擇控制律即為

(21)

其中，c2是一個(gè)正定對稱的常數(shù)矩陣。且有

(22)

顯然，V2≤0成立。根據(jù)李雅普諾夫穩(wěn)定性要求，誤差變量e1和e2最終漸近穩(wěn)定。由此，本控制方法滿足李雅普諾夫穩(wěn)定性理論條件，從而保證了系統(tǒng)的漸近穩(wěn)定性。

4 仿真

為驗(yàn)證該方法對水下機(jī)器人機(jī)械手系統(tǒng)軌跡跟蹤的控制性能，對其進(jìn)行仿真。假設(shè)水下機(jī)器人本體期望運(yùn)動(dòng)軌跡為α=sin(π/3t)，初始角度為1rad。關(guān)節(jié)1的期望運(yùn)動(dòng)軌跡為θ1=2.5+0.1sin(π/2t)，關(guān)節(jié)2的期望運(yùn)動(dòng)軌跡為θ2=1.4-0.1sin(π/5t)?？紤]到水下機(jī)器人機(jī)械手系統(tǒng)的總質(zhì)量為15kg以及系統(tǒng)所受水下環(huán)境干擾的特性，可以假設(shè)外界干擾對系統(tǒng)的作用力為d=15sin(π/5t)N。仿真結(jié)果如圖2至圖7所示。

圖2 ROV主體軌跡跟蹤

圖3 反演控制關(guān)節(jié)1軌跡跟蹤

圖4 反演控制關(guān)節(jié)2軌跡跟蹤

圖5 基于觀測器關(guān)節(jié)1軌跡跟蹤

圖6 基于觀測器關(guān)節(jié)2軌跡跟蹤

圖7 外界干擾d和觀測值

從圖3至圖4可以看出，基于反演算法的水下機(jī)械手軌跡跟蹤器控制器系統(tǒng)存在明顯的抖動(dòng)，其中關(guān)節(jié)1的最大跟蹤誤差接近20%，并且關(guān)節(jié)2的最大誤差也有10%。對比仿真圖3至圖6，在引入非線性干擾觀測器后，系統(tǒng)抖動(dòng)基本被消除，同時(shí)跟蹤誤差明顯減小，接近于零，系統(tǒng)跟蹤性能明顯提高。在跟蹤速度上后者稍有加快，關(guān)節(jié)1在2秒左右實(shí)現(xiàn)完全跟蹤，關(guān)節(jié)2在1.5秒左右實(shí)現(xiàn)完全跟蹤。

圖7為對外界對系統(tǒng)干擾d和干擾觀測器對其的預(yù)估曲線，從圖中可以看出，干擾觀測器能夠準(zhǔn)確預(yù)估出外界干擾。因此干擾觀測器可以明顯抑制外界擾動(dòng)對系統(tǒng)的影響，使得軌跡跟蹤誤差變小。

同時(shí)分析上述兩種仿真結(jié)果后可知，關(guān)節(jié)1的軌跡跟蹤誤差要大于關(guān)節(jié)2，這主要是因?yàn)殛P(guān)節(jié)1與水下機(jī)器人本體是直接剛性連接，由于干擾的存在以及機(jī)器人本體的運(yùn)動(dòng)對關(guān)節(jié)1的影響要稍大于對關(guān)節(jié)2的影響。

5 結(jié)論

水下機(jī)器人機(jī)械手系統(tǒng)廣泛應(yīng)用于水下工程作業(yè)，而海流等未知水下環(huán)境干擾會(huì)顯著降低水下機(jī)器人機(jī)械手系統(tǒng)的性能，為了提高在復(fù)雜水下環(huán)境下機(jī)械手的穩(wěn)定性，提出了一種非線性擾動(dòng)觀測器的反演控制器。干擾觀測器對外界海流等未知干擾進(jìn)行估計(jì)補(bǔ)償，不需要對干擾信號(hào)建立準(zhǔn)確的數(shù)學(xué)模型，對控制系統(tǒng)限制較少，結(jié)構(gòu)也比較簡單。對水下機(jī)械手的軌跡跟蹤進(jìn)行仿真，結(jié)果表明所提方法能夠?qū)崿F(xiàn)軌跡精確跟蹤，跟蹤精度提高約15%，控制效果明顯改善。該方法能夠有效解決水下機(jī)械手控制中的抖動(dòng)問題，提高水下機(jī)械手控制精度，為實(shí)現(xiàn)海洋環(huán)境下水下機(jī)器人機(jī)械手系統(tǒng)精細(xì)化作業(yè)提供了有力支撐。