竇 剛，姜文剛，張楠楠

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

水下機(jī)器人作為人類探索海洋的重要手段，在海洋資源的開發(fā)和利用中起至關(guān)重要的作用。隨著微電子、計算機(jī)、智能化、導(dǎo)航等技術(shù)的快速發(fā)展，自主水下航行器(AUV)的研究逐漸成為各國海洋裝備技術(shù)的前沿[1-4]。AUV是一種自身攜帶能源、推進(jìn)器、導(dǎo)航、傳感等設(shè)備，無需人為操控的水下機(jī)器人，依靠嵌入式控制計算機(jī)和自主控制軟件來執(zhí)行預(yù)定的任務(wù)。目前，AUV已經(jīng)多次成功應(yīng)用于海底石油、天然氣以及礦產(chǎn)資源的勘探。發(fā)展具備水下作業(yè)能力的AUV是目前水下機(jī)器人領(lǐng)域的一個重要方向，其目標(biāo)是依靠自身攜帶的機(jī)械手等裝置，自主完成水下精密作業(yè)任務(wù)。

動力定位早先應(yīng)用于船舶控制系統(tǒng)，逐漸取代了傳統(tǒng)的錨泊定位，成為深海作業(yè)船舶依賴的關(guān)鍵技術(shù)，廣泛應(yīng)用于海上定點作業(yè)和系泊。其由于強(qiáng)大的適用性和機(jī)動性，在其他領(lǐng)域也發(fā)揮著至關(guān)重要的作用，如海洋平臺、ROV、AUV等。水下機(jī)器人動力定位系統(tǒng)是一種閉環(huán)的控制系統(tǒng)，其功能是通過不斷檢測實際位置和期望位置的偏差，再根據(jù)外界工作環(huán)境的影響計算出使本體恢復(fù)或者保持指定位置的推力大小，并通過主推進(jìn)器和多個輔助推進(jìn)器的操控，達(dá)到克服海流等環(huán)境干擾，保持自身位置和姿態(tài)穩(wěn)定的目的。這是保證正確有效地執(zhí)行水下作業(yè)的關(guān)鍵[5]。從20世紀(jì)末開始，人們便采用PID控制、自適應(yīng)控制、滑?？刂啤⒛：刂?、神經(jīng)網(wǎng)絡(luò)控制及其改進(jìn)算法等，開展了水下機(jī)器人運動控制的理論和實踐研究，并取得了一系列成果[6-9]。其中，對于動力定位控制問題也進(jìn)行了大量的研究。文獻(xiàn)[10]針對水流速度未知干擾，提出了一種基于反演技術(shù)的自適應(yīng)變結(jié)構(gòu)控制策略，但控制模型較為簡單，僅考慮了水平面的情況；文獻(xiàn)[11] 提出了一種基于Kalman濾波的海流、不平衡力估計方法，對以后控制方法的研究有重要意義；文獻(xiàn)[12]提出了一種新的環(huán)境最優(yōu)動力定位控制策略，保持艏向角頂著海流，降低了輔助推進(jìn)器的能源消耗，雖然沒有考慮姿態(tài)角的控制要求，但是能耗的降低具有實際意義。文獻(xiàn)[13]提出了一種基于Lyapunov的模型預(yù)測控制(LMPC)方法，通過推力的優(yōu)化策略，實現(xiàn)了自主水下航行器的動態(tài)定位控制。近年來，一些學(xué)者分別采用被動機(jī)械臂(Passive Arm)[14]和視覺系統(tǒng)(Visual System)[15]獲得相對位置信息，但由于一些限制，無法獲得遠(yuǎn)距離的位置。文獻(xiàn)[16]提出了一種基于逆USBL系統(tǒng)的AUV水下動力定位控制方案，將多個USBL應(yīng)答器固定在工作區(qū)域，同時將收發(fā)器安裝在AUV上，避免了水聲信號的遠(yuǎn)距離傳播，可以獲得精確的全局位置信息，從而實現(xiàn)動力定位控制。

目前存在的動力定位智能控制算法主要應(yīng)用在船舶和大型海洋平臺[17]，對于小型水下機(jī)器人的研究動力定位極為有限。自主水下機(jī)器人結(jié)構(gòu)小、建模復(fù)雜、模型參數(shù)易發(fā)生漂移，對控制器的設(shè)計提出了更高的要求。文中重點研究了在動力定位過程中如何克服未知定常海流、外部無法估計測量干擾以及實際作業(yè)時存在的參數(shù)漂移問題，設(shè)計控制器并且證明了在未知干擾下良好的定位能力。

1 海流環(huán)境中AUV數(shù)學(xué)模型

海流是海水大范圍大規(guī)模相對穩(wěn)定的非周期性流動，目前還沒有精確描述其運動規(guī)律的模型，海流的變化一般發(fā)生在大尺度的時間和空間變化之下。對于水下作業(yè)的機(jī)器人來說，時間和空間都處于特定的情況，一般可以認(rèn)為海流流速和流向都是穩(wěn)定的，故海流的干擾可以看成是未知定常干擾。

本文研究對象是裝有側(cè)向輔助推進(jìn)器和垂向輔助推進(jìn)器的六自由度全驅(qū)動AUV，海洋環(huán)境中六自由度運動坐標(biāo)系如圖1所示。

圖1 運動坐標(biāo)系

水下機(jī)器人的運動坐標(biāo)系由地面坐標(biāo)系和載體坐標(biāo)系組成，AUV在水下任意位置和姿態(tài)都可以用原點O為中心的地面坐標(biāo)系表示，其中原點O表示定位目標(biāo)點。根據(jù)國際水池會議及造船和輪機(jī)工程學(xué)會推薦的參數(shù)符號[18]來描述水下機(jī)器人的運動，具體變量含義如表1所示。

表1 6DOF運動參數(shù)

可用如下方程描述AUV運動動力學(xué)特性：

(1)

CAM(vr)vr+D(vr)vr+g(η)=τ+τenv

(2)

式中，η表示地面坐標(biāo)系下AUV位置和姿態(tài)向量；v表示機(jī)器人運動的速度加速度矢量；R表示載體坐標(biāo)系到地面坐標(biāo)系的轉(zhuǎn)換矩陣；MRB為剛體慣性矩陣；CRB是剛體科里奧利向心力矩陣,MAM為流體作用產(chǎn)生的附加質(zhì)量慣性矩陣；CAM是流體作用產(chǎn)生的科里奧利向心力矩陣；D是由水動力產(chǎn)生的阻尼力和力矩矩陣；vr表示AUV在載體坐標(biāo)系下相對流體的速度；vc表示載體坐標(biāo)系下水流的速度矢量；g(η)為回復(fù)力矢量；τ為推進(jìn)器各方向合力與力矩矢量；τenv為外部環(huán)境擾動力矢量。

(3)

g(η)=τ+τenv

(4)

上式中各向量和矩陣的具體表達(dá)如下：

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

D(vr)=-diag{Xu+Xu|u||ur|,Yv+Yv|v||vr|,

Zw+Xw|w||wr|,Kp+Kpp|p|,Mq+Mq|q||q|,

Nr+Nr|r||r|}

(14)

(15)

(16)

其中：W為水下機(jī)器人重力；B為水下機(jī)器人浮力；(xB,yB,zB)為水下機(jī)器人載體浮心坐標(biāo)；c、s和t分別是cos、sin和tan的簡寫。

本文的控制目標(biāo)是選擇控制力和力矩τ使AUV從任意初始狀態(tài)η0平穩(wěn)精確到達(dá)期望的位姿ηd并保持，從而保證作業(yè)任務(wù)過程中的穩(wěn)定性。

2 控制器設(shè)計

復(fù)雜環(huán)境下的AUV控制同時存在模型參數(shù)不確定性、傳感器測量誤差以及外界環(huán)境干擾，傳統(tǒng)單一的控制方法難以取得良好的效果。完整的動力定位系統(tǒng)由控制器單元和推力分配單元組成，其工作原理如下：首先，位置傳感器、姿態(tài)傳感器、海流傳感器等開始實時測量數(shù)據(jù)，將這些數(shù)據(jù)通過非線性觀測器實時估計位置信息和水流信息；然后，將這些信息輸入計算設(shè)備中，與預(yù)定的位置姿態(tài)進(jìn)行比照，由自適應(yīng)滑?？刂破饔嬎愕玫剿璧耐屏土?；最后，生成推力指令，由推力分配單元調(diào)整各個方向推進(jìn)器的推力進(jìn)行推力的一次修正。如此不斷地調(diào)整推力指令，進(jìn)行差別修正，完成最終的動力定位。一般來說，只需幾分鐘的時間就可以穩(wěn)定停在預(yù)定位置，并且面對突然的外界干擾可以實時計算出恢復(fù)力和力矩，保持系統(tǒng)穩(wěn)定。整個系統(tǒng)組成如圖2所示。

圖2 動力定位控制系統(tǒng)組成

2.1 滑模控制器設(shè)計

水下機(jī)器人具有強(qiáng)耦合和非線性的特點，而且考慮到時變運動，環(huán)境復(fù)雜不確定，難以建立精確的運動模型。在機(jī)器人的各種控制算法中，滑模變結(jié)構(gòu)控制的滑動模態(tài)對于參數(shù)變化以及系統(tǒng)未建模的部分具有很強(qiáng)的魯棒性，這使得滑?？刂品椒ǔ蔀榭刂扑聶C(jī)器人的有效方法之一。

滑模變結(jié)構(gòu)控制是一種特殊的非線性控制方法，表現(xiàn)為控制的不連續(xù)性，其特殊之處表現(xiàn)為結(jié)構(gòu)的不固定，可以根據(jù)系統(tǒng)當(dāng)前狀態(tài)不斷變化，使系統(tǒng)到達(dá)預(yù)定滑動模態(tài)。滑動模態(tài)的存在使這種方法具有快速響應(yīng)、對參數(shù)變化和擾動不敏感、設(shè)計方法簡單等特點。AUV系統(tǒng)是一個典型的非線性2階系統(tǒng)，下面將利用反步法設(shè)計滑模控制器。

第1步：

定義地球坐標(biāo)系下位置姿態(tài)跟蹤誤差z1，其中η為當(dāng)前的位姿信息，ηd為期望位置姿態(tài)矢量，有：

z1=η-ηd

(17)

選取第一個Lyapunov函數(shù)：

(18)

對V1求導(dǎo)并代入運動學(xué)方程公式(3)可得：

(19)

設(shè)載體坐標(biāo)系下AUV相對速度vr為虛擬控制量，K1為正定對角矩陣，取穩(wěn)定項α：

α=-R-1(K1z1+vc-ηd)

(20)

第2步：

定義載體坐標(biāo)系下相對速度跟蹤誤差：

z2=vr-α

(21)

選取第2個Lyapunov函數(shù)：

(22)

然后對V2求導(dǎo)并將第一個Lyapunov函數(shù)代入有：

(23)

由于慣性矩陣M為正定對角矩陣，為方便后面計算，取K2=M，代入動力學(xué)方程(4)并結(jié)合式(21)可推導(dǎo)得：

(24)

D(vr)α-τenv

(25)

選取正實數(shù)對角參數(shù)矩陣Λ，設(shè)計滑模切換控制率：

τst=-Λsign(s)

(26)

考慮到滑?？刂品椒ù嬖诘亩墩駟栴}，利用邊界層處理方法，設(shè)置邊界厚度為0.02，sat(s)是經(jīng)過處理后的切換控制，則最后設(shè)計的控制率為：

τ=τeq+τst=-K3z2-RTz1+

(27)

以上的控制率設(shè)計均假設(shè)AUV模型中的參數(shù)已知且不變，但在實際系統(tǒng)中，參數(shù)往往會隨著時間發(fā)生一定的偏移，雖然滑模變結(jié)構(gòu)控制方法具有一定的魯棒性，但還是會對動力定位的結(jié)果產(chǎn)生一定影響，如何消除參數(shù)攝動的影響需要對控制器進(jìn)一步設(shè)計。

2.2 自適應(yīng)參數(shù)估計器設(shè)計

基于物理機(jī)理建立的動力學(xué)模型參數(shù)很難精確獲取[19]，為實現(xiàn)高精度系統(tǒng)建模，需要通過在線估計系統(tǒng)未知參數(shù)以降低模型不確定性對建模的影響。以下將利用自適應(yīng)參數(shù)估計方法研究參數(shù)在不確定情況下的在線參數(shù)估計問題。首先，對AUV動力學(xué)模型做如下假設(shè)：

假設(shè)1：AUV模型不確定項一致有界；

假設(shè)2：模型參數(shù)具有線性化參數(shù)形式,即滿足如下關(guān)系式：

(28)

為了得到參數(shù)估計的自適應(yīng)率取第3個Lyapunov函數(shù)：

(29)

(30)

為了消除估計誤差，設(shè)計如下參數(shù)自適應(yīng)估計率使上式負(fù)定或者負(fù)半定，γ表示參數(shù)估計率：

(31)

結(jié)合2.1中設(shè)計的滑模控制器，得到如下模型參數(shù)自適應(yīng)滑模控制率：

τ=-K3z2-RTz1-Λsat(s)+

(32)

2.3 水流速度觀測器設(shè)計

一般而言，AUV工作環(huán)境的水流并不是靜止的，而自身攜帶的傳感設(shè)備很難測到AUV的絕對速度，此時必須考慮模型中未知的水流速度。下面將單獨設(shè)計水流速度觀測器用于在線估計水流速度，以便在控制通道中消除影響。

根據(jù)參考文獻(xiàn)[20]設(shè)計如下狀態(tài)觀測器：

(33)

(34)

結(jié)合上面2個公式可得到觀測器的誤差狀態(tài)方程：

(35)

2.4 穩(wěn)定性分析

定理1 對于由式(3)、式(4)描述的六自由度AUV，給定合適的控制參數(shù)矩陣K1、K2、K3、Λ，參數(shù)學(xué)習(xí)率γ以及增益矩陣A和B，在控制率式(32)、參數(shù)自適應(yīng)率式(31)和海流觀測器式(33)的共同作用下，從任意初始狀態(tài)η0出發(fā)，能夠在有限時間內(nèi)到達(dá)預(yù)設(shè)狀態(tài)ηd，且誤差全局漸進(jìn)穩(wěn)定。

證明首先構(gòu)造一個適合的李雅普諾夫函數(shù)：

V(t) =V1(t) +V2(t) +V3(t) +V4(t)=

(36)

(37)

在2.1節(jié)的設(shè)計中已經(jīng)保證式(37)前3項是全局漸進(jìn)穩(wěn)定的。下面證明加入海流觀測器后系統(tǒng)的全局穩(wěn)定性，取矩陣C，有

(38)

設(shè)Q為任意正定矩陣，取矩陣P使如下關(guān)系式成立：

CTP+PC=-Q

(39)

對第4項李雅普諾夫函數(shù)求導(dǎo)可得：

(40)

代入式(37)最終得：

(41)

3 算例仿真

為了闡述本文提出的方法的有效性和實用性，采用某六自由度全驅(qū)動AUV為控制對象進(jìn)行仿真，各自由度方向標(biāo)稱參數(shù)如下：

m=117,Ixx=10.7,Iyy=11.8,Izz=13.4

Xu=-120,Yv=-90,Zw=-150

Kp=-15,Mq=-15,Nr=-18

Xu|u|=-90,Yv|v|=-90,Zw|w|=-120

Kp|p|=-10,Mq|q|=-12,Nr|r|=-15

動力定位初始狀態(tài)為：

動力定位期望位姿狀態(tài)為：

地面坐標(biāo)系下海流速度為：

海流觀測器初始值為：

控制器中矩陣參數(shù)選擇如下：

K1=0.2I,K2=MI,K3=I

γ=0.1,Λ=20I,A=B=I

利用Matlab編寫S函數(shù)在Similink中進(jìn)行仿真，仿真時間設(shè)為100 s，在第40 s時對AUV的縱向和側(cè)向施加大小分別為50、100 N，持續(xù)時間3.5 s的干擾力。定位過程中水下機(jī)器人位置和姿態(tài)變化曲線見圖3；自適應(yīng)滑?？刂破鞯牧杂啥韧屏土剌敵鲆妶D4；海流觀測器的水流速度估計曲線見圖5，穩(wěn)定狀態(tài)下的定位誤差見表2。

圖3 位置姿態(tài)變化曲線

圖4 推力和力矩輸出曲線

圖5 海流速度分量估計曲線

xyzφθΨ0.020.020.0110-45×10-50.003

以上仿真顯示，海流觀測器在10 s之內(nèi)準(zhǔn)確估計出海流速度；在推力指令的控制之下水下機(jī)器人30 s左右到達(dá)了預(yù)定狀態(tài)，整個過程平緩無超調(diào)；40 s時在突變干擾下迅速調(diào)整位姿保持穩(wěn)定；穩(wěn)定之后仍然繼續(xù)產(chǎn)生推力指令，此時已到達(dá)目標(biāo)位置，推力主要用于抵消海流的影響，保證AUV作業(yè)時的穩(wěn)定性；在動力過程中由于回復(fù)力的存在和各自由度之間的耦合作用，各個姿態(tài)角都會發(fā)生一定的變化，但在控制器的作用下逐漸穩(wěn)定?？梢钥闯觯何闹刑岢龅淖赃m應(yīng)滑模算法具有很快的響應(yīng)速度；動力定位過程中位置定位誤差小于2 cm，姿態(tài)角定位誤差小于0.003 rad，精度高，滿足作業(yè)需求；并且可以從突變干擾中快速恢復(fù)穩(wěn)定，整個動力定位系統(tǒng)具有良好的穩(wěn)態(tài)性能和較好的魯棒性。

4 結(jié)束語

本文主要研究在海流干擾下作業(yè)型自主水下機(jī)器人的動力定位問題。根據(jù)AUV在水下作業(yè)的精度和魯棒性的要求，綜合考慮了水流的干擾和作業(yè)設(shè)備引起的模型參數(shù)變化，提出了基于海流速度觀測器的自適應(yīng)滑?？刂品椒ǎ煤Ａ魉俣扔^測器在線估計并在控制通道中進(jìn)行補(bǔ)償，通過自適應(yīng)方法和滑模控制理論設(shè)計參數(shù)自適應(yīng)率和推力控制率，克服AUV作業(yè)過程中的參數(shù)變化對系統(tǒng)穩(wěn)定性的影響，理論證明系統(tǒng)的全局穩(wěn)定性。最后的仿真結(jié)果顯示：此動力定位控制系統(tǒng)具有良好的穩(wěn)定性和極小的定位誤差，說明控制器的設(shè)計是合理的，可以考慮將此方法應(yīng)用于作業(yè)型自主水下機(jī)器人。