黃 琰 ，胡 峰 ，俞建成 *，喬佳楠 ，王振宇，謝宗伯

（1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所 機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽(yáng) 110016；2.中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽(yáng) 110016；3.中國(guó)科學(xué)院大學(xué)，北京 100049）

自主式水下航行器（Autonomous Underwater Vehicle，AUV）作為目前主要的海洋自主移動(dòng)平臺(tái)已經(jīng)獲得廣泛的應(yīng)用[1-3]，而有限的續(xù)航能力是影響AUV技術(shù)在海洋觀測(cè)領(lǐng)域中拓展應(yīng)用的主要制約因素之一。區(qū)別于常規(guī)AUV，面向海洋時(shí)變中小尺度過(guò)程長(zhǎng)期精細(xì)觀測(cè)需求而研制的輕型長(zhǎng)航程AUV具備類(lèi)似于水下滑翔機(jī)的浮力調(diào)節(jié)和重心調(diào)節(jié)機(jī)構(gòu)[4-5]，使得這種AUV在定深直航過(guò)程中可以根據(jù)海水密度自適應(yīng)調(diào)節(jié)載體凈浮力和航行姿態(tài)，具備長(zhǎng)時(shí)間高效巡航能力，大幅提升了AUV的巡航范圍。

目前，國(guó)內(nèi)外在浮力可變的AUV的航行控制算法領(lǐng)域研究較少。美國(guó)Tethys長(zhǎng)航程AUV利用PID深度控制方法，采用并行PID外環(huán)讀取深度誤差和深度變化率，并產(chǎn)生俯仰和浮力命令，控制內(nèi)部浮力調(diào)節(jié)機(jī)構(gòu)、水平舵調(diào)節(jié)機(jī)構(gòu)和質(zhì)量塊調(diào)節(jié)機(jī)構(gòu)3種執(zhí)行器[6-7]；張勛等[8]在設(shè)計(jì)了AUV均衡系統(tǒng)的基礎(chǔ)上，構(gòu)建了深度控制、縱傾控制和浮態(tài)控制模型，并基于PID和均衡系統(tǒng)設(shè)計(jì)了深度控制、縱傾控制和浮態(tài)控制方法；姜志斌等[9]針對(duì)具備均衡系統(tǒng)的定點(diǎn)觀測(cè)型AUV設(shè)計(jì)了基于級(jí)聯(lián)控制和跟蹤微分器的AUV下潛控制方法。上述研究在AUV技術(shù)實(shí)現(xiàn)時(shí)均采用PID控制或分段PID進(jìn)行航行運(yùn)動(dòng)控制，當(dāng)在海洋環(huán)境突然發(fā)生變化（如海水密度減小）時(shí)，采用基于PID的控制算法調(diào)整浮力和重心可能造成俯仰角震蕩、浮力超調(diào)等問(wèn)題，使得執(zhí)行機(jī)構(gòu)在產(chǎn)生多余不必要的能耗的同時(shí)，系統(tǒng)無(wú)法得到令人滿意的動(dòng)態(tài)響應(yīng)，進(jìn)而也影響了AUV續(xù)航指標(biāo)的提升。

采用模型預(yù)測(cè)控制算法可以很好地解決AUV系統(tǒng)時(shí)滯而產(chǎn)生的動(dòng)態(tài)響應(yīng)和軌跡跟蹤等問(wèn)題。Geng等[10]將帶執(zhí)行機(jī)構(gòu)約束條件的模型預(yù)測(cè)控制應(yīng)用于AUV的深度控制中，并在試驗(yàn)中表現(xiàn)出較好的控制性能。杜亮等[11]研究了近水面航行的AUV模型預(yù)測(cè)控制方法，該方法提高了AUV在波浪干擾下的系統(tǒng)動(dòng)態(tài)響應(yīng)。本文針對(duì)輕型長(zhǎng)航程AUV實(shí)現(xiàn)零攻角高效定深航行的要求，開(kāi)展輕型長(zhǎng)航程AUV航行控制技術(shù)研究。在輕型長(zhǎng)航程AUV系統(tǒng)組成及其航行控制系統(tǒng)設(shè)計(jì)基礎(chǔ)上，基于模型預(yù)測(cè)算法設(shè)計(jì)了AUV深度控制器。最后，通過(guò)仿真驗(yàn)證了控制算法的有效性。

1 輕型長(zhǎng)航程AUV的系統(tǒng)組成

本文討論的輕型長(zhǎng)航程AUV采用模塊化設(shè)計(jì)思想，兼顧系統(tǒng)的功能性、可靠性和可擴(kuò)展性，分為艏部傳感器艙段、能源艙段、姿態(tài)調(diào)節(jié)舯段、浮力調(diào)節(jié)和航向控制艙段和推進(jìn)艙段5個(gè)艙段。AUV載體外觀如圖1所示，內(nèi)部關(guān)鍵機(jī)構(gòu)布置結(jié)構(gòu)如圖2所示。

圖1 輕型長(zhǎng)航程AUV載體外觀圖

圖2 輕型長(zhǎng)航程AUV內(nèi)部機(jī)構(gòu)布置示意圖

AUV的艏部艙段主要安裝避碰聲吶、多普勒測(cè)速儀（DVL）、溫鹽深傳感器（CTD）以及其他科學(xué)載荷；能源艙段主要安裝有固定電池組和浮力調(diào)節(jié)裝置；姿態(tài)調(diào)節(jié)艙段安裝有重心調(diào)節(jié)裝置、包括電子羅盤(pán)在內(nèi)的航行控制系統(tǒng)、通信系統(tǒng)以及應(yīng)急拋載裝置等；航向控制艙段安裝有垂直舵組件和水平舵組件；推進(jìn)艙段安裝有2葉低速螺旋槳推進(jìn)裝置。

經(jīng)系統(tǒng)優(yōu)化設(shè)計(jì)，輕型長(zhǎng)航程AUV在空氣中自重約200 kg，最大航行速度2 kn，最大不間斷巡航距離可達(dá)2 000 km。

2 AUV航行控制系統(tǒng)設(shè)計(jì)

AUV的航行控制系統(tǒng)由航行控制器、執(zhí)行機(jī)構(gòu)、狀態(tài)觀測(cè)器以及各傳感器構(gòu)成，控制系統(tǒng)的結(jié)構(gòu)如圖3所示。

圖3 AUV的航行控制系統(tǒng)的組成示意圖

在航行過(guò)程中，系統(tǒng)通過(guò)獲取電子羅盤(pán)的三軸姿態(tài)數(shù)據(jù)、溫鹽深傳感器（CTD）換算獲得的海水密度和深度數(shù)據(jù)、以及多普勒測(cè)速儀（DVL）的三向速度數(shù)據(jù)，估計(jì)AUV的航行狀態(tài)和環(huán)境參數(shù)。通過(guò)狀態(tài)估計(jì)和航行目標(biāo)期望值相比較，計(jì)算AUV的航行狀態(tài)偏差，并通過(guò)控制器算法輸出控制量給各執(zhí)行機(jī)構(gòu)。在本研究中，執(zhí)行機(jī)構(gòu)主要包括浮力平衡機(jī)構(gòu)、重心調(diào)節(jié)機(jī)構(gòu)和水平舵。

AUV在水下長(zhǎng)時(shí)間大范圍巡航時(shí)，可能會(huì)跨越多個(gè)不同密度的水團(tuán)，而不同海水密度的變化會(huì)帶來(lái)浮力的改變，常規(guī)AUV不得不通過(guò)調(diào)整航行姿態(tài)以克服浮力變化的影響，導(dǎo)致自身航行效率降低。輕型長(zhǎng)航程AUV由于其浮力量和重心位置的可調(diào)性，可以通過(guò)浮力和重心調(diào)節(jié)始終保持零攻角的航行狀態(tài)，最大限度地減少海洋環(huán)境變化帶來(lái)的航行效率損失。

另一方面，由于輕型長(zhǎng)航程AUV的重心調(diào)節(jié)和浮力調(diào)節(jié)成為AUV航行控制的新增輸入量，控制輸入量間的非線性耦合無(wú)法被忽略，直接通過(guò)PID等傳統(tǒng)控制方法無(wú)法得到令人滿意的系統(tǒng)動(dòng)態(tài)響應(yīng)，因此需采用模型預(yù)測(cè)控制算法實(shí)現(xiàn)AUV的高效定深航行性能。

3 模型預(yù)測(cè)控制器設(shè)計(jì)

3.1 深度控制目標(biāo)轉(zhuǎn)化

輕型長(zhǎng)航程AUV不具備側(cè)向推進(jìn)能力，只能采用水平舵、重心調(diào)節(jié)機(jī)構(gòu)控制俯仰角進(jìn)行航行深度的調(diào)節(jié)，具有一定的欠驅(qū)動(dòng)特性。采用簡(jiǎn)化視線導(dǎo)航法（Line-of-Sight，LOS）[12]將 AUV 垂直面深度控制轉(zhuǎn)化成俯仰角控制問(wèn)題，如圖4所示。圖中，H0為目標(biāo)深度線，當(dāng)前AUV質(zhì)心Pt(ht)距目標(biāo)深度線的垂直距離（航跡偏差）為ek，且ek需要收斂到0。以當(dāng)前位置點(diǎn)作一個(gè)半徑為R的圓（R足夠大），則所作的圓與目標(biāo)深度線的交點(diǎn)中位于AUV前方的交點(diǎn)定為當(dāng)前視線導(dǎo)引點(diǎn)Plos，進(jìn)而可求出當(dāng)前目標(biāo)俯仰角為：

式中：Θlos為目標(biāo)俯仰角；Θmax為AUV允許的最大俯仰角。

圖4 AUV深度控制視線導(dǎo)航法示意圖

由于AUV航行深度的變化是通過(guò)俯仰角的改變實(shí)現(xiàn)的，而在使AUV產(chǎn)生俯仰力矩的同時(shí)，必然會(huì)在垂直載體軸線方向上產(chǎn)生一個(gè)與AUV目標(biāo)深度變化方向相反的垂向速度。如圖5所示，當(dāng)AUV目標(biāo)深度線位于載體下方時(shí)，需要使AUV保持向下的俯仰角。此時(shí)，AUV具有載體軸向方向上的速度u以及垂直載體向上的垂向速度w，兩者在深度方向上投影的差值使AUV趨向目標(biāo)深度線運(yùn)動(dòng)，且w的影響在大部分時(shí)間可以忽略不計(jì)。但是當(dāng)AUV接近目標(biāo)深度線時(shí)，其目標(biāo)俯仰角θ較小。當(dāng)usinθ=wcosθ時(shí)，深度方向上的絕對(duì)速度分量為0，AUV會(huì)保持當(dāng)前的俯仰狀態(tài)向前航行，即不能達(dá)到目標(biāo)深度又不能保持零攻角高效航行狀態(tài)。為解決這一問(wèn)題，在長(zhǎng)航程AUV深度控制中，引入俯仰角修正項(xiàng)，對(duì)目標(biāo)俯仰角進(jìn)行修正。當(dāng)θ較小時(shí)，sinθ=θ,cosθ=1，因此目標(biāo)俯仰角修正項(xiàng)可以表示為：

則修正后的長(zhǎng)航程AUV深度控制目標(biāo)俯仰角為：

圖5 AUV速度分解示意圖

3.2 面向控制的垂直面系統(tǒng)建模

3.2.1 浮力和重心可調(diào)的AUV垂直面動(dòng)力學(xué)建模參考潛艇標(biāo)準(zhǔn)六自由度運(yùn)動(dòng)方程以及平面運(yùn)動(dòng)假設(shè)[13]，并考慮AUV浮力和重心調(diào)節(jié)機(jī)構(gòu)對(duì)剩余浮力和重心的改變，得到輕型長(zhǎng)航程AUV在垂直面內(nèi)的動(dòng)力學(xué)模型為：

式中：m為AUV質(zhì)量；ρ為環(huán)境海水密度；l為AUV艇長(zhǎng)；g為重力加速度；xG為AUV重心坐標(biāo)；h為 AUV 穩(wěn)心高；u,w,q為 AUV 速度；u˙,w˙,q˙為 AUV加速度；θ為AUV俯仰角；ΔB為AUV剩余浮力；δs為水平舵舵角；Iyy為AUV繞y軸轉(zhuǎn)動(dòng)慣量；Voil為排油量（向外油囊排油為正）；msl為滑塊質(zhì)量；xsl為滑塊移動(dòng)位移（向艏部移動(dòng)為正）；Z'q˙，M'q˙等為 AUV所受的無(wú)因次化水動(dòng)力系數(shù)項(xiàng)。

由式（10）可知，由于在系統(tǒng)模型中重心調(diào)節(jié)和浮力控制量存在耦合，同時(shí)控制會(huì)造成系統(tǒng)的動(dòng)態(tài)響應(yīng)較差。因此，為了設(shè)計(jì)符合AUV航行需求的模型預(yù)測(cè)控制器，首先需建立兩個(gè)獨(dú)立的狀態(tài)空間方程來(lái)實(shí)現(xiàn)系統(tǒng)的解耦。即在實(shí)際AUV巡航過(guò)程中，采用浮力調(diào)節(jié)機(jī)構(gòu)和重心調(diào)節(jié)機(jī)構(gòu)分時(shí)執(zhí)行的策略，水平舵始終參與控制保證AUV的航行姿態(tài)穩(wěn)定。

3.2.2 浮力調(diào)節(jié)過(guò)程的AUV狀態(tài)空間方程 輕型長(zhǎng)航程AUV浮力調(diào)節(jié)機(jī)構(gòu)工作時(shí)，AUV剩余浮力狀態(tài)的改變會(huì)導(dǎo)致系統(tǒng)狀態(tài)方程產(chǎn)生偏差。為了反映出浮力調(diào)節(jié)對(duì)系統(tǒng)狀態(tài)的影響，以及便于后續(xù)設(shè)計(jì)觀測(cè)器對(duì)剩余浮力進(jìn)行辨識(shí)，對(duì)系統(tǒng)狀態(tài)變量進(jìn)行擴(kuò)展，加入剩余浮力ΔB項(xiàng)。將x=(w,q,θ,ΔB)T代入式（10）并重新整理，得到：

將其在時(shí)間域上進(jìn)行離散，得到擴(kuò)展后的輕型長(zhǎng)航程AUV空間狀態(tài)方程：

式中：x=（w,q,θ,ΔB）T表示系統(tǒng)狀態(tài)矢量；u=（δS，Voil）T表示控制輸入量為水平舵舵角和浮力油囊調(diào)節(jié)量；yk=（Θ,ΔB）T為系統(tǒng)輸出；Ts為采樣時(shí)間；A1，B1，C1分別表示維數(shù)為 4×4，4×2 和 2×4 的狀態(tài)系數(shù)矩陣；dk為系統(tǒng)擾動(dòng)。

3.2.3 重心調(diào)節(jié)過(guò)程的AUV狀態(tài)空間方程 將AUV重心坐標(biāo)xG擴(kuò)展加入到系統(tǒng)狀態(tài)變量中。將x=(w,q,θ,xG)T代入到AUV狀態(tài)方程中，并在時(shí)間域上進(jìn)行離散，得到擴(kuò)展后的基于水平舵和重心調(diào)節(jié)的狀態(tài)空間方程：

式中：x=（w,q,θ,xG）T表示系統(tǒng)狀態(tài)矢量；u=（δS,xslide）rT表示控制輸入量為水平舵舵角和浮仰滑塊調(diào)節(jié)量；yk=（Θ,ω）T為系統(tǒng)輸出；A2，B2，C2分別表示維數(shù)為4×4，4×2和2×4的狀態(tài)系數(shù)矩陣。

3.3 雙模型切換的模型預(yù)測(cè)定深控制器設(shè)計(jì)

在AUV長(zhǎng)期水下定深直航過(guò)程中，浮力調(diào)節(jié)過(guò)程的AUV狀態(tài)空間方程（3.2.2節(jié)所述）和重心調(diào)節(jié)過(guò)程的AUV狀態(tài)空間方程（3.2.3節(jié)所述）的建立基于雙模型切換的模型預(yù)測(cè)定深航行控制器，實(shí)現(xiàn)AUV在海洋環(huán)境變化（如密度變化、上升流等）時(shí)的快速調(diào)整響應(yīng)?；陔p模型切換的模型預(yù)測(cè)定深航行控制器主要由模型切換規(guī)劃器、浮力調(diào)節(jié)模型預(yù)測(cè)控制器、重心調(diào)節(jié)模型預(yù)測(cè)控制器3部分組成，如圖6所示。

圖6 AUV雙模型切換定深控制器

由式（12）和式（13）可知，AUV 基于浮力調(diào)節(jié)過(guò)程的空間狀態(tài)方程和基于重心調(diào)節(jié)過(guò)程的空間狀態(tài)方程具有相同的表現(xiàn)形式：

因此，可以統(tǒng)一描述兩種模型預(yù)測(cè)控制器的設(shè)計(jì)過(guò)程。將狀態(tài)方程（14）進(jìn)行迭代、整理即可得到系統(tǒng)的輸出預(yù)測(cè)方程為：

其中：yk+n|k表示在k時(shí)刻預(yù)測(cè)k+n時(shí)刻的系統(tǒng)輸出值；xk表示當(dāng)前狀態(tài)量；uk-1表示前一步輸入控制量；Δuk+n|k表示預(yù)測(cè)k+n時(shí)刻控制輸入增量；Hu表示控制時(shí)域；Hp表示預(yù)測(cè)時(shí)域。

考慮AUV控制過(guò)程中的控制量極限約束和控制增量約束[14]?？刂屏考s束和控制量增量約束的表達(dá)形式為：

由于在控制時(shí)域內(nèi)需要求解的是控制量的增量，因此約束條件中也只能以控制增量形式出現(xiàn)，需要對(duì)控制量約束式(16)進(jìn)行轉(zhuǎn)換：

式中：Umin和Umax分別表示根據(jù)控制量約束計(jì)算得到的控制量增量上下限。

為保證AUV能夠快速平穩(wěn)地追蹤期望軌跡，目標(biāo)函數(shù)需要加入系統(tǒng)輸出量的偏差和控制量的優(yōu)化。同時(shí)，為解決有約束條件下可能出現(xiàn)的無(wú)法得到最優(yōu)解的情況，有必要在目標(biāo)函數(shù)中加入松弛因子[15]。目標(biāo)函數(shù)如下：

式中：r→k+1表示目標(biāo)軌跡；W和R分別表示輸出量權(quán)重系數(shù)和控制量增量權(quán)重系數(shù)；ρ為權(quán)重系數(shù)；ε為松弛因子。

綜合目標(biāo)函數(shù)和約束條件，基于空間狀態(tài)方程的預(yù)測(cè)控制器在每個(gè)控制周期內(nèi)要解決如下的優(yōu)化問(wèn)題：

式中：

采用內(nèi)點(diǎn)法[16]進(jìn)行最優(yōu)控制量的計(jì)算。在每個(gè)控制周期內(nèi)完成上述優(yōu)化問(wèn)題的求解后，得到控制時(shí)域內(nèi)的一系列控制輸入增量和松弛因子：

將該控制序列中第一個(gè)元素作為實(shí)際控制輸入增量作用于系統(tǒng)，即：

進(jìn)入下一個(gè)控制周期后，重復(fù)上述過(guò)程，如此循環(huán)實(shí)現(xiàn)對(duì)AUV期望軌跡的控制。

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

通過(guò)仿真對(duì)AUV深度控制策略進(jìn)行驗(yàn)證。假設(shè)AUV在水中10 m深處進(jìn)行定深航行，航速為0.5 m/s。AUV水平水平舵舵角最大為±20 deg，最大轉(zhuǎn)動(dòng)速度4 deg/s；浮力調(diào)節(jié)最大排油量為±0.2 L，最大排油速度為0.04 L/s；滑塊最大移動(dòng)量為±0.1 m，最大移動(dòng)速度0.02 m/s。同時(shí)假設(shè)AUV在1 000 s時(shí)由于外界環(huán)境因素（海水密度）的變化導(dǎo)致自身浮力值減少1.5 N，并通過(guò)3.3節(jié)設(shè)計(jì)的雙模型切換MPC控制AUV定深航行。仿真結(jié)果如圖7～圖9所示。

圖7 AUV深度和俯仰角輸出曲線

圖8 AUV輸入控制量曲線

由仿真結(jié)果曲線可知，初始時(shí)AUV在水下10 m處進(jìn)行零攻角定深航行。在1 000 s時(shí)，由于AUV自身浮力的改變，導(dǎo)致AUV偏離預(yù)定航行軌跡，觸發(fā)系統(tǒng)觀測(cè)器辨識(shí)出自身剩余浮力差值。假設(shè)在1 250 s時(shí)，系統(tǒng)啟動(dòng)水平舵加浮力調(diào)節(jié)控制器對(duì)浮力進(jìn)行調(diào)節(jié)，對(duì)AUV軌跡進(jìn)行修正，并在1 400 s處使AUV重新進(jìn)入無(wú)剩余浮力穩(wěn)定運(yùn)行狀態(tài)（歷時(shí)150 s）。可以發(fā)現(xiàn)，此時(shí)AUV定深航行的深度有偏差（9.8 m），航行縱傾角也不能保持零攻角狀態(tài)（-2 deg）。為了使自身恢復(fù)零攻角直航狀態(tài)，在1 500 s時(shí)AUV切換為水平舵加重心調(diào)節(jié)控制器，進(jìn)行縱傾角和軌跡的修正，在1 800 s時(shí)各機(jī)構(gòu)調(diào)節(jié)完成（歷時(shí)300 s），最后AUV在慣性作用下于2 000 s處恢復(fù)零攻角定深直航（10 m，0 deg）。仿真結(jié)果表明，通過(guò)水平舵加浮力調(diào)節(jié)控制器和水平舵加重心調(diào)節(jié)控制器的聯(lián)合控制，可以均衡AUV自身浮力變化對(duì)AUV航行性能的影響，實(shí)現(xiàn)AUV高效的定深航行能力。同時(shí)，系統(tǒng)輸出曲線較為光滑，執(zhí)行機(jī)構(gòu)的調(diào)節(jié)頻率較低，基本上無(wú)超調(diào)現(xiàn)象。兩種控制器進(jìn)行深度控制時(shí)均具有良好的姿態(tài)保持和定深性能。

圖9 AUV垂向速度和俯仰角速度曲線

5 結(jié)論

與常規(guī)AUV主要依靠水平舵或垂直槽道槳抵消凈浮力進(jìn)行定深航行的方式不同，輕型長(zhǎng)航程AUV具備了浮力調(diào)節(jié)和重心調(diào)節(jié)能力，可實(shí)現(xiàn)中性零攻角的長(zhǎng)時(shí)間高效定深航行。針對(duì)新的控制輸入量帶來(lái)的控制耦合問(wèn)題，文中設(shè)計(jì)了一種基于雙模型切換的模型預(yù)測(cè)深度控制器，通過(guò)分別建立基于浮力調(diào)節(jié)和基于重心調(diào)節(jié)的擴(kuò)展空間狀態(tài)方程，實(shí)現(xiàn)了對(duì)浮力和重心控制輸入量在控制模型上的解耦。通過(guò)仿真驗(yàn)證，基于模型切換的模型預(yù)測(cè)深度控制器能夠有效對(duì)AUV浮力變化進(jìn)行均衡響應(yīng)和姿態(tài)調(diào)整，使AUV恢復(fù)零攻角直航狀態(tài)?？刂七^(guò)程中浮力、俯仰和水平舵的調(diào)節(jié)頻率較低，可以有效降低執(zhí)行機(jī)構(gòu)的磨損以及能量的消耗。該控制器設(shè)計(jì)方法對(duì)其他可變浮力的長(zhǎng)航程AUV控制器設(shè)計(jì)具有參考借鑒作用。