引言

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

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

1控制對象模型

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

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

,和v表達式如下:

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

RT(,)型表達式如下:

TRB的表達式如下:

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

MRB的表達式如下:

式中:m為UUV的質(zhì)量:xG,yG,:G為真實位置坐標的向量表達式:J(·)為轉(zhuǎn)動慣量。

CRB(v)的表達式如下:

其中,

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

2控制器設(shè)計

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

2.1控制器架構(gòu)

本文采用基于Los的虛擬向?qū)Хㄅc模型預(yù)測控制相結(jié)合,進行水平面控制?？刂破骷軜?gòu)如圖1所示。

2.2運動學控制器設(shè)計

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

路徑跟隨部分的算法設(shè)計分為以下三個步驟:運動學方程轉(zhuǎn)換、視線角導(dǎo)航、控制率設(shè)計。控制率的設(shè)計除了x軸方向的參考速度及贈向角的角速度,還增加了一個參考路徑切向速度nr=s·。相當于在參考路徑上加入了一個虛擬向?qū)?艇的控制中多了一個自由度的控制量。以下是具體的算法設(shè)計步驟。

2.2.1運動學方程轉(zhuǎn)換

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

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

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

式(10)可具體寫成:

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

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

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

2.2.2視線角導(dǎo)航

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

設(shè)(△,0)為下一參考目標點的位置,s-F坐標系中的視線角表達式為

其等效正弦表達式為

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

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

2.2.3控制率設(shè)計

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

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

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

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

對該函數(shù)進行微分:

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

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

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

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

綜上,運動學控制率為:

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

2.3動力學控制器設(shè)計

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

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

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

具體的動力學MPC控制器設(shè)計如下:

首先對動力學關(guān)系式進行泰勒級數(shù)展開,忽略高階項只保留一階項,即對非線性模型線性化:

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

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

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

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

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

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

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

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

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

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

轉(zhuǎn)換為標準二次型形式如下:

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

由于目標函數(shù)中沒有關(guān)于控制量本身的計算,只有關(guān)于控制量增量的計算,因此需要將控制量約束轉(zhuǎn)變成增量的約束,即需要對式(+9)進行轉(zhuǎn)換,因為存在如下關(guān)系:

可令:

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

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

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

3仿真驗證

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

3.1正弦曲線

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

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

3.2直線

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

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

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

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

4結(jié)語

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