張 揚(yáng),彭鵬菲,曹 杰

(1.海軍工程大學(xué) 電子工程學(xué)院,武漢 430000;2.中國(guó)人民解放軍92057部隊(duì),廣東 湛江 524000;3.中國(guó)人民解放軍91526部隊(duì),廣東 湛江 524000)

0 引言

水面無(wú)人艇在航行中會(huì)遇到未知的障礙物,例如電子海圖未及時(shí)更新的靜態(tài)障礙物,還有大型魚(yú)類(lèi)、移動(dòng)船舶等動(dòng)態(tài)障礙物。在復(fù)雜障礙物環(huán)境和動(dòng)態(tài)環(huán)境下水面無(wú)人艇路徑規(guī)劃的主要目標(biāo)不是路徑長(zhǎng)度和能量消耗,而是無(wú)人艇應(yīng)對(duì)危險(xiǎn)規(guī)避障礙物的能力。傳感器探測(cè)的范圍作為局部路徑規(guī)劃的環(huán)境,此環(huán)境通常是未知且動(dòng)態(tài)變化的,故局部路徑規(guī)劃需要快速且高效的環(huán)境建模,對(duì)路徑搜索和避障的動(dòng)態(tài)性能要求更高。無(wú)人艇動(dòng)態(tài)避障算法通常有快速擴(kuò)展隨機(jī)樹(shù)算法[1](rapidly-exploring random tree,RRT)、速度障礙法[2](reciprocal velocity obstacle,RVO)、粒子群算法(PSO)[3]、動(dòng)態(tài)窗口法[4-5](dynamic window approach,DWA)、人工勢(shì)場(chǎng)法[6](artificial potential field,APF)等。

人工勢(shì)場(chǎng)法(APF)根據(jù)無(wú)人艇當(dāng)前位置、障礙物和目標(biāo)點(diǎn)的相對(duì)態(tài)勢(shì)引導(dǎo)水面無(wú)人艇安全航行,因其在未知環(huán)境和動(dòng)態(tài)環(huán)境下的良好適應(yīng)性而得到廣泛應(yīng)用。然而,人工勢(shì)場(chǎng)法存在一定的局限性,例如規(guī)劃的路徑不平滑,出現(xiàn)目標(biāo)不可達(dá)等問(wèn)題。因此,國(guó)內(nèi)外學(xué)者提出大量的改進(jìn)辦法。易先軍等[7]對(duì)斥力勢(shì)場(chǎng)劃分成不同的梯度等級(jí),同時(shí)采用逃逸力,解決了局部最小的問(wèn)題;陳勁峰等[8]對(duì)機(jī)器人受到的無(wú)效的斥力進(jìn)行忽略,并采用逃離的方法解決局部最小值問(wèn)題;徐小強(qiáng)等[9]為了解決路徑過(guò)長(zhǎng)的問(wèn)題,采用安全距離的概念,并且在航行中進(jìn)行距離預(yù)測(cè)解決局部最小值的問(wèn)題;程志等[10]對(duì)斥力進(jìn)行改進(jìn),并在陷入局部最小值后通過(guò)設(shè)置虛擬目標(biāo)點(diǎn)引導(dǎo)機(jī)器人逃離該區(qū)域;羅強(qiáng)等[11]在機(jī)器人行駛中僅考慮一定范圍內(nèi)受到的斥力,其余斥力不考慮,并對(duì)不同類(lèi)型障礙物形成的局部最小問(wèn)題采用不同的解決策略。

本文中采用改進(jìn)的人工勢(shì)場(chǎng)法對(duì)水面無(wú)人艇的局部路徑進(jìn)行規(guī)劃。針對(duì)人工勢(shì)場(chǎng)法出現(xiàn)的局部最小值和路徑不平滑的問(wèn)題,對(duì)其斥力系數(shù)進(jìn)行改進(jìn),并引入逃逸力。為了防止由于船舶隨意航行造成海上事故,在避碰過(guò)程中需遵守相關(guān)的海上船舶航行的規(guī)范。因此,在動(dòng)態(tài)避障中結(jié)合《避碰規(guī)則》引入轉(zhuǎn)向力。通過(guò)仿真實(shí)驗(yàn),得到的路徑更加平滑且解決了局部最小值問(wèn)題,在與動(dòng)態(tài)障礙物會(huì)遇時(shí),均能在符合避碰規(guī)則的前提下安全避碰。

1 局部路徑規(guī)劃問(wèn)題分析

1.1 動(dòng)態(tài)避碰問(wèn)題分析

水面無(wú)人艇在未知環(huán)境中航行時(shí),其只知道當(dāng)前位置和終點(diǎn)坐標(biāo),依據(jù)自身傳感器探測(cè)環(huán)境信息,獲得障礙物的大小、形狀、方位以及動(dòng)態(tài)障礙物的移動(dòng)速度。根據(jù)障礙物信息進(jìn)行局部范圍的調(diào)整,避開(kāi)障礙物后回到原有航線(xiàn)繼續(xù)航行,局部避碰流程如圖1所示。

圖1 局部避碰流程

1.2 基于USV特征的避碰規(guī)則

無(wú)人艇作為海上無(wú)人駕駛平臺(tái),避碰決策時(shí)為了保證本艇與障礙船的安全,在航行中也需遵守國(guó)際海上避碰規(guī)則(international regulations for preventing collisions at sea,COLREGS)[12],按照規(guī)定的要求進(jìn)行避讓,防止在海面上任意航行導(dǎo)致與會(huì)遇船的行動(dòng)不協(xié)調(diào),從而對(duì)它船及人員造成傷害。

COLREGs對(duì)機(jī)動(dòng)船只在海洋航行過(guò)程中發(fā)生碰撞的情形做出了相應(yīng)的規(guī)定。由于海上避碰規(guī)則是為有人船的船員航行中提供操縱船舶進(jìn)行避碰的參考策略,并不能完全適用于USV避碰。USV船體較小、雷達(dá)反射面積小、探測(cè)信號(hào)易被海波干擾,會(huì)使得其他船舶在探測(cè)USV位置等運(yùn)動(dòng)信息時(shí)存在困難。但水面無(wú)人艇機(jī)動(dòng)性強(qiáng),因此,在實(shí)際會(huì)遇局面中,無(wú)人艇作為主動(dòng)避讓方執(zhí)行規(guī)避動(dòng)作將更加安全。由于USV快速靈活,方位角度劃分過(guò)小易造成會(huì)遇局勢(shì)頻繁改變,不利于避碰策略的計(jì)算,所以結(jié)合USV的運(yùn)動(dòng)特點(diǎn)將會(huì)遇局勢(shì)劃分為四個(gè)部分,分別為追越、對(duì)遇、左交叉相遇、右交叉相遇,如圖2所示。其中USV表示本艇,Obs表示障礙船。

圖2 會(huì)遇態(tài)勢(shì)劃分

在USV與障礙物發(fā)生以上4種會(huì)遇態(tài)勢(shì)時(shí),USV均為讓路船,其他障礙物定義為直行船。綜上所述,根據(jù)USV的特性和COLREGS的相關(guān)規(guī)則,USV在海洋中航行時(shí)對(duì)于移動(dòng)障礙物的避碰規(guī)則被設(shè)計(jì)如下:

1) 追越:當(dāng)無(wú)人艇與障礙船處于追越局面時(shí),無(wú)人艇應(yīng)從障礙船左舷方向通過(guò),如圖3(a)所示。

圖3 USV在不同情況下的海事規(guī)則

2) 對(duì)遇:當(dāng)無(wú)人艇與障礙船對(duì)遇,兩者的航向相反或近似相反并存在發(fā)生碰撞的危險(xiǎn)時(shí),無(wú)人艇應(yīng)從障礙船左舷,即己方右舷方向通過(guò),如圖3(b)所示。

3) 交叉相遇:當(dāng)無(wú)人艇與障礙船處于交叉局面時(shí),無(wú)人艇應(yīng)從障礙船船尾方向通過(guò),避免從他船的前方通過(guò),如圖3(c)、圖3(d)所示。

2 人工勢(shì)場(chǎng)法

2.1 原理

人工勢(shì)場(chǎng)法最早由Khatib[13]提出,其原理是將無(wú)人艇與障礙物和目標(biāo)點(diǎn)之間的相互影響視為一個(gè)虛擬力場(chǎng),無(wú)人艇的出發(fā)點(diǎn)在高勢(shì)場(chǎng)處,終點(diǎn)在低勢(shì)場(chǎng)處,在航行中受到障礙物的斥力作用和目標(biāo)點(diǎn)對(duì)其吸引力作用,最終在合力的作用下避開(kāi)障礙物安全航行至目標(biāo)點(diǎn)。

引力勢(shì)場(chǎng)函數(shù)隨著無(wú)人艇與目標(biāo)點(diǎn)的距離增加而增加,公式為

(1)

式中:Uatt為引力勢(shì)場(chǎng);Xp為當(dāng)前位置;Xg為目標(biāo)點(diǎn)位置;ρ(Xp,Xg)為水面無(wú)人艇離目標(biāo)點(diǎn)距離,katt>0為引力勢(shì)場(chǎng)增益因子,由式(1)得出,引力勢(shì)場(chǎng)由引力勢(shì)場(chǎng)因子和距離ρ(Xp,Xg)共同決定,其大小與ρ(Xp,Xg)的平方成正比,在目標(biāo)點(diǎn)處Uatt最小。由于無(wú)人艇從勢(shì)能高的位置移向勢(shì)能低的位置,因此,其在引力場(chǎng)的作用下向目標(biāo)點(diǎn)移動(dòng)。

Fatt為Uatt的負(fù)梯度,也是Uatt下降最快的方向,公式為:

kattρ(Xp,Xg)Frep(Oi)

(2)

Fatt(X)的大小與距離ρ(Xp,Xg)成正比,距離越大,引力越大,到達(dá)目的點(diǎn)時(shí)引力為0,方向?yàn)閺臒o(wú)人艇當(dāng)前位置指向目標(biāo)點(diǎn)。引力對(duì)無(wú)人艇的作用是全局的,只要無(wú)人艇存在于人工勢(shì)場(chǎng)范圍內(nèi),就受到目標(biāo)點(diǎn)引力的影響,因此,引力場(chǎng)函數(shù)是連續(xù)變化的。

每個(gè)障礙物的斥力影響是有范圍的,無(wú)人艇進(jìn)入障礙物的斥力影響區(qū)域內(nèi),才受到斥力作用,斥力的大小與二者相對(duì)距離成反比,當(dāng)無(wú)人艇與障礙物無(wú)限靠近時(shí),受到的斥力接近無(wú)窮大,無(wú)人艇所受到的斥力是分段的非連續(xù)。

斥力勢(shì)場(chǎng)函數(shù)公式為

(3)

式中:ρ(Xp,X0)為水面無(wú)人艇到障礙物的距離;ρ0為障礙物影響范圍的半徑;krep>0為斥力勢(shì)場(chǎng)的增益因子。

障礙物對(duì)無(wú)人艇的斥力為斥力勢(shì)場(chǎng)函數(shù)的負(fù)梯度,斥力函數(shù)公式為:

Frep=-▽Urep=

(4)

Frep的作用下使得無(wú)人艇遠(yuǎn)離障礙物。Frep與ρ(Xp,X0)近似成反比。

無(wú)人艇航行中會(huì)遇到多個(gè)障礙物,其在某位置最終受到的合力為受到的所有斥力與目標(biāo)點(diǎn)的引力之和,合力勢(shì)場(chǎng)函數(shù)為

(5)

式中:Uall為無(wú)人艇受到的總勢(shì)場(chǎng);N為障礙物的個(gè)數(shù);Urep(Oi)為第i個(gè)障礙物產(chǎn)生的斥力勢(shì)場(chǎng)。所受合力為

(6)

式中:Fall為合力;Frep(Oi)為第i個(gè)障礙物的斥力。

2.2 存在問(wèn)題

人工勢(shì)場(chǎng)法通過(guò)勢(shì)場(chǎng)函數(shù)反映了無(wú)人艇、障礙物、目標(biāo)點(diǎn)三者之間的實(shí)時(shí)狀態(tài)信息,通過(guò)由障礙物的斥力與目標(biāo)點(diǎn)的引力組成的合力引導(dǎo)無(wú)人艇行駛。該算法實(shí)現(xiàn)簡(jiǎn)單,實(shí)效性高,適用于局部路徑規(guī)劃,但也存在著許多問(wèn)題。

1) 目標(biāo)不可達(dá)

無(wú)人艇在行駛中受到引力場(chǎng)和斥力場(chǎng)的共同作用,在理想情況下,會(huì)在合力的作用下遠(yuǎn)離障礙物最終到達(dá)目標(biāo)點(diǎn)。由于斥力隨著無(wú)人艇與障礙物的距離的減小而增大,而引力隨著無(wú)人艇與目標(biāo)點(diǎn)的距離減小而減小,則在航行過(guò)程遇到障礙物時(shí),會(huì)存在斥力大于引力的區(qū)域。當(dāng)目標(biāo)點(diǎn)位于這些區(qū)域時(shí),無(wú)人艇接近目標(biāo)點(diǎn),引力在不斷減小,同時(shí)離障礙物的距離也在減小,對(duì)其產(chǎn)生的斥力不斷增加,其所受斥力大于引力,無(wú)人艇將無(wú)法到目標(biāo)點(diǎn),導(dǎo)致局部路徑規(guī)劃失敗,如圖4所示。

圖4 目標(biāo)點(diǎn)不可達(dá)示意圖

2) 局部極小值

當(dāng)某點(diǎn)的引力勢(shì)場(chǎng)等于斥力勢(shì)場(chǎng)時(shí),無(wú)人艇受到的合力為0,該點(diǎn)稱(chēng)為局部極小值點(diǎn)。陷入局部極小值點(diǎn)的無(wú)人艇會(huì)停止運(yùn)動(dòng)或者在局部極小值點(diǎn)附近來(lái)回振動(dòng)無(wú)法脫離,無(wú)人艇的航向、航速就會(huì)快速變化,處于振蕩狀態(tài),在實(shí)際航行中,會(huì)導(dǎo)致無(wú)人艇頻繁操舵,陷入局部最小值。此情況下無(wú)人艇、障礙物與目標(biāo)點(diǎn)相對(duì)位置如圖5所示。

圖5 局部極小值狀態(tài)示意圖

3 人工勢(shì)場(chǎng)法的改進(jìn)

3.1 斥力系數(shù)的改進(jìn)

傳統(tǒng)APF方法中,krep>0為斥力勢(shì)場(chǎng)的增益系數(shù),其固定不變。由于離障礙物越近其危險(xiǎn)度越高,將krep設(shè)為隨距離動(dòng)態(tài)調(diào)整,剛進(jìn)入障礙物作用區(qū)域內(nèi),瞬間觸發(fā)排斥力,隨著離障礙物的距離接近,krep的值不斷增大,其設(shè)置如下

式中:ρ0為障礙物作用距離;d為無(wú)人艇距離障礙物距離;k0為常量;amax和amin為斥力系數(shù)條件參數(shù);dgoal為無(wú)人艇。

3.2 逃逸力的提出

由3.2節(jié)可知,當(dāng)斥力與引力大小相等,方向相反時(shí),無(wú)人艇所受合力為0,出現(xiàn)局部最優(yōu)的情況。針對(duì)此問(wèn)題,提出逃逸力表達(dá)式為

(8)

式中:dgoal為無(wú)人艇與目標(biāo)點(diǎn)距離;dob-g為障礙物與目標(biāo)點(diǎn)的距離,障礙物對(duì)無(wú)人艇的影響不僅與障礙物和無(wú)人艇的距離有關(guān),還與無(wú)人艇與目標(biāo)點(diǎn)兩者間的相對(duì)角度有關(guān)。ω為無(wú)人艇與障礙物之間夾角的系數(shù),其公式如下

(9)

圖6 逃逸力的設(shè)計(jì)

3.3 轉(zhuǎn)向力的提出

為了使無(wú)人艇在遇到動(dòng)態(tài)船舶時(shí),按照《避碰規(guī)則》進(jìn)行避碰,根據(jù)會(huì)遇時(shí)無(wú)人艇與障礙船速度的夾角情況,引入轉(zhuǎn)向力,使得無(wú)人艇向正確的方向進(jìn)行避讓。圖7為USV與障礙物的方位圖。

圖7 USV與障礙物的方位圖

轉(zhuǎn)向力的定義如下:

(10)

式中:k為常數(shù);α為會(huì)遇船與無(wú)人艇航向之間的夾角;φ為無(wú)人艇當(dāng)前航向;β為會(huì)遇船航行方向與無(wú)人艇航向的夾角,當(dāng)β∈(-67.5°,67.5°)為追越狀態(tài),無(wú)人艇需向左轉(zhuǎn)向,當(dāng)β∈(67.5°,165°)時(shí),為右交叉相遇,無(wú)人艇需向右轉(zhuǎn)向,當(dāng)β∈(-165°,-67.5°)為左交叉相遇,無(wú)人艇需向左轉(zhuǎn)向,當(dāng)為對(duì)遇狀態(tài)時(shí)無(wú)人艇需向右轉(zhuǎn)向,此時(shí)β∈(-180°,-165°)∪(165°,180°)。

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

為驗(yàn)證改進(jìn)APF算法的有效性,分別對(duì)靜態(tài)未知障礙物以及動(dòng)態(tài)未知障礙物進(jìn)行仿真實(shí)驗(yàn)。

4.1 靜態(tài)未知障礙物實(shí)驗(yàn)

水面無(wú)人艇在航行中,除了已知障礙物外,還會(huì)遇沉船、礁石、拋錨船舶等靜態(tài)障礙物。由于這些障礙物提前并不知道,在航行中傳感器獲得到的障礙物信息通常是不規(guī)則的形狀,常見(jiàn)的方式是將障礙物用圓形表示,通過(guò)雷達(dá)掃描獲得其地理位置,并以此為圓心,設(shè)定一定距離的安全半徑。

1) 局部極小點(diǎn)仿真實(shí)驗(yàn)

將無(wú)人艇起始點(diǎn)坐標(biāo)設(shè)為(5,5),目標(biāo)點(diǎn)坐標(biāo)為(48,48),障礙物1半徑r1為3 m,坐標(biāo)為(20,20),障礙物2半徑r2為2 m,坐標(biāo)為(35,40),斥力影響范圍ρ0=3r。此時(shí)障礙物正好出現(xiàn)在無(wú)人艇與目標(biāo)點(diǎn)連線(xiàn)上,引力和斥力的方向相反,采用傳統(tǒng)的人工勢(shì)場(chǎng)法函數(shù)會(huì)出現(xiàn)無(wú)人艇停止在障礙物前方,如圖8(a)所示。

圖8 局部最小問(wèn)題解決示意圖

針對(duì)局部最小的問(wèn)題,對(duì)APF算法的斥力系數(shù)進(jìn)行調(diào)整,使得離障礙物越近斥力系數(shù)越大,并且在無(wú)人艇行駛方向接近障礙物時(shí),引入逃逸力,與無(wú)人艇所受的斥力方向垂直,在其作用下無(wú)人艇遠(yuǎn)離障礙物,解決局部最小的問(wèn)題,如圖8(b)所示。

2) 靜態(tài)多障礙物實(shí)驗(yàn)

在60 m×60 m的區(qū)域內(nèi),設(shè)定無(wú)人艇的起始點(diǎn)為(5,5),速度為3 m/s,目標(biāo)點(diǎn)為(48,48),障礙物個(gè)數(shù)為 5個(gè),障礙物坐標(biāo)分別為{(20,19.8);(35,40);(32,40);(30,40);(42,35)},障礙物半徑分別為{3;2;1;1;2 }。

傳統(tǒng)APF算法與改進(jìn)APF算法、文獻(xiàn)[7]中算法規(guī)劃的路徑對(duì)比如圖9所示。傳統(tǒng)APF算法距離障礙物最近距離0.77 m,總路徑長(zhǎng)度65.0 m,文獻(xiàn)[7]中算法距離障礙物最近距離為1.58 m?？偮窂介L(zhǎng)度64.5 m,改進(jìn)APF算法距離障礙物最近距離2.05 m,總路徑長(zhǎng)度64.0 m,改進(jìn)后的算法路徑縮短,距離障礙物更遠(yuǎn),安全性得到提高,路徑更加平滑,更符合無(wú)人艇的動(dòng)力性能。

圖9 靜態(tài)多個(gè)未知障礙物路徑對(duì)比

4.2 動(dòng)態(tài)未知障礙物實(shí)驗(yàn)

在同一環(huán)境下設(shè)置了具有不同運(yùn)動(dòng)態(tài)勢(shì)的多個(gè)障礙物,以此來(lái)驗(yàn)證算法在多障礙局面下的避碰效果。

設(shè)定無(wú)人艇起始位置為(5,5),目標(biāo)點(diǎn)為(78,48),無(wú)人艇航速為 3 m/s,障礙物數(shù)量為 4,障礙物半徑r為5,障礙物斥力的影響范圍ρ0=3r,USV的禁航區(qū)用一個(gè)紅色圓圈表示,4個(gè)障礙物起始坐標(biāo)分別為{(15,9);(70,20);(80,44);(85,87)},障礙物速度大小和速度方向?yàn)閧(1,20°);(3,160°);(2,200°);(2.8,250°)}。4個(gè)動(dòng)態(tài)障礙的參數(shù)設(shè)置如表1所示。

表1 障礙物運(yùn)動(dòng)參數(shù)

無(wú)人艇與多個(gè)障礙物的避碰過(guò)程如圖10所示。圖10(a)中無(wú)人艇在航行中首先與障礙物1呈現(xiàn)追越態(tài)勢(shì),根據(jù)COLREGs規(guī)則通過(guò)向左轉(zhuǎn)向超越障礙物A,圖10(b)中與障礙物2呈現(xiàn)右交叉相遇,根據(jù)COLREGs規(guī)則無(wú)人艇向右進(jìn)行轉(zhuǎn)向,圖10(c)中與障礙物3呈現(xiàn)對(duì)遇態(tài)勢(shì),根據(jù)COLREGs規(guī)則無(wú)人艇向右轉(zhuǎn)向,圖10(d)中與障礙物4呈現(xiàn)左交叉相遇局面,根據(jù)COLREGs規(guī)則向左轉(zhuǎn)向,隨后安全駛向目標(biāo)點(diǎn)。

圖10 多船會(huì)遇場(chǎng)景仿真

仿真結(jié)果顯示,改進(jìn)APF算法在復(fù)雜環(huán)境下,能引導(dǎo)無(wú)人艇對(duì)多個(gè)動(dòng)態(tài)障礙進(jìn)行安全規(guī)避。避碰軌跡具有較好的平滑性和連續(xù)性,滿(mǎn)足無(wú)人艇的運(yùn)動(dòng)能力約束,同時(shí)能很好地兼顧C(jī)OLREGs的避碰要求。

5 結(jié)論

針對(duì)人工勢(shì)場(chǎng)法的原理與缺點(diǎn)進(jìn)行介紹,結(jié)合《避碰規(guī)則》設(shè)計(jì)了無(wú)人艇動(dòng)態(tài)避碰策略,并進(jìn)行了仿真實(shí)驗(yàn)。通過(guò)對(duì)靜態(tài)未知障礙物避碰實(shí)驗(yàn)結(jié)果進(jìn)行分析,局部最優(yōu)問(wèn)題得到解決且獲得更加安全平滑的避碰路徑。通過(guò)對(duì)移動(dòng)未知障礙物會(huì)遇情形的仿真實(shí)驗(yàn),驗(yàn)證了USV能夠在遵守《避碰規(guī)則》的前提下進(jìn)行安全避碰。