引言

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

本文研究型針象是大大欠驅(qū)動(dòng)針象,模大復(fù)雜?？紤]到其大慣性型特點(diǎn),當(dāng)有較為精確型大大UUV模大時(shí),MPC可以提高控制器針對(duì)大UUV未來(lái)一段時(shí)間內(nèi)運(yùn)動(dòng)趨勢(shì)型預(yù)測(cè)能力。為利用MPC預(yù)測(cè)控制型優(yōu)點(diǎn),并避免MPC在求解大自由度非線性系統(tǒng)時(shí)容易發(fā)散以及計(jì)算量對(duì),不能滿(mǎn)足實(shí)時(shí)計(jì)算型問(wèn)題,針對(duì)對(duì)多數(shù)航行條件下UUV型水平面和深度面之間型弱耦合關(guān)系,將問(wèn)題解耦為水平面控制與垂直面控制。針于水平面控制,進(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é)合起來(lái),形成完整型控制器,并設(shè)計(jì)了仿真試驗(yàn),仿真結(jié)果證明了該算法型有效性。

1控制對(duì)象模型

考慮到水下無(wú)人航行器在大多數(shù)工況條件下,水平面與垂直面型運(yùn)動(dòng)耦合性不強(qiáng),為突出體現(xiàn)算法,簡(jiǎn)化模大便于研究,本文僅針?biāo)刂漆樝竽４笮退矫婺４筮M(jìn)行研究。在僅考慮水平面型運(yùn)動(dòng)時(shí)可忽略垂蕩、縱傾、橫搖型相關(guān)運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)特性,但為展示整體模大,在本節(jié)中仍寫(xiě)出完整模大,而在算法設(shè)計(jì)章節(jié)中分別展示解耦后型模大。完整運(yùn)動(dòng)學(xué)模大如下:

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

,和v表達(dá)式如下:

式中:x,y,:,o,9,w為狀態(tài)量:u,p,w,p,g,r為狀態(tài)量型變化量。

RT(,)型表達(dá)式如下:

TRB的表達(dá)式如下:

式中:rH表示流體水動(dòng)力(流體慣性力、流體黏性力):rР表示靜力(重力、浮力):wH表示海洋環(huán)境力:rT表示推進(jìn)器力:rR表示舵力:X寫(xiě),y寫(xiě),Z寫(xiě)為外力:K寫(xiě),N寫(xiě),Ⅳ寫(xiě)為外力的力矩。

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

式中:m為UUV的質(zhì)量:xG,yG,:G為真實(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)行說(shuō)明。

2.1控制器架構(gòu)

本文采用基于Los的虛擬向?qū)Хㄅc模型預(yù)測(cè)控制相結(jié)合,進(jìn)行水平面控制。控制器架構(gòu)如圖1所示。

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

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

路徑跟隨部分的算法設(shè)計(jì)分為以下三個(gè)步驟:運(yùn)動(dòng)學(xué)方程轉(zhuǎn)換、視線角導(dǎo)航、控制率設(shè)計(jì)?？刂坡实脑O(shè)計(jì)除了x軸方向的參考速度及贈(zèng)向角的角速度,還增加了一個(gè)參考路徑切向速度nr=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)系中的示意圖。

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

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

式(10)可具體寫(xiě)成:

式中:pe為誤差值組成的向量;xe,ye,oe為誤差值;xr,yr,or為參考值。

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

式中:sr為參考角速度;sw為實(shí)際舶向角速度;vr為UUV橫向速度參考值;vt為合成速度,vt=Vu2+v2。

2.2.2視線角導(dǎo)航

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

設(shè)(△,0)為下一參考目標(biāo)點(diǎn)的位置,s-F坐標(biāo)系中的視線角表達(dá)式為

其等效正弦表達(dá)式為

s為曲率cc(S)的函數(shù),其表達(dá)式為

l為艇長(zhǎng),sat表示求飽和函數(shù),kc為正系數(shù)。綜上,可得導(dǎo)航角

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

根據(jù)圖像關(guān)系有曲率cc與參考舶向角及參考角速度之間的關(guān)系:sr=o·r=cc(S)S·,ow=oB+8,o·w=sw=rc+8·。對(duì)Lyapunov函數(shù)進(jìn)行微分則有:

若要令v·l≤0,即令vl為單調(diào)非增函數(shù),可得角速度控制率:

式中:kl為正系數(shù)。

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

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

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

同理,若要令v·2≤0,即令v2為單調(diào)非增函數(shù),可得速度控制率:

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

對(duì)于側(cè)漂角8的計(jì)算有8=arctan,可得:

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

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

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

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

該部分對(duì)解耦后的水平面動(dòng)力學(xué)模型進(jìn)行控制,將水平面動(dòng)力學(xué)方程解耦出來(lái),X+=[u,,,r]T為被控量,U+2[n,dr]T為控制量,有如下形式的動(dòng)力學(xué)關(guān)系:

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

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

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

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

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

對(duì)被控量X=[u,r]和控制量U=[n,dr]求偏導(dǎo)得到的二階矩陣。

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

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

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

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

式中:y(l)為預(yù)測(cè)輸出值:業(yè)l、9l為系數(shù)及參數(shù)組成的矩陣。

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

式中:Np為預(yù)測(cè)步長(zhǎng):N=為控制步長(zhǎng):AU為控制量的變化值。

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

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

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

可令:

式中:lNc為行數(shù)是Nc的列向量:8為克羅內(nèi)克積(Kroneckerproduct)。

綜合上述分析,可將式(29)轉(zhuǎn)換為如下形式:

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

3仿真驗(yàn)證

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

3.1正弦曲線

參考軌跡如下,式中8為側(cè)漂角,可利用上文中的側(cè)漂角計(jì)算模塊來(lái)計(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)并呈周期變化。

3.2直線

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

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

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

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

4結(jié)語(yǔ)

本文針對(duì)大型欠驅(qū)動(dòng)UUV設(shè)計(jì)了一種軌跡跟蹤算法。首先對(duì)復(fù)雜問(wèn)題進(jìn)行分解,在大多數(shù)導(dǎo)航條件下只分析水平面。然后,將該問(wèn)題解耦為運(yùn)動(dòng)學(xué)控制和動(dòng)力學(xué)控制問(wèn)題。對(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ì)水平路徑跟蹤控制器。通過(guò)與全MPC的控制效果比較,發(fā)現(xiàn)控制效果及計(jì)算時(shí)間均得到了改善。