自主式水下航行器自抗擾控制

陳增強(qiáng)， 宋莞平， 孫明瑋， 孫青林

(1.南開大學(xué) 人工智能學(xué)院， 天津 300350； 2.天津市智能機(jī)器人重點(diǎn)實(shí)驗(yàn)室， 天津 300350)

水下機(jī)器人是海洋資源可持續(xù)開發(fā)利用的重要技術(shù)手段。自主式水下航行器(autonomous underwater vehicle，AUV)是集能源和推進(jìn)技術(shù)、傳感器和信號(hào)處理技術(shù)，通訊和導(dǎo)航技術(shù)、作戰(zhàn)和人工干涉技術(shù)以及自動(dòng)控制技術(shù)為一體的具有標(biāo)準(zhǔn)化、模塊化和自主能力的水下無人平臺(tái)[1]。AUV的水下運(yùn)動(dòng)有6個(gè)自由度，對(duì)AUV進(jìn)行控制操作時(shí)，每個(gè)操作動(dòng)作會(huì)對(duì)其各個(gè)自由度產(chǎn)生不同程度的影響，因此AUV運(yùn)動(dòng)具有強(qiáng)耦合性和嚴(yán)重非線性的特點(diǎn)[2]。于是，使?jié)摵狡饕砸蟮男阅苤笜?biāo)運(yùn)動(dòng)是有實(shí)際意義的。經(jīng)典控制理論、現(xiàn)代控制理論和智能控制理論中的許多控制方法，都已用在AUV的運(yùn)動(dòng)控制問題上[3]。PID控制是主要的AUV控制方法之一，其不依賴于被控對(duì)象模型，是基于誤差信號(hào)的反饋控制。王智學(xué)等[4]通過5個(gè)PID控制器，實(shí)現(xiàn)了在有水動(dòng)力干擾下的AUV五自由度運(yùn)動(dòng)控制。但對(duì)于復(fù)雜的非線性系統(tǒng)僅僅使用常規(guī)PID控制是不夠的。為了增強(qiáng)控制器的魯棒性，針對(duì)強(qiáng)非線性的AUV系統(tǒng)，張子迎[5]使用神經(jīng)網(wǎng)絡(luò)PID控制技術(shù)，使控制系統(tǒng)具有自適應(yīng)、自學(xué)習(xí)的能力。Khodayari等[6]建立的自適應(yīng)模糊PID控制器，使AUV實(shí)現(xiàn)了快速三維路徑跟蹤，并驗(yàn)證了所提方法在魯棒性和超調(diào)等方面較PID方法有明顯的改善。除了在控制過程中根據(jù)系統(tǒng)特征引入一些非線性控制方式的智能控制算法，還可以通過對(duì)被控對(duì)象的誤差進(jìn)行及時(shí)的觀測(cè)和補(bǔ)償來提升控制性能。我國(guó)著名控制論學(xué)者韓京清先生于20世紀(jì)90年代創(chuàng)立的自抗擾控制理論(active disturbance rejection control，ADRC)繼承了經(jīng)典PID控制理論“基于誤差來消除誤差的思想”[7-8]，其核心為擴(kuò)張狀態(tài)觀測(cè)器(extended state observer，ESO)，可以將被控對(duì)象中各種未知的不確定性因素歸結(jié)為總擾動(dòng)，將系統(tǒng)補(bǔ)償為標(biāo)準(zhǔn)的積分串聯(lián)型，輔以簡(jiǎn)單的比例-微分控制就可以實(shí)現(xiàn)誤差控制。因非線性自抗擾控制(nonlinear active disturbance rejection control, NLADRC)控制器需整定的參數(shù)多且整定過程復(fù)雜繁瑣。高志強(qiáng)[9]將NLADRC中的擴(kuò)張狀態(tài)觀測(cè)器和誤差反饋控制以線性形式實(shí)現(xiàn)，提出了線性自抗擾控制(LADRC)方法，用帶寬的概念確定控制器參數(shù)，將控制參數(shù)由原來的12個(gè)大幅度降為4個(gè)，使得整個(gè)系統(tǒng)便于調(diào)試和應(yīng)用。

LADRC方法控制參數(shù)少且易于整定，在自抗擾領(lǐng)域得到了廣泛的應(yīng)用。且目前針對(duì)AUV的控制問題研究多是基于單純升沉、偏航或其他形式的簡(jiǎn)化模型。以上特點(diǎn)構(gòu)成了本文的思路。本文使用LADRC方法對(duì)AUV的六自由度模型進(jìn)行深度控制并添加控制器對(duì)AUV的運(yùn)動(dòng)進(jìn)行解耦，實(shí)驗(yàn)結(jié)果與PID方法對(duì)比，驗(yàn)證了LADRC方法在AUV控制問題上的有效性，展現(xiàn)了LADRC方法在控制參數(shù)較少的情況下有較PID方法更高的控制精度和控制穩(wěn)定性。

1 AUV的運(yùn)動(dòng)與建模

1.1 坐標(biāo)系與轉(zhuǎn)換矩陣

水下機(jī)器人的運(yùn)動(dòng)研究分為運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)2個(gè)方面，其中描述機(jī)器人位置、速度、姿態(tài)等問題屬于運(yùn)動(dòng)學(xué)問題，研究機(jī)器人在受力和力矩作用后的位置、姿態(tài)變化問題屬于動(dòng)力學(xué)問題。為了清晰描述AUV運(yùn)動(dòng)，參考有關(guān)資料，建立2種坐標(biāo)系。

固定坐標(biāo)系E-ξηζ，也稱為地面坐標(biāo)系。原點(diǎn)E一般取在水平面上任意一點(diǎn)，Eξ軸正方向指向AUV前進(jìn)方向，Eη軸與Eξ軸位于同一水平面內(nèi)，Eζ垂直于水平面，正方向指向地心。E-ξηζ成右手直角坐標(biāo)系。運(yùn)動(dòng)坐標(biāo)系O-xyz，為建立在船體上的坐標(biāo)系，又稱船體坐標(biāo)系。一般運(yùn)動(dòng)坐標(biāo)系的原點(diǎn)取在AUV浮心處，Ox指向艏向，Oy指向右舷，Oz指向AUV底部。

圖1 固定坐標(biāo)系與運(yùn)動(dòng)坐標(biāo)系Fig.1 Earth-fixed and body-fixed coordinate

表1中列出了AUV的速度、角速度、力和力矩在運(yùn)動(dòng)坐標(biāo)系下的參量符號(hào)。

表1 運(yùn)動(dòng)坐標(biāo)系的參量符號(hào)Table 1 Parameters of moving coordinate system

固定坐標(biāo)系與運(yùn)動(dòng)坐標(biāo)系之間關(guān)于平移速度的轉(zhuǎn)換表達(dá)式：

(1)

其中:

式中：x、y、z為固定坐標(biāo)系描述的AUV位置參數(shù);ψ為艏向角；θ為縱傾角;φ為橫傾角為固定坐標(biāo)系描述的AUV姿態(tài)角。

固定坐標(biāo)系與運(yùn)動(dòng)坐標(biāo)系之間關(guān)于轉(zhuǎn)動(dòng)速度的轉(zhuǎn)換表達(dá)式：

(2)

式中：

根據(jù)剛體動(dòng)力學(xué)理論，AUV六自由度運(yùn)動(dòng)的一般方程如下：

(3)

式中：m為AUV的質(zhì)量；Ix、Iy、Iz為AUV的質(zhì)量m對(duì)Ox、Oy、Oz3個(gè)坐標(biāo)軸的轉(zhuǎn)動(dòng)慣量；xg、yg、zg為AUV的重心坐標(biāo)。方程右端X、Y、Z、K、M、N為作用在AUV上的作用力和力矩，包括了流體動(dòng)力、推力、浮力重力等。

1.2 AUV空間運(yùn)動(dòng)受力與方程

目前所使用的潛航器運(yùn)動(dòng)方程有很多，但各個(gè)方程只是在數(shù)學(xué)描述和數(shù)學(xué)處理方法上不同[5]。例如一種應(yīng)用廣泛的受力建模方法把AUV受力分為流體動(dòng)力和其他外力2種[10]，其中流體動(dòng)力為由于AUV運(yùn)動(dòng)而引起運(yùn)動(dòng)的水對(duì)AUV的反作用力，其他外力包括了推力、浮力和重力等。本文參考的AUV型號(hào)為REMUS自主式水下機(jī)器人。作用在AUV上的總作用力和力矩的表達(dá)形式為[11]：

式中：·HS為流體靜力；·u|u|、·v|v|、·w|w|、·p|p|、·q|q|、·r|r|為流體動(dòng)力阻尼系數(shù)；Yuv、Yuuδr、Zuw、Zuuδs、Muw、Muuδs、Nuv、Nuuδr為主體升力、控制鰭升力與升力矩系數(shù)；Xprop為螺旋槳推力;Kprop為螺旋槳扭矩；δsδr為AUV的縱搖鰭角和舵角。其余系數(shù)為附加質(zhì)量系數(shù)。將(4)式代入(3)式右端，將其中的加速度項(xiàng)移動(dòng)到公式左端，整理后可得到非線性運(yùn)動(dòng)方程組：

(5)

式中：∑X…∑N為除加速度項(xiàng)以外的其余各項(xiàng)。

方程組(5)結(jié)合表達(dá)式(1)、(2)便可得到AUV的六自由度非線性運(yùn)動(dòng)方程組。

1.3 AUV系統(tǒng)模型

由AUV運(yùn)動(dòng)方程組可知，AUV運(yùn)動(dòng)控制的輸入為鰭角δs、舵角δr和螺旋槳推力Xprop。其中AUV的縱搖鰭角δs控制潛航器進(jìn)行俯仰運(yùn)動(dòng)，舵角δr控制使?jié)摵狡鬟M(jìn)行偏航運(yùn)動(dòng)。文獻(xiàn)[11]中實(shí)驗(yàn)表明，在螺旋槳轉(zhuǎn)速為1 500 r/min時(shí)，REMUS可以保持1.51 m/s的速度前進(jìn)。AUV系統(tǒng)模型圖如圖2所示。AUV控制量的輸入影響AUV空間受力后通過六自由度運(yùn)動(dòng)方程計(jì)算得到速度、角速度輸出，再經(jīng)過坐標(biāo)轉(zhuǎn)換可得到AUV的位置、姿態(tài)角輸出。

圖2 AUV系統(tǒng)模型Fig.2 AUV system mode

2 LADRC原理

韓京清教授構(gòu)建的ADRC主要由跟蹤微分器(TD)、擴(kuò)張狀態(tài)觀測(cè)器(ESO)以及非線性狀態(tài)誤差反饋(NLSEF)組成，其基本結(jié)構(gòu)如圖3所示。TD可以跟蹤輸入信號(hào)v得到其過渡過程v1和各階微分信號(hào)[12-13]；ESO是ADRC的核心部分，用于實(shí)時(shí)估計(jì)與在線補(bǔ)償被控系統(tǒng)的總擾動(dòng)；NLSEF根據(jù)系統(tǒng)狀態(tài)誤差e1～en計(jì)算控制信號(hào)u0。ADRC在有效抑制擾動(dòng)影響的同時(shí)可以減弱對(duì)被控模型的依賴，有較強(qiáng)的魯棒性。陳增強(qiáng)等[14]提出的一種自抗擾廣義預(yù)測(cè)控制(ADRC-GPC)算法改善了原GPC算法對(duì)被控對(duì)象精確數(shù)學(xué)模型的強(qiáng)依賴性，并使計(jì)算量大大減少。

圖3 ADRC基本結(jié)構(gòu)Fig.3 ADRC basic structure

如果ESO和NLSEF都采用線性函數(shù)LESO、LSEF[9]，則可以將ADRC簡(jiǎn)化為L(zhǎng)ADRC。

2.1 線性擴(kuò)張狀態(tài)觀測(cè)器

以二階非線性系統(tǒng)為例：

(6)

式中：y和u為系統(tǒng)的輸出和輸入控制信號(hào)；w為系統(tǒng)的外部擾動(dòng)；b為控制器增益且部分已知，設(shè)已知部分為b0。則式(6)可寫為：

(7)

(8)

(9)

式中：z→x為觀測(cè)器的狀態(tài)向量；L為需要設(shè)計(jì)的觀測(cè)器增益矩陣。由于在本文中系統(tǒng)輸出狀態(tài)y可通過實(shí)時(shí)求解AUV運(yùn)動(dòng)方程組直接得到，所以本文采用降階的LESO，刪除式(9)中與狀態(tài)y有關(guān)的部分，得到新的觀測(cè)器方程：

(10)

2.2 線性狀態(tài)誤差反饋控制律

(11)

2.3 參數(shù)整定與簡(jiǎn)化

由以上可知，二階系統(tǒng)的LADRC需要確定的控制器參數(shù)只有b0、ωc、ω03個(gè)。對(duì)于大部分工程對(duì)象，ωo和ωo常按ωo=(3～5)ωc的關(guān)系選擇[15]，這樣LADRC方法需整定的參數(shù)在一定意義上有所減少。

3 AUV仿真與實(shí)驗(yàn)結(jié)果分析

進(jìn)行AUV仿真與運(yùn)動(dòng)控制的重要前提是獲得AUV的準(zhǔn)確的水動(dòng)力系數(shù)。高婷等[16]提出的一種空間拘束運(yùn)動(dòng)模擬方法，經(jīng)過一次算例就可以得到全部水動(dòng)力系數(shù)，在保證了計(jì)算精度的同時(shí)極大地縮短了計(jì)算時(shí)間。本文的實(shí)驗(yàn)對(duì)象參考了文獻(xiàn)[11]中的模型與水動(dòng)力系數(shù),文中通過數(shù)值計(jì)算與實(shí)地實(shí)驗(yàn)，獲得了相對(duì)準(zhǔn)確的水動(dòng)力系數(shù)。

為了驗(yàn)證模型的可操作性，首先進(jìn)行AUV手動(dòng)控制實(shí)驗(yàn)。以AUV深度控制為例，進(jìn)行仿真實(shí)驗(yàn)，并考慮AUV在運(yùn)動(dòng)時(shí)控制量變化后六自由度之間耦合的情況，對(duì)控制器進(jìn)行了優(yōu)化。

3.1 手動(dòng)控制實(shí)驗(yàn)

手動(dòng)控制實(shí)驗(yàn)以AUV下沉實(shí)驗(yàn)為例，假設(shè)AUV具有1.54 m/s的初始速度。0～2 s時(shí)AUV控制輸入δs、δr均為0°，在2～5 s時(shí)控制輸入δs=8°、δr=0°。從圖4可以看出，在AUV下沉過程中，若方向舵沒有控制量輸入，AUV各自由度之間的耦合會(huì)使其產(chǎn)生偏航運(yùn)動(dòng)。

圖4 時(shí)間-偏航曲線Fig.4 Time-yaw curve

3.2 LADRC控制與優(yōu)化

良好的深度控制可以幫助AUV完成一系列復(fù)雜的海底勘探等任務(wù)，因此AUV深度控制的控制效果應(yīng)該在設(shè)定深度上擁有一定程度的穩(wěn)定性。

圖5為L(zhǎng)ADRC、NLADRC與PID 3種控制方法的深度控制效果，且未考慮偏航控制。PID方法與LADRC方法在到達(dá)指定深度后振蕩分別為-0.35～0.33 m和-0.21～0.16 m。從圖中可以看出，PID方法雖然響應(yīng)速度較快，但PID方法比LADRC方法超調(diào)更大，且不易穩(wěn)定。NLADRC方法在到達(dá)指定深度后的振蕩為 -0.18～0.17 m。NLADRC方法比LADRC方法響應(yīng)速度快，但快速的響應(yīng)會(huì)帶來明顯的超調(diào)量，調(diào)整NLADRC的控制器參數(shù)需要付出大量的時(shí)間，且LADRC控制器的控制性能在優(yōu)化控制器結(jié)構(gòu)之后可以取得更好的控制效果，所以選擇LADRC控制器進(jìn)行進(jìn)一步的仿真實(shí)驗(yàn)。

圖5 3種方法的深度控制曲線Fig.5 Depth control curves of three methods

為了解決潛航器升沉和偏航運(yùn)動(dòng)之間的耦合問題，接下來在原LADRC深度控制器的基礎(chǔ)上，添加PD偏航控制器，使?jié)摵狡髟谙鲁吝\(yùn)動(dòng)過程中，實(shí)時(shí)計(jì)算并消除航向偏差。圖6(a)為添加偏航控制后的時(shí)間-深度平面圖像，數(shù)值分析可以看出添加偏航控制后的LADRC深度控制器在到達(dá)指定深度后的震蕩幅度從原來的-0.21～0.16 m減小為-0.05～0.02 m，但PID深度控制器的震蕩幅度沒有改變。圖6(b)為加入偏航控制器之后的時(shí)間-偏航平面圖，雙PID控制下的AUV偏航運(yùn)動(dòng)的最終震蕩幅度為-0.01～0.01 m，而增添PD控制器的LADRC控制器的最終震蕩幅度為-0.001～0.002 m。作為參考，本文研究對(duì)象REMUS自主式水下機(jī)器人的最大船體直徑約為0.2 m，由以上數(shù)據(jù)和結(jié)果可知LADRC方法可以取得較好的控制性能。圖7為2種控制方法控制效果的三維圖像。

圖6 添加偏航控制的深度和偏航曲線Fig.6 Depth and yaw curves with yaw control

圖7 控制效果三維圖Fig.7 3-D diagram of control effect

4 結(jié)論

1)LADRC方法在AUV運(yùn)動(dòng)控制上是有效的，LESO可以很好地實(shí)時(shí)估計(jì)被控系統(tǒng)的總擾動(dòng)。

2)在AUV到達(dá)指定深度的穩(wěn)定階段，LADRC方法可以使AUV有更高的穩(wěn)定性，當(dāng)AUV需要完成高精度要求的工作時(shí)，LADRC方法有更高的適用性。

3)在添加偏航控制器后，LADRC方法的深度控制效果得到明顯改善，證明了LADRC方法在AUV六自由度運(yùn)動(dòng)解耦問題上的有效性和優(yōu)勢(shì)。

4)LADRC方法需整定的參數(shù)個(gè)數(shù)與PID相等，與NLADRC方法相比，LADRC參數(shù)整定更加便捷、更易推廣和在工程上使用。

在實(shí)際應(yīng)用中，AUV的運(yùn)動(dòng)控制問題更為復(fù)雜：不同的位置目標(biāo)，控制器的參數(shù)也不盡相同。因此，未來將向著不基于模型的控制器參數(shù)自調(diào)整方面進(jìn)行研究。