基于人工勢場法引導的Bi-RRT的水面無人艇路徑規劃算法

張一帆， 史國友， 徐家晨

(大連海事大學 a. 航海學院； b. 遼寧省航海安全保障重點實驗室， 遼寧 大連 116026)

0 引 言

水面無人艇(unmanned surface vehicle, USV)是一種具有自主規劃和航行能力的小型水面船舶，被廣泛應用于海上搜救、巡邏、資源勘探等領域[1]。其核心系統包括制導系統、導航系統和控制系統，其中制導系統為USV的關鍵技術，它要求在一定的約束條件下，按照經濟性、安全性原則自動規劃出一條從起始點到目標點的無障礙路徑，包括環境信息已知的全局路徑規劃和環境信息未知的局部路徑規劃以及路徑適時調整。目前適用于USV的全局路徑規劃算法可分為4類：基于圖形學、基于仿生學、基于勢場引導和基于隨機采樣的路徑規劃算法。基于圖形學的路徑規劃算法主要包括Dijkstra算法[2]和A*算法[3]：Dijkstra算法原理簡單，但占用內存大，計算效率低，一般適用于小規模路徑規劃問題；A*算法運行速度比Dijkstra算法的快，但依賴啟發函數，計算量巨大。莊佳園等[4]采用基于距離尋優的Dijkstra算法，有效解決了Dijkstra算法占用內存大的問題，減少了規劃時間；WANG等[5]提出將A*算法與動態窗口法相結合的USV混合路徑規劃方案，提高了路徑規劃效率。基于仿生學的路徑規劃算法包括蟻群算法[6]、遺傳算法[7]等，其中，蟻群算法易發生早熟和停滯現象，遺傳算法收斂速度慢且局部搜索能力差。以上基于智能仿生學的路徑規劃算法雖然克服了傳統路徑規劃算法的部分缺點，但仍存在易陷入局部最優和參數設置過多等問題。曾明如等[8]提出多步長蟻群算法，不局限于周圍柵格，可以搜索出更短的路徑，但不適用于大規模地圖；LONG等[9]提出自適應交叉概率和變異概率公式，加快遺傳算法的收斂速度，規劃出更短的路徑。基于勢場引導的人工勢場法(artificial potential field, APF)[10]在路徑規劃中應用廣泛，通過對障礙物和目標點建立勢場函數，使USV在斥力和引力作用下向目標點靠近，算法實時性高、反應迅速且計算簡單，但存在易陷入局部極值和目標點不可達等問題。薛學華等[11]增加艏向角約束解決船舶狹窄通道抖振問題，并提出自適應參數調整回溯法脫離局部極小值，與可視圖法相結合解決目標點不可達問題，但在解決局部極小值問題時未考慮航向約束，生成的航線較曲折，且依賴于算法參數的設置。基于隨機采樣的路徑規劃算法如快速擴展隨機樹(rapidly-exploring random trees, RRT)[12]算法，在搜索空間內隨機采樣，繞過障礙物到達目標點，系統復雜度小，無須環境建模，適合解決高維空間和復雜約束下的路徑規劃問題，但該算法在采樣時具有較大的隨機性和盲目性，導致路徑搜索效率較低，路徑平滑性較差，路徑質量較低。李躍芳等[13]利用激光雷達感知系統獲取水域信息構建環境模型，用RRT算法搜索路徑，通過動態窗口策略進行局部規劃，可在復雜水域內快速高效地生成可解路徑，但生成的路徑質量較低，存在轉向角太大和接觸障礙物的問題。USV體積小、速度快，需要為其規劃高質量路徑，保證其安全到達目標點并完成相應的水上任務，然而基于圖形學和仿生學的算法需要依靠精確的環境建模才能規劃出安全可靠的航線，但海上環境信息有限且易受干擾，無法保證規劃的安全性和實時性，因此基于隨機采樣的RRT算法是一個很好的選擇。

經典RRT算法具有空間搜索能力強、避障性能優、實時性強等特點，但由于其強大的空間搜索能力，在全局空間內盲目擴展出過多的無效節點，導致最終生成的路徑質量較差，且在復雜的規劃空間內其規劃時間也大大增加。為了解決以上經典RRT算法存在的問題，研究人員提出了很多改進方法，大致可以分為兩大類[14]：一類是對經典RRT 算法在擴展方向、步長選擇等方面進行改進，例如基于目標導向的RRT算法[15]、雙向RRT (bidirectional RRT, Bi-RRT)算法[16]、動態步長RRT算法[17]等；另一類是與其他路徑規劃算法融合的改進，例如結合A*算法[18]。相較于經典RRT算法，改進后的算法在保證強搜索能力的同時一定程度上降低了節點擴展的隨機性，提高了路徑生成效率，但依舊存在轉向角過大、路徑太長、路徑不平滑等問題，難以適應海上航行的要求。本文針對以上基于RRT算法的路徑規劃中仍未解決的問題，改進Bi-RRT算法：①改進Bi-RRT算法的隨機點采樣機制，使隨機點有一定概率落在目標區域，進而降低算法的隨機性；②在新節點生成時引入目標引力場和障礙物斥力場，使兩棵隨機樹在隨機點和合力的共同作用下彼此交替生長，既可保證算法概率完備性又可避免陷入局部極小值；③為解決目標點不可達問題，修改斥力場函數，引入本船與目標點的相對距離調節因子，調節斥力方向，使新節點在遠離障礙物的同時更偏向于目標點；④根據USV的轉向性能要求，增加轉向角約束，使規劃出的路徑更加符合實際USV操縱情況。

1 Bi-RRT算法

1998 年LAVALLE首次提出經典RRT算法。該算法是一種基于增量采樣的路徑規劃算法，它以起始點為根節點，在自由空間內隨機采樣生成隨機點，根據隨機點的方向和步長確定新節點坐標，引導擴展樹向全局空間內未覆蓋的區域擴展，最終找到一條通向目標點的路徑。然而，RRT算法采樣的完全隨機性，使其盲目搜索擴展，效率很低。因此，在2000年KUFFNER, Jr.和LAVALLE提出Bi-RRT算法[19]。Bi-RRT算法分別以起始點和目標點為根節點，構造兩棵擴展樹并依次向對方擴展，當滿足相遇條件時，從連接點進行回溯即可獲得一條連接起始點與目標點的避障路徑。改進后的算法較經典RRT算法搜索目的性更強，搜索效率更高。

Bi-RRT算法構建過程如圖1所示。設二維狀態空間為D，其中不可通航區為障礙區Sobs，可通航區為自由區Sfree。從起始點Xs和目標點Xg生成兩棵擴展樹,分別記為Ts和Tg。在自由區均勻、隨機選取一個狀態節點Xrand，在擴展樹Ts中搜索與節點Xrand最近的節點Xnearest并進行基礎步長為ξ的擴展，得到新節點Xnew：

(1)

圖1 Bi-RRT算法構建過程

圖2 Bi-RRT算法規劃效果

雖然Bi-RRT算法在運行時間和搜索效率上優于經典RRT算法，但仍存在路徑不平滑、轉向角過大、穩定性差等問題，甚至出現觸碰障礙物邊緣的情況(見圖2)，無法一次規劃出最優或次優路徑，需要結合其他相關路徑平滑算法進行優化。

2 算法改進

2.1 隨機點采樣機制改進

針對Bi-RRT算法在全局空間內產生隨機點導致其在復雜環境中規劃效率低的問題，本文提出雙向目標偏置采樣的方法。設置偏置概率為λ，在[0, 1]內生成隨機數P。若P<λ，則將采樣點設置為對方擴展樹距離當前節點最近的節點Xnearest，使兩棵擴展樹均以一定概率向雙方最近節點擴展；若P≥λ，則在全局空間自由區內隨機產生采樣點Xfree，保證算法的概率完備性。

(2)

2.2 節點擴展方式改進

在APF引導的Bi-RRT算法(簡寫為APF-Bi-RRT算法)中，每次節點的擴展不僅由隨機采樣點Xrand相對Xnearest的方向和步長ξ決定，還由USV在Xnearest處受到的合力決定，新節點生成公式如下：

(3)

式中：α為合力系數；Ftotal為USV在Xnearest處受到的最近障礙物斥力與目標點引力的合力。傳統APF引力場函數為

(4)

式中：katt為引力增益系數；ρ(X,Xg)為本船X與目標點Xg的相對距離。對Uatt(X)進行負梯度求導可得到引力Fatt，其方向由本船指向目標點。由式(5)可知，USV與目標點距離越遠，受到的引力越大。

Fatt=-?Uatt(X)=-kattρ(X,Xg)

(5)

USV在遠離障礙物時幾乎不受到斥力作用，而是受到目標點引力作用向目標區域擴展；USV在進入障礙物影響區域時會受與障礙物的距離相關的斥力作用，從而遠離障礙物。然而，傳統的APF存在目標點不可達問題，例如在目標點附近存在障礙物的情況下，USV到達目標點附近時會受到較小的目標點引力和較大的障礙物斥力，在兩者合力作用下，USV最終可能會向著遠離目標點的方向運動，即使有隨機點的影響，也需不斷重復擴展以重新規劃繞航路線，導致時間成本增加。為解決此問題，只需將目標點始終保持為全局勢能最低點，為此對斥力場函數進行修改，引入USV與目標點的相對距離ρ(X,Xg)和相對距離調節因子m。修改后的斥力不僅與USV與障礙物間的距離有關，還與USV與目標點間的距離有關，這樣USV在運動到目標區域時，由于受到與目標點的相對距離的影響，目標點依舊為全局勢能最低點，有效解決了目標點不可達問題。根據本文需求，新建立的斥力場函數表達式如下：

(6)

式中：krep為斥力增益系數；ρ(X,Xobs)為本船與障礙物的相對距離；ρ0為障礙物的最大影響半徑。

同理，對斥力場函數進行負梯度求導，可得到斥力場函數的兩個分力Frep1和Frep2，其大小分別為

(7)

USV在障礙物影響區域受到的新的障礙物斥力由兩個方向的斥力合成，其中，Frep1為障礙物指向USV的斥力，Frep2為USV指向目標點的斥力。USV航行到障礙物影響區域時會受到總斥力與引力的合力Ftotal的影響：

Ftotal=Fatt+Frep1+Frep2

(8)

APF-Bi-RRT算法的構建過程如圖3所示：首先擴展第一棵樹Ts，改進后的斥力與引力的合力Ftotal繼續與隨機方向上的步長ξ進行合成，得到新節點Xnew，再切換至第二棵擴展樹Tg進行擴展；最終兩棵擴展樹在目標點引力和障礙物斥力的共同作用下向彼此交替擴展，直到滿足相遇條件，算法終止。

圖3 APF-Bi-RRT算法構建過程

為驗證改進斥力場函數是否能有效解決目標點不可達問題，設計仿真實驗進行驗證。生成10.0 m×10.0 m的地圖，設置9個障礙物并將最后一個障礙物設置在目標點附近。其他設置如下：起始點坐標為(0, 0)，目標點坐標為(9.5 m, 10.0 m)，引力增益系數katt為1，斥力增益系數krep為10，相對距離調節因子m為2。改進前后的APF實驗對比如圖4所示：傳統的APF無法到達目標點，而在引入本船與目標點的相對距離及其調節因子后，算法如期到達預設的目標點，且穩定性相對較好，沒有出現振蕩現象。

圖4 改進前后的APF實驗對比

2.3 轉向角約束

USV在海上航行時，為保證舵角的有效性，通常將其控制在極限舵角范圍內。因此，為更符合海上實際操縱情況，將最大轉向角設置為極限舵角δ(δ=40°)，使新生成的軌跡不出現大角度轉向。

以擴展樹Ts的延伸過程(見圖5)為例，當新節點通過碰撞檢測后，對其施加轉向角約束。計算向量XnearestXnew與向量XparentXnearest的夾角θ：若θ>δ，則表明新節點Xnew不滿足轉向角約束，放棄Xnew，擴展樹Ts延伸失敗，進入下一次搜索；若θ≤δ，則延伸成功，將Xnew加入Ts中。擴展樹Tg的搜索原理與Ts的一致。

圖5 轉向角約束示意圖

2.4 算法流程

APF-Bi-RRT算法流程如圖6所示：分別在起始點Xs和目標點Xg初始化擴展樹Ts和Tg；根據改進的隨機點采樣機制，產生隨機點Xrand；遍歷擴展樹Ts中與Xrand最近的節點Xnearest，確定Xnearest與最近障礙物的距離ρ(X,Xobs)；以擴展樹Tg中新節點為目標點，計算其與目標點的距離ρ(X,Xg)；分別計算Xnearest與障礙物和目標點的夾角，根據式(3)計算新節點Xnew；判斷新節點Xnew與Xnearest之間是否有障礙物以及是否滿足轉向角約束，若均滿足則將新節點加入擴展樹Ts中；最后判斷兩棵樹是否滿足相遇條件，若不滿足則繼續另一棵擴展樹Tg的擴展(原理與第一棵樹的相同)，若滿足要求則從連接點進行回溯得到USV的全局路徑。

圖6 APF-Bi-RRT算法流程

3 實驗結果與分析

為驗證APF-Bi-RRT算法的有效性，利用MATLAB 2019 (Windows 10, Intel(R) Core(TM) i7-10710U CPU@1.10 GHz 1.61 GHz, 內存16 GB)，分別進行簡單、復雜和特殊3種水域環境下的實驗，生成1 000 m×1 000 m的地圖。起點坐標為(50 m, 50 m)，終點坐標為(900 m, 950 m)，基本步長ξ為15 m，引力增益系數katt為0.2，斥力增益系數krep為1 000，障礙物的最大影響半徑ρ0為30 m，相對距離調節因子m為2。比較APF-Bi-RRT算法、Bi-RRT算法、APF和A*算法的路徑規劃效果，其中：A*算法用于獲取最佳路徑以考察改進算法的規劃性能，為提高A*算法的運行速度，將地圖分辨率降低為原來的1/25，在縮短運行時間的同時保證生成路徑的質量；其他算法使用相對相似的參數在相同的實驗條件下進行仿真實驗。采用規劃時間、路徑長度、最大轉向角和節點數量作為指標對算法進行評價。每組進行20次尋優實驗，統計每個算法指標的平均值。

簡單水域環境實驗。設置3個障礙物(分別用不同半徑的黑色圓形表示)。4種算法的最終路徑規劃效果見圖7，指標統計結果見表1。APF-Bi-RRT算法的路徑規劃過程：若節點在障礙物影響范圍外，則較大的引力使得兩棵擴展樹更快地向彼此擴展，避免在全局空間內采樣搜索，提高了路徑生成效率；若節點在障礙物影響范圍內，則受到的障礙物斥力使其遠離，避免發生觸碰或貼合障礙物的情況。相比于其他3種算法規劃出的路徑：APF-Bi-RRT算法因為引入了轉向角約束，所以在路徑長度、路徑平滑度和節點數量上有了很大的改進；Bi-RRT算法由于在全局空間內搜索擴展，所以規劃效率不高，且生成的路徑曲折度較大，路徑太長；APF在路徑規劃中由于缺乏隨機點引導，無法快速脫離障礙物，導致運行時間增加；A*算法規劃出的路徑避障效果最佳，但存在繞行且規劃時間較長等問題。APF-Bi-RRT算法表現較好：其在路徑長度和規劃時間上均優于A*算法；相較于Bi-RRT算法，其節點數量減少了50%左右，路徑長度縮短了18%，規劃時間減少了42%。APF在路徑長度和規劃時間上均不如其他3種算法。

a) APF-Bi-RRT算法

b) Bi-RRT算法

c) APF

d) A*算法

表1 簡單水域環境下4種算法的指標統計結果

復雜水域環境實驗。設置40個障礙物，且存在障礙物部分重疊的情況。4種算法的最終路徑規劃效果見圖8，指標統計結果見表2。APF-Bi-RRT算法相較于Bi-RRT算法，在相同的時間內可以規劃出更平滑、節點更少的路徑，保證USV在縮短航程的同時可以連續小幅度轉向避讓障礙物，更加符合航海操縱需求；傳統APF在復雜水域環境中極易陷入局部極小值且依賴于參數設置，導致運行時間較長，無法得到最優路徑；A*算法依舊存在路徑轉向角大、規劃時間長等問題。APF-Bi-RRT算法表現較好：與Bi-RRT算法相比，該算法幾乎在相同的規劃時間內將節點數量減少63%，將路徑長度縮短20%左右；與APF相比，該算法在規劃時間和路徑長度上的表現更優；與A*算法規劃出的最短路徑相比，該算法得出的路徑長度縮短了10%左右。

a) APF-Bi-RRT算法

b) Bi-RRT算法

c) APF

d) A*算法

表2 復雜水域環境下4種算法的指標統計結果

特殊水域環境實驗。設置3個障礙物，其中1個為大障礙物，考察APF-Bi-RRT算法在大障礙物環境下的繞行能力。4種算法的最終路徑規劃效果見圖9，指標統計結果見表3。APF-Bi-RRT算法由于斥力的作用，規劃出的路徑相較于Bi-RRT算法更平滑、轉向點更少、繞行效果更好，同時由于其增加了最大轉向角約束，避免了繞行時左右頻繁大角度轉舵，規劃的路徑長度相較于A*算法的更短； Bi-RRT算法繞行效果較差；APF在避離障礙物時耗費了大量時間，規劃的路徑較長；A*算法規劃時間較長，不符合USV的短時間作業需求。APF-Bi-RRT算法表現較好：相較于Bi-RRT算法，其節點數量減少了44%，路徑長度縮短了9%；與APF相比，其路徑長度縮短了32%；該算法在較短時間內規劃出了比A*算法更短的最優路徑。

a) APF-Bi-RRT算法

b) Bi-RRT算法

c) APF

d) A*算法

表3 特殊水域環境下4種算法的指標統計結果

4 結 論

為滿足水面無人艇(USV)高速、自主航行需求，需規劃出實時性強、避障精準性高、平滑度優、航程短的路徑。為此，改進雙向快速擴展隨機樹(Bi-RRT)算法的隨機點生成機制；引入人工勢場法(APF)引力場函數和改進斥力場函數，引導新節點的生成，明確方向性和提高避障精準性；增加轉向角約束，進一步平滑路徑，使其更加符合USV的實際操縱需求。為驗證改進算法(APF-Bi-RRT算法)的精確性和泛化能力，對APF-Bi-RRT算法、Bi-RRT算法、APF、A*算法分別在簡單、復雜、特殊3種水域環境下進行仿真實驗，其中A*算法用于規劃最佳路徑。結果顯示，與最佳路徑相比，本文提出的合力約束和轉向角約束可以更好地提高路徑質量，如提高路徑平滑度、減少轉向節點和路徑長度。

本文所設計的算法能滿足USV路徑規劃需求，但未考慮路徑規劃所涉及的其他相關因素，例如海上風、浪、流對USV操縱的影響。因此，如何在環境因素的干擾下進行USV路徑規劃將是下一步探討的問題。