基于RRT算法的無人艇航線重規劃方法

鐘富川，楊雪鋒，2*，袁志瑤

(1.重慶交通大學 航運與船舶工程學院，重慶 400074;2.交通安全應急信息技術國家工程實驗室，北京 100011)

航線規劃是無人艇的核心技術，是實現無人艇自主航行的關鍵[1-2]，目前，航線規劃方法大致可分為3類：①以歷史航跡數據為基礎，通過建立推薦航線庫，結合季風、洋流、臺風等氣象因素推薦計劃航線[3-5]；②通過建立船舶航行區域內障礙物的數學模型，利用傳統的智能算法和圖論知識進行航線規劃[6-8]；③以利用RRT(rapidly-exploring random tree)算法為代表的隨機采樣算法進行航線規劃[9-10]。RRT算法目前應用較為廣泛[11]，不需要對復雜的任務空間進行數學建模，只需對采樣點進行碰撞檢測；RRT算法工作原理簡單，且在搜索過程中可以考慮運動學約束條件，從而能夠有效地解決復雜環境下的運動規劃問題[12]。然而，由于采用隨機采樣的方式進行規劃，RRT算法又存在實時性低、規劃的路徑不是最優解等問題。許多學者提出了一系列改進的策略[13-18]。D. Ferguson等提出了一種基于反向隨機樹生長策略的實時重規劃算法DRRT(dynamic rapidly-exploring random trees)，將規劃終點作為隨機樹生長的起始點，分割去除在路徑受阻時與障礙物發生碰撞的隨機樹部分，并在此基礎上重新生長以獲取重規劃路徑[19]。利用該思想，重規劃只分刪除部分受障礙物影響的隨機樹節點，保留大部分未受影響的節點，在一定程度上保留并利用初始航線的規劃成果，可縮短規劃時間。

可見，利用分割隨機樹的思想進行重規劃的方法是縮短耗時的有效手段，然而，新增障礙物的位置對重規劃效率方面的影響研究尚未見報道，而且大部分研究未將受障礙物影響失效的隨機樹節點與邊分開考慮。為此，考慮針對無人艇航行區域的特征，提出一種針對失效節點和失效邊的隨機樹分割方法，解決無人艇的航線重規劃問題，并利用貪心算法對航線規劃結果進行優化。根據廣州港附近水域、大連港附近水域和天津港附近水域構建任務空間，通過仿真實驗探討新增障礙物位置對重規劃耗時的影響，對比利用RRT算法的傳統航線重規劃方法與利用隨機樹分割的航線重規劃方法，在規劃耗時和航程方面的性能。

1 航線重規劃方法

1.1 整體流程

在無人艇執行原計劃航線任務時，當任務空間更新后，發現航行任務空間中有新增障礙物，此時需要判斷初始計劃航線是否受阻，如能安全通行則繼續執行航行任務，否者就需重新規劃航線。具體流程見圖1。

圖1 航線重規劃算法流程

1.2 具有偏置概率的RRT(BP-RRT)

BP-RRT算法進行路徑規劃主要步驟見表1。

表1 BP_RRT算法偽代碼

1)設定起始位置Uinit為隨機樹T的根節點。

2)產生一個隨機數x(0～1之間)，如果x小于偏置概率Pg，則將目標點Ugoal作為隨機采樣點Urand；否則于偏置概率Pg，將在狀態空間中隨機選擇一點作為Urand進行生長。

3)尋找隨機樹T中與Urand最近的節點Unear，并根據步長λ計算得到子節點Unew。

4)判斷Unear與Unew之間是否存在障礙物，若不存在障礙物，則將Unew作為T的子節點，添加邊[Unear,Unew]；否則，返回步驟上一步。

5)當隨機樹中存在子節點與Ugoal之間的距離小于步長，且無障礙物，便可以在隨機樹中找到一條由節點組成的從出發點到目標點的路徑。

1.3 隨機樹分割方法

借鑒DRRT算法的分割邏輯，結合無人艇的任務空間特性，當任務空間中產生新的障礙物時，初始隨機樹中部分節點(下換失效節點)或者邊將處在無人艇不可航行的障礙區域內。根據隨機樹的生成原理可知，以失效節點為父節點的所有節點，或者以失效邊連接節點為父節點的所有節點，將不可到達，這些節點均應被當作失效節點。隨機樹分割的目的就是根據新增的障礙物，刪除初始隨機樹中的失效節點和失效邊，得到一棵殘余隨機樹，為無人艇航線重規劃提供基礎條件，其過程及偽代碼見圖2和表2。

圖2 隨機樹分割過程示意

表2 隨機樹分割算法偽代碼

圖2中第一棵隨機樹表示初始隨機樹，黑色矩形表示新增障礙物。

對于一個包含有M個節點的隨機樹(T)，根據障礙物和隨機樹，利用FIND_DEAE_NODE函數確定失效節點，并將這些節點的Parent屬性設置為0。循環遍歷隨機樹中的所有節點，查找父節點Parent屬性為0的節點，并將這些節點的Parent屬性也設置為0，直到隨機樹中所有節點的Parent屬性不再改變為止。

1.4 航線平滑步驟

對于一個包含N個節點的航線(route)，假設每個點表示為Ui(i=1～N)。

1)令Ustart=U1，Ugoal=UN，連接Ustart和Ugoal。

2)通過直線連接Ustart和Ugoal，如果2個點之間有障礙物，則將Ugoal替換為前一個點，即令Ugoal=UN-1。

3)重復上一步驟，直到Ustart和Ugoal之間的直線上無障礙物為止。

4)一旦Ustart和Ugoal無障礙物，刪除Ustart與Ugoal之間所有節點，令Ustart=Ugoal，Ugoal=UN。

5)重復步驟2)～4)，直到Ustart=UN。

航線平滑方法偽代碼見表3。

表3 航線平滑處理偽代碼

2 無人艇航線重規劃仿真實驗

仿真實驗軟件平臺：Matlab R2020a。

硬件條件：Windows 7 旗艦版Intel(R)core(TM)i3CPU M370 @2.40 GHz 2.39 GHz。

2.1 可行性實驗

分別選擇大連港、廣州港和天津港附近水域的實際環境，建立黑白二值地圖(尺寸：500×500像素)作為規劃任務空間，圖中黑色區域表示非可航區域(陸地、近岸島礁、淺水區等)，白色代表可航區域,見圖3。

圖3 任務空間黑白二值地圖

以廣州港附近水域建立的航線規劃空間為例，在檢測到初始航線受阻見圖4a)，隨后獲取初始隨機樹見圖4b)，利用障礙物區域對初始隨機樹進行分割得到殘余隨機樹結果見圖4c)，利用殘余隨機樹生長見圖4d)，生成重規劃航線，并做平滑處理見圖4e)。具體參數設置如下：航線任務的起點坐標為[10,10]，終點坐標為[400, 250]，偏置概率Pg為0.1，其中圓點分別表示起點和終點位置，在原有計劃航線的中點處構建5×5(像素)大小的矩形區域模擬新增的障礙物。

建立的在廣州港、大連港及天津港附近水域任務空間中開展仿真實驗。針對不同任務空間，將算法步長分別設置為5、10、15、20 pixels，共計12個實驗項目，每個實驗項目分別進行100次重復實驗，統計重規劃耗時的平均值與標準誤差，結果見圖5。

圖5 不同步長下的重規劃耗時平均值

從圖5可以看出，利用隨機樹分割的航線重規劃方法在所構建的實際的任務空間中確實可行，航線重規劃耗時均小于2 s。此外,在(5～20)pixels步長范圍內，很明顯，規劃耗時隨步長的增加而明顯降低，且在復雜程度不同的任務空間中，規劃耗時差異明顯。

2.2 對比實驗

將利用隨機樹分割的航線重規劃方法與利用傳統RRT、BP-RRT的重規劃方法，在重規劃耗時方面進行對比。各算法的初始參數設置如下：步長λ=10 pixels；偏置概率Pg=0.1；在初始計劃航線1/2處模擬新增障礙物(5×5 pixels)阻斷計劃航線。

2.2.1 算法耗時

統計航線重規劃耗時、航程長度的平均值和標準誤差，具體見圖6、7。

由圖6可見，利用隨機樹分割的航線重規劃方法能夠顯著降低航線重規劃耗時，且明顯優于傳統RRT。相比于偏置RRT的航線重規劃方法，在廣州港任務空間中規劃耗時降低約30%，在大連港任務空間中規劃耗時降低約80%，在天津港任務空間中規劃耗時降低約71%。

整體來說，進行重規劃時，分割RRT算法的耗時少于偏置RRT算法，但在個別航線重規劃案例中，會出現傳統RRT算法優于分割RRT算法的情況。如廣州港附近水域航線重規劃時，第73組與87組實驗，傳統RRT和偏置RRT算法均優于分割RRT算法，見表4。

表4 廣州港航線重規劃耗時統計特例 s

2.2.2 重規劃航線長度

從圖7，分割隨機樹方法規劃的航程最短，相較于傳統RRT與偏置RRT方法，在廣州港任務空間，分別縮短了約22.49%、16.14%；在天津港任務空間，分別縮短了約21.36%、19.94%；在大連港任務空間，分別縮短了約18.74%、13.82%，效果明顯。

圖7 航線長度對比

3 新增障礙物位置的影響

3.1 仿真實驗

在初始隨機樹中，如果新增障礙物出現的位置不同，那么最終得到的失效節點數量也將不同，殘余隨機樹的節點個數也會有較大的差異，會對航線重規劃產生影響。調整新增障礙的位置進行仿真實驗，探討新增障礙物位置對航線重規劃的影響。以廣州港、天津港及大連港附近水域為例，設每個地圖的起始點與終點和上述實驗條件一致，偏置概率為Pg=0.1，步長λ=10 px，最小閾值mix=9 px。令初始計劃航線的長度為L，新航行障礙區域的位置分別位于距離起始點(1/6)L、(1/3)L、(1/2)L、(2/3)L、(5/6)L處。根據新增障礙物位置不同，可得到5個組實驗，共計15各項目，每組實驗項目分別進行100次重復實驗統計算法耗時，得到耗時情況見圖8。

圖8 障礙物不同位置耗時平均值對比

由圖8，在天津港的任務空間中，重規劃耗時隨著障礙物出現的位置離出發點越遠，則重規劃的耗時就越短，即殘余隨機樹節點個數與初始隨機樹節點個數之比，該比值越大，航線規劃耗時越少，符合一般設想。

然而在廣州港(1/2)L處和大連港(2/3)L處卻出現耗時突增的現象。在廣州港任務空間中由(1/3)L處到(1/2)L處，平均耗時由0.38 s突增到1.54 s增加了近75.3%；在大連港任務空間中由(1/2)L處到(2/3)L處，平均耗時由1.69 s突增到2.57 s增加了近34.2%。

3.2 算法耗時突增現象分析

一般而言，新增障礙物出現的位置離終點距離越近，則航線重規劃所需的時間越少。然而，從圖8各種航線重規劃算法耗時的統計結果來看，這一結論并不完全正確。以廣州港和大連港附近水域的航線規劃任務空間為例進行分析說明。

對廣州港附近水域而言，無人艇從起點到終點有2條通道可以選擇，分別是通道1與通道2，見圖9。當原計劃航線處于通道2時，新增障礙物可能會完全堵塞通道2。此時，如果再以殘余隨機樹為基礎進行航線規劃，大量的新增節點Unew出現在被堵塞通道內，極大可能無法通過碰撞檢測，這將導致隨機樹生長失敗的概率升高，重規劃算法耗時增加。

圖9 任務空間通道口示意

對大連港附近水域而言，無人艇從起點到終點的通道如圖9b)所示，在初始計劃航線(2/3)L處增加障礙物時，障礙物恰好位港池的入口處，堵塞通道，其效果與廣州港附近水域類似，將導致隨機采樣失敗率增加，提高算法耗時。

綜合上述分析，一般情況下新增障礙物離終點的距離越近，利用殘余隨機樹進行航線重規劃耗時就越少。但是，當新增障礙引起任務空間結構變化較大，即減少了任務空間通路或使通路變狹窄等情況時，航線重規劃耗時將顯著增加。

4 結論

1)利用分割隨機樹的方法進行無人艇航線重規劃可行，通過隨機樹分割得到殘余隨機樹，同時利用殘余隨機樹進行隨機樹生長可縮短算法重規劃耗時。

2)利用航線平滑處理方式能明顯縮短所規劃的航線長度，使航線趨于最優。

3)一般而言，新增障礙物在初始計劃航線上出現的位置，即殘余隨機樹節點個數與初始隨機樹節點個數之比，該比值越大，航線規劃耗時越少，然而當新增障礙物嚴重影響了任務空間的約束，則會導致重規劃效率急劇降低。