改進速度障礙法的無人艇局部路徑規(guī)劃

趙貴祥, 王晨旭, 王賀平, 李云淼

(天津大學海洋科學與技術學院, 天津 300110)

0 引 言

水面無人艇(unmanned surface vehicle, USV)作為一種輕量化、智能化的水上交通工具,具有操縱性好、搭載平臺多樣化等優(yōu)點,常被用來執(zhí)行海洋調查、海上搜救和環(huán)境監(jiān)測等工作[1-2]。路徑規(guī)劃作為運動控制的基礎,是USV研究的核心問題[3-4]。

根據(jù)環(huán)境信息的知悉情況,路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃,前者能在已知環(huán)境中引導USV到達目的地,后者能對未知障礙物實時避碰[5]。局部路徑規(guī)劃主要包括動態(tài)窗口算法(dynamic window approach, DWA)[6-7]、人工勢場(artificial potential field, APF)算法[8-9]與速度障礙(velocity obstacles, VO)法等。VO法由Fiorini等[10]在1998年首次提出,具有原理簡單、計算速度快的優(yōu)點,不僅能夠評估碰撞風險,同時也能提供避碰方法,因此被廣泛應用于局部路徑規(guī)劃的研究中。然而,目前VO法多用于無人車和無人機,很少被應用于USV的路徑規(guī)劃中[11-15]。近年來,國內(nèi)外學者展開了基于VO法的USV路徑規(guī)劃,并取得了一些成果。徐小強等[16]通過VO法和改進的APF算法設計了一種交替局部路徑規(guī)劃方法,通過調整算法的參數(shù),可對靜態(tài)和動態(tài)障礙物實現(xiàn)準確避碰。洪曉斌等[17]構建了矩形包圍的障礙物模型和USV相對運動模型,對VO法進行改進,實現(xiàn)了復雜環(huán)境下的USV路徑規(guī)劃。Zhu等[18]使用橢圓表示障礙物的安全距離,克服了VO法存在過度約束和以圓形表示障礙物不適用的問題。USV在海上航行時難免會遇到有人船,因此應遵守國際海上避碰規(guī)則(International Regulations for Preventing Collisions at Sea, COLREGs),以避免發(fā)生碰撞[19-21]。Kuwata等[22]執(zhí)行將COLREGs與VO法結合,用于USV的路徑規(guī)劃。Zhang等[23]提出了一種APF和VO法的混合算法,同時考慮了COLREGs中關于讓路船條款的約束。Emil等[24]基于COLREGs確定USV從障礙物的哪一側通過,并通過為動態(tài)障礙物分配一個擴展域,進而根據(jù)擴展域制定VO法。Niu等[25]基于COLREGs改進了VO法,右交叉和對遇局面的避讓能夠滿足COLREGs的要求,但忽略了對左交叉會遇的約束,可能會導致避讓的不協(xié)調,甚至帶來碰撞的威脅。經(jīng)過梳理和分析發(fā)現(xiàn),目前改進的VO法中障礙物擴展域的確定較為主觀,往往缺乏對兩船的尺度、速度、操縱性等因素的考慮,導致局部路徑規(guī)劃的避碰路徑長度和避碰時間過長。此外,基于COLREGs的VO法通常忽略緊迫局面會遇中避讓責任的約束[26]。

針對上述問題,本文基于文獻[25]的成果進一步對VO法進行改進:為減少規(guī)劃路徑的長度和時間,首先通過四元船舶領域確定了障礙物的擴展域,然后構建了碰撞威脅距離模型,以確定VO法執(zhí)行避讓的時機。為了更好地處理緊迫局面下的避碰責任約束,本文引入最晚施舵距離,建立了緊迫局面模型,并通過COLREGs中第8條、第13～第17條的條款對避碰模型進行了約束。

1 研究基礎

1.1 速度障礙法

文獻[10]詳細介紹了VO法的原理,此處只對VO法進行簡單的回顧。在局部路徑規(guī)劃中,VO法將USV視為質點,USV的尺度大小與兩者之間需保持的距離都擴展到目標船上,將在目標船上形成的擴展半徑稱為安全距離。若USV在碰撞威脅距離內(nèi)探測到目標船,USV與擴展圓形成一個錐形障礙區(qū)間,當相對速度落入錐形區(qū)域,視為存在碰撞沖突,具有避讓責任的船應進行避讓,速度障礙錐如圖1所示。

圖1 速度障礙法示意圖Fig.1 Schematic diagram of VO method

USV和目標船分別位于O和T兩點,速度分別為vo和vt,vr為目標船相對USV的速度,l為vr的延長線,di為安全距離,安全距離形成的圓形區(qū)域為Po,RCC為相對碰撞區(qū),RCC的數(shù)學表達式如下:

RCC={l∩PO≠0}

(1)

傳統(tǒng)VO法的執(zhí)行步驟如下:① 通過兩船的會遇參數(shù)計算目標船是否在碰撞錐內(nèi):若不在碰撞錐內(nèi)則無需避碰,否則應執(zhí)行下一步;② 改變本船速度大小或方向,使得本船能在兩船碰撞錐外通過;③ 若本船安全通過會遇船,本船朝著目標點駛去。

1.2 國際海上避碰規(guī)則分析

在能見度良好的寬闊水域中,USV的避碰行動通常應考慮COLREGs第8條和第13～17條的條款,以保證航行的安全。其中,第8條為避免碰撞行動的條款,規(guī)定了避讓行動應是及早的、大幅度的;若水域環(huán)境許可,僅靠轉向避碰是期望采取的;避碰行動在兩船駛過讓清后結束。第13～15條分別定義了追越局面、對遇局面和交叉局面,同時將會遇局面中的兩船分別定義為讓路船、直航船和具有同等避讓責任的船舶。第16條要求讓路船的避讓行動應遵守“早、大、寬、清”的原則,即讓路船應及早、大幅度地避讓,保證兩船在寬裕的距離上駛過讓清。第17條規(guī)定了直航船的避讓規(guī)則,若兩船發(fā)生碰撞威脅,直航船應保持航向和航速等待讓路船避讓,若讓路船違反了COLREGs的避讓條款使兩船形成緊迫局面,直航船具有緊急避讓的義務。緊迫局面指單憑一船避讓不能使兩船在安全距離外通過的局面。

為了將COLREGs集成到USV的避碰方法中,將常見的會遇態(tài)勢分為對遇、右交叉、左交叉和追越4種,如圖2所示。追越局面又可分為本船追越它船和本船被追越兩種。當兩船構成對遇局面時,兩船具有同等避讓責任;當兩船構成右交叉和追越它船的局面時,本船為讓路船,應及時對目標船避讓;當兩船構成左交叉或被追越的局面時,本船為直航船,目標船應給本船讓路。本文綜合方位、航向等信息采用的會遇態(tài)勢的計算公式為

(2)

圖2 會遇態(tài)勢的劃分Fig.2 Division of encounter situation

式中:θr為目標船相對本船的真方位;ΔC為兩船的航相差。

2 VO法的改進

2.1 安全距離的確定

傳統(tǒng)VO法通常通過障礙物的尺度主觀確定避碰的安全距離,能夠解決全驅動地面機器人的路徑規(guī)劃問題。然而,USV具有欠驅動和運動滯后的特點,在避碰時還應考慮船舶速度和操縱性的影響,否則不當?shù)谋芘鼍嚯x可能會導致碰撞的發(fā)生。四元船舶領域[27]考慮了船舶尺度、速度、操縱性和會遇參數(shù)等,能夠根據(jù)會遇情況隨時調整領域的大小,因此被用于計算本文VO法的安全距離。為了安全起見,取四元船舶領域模型中的最大半軸與目標船長的閔可夫斯基之和作為本船的安全距離。四元船舶領域的邊界方程為

(3)

(4)

式中:Rfore,Raft,Rport,Rstarb分別為船首船尾、左舷和右舷的橢圓半徑,計算公式可從文獻[27]中得到;λ為四元船舶領域邊界值;φ為橢圓上一點與橢圓長軸的夾角。兩船的安全距離dsafe可表示為

dsafe=max(λ)⊕lobject

(5)

式中:⊕為閔可夫斯基之和;lobject為目標船的長度,可通過船舶自動識別系統(tǒng)獲得。

2.2 構建碰撞威脅距離

在傳統(tǒng)的VO法中,當USV在自身探測范圍內(nèi)發(fā)現(xiàn)目標船時,開始判斷兩船速度是否落在碰撞錐內(nèi),若發(fā)生碰撞沖突,USV立即進行避讓。然而,不同USV配備的雷達的量程有所不同,影響著USV的避碰時機。為了讓兩船能在安全距離外通過,USV應在某個合理的距離上采取適當?shù)谋茏屝袆?在本文中這個距離被稱為碰撞威脅距離。船舶動界的概念和碰撞威脅距離類似,并且可根據(jù)兩船的操縱性和會遇參數(shù)靈活調整大小,因此本文通過動界的公式確定USV碰撞威脅距離,其取值[28]如下所示:

Darena=Df+k·vr

(6)

式中:Darena為動界邊界值;k為時間參數(shù);Advo為本船的進距,可通過旋回實驗得到;Advt為目標船的進距,可通過如下經(jīng)驗公式[28]得到:

Advt=2.531lobject+4.062bobject+23.83

(7)

式中:bobject為目標船的寬度,可通過船舶自動識別系統(tǒng)獲得。

2.3 緊迫局面的判斷

當本船與目標船構成左交叉及被追越時,本船為直航船。根據(jù)COLREGs第17條1、2款的要求,本船在碰撞的初始階段應保向保速等待讓路船避讓,但當目標船未及時避讓且兩船距離小于緊迫局面時的距離時形成緊迫局面,直航船可進行右轉避讓。航海界將最晚施舵時的距離作為碰撞危險局面過渡到緊迫局面的臨界值[29]。為方便計算緊迫局面的臨界值,建立北東坐標系,如圖3所示。本文將本船轉向90°后恰好能夠在安全的距離外成功避讓違反COLREGs的直航船時兩船的距離作為最晚施舵距離。

圖3 本船最晚施舵點示意圖Fig.3 Schematic diagram of the last rudder point of the ownship

(8)

(9)

假設本船轉向時速度不發(fā)生變化,t為船舶轉向90°所用的時間,兩船相對運動距離S=vrt[30],t計算如下:

(10)

式中:Dt為本船的旋回初徑。兩船相對速度vr的求法如下:

(11)

式中:φo和φt分別為本船和目標船的航向。聯(lián)立式(8)～式(11)并化簡,可得緊迫局面d的表達式并可求其解析解:

2dS(sinBt·sinθr+cosBt·cosθr)

(12)

3 USV避碰模型

傳統(tǒng)VO法可以判斷USV是否會與障礙物發(fā)生碰撞,但USV具體從何時開始與結束避障,還要進一步的判斷?；谝陨蠈O法的改進以及考慮COLREGs第8條和第13～17條的約束,本文設計的USV避碰算法流程如圖4所示。

圖4 局部路徑規(guī)劃流程圖Fig.4 Flowchart of local path planning

3.1 碰撞避讓模型

根據(jù)COLREGs第8條的描述,若環(huán)境允許,單憑轉向可能是避碰最有效的行動,因此本文在避讓時只考慮轉向。當目標船落在碰撞威脅距離內(nèi),開始判斷相對速度是否落在碰撞錐內(nèi),若落在碰撞錐內(nèi)且USV具有避碰義務,USV開始執(zhí)行避碰行動。

當本船為讓路船或為具有同等避讓責任的船時,本船應及時向右調整航向并朝向目標船擴展圓的右切點移動;當本船為直航船,在碰撞初始階段本船應保向保速等待讓路船避讓,但若目標船未及時避讓且兩船距離小于緊迫局面距離時形成緊迫局面,本船可緊急避讓。由于COLREGs第17條第3款規(guī)定“若當時環(huán)境許可,不應對在本船左舷的船采取向左轉向”,因此當USV為緊迫局面的直航船時,采取右轉避讓為宜。

在改進的VO法中,(x1,y1)和(x2,y2)為目標船擴展圓上的兩個切點;(xo,yo)和(xt,yt)分別為USV和目標船的位置;D為兩船的距離,可通過式(13)求解; Δθ為障礙錐的圓周角;θ2和θ1分別為左右切線與x軸的夾角;θ為兩船位置連線與x軸的夾角,可通過式(14)求解;θr為本船相對目標船的速度方向與x軸的夾角,可通過式(15)求解,碰撞錐的角度可通過式(16)求解,碰撞錐示意圖如圖5所示。

圖5 本船與目標船形成的碰撞錐Fig.5 Collision cone formed by ownship and target ship

(13)

(14)

(15)

(16)

如果θr∈(θ-Δθ/2,θ+Δθ/2),則兩船將在未來發(fā)生碰撞,USV應向右轉向,使相對運動線沿著右側的切線,求解公式如下:

(17)

求解式(17)方程組可得(x1,y1),(x2,y2)兩組解,分別為圖5中的兩個切點,如果Δθ滿足式(20),則求出來的(x1,y1)為右側切點。

(18)

(19)

Δθ=θ2-θ1, 0<Δθ<π; Δθ<-π

(20)

在已知本船和目標船航速航向的條件下,若要求僅改變本船的航向、使得兩船的相對運動能夠沿著右側切線航行,本船需要改變的新航向為

(21)

式中:φnew為滿足相對運動、沿著右側切線時USV調整后的新航向。

在下一周期USV和目標船的位置更新如下所示:

(22)

(23)

式中:Δt為位置更新周期。

3.2 航向恢復的時機

COLREGs中第8條第4款、第13條第4款、第16條多次出現(xiàn)“駛過讓清”一詞,即一旦形成某一種會遇態(tài)勢,則要求避讓行動一直進行,除非兩船解除碰撞威脅。本文將本船運動到兩船的速度碰撞錐與障礙物擴展圓的右側切點視為解除碰撞威脅,若滿足:

(xo,yo)=(x1,y1)

(24)

USV可改變航向朝著局部目標點航行。

4 仿真實驗與結果分析

4.1 4種會遇態(tài)勢的仿真結果

為了驗證本文改進VO法的有效性,以“Dolphin-I”號USV為例進行仿真。在仿真計算中,本船和目標船的船舶尺度及操縱性能等參數(shù)都參考“Dolphin-I”,如表1所示。為了各組數(shù)據(jù)的一致性,假定目標船航向航速恒定,在會遇時能夠與本船構成對遇、右交叉、左交叉和追越他船的會遇態(tài)勢。本文不考慮風浪流的影響,在北東坐標系中,設定USV的起始點坐標為(100,100),目標點坐標為(100,1 020),USV速度為3 m/s,轉艏速率為2.5°/s,航向為90°。在4種不同場景中,目標船的初始運動信息如表2所示。

表1 Dolphin-I參數(shù)

表2 目標船的初始信息

在上述4種場景中,分別驗證了改進算法避讓的有效性。在實驗結果中,綠色軌跡表示USV航行的軌跡,粉紅色軌跡表示目標船的軌跡。當目標船的顏色為黃色時,表示此時無需避讓或避讓完成;當目標船的顏色變成紅色時,表示USV需要執(zhí)行避讓行動。如圖6(a)所示,兩船構成對遇局面,當時間t=150 s時,兩船相距205.17 m,碰撞威脅距離為218.24 m,相對運動落在碰撞錐內(nèi),USV具有避碰責任,需要給目標船讓路。USV開始右轉使相對運動沿著碰撞錐右側的切線運動。當t=205 s時,已成功避讓目標船,此時解除兩船右交叉的會遇態(tài)勢,本船逐漸轉向并駛向目標點,如圖6(b)所示。同理,可對場景2～場景4中的右交叉、左交叉和追越他船的會遇態(tài)勢進行分析,可以得到類似的結果,都能夠在滿足安全距離的同時成功避讓目標船,場景2～場景4的避讓結果如圖7～圖9所示。

圖6 對遇局面的避讓行動Fig.6 Avoidance action in head-on situation

圖7 右交叉局面的避讓行動Fig.7 Avoidance action in right-crossing situation

圖9 追越局面的避讓行動Fig.9 Avoidance action in over-taking situation

4.2 算法對比

將本文改進的VO法與文獻[25]中的改進VO法方法進行對比,驗證改進的有效性。對比實驗中船舶安全通過的閾值參考原文獻中的數(shù)據(jù),其他參數(shù)參考表1和表2的數(shù)值。圖10和圖11分別表示了改進前后避讓過程兩船的距離,圖12為改進前左交叉局面的避讓圖,表3～表5記錄了改進前后4種會遇避讓過程的參數(shù)。

圖10 改進前各場景避讓過程所保持的距離Fig.10Distance of each scene in avoidance process before improvement

圖11 改進后各場景避讓過程所保持的距離Fig.11 Distance of each scene in avoidance process after improvement

圖12 改進前的左交叉局面的避讓行動Fig.12 Avoidance action in left-crossing situation before improvements

表3 場景1～場景4的避碰結果

表4 改進前避碰過程中的相關參數(shù)

表5 改進后避碰過程中的相關參數(shù)

(1) 在改進前的局部路徑規(guī)劃中,未能夠根據(jù)船舶實際情況設置安全閾值,USV較早地避讓目標船,導致避碰的距離和時間較長,如圖10和表4所示。改進后考慮了船舶尺度、速度和操縱性等,確定了安全距離和碰撞威脅距離,在4種場景的避讓過程中,兩船的距離始終都不低于本船所要求的安全距離。在確保安全避讓的條件下,推遲了避讓的時機,使得避碰的路徑和時間較短,如圖11所示。改進后的算法較改進前平均路徑長度縮短了38.88%,平均規(guī)劃時間減少了44.26%,如表3所示。驗證了本文改進算法可以有效地縮短避碰的路徑和時間。

(2) 改進前的算法沒有考慮當本船作為直航船時的避讓責任。如圖12所示,當場景3中的USV與目標船構成左交叉的會遇態(tài)勢時,USV并沒有認清自身為直航船的身份,仍按照主動避讓的方式進行避碰,在形成碰撞危險時執(zhí)行避讓行動。這種避碰模型違背了COLREGs對直航船避碰行動的要求。改進后的算法通過最晚施舵距離確定了緊迫局面。從表5可知,場景3在避碰過程中形成了緊迫局面。如圖8(a)所示,USV在碰撞危險的初始階段仍保向保速,直到兩船形成緊迫局面,USV開始執(zhí)行避讓的行動,滿足了COLREGs中第17條的規(guī)定,如圖8(b)所示。此外,在場景1、2和4的會遇中,USV均為讓路船且都按照COLREGs第13～15條的條款主動避碰,同時避碰行動符合“早”“大”“寬”“清”的原則,滿足COLREGs第8條和第16條的要求,如圖6、圖7和圖9所示。因此,證明了改進的VO法規(guī)劃的路徑在保證避碰安全的同時滿足COLREGs第8條和第13～17條的避讓要求。

5 結束語

針對USV基于VO法的局部路徑規(guī)劃時避碰的路徑和時間較長以及忽略緊迫局面中避碰責任約束的問題,提出一種改進的VO法,通過仿真驗證得出以下結論:

(1) 通過四元船舶領域和船舶動界分別確定VO法中的安全距離和碰撞威脅距離,能夠縮短避碰路徑長度和避碰耗時。改進后的VO法較改進前平均路徑長度縮短38.88%,平均規(guī)劃時間減少了44.26%。

(2) 改進的VO法規(guī)劃的路徑在保證避碰安全的同時滿足COLREGs第8條和第13～17條的避讓要求。當USV為讓路船或具有同等避讓責任的船時,能夠及時避讓目標船;當USV為直航船時,能夠保持航速和航向、等待目標船執(zhí)行避讓的行動,同時能夠在兩船構成緊迫局面時進行緊急避讓。

在未來的研究中,應考慮風浪流等環(huán)境因素和目標船機動的不確定因素,并研究多智能體之間的協(xié)調避碰。同時,結合全局路徑規(guī)劃,以滿足USV在海上自主航行的實際情況。