基于模型預測控制算法的輕型長航程AUV航控系統(tǒng)研究

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

（1.中國科學院沈陽自動化研究所 機器人學國家重點實驗室，遼寧 沈陽 110016；2.中國科學院機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110016；3.中國科學院大學，北京 100049）

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

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

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

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

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

圖1 輕型長航程AUV載體外觀圖

圖2 輕型長航程AUV內部機構布置示意圖

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

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

2 AUV航行控制系統(tǒng)設計

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

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

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

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

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

3 模型預測控制器設計

3.1 深度控制目標轉化

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

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

圖4 AUV深度控制視線導航法示意圖

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

則修正后的長航程AUV深度控制目標俯仰角為：

圖5 AUV速度分解示意圖

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

3.2.1 浮力和重心可調的AUV垂直面動力學建模參考潛艇標準六自由度運動方程以及平面運動假設[13]，并考慮AUV浮力和重心調節(jié)機構對剩余浮力和重心的改變，得到輕型長航程AUV在垂直面內的動力學模型為：

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

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

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

將其在時間域上進行離散，得到擴展后的輕型長航程AUV空間狀態(tài)方程：

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

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

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

3.3 雙模型切換的模型預測定深控制器設計

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

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

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

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

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

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

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

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

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

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

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

式中：

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

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

進入下一個控制周期后，重復上述過程，如此循環(huán)實現(xiàn)對AUV期望軌跡的控制。

4 仿真試驗與分析

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

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

圖8 AUV輸入控制量曲線

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

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

5 結論

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