基于自抗擾控制的小型ROV姿態(tài)與深度控制研究

趙興隆，商 蕾，喬玉蓬

(1.武漢理工大學(xué)，湖北 武漢 430063；2.杭州?？低晹?shù)字技術(shù)股份有限公司，浙江 杭州 310051)

隨著水下資源開發(fā)以及水下探測與作業(yè)的不斷發(fā)展，水下航行器得到了廣泛的應(yīng)用，小型有纜式水下航行器(ROV)因具有體積小、比較靈活、成本較低等特點(diǎn)，也越來越受到關(guān)注。尤其是在淺水水域的探測和作業(yè)任務(wù)中，小型ROV具有很高的應(yīng)用價(jià)值。但由于ROV具有非線性、強(qiáng)耦合和狀態(tài)多變等特性，使得ROV的運(yùn)動(dòng)控制成為一個(gè)必須要解決的難題。

水下航行器的運(yùn)動(dòng)控制方法大體有：PID控制、滑模控制、自適應(yīng)控制、神經(jīng)網(wǎng)絡(luò)、反步法、H∞控制、反饋線性法等方法。B Jalving將PID控制器應(yīng)用到Norwegian Defencen Research Establishment研究的水下機(jī)器人上，對水下航行器進(jìn)行航向、升降和航速控制，并進(jìn)行了海洋測試。Popovich，Nenad等定義了非線性潛艇深度控制系統(tǒng)模型并對其進(jìn)行了穩(wěn)定性分析，并用階躍輸入和斜坡輸入函數(shù)進(jìn)行了試驗(yàn)。但對于水下機(jī)器人這種非線性系統(tǒng)來說，需要在平衡點(diǎn)位置線性化后才可以使用PID進(jìn)行控制，所以PID不能滿足高標(biāo)準(zhǔn)的控制要求，控制精度較差。范士波[1]采用反步法設(shè)計(jì)了ROV的運(yùn)動(dòng)控制器，試驗(yàn)結(jié)果說明了反步法在控制穩(wěn)定性方面優(yōu)于PID控制方法。霍星星等[2]根據(jù)參數(shù)發(fā)生變化以及有其他未知干擾對深海ROV控制的影響，設(shè)計(jì)出了基于模糊補(bǔ)償?shù)腞OV自適應(yīng)位置與姿態(tài)控制器，實(shí)驗(yàn)結(jié)果顯示其具有良好的跟蹤性能、抗擾性能和穩(wěn)健性。俞建成等[3-4]將神經(jīng)網(wǎng)絡(luò)算法與自適應(yīng)算法結(jié)合，應(yīng)用于水下機(jī)器人的控制中，論證了該方法的可行性。但上述方法是基于精確模型的控制，實(shí)際工程中往往不能建立很準(zhǔn)確的數(shù)學(xué)模型，而且外界環(huán)境的實(shí)時(shí)變化也使得控制效果大打折扣。

針對上述問題，在分析了目前ROV位置與姿態(tài)控制研究中的主要算法后，基于自抗擾控制算法(Active Disturbance Rejection Control，ADRC)不依賴精確模型，能對擾動(dòng)進(jìn)行觀測和補(bǔ)償?shù)忍攸c(diǎn)，以小型六推進(jìn)式水下航行器為研究對象，使用自抗擾控制技術(shù)來實(shí)現(xiàn)小型水下航行器姿態(tài)與深度控制。

1 ROV數(shù)學(xué)模型

根據(jù)國際水池會(huì)議(ITTC)和造船與輪機(jī)工程學(xué)會(huì)(SNAME)術(shù)語公報(bào)推薦的體系，建立ROV的慣性坐標(biāo)系E-ξηζ和載體坐標(biāo)系O-xyz，ROV的2種坐標(biāo)系如圖1所示，慣性坐標(biāo)系為固定不變的坐標(biāo)系，載體坐標(biāo)系為隨ROV一起運(yùn)動(dòng)的坐標(biāo)系，兩者之間可以通過轉(zhuǎn)換矩陣J(η)互相轉(zhuǎn)換。

圖1 ROV的2種坐標(biāo)系

在建立ROV的數(shù)學(xué)模型時(shí)，需引進(jìn)下列假設(shè)條件：假設(shè)1，ROV具有3個(gè)近似的對稱面；假設(shè)2，ROV的載體坐標(biāo)系原點(diǎn)與重心原點(diǎn)重合。

根據(jù)文獻(xiàn)[5]，ROV的六自由度非線性動(dòng)力學(xué)模型方程可表示為：

Mv+C(v)v+D(v)v+g(η)=τc+Δf,

η=J(η)v,

(1)

式中，M和C(v)分別為包含附加質(zhì)量在內(nèi)的ROV的慣性矩陣、科氏力和向心力矩陣；D(v)為黏性類水動(dòng)力矩陣；g(η)為靜力產(chǎn)生的力與力矩；τc為推進(jìn)器推力與力矩,τc=[FX,FY,FZ,FK,FM,FN]，其中，F(xiàn)X、FY、FZ為x、y、z3個(gè)坐標(biāo)軸方向上的合力,FK、FM、FN為x、y、z3個(gè)坐標(biāo)軸方向上的合力矩；Δf為外界的干擾力和力矩；η=[x,y,z,φ,θ,ψ]，v=[u,v,w,p,q,r]。

J(η)為坐標(biāo)轉(zhuǎn)換矩陣，可表示為：

(2)

式中，O3×3代表大小為3×3的零矩陣。

ROV采用6個(gè)推進(jìn)器進(jìn)行控制，能進(jìn)行5個(gè)自由度的運(yùn)動(dòng)控制，推進(jìn)器布置圖如圖2所示。

圖2 推進(jìn)器布置圖

推進(jìn)器的推力Fi表示為：

(3)

式中，ρ為水的密度；n為螺旋槳轉(zhuǎn)速；D為螺旋槳直徑；K為推進(jìn)器推力系數(shù)；i為推進(jìn)器編號(hào)。

在圖2中，推進(jìn)器1、2、5為反置螺旋槳，推進(jìn)器3、4、6為正置螺旋槳，水平面四推進(jìn)器的夾角α為45°，使得ROV能保持受力平衡。

根據(jù)文獻(xiàn)[5]所述方法，ROV的模型為：

(4)

在該方程中，耦合度很高的姿態(tài)運(yùn)動(dòng)方程和潛浮方向運(yùn)動(dòng)方程變成了3個(gè)相互獨(dú)立的方程，這樣就可以分別設(shè)計(jì)姿態(tài)角2個(gè)自由度上的姿態(tài)控制器和深度控制器。

2 基于自抗擾的ROV姿態(tài)與深度控制

自抗擾控制方法主要由跟蹤微分控制器(TD)、擴(kuò)張狀態(tài)觀測器(ESO)、誤差的非線性反饋(NLSEF)以及擾動(dòng)估計(jì)補(bǔ)償?shù)炔糠纸M成[7]。將自抗擾控制用于ROV的姿態(tài)與深度控制系統(tǒng)中，基于ADRC的ROV姿態(tài)與深度控制系統(tǒng)結(jié)構(gòu)框圖如圖3所示。圖3中，φ、Ψ、z分別為橫傾信息、偏航信息和深度信息；u1、u2、u3分別為所受合力；F1、F2、F3、F4、F5為相應(yīng)推進(jìn)器推力輸出。

圖3 基于ADRC的ROV姿態(tài)與深度控制系統(tǒng)結(jié)構(gòu)框圖

根據(jù)自抗擾控制特點(diǎn)，可將式(4)中關(guān)于姿態(tài)和深度的方程改寫為[8]：

(5)

在姿態(tài)控制器上，以橫傾信息φ為例，分別設(shè)計(jì)安排過渡過程，擴(kuò)張狀態(tài)觀測器和非線性狀態(tài)反饋等過程，來說明自抗擾控制器的設(shè)計(jì)過程。

安排過渡過程：

(6)

式中，fh為跟蹤微分函數(shù)；vφ(k)為輸入的期望信號(hào)；φ1(k)、φ2(k)為跟蹤微分器的輸出值；s為跟蹤參數(shù)；h為運(yùn)算步長；fhan(φ1，φ2,r,h)為非線性函數(shù)。

擴(kuò)張狀態(tài)觀測器[9]：

， (7)

式中，zφ1、zφ2、zφ3為狀態(tài)變量的觀測值；eφ(k)為橫傾通道上，估計(jì)值與真實(shí)值之間的誤差；βφ1、βφ2、βφ3為擴(kuò)張狀態(tài)觀測器的增益；fal(.)為非線性函數(shù)。

(8)

式中，e為跟蹤誤差；a為函數(shù)fal(.)的冪次；δ為函數(shù)fal(.)的線性區(qū)間大小。

計(jì)算初始控制量：

(9)

式中，uφ0為控制輸出；a1、a2為公式的修正系數(shù)；k1、k2分別是函數(shù)fal(.)的系數(shù)。

補(bǔ)償擾動(dòng)：

(10)

式中，b可以根據(jù)已有的參數(shù)估計(jì)出大致范圍，在設(shè)計(jì)自抗擾控制器時(shí)可采用分離性原理分別設(shè)計(jì)，然后進(jìn)行組合。

3 仿真分析與試驗(yàn)驗(yàn)證

3.1 仿真與分析

為了驗(yàn)證本文所設(shè)計(jì)姿態(tài)與深度控制器的性能，在MATLAB/Simulink中搭建仿真系統(tǒng)，控制器參數(shù)見表1。表1中β1、β2、β3與公式(7)中的βφ1、βφ2、βφ3是同一含義，因公式(7)以橫傾角φ為例，所以表示為βφ1、βφ2、βφ3。

表1 自抗擾控制器參數(shù)表

本節(jié)通過ADRC與PID控制器的對比，比較2種控制器的抗干擾效果。仿真初始時(shí)刻設(shè)定偏航角和橫傾角為π/6 rad，深度設(shè)定為下潛1 m。在第2秒加入持續(xù)1 s的階躍擾動(dòng)，圖4～圖6分別為偏航角、橫傾角和深度的仿真結(jié)果。其中，d為設(shè)定值，ADRC為自抗擾控制下的仿真曲線，PID為PID控制下的仿真曲線。

圖4 偏航角仿真結(jié)果

圖5 橫傾角仿真結(jié)果

圖6 深度仿真結(jié)果

從圖4可知，ADRC跟蹤響應(yīng)時(shí)間比PID控制慢了大約0.25 s，但是PID出現(xiàn)了超調(diào)，在出現(xiàn)階躍擾動(dòng)的情況下，PID偏離設(shè)定角更是達(dá)到了30.2%，而ADRC控制偏離設(shè)定值只有大約5%。說明了ADRC的抗干擾能力明顯優(yōu)于PID控制器。

3.2 試驗(yàn)驗(yàn)證

1)無干擾船池實(shí)驗(yàn)與分析。首先在試驗(yàn)開始階段分別將ROV的偏航角和橫傾角維持在0左右，深度控制為1 m。 在試驗(yàn)開始后將偏航角設(shè)為右偏π/6 rad，ROV深度設(shè)為2 m，橫傾角設(shè)定為左偏π/18 rad。采用經(jīng)過多次參數(shù)調(diào)試后的PID控制器和ADRC控制器，對偏航、橫傾和潛浮自由度分別做3組實(shí)驗(yàn)。偏航角、橫傾角、深度跟蹤控制如圖7～圖9所示，圖7～圖9中每一個(gè)采樣點(diǎn)間隔為0.1 s。

圖7 偏航角跟蹤控制

圖8 橫傾角跟蹤控制

圖9 深度跟蹤控制

由圖7可知，在對偏航角跟蹤控制時(shí)，PID的響應(yīng)速度比ADRC的響應(yīng)速度快0.6 s，但是PID控制器出現(xiàn)了超調(diào)現(xiàn)象，且在采樣點(diǎn)100之后可以看出，PID的振蕩明顯比ADRC控制器要高。從圖8可知，PID進(jìn)行橫傾控制時(shí)，出現(xiàn)的振蕩頻率和峰值也高于ADRC控制器。從圖9可知，在進(jìn)行深度控制時(shí)，PID和ADRC的響應(yīng)速度相差不大，但同樣在PID控制器上出現(xiàn)了超調(diào)現(xiàn)象。

綜上所述，ADRC控制器在對ROV進(jìn)行跟蹤控制時(shí)，響應(yīng)速度稍弱于PID控制器，但是在抑制超調(diào)和振蕩現(xiàn)象上明顯優(yōu)于PID控制器。

2)水流干擾下的船池實(shí)驗(yàn)與分析。設(shè)定偏航角維持在-π/6 rad，橫傾角維持在π/18 rad，下潛深度為1 m，實(shí)驗(yàn)結(jié)果如圖10～圖12所示，偏航角抗干擾對比見圖10，橫傾角抗干擾對比見圖11，深度抗干擾對比見圖12。

圖10 偏航角抗干擾對比

圖11 橫傾角抗干擾對比

圖12 深度抗干擾對比

從圖10可知，在進(jìn)行偏航角實(shí)驗(yàn)時(shí)，在水流擾動(dòng)大小相似的情況下，PID控制器的誤差范圍為±4.27%，自抗擾控制器的誤差范圍為±1.94%，自抗擾控制器的波動(dòng)量明顯小于PID控制器。如圖12所示，在進(jìn)行深度抗干擾實(shí)驗(yàn)時(shí)，PID控制器的波動(dòng)范圍是0.985～1.043 m，而ADRC控制器的波動(dòng)范圍是0.992～1.018 m，振蕩幅度低于PID控制器，在采樣點(diǎn)241之后也可以看出，PID控制器的振蕩幅度明顯高于ADRC控制器。

綜上所述，自抗擾控制器在小型ROV的運(yùn)動(dòng)控制領(lǐng)域的整體性能要優(yōu)于PID控制器，尤其是在抗干擾能力方面，比PID控制器有明顯的提升。在進(jìn)行ROV跟蹤控制時(shí)，可以避免超調(diào)現(xiàn)象的發(fā)生，減少系統(tǒng)的振蕩。

4 結(jié)束語

本文將ADRC算法與PID算法進(jìn)行對比分析，驗(yàn)證ADRC算法在ROV位置與姿態(tài)控制中的優(yōu)越性。最后通過實(shí)際的水池試驗(yàn)，驗(yàn)證了ADRC算法在實(shí)際應(yīng)用中比PID控制的抗干擾性更強(qiáng)。