海流環境自主水下機器人動力定位控制器設計

竇 剛，姜文剛，張楠楠

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

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

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

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

1 海流環境中AUV數學模型

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

本文研究對象是裝有側向輔助推進器和垂向輔助推進器的六自由度全驅動AUV，海洋環境中六自由度運動坐標系如圖1所示。

圖1 運動坐標系

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

表1 6DOF運動參數

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

(1)

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

(2)

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

(3)

g(η)=τ+τenv

(4)

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

(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為水下機器人重力；B為水下機器人浮力；(xB,yB,zB)為水下機器人載體浮心坐標；c、s和t分別是cos、sin和tan的簡寫。

本文的控制目標是選擇控制力和力矩τ使AUV從任意初始狀態η0平穩精確到達期望的位姿ηd并保持，從而保證作業任務過程中的穩定性。

2 控制器設計

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

圖2 動力定位控制系統組成

2.1 滑模控制器設計

水下機器人具有強耦合和非線性的特點，而且考慮到時變運動，環境復雜不確定，難以建立精確的運動模型。在機器人的各種控制算法中，滑模變結構控制的滑動模態對于參數變化以及系統未建模的部分具有很強的魯棒性，這使得滑模控制方法成為控制水下機器人的有效方法之一。

滑模變結構控制是一種特殊的非線性控制方法，表現為控制的不連續性，其特殊之處表現為結構的不固定，可以根據系統當前狀態不斷變化，使系統到達預定滑動模態。滑動模態的存在使這種方法具有快速響應、對參數變化和擾動不敏感、設計方法簡單等特點。AUV系統是一個典型的非線性2階系統，下面將利用反步法設計滑模控制器。

第1步：

定義地球坐標系下位置姿態跟蹤誤差z1，其中η為當前的位姿信息，ηd為期望位置姿態矢量，有：

z1=η-ηd

(17)

選取第一個Lyapunov函數：

(18)

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

(19)

設載體坐標系下AUV相對速度vr為虛擬控制量，K1為正定對角矩陣，取穩定項α：

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

(20)

第2步：

定義載體坐標系下相對速度跟蹤誤差：

z2=vr-α

(21)

選取第2個Lyapunov函數：

(22)

然后對V2求導并將第一個Lyapunov函數代入有：

(23)

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

(24)

D(vr)α-τenv

(25)

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

τst=-Λsign(s)

(26)

考慮到滑模控制方法存在的抖振問題，利用邊界層處理方法，設置邊界厚度為0.02，sat(s)是經過處理后的切換控制，則最后設計的控制率為：

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

(27)

以上的控制率設計均假設AUV模型中的參數已知且不變，但在實際系統中，參數往往會隨著時間發生一定的偏移，雖然滑模變結構控制方法具有一定的魯棒性，但還是會對動力定位的結果產生一定影響，如何消除參數攝動的影響需要對控制器進一步設計。

2.2 自適應參數估計器設計

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

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

假設2：模型參數具有線性化參數形式,即滿足如下關系式：

(28)

為了得到參數估計的自適應率取第3個Lyapunov函數：

(29)

(30)

為了消除估計誤差，設計如下參數自適應估計率使上式負定或者負半定，γ表示參數估計率：

(31)

結合2.1中設計的滑模控制器，得到如下模型參數自適應滑模控制率：

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

(32)

2.3 水流速度觀測器設計

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

根據參考文獻[20]設計如下狀態觀測器：

(33)

(34)

結合上面2個公式可得到觀測器的誤差狀態方程：

(35)

2.4 穩定性分析

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

證明首先構造一個適合的李雅普諾夫函數：

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

(36)

(37)

在2.1節的設計中已經保證式(37)前3項是全局漸進穩定的。下面證明加入海流觀測器后系統的全局穩定性，取矩陣C，有

(38)

設Q為任意正定矩陣，取矩陣P使如下關系式成立：

CTP+PC=-Q

(39)

對第4項李雅普諾夫函數求導可得：

(40)

代入式(37)最終得：

(41)

3 算例仿真

為了闡述本文提出的方法的有效性和實用性，采用某六自由度全驅動AUV為控制對象進行仿真，各自由度方向標稱參數如下：

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

動力定位初始狀態為：

動力定位期望位姿狀態為：

地面坐標系下海流速度為：

海流觀測器初始值為：

控制器中矩陣參數選擇如下：

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

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

利用Matlab編寫S函數在Similink中進行仿真，仿真時間設為100 s，在第40 s時對AUV的縱向和側向施加大小分別為50、100 N，持續時間3.5 s的干擾力。定位過程中水下機器人位置和姿態變化曲線見圖3；自適應滑模控制器的六自由度推力和力矩輸出見圖4；海流觀測器的水流速度估計曲線見圖5，穩定狀態下的定位誤差見表2。

圖3 位置姿態變化曲線

圖4 推力和力矩輸出曲線

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

xyzφθΨ0.020.020.0110-45×10-50.003

以上仿真顯示，海流觀測器在10 s之內準確估計出海流速度；在推力指令的控制之下水下機器人30 s左右到達了預定狀態，整個過程平緩無超調；40 s時在突變干擾下迅速調整位姿保持穩定；穩定之后仍然繼續產生推力指令，此時已到達目標位置，推力主要用于抵消海流的影響，保證AUV作業時的穩定性；在動力過程中由于回復力的存在和各自由度之間的耦合作用，各個姿態角都會發生一定的變化，但在控制器的作用下逐漸穩定。可以看出：文中提出的自適應滑模算法具有很快的響應速度；動力定位過程中位置定位誤差小于2 cm，姿態角定位誤差小于0.003 rad，精度高，滿足作業需求；并且可以從突變干擾中快速恢復穩定，整個動力定位系統具有良好的穩態性能和較好的魯棒性。

4 結束語

本文主要研究在海流干擾下作業型自主水下機器人的動力定位問題。根據AUV在水下作業的精度和魯棒性的要求，綜合考慮了水流的干擾和作業設備引起的模型參數變化，提出了基于海流速度觀測器的自適應滑模控制方法，利用海流速度觀測器在線估計并在控制通道中進行補償，通過自適應方法和滑模控制理論設計參數自適應率和推力控制率，克服AUV作業過程中的參數變化對系統穩定性的影響，理論證明系統的全局穩定性。最后的仿真結果顯示：此動力定位控制系統具有良好的穩定性和極小的定位誤差，說明控制器的設計是合理的，可以考慮將此方法應用于作業型自主水下機器人。