基于動態領域勢場法的船舶避碰路徑規劃

孫 碩,楊少龍,向先波,范 雪

(華中科技大學 船舶與海洋工程學院,湖北 武漢,430074)

0 引言

隨著智能船舶技術的蓬勃興起,船舶路徑規劃作為重要支撐技術,在商船遠洋運輸航行、海洋區域探測搜索等場景中均有重要意義[1-3]。船舶路徑規劃是指在海洋環境中設計出一條合理路徑使船舶能安全、經濟地從出發點到達目標點的規劃算法。常用的規劃算法有遺傳算法[4-5]、蟻群算法[6]、A*算法[7]和人工勢場法[8]。

人工勢場法因其計算量小、實時性高等特點成為路徑規劃領域有望用于實際場景的避碰規劃算法之一,在船舶路徑規劃方面也有著諸多報道。文獻[9]～[11]通過在斥力勢場函數中引入船舶與障礙物相對距離解決目標不可達問題,其中張琪等[9]通過引入逃逸力的方法來幫助船舶逃離局部最小區域。陳超等[12]針對無人艇在勢場中運行可能遇到的二力平衡造成的局部最小值問題給引力加入振蕩函數F(x)協助避免二力平衡。馬小軒等[13]針對水下復雜環境進行了基于改進人工勢場法的水下自主航行器的路徑規劃。Yu 等[14]采用全局和局部路徑規劃切換方式,先用全局規劃算法設計跟蹤路徑,再在局部復雜障礙環境中用人工勢場法進行局部路徑規劃,完成復雜環境的路徑規劃任務。然而,現有研究在人工勢場法斥力模型的作用距離設定仍有顯著的局限性,固定的障礙物斥力勢場范圍使得避碰時機在整個路徑規劃中全程固化,過大障礙物斥力勢場范圍導致出現不必要的碰撞威脅以及路程的額外耗費;過小的障礙物斥力勢場范圍則會導致航行安全難以保證。在實際航海中,針對不同航行任務或某一航行任務的不同路段,有經驗的駕駛員也會精心考量避碰時機,動態調整避讓距離,以實現既安全又經濟的航行。因此,根據航行狀況和任務要求,動態調整避碰時機和避讓距離將是少人化智能船舶實現安全經濟航行任務的重要能力。實現像有經驗駕駛員一樣的優化避碰考量是避碰路徑規劃算法面臨的重大挑戰之一。

文中針對傳統人工勢場法在避碰時機、避讓距離方面的局限性,提出一種基于船舶動態安全領域的改進人工勢場法優化船舶的避碰路徑規劃。通過構建動態的船舶避讓領域,實現船舶避讓距離的自適應,從而實現更具針對性的航行危險感知。并考慮在復雜的真實海圖場景中對大型障礙物避碰面臨的局部最小值和路徑抖動問題,提出改進的子目標點設立方法,完成復雜海圖場景的船舶避碰路徑規劃。通過模擬海圖場景中無人艇的仿真試驗,驗證了算法合理性。

1 傳統人工勢場法

人工勢場法是最早由Khatib[15]提出的一種虛擬勢場法,該方法將整個環境設想為抽象的勢場空間。在研究船舶避碰路徑規劃過程中,把船舶假設在由目標點產生的引力勢場和障礙物產生的斥力勢場組成的環境中,如圖1 所示,整個環境就是由障礙物和目標點分別引起的勢場相互疊加,并共同影響的一個虛擬勢場。船舶在目標點引力和障礙物斥力的共同作用下向目標點行進,從而完成船舶的避碰路徑規劃。

圖1 人工勢場避碰路徑規劃示意圖Fig.1 Artificial potential field collision avoidance path planning

勢場力定義為勢場的負梯度,勢能下降最快的方向,通過對引力勢場函數求導得出引力,對斥力勢場函數求導得出斥力。將船舶簡化為質點,其實時位置p(x,y),則引力勢場函數為

式中:k1為引力勢場系數;d(p,pg)為船舶和目標點的距離;n1為引力勢場因子;pg為目標點位置。

引力Fatt的方向為勢能下降最快的方向,且引力大小為

斥力勢場函數為

式中:k2表示斥力勢場系數;n2為斥力勢場因子;d(p,po)表示船舶和障礙物的距離;po為障礙物位置;d0為障礙物斥力勢場范圍。

船舶在運行到障礙物斥力勢場范圍以內才會受到斥力影響,否則斥力為0。則斥力為

合力由受到的引力和斥力矢量疊加求得

2 改進人工勢場避碰路徑規劃

2.1 人工勢場法固定避讓距離的改進

設定航行全程固定的障礙物斥力勢場范圍對航行過程中不同避碰場景的適配性欠佳,會出現不必要的轉彎,或沒有及時采取避碰措施而導致距離障礙物過近,進而影響航行安全。受四元安全領域理論啟發,引入動態領域,改進斥力勢場函數。構造與速度有關的動態船舶避讓領域,當檢測到障礙物進入避讓領域后,根據與障礙物的距離計算所受斥力,避讓領域以外船舶不受斥力勢場影響,實現船舶與障礙物的避讓距離在航行過程中的自適應調整。

Wang[16]提出以前后左右4 個半徑Rfore、Raft、Rport和Rstarb為四元的安全領域,即

四元安全領域模型為

式中:k取1,2,3,···,N,k的取值不同導致領域形狀不同;L為本船船長;kAD和kDT為船舶進距和旋回初徑的系數,其不僅可以通過進距AD和旋回初徑DT的數據計算求取,還可以利用船速估算得出

式中,v為本船船速。

基于四元安全領域的啟發,進行相應簡化。前方是避碰時最需要考慮的方向,前半徑Rfore也是4 個方向半徑中最大的,利用Rfore可構建1 個足夠安全的自適應一元圓形船舶安全領域。安全領域是保證航行安全不被侵入的區域,為避免或減少對安全領域的入侵,應有一個范圍更大的避讓領域,當障礙物進入到避讓領域(即船舶與障礙物距離小于避讓距離)時可認為存在碰撞危險,船舶應采取相應避碰措施來減少或者避免對安全領域的入侵。障礙物斥力勢場范圍d0實際上為避讓距離,將安全領域半徑放大1 倍作為避讓領域半徑,在利用動態避讓領域改進后避讓領域半徑d0=2×Rfore。為避免或減少對安全領域的入侵,當船舶與障礙物距離小于避讓距離時,障礙物會有斥力產生,引導船舶在路徑規劃時進行避碰。安全領域與避讓領域如圖2 所示。

圖2 船舶安全領域與避讓領域Fig.2 Ship safety domain and avoidance domain

構建的一元船舶安全領域邊界為

對應的船舶動態避讓領域邊界為

當障礙物進入到外層邊界范圍后,船舶會受到障礙物的斥力影響,此時,d0不再是一個定值,而是會依據船舶速度在航行中的變化進行自適應調整。簡化避讓距離的設定調試過程,優化了對航行狀況變化的適應性,有助于改善在固定障礙物斥力勢場范圍下較低速度時不必要的障礙物斥力影響、不必要的路徑抖動以及較高速度時距障礙物過近等問題。

2.2 局部最小值和路徑抖動的改進

針對傳統人工勢場算法在面對有大型障礙物的復雜場景時易出現局部最小值和路徑抖動問題(如圖3 所示),通過設立子目標點的方法進行優化。主要流程是檢測是否需要設立子目標點,搜索設立子目標點的方向,確定設定半徑并設立子目標點,最后通過設立的子目標點引導無人船逃離受限區域來改善上述航行規劃問題,完成逃離后則向目標點行進繼續規劃路徑。

圖3 局部最小值和路徑抖動問題Fig.3 Local minimum and path jitter problem

檢測是否陷入局部最小或路徑抖動問題,采取以下判定式[17]

式(13)表示合力為零。式(14)表示位移小于一段時間內移動的距離,其中 |Pa-Pb|表示a,b點的直線長度,也就是從其中一點到另一點的位移;Sab表示兩點之間行進的距離;α為0～1 的值。當滿足式(13)和(14)其中之一時,可認為陷入了局部最小或路徑抖動問題。

分析α的取值可知,較小的取值可更精確地檢測是否陷入局部最小值問題,但α值越小需要越長的判斷時間,改善路徑抖動的效果也會很差。α值越大,可越快判斷是否陷入局部最小值或路徑是否出現抖動,但較大的取值會導致到達子目標點以后由于切換目標點引起的轉彎等現象也可能被式(14)判斷為真,這樣會向設立的多余的子目標點駛去導致偏航,或根據算法無法正常規劃子目標點。所以,為獲得更快更合理的判斷局部最小值的方法和面對大型障礙物時實現路徑波動問題優化,在式(13)和式(14)(α取較大值)組成的判定式后加上一道判斷程序,排除不是由于障礙物引起的路徑抖動問題。當滿足任意式(13)或式(14)后判斷在船舶避讓領域內是否有障礙物存在,船舶距障礙物的最近距離為

上式如果結果為真,表示雖然滿足了式(13)與式(14),但這并不是由障礙物引起的路徑偏轉或波動,故不規劃新的子目標點;如果結果為假,則繼續規劃子目標點的位置。

圖4為確定子目標點位置的示意圖,首先確定設立子目標點的方向,從引力方向開始向兩側不斷地掃描,初始探測方向即為引力方向。探測距離為l,若探測到障礙物就向兩側各偏轉Δθ角直到一側探測不到障礙物,則此方向為首次探測到沒有障礙物的方向θnew。圖中:rmin為θnew角度搜索線上從無人船到障礙物的最近距離;θa為調整角。

圖4 子目標點規劃示意圖Fig.4 Planning sub-target points

在θnew方向上設立子目標點雖不會設立到障礙物上,但可能造成子目標點距障礙物過近,故需增加調整角θa,即

式中:c為常量;rmin也可用作設定子目標點的距離半徑,在θ方向上rmin遠處設立子目標點,以實現θa隨設定半徑大小的變化。通過rmin和θ實現了設定半徑的自適應,并保證子目標點距障礙物有適當的距離。改進后算法流程如圖5 所示。

圖5 改進后算法流程圖Fig.5 Flow chart of improved algorithm

3 仿真試驗

3.1 試驗系統

文中的仿真試驗系統如圖6 所示。

圖6 仿真試驗系統示意圖Fig.6 Simulation test system

1) 船舶模型參數見表1[18]。

表1 船舶模型參數Table 1 Ship model parameters

2) 感知系統主要用來獲取自身狀態和周圍環境信息,輸送給導航系統障礙物信息。

3) 制導系統根據周圍環境指導如何進行下一步航行規劃。

4) 底層控制器將輸入偏差轉化為控制力和力矩輸出至船舶模型,主要由分別用于速度和航向的2 個控制器組成,且

式中:Kp,u,Kp,ψ分別為速度控制和航向控制的比例增益;Td,ψ為微分系數,其取值分別為1.0、5.0 和3.2;其余參數為船舶模型相關參數[18]。

3.2 試驗結果

3.2.1 傳統人工勢場避碰路徑規劃

針對傳統人工勢場原理,設參數n1=2,n2=2;k1=10、k2=1×1010;d0=100 m,在適當的仿真環境下,制定1 000 m×1 000 m 的地圖,起點在(0,0)位置,終點(1 000,1 000)位置,障礙物由黑色橢圓表示。

如圖7 所示,采用傳統人工勢場法在簡化的設定避碰環境中進行避碰路徑規劃。紅色線條為船舶船位組成的軌跡,可以看出在經過障礙物附近時會出現一些偏轉,躲避障礙物,完成避碰路徑規劃任務。

圖7 傳統人工勢場避碰路徑仿真結果Fig.7 Simulation results of collision avoidance path of traditional artificial potential field

3.2.2 動態領域改進的人工勢場避碰路徑規劃

為檢驗基于動態領域改進的人工勢場法的合理性,分別采用不同的控制速度進行試驗,仿真環境方面,在(100,150)、(400,450)、(350,150)、(650,500)點設立4 個大小不一的橢圓障礙物,起始點和目標點仍分別位于(0,0)、(1 000,1 000),如圖8 所示。從圖中可見,速度分別為1、5、10、15 m/s 時路徑規劃和避碰狀況存在明顯差異。在虛線轉彎處,速度越大,船舶越遠離障礙物,避碰路徑規劃越安全。

圖8 基于動態領域改進的人工勢場法仿真結果Fig.8 Simulation results of improved artificial potential field method based on dynamic domain

圖9為速度在1、5、10、15 m/s 時固定d0(d0=30、100、200 m)和改進后動態d0的避碰試驗路徑對比。圖中也展示出動態d0時船舶安全領域和避讓領域。圖9(a)中,當速度為1 m/s 時,只有d0=30 m和動態d0的仿真試驗在全程安全航行的前提下減少或減弱了避讓、偏轉行為和路徑抖動。然而,在控制速度分別提高至5、10、15 m/s 的試驗中,過小的d0(d0=30 m)則出現明顯的距離障礙物過近的現象,難以保證足夠的安全。

圖9 不同速度下避碰試驗路徑對比Fig.9 Comparison of collision avoidance paths at different speeds

圖10 展示了圖9 中各航次試驗的總航程數據,可知改進后算法構造的動態避讓距離d0在航行速度較低時保留了小避讓距離的優勢,減少或減弱避讓、偏轉行為和路徑抖動,使行駛路徑更加平緩,減少了航程消耗。在速度為1 m/s 時,動態d0和d0=30 m 的總航程耗費明顯少于d0=100 m 和d0=200 m 時。動態d0相對于d0=100 m 和d0=200 m時分別節省航程8%和9%。結合圖8 可見改進后的動態d0能夠很好地完成各速度工況下避碰路徑規劃任務試驗,適應不同的航行狀況。

圖10 不同速度下避碰試驗航程比較Fig.10 Comparison of collision avoidance voyages at different speeds

基于四元安全領域簡化的船舶安全領域和船舶避讓領域,考慮船舶速度實時計算避讓距離,調整避碰時機。在高速時保證足夠的避讓距離減小碰撞危險;在低速時通過較小的避讓距離篩掉不必要的碰撞危險,在安全航行前提下減少避讓次數和因避讓造成的航程浪費。

3.2.3 真實海圖場景的改進人工勢場避碰路徑規劃

進一步在真實海圖場景中驗證改進后人工勢場避碰路徑規劃的局部避障能力。截取一段真實海圖上的復雜海陸環境作為避碰檢驗場景,范圍為2 800 m×6 360 m,對地圖進行二值化處理,生成二值化圖像,用于構建仿真場景。針對更大范圍的試驗環境,設置k1=10,k2=1×1016,速度為10 m/s(19.44 kn)。

圖11為動態領域勢場法在真實海圖的仿真試驗結果。圖12為采用子目標法后的試驗結果,其中α=0.3,探索可設立子目標點的位置時,每次的偏轉角度為3°。如圖11 所示,在真實航海環境下,存在著各種島嶼等大型障礙物,改進動態領域勢場法盡管能完成路徑規劃任務,但在靠近大型島嶼障礙物時仍有明顯路徑徘徊和抖動,最終軌跡十分復雜。相比之下,如圖12 所示,根據2.2 節改進子目標點設立方法可有效解決陷入受限區域后的航行指引,減少航行軌跡的徘徊和抖動。

圖11 真實海圖環境船舶避碰路徑規劃仿真結果Fig.11 Simulation results of ship collision avoidance path planning in real chart environment

圖12 采取子目標點法的真實海圖環境仿真結果Fig.12 Simulation results of real chart environment with sub-target point method

為了更加優化逃脫局部最小值問題和軌跡抖動問題,設定α=0.8,在判定式(13)和式(14)之后采用式(15),濾掉不是由障礙物引起的路徑抖動,再次進行同參數設定的仿真試驗,由圖13 可見改進后人工勢場避碰路徑規劃算法可更簡潔快速地改善局部最小值問題,在面對大型障礙物時出現的路徑抖動得以優化。

圖13 優化后的真實海圖環境仿真結果Fig.13 Real chart environment simulation results after optimization

4 結論

針對傳統人工勢場避碰路徑規劃面臨的避讓距離固化、避碰易陷入局部最小值等問題,提出一種基于改進人工勢場的船舶避碰路徑規劃方法,通過考慮航速動態調整避讓距離,實現更加符合航海實踐的運動規劃,解決復雜海圖場景中避碰路徑規劃。主要結論如下:

1) 考慮到避讓距離d0對避障場景的適應需求,基于四元安全領域構造一元船舶安全領域和動態避讓領域,通過避讓領域動態計算受障礙物影響的避讓距離,實現不同航行狀況的避碰時機自適應調整;

2) 針對大型障礙物存在的真實海圖場景下傳統人工勢場法避碰規劃易出現的局部最小值和路徑抖動問題,提出改進的子目標點規劃方法,通過設定子目標點實現逃離局部路徑規劃困難區域。

文中尚未考慮動態環境避障,后續會深入拓展研究動態領域勢場法的移動目標避碰,并進一步優化改進后無人船面對大型障礙物進行路徑規劃時所存在的些許軌跡抖動問題。