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

陳 偉，劉金龍，王偉然

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

1 引言

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

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

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

2 水下機械手數(shù)學模型

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

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

圖1 水下機械手簡化示意圖

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

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

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

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

(1)

由式(1)，可得到水下機械手動力學方程為

(2)

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

(3)

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

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

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

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

(4)

(5)

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

(6)

(7)

進一步推導可得

(8)

(9)

(10)

(11)

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

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

e1=q2-qd

(12)

對上述式(12)進行求導可得

(13)

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

(14)

對式(14)求導可得

(15)

定義中間虛擬控制變量e2

(16)

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

(17)

(18)

對其求導可得

(19)

(20)

最終選擇控制律即為

(21)

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

(22)

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

4 仿真

為驗證該方法對水下機器人機械手系統(tǒng)軌跡跟蹤的控制性能，對其進行仿真。假設(shè)水下機器人本體期望運動軌跡為α=sin(π/3t)，初始角度為1rad。關(guān)節(jié)1的期望運動軌跡為θ1=2.5+0.1sin(π/2t)，關(guān)節(jié)2的期望運動軌跡為θ2=1.4-0.1sin(π/5t)。考慮到水下機器人機械手系統(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可以看出，基于反演算法的水下機械手軌跡跟蹤器控制器系統(tǒng)存在明顯的抖動，其中關(guān)節(jié)1的最大跟蹤誤差接近20%，并且關(guān)節(jié)2的最大誤差也有10%。對比仿真圖3至圖6，在引入非線性干擾觀測器后，系統(tǒng)抖動基本被消除，同時跟蹤誤差明顯減小，接近于零，系統(tǒng)跟蹤性能明顯提高。在跟蹤速度上后者稍有加快，關(guān)節(jié)1在2秒左右實現(xiàn)完全跟蹤，關(guān)節(jié)2在1.5秒左右實現(xiàn)完全跟蹤。

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

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

5 結(jié)論

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