詹康怡 馬斌
(中國艦船研究設(shè)計中心,湖北武漢 430064)
針對大型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é)果證明了該算法的有效性。
考慮到水下無人航行器在大多數(shù)工況條件下,水平面與垂直面的運動耦合性不強,為突出體現(xiàn)算法,簡化模型便于研究,本文僅對所控制對象模型的水平面模型進行研究。在僅考慮水平面的運動時可忽略垂蕩、縱傾、橫搖的相關(guān)運動學及動力學特性[1],但為展示整體模型,在本節(jié)中仍寫出完整模型,而在算法設(shè)計章節(jié)中分別展示解耦后的模型。完整運動學模型如下[2]:
式中:η為UUV所有狀態(tài)量的值所組成的向量;RT(η)為動系到定系的轉(zhuǎn)換矩陣;v為狀態(tài)量的變化速度;MRB(v)∈R6×6為慣性矩陣;CRB(v)∈R6×6為船體科里奧利向心力矩陣;τRB為UUV所受外力及外力矩所組成的向量。
η和v表達式如下:
式中:x,y,z,φ,θ,ψ為狀態(tài)量;u,v,w,p,q,r為狀態(tài)量的變化量。
RT(η)的表達式如下:
式中:τH表示流體水動力(流體慣性力、流體黏性力);τP表示靜力(重力、浮力);wH表示海洋環(huán)境力;τT表示推進器力;τR表示舵力;X∑,Y∑,Z∑為外力;K∑,M∑,N∑為外力的力矩。
MRB的表達式如下:
式中:m為UUV的質(zhì)量;xG,yG,zG為真實位置坐標的向量表達式;J(·)為轉(zhuǎn)動慣量。
CRB(v)的表達式如下:
根據(jù)所建立的六自由度模型,配合大型UUV水動力系數(shù),得到后續(xù)仿真研究所使用的大型UUV數(shù)字模型。
考慮到在大多數(shù)工況下,水下航行器水平面與深度面的控制耦合性不強,針對水平面和深度面可單獨設(shè)計控制器,下面僅以水平面為例對本文所設(shè)計的方法進行說明。
本文采用基于LOS的虛擬向?qū)Хㄅc模型預(yù)測控制相結(jié)合,進行水平面控制[3-4]??刂破骷軜?gòu)如圖1所示。
圖1 控制器整體框架
該部分對解耦后的水平面運動學模型進行控制,在無海流影響下,可得所研究的模型的水平面運動學模型,具體方程如下:
路徑跟隨部分的算法設(shè)計分為以下三個步驟:運動學方程轉(zhuǎn)換、視線角導(dǎo)航、控制率設(shè)計??刂坡实脑O(shè)計除了x軸方向的參考速度及艏向角的角速度,還增加了一個參考路徑切向速度vr=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坐標系中的示意圖。
圖2 模型在Serret-Frenet坐標系中的示意圖
得到模型在Serret-Frenet坐標系中的誤差模型表達式如下:
式中:RIBF為簡化模型的轉(zhuǎn)化矩陣;pr為參考值組成的向量。
式(10)可具體寫成:
式中:pe為誤差值組成的向量;xe,ye,ψe為誤差值;xr,yr,ψr為參考值。
誤差模型的一階導(dǎo)數(shù)可寫成如下公式:
式中:ωr為參考角速度;ωw為實際艏向角速度;vr為UUV橫向速度參考值;vt為合成速度
2.2.2 視線角導(dǎo)航
這部分主要用于計算在Serret-Frenet坐標系中視線角的表達方法,圖3為Serret-Frenet坐標系中視線角的表達。
圖3 Serret-Frenet坐標系中視線角的表達
2.2.3 控制率設(shè)計
給定參考路徑s以及前向運動速度ud,跟蹤誤差pe=
式中:k1為正系數(shù)。
再選定另外一個Lyapunov函數(shù):
對該函數(shù)進行微分:
式中:ψw為合成速度vt與固定坐標系x軸的夾角;vr為UUV橫向速度參考值。
同理,若要令V˙2≤0,即令V2為單調(diào)非增函數(shù),可得速度控制率:
式中:k2為正系數(shù)。
綜上,運動學控制率為:
式中:vr為速度控制率,可將其視為路徑上“虛擬向?qū)c”的速度,會根據(jù)無人航行器與參考路徑的位置關(guān)系即xe的值調(diào)整其自身數(shù)值大小,以引導(dǎo)無人航行器加速或減速運動。
航向控制部分運用模型預(yù)測算法。對于該部分的控制器設(shè)計,需假設(shè)在此之前的軌跡跟隨控制器可以實現(xiàn)一個“完美”的跟蹤控制,令uref=vr,rref=rc,即將vr、rc作為參考值輸入航向控制器。
該部分對解耦后的水平面動力學模型進行控制,將水平面動力學方程解耦出來,X2=[u,v,r]T為被控量,U2=[n,dr]T為控制量,有如下形式的動力學關(guān)系:
本文運用S-function將控制器與模型建立關(guān)系,可隨時間變化不斷將更新的控制量的值送入被控對象模型,同時再把模型的值輸出到控制器,達到迭代更新的效果。
具體的動力學MPC控制器設(shè)計如下[4]:
首先對動力學關(guān)系式進行泰勒級數(shù)展開,忽略高階項只保留一階項,即對非線性模型線性化:
式中:f(Xr,Ur)為參考點處的值,Xr為狀態(tài)量參考值,Ur為控制量參考值;X為狀態(tài)量實際值;U為控制量實際值。
將上述兩式相減得線性化的誤差模型:
根據(jù)線性化的誤差模型公式,運用前向歐拉法進行離散化處理:
式中:A、B為雅克比矩陣。
為便于轉(zhuǎn)化為標準二次型需要對式(24)進行適應(yīng)化修改,如下:
經(jīng)過推導(dǎo),可得預(yù)測的輸出如下:
式中:Y(t)為預(yù)測輸出值;ψt、θt為系數(shù)及參數(shù)組成的矩陣。
接下來進行目標函數(shù)的設(shè)計,為便于對每個采樣周期里的增量進行控制,在傳統(tǒng)二次規(guī)劃函數(shù)中添加約束ρε2,可將目標函數(shù)寫成如下形式:
式中:Np為預(yù)測步長;Nc為控制步長;ΔU為控制量的變化值。
轉(zhuǎn)換為標準二次型形式如下:
接下來進行約束條件設(shè)置,需要考慮的是控制量的表達式與控制增量的表達式,實際情況可以寫成如下形式:
由于目標函數(shù)中沒有關(guān)于控制量本身的計算,只有關(guān)于控制量增量的計算,因此需要將控制量約束轉(zhuǎn)變成增量的約束,即需要對式(29)進行轉(zhuǎn)換,因為存在如下關(guān)系:
可令:
式中:1Nc為行數(shù)是Nc的列向量;?為克羅內(nèi)克積(Kronecker product)。
綜合上述分析,可將式(29)轉(zhuǎn)換為如下形式:
本文中設(shè)計的約束條件為:
本節(jié)針對設(shè)計的控制器及特定的大型航行器被控對象,分別設(shè)計了正弦曲線及直線為參考路徑進行仿真,以驗證本文算法的有效性。
參考軌跡如下,式中β為側(cè)漂角,可利用上文中的側(cè)漂角計算模塊來計算。
圖4、圖5為具體的正弦路徑跟蹤仿真圖,由圖可知,本文所設(shè)計的算法與MPC全控算法都能跟上參考軌跡,但可以觀察到,MPC全控算法的誤差較大,特別是在路徑轉(zhuǎn)彎處偏離最大,而本文所設(shè)計算法誤差更小,且相對于MPC全控算法能較快收斂至參考軌跡;所設(shè)計控制器的兩個控制量的變化基本也同狀態(tài)量趨勢一致,艉方向舵穩(wěn)定后呈周期變化,轉(zhuǎn)速穩(wěn)定后為定值,而艏向角也能迅速反應(yīng)并呈周期變化。
圖4 正弦曲線跟蹤效果圖
圖5 本文算法控制量變化曲線
參考軌跡設(shè)定如下,設(shè)定一起點不為零的斜線。
圖6、圖7為具體的直線軌跡跟蹤仿真圖,由圖可知,本文所設(shè)計的算法與MPC全控算法都能跟上參考軌跡,但可以觀察到,本文所設(shè)計算法收斂速度較快,與參考軌跡的誤差較小,而MPC全控算法收斂速度相比較來說更慢,且在初始運動時與參考軌跡偏差較大;所設(shè)計控制器的兩個控制量的變化基本也同狀態(tài)量趨勢一致,都在穩(wěn)定后收斂于定值,與參考軌跡相符。
圖6 直線跟蹤效果圖
圖7 本文算法控制量變化曲線
下面對比了本文算法與MPC全控算法每輪計算的時間,設(shè)計仿真實驗分別統(tǒng)計了兩種算法的計算時間,圖8為具體仿真圖。
由圖8可以發(fā)現(xiàn),本文所設(shè)計的算法每輪計算時間平均在0.005 s左右,而MPC每輪計算時間平均在0.015 s左右,由此驗證本文算法在節(jié)省計算時間上的確有較明顯的優(yōu)勢。
圖8 本文算法與全MPC算法每輪計算時間對比
本文針對大型欠驅(qū)動UUV設(shè)計了一種軌跡跟蹤算法。首先對復(fù)雜問題進行分解,在大多數(shù)導(dǎo)航條件下只分析水平面。然后,將該問題解耦為運動學控制和動力學控制問題。對于運動學控制器,采用基于在Serret-Frenet坐標系中進行LOS角導(dǎo)航的虛擬制導(dǎo)方法;對于動力學控制器,采用模型預(yù)測方法設(shè)計水平路徑跟蹤控制器。通過與全MPC的控制效果比較,發(fā)現(xiàn)控制效果及計算時間均得到了改善。