謝爭明,曾慶軍,朱志宇,周啟潤,馬洪潮

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

自主水下機(jī)器人(autonomous underwater vehicle,AUV)動(dòng)力定位控制是利用推進(jìn)系統(tǒng)克服浪流等擾動(dòng),使AUV以指定姿態(tài)和位置保持在基準(zhǔn)點(diǎn)或基準(zhǔn)線附近[1]．隨著海洋探索工具的發(fā)展和資源探索的深入,AUV工作愈復(fù)雜多樣[2-3],包括水下懸停、定速巡航、航跡保持等,其控制要求也隨之提高．全驅(qū)動(dòng)AUV使用多個(gè)推進(jìn)器提供推力,能夠有效提升AUV的可操作性及抗干擾能力,確保更高的準(zhǔn)確性及穩(wěn)定性[4]．Liu等[5]設(shè)計(jì)了一種利用虛擬閉環(huán)系統(tǒng)提供虛擬參考軌跡的控制策略,解決了AUV跟蹤控制的初始誤差問題,但未進(jìn)行湖上試驗(yàn),對實(shí)際擾動(dòng)考慮不足;Guerrero等[6]基于一組軌跡跟蹤的飽和函數(shù)設(shè)計(jì)非線性PID控制器,在控制系統(tǒng)參數(shù)變化和洋流擾動(dòng)影響下完成了深度和航向角控制試驗(yàn),結(jié)果表明在目標(biāo)航向角改變時(shí),AUV控制曲線會(huì)產(chǎn)生較大超調(diào)量．AUV因設(shè)備條件限制,推進(jìn)系統(tǒng)的輸出常常受到約束．Rashidi等[7]將拉蓋爾函數(shù)與預(yù)測控制結(jié)合,完成約束條件下的仿真試驗(yàn),能大幅提高計(jì)算效率,但對于深度和航向角的跟蹤存在一定的滯后性;Campos等[8]基于具有可變參數(shù)的飽和函數(shù),設(shè)計(jì)了一種用于AUV定點(diǎn)校準(zhǔn)及軌跡跟蹤控制的非線性控制器,能夠提高參數(shù)不確定系統(tǒng)的魯棒性,但該控制器僅利用PD控制,未能實(shí)現(xiàn)積分環(huán)節(jié),系統(tǒng)穩(wěn)態(tài)性不佳．本文以“探海I型”AUV為研究對象,擬建立六自由度空間運(yùn)動(dòng)模型,分析AUV推進(jìn)系統(tǒng)與隨機(jī)浪流擾動(dòng)模型,設(shè)計(jì)基于擴(kuò)張狀態(tài)觀測器(extended state observer,ESO)的AUV動(dòng)力定位滑?？刂破?sliding mode control,SMC),結(jié)合AUV結(jié)構(gòu)特點(diǎn)及推進(jìn)器分配位置,將動(dòng)力定位控制分為垂直面與水平面兩部分,分別通過滑模控制器計(jì)算控制要求的推力與轉(zhuǎn)矩,同時(shí)利用ESO對AUV系統(tǒng)模型偏差和浪流等干擾進(jìn)行補(bǔ)償,以期AUV能夠根據(jù)指定作業(yè)需求動(dòng)態(tài)保持在某一位置或軌跡．

1 全驅(qū)動(dòng)AUV運(yùn)動(dòng)模型

圖1為“探海I型”AUV實(shí)體樣機(jī),其外形采用魚雷設(shè)計(jì),AUV的橫向與縱向各有2個(gè)輔助推進(jìn)器,負(fù)責(zé)控制AUV姿態(tài);機(jī)器人艉部裝載1個(gè)主推,用來提供AUV前進(jìn)的推力,可通過調(diào)節(jié)艉部X型舵的角度以平衡附加轉(zhuǎn)矩[9]．AUV須多次調(diào)試以提高控制系統(tǒng)在實(shí)際運(yùn)行中的穩(wěn)定性,考慮到試驗(yàn)成本較高,可通過仿真測試控制器及算法的可行性,從而提高試驗(yàn)效率并降低成本[10],本文通過Fluent軟件辨識仿真試驗(yàn)中的水動(dòng)力參數(shù)．

圖1 “探海I型”AUVFig.1 “T-SEA I”AUV

圖2為設(shè)計(jì)AUV仿真及運(yùn)動(dòng)模型時(shí)建立的兩個(gè)坐標(biāo)系,O0x0y0z0為大地坐標(biāo)系,Oxyz為AUV運(yùn)動(dòng)坐標(biāo)系．其中φ,θ,ψ分別為橫傾角、縱傾角、航向角;X,Y,Z分別為AUV在x,y,z軸的合力[11];K,M,N分別為AUV圍繞x,y,z軸的合力矩;u,v,w分別為前進(jìn)速度、橫向速度、垂向速度;p,q,r分別為橫傾角速度、縱傾角速度、航向角速度[12]．

圖2 坐標(biāo)系Fig.2 Coordinate system

“探海I型”AUV簡化運(yùn)動(dòng)模型為

(1)

式中m為AUV質(zhì)量;Ix,Iy,Iz為轉(zhuǎn)動(dòng)慣量;xg,yg,zg分別為x,y,z軸重心坐標(biāo)．AUV受到的合力和合力矩的矩陣為

F=[X,Y,Z,K,M,N]T=FHS+FH+Fprop,

(2)

其中FHS為AUV受到回復(fù)力和力矩的矩陣;Fprop為AUV受到推力和力矩的矩陣;FH為AUV受到水動(dòng)力和力矩的矩陣．水動(dòng)力影響大小通常與運(yùn)動(dòng)的速度和加速度有關(guān)[13],可表示為FH=[XH,YH,ZH,KH,MH,NH]T,式中XH,YH,ZH分別為x,y,z軸方向的水動(dòng)力;KH,MH,NH分別為水動(dòng)力圍繞x,y,z軸作用的力矩．回復(fù)力為重力及浮力對AUV的作用,依照實(shí)際參數(shù)化簡為

(3)

式中W為重力;B為浮力;xb,yb,zb為浮心坐標(biāo)．

在對AUV進(jìn)行運(yùn)動(dòng)分析時(shí),應(yīng)計(jì)算AUV與流體的相對速度,計(jì)算公式為

(4)

式中uA,vA,wA為相對速度;uc,vc,wc為運(yùn)動(dòng)坐標(biāo)系下的水流速度．設(shè)定水流干擾為零均值水流干擾,若水流包含初始速度V0和水流方向ψ0,則

(5)

式中Vc為水流速度,ψc為水流方向,ω為高斯白噪聲,μ1,μ2為非負(fù)常數(shù)．利用坐標(biāo)變換矩陣Rn計(jì)算運(yùn)動(dòng)坐標(biāo)系下水流

(6)

(7)

式中Xprop,Yprop,Zprop分別為x,y,z軸方向的推力;Kprop,Mprop,Nprop分別為推力圍繞x,y,z軸作用的力矩;P1為主推進(jìn)器推力,P2和P5為側(cè)向推進(jìn)器推力,P3和P4為垂向推進(jìn)器推力;L2,L3,L4,L5為推力的力臂;Q1,Q2,Q3,Q4,Q5為轉(zhuǎn)矩．

2 基于ESO的AUV動(dòng)力定位滑?？刂?/h2>

本文將AUV運(yùn)動(dòng)分解為垂直面與水平面的運(yùn)動(dòng),并將式(1)化簡為垂直面和水平面方程

(8)

(9)

2.1 擴(kuò)張狀態(tài)觀測器設(shè)計(jì)

將式(8)和式(9)化簡為

(10)

式中f1(w,q,u),f2(w,q,θ,u),f3(u,v,r)為非線性函數(shù);Bi為控制增益;Ut,i為控制量．將狀態(tài)量w,q,u,v,r,θ,ζ擴(kuò)張成二階形式

(11)

式中Z1為狀態(tài)估計(jì)量;Z2為擴(kuò)張狀態(tài);h1,h2為增益矢量;e1為狀態(tài)誤差;α為常數(shù)矩陣;δ為濾波因子矩陣;fal(e1,α,δ)為函數(shù)濾波器[14],能夠較好處理AUV等非線性系統(tǒng)的噪聲干擾問題．

2.2 滑?？刂破髟O(shè)計(jì)

將式(10)改寫為

(12)

(13)

(14)

3 AUV動(dòng)力定位仿真

為驗(yàn)證本文設(shè)計(jì)的控制器對動(dòng)力定位的控制效果,在仿真中引入浪流等隨機(jī)干擾,利用式(5)和式(6)計(jì)算干擾大小及方向．初始深度為0 m,設(shè)定目標(biāo)深度8 m,目標(biāo)航向角100°,目標(biāo)橫傾角及縱傾角為0°,在相同工況下設(shè)計(jì)動(dòng)力定位試驗(yàn),對比不同控制器的性能差異,仿真結(jié)果如圖3和圖4所示．由圖3可知,引入隨機(jī)浪流等擾動(dòng)后,PID動(dòng)力定位控制超調(diào)較大且易波動(dòng),在受到較大干擾時(shí)需要較長時(shí)間調(diào)整至期望狀態(tài);ESO滑?？刂瞥{(diào)較小,能夠在較短時(shí)間內(nèi)調(diào)整至期望值并保持穩(wěn)定．由圖4可知,利用PID進(jìn)行動(dòng)力定位姿態(tài)角控制時(shí)收斂較慢,其中航向角在穩(wěn)態(tài)時(shí)依然存在一定的誤差;而ESO滑模控制在姿態(tài)角控制時(shí),響應(yīng)速度較快且能夠較好地抑制擾動(dòng),快速平穩(wěn)回到期望狀態(tài)．

圖3 深度控制對比圖Fig.3 Comparison diagram of depth control

圖4 動(dòng)力定位姿態(tài)角控制對比圖Fig.4 Comparison diagram of attitude angle control

4 湖上試驗(yàn)

AUV湖上試驗(yàn)于2020年2月在某湖完成,如圖5所示．湖深5 m以上,水流速度小于2 m·s-1,水下環(huán)境較好,設(shè)計(jì)定深、定航動(dòng)力定位控制任務(wù):目標(biāo)深度2 m,目標(biāo)航向角200°,控制時(shí)長5 min．圖6～8分別為AUV湖上試驗(yàn)的三維軌跡、深度及姿態(tài)角變化情況．由圖6～8可知,設(shè)定任務(wù)后,AUV能夠快速到達(dá)指定動(dòng)力定位控制目標(biāo)深度及航向,在100 s左右AUV航向角因擾動(dòng)出現(xiàn)波動(dòng),但能夠迅速回到穩(wěn)定狀態(tài),并控制深度誤差在±0.1 m以內(nèi),航向角誤差在±2°以內(nèi),達(dá)到指定控制目標(biāo),能夠應(yīng)用在工程項(xiàng)目中．

圖5 AUV湖上試驗(yàn)Fig.5 AUV experiments on the lake

圖6 動(dòng)力定位三維軌跡Fig.6 3D trajectories for dynamic positioning

圖7 深度控制Fig.7 Depth control

圖8 姿態(tài)角控制Fig.8 Attitude Angle control