基于MPC的AUV軌跡跟蹤控制研究

摘" 要：文章研究了自主水下航行器（AUV）的軌跡跟蹤控制問題。為了提高自主水下航行器的軌跡跟蹤性能，提出一種基于Lyapunov的模型預測控制（LMPC）方法來設計控制器。首先，基于該方法，考慮執行器飽和等實際約束，設計非線性反步跟蹤控制律，在基于Lyapunov的模型預測控制問題中構造約束條件，使閉環的穩定性得到保證。其次，傳統的控制器參數設置方法一般為試湊法，根據經驗代入不同參數觀察AUV的跟蹤效果。對于其中的權重矩陣，采用改進的布谷鳥算法進行優化。最后，在MATLAB上的仿真結果表明，所提出的方法顯著提高了AUV的軌跡跟蹤控制性能。

關鍵詞：自主水下航行器；軌跡跟蹤；模型預測控制；布谷鳥搜索算法

中圖分類號：TP18" " 文獻標識碼：A" 文章編號：2096-4706（2024）10-0188-06

Research on Trajectory Tracking Control of AUV Based on MPC

SHI Wenhui， JIN Lina， MA Nannan

（Liaoning Petrochemical University， Fushun" 113001， China）

Abstract： This paper investigates the trajectory tracking control problem of AUVs. In order to improve the trajectory tracking performance of AUV， a method of Model Predictive Control based on Lyapunov is proposed to design the controller. Firstly， based on the method， considering practical constraints such as actuator saturation， a nonlinear backstepping tracking control law is designed， and constraint conditions are constructed in the problem of Model Predictive Control based on Lyapunov to ensure the stability of the closed-loop. Secondly， the traditional method for setting controller parameters is generally the trial and error method， where different parameters are substituted based on experience to observe the tracking effect of AUV. For the weight matrix in the Model Predictive Control based on Lyapunov， an improved Cuckoo Search algorithm is used for optimization. Finally， the simulation results on MATLAB show that the proposed method significantly improves the trajectory tracking control performance of AUVs.

Keywords： Autonomous Underwater Vehicle; trajectory tracking; model predictive control; Cuckoo Search algorithm

0" 引" 言

自主水下航行器（AUV）是水下救援、油氣勘探的重要載體[1]。由于其在各種潛艇項目中有大幅降低風險和運營成本的潛力，引起了工業界和學術界越來越多的關注[2]。AUV的模型具有非線性、不確定性和高耦合性，因此在研究AUV的相關問題中常采用欠驅動的形式。軌跡跟蹤作為AUV控制研究的重要內容，其控制魯棒性和運行穩定性有待提高。

近年來，在AUV的運動控制中經常用到反步控制、滑模控制（SMC）、PID控制、自適應控制和神經網絡控制等。Cho等[3]采用反步法和時滯估計來設計AUV軌跡跟蹤控制器，仿真結果表明AUV具有準確和魯棒的跟蹤性能。但是反步控制的參數調整比較困難。Wang等[4]利用魯棒滑膜控制來實現AUV的航向控制，并通過強化學習來調整SMC的參數，取得了不錯的效果。Xiong等[5]在設計AUV軌跡跟蹤控制器時，將智能PID和PD前饋控制器相結合，取得更好的擾動抑制和初始跟蹤誤差補償，但在控制低速時，會出現積分飽和問題，導致系統出現超調等問題。Cui等[6]將神經網絡，加入自適應控制設計中。引入臨界神經網絡評估所設計的控制器的性能，在自適應控制中設計了補償項。

與大多數傳統的控制方法相比，模型預測控制（MPC）的顯著特點在于它能夠在控制器設計中處理系統約束。MPC可以通過預測未來系統狀態和輸出、及時處理實時干擾、確保跟蹤性能和閉環系統穩定性來進行優化。Shen等[7]提出一種MPC方案來解決AUV平面軌跡的規劃和跟蹤問題，首次給出了基于MPC的AUV軌跡跟蹤控制的穩定性條件，但是沒有考慮在垂直方向的運動。Yan等[8]設計了一種新的基于MPC的雙閉環軌跡跟蹤方法。外環控制器產生的速度傳遞給內環控制器，內環控制器產生控制輸入，確保閉環軌跡跟蹤。明確考慮了系統輸入和狀態的約束，提高了AUV的穩定運行能力。Zhang [9]提出一種以速度增量為控制輸入，同時考慮輸入約束和狀態約束的模型預測控制器，驗證MPC是一種基于優化的時域控制方法，以運動學為框架設計了一種控制器，通過速度反向尋求控制力。一方面，控制力不利于作用在推進器上；另一方面，這種控制器設計缺乏穩定性證明。

本文針對AUV的軌跡跟蹤控制問題，提出一種基于Lyapunov的非線性模型預測控制（LMPC）方案。LMPC能夠很好地處理非線性和交叉耦合的系統動力學以及推力限制方面的系統約束[10]。它避免了標準模型預測控制所需要的局部線性化，以相對簡單的方式保證了閉環穩定性，對于推力分配問題，協調推進器生成跟蹤控制律要求的廣義力和力矩。利用基于Lyapunov的反步技術在LMPC問題公式中構造收縮約束，從理論上保證了所提出的LMPC控制算法的閉環穩定性。利用改進的布谷鳥搜索算法對LMPC權重矩陣進行尋優處理[11]，將軌跡跟蹤誤差的絕對值作為其目標函數，尋找最佳的權重矩陣，使AUV能夠較好的跟蹤期望軌跡。

1" AUV模型

首先對AUV的坐標系進行說明，固定坐標系和隨體坐標系如圖1。固定坐標系是對地坐標系，原點為地球中的某一點。隨體坐標系主要針對AUV坐標系，其原點為AUV的重心。

實驗對象是Saab SeaEye Falcon，假設Saab SeaEye Falcon質量均勻分布，由于推力器布局不允許對橫搖和俯仰運動進行主動控制，因此我們研究Falcon的進退、橫移、潛浮和轉艏四自由度軌跡跟蹤控制。

AUV的運動學模型為：

式中， 表示AUV在固定坐標系下的位置和航向， 表示AUV在隨體坐標系下的速度和偏航角速度。J（ψ）是取決于航向ψ的旋轉矩陣。J（ψ）的形式為：

AUV的動力學模型為：

式中， 表示廣義推力和力矩。 表示包含附加質量的慣性矩陣；C（v）表示科里奧利和向心矩陣；D（v）表示阻尼矩陣。

g（η）表示恢復力。廣義推力τ是由五個推力器" 產生的，遵循 。其中，α表示在隨體坐標系中推進器的方位矢量。對于獵鷹，方位角是固定的。因此，有推力分布：

式中，N是一個常數輸入矩陣。

根據式（1）（3）和（6）可知AUV的模型可以表示為：

式中，狀態定義為 ，控制定義為 。

在控制器設計中有如下假設：

假設1：科里奧利和向心矩陣是斜對稱的： ；

假設2：阻尼矩陣是正定的：；

假設3：輸入矩陣" 是非奇異；

假設4：所有推進器的容量都一樣即：，i = 1，2，3，4，5。

2" 相關算法介紹

2.1" 模型預測控制算法

在控制過程中，存在一條軌跡作為跟蹤對象。如圖2所示，k為當前時刻。控制器通過當前AUV狀態測量值和預測模型，預測未來一段時域[k，k + Np]系統的輸出。通過求解滿足目標函數及各種約束的優化問題，得到[k，k + Nc]內的一系列控制變量序列，并將該時控制序列中的第一個元素作為被控對象的實際控制量，當來到下一時刻k + 1時重復上述過程。如此滾動的完成優化問題，實現對被控對象持續控制。

2.2" 改進的布谷鳥搜索算法

布谷鳥搜索算法是一種基于群體智能的優化算法，模擬了布谷鳥寄生在其他鳥巢中產卵的行為。原始布谷鳥搜索算法通過式（8）來更新巢穴位置：

式中，n表示比例因子，是（0，1）的均勻分布隨機數，post， j、post， k表示t代中的兩個隨機解。在改進的布谷鳥搜索算法中，通過式（9）來更新位置矩陣為nestN×Di的位置：

式中，種群規模為N，維度為D，每個維度的上、下限分別為upN×Di、lowN×Di；

在目標函數中，權重矩陣Q、R、P影響著AUV軌跡跟蹤效果。因此，需要設置合適的Q、R、P，使得AUV達到較好的跟蹤效果。傳統的控制器參數設置方法一般為試湊法，根據經驗代入參數值來觀察AUV的跟蹤效果。由于改進的布谷鳥搜索算法尋優精度、搜索活力較高。因此，這里我們使用改進的布谷鳥算法搜索合適的控制器權重矩陣的值。改進的布谷鳥搜索算法總結如下：

1）初始化。定義目標函數，將AUV軌跡跟蹤誤差的絕對值作為其目標函數，設置種群規模N = 100、搜索空間維數為21、最大發現概率Pa = 0.25、最大迭代次數為300，并隨機生成巢穴初始位置；

2）根據萊維飛行產生新的巢穴位置，保留適應度最高的巢穴位置；

3）選擇更新。丟棄不好的巢穴位置；計算目標函數的適應度值，將適應度較好的巢穴保留到下一代；

4）逐維更新。對步驟3）中保留下來的解，進行逐維學習。更新當前最優函數解并保存進化信息，記錄最優解；

5）結束判決。判斷當前解的質量是否滿足算法結束條件，若滿足算法結束條件，則結束，輸出最終的解；否則返回步驟2），直到滿足算法結束條件時為止。

3" 控制器設計

在AUV的軌跡跟蹤控制中，給定一個期望軌跡re（t） + [xd（t），yd（t），zd（t）]T，定義了AUV的位置。假設期望軌跡re（t）及其導數是光滑有界的（至少有二階導數）。根據期望軌跡計算出AUV的期望狀態值如式（10）所示：

式中，ud表示期望縱向速度；rd表示期望偏航角速度；wd表示z軸方向的速度；ψd表示期望偏航角。

同時，假設" 符合動力學方程（3），可有：

得到參考控制力 。

式中， 可通過對（7）求時間導數來計算。此外，由于滿足假設3，利用Moore-Penrose偽逆實現來解決推力分配子問題，得到各推進器的參考控制：

3.1" 非線性反步控制器

需要找到一個狀態反饋控制器以及相應的Lyapunov函數在原MPC公式中添加收縮約束來實現LMPC問題。針對AUV軌跡跟蹤問題，利用反步法設計基于Lyapunov的非線性控制器。定義如下變量：

式中， 表示位置跟蹤誤差。通過運動學方程（1），可得到：

將v作為軌跡跟蹤控制的虛擬控制量：

令 ，然后將其代入式（16）（17）可得：

首先選取李雅普諾夫函數為：

式中， 其是指定的控制增益矩陣。對Lyapunov函數求導可得：

進一步構造Lyapunov函數候選：

式中，：

對V2求導：

將動力學方程（3）代入可以得到：

式中，，：

通過假設1，可以證明 。

因此，選擇以下控制律：

其中，Kd＞0。

將式（24）代入式（22）可得：

根據假設2，可以得到 。然后根據標準Lyapunov參數，式（24）下的閉環系統對于平衡" 是全局漸近穩定的。

3.2" 模型預測控制器設計

3.2.1" 非線性狀態空間方程

根據前面的推導，可得到系統的非線性狀態方程為：

式中，。

然后對預測模型進行離散化。離散化后的狀態方程為：

其中，fd表示f離散化后的函數。

3.2.2" 目標函數

其中， 表示預測控制" 的預測狀態； 表示狀態誤差， 表示控制誤差；Np表示預測時域，Nc表示控制時域；權重矩陣Q、R、P是正定的；目標函數的第一項表示AUV跟蹤上期望軌跡的能力。第二項是控制輸入懲罰項，用來限制控制輸入的變化范圍。第三項是終端代價項，以在有限時間內將系統狀態推向最優穩態。

3.2.3" 約束項

在對目標函數進行求解時，還需要滿足下列約束。

控制量約束：

控制增量約束：

系統輸出量約束：

對于AUV非線性系統，采用標準的MPC設計技術，需要進行局部線性化，以選擇合適的加權矩陣并構造輔助的反饋控制律。為了避免局部線性化，同時保證MPC跟蹤控制的閉環穩定性，設計輔助的基于Lyapunov的非線性跟蹤控制律，作為收縮約束添加在原MPC公式中來實現LMPC問題。

其中，h（·）表示輔助的基于Lyapunov的非線性跟蹤控制律，V（·）對應的Lyapunov函數。收縮約束的存在能夠證明LMPC控制器繼承了狀態反饋控制 的穩定性屬性。

3.3" 模型預測控制穩定性分析

LMPC算法下的閉環系統相對于平衡點" 漸近穩定，即在基于LMPC的軌跡跟蹤控制下，AUV將收斂到期望軌跡。

證明：由于有（21）中的李雅普諾夫函數V2，連續可微且根本無界，由逆李雅普諾夫定理[13]，存在函數 ，i = 1，2，3，它們屬于" 類，使得下列不等式成立：

根據式（32），并且每次對一個采樣周期執行最優解 ，有：

通過Lyapunov論證，可以得出LMPC算法下的閉環系統是漸近穩定的。

4" 仿真實驗與分析

為驗證本文所提出的基于Lyapunov的模型預測控制方法對時變軌跡的跟蹤控制，在MATLAB上對正弦函數進行仿真實驗。仿真中所用到的模型參數和水動力阻尼系數都是基于Falcon。

用正弦函數來測試AUV的跟蹤控制：

對于LMPC控制器，使用以下參數：采樣周期δ = 0.1秒；預測范圍為T = 5δ；控制增益Kp = Kd = diag（1，1，1，1）。初始位置設置為[x，y，z，ψ]T = [2，-0.3，0，0]T，初始速度設置為[u，v，w，r]T = [0.1，0，0，0]T。

每個推進器的極限是500 N。由改進的布谷鳥搜索算法尋找的權重矩陣為：

軌跡跟蹤結果如圖3所示。分別為采用反步控制（BSC）的AUV仿真軌跡，采用LMPC控制的軌跡，以及期望軌跡。從圖中可以看出，BSC和LMPC都可以將AUV驅動到期望的軌跡，驗證了閉環穩定性。但顯然LMPC控制器的收斂速度比BSC控制器快得多。仿真結果驗證了在線優化后跟蹤控制性能的提高。從圖4可以看出剛開始時初始位置與期望位置相差較大，隨著AUV的調整，誤差逐漸較小，實現精確的軌跡跟蹤。圖5為軌跡跟蹤中的速度響應曲線。每個推進器所需的控制力如圖6所示。我們觀察到，在跟蹤開始時，LMPC控制器充分利用機載推力能力，以產生盡可能快的收斂，同時尊重推進器的物理極限，推力的大小保持在預期的允許范圍內。

5" 結" 論

在本文中，提出了一種新的LMPC算法用于自主水下航行器的軌跡跟蹤控制，對于控制器中的參數，利用改進的布谷鳥算法對其進行優化，可以大大提高控制性能和魯棒性。此外，推力分配子問題不需要單獨考慮，因為在線優化允許它與LMPC控制器設計同時求解，嚴格證明了LMPC控制的閉環穩定性。對參考軌跡的跟蹤仿真結果表明了所提出的基于LMPC的軌跡跟蹤控制的優越性。

參考文獻：

[1] 康帥，俞建成，張進.微小型自主水下機器人研究現狀 [J].機器人，2023，45（2）：218-237.

[2] 宋保維，潘光，張立川，等.自主水下航行器發展趨勢及關鍵技術 [J].中國艦船研究，2022，17（5）：27-44.

[3] CHO G R，LI J H，PARK D，et al. Robust Trajectory Tracking of Autonomous Underwater Vehicles Using Back-Stepping Control and Time Delay Estimation [J/OL].Ocean Engineering，2020，201：107131.（2020-03-05）.https：//doi.org/10.1016/j.oceaneng.2020.107131.

[4] WANG D，SHEN Y，WAN J，et al. Sliding Mode Heading Control for AUV Based on Continuous Hybrid Model-Free and Model-Based Reinforcement Learning [J/OL].Applied Ocean Research，2022，118：102960（2021-12-13）.https：//doi.org/10.1016/j.apor.2021.102960.

[5] XIONG Y，PAN L，XIAO M，et al. Motion Control and Path Optimization of Intelligent AUV Using Fuzzy Adaptive PID and Improved Genetic Algorithm [J].Mathematical Biosciences and Engineering，2023，20（5）：9208-9245.

[6] CUI R，YANG C，LI Y，et al. Adaptive Neural Network Control of AUVs with Control Input Nonlinearities Using reinforcement learning [J].IEEE Transactions on Systems， Man， and Cybernetics： Systems，2017，47（6）：1019-1029.

[7] SHEN C，SHI Y，BUCKHAM B. Integrated Path Planning and Tracking Control of an AUV： A Unified Receding Horizon Optimization Approach [J].IEEE/ASME Transactions on Mechatronics，2016，22（3）：1163-1173.

[8] YAN Z，GONG P，ZHANG W，et al. Model Predictive Control of Autonomous Underwater Vehicles for Trajectory Tracking with External Disturbances [J/OL].Ocean Engineering，2020，217：107884（2020-08-06）.https：//doi.org/10.1016/j.oceaneng.2020.107884.

[9] ZHANG Y，LIU X，LUO M，et al. MPC-Based 3-D Trajectory Tracking for an Autonomous Underwater Vehicle with Constraints in Complex Ocean Environments [J/OL].Ocean Engineering，2019，189：106309（2019-08-23）.https：//doi.org/10.1016/j.oceaneng.2019.106309.

[10] 趙洪壇，朱大奇.UUV水下模型預測滑模跟蹤控制算法 [J].控制工程，2022，29（7）：1195-1203.

[11] 黃閩茗，何慶，文熙.基于逐維反向學習的動態適應布谷鳥算法 [J].計算機應用研究，2020，37（4）：1015-1019.

作者簡介：石文會（1997—），女，漢族，河南周口人，碩士在讀，研究方向：無人水下航行器的軌跡跟蹤控制；金麗娜（1985—），女，漢族，遼寧大連人，講師，博士，研究方向：多智能體非線性控制；馬楠楠（1997—），女，漢族，河南南陽人，碩士在讀，研究方向：機器學習、機器人協同控制。