基于改進APF算法的水面無人艇局部路徑規(guī)劃

張 揚,彭鵬菲,曹 杰

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

0 引言

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

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

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

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

1.1 動態(tài)避碰問題分析

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

圖1 局部避碰流程

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

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

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

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

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

1) 追越:當無人艇與障礙船處于追越局面時,無人艇應從障礙船左舷方向通過,如圖3(a)所示。

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

2) 對遇:當無人艇與障礙船對遇,兩者的航向相反或近似相反并存在發(fā)生碰撞的危險時,無人艇應從障礙船左舷,即己方右舷方向通過,如圖3(b)所示。

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

2 人工勢場法

2.1 原理

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

引力勢場函數(shù)隨著無人艇與目標點的距離增加而增加,公式為

(1)

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

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

kattρ(Xp,Xg)Frep(Oi)

(2)

Fatt(X)的大小與距離ρ(Xp,Xg)成正比,距離越大,引力越大,到達目的點時引力為0,方向為從無人艇當前位置指向目標點。引力對無人艇的作用是全局的,只要無人艇存在于人工勢場范圍內,就受到目標點引力的影響,因此,引力場函數(shù)是連續(xù)變化的。

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

斥力勢場函數(shù)公式為

(3)

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

障礙物對無人艇的斥力為斥力勢場函數(shù)的負梯度,斥力函數(shù)公式為:

Frep=-▽Urep=

(4)

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

無人艇航行中會遇到多個障礙物,其在某位置最終受到的合力為受到的所有斥力與目標點的引力之和,合力勢場函數(shù)為

(5)

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

(6)

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

2.2 存在問題

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

1) 目標不可達

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

圖4 目標點不可達示意圖

2) 局部極小值

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

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

3 人工勢場法的改進

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

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

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

3.2 逃逸力的提出

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

(8)

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

(9)

圖6 逃逸力的設計

3.3 轉向力的提出

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

圖7 USV與障礙物的方位圖

轉向力的定義如下:

(10)

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

4 仿真實驗

為驗證改進APF算法的有效性,分別對靜態(tài)未知障礙物以及動態(tài)未知障礙物進行仿真實驗。

4.1 靜態(tài)未知障礙物實驗

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

1) 局部極小點仿真實驗

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

圖8 局部最小問題解決示意圖

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

2) 靜態(tài)多障礙物實驗

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

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

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

4.2 動態(tài)未知障礙物實驗

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

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

表1 障礙物運動參數(shù)

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

圖10 多船會遇場景仿真

仿真結果顯示,改進APF算法在復雜環(huán)境下,能引導無人艇對多個動態(tài)障礙進行安全規(guī)避。避碰軌跡具有較好的平滑性和連續(xù)性,滿足無人艇的運動能力約束,同時能很好地兼顧COLREGs的避碰要求。

5 結論

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