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

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

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

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

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

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

1 輕型長航程AUV的系統組成

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

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

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

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

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

2 AUV航行控制系統設計

AUV的航行控制系統由航行控制器、執行機構、狀態觀測器以及各傳感器構成，控制系統的結構如圖3所示。

圖3 AUV的航行控制系統的組成示意圖

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

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

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

3 模型預測控制器設計

3.1 深度控制目標轉化

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

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

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

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

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

圖5 AUV速度分解示意圖

3.2 面向控制的垂直面系統建模

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

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

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

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

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

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

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

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

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

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

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

由式（12）和式（13）可知，AUV 基于浮力調節過程的空間狀態方程和基于重心調節過程的空間狀態方程具有相同的表現形式：

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

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

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

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

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

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

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

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

式中：

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

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

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

4 仿真試驗與分析

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

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

圖8 AUV輸入控制量曲線

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

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

5 結論

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