基于大型UUV的MPC解耦路徑跟蹤控制

詹康怡 馬斌

（中國艦船研究設計中心，湖北武漢 430064）

0 引言

針對大型UUV的大慣性特性帶來的控制響應較慢、控制效果滯后的現象，運動控制算法的選擇須充分考慮對航行器趨勢的預判與反饋修正，同時必須考慮到艉方向舵和螺旋槳的能力有限，不能頻繁改變作用量。本文針對上述要求設計了控制算法和策略。

本文研究的對象是大型欠驅動對象，模型復雜。考慮到其大慣性的特點，當有較為精確的大型UUV模型時，MPC可以提高控制器對大型UUV未來一段時間內運動趨勢的預測能力。為利用MPC預測控制的優點，并避免MPC在求解大自由度非線性系統時容易發散以及計算量大，不能滿足實時計算的問題，針對大多數航行條件下UUV的水平面和深度面之間的弱耦合關系，將問題解耦為水平面控制與垂直面控制。對于水平面控制，進一步簡化為運動學控制和動力學控制。采用基于視線角的虛擬導引法作為運動學控制器，首先在Serret-Frenet坐標系中進行視線角導航，并根據此導航角度設計控制率，最終控制率作為參考值輸入動力學控制器；動力學控制器運用MPC算法，采用二次規劃方法求得最優解。最后，在Simulink模型中使用S函數將這兩部分結合起來，形成完整的控制器，并設計了仿真試驗，仿真結果證明了該算法的有效性。

1 控制對象模型

考慮到水下無人航行器在大多數工況條件下，水平面與垂直面的運動耦合性不強，為突出體現算法，簡化模型便于研究，本文僅對所控制對象模型的水平面模型進行研究。在僅考慮水平面的運動時可忽略垂蕩、縱傾、橫搖的相關運動學及動力學特性[1]，但為展示整體模型，在本節中仍寫出完整模型，而在算法設計章節中分別展示解耦后的模型。完整運動學模型如下[2]：

式中：η為UUV所有狀態量的值所組成的向量；RT（η）為動系到定系的轉換矩陣；v為狀態量的變化速度；MRB（v）∈R6×6為慣性矩陣；CRB（v）∈R6×6為船體科里奧利向心力矩陣；τRB為UUV所受外力及外力矩所組成的向量。

η和v表達式如下：

式中：x，y，z，φ，θ，ψ為狀態量；u，v，w，p，q，r為狀態量的變化量。

RT（η）的表達式如下：

式中：τH表示流體水動力（流體慣性力、流體黏性力）；τP表示靜力（重力、浮力）；wH表示海洋環境力；τT表示推進器力；τR表示舵力；X∑，Y∑，Z∑為外力；K∑，M∑，N∑為外力的力矩。

MRB的表達式如下：

式中：m為UUV的質量；xG，yG，zG為真實位置坐標的向量表達式；J（·）為轉動慣量。

CRB（v）的表達式如下：

根據所建立的六自由度模型，配合大型UUV水動力系數，得到后續仿真研究所使用的大型UUV數字模型。

2 控制器設計

考慮到在大多數工況下，水下航行器水平面與深度面的控制耦合性不強，針對水平面和深度面可單獨設計控制器，下面僅以水平面為例對本文所設計的方法進行說明。

2.1 控制器架構

本文采用基于LOS的虛擬向導法與模型預測控制相結合，進行水平面控制[3-4]。控制器架構如圖1所示。

圖1 控制器整體框架

2.2 運動學控制器設計

該部分對解耦后的水平面運動學模型進行控制，在無海流影響下，可得所研究的模型的水平面運動學模型，具體方程如下：

路徑跟隨部分的算法設計分為以下三個步驟：運動學方程轉換、視線角導航、控制率設計。控制率的設計除了x軸方向的參考速度及艏向角的角速度，還增加了一個參考路徑切向速度vr=s˙。相當于在參考路徑上加入了一個虛擬向導，艇的控制中多了一個自由度的控制量。以下是具體的算法設計步驟。

2.2.1 運動學方程轉換

Serret-Frenet坐標系是以參考軌跡上的點為原點，以該點引出軌跡的切向方向為x軸的坐標系，該坐標系的應用可簡化側漂角以及視線角的表達。該坐標系中運動學方程的建立，需要將慣性坐標系中的運動學方程進行轉換，即左乘旋轉矩陣，該旋轉矩陣為慣性坐標系到Serret-Frenet坐標系的矩陣，圖2為模型在Serret-Frenet坐標系中的示意圖。

圖2 模型在Serret-Frenet坐標系中的示意圖

得到模型在Serret-Frenet坐標系中的誤差模型表達式如下：

式中：RIBF為簡化模型的轉化矩陣；pr為參考值組成的向量。

式（10）可具體寫成：

式中：pe為誤差值組成的向量；xe，ye，ψe為誤差值；xr，yr，ψr為參考值。

誤差模型的一階導數可寫成如下公式：

式中：ωr為參考角速度；ωw為實際艏向角速度；vr為UUV橫向速度參考值；vt為合成速度

2.2.2 視線角導航

這部分主要用于計算在Serret-Frenet坐標系中視線角的表達方法，圖3為Serret-Frenet坐標系中視線角的表達。

圖3 Serret-Frenet坐標系中視線角的表達

2.2.3 控制率設計

給定參考路徑s以及前向運動速度ud，跟蹤誤差pe=

式中：k1為正系數。

再選定另外一個Lyapunov函數：

對該函數進行微分：

式中：ψw為合成速度vt與固定坐標系x軸的夾角；vr為UUV橫向速度參考值。

同理，若要令V˙2≤0，即令V2為單調非增函數，可得速度控制率：

式中：k2為正系數。

綜上，運動學控制率為：

式中：vr為速度控制率，可將其視為路徑上“虛擬向導點”的速度，會根據無人航行器與參考路徑的位置關系即xe的值調整其自身數值大小，以引導無人航行器加速或減速運動。

2.3 動力學控制器設計

航向控制部分運用模型預測算法。對于該部分的控制器設計，需假設在此之前的軌跡跟隨控制器可以實現一個“完美”的跟蹤控制，令uref=vr，rref=rc，即將vr、rc作為參考值輸入航向控制器。

該部分對解耦后的水平面動力學模型進行控制，將水平面動力學方程解耦出來，X2=[u，v，r]T為被控量，U2=[n，dr]T為控制量，有如下形式的動力學關系：

本文運用S-function將控制器與模型建立關系，可隨時間變化不斷將更新的控制量的值送入被控對象模型，同時再把模型的值輸出到控制器，達到迭代更新的效果。

具體的動力學MPC控制器設計如下[4]：

首先對動力學關系式進行泰勒級數展開，忽略高階項只保留一階項，即對非線性模型線性化：

式中：f（Xr，Ur）為參考點處的值，Xr為狀態量參考值，Ur為控制量參考值；X為狀態量實際值；U為控制量實際值。

將上述兩式相減得線性化的誤差模型：

根據線性化的誤差模型公式，運用前向歐拉法進行離散化處理：

式中：A、B為雅克比矩陣。

為便于轉化為標準二次型需要對式（24）進行適應化修改，如下：

經過推導，可得預測的輸出如下：

式中：Y（t）為預測輸出值；ψt、θt為系數及參數組成的矩陣。

接下來進行目標函數的設計，為便于對每個采樣周期里的增量進行控制，在傳統二次規劃函數中添加約束ρε2，可將目標函數寫成如下形式：

式中：Np為預測步長；Nc為控制步長；ΔU為控制量的變化值。

轉換為標準二次型形式如下：

接下來進行約束條件設置，需要考慮的是控制量的表達式與控制增量的表達式，實際情況可以寫成如下形式：

由于目標函數中沒有關于控制量本身的計算，只有關于控制量增量的計算，因此需要將控制量約束轉變成增量的約束，即需要對式（29）進行轉換，因為存在如下關系：

可令：

式中：1Nc為行數是Nc的列向量；?為克羅內克積（Kronecker product）。

綜合上述分析，可將式（29）轉換為如下形式：

本文中設計的約束條件為：

3 仿真驗證

本節針對設計的控制器及特定的大型航行器被控對象，分別設計了正弦曲線及直線為參考路徑進行仿真，以驗證本文算法的有效性。

3.1 正弦曲線

參考軌跡如下，式中β為側漂角，可利用上文中的側漂角計算模塊來計算。

圖4、圖5為具體的正弦路徑跟蹤仿真圖，由圖可知，本文所設計的算法與MPC全控算法都能跟上參考軌跡，但可以觀察到，MPC全控算法的誤差較大，特別是在路徑轉彎處偏離最大，而本文所設計算法誤差更小，且相對于MPC全控算法能較快收斂至參考軌跡；所設計控制器的兩個控制量的變化基本也同狀態量趨勢一致，艉方向舵穩定后呈周期變化，轉速穩定后為定值，而艏向角也能迅速反應并呈周期變化。

圖4 正弦曲線跟蹤效果圖

圖5 本文算法控制量變化曲線

3.2 直線

參考軌跡設定如下，設定一起點不為零的斜線。

圖6、圖7為具體的直線軌跡跟蹤仿真圖，由圖可知，本文所設計的算法與MPC全控算法都能跟上參考軌跡，但可以觀察到，本文所設計算法收斂速度較快，與參考軌跡的誤差較小，而MPC全控算法收斂速度相比較來說更慢，且在初始運動時與參考軌跡偏差較大；所設計控制器的兩個控制量的變化基本也同狀態量趨勢一致，都在穩定后收斂于定值，與參考軌跡相符。

圖6 直線跟蹤效果圖

圖7 本文算法控制量變化曲線

下面對比了本文算法與MPC全控算法每輪計算的時間，設計仿真實驗分別統計了兩種算法的計算時間，圖8為具體仿真圖。

由圖8可以發現，本文所設計的算法每輪計算時間平均在0.005 s左右，而MPC每輪計算時間平均在0.015 s左右，由此驗證本文算法在節省計算時間上的確有較明顯的優勢。

圖8 本文算法與全MPC算法每輪計算時間對比

4 結語

本文針對大型欠驅動UUV設計了一種軌跡跟蹤算法。首先對復雜問題進行分解，在大多數導航條件下只分析水平面。然后，將該問題解耦為運動學控制和動力學控制問題。對于運動學控制器，采用基于在Serret-Frenet坐標系中進行LOS角導航的虛擬制導方法；對于動力學控制器，采用模型預測方法設計水平路徑跟蹤控制器。通過與全MPC的控制效果比較，發現控制效果及計算時間均得到了改善。