詹康怡 馬斌

（中國艦船研究設(shè)計(jì)中心，湖北武漢 430064）

0 引言

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

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

1 控制對(duì)象模型

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

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

η和v表達(dá)式如下：

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

RT（η）的表達(dá)式如下：

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

MRB的表達(dá)式如下：

式中：m為UUV的質(zhì)量；xG，yG，zG為真實(shí)位置坐標(biāo)的向量表達(dá)式；J（·）為轉(zhuǎn)動(dòng)慣量。

CRB（v）的表達(dá)式如下：

根據(jù)所建立的六自由度模型，配合大型UUV水動(dòng)力系數(shù)，得到后續(xù)仿真研究所使用的大型UUV數(shù)字模型。

2 控制器設(shè)計(jì)

考慮到在大多數(shù)工況下，水下航行器水平面與深度面的控制耦合性不強(qiáng)，針對(duì)水平面和深度面可單獨(dú)設(shè)計(jì)控制器，下面僅以水平面為例對(duì)本文所設(shè)計(jì)的方法進(jìn)行說明。

2.1 控制器架構(gòu)

本文采用基于LOS的虛擬向?qū)Хㄅc模型預(yù)測(cè)控制相結(jié)合，進(jìn)行水平面控制[3-4]?？刂破骷軜?gòu)如圖1所示。

圖1 控制器整體框架

2.2 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

該部分對(duì)解耦后的水平面運(yùn)動(dòng)學(xué)模型進(jìn)行控制，在無海流影響下，可得所研究的模型的水平面運(yùn)動(dòng)學(xué)模型，具體方程如下：

路徑跟隨部分的算法設(shè)計(jì)分為以下三個(gè)步驟：運(yùn)動(dòng)學(xué)方程轉(zhuǎn)換、視線角導(dǎo)航、控制率設(shè)計(jì)?？刂坡实脑O(shè)計(jì)除了x軸方向的參考速度及艏向角的角速度，還增加了一個(gè)參考路徑切向速度vr=s˙。相當(dāng)于在參考路徑上加入了一個(gè)虛擬向?qū)ВУ目刂浦卸嗔艘粋€(gè)自由度的控制量。以下是具體的算法設(shè)計(jì)步驟。

2.2.1 運(yùn)動(dòng)學(xué)方程轉(zhuǎn)換

Serret-Frenet坐標(biāo)系是以參考軌跡上的點(diǎn)為原點(diǎn)，以該點(diǎn)引出軌跡的切向方向?yàn)閤軸的坐標(biāo)系，該坐標(biāo)系的應(yīng)用可簡(jiǎn)化側(cè)漂角以及視線角的表達(dá)。該坐標(biāo)系中運(yùn)動(dòng)學(xué)方程的建立，需要將慣性坐標(biāo)系中的運(yùn)動(dòng)學(xué)方程進(jìn)行轉(zhuǎn)換，即左乘旋轉(zhuǎn)矩陣，該旋轉(zhuǎn)矩陣為慣性坐標(biāo)系到Serret-Frenet坐標(biāo)系的矩陣，圖2為模型在Serret-Frenet坐標(biāo)系中的示意圖。

圖2 模型在Serret-Frenet坐標(biāo)系中的示意圖

得到模型在Serret-Frenet坐標(biāo)系中的誤差模型表達(dá)式如下：

式中：RIBF為簡(jiǎn)化模型的轉(zhuǎn)化矩陣；pr為參考值組成的向量。

式（10）可具體寫成：

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

誤差模型的一階導(dǎo)數(shù)可寫成如下公式：

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

2.2.2 視線角導(dǎo)航

這部分主要用于計(jì)算在Serret-Frenet坐標(biāo)系中視線角的表達(dá)方法，圖3為Serret-Frenet坐標(biāo)系中視線角的表達(dá)。

圖3 Serret-Frenet坐標(biāo)系中視線角的表達(dá)

2.2.3 控制率設(shè)計(jì)

給定參考路徑s以及前向運(yùn)動(dòng)速度ud，跟蹤誤差pe=

式中：k1為正系數(shù)。

再選定另外一個(gè)Lyapunov函數(shù)：

對(duì)該函數(shù)進(jìn)行微分：

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

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

式中：k2為正系數(shù)。

綜上，運(yùn)動(dòng)學(xué)控制率為：

式中：vr為速度控制率，可將其視為路徑上“虛擬向?qū)c(diǎn)”的速度，會(huì)根據(jù)無人航行器與參考路徑的位置關(guān)系即xe的值調(diào)整其自身數(shù)值大小，以引導(dǎo)無人航行器加速或減速運(yùn)動(dòng)。

2.3 動(dòng)力學(xué)控制器設(shè)計(jì)

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

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

本文運(yùn)用S-function將控制器與模型建立關(guān)系，可隨時(shí)間變化不斷將更新的控制量的值送入被控對(duì)象模型，同時(shí)再把模型的值輸出到控制器，達(dá)到迭代更新的效果。

具體的動(dòng)力學(xué)MPC控制器設(shè)計(jì)如下[4]：

首先對(duì)動(dòng)力學(xué)關(guān)系式進(jìn)行泰勒級(jí)數(shù)展開，忽略高階項(xiàng)只保留一階項(xiàng)，即對(duì)非線性模型線性化：

式中：f（Xr，Ur）為參考點(diǎn)處的值，Xr為狀態(tài)量參考值，Ur為控制量參考值；X為狀態(tài)量實(shí)際值；U為控制量實(shí)際值。

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

根據(jù)線性化的誤差模型公式，運(yùn)用前向歐拉法進(jìn)行離散化處理：

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

為便于轉(zhuǎn)化為標(biāo)準(zhǔn)二次型需要對(duì)式（24）進(jìn)行適應(yīng)化修改，如下：

經(jīng)過推導(dǎo)，可得預(yù)測(cè)的輸出如下：

式中：Y（t）為預(yù)測(cè)輸出值；ψt、θt為系數(shù)及參數(shù)組成的矩陣。

接下來進(jìn)行目標(biāo)函數(shù)的設(shè)計(jì)，為便于對(duì)每個(gè)采樣周期里的增量進(jìn)行控制，在傳統(tǒng)二次規(guī)劃函數(shù)中添加約束ρε2，可將目標(biāo)函數(shù)寫成如下形式：

式中：Np為預(yù)測(cè)步長；Nc為控制步長；ΔU為控制量的變化值。

轉(zhuǎn)換為標(biāo)準(zhǔn)二次型形式如下：

接下來進(jìn)行約束條件設(shè)置，需要考慮的是控制量的表達(dá)式與控制增量的表達(dá)式，實(shí)際情況可以寫成如下形式：

由于目標(biāo)函數(shù)中沒有關(guān)于控制量本身的計(jì)算，只有關(guān)于控制量增量的計(jì)算，因此需要將控制量約束轉(zhuǎn)變成增量的約束，即需要對(duì)式（29）進(jìn)行轉(zhuǎn)換，因?yàn)榇嬖谌缦玛P(guān)系：

可令：

式中：1Nc為行數(shù)是Nc的列向量；?為克羅內(nèi)克積（Kronecker product）。

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

本文中設(shè)計(jì)的約束條件為：

3 仿真驗(yàn)證

本節(jié)針對(duì)設(shè)計(jì)的控制器及特定的大型航行器被控對(duì)象，分別設(shè)計(jì)了正弦曲線及直線為參考路徑進(jìn)行仿真，以驗(yàn)證本文算法的有效性。

3.1 正弦曲線

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

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

圖4 正弦曲線跟蹤效果圖

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

3.2 直線

參考軌跡設(shè)定如下，設(shè)定一起點(diǎn)不為零的斜線。

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

圖6 直線跟蹤效果圖

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

下面對(duì)比了本文算法與MPC全控算法每輪計(jì)算的時(shí)間，設(shè)計(jì)仿真實(shí)驗(yàn)分別統(tǒng)計(jì)了兩種算法的計(jì)算時(shí)間，圖8為具體仿真圖。

由圖8可以發(fā)現(xiàn)，本文所設(shè)計(jì)的算法每輪計(jì)算時(shí)間平均在0.005 s左右，而MPC每輪計(jì)算時(shí)間平均在0.015 s左右，由此驗(yàn)證本文算法在節(jié)省計(jì)算時(shí)間上的確有較明顯的優(yōu)勢(shì)。

圖8 本文算法與全MPC算法每輪計(jì)算時(shí)間對(duì)比

4 結(jié)語

本文針對(duì)大型欠驅(qū)動(dòng)UUV設(shè)計(jì)了一種軌跡跟蹤算法。首先對(duì)復(fù)雜問題進(jìn)行分解，在大多數(shù)導(dǎo)航條件下只分析水平面。然后，將該問題解耦為運(yùn)動(dòng)學(xué)控制和動(dòng)力學(xué)控制問題。對(duì)于運(yùn)動(dòng)學(xué)控制器，采用基于在Serret-Frenet坐標(biāo)系中進(jìn)行LOS角導(dǎo)航的虛擬制導(dǎo)方法；對(duì)于動(dòng)力學(xué)控制器，采用模型預(yù)測(cè)方法設(shè)計(jì)水平路徑跟蹤控制器。通過與全MPC的控制效果比較，發(fā)現(xiàn)控制效果及計(jì)算時(shí)間均得到了改善。