基于改進(jìn)積分視線導(dǎo)引策略的欠驅(qū)動(dòng)無(wú)人水面艇路徑跟蹤

陳霄， 劉忠， 張建強(qiáng)， 董蛟， 周德超

(海軍工程大學(xué) 電子工程學(xué)院， 武漢 430000)

無(wú)人水面艇(Unmanned Surface Vehicle，USV)作為一種小型水面智能任務(wù)平臺(tái)，具有體積小、成本低、高速智能、雷達(dá)反射面積小、無(wú)人員傷亡等優(yōu)勢(shì)，在軍用和民用領(lǐng)域具有廣泛的應(yīng)用前景[1]。同時(shí)，USV還是整合低空無(wú)人機(jī)和水下機(jī)器人跨網(wǎng)絡(luò)的關(guān)鍵節(jié)點(diǎn)，備受世界各國(guó)科研人員的關(guān)注[2]。路徑跟蹤是USV完成各項(xiàng)使命任務(wù)的關(guān)鍵技術(shù)之一，定義為USV在控制系統(tǒng)的驅(qū)動(dòng)下，不考慮時(shí)間約束，從任意初始位置出發(fā)，駛?cè)腩A(yù)先設(shè)定的期望航線，并沿此航線最終到達(dá)指定位置[3]。不具有側(cè)向推進(jìn)器或側(cè)向推進(jìn)器在高速航行時(shí)失效的USV是一類典型的欠驅(qū)動(dòng)系統(tǒng)，由于控制輸入個(gè)數(shù)少于其所需控制的自由度個(gè)數(shù)，故該類系統(tǒng)存在不可積的加速度約束[3]。目前國(guó)內(nèi)外對(duì)于欠驅(qū)動(dòng)USV路徑跟蹤控制已有較多的研究成果。

Caharija等設(shè)計(jì)了基于坐標(biāo)變換的全局k-指數(shù)直線路徑跟蹤控制器和海流觀測(cè)器，但并未考慮更為一般的曲線路徑跟蹤控制問(wèn)題[4]。田勇等基于視線導(dǎo)引策略和抗飽和PID控制算法設(shè)計(jì)了直線航跡跟蹤控制器，并進(jìn)行湖上試驗(yàn)驗(yàn)證，但并未考慮外界擾動(dòng)的影響[5]。Fossen和Lekkas考慮海流等外界干擾對(duì)USV運(yùn)動(dòng)學(xué)模型的擾動(dòng),進(jìn)行了路徑跟蹤控制研究，但并未考慮USV模型的非對(duì)稱特性[6]。B?rhaug等提出了用于補(bǔ)償外界環(huán)境干擾的積分視線(Integral Line-of-Sight，ILOS)導(dǎo)引策略，并給出系統(tǒng)的穩(wěn)定性證明[7]。文獻(xiàn)[8]設(shè)計(jì)了基于相對(duì)速度模型的路徑跟蹤ILOS導(dǎo)引策略，并證明了控制系統(tǒng)是全局k-指數(shù)穩(wěn)定(Globalk-Exponential Stable,GKES)的，但只實(shí)現(xiàn)直線路徑跟蹤。

本文在研究國(guó)內(nèi)外現(xiàn)有成果的基礎(chǔ)上，提出了2種改進(jìn)ILOS導(dǎo)引策略，之后綜合考慮USV的欠驅(qū)動(dòng)性和不對(duì)稱性以及外界擾動(dòng)的影響，基于改進(jìn)ILOS導(dǎo)引策略和反饋控制思想實(shí)現(xiàn)了USV水平面內(nèi)的直線和曲線路徑跟蹤控制，并利用級(jí)聯(lián)系統(tǒng)理論證明了當(dāng)所有控制目標(biāo)實(shí)現(xiàn)時(shí)，控制系統(tǒng)為全局k-指數(shù)穩(wěn)定的。仿真對(duì)比實(shí)驗(yàn)表明了算法的有效性和先進(jìn)性。

1 USV運(yùn)動(dòng)數(shù)學(xué)模型及問(wèn)題描述

1.1 USV運(yùn)動(dòng)建模

欠驅(qū)動(dòng)USV的水平面三自由運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型[3]如下：

(1)

式中:η=[x,y,ψ]T為大地坐標(biāo)系i下USV在水平面的位置(x,y)和航向角ψ；J(ψ)為旋轉(zhuǎn)矩陣；v=[u,v,r]T為USV的縱向、橫向線速度(u、v)及航向角速度(r)；f=[Tu,Tr]T為控制輸入矩陣，Tu和Tr分別為前進(jìn)推力和轉(zhuǎn)艏力矩；B為控制輸入配置矩陣；M為慣性參數(shù)矩陣；C(v)=CRB(v)+CA(v),CRB(v)和CA(v)分別為剛體自身和附加的科氏力和向心力矩陣;D(v)為阻尼參數(shù)矩陣。各矩陣定義如下：

(2)

(3)

(4)

(5)

(6)

上述USV模型中沒(méi)有考慮作用在艇體上的外界干擾，因此以上模型中的速度均為絕對(duì)運(yùn)動(dòng)速度。考慮存在海流等外界干擾的情況，若滿足以下2個(gè)條件：

條件2CRB(v)與速度u、v無(wú)關(guān)，即CRB(v)=CRB(vr),其中vr為相對(duì)運(yùn)動(dòng)速度。

此時(shí)可通過(guò)在USV運(yùn)動(dòng)數(shù)學(xué)模型中加入海流速度Vc=[Vx,Vy,0]T的方式，直接用相對(duì)運(yùn)動(dòng)速度vr=[ur,vr,r]T替代式(1)中的絕對(duì)運(yùn)動(dòng)速度v=[u,v,r]T，模型如下：

(7)

模型式(7)不僅可避免同時(shí)使用絕對(duì)前進(jìn)速度u和相對(duì)前進(jìn)速度ur給控制器設(shè)計(jì)帶來(lái)的復(fù)雜公式推導(dǎo)，而且由于Tu作用于船體產(chǎn)生的速度為ur，而不是u，因此可通過(guò)控制總能量消耗直接控制USV的相對(duì)前進(jìn)速度ur。

1.2 USV模型坐標(biāo)變換

為便于2.3節(jié)欠驅(qū)動(dòng)USV航向、航速控制律的設(shè)計(jì)，本文將船體坐標(biāo)系的原點(diǎn)變換到船體樞心位置處，去除偏航力矩對(duì)橫向運(yùn)動(dòng)的直接影響[9]。

定義：

(8)

式中：ε=-(m33b22-m23b32)/(m22b32-m23b22)。坐標(biāo)變換后，式(7)可寫為

(9)

(10)

(11)

(12)

(13)

(14)

其中：

(15)

(16)

(17)

(18)

1.3 問(wèn)題描述

(19)

(20)

對(duì)ye(t)微分得

(21)

由式(19)可得

(22)

由圖1所示的幾何關(guān)系可知：

(23)

將式(7)、式(22)及式(23)代入式(21)中得

(24)

那么，式(24)可改寫為

(25)

圖1 基于前視距離的ILOS導(dǎo)引策略原理圖

2 路徑跟蹤

本文設(shè)計(jì)的路徑跟蹤控制系統(tǒng)采用ILOS導(dǎo)引子系統(tǒng)與航向控制子系統(tǒng)級(jí)聯(lián)的方式。導(dǎo)引子系統(tǒng)根據(jù)USV的位置、姿態(tài)信息和計(jì)劃航線的信息，求解出期望的USV航向，而航向控制子系統(tǒng)則根據(jù)導(dǎo)引子系統(tǒng)給定的期望信號(hào)和當(dāng)前USV的運(yùn)動(dòng)信息計(jì)算出期望的舵機(jī)操縱信號(hào)，以此使得USV路徑跟蹤的位置和航向都收斂到平衡位置，同時(shí)航速控制子系統(tǒng)實(shí)現(xiàn)USV對(duì)期望速度的跟蹤。

因此本文的3自由度欠驅(qū)動(dòng)USV路徑跟蹤控制目標(biāo)為

(26)

(27)

(28)

式中：ψd為導(dǎo)引子系統(tǒng)給出的航向參考信號(hào)，即期望航向角；ud為期望的相對(duì)前進(jìn)速度。

2.1 傳統(tǒng)ILOS導(dǎo)引策略

ILOS導(dǎo)引策略是一種經(jīng)典有效的導(dǎo)航算法，文獻(xiàn)[3]中定義了船舶運(yùn)動(dòng)控制領(lǐng)域中視線導(dǎo)引的概念：ILOS矢量是船舶當(dāng)前位置在計(jì)劃航線投影點(diǎn)的切線上一點(diǎn)與船舶當(dāng)前位置的連線。通過(guò)控制船舶合速度的方向始終對(duì)準(zhǔn)ILOS矢量，引導(dǎo)船舶逼近計(jì)劃航線。ILOS導(dǎo)引原理獨(dú)立于動(dòng)力學(xué)控制，不依賴于任何模型，對(duì)高頻白噪聲敏感度低，且需要設(shè)計(jì)的參數(shù)少，期望航向的獲取只與船舶的實(shí)時(shí)位置和給定的期望航線有關(guān)，能夠?qū)崟r(shí)高效地計(jì)算期望航向并傳遞給控制層。ILOS導(dǎo)引策略可分為基于包圍圈和前視距離2種，本文采用基于前視距離的ILOS導(dǎo)引策略來(lái)完成路徑跟蹤目標(biāo)，傳統(tǒng)ILOS導(dǎo)引策略[7]定義如下：

(29)

(30)

式中:yint為虛擬控制輸入，是一個(gè)積分環(huán)節(jié)。

2.2 改進(jìn)ILOS導(dǎo)引策略

本節(jié)基于自適應(yīng)觀測(cè)技術(shù)，設(shè)計(jì)自適應(yīng)觀測(cè)器來(lái)估計(jì)漂角β，再選擇恰當(dāng)?shù)奶摂M控制輸入對(duì)漂角進(jìn)行補(bǔ)償，最終實(shí)現(xiàn)對(duì)路徑的精確跟蹤。基于以上思想，本文提出2種改進(jìn)ILOS導(dǎo)引策略。

第1種改進(jìn)ILOS導(dǎo)引策略設(shè)計(jì)如下：

ψd=γp(θ)-arctan(kpye(t)+yint)

(31)

(32)

(33)

式中：kp=1/Δ、ki和ρ為要設(shè)計(jì)的嚴(yán)格大于零的常值參數(shù)。

第2種改進(jìn)ILOS導(dǎo)引策略在前者的基礎(chǔ)上將常值增益kp用位置誤差函數(shù)來(lái)替代：

(34)

式中：λ為嚴(yán)格大于零的常值參數(shù)；Δmax和Δmin分別為前視距離Δ的最大值和最小值。設(shè)計(jì)如下：

ψd=γp(θ)-arctan(kpye(t)+yint)

(35)

(36)

(37)

(38)

下面給出以上導(dǎo)引子系統(tǒng)的穩(wěn)定性證明。

(39)

又因?yàn)?/p>

(40)

(41)

將式(40)和式(41)代入式(39)中，可得

(42)

式中：

(43)

(44)

(45)

(46)

整理得

(47)

設(shè)計(jì)李雅普諾夫函數(shù)V1，定義如下：

(48)

對(duì)其微分得

(49)

(50)

故導(dǎo)引子系統(tǒng)在平衡點(diǎn)ye(t)=0處是一致全局漸近穩(wěn)定的(Uniform Global Asymptotic Stability，UGAS)。

(51)

證畢

2.3 航向、航速控制子系統(tǒng)設(shè)計(jì)

本節(jié)采用反饋線性化比例微分控制和比例控制分別設(shè)計(jì)航向、航速控制器，實(shí)現(xiàn)對(duì)導(dǎo)引子系統(tǒng)給出的參考信號(hào)ψd和期望速度的跟蹤[11]。控制律設(shè)計(jì)如下：

(52)

(53)

(54)

(55)

證畢

3 系統(tǒng)穩(wěn)定性證明

證明將位置誤差動(dòng)態(tài)與航向跟蹤誤差動(dòng)態(tài)寫成如下級(jí)聯(lián)形式：

(56)

(57)

式中：

g(t,ye(t),h)=

(58)

(59)

(60)

(61)

證畢

下面給出級(jí)聯(lián)系統(tǒng)式(60)和式(61)的穩(wěn)定性及證明過(guò)程。

定理1系統(tǒng)F(t,z)在平衡點(diǎn)(ye(t),h)處是全局k-指數(shù)穩(wěn)定的。

(62)

(63)

式中：

(64)

g(t,ye(t),h)=

(65)

(66)

(67)

|g(t,ye(t),h)|≤1.73U(1 +β0max)

(68)

式中：β0max為漂角的最大值。

由式(68)知|g(t,ye(t),h)|不恒為零，因此級(jí)聯(lián)系統(tǒng)式(62)、式(63)是全局k-指數(shù)穩(wěn)定的，即系統(tǒng)式(58)的標(biāo)稱系統(tǒng)F(t,z)是全局k-指數(shù)穩(wěn)定的。

證畢

定理2級(jí)聯(lián)系統(tǒng)式(58)、式(59)是全局k-指數(shù)穩(wěn)定的。

證明首先證明式(59)是全局k-指數(shù)穩(wěn)定的，取李雅普諾夫函數(shù)：

(69)

對(duì)其微分得

(70)

證畢

最后，討論時(shí)變參數(shù)kp和積分增益k對(duì)導(dǎo)引子系統(tǒng)穩(wěn)定性的影響：根據(jù)式(36)可知，較大的前視距離Δ和積分增益k會(huì)限制導(dǎo)引系統(tǒng)的指數(shù)穩(wěn)定域。在第1種改進(jìn)導(dǎo)引策略中，只有參數(shù)k影響系統(tǒng)的指數(shù)穩(wěn)定域，而在第2種改進(jìn)ILOS導(dǎo)引策略中，參數(shù)Δ和k一起限制了系統(tǒng)的指數(shù)穩(wěn)定域[13-14]。

4 仿真實(shí)驗(yàn)

為驗(yàn)證本文提出的改進(jìn)ILOS導(dǎo)引策略的有效性和優(yōu)越性，以文獻(xiàn)[15]中的模型為例，在MATLAB/Simulink中進(jìn)行路徑跟蹤對(duì)比仿真實(shí)驗(yàn)。USV模型具體參數(shù)如表1所示。

表1 USV模型參數(shù)

通常情況下USV的期望航線分為直線和曲線2種，故將本文提出的2種改進(jìn)ILOS導(dǎo)引新策略下和傳統(tǒng)ILOS導(dǎo)引策略下的路徑跟蹤控制算法在直線和曲線2種工況下進(jìn)行仿真實(shí)驗(yàn)對(duì)比。2種工況下航向、航速控制子系統(tǒng)的參數(shù)選擇為：k1=5，k2=3，kur=5；導(dǎo)引子系統(tǒng)參數(shù)選擇如下：傳統(tǒng)的ILOS導(dǎo)引策略中，γ=0.005，Δ=8 m；第1種改進(jìn)ILOS導(dǎo)引策略中，選擇ki=0.001，ρ=5，Δ=8 m；第2種改進(jìn)ILOS導(dǎo)引策略中，選擇ki=0.000 9，ρ=5，λ=3，Δmin=6 m，Δmax=14 m。

1) 工況1。USV的初始位置為(x0,y0)=(50,-100)m，初始航向ψ0=π/4 rad，初始航速ur=0.5 m/s；大地坐標(biāo)系下的海流設(shè)置為：Uc=0.2 m/s，ψc=30°；期望航線為y=x，期望航速ud=3 m/s。仿真結(jié)果如圖2所示。其中，傳統(tǒng)ILOS導(dǎo)引策略下(以下簡(jiǎn)稱為算法1)和2種改進(jìn)ILOS導(dǎo)引策略下(以下簡(jiǎn)稱為算法2、3)的路徑跟蹤控制算法的相對(duì)縱向速度分別對(duì)應(yīng)變量下標(biāo)為1、2、3的仿真結(jié)果。

2) 工況2。USV的初始位置為(x0,y0)=(0,-10) m，初始航向ψ0=π/4 rad，初始航速ur=0.5 m/s；大地坐標(biāo)系下的海流設(shè)置為：Uc=0.2 m/s，ψc=30°；期望航線為由(0,0) m、(100,50) m、(200,100) m、(300,50) m、(600,200) m經(jīng)過(guò)3次樣條差值生成的連續(xù)曲線，期望航速ud=3 m/s。仿真結(jié)果如圖3所示。

分析圖2(a)和圖3(a)可知，在僅有縱向控制力和轉(zhuǎn)艏控制力矩、沒(méi)有側(cè)向推進(jìn)器的欠驅(qū)動(dòng)情況下，本文設(shè)計(jì)的3種控制算法均能夠保證欠驅(qū)動(dòng)USV快速地跟蹤期望的直線和曲線路徑，并沿該路徑航行；曲線路徑跟蹤過(guò)程中，在曲率變化較大時(shí)，傳統(tǒng)ILOS導(dǎo)引策略超調(diào)和振蕩較大，而第1和第2種改進(jìn)ILOS導(dǎo)引策略超調(diào)和振蕩較小，且第2種ILOS改進(jìn)導(dǎo)引策略最優(yōu)。

從圖2(b)和圖3(b)可以看出，直線和曲線路徑跟蹤過(guò)程中，3種控制算法均能夠保證位置誤差ye最終收斂到0附近；傳統(tǒng)ILOS導(dǎo)引策略下，ye的振蕩大、持續(xù)時(shí)間長(zhǎng)，第1和第2種改進(jìn)ILOS導(dǎo)引策略下，ye的振蕩小、持續(xù)時(shí)間較短。

圖2 不同導(dǎo)引策略下USV直線路徑跟蹤、橫向誤差、相對(duì)縱向速度、航向角、轉(zhuǎn)艏力矩及前進(jìn)推力的變化(工況1)

圖3 不同導(dǎo)引策略下USV曲線路徑跟蹤、橫向誤差、相對(duì)縱向速度、航向角、轉(zhuǎn)艏力矩及前進(jìn)推力的變化(工況2)

分析圖2(c)、(f)和圖3(c)、(f)可以看出，直線和曲線路徑跟蹤過(guò)程中，3種控制算法均能夠保證航速控制在ud=3 m/s附近，穩(wěn)態(tài)誤差較小，但算法1較其他2種改進(jìn)算法作用下的前進(jìn)推力在初始加速階段所需推力較大。

從圖2(d)可以看出，直線路徑跟蹤過(guò)程中，3種算法都能很好的跟蹤期望航向角，但算法1給出的期望航向角振蕩較大，而算法2、3給出的期望航向角相對(duì)較為平緩，這也導(dǎo)致圖2(e)中算法1作用下的轉(zhuǎn)艏力矩振蕩幅度較大，而算法2、3則作用下的轉(zhuǎn)艏力矩變化相對(duì)較為平滑。

從圖3(d)、(e)可以看出，曲線路徑跟蹤過(guò)程中，3種算法都能很好的跟蹤期望航向角，且算法3給出的期望航向角最為平滑，同時(shí)轉(zhuǎn)艏力矩也最小。

5 結(jié) 論

1) 本文研究了一類非對(duì)稱欠驅(qū)動(dòng)USV的路徑跟蹤問(wèn)題，綜合考慮了USV的欠驅(qū)動(dòng)性、非對(duì)稱性以及外界干擾如海流等因素，提出了2種改進(jìn)ILOS導(dǎo)引策略和反饋控制思想的路徑跟蹤控制算法，實(shí)現(xiàn)了USV水平面內(nèi)的路徑跟蹤。

2) 基于級(jí)聯(lián)系統(tǒng)和李雅普諾夫理論證明了當(dāng)所有控制目標(biāo)實(shí)現(xiàn)時(shí)，控制系統(tǒng)是全局k-指數(shù)穩(wěn)定的。

3) 通過(guò)與傳統(tǒng)ILOS導(dǎo)引策略的路徑跟蹤算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比分析可以看出，所提出的2種改進(jìn)引導(dǎo)策略算法具有更好的路徑跟蹤效果和動(dòng)態(tài)性能，具有一定的先進(jìn)性。

下一步將研究風(fēng)、浪、流等復(fù)雜海況下以及模型不確定情況下欠驅(qū)動(dòng)USV的路徑跟蹤控制問(wèn)題。

參考文獻(xiàn)(References)

[1] 張樹凱,劉正江,張顯庫(kù),等.無(wú)人船艇的發(fā)展及展望[J].世界海運(yùn),2015,38(9):29-36.

ZHANG S K,LIU Z J,ZHANG X K,et al.The development and outlook of unmanned vessel[J].World Shipping,2015,38(9):29-36(in Chinese).

[2] 薛春祥,黃孝鵬,朱咸軍,等.外軍無(wú)人系統(tǒng)現(xiàn)狀與發(fā)展趨勢(shì)[J].雷達(dá)與對(duì)抗,2016(36):1-5.

XUE C X,HUANG X P,ZHU X J,et al.Status quo and development trends of foreign military’s unmanned systems[J].Radar & ECM,2016(36):1-5(in Chinese).

[3] FOSSEN T I.Handbook of marine craft hydrodynamics and motion control[M].Hudson County:John Wiley & Sons,2011:60-89.

[4] CAHARIJA W,PETTERSEN K Y,S?RENSEN A J,et al.Relative velocity control and integral LOS for path following of autonomous surface vessels:Merging intuition with theory[J].Engineering for the Marine Environment,2014,228(2):180-191.

[5] 田勇,王丹,彭周華,等.無(wú)人水面艇直線航跡跟蹤控制器的設(shè)計(jì)與驗(yàn)證[J].大連海事大學(xué)學(xué)報(bào),2015,41(4):14-18.

TIAN Y,WANG D,PENG Z H,et al.Design and validation of path tracking controller for USV along straight-lines[J].Journal of Dalian Maritime University,2015,41(4):14-18(in Chinese).

[6] FOSSEN T I,LEKKAS A M.Direct and indirect adaptive integral line-of-sight path-following controllers for marine craft exposed to ocean currents[J].International Journal of Adaptive Control and Signal Processing,2015,28(3):20-35.

[7] B?RHAUG E,PAVLOV A,PETTERSEN K Y.Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents[C]∥Proceedings of the 47th IEEE Conference on Decision and Control.Piscataway,NJ:IEEE Press,2008:4984-4991.

[8] WANG H,WANG D,PENG Z H.Adaptive neural control for cooperative path following of marine surface vehicles:State and output feedback[J].International Journal of Systems Science,2016,47(2):343-359.

[9] FREDRIKSEN E,PETTERSEN K Y.Global-exponential waypoint maneuvering of ships:Theory and experiments[J].Automatica,2006,42(4):677-687.

[10] FOSSEN T I,PETTERSEN K Y,GALEAZZI R.Line-of-sight path following for Dubins paths with adaptive sideslip compensation of drift forces[J].IEEE Transactions on Control Systems Technology,2015,23(2):820-827.

[11] LEKKAS A M,FOSSEN T I.Integral LOS path following for curved paths based on a monotone cubic Hermite spline parametrization[J].IEEE Transactions on Control Systems Technology,2014,22(6):2287-2301.

[12] ZHENG Z W,HUO W,WU Z.Direct-adaptive fuzzy path following control for an autonomous airship[J].Control and Decision,2014,29(3):1418-1424.

[13] WIIG M S,PETTERSEN K Y,KROGSTAD T R.Uniform semiglobal exponential stability of integral line-of-sight guidance laws[J].IFAC Papers Online,2015,48(16):61-68.

[14] FOSSEN T I,PETTERSEN K Y.On uniform semiglobal exponential stability (USGES) of proportional line-of-sight guidance laws[J].Automatica,2014,50(11):2912-2917.

[15] BREIVIK M.Nonlinear maneuvering control of underactuated ships[D].Trondheim:Norwegian University of Science and Technology,2003:10-30.