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

摘" 要：文章研究了自主水下航行器（AUV）的軌跡跟蹤控制問題。為了提高自主水下航行器的軌跡跟蹤性能，提出一種基于Lyapunov的模型預測控制（LMPC）方法來設計控制器。首先，基于該方法，考慮執(zhí)行器飽和等實際約束，設計非線性反步跟蹤控制律，在基于Lyapunov的模型預測控制問題中構(gòu)造約束條件，使閉環(huán)的穩(wěn)定性得到保證。其次，傳統(tǒng)的控制器參數(shù)設置方法一般為試湊法，根據(jù)經(jīng)驗代入不同參數(shù)觀察AUV的跟蹤效果。對于其中的權重矩陣，采用改進的布谷鳥算法進行優(yōu)化。最后，在MATLAB上的仿真結(jié)果表明，所提出的方法顯著提高了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]。由于其在各種潛艇項目中有大幅降低風險和運營成本的潛力，引起了工業(yè)界和學術界越來越多的關注[2]。AUV的模型具有非線性、不確定性和高耦合性，因此在研究AUV的相關問題中常采用欠驅(qū)動的形式。軌跡跟蹤作為AUV控制研究的重要內(nèi)容，其控制魯棒性和運行穩(wěn)定性有待提高。

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

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

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

1" AUV模型

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

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

AUV的運動學模型為：

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

AUV的動力學模型為：

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

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

式中，N是一個常數(shù)輸入矩陣。

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

式中，狀態(tài)定義為 ，控制定義為 。

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

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

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

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

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

2" 相關算法介紹

2.1" 模型預測控制算法

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

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

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

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

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

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

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

2）根據(jù)萊維飛行產(chǎn)生新的巢穴位置，保留適應度最高的巢穴位置；

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

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

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

3" 控制器設計

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

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

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

得到參考控制力 。

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

3.1" 非線性反步控制器

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

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

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

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

首先選取李雅普諾夫函數(shù)為：

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

進一步構(gòu)造Lyapunov函數(shù)候選：

式中，：

對V2求導：

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

式中，，：

通過假設1，可以證明 。

因此，選擇以下控制律：

其中，Kd＞0。

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

根據(jù)假設2，可以得到 。然后根據(jù)標準Lyapunov參數(shù)，式（24）下的閉環(huán)系統(tǒng)對于平衡" 是全局漸近穩(wěn)定的。

3.2" 模型預測控制器設計

3.2.1" 非線性狀態(tài)空間方程

根據(jù)前面的推導，可得到系統(tǒng)的非線性狀態(tài)方程為：

式中，。

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

其中，fd表示f離散化后的函數(shù)。

3.2.2" 目標函數(shù)

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

3.2.3" 約束項

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

控制量約束：

控制增量約束：

系統(tǒng)輸出量約束：

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

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

3.3" 模型預測控制穩(wěn)定性分析

LMPC算法下的閉環(huán)系統(tǒng)相對于平衡點" 漸近穩(wěn)定，即在基于LMPC的軌跡跟蹤控制下，AUV將收斂到期望軌跡。

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

根據(jù)式（32），并且每次對一個采樣周期執(zhí)行最優(yōu)解 ，有：

通過Lyapunov論證，可以得出LMPC算法下的閉環(huán)系統(tǒng)是漸近穩(wěn)定的。

4" 仿真實驗與分析

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

用正弦函數(shù)來測試AUV的跟蹤控制：

對于LMPC控制器，使用以下參數(shù)：采樣周期δ = 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。由改進的布谷鳥搜索算法尋找的權重矩陣為：

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

5" 結(jié)" 論

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

參考文獻：

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

[2] 宋保維，潘光，張立川，等.自主水下航行器發(fā)展趨勢及關鍵技術 [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] 黃閩茗，何慶，文熙.基于逐維反向?qū)W習的動態(tài)適應布谷鳥算法 [J].計算機應用研究，2020，37（4）：1015-1019.

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