基于改進積分視線導引策略的欠驅動無人水面艇路徑跟蹤

陳霄， 劉忠， 張建強， 董蛟， 周德超

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

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

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

本文在研究國內外現有成果的基礎上，提出了2種改進ILOS導引策略，之后綜合考慮USV的欠驅動性和不對稱性以及外界擾動的影響，基于改進ILOS導引策略和反饋控制思想實現了USV水平面內的直線和曲線路徑跟蹤控制，并利用級聯系統理論證明了當所有控制目標實現時，控制系統為全局k-指數穩定的。仿真對比實驗表明了算法的有效性和先進性。

1 USV運動數學模型及問題描述

1.1 USV運動建模

欠驅動USV的水平面三自由運動學和動力學模型[3]如下：

(1)

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

(2)

(3)

(4)

(5)

(6)

上述USV模型中沒有考慮作用在艇體上的外界干擾，因此以上模型中的速度均為絕對運動速度。考慮存在海流等外界干擾的情況，若滿足以下2個條件：

條件2CRB(v)與速度u、v無關，即CRB(v)=CRB(vr),其中vr為相對運動速度。

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

(7)

模型式(7)不僅可避免同時使用絕對前進速度u和相對前進速度ur給控制器設計帶來的復雜公式推導，而且由于Tu作用于船體產生的速度為ur，而不是u，因此可通過控制總能量消耗直接控制USV的相對前進速度ur。

1.2 USV模型坐標變換

為便于2.3節欠驅動USV航向、航速控制律的設計，本文將船體坐標系的原點變換到船體樞心位置處，去除偏航力矩對橫向運動的直接影響[9]。

定義：

(8)

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

(9)

(10)

(11)

(12)

(13)

(14)

其中：

(15)

(16)

(17)

(18)

1.3 問題描述

(19)

(20)

對ye(t)微分得

(21)

由式(19)可得

(22)

由圖1所示的幾何關系可知：

(23)

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

(24)

那么，式(24)可改寫為

(25)

圖1 基于前視距離的ILOS導引策略原理圖

2 路徑跟蹤

本文設計的路徑跟蹤控制系統采用ILOS導引子系統與航向控制子系統級聯的方式。導引子系統根據USV的位置、姿態信息和計劃航線的信息，求解出期望的USV航向，而航向控制子系統則根據導引子系統給定的期望信號和當前USV的運動信息計算出期望的舵機操縱信號，以此使得USV路徑跟蹤的位置和航向都收斂到平衡位置，同時航速控制子系統實現USV對期望速度的跟蹤。

因此本文的3自由度欠驅動USV路徑跟蹤控制目標為

(26)

(27)

(28)

式中：ψd為導引子系統給出的航向參考信號，即期望航向角；ud為期望的相對前進速度。

2.1 傳統ILOS導引策略

ILOS導引策略是一種經典有效的導航算法，文獻[3]中定義了船舶運動控制領域中視線導引的概念：ILOS矢量是船舶當前位置在計劃航線投影點的切線上一點與船舶當前位置的連線。通過控制船舶合速度的方向始終對準ILOS矢量，引導船舶逼近計劃航線。ILOS導引原理獨立于動力學控制，不依賴于任何模型，對高頻白噪聲敏感度低，且需要設計的參數少，期望航向的獲取只與船舶的實時位置和給定的期望航線有關，能夠實時高效地計算期望航向并傳遞給控制層。ILOS導引策略可分為基于包圍圈和前視距離2種，本文采用基于前視距離的ILOS導引策略來完成路徑跟蹤目標，傳統ILOS導引策略[7]定義如下：

(29)

(30)

式中:yint為虛擬控制輸入，是一個積分環節。

2.2 改進ILOS導引策略

本節基于自適應觀測技術，設計自適應觀測器來估計漂角β，再選擇恰當的虛擬控制輸入對漂角進行補償，最終實現對路徑的精確跟蹤。基于以上思想，本文提出2種改進ILOS導引策略。

第1種改進ILOS導引策略設計如下：

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

(31)

(32)

(33)

式中：kp=1/Δ、ki和ρ為要設計的嚴格大于零的常值參數。

第2種改進ILOS導引策略在前者的基礎上將常值增益kp用位置誤差函數來替代：

(34)

式中：λ為嚴格大于零的常值參數；Δmax和Δmin分別為前視距離Δ的最大值和最小值。設計如下：

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

(35)

(36)

(37)

(38)

下面給出以上導引子系統的穩定性證明。

(39)

又因為

(40)

(41)

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

(42)

式中：

(43)

(44)

(45)

(46)

整理得

(47)

設計李雅普諾夫函數V1，定義如下：

(48)

對其微分得

(49)

(50)

故導引子系統在平衡點ye(t)=0處是一致全局漸近穩定的(Uniform Global Asymptotic Stability，UGAS)。

(51)

證畢

2.3 航向、航速控制子系統設計

本節采用反饋線性化比例微分控制和比例控制分別設計航向、航速控制器，實現對導引子系統給出的參考信號ψd和期望速度的跟蹤[11]。控制律設計如下：

(52)

(53)

(54)

(55)

證畢

3 系統穩定性證明

證明將位置誤差動態與航向跟蹤誤差動態寫成如下級聯形式：

(56)

(57)

式中：

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

(58)

(59)

(60)

(61)

證畢

下面給出級聯系統式(60)和式(61)的穩定性及證明過程。

定理1系統F(t,z)在平衡點(ye(t),h)處是全局k-指數穩定的。

(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)|不恒為零，因此級聯系統式(62)、式(63)是全局k-指數穩定的，即系統式(58)的標稱系統F(t,z)是全局k-指數穩定的。

證畢

定理2級聯系統式(58)、式(59)是全局k-指數穩定的。

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

(69)

對其微分得

(70)

證畢

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

4 仿真實驗

為驗證本文提出的改進ILOS導引策略的有效性和優越性，以文獻[15]中的模型為例，在MATLAB/Simulink中進行路徑跟蹤對比仿真實驗。USV模型具體參數如表1所示。

表1 USV模型參數

通常情況下USV的期望航線分為直線和曲線2種，故將本文提出的2種改進ILOS導引新策略下和傳統ILOS導引策略下的路徑跟蹤控制算法在直線和曲線2種工況下進行仿真實驗對比。2種工況下航向、航速控制子系統的參數選擇為：k1=5，k2=3，kur=5；導引子系統參數選擇如下：傳統的ILOS導引策略中，γ=0.005，Δ=8 m；第1種改進ILOS導引策略中，選擇ki=0.001，ρ=5，Δ=8 m；第2種改進ILOS導引策略中，選擇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；大地坐標系下的海流設置為：Uc=0.2 m/s，ψc=30°；期望航線為y=x，期望航速ud=3 m/s。仿真結果如圖2所示。其中，傳統ILOS導引策略下(以下簡稱為算法1)和2種改進ILOS導引策略下(以下簡稱為算法2、3)的路徑跟蹤控制算法的相對縱向速度分別對應變量下標為1、2、3的仿真結果。

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

分析圖2(a)和圖3(a)可知，在僅有縱向控制力和轉艏控制力矩、沒有側向推進器的欠驅動情況下，本文設計的3種控制算法均能夠保證欠驅動USV快速地跟蹤期望的直線和曲線路徑，并沿該路徑航行；曲線路徑跟蹤過程中，在曲率變化較大時，傳統ILOS導引策略超調和振蕩較大，而第1和第2種改進ILOS導引策略超調和振蕩較小，且第2種ILOS改進導引策略最優。

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

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

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

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

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

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

5 結 論

1) 本文研究了一類非對稱欠驅動USV的路徑跟蹤問題，綜合考慮了USV的欠驅動性、非對稱性以及外界干擾如海流等因素，提出了2種改進ILOS導引策略和反饋控制思想的路徑跟蹤控制算法，實現了USV水平面內的路徑跟蹤。

2) 基于級聯系統和李雅普諾夫理論證明了當所有控制目標實現時，控制系統是全局k-指數穩定的。

3) 通過與傳統ILOS導引策略的路徑跟蹤算法進行仿真實驗對比分析可以看出，所提出的2種改進引導策略算法具有更好的路徑跟蹤效果和動態性能，具有一定的先進性。

下一步將研究風、浪、流等復雜海況下以及模型不確定情況下欠驅動USV的路徑跟蹤控制問題。

參考文獻(References)

[1] 張樹凱,劉正江,張顯庫,等.無人船艇的發展及展望[J].世界海運,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] 薛春祥,黃孝鵬,朱咸軍,等.外軍無人系統現狀與發展趨勢[J].雷達與對抗,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] 田勇,王丹,彭周華,等.無人水面艇直線航跡跟蹤控制器的設計與驗證[J].大連海事大學學報,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.