基于自抗擾控制的小型ROV姿態與深度控制研究

趙興隆，商 蕾，喬玉蓬

(1.武漢理工大學，湖北 武漢 430063；2.杭州海康威視數字技術股份有限公司，浙江 杭州 310051)

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

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

針對上述問題，在分析了目前ROV位置與姿態控制研究中的主要算法后，基于自抗擾控制算法(Active Disturbance Rejection Control，ADRC)不依賴精確模型，能對擾動進行觀測和補償等特點，以小型六推進式水下航行器為研究對象，使用自抗擾控制技術來實現小型水下航行器姿態與深度控制。

1 ROV數學模型

根據國際水池會議(ITTC)和造船與輪機工程學會(SNAME)術語公報推薦的體系，建立ROV的慣性坐標系E-ξηζ和載體坐標系O-xyz，ROV的2種坐標系如圖1所示，慣性坐標系為固定不變的坐標系，載體坐標系為隨ROV一起運動的坐標系，兩者之間可以通過轉換矩陣J(η)互相轉換。

圖1 ROV的2種坐標系

在建立ROV的數學模型時，需引進下列假設條件：假設1，ROV具有3個近似的對稱面；假設2，ROV的載體坐標系原點與重心原點重合。

根據文獻[5]，ROV的六自由度非線性動力學模型方程可表示為：

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

η=J(η)v,

(1)

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

J(η)為坐標轉換矩陣，可表示為：

(2)

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

ROV采用6個推進器進行控制，能進行5個自由度的運動控制，推進器布置圖如圖2所示。

圖2 推進器布置圖

推進器的推力Fi表示為：

(3)

式中，ρ為水的密度；n為螺旋槳轉速；D為螺旋槳直徑；K為推進器推力系數；i為推進器編號。

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

根據文獻[5]所述方法，ROV的模型為：

(4)

在該方程中，耦合度很高的姿態運動方程和潛浮方向運動方程變成了3個相互獨立的方程，這樣就可以分別設計姿態角2個自由度上的姿態控制器和深度控制器。

2 基于自抗擾的ROV姿態與深度控制

自抗擾控制方法主要由跟蹤微分控制器(TD)、擴張狀態觀測器(ESO)、誤差的非線性反饋(NLSEF)以及擾動估計補償等部分組成[7]。將自抗擾控制用于ROV的姿態與深度控制系統中，基于ADRC的ROV姿態與深度控制系統結構框圖如圖3所示。圖3中，φ、Ψ、z分別為橫傾信息、偏航信息和深度信息；u1、u2、u3分別為所受合力；F1、F2、F3、F4、F5為相應推進器推力輸出。

圖3 基于ADRC的ROV姿態與深度控制系統結構框圖

根據自抗擾控制特點，可將式(4)中關于姿態和深度的方程改寫為[8]：

(5)

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

安排過渡過程：

(6)

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

擴張狀態觀測器[9]：

， (7)

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

(8)

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

計算初始控制量：

(9)

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

補償擾動：

(10)

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

3 仿真分析與試驗驗證

3.1 仿真與分析

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

表1 自抗擾控制器參數表

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

圖4 偏航角仿真結果

圖5 橫傾角仿真結果

圖6 深度仿真結果

從圖4可知，ADRC跟蹤響應時間比PID控制慢了大約0.25 s，但是PID出現了超調，在出現階躍擾動的情況下，PID偏離設定角更是達到了30.2%，而ADRC控制偏離設定值只有大約5%。說明了ADRC的抗干擾能力明顯優于PID控制器。

3.2 試驗驗證

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

圖7 偏航角跟蹤控制

圖8 橫傾角跟蹤控制

圖9 深度跟蹤控制

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

綜上所述，ADRC控制器在對ROV進行跟蹤控制時，響應速度稍弱于PID控制器，但是在抑制超調和振蕩現象上明顯優于PID控制器。

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

圖10 偏航角抗干擾對比

圖11 橫傾角抗干擾對比

圖12 深度抗干擾對比

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

綜上所述，自抗擾控制器在小型ROV的運動控制領域的整體性能要優于PID控制器，尤其是在抗干擾能力方面，比PID控制器有明顯的提升。在進行ROV跟蹤控制時，可以避免超調現象的發生，減少系統的振蕩。

4 結束語

本文將ADRC算法與PID算法進行對比分析，驗證ADRC算法在ROV位置與姿態控制中的優越性。最后通過實際的水池試驗，驗證了ADRC算法在實際應用中比PID控制的抗干擾性更強。