溫錦元 黃宴委
摘要:針對(duì)航速和航道未知擾動(dòng)等因素,提出一種速度矢量場(chǎng)二階滑模無人水面艇(USV)引導(dǎo)律。首先,建立無人艇運(yùn)動(dòng)學(xué)和航向角動(dòng)力學(xué)模型;其次構(gòu)造路徑誤差(ye)模型,設(shè)計(jì)基于航速(Vg)的路徑誤差矢量場(chǎng),速度越大,航向角變化越??;再結(jié)合二階滑模面設(shè)計(jì)一種速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律,并考慮未知擾動(dòng)因素Δ分析速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律的穩(wěn)定性。仿真結(jié)果表明:相比于經(jīng)典矢量場(chǎng),速度矢量場(chǎng)有效實(shí)現(xiàn)航速Vg越快,航向角變化率越小,矢量場(chǎng)越平緩,提高了USV航行安全性和穩(wěn)定性;基于速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律的路徑跟蹤控制系統(tǒng)魯棒性更強(qiáng),路徑跟蹤準(zhǔn)確度更高,能夠較好地完成路徑跟蹤。
關(guān)鍵詞:無人水面艇;矢量場(chǎng)引導(dǎo)律;路徑跟蹤;滑??刂?/p>
中圖分類號(hào):TP 273文獻(xiàn)標(biāo)志碼:A
文章編號(hào):1000-5013(2024)03-0324-08
Second-Order Sliding Mode Guidance Law in Velocity Vector Field for Unmanned Surface Vessel
WEN Jinyuan,HUANG Yanwei
(College of Electrical Engineering and Automation,F(xiàn)uzhou University,F(xiàn)uzhou 350116,China)
Abstract:Aiming at factors such as unknown disturbances of course speed and course path,a second-order sliding mode guidance law in velocity vector field fof unmanned surface vessel ( USV) is proposed in the paper. Firstly,kinematics and course angle dynamics models of USV are established. Secondly,the path error (ye)model is constructed,and the path error vector field based on course speed (Vg)is designed. The greater the speed,the smaller the change of course angle. Recombined with the second-order sliding mode surface,a velocity vector field second-order sliding mode USV guidance law is designed,and the stability of velocity vector field second-order sliding mode USV guidance law is analyzed considering the unknown disturbance factor Δ. The simulation results show that compared with the classical vector field,the velocity vector field achieves faster course speed Vgwith smaller course angle change rate and smoother vector field,which improves the navigation safety and stability of USV. The path tracking control system based on the second-order sliding mode guidance law in a velocity vector field exhibits enhanced robustness and higher accuracy in path following, which achieves path tracking with remarkable precision.
Keywords:unmanned surface vessel;vector field guidance law;path following;sliding mode control
無人水面艇(unmanned surface vessel,USV)是一種用于水上的獨(dú)立可控的無人駕駛平臺(tái)[1]。 路徑跟蹤問題是實(shí)現(xiàn)USV高精度控制的重中之重,而導(dǎo)引算法是完成路徑跟蹤任務(wù)的關(guān)鍵。 現(xiàn)階段導(dǎo)引算法種類繁多,例如,純跟蹤算法[2]、視線導(dǎo)引算法[3-4]、矢量場(chǎng)導(dǎo)引(vector field guidance,VFG)算法[5-7]。純跟蹤算法可以縮小位置誤差,但無法縮小角度誤差;視線導(dǎo)引算法對(duì)前視距離依賴大,變化敏感。VFG分為矢量場(chǎng)(vector field,VF)設(shè)計(jì)和引導(dǎo)律設(shè)計(jì)兩部分,對(duì)比純追蹤算法和視線導(dǎo)引算法,矢量場(chǎng)算法[8-10]設(shè)計(jì)簡(jiǎn)單且控制性能優(yōu)秀。
然而,VF主要考慮路徑誤差設(shè)計(jì)跟蹤軌跡。針對(duì)三維路徑跟蹤問題,文獻(xiàn)[11]擴(kuò)展二維全局路徑誤差VF,使機(jī)器人能夠?qū)崿F(xiàn)三維路徑跟蹤。文獻(xiàn)[12]提出一種利用亥姆霍茲定理構(gòu)建二維和三維路徑誤差VF。文獻(xiàn)[13]提出一種能夠調(diào)節(jié)沖擊時(shí)間和飛行角度的三維路徑誤差VF。針對(duì)無人機(jī)路徑跟蹤精度和安全性問題,文獻(xiàn)[14]利用亥姆霍茲定理構(gòu)建二維路徑誤差VF,并結(jié)合反步法設(shè)計(jì)控制器,提高欠驅(qū)動(dòng)飛艇跟蹤精度。文獻(xiàn)[15]利用滑模控制理論設(shè)計(jì)位置誤差VF,將VF視為滑模面,保證任意初始狀態(tài)的無人機(jī)都能跟蹤上期望路徑。這些研究利用路徑誤差設(shè)計(jì)VF,忽略了運(yùn)動(dòng)對(duì)象的動(dòng)態(tài)特征。然而,動(dòng)態(tài)特征直接影響系統(tǒng)的安全性和精度。因此,引入運(yùn)動(dòng)對(duì)象的動(dòng)態(tài)特性來設(shè)計(jì)VF是非常重要的。路徑跟蹤精度還取決于VFG引導(dǎo)律設(shè)計(jì),文獻(xiàn)[15]在路徑誤差VF的基礎(chǔ)上,利用飽和函數(shù)設(shè)計(jì)VFG引導(dǎo)律,有效地提高跟蹤系統(tǒng)的魯棒性。文獻(xiàn)[16] 在一階航向角方程中引入擾動(dòng)量,根據(jù)航向角誤差設(shè)計(jì)自適應(yīng)VFG引導(dǎo)律,實(shí)時(shí)估計(jì)外界擾動(dòng)進(jìn)行補(bǔ)償,使無人機(jī)在復(fù)雜的外界情況下,能夠準(zhǔn)確跟蹤期望路徑?;赩FG控制精度高特點(diǎn),文獻(xiàn)[17]將VF應(yīng)用于USV路徑跟蹤,設(shè)計(jì)了一種基于VFG的二維路徑跟蹤控制系統(tǒng),使用LS-SVM方法辨識(shí)參數(shù),該控制系統(tǒng)具有較好的魯棒性。文獻(xiàn)[18]提出了一種基于路徑誤差的時(shí)變VFG引導(dǎo)律,利用估計(jì)器補(bǔ)償外界擾動(dòng),設(shè)計(jì)自動(dòng)駕駛儀,給出半葉指數(shù)穩(wěn)定性證明,由此構(gòu)建的級(jí)聯(lián)系統(tǒng)魯棒性較好,但這些研究成果未考慮對(duì)象的運(yùn)動(dòng)模型特性而設(shè)計(jì)VFG,難以保證路徑跟蹤的性能。
考慮航速和航道未知擾動(dòng)等因素,本文提出基于一種基于速度矢量場(chǎng)的二階滑模無人艇引導(dǎo)律,能夠?qū)崿F(xiàn)快速準(zhǔn)確跟蹤期望路徑。
1 USV平面模型
在忽略垂蕩、橫搖和縱搖等自由度時(shí),USV在水面運(yùn)動(dòng)可以簡(jiǎn)化為3個(gè)自由度:縱蕩,橫蕩和艏搖。USV平面運(yùn)動(dòng)示意圖,如圖1所示。圖1中:XY為地球坐標(biāo)系,X為正北向,Y為正東向;ψ為艏向角;β為漂移角;χ為航向角;(x,y)為USV當(dāng)前的位置坐標(biāo);u,v,r分別為船體坐標(biāo)系xgyg下的縱蕩速度,橫蕩速度和轉(zhuǎn)艏角速度;Vg為航速;L為期望路徑;期望路徑P點(diǎn)坐標(biāo)為(xP,yP),αP為P點(diǎn)的路徑切向角;ye為路徑位置誤差。
1.1 運(yùn)動(dòng)學(xué)模型
USV運(yùn)動(dòng)學(xué)模型為
式(1)中:u=Vgcos β;v=Vgsin β;χ=ψ+β。
式(1)化簡(jiǎn)為位置運(yùn)動(dòng)學(xué)模型,即
1.2 航向角動(dòng)力學(xué)模型
在實(shí)際跟蹤過程中,環(huán)境擾動(dòng)的形式非常復(fù)雜,并依賴于許多參數(shù),航向角動(dòng)力學(xué)方程[18]為
式(3)中:χcε為矢量場(chǎng)引導(dǎo)律;a為時(shí)間常數(shù);Δ為外界干擾。
2 速度的路徑誤差矢量場(chǎng)
2.1 位置誤差設(shè)計(jì)
USV路徑跟蹤示意圖,如圖2所示。圖2中:跟蹤與P相切半徑為R的圓o;XY地球坐標(biāo)系轉(zhuǎn)為X1Y1為極坐標(biāo)系;圓心坐標(biāo)為(xo,yo);d為USV與圓心距離;γ為USV相對(duì)于圓心角度。
圓心坐標(biāo)o(xo,yo)可以表示為
將式(4)代入式(2),有
由圖2可知,路徑誤差(ye)計(jì)算式為
ye=d-R。(6)
對(duì)式(6)求導(dǎo),并代入式(5),有
2.2 速度路徑誤差矢量場(chǎng)的設(shè)計(jì)
航速矢量場(chǎng)(χd)為
式(8)中:Vm為航速最大值;k1為常數(shù),k1>0。
由式(8)可知:當(dāng)ye很大時(shí),矢量場(chǎng)χd=γ-π;當(dāng)ye=0時(shí),χd=γ-π/2。與經(jīng)典矢量場(chǎng)[15]對(duì)比,在ye的基礎(chǔ)上,考慮Vg因素。當(dāng)Vg=0時(shí),航速矢量場(chǎng)與經(jīng)典矢量場(chǎng)一致,Vg越大,χd越小,表明航速越快,航向角越小,有效減小USV側(cè)翻風(fēng)險(xiǎn),提高航行安全,更全面合理。
設(shè)李雅普諾夫函數(shù)(W)為
對(duì)式(9)求導(dǎo),并代入式(7),(8),有
因此,當(dāng)χ=χd時(shí),能夠精準(zhǔn)跟蹤期望路徑,即ye→0;航速矢量場(chǎng)(χd)能夠收斂到給定路徑中。
3 速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律
3.1 速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律的設(shè)計(jì)
定義航向角誤差(χe)為
χe=χ-χd。(11)
設(shè)計(jì)速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律(VVFSOSG)的χcε為
χcε=χc+Δχ。(12)
式(12)中:χc為等效航向角引導(dǎo)律;Δχ為魯棒補(bǔ)償律。
在不考慮不確定量(Δ)的情況下,χc為
對(duì)式(11)求導(dǎo),代入式(8)和式(13)可得
因此,USV路徑跟蹤過程可視為勻速運(yùn)動(dòng),V·g≈0,故式(14)可簡(jiǎn)化為
選擇等效航向角引導(dǎo)律為
為抑制不確定量(Δ)對(duì)航向角帶來的不利影響,需設(shè)計(jì)魯棒補(bǔ)償項(xiàng)(Δχ)。
對(duì)航向角誤差進(jìn)行微分,有
構(gòu)造滑模面S[19-21],有
S=χe。(18)
對(duì)式(18)求導(dǎo),并代入式(16),有
在式(19)的基礎(chǔ)上構(gòu)造二階滑模面[22-23],有
式(20)中:m為常數(shù),且m>0。
對(duì)式(20)求導(dǎo),有
選取滑模面ξ控制律[24],有
式(22)中:λ,p和q為常數(shù),且λ>0,p,q∈{2n+1,n=0,1,2,…},1
由式(21),(22),有
對(duì)式(19)求導(dǎo),有
將式(24)代入式(23),可最終求得控制量,即
為有效抑制Δ·,有
其中,Δ[25]為
Δ=Awsin(ωet+θ)。(27)
式(27)中:Aw為擾動(dòng)幅值;ωe為擾動(dòng)頻率;θ為擾動(dòng)相位。
對(duì)式(27)求導(dǎo),有
將式(26)代入式(29),有
由式(30)可知,當(dāng)ηδ>|Awωe|時(shí),能夠有效抑制Δ·,則式(25)可簡(jiǎn)化為
綜合式(12),(16),(31),可得到χcε,即
3.2 VVFSOSG穩(wěn)定性分析
李雅普諾夫函數(shù)(V)為
對(duì)式(33)求導(dǎo),有
將式(24)代入式(34),有
將式(31)代入式(35),有
將式(30)代入式(36),有
式(37)說明在有限時(shí)間內(nèi),滑模面ξ收斂至0,根據(jù)滑模面ξ結(jié)構(gòu),滑模面S也將收斂至0,因此,航向角(χ)在VVFSOSG作用下能夠到達(dá)滑模面ξ并最終收斂至χd。
4 仿真驗(yàn)證
基于VVFSOSG的路徑跟蹤系統(tǒng)框圖[26],如圖3所示。圖3中:τu為前向推力;τr為轉(zhuǎn)艏力矩。
首先,根據(jù)船體位置和期望路徑,由式(7)求取位置誤差,由式(8)得到VVF的航速矢量場(chǎng)(χd)。VVF的χd和航向角(χ)通過式(32)得到VVFSOSG的χcε,最終利用航向和航速PID控制器實(shí)現(xiàn)USV路徑跟蹤控制。
Fossen模型參數(shù)[26]如下:船體長(zhǎng)度為1.180 m,寬度為0.355 m,質(zhì)量為8.46 kg,Izz=1.07,Xu′=-0.42,Yv′=-1.07,Nr′=-0.13,Xu=4.39,Yv=20,Yr=-0.2,Nv=-0.2,Nr=5,Xuu=-10.65,Yvv=-15.6,Nvr=-15。
基于VVFSOSG路徑跟蹤系統(tǒng),式(8)中VVF參數(shù)如下:Vm=10 m·s-1,k1=1。式(32)VVFSOSG參數(shù)如下:a=1.5,λ=50,η=25,δ=1,m=1/70,航向和航速均采用PID控制??刂破鲄?shù)如下:kp=150,ki=10,kd=100。
為展示VVF和VVFSOSG的優(yōu)越性,引入VF和VFG[16]。VF參數(shù)如下:α=1.5,k=1.5;VFG參數(shù)如下:κ=3.5,施加擾動(dòng)Δ[25],Aw=5,θ=0 rad,ωe=0.1 rad·s-1。期望路徑(L)設(shè)置如下:xP=t,yP=5sin(0.01t)和 5t+4,0≤t≤50 s。
當(dāng)航速(Vg)為7 m·s-1時(shí),USV直線跟蹤軌跡和位置誤差(ye),分別如圖4,5所示。由圖5可知:VFG跟蹤軌跡發(fā)生了抖動(dòng),位置誤差(ye)為±0.100 m,而VVFSOSG位置誤差(ye)僅有±0.001 m。
USV曲線跟蹤軌跡和位置誤差(ye),分別如圖6,7所示。由圖7可知:VFG跟蹤軌跡抖動(dòng)加劇,VFG位置誤差(ye)為±0.200 m,而VVFSOSG位置誤差(ye)僅有±0.003 m;基于VVFSOSG的路徑跟蹤系統(tǒng)能夠有效抵抗干擾,跟蹤系統(tǒng)的魯棒性更強(qiáng),跟蹤的精度更高。
當(dāng)k1=1時(shí),不同Vg時(shí)的USV跟蹤軌跡和航向角(χ)響應(yīng),分別如圖8,9所示。當(dāng)航速(Vg)為7 m·s-1時(shí),不同k1時(shí)的USV跟蹤軌跡和航向角(χ)響應(yīng),分別如圖10,11所示。
由圖9可知:不同航速Vg下,VVF 的USV跟蹤軌跡和航向角(χ)響應(yīng),表明航速(Vg)越大,航向角(χ)變化率越小,收斂時(shí)間越長(zhǎng),跟蹤軌跡越順滑;當(dāng)航速(Vg)為7 m·s-1時(shí),能夠提高USV航行安全性,并且收斂時(shí)間合適。
由圖10,11可知:k1越大,收斂時(shí)間越小,航向角變化率越大;當(dāng)k1=5時(shí),USV無法平穩(wěn)跟蹤期望路徑,出現(xiàn)抖動(dòng),因此,選擇k1=1最為合適。
5 結(jié)論
在實(shí)際航行中,由于航速(Vg)和航道未知擾動(dòng)的影響,致使轉(zhuǎn)艏角速度出現(xiàn)偏差,進(jìn)而影響航向角,最終導(dǎo)致了USV無法精準(zhǔn)跟蹤期望路徑。速度矢量場(chǎng)二階滑模無人艇引導(dǎo)律能實(shí)現(xiàn)快速準(zhǔn)確跟蹤期望路徑。
首先,建立運(yùn)動(dòng)學(xué)和航向角動(dòng)力學(xué)模型;其次,構(gòu)造路徑誤差模型,引入航速變量(Vg)設(shè)計(jì)VVF,以達(dá)到速度越大航向角變化越小,速度矢量場(chǎng)越平緩;最后,結(jié)合二階滑模面,設(shè)計(jì)一種VVFSOSG,并考慮未知擾動(dòng)分析VVFSOSG的穩(wěn)定性。
仿真結(jié)果表明:相比于VF,VVF有效實(shí)現(xiàn)航速(Vg)越快,航向角越小,能減小USV側(cè)翻風(fēng)險(xiǎn),有助于提高航行安全;基于VVFSOSG的路徑跟蹤控制系統(tǒng)魯棒性及路徑跟蹤準(zhǔn)確度更高。
參考文獻(xiàn):
[1]ZHANG Weidong,LIU Xiaocheng,HANG Peng.Progress and challenges of overwater unmanned systems[J].Acta Aytomatica Sinica,2022,46(5):847-857.DOI:10.16383/j.aas.c190363.
[2]JOONWOO A,SEHO S,MISUNG K,et al.Accurate path tracking by adjusting look-ahead point in pure pursuit method[J].International Journal of Automotive Technology,2021,22(1):119-129.DOI:10.1007/s12239-021-0013-7.
[3]LIU Lu,WANG Dang,ZHOU Huapeng,et al.Predictor-based LOS guidance law for path following of underactuated marine surface vehicles with side slip compensation[J].Ocean Engineering,2016,124:340-348.DOI:10.1109/ICICIP.2015.7388184.
[4]LIU Fengxu,SHEN Yue,WANG Dianrui,et al.Drift angle compensation-based adaptive line-of-sight path following for autonomous underwater vehicle[J].Applied Ocean Research,2019,93:101943.DOI:10.1016/j.apor.2019.101943.
[5]趙述龍,王祥科,張代兵,等.固定翼無人機(jī)曲線路徑跟蹤的積分向量場(chǎng)方法[J].國(guó)防科技大學(xué)學(xué)報(bào),2018,40(2):119-124.DOI:10.11887/j.cn.201802019.
[6]LIANG Yueqian,JIA Yingmin.Tangent vector field approach for curved path following with input saturation[J].Systems and Control Letter,2017,104:49-58.DOI:10.1016/j.sysconle.2017.04.001.
[7]LAWRENCE D A,F(xiàn)REW E W,PISANO W J.Lyapunov vector fields for autonomous unmanned aircraft flight control[J].Journal of Guidance,Control,and Dynamics: A Publication of the American Institute of Aeronautics and Astronautics Devoted to the Technology of Dynamics and Control,2008(5):31.DOI:10.2514/1.34896.
[8]FREW E W,LAWRENCE D A,MORRIS S.Coordinated standoff tracking of moving targets using Lyapunov guidance vector fields[J].Journal of Guidance Control and Dynamics,2008,31(2):290-306.DOI:10.2514/1.30507.
[9]JUN Woyoung,LIM S H,LE Dongjin,et al.Unmanned aircraft vector field path following with arrival angle control[J].Journal of Intelligent and Robotic Systems,2016,84:311-325.DOI:10.1007/s10846-016-0332-5.
[10]YAO Xuliang,WANG Xiaowei.Integral vector field control for three-dimensional path following of autonomous underwater vehicle[J].Journal of Marine Science and Technology,2021,26(1):159-173.DOI:10.1007/s00773-020-00730-9.
[11]YAO W,KAPITANYUK Y A,CAO M.Robotic path following in 3D using a guiding vector field[C]∥IEEE Conference on Decision and Control.New York:IEEE Press,2018:4475-4480.DOI:10.1109/CDC.2018.8619780.
[12]LIANG Yueqiang,JIA Yingmin.Combined vector field approach for 2D and 3D arbitrary twice differentiable curved path following with constrained UAVs[J].Journal of Intelligent and Robotic Systems,2016,83(1):133-160.DOI:10.1007/s10846-015-0308-x.
[13]DONG Wei,WANG Chunyan,LIU JunHui,et al.Three-dimensional vector guidance law with impact time and angle constraints[J].Journal of the Franklin Institute,2023,360(2):693-718.DOI:10.1016/j.jfranklin.2022.11.035.
[14]左宗玉,王欣欣.欠驅(qū)動(dòng)飛艇平面路徑跟蹤控制[J].控制與決策,2017,32(6):6.DOI:10.13195/j.kzyjc.2016.0464.
[15]NELSON D,BARBER D B,MCLAIN T,et al.Vector field path following for miniature air vehicles[J].IEEE Transactions on Robotics,2007,23(3):519-529.DOI:10.1109/TRO.2007.898976.
[16]WANG Ximen,SPANDAN R,STEFANO F,et al.Adaptive vector field guidance without a priori knowledge of course dynamics and wind[J].IEEE/ASME Transactions on Mechatronics,2022,27(6):4597-4607.DOI:10.1109/TMECH.2022.3160480.
[17]HX A,TIF B,CGS A.Uniformly semiglobally exponential stability of vector field guidance law and autopilot for path-following[J].European Journal of Control,2020,53:88-97.DOI:10.1016/j.ejcon.2019.09.007.
[18]XU Haitong,GUEDES S C.Vector field path following for surface marine vessel and parameter identification based on LS-SVM[J].Ocean Engineering,2016,113:151-161.DOI:10.1016/j.oceaneng.2015.12.037.
[19]DAS M,MAHANTA C.Optimal second order sliding mode control for linear uncertain systems[J].ISA Transactions,2014,53(6):1807-1815.DOI:10.1016/j.isatra.2014.08.010.
[20]王勃,王天擎,于泳,等.感應(yīng)電機(jī)電流環(huán)非線性積分滑??刂撇呗訹J].電工技術(shù)學(xué)報(bào),2021,36(10):2039-2048.DOI:10.19595/j.cnki.1000-6753.tces.200250.
[21]康爾良,賀建智,王一琛.永磁同步電機(jī)非奇異終端滑??刂破鞯脑O(shè)計(jì)[J].電機(jī)與控制學(xué)報(bào),2021,25(12):58-64.DOI:10.15938/j.emc.2021.12.007.
[22]陳正升,王雪松,程玉虎.考慮擾動(dòng)與輸入飽和的機(jī)械臂連續(xù)非奇異快速終端滑??刂芠J].控制與決策,2022,37(4):903-912.DOI:10.13195/j.kzyjc.2020.1335.
[23]CHALANGA A,KAMA S,F(xiàn)RIDMAN L M,et al.Implementation of super-twisting control: Super-twisting and higher order sliding-mode observer-based approaches[J].IEEE Transactions on Industrial Electronics,2016,63(6):3677-3685.DOI:10.1109/TIE.2016.2523913.
[24]GAO Weibing,HUNG J C.Variable structure control of nonlinear systems: A new approach[J].IEEE Transactions on Industrial Eletronics,1993,40(1):45.DOI:10.1109/41.184820.
[25]LUO Weilin,F(xiàn)AN Zhang.Time-invariant discontinuous feedback stablization of underactuated surface vessel under wave disturbance[C]∥IEEE International Conference on Security,Pattern Analysis,and Cybernetics.Chengdu:IEEE Press.2021:466-469.
[26]FOSSEN T I.Handbook of marine craft hydrodynamics and motion control[M].West Sussex: John Wiley and Sons,2011.
(責(zé)任編輯:陳志賢 ?英文審校:陳婧)