基于改進RRT 算法的無人艇編隊路徑規劃技術

歐陽子路，王鴻東*，黃一，楊楷文，易宏

1 上海交通大學海洋智能裝備與系統教育部重點實驗室，上海200240 2 上海交通大學海洋工程國家重點實驗室，上海200240

0 引 言

近年來，隨著世界各國對海洋安全防護、海洋資源勘探與開發以及無人軍事作戰系統的重視，水面無人艇（USV）因其自主程度高、機動性強等特點，已成為國內外研究的熱點。隨著無人艇技術的深入研究與軍民兩用的發展需求，無人艇編隊的概念應運而生。相比單艘無人艇，無人艇編隊在執行任務時具有效率高、容錯性強及覆蓋范圍廣的特點，在實際工程應用中有著重要的意義。

路徑規劃是無人艇編隊的核心技術之一，用于在保持無人艇編隊形狀穩定的同時對障礙物進行避碰。目前，國內外已有專家學者對無人運載器編隊路徑規劃問題展開研究并取得了一定的成果。Tam 等［1］提出了一種符合COLREGS 規則的多艇協同路徑規劃算法，仿真結果表明，在各種交通場景下，算法的輸出是有效且一致的。Ni等［2］提出了一種虛擬力場的算法，通過引入面積比參數，增加了模糊控制模塊來解決動態環境中的避障問題，并通過仿真實驗演示了該方法的效率。Geng等［3］采用改進的粒子群算法提高了路徑搜索的效率，在仿真實驗中驗證了所提方法在生成優化路徑時的高效性。王躍午［4］提出了一種基于可變快速行進平方法的無人艇編隊路徑規劃技術，該方法將可行區域看作各向異性，并認為某點的安全性與該點距障礙物的距離線性相關，具有實時性好的特點。包昕幼［5］在對常見的全覆蓋路徑規劃進行分析后，采用往返式覆蓋算法作為無人艇編隊的覆蓋方法，同時針對未覆蓋區域采用改進的A-star 算法，通過Matlab 仿真實驗驗證了融合算法的可行性。蘇青［6］對基于雙層模糊邏輯的多機器人路徑規劃與協調避碰系統進行了研究，利用模糊控制的魯棒性，提高了算法效率與可行性。從上述國內外研究情況來看，解決無人艇編隊路徑規劃問題的思路主要為拓展演進經典算法，提高算法的實時性與規劃效率。

受前人研究的啟發，本文將提出一種基于改進快速搜索隨機樹（rapidly exploring random tree，RRT）算法的無人艇編隊路徑規劃技術。首先，針對無人艇編隊形狀穩定問題，在RRT 算法擴展環節提出一種非嚴格保形修正向量與非嚴格保形控制圓區域，使搜索樹有朝著嚴格保形坐標點生長的趨勢；接著，針對突發障礙物與非嚴格保形規劃點碰撞的問題，在RRT 算法碰撞檢測環節提出可調節避碰圓區域與障礙物修正向量，使無人艇安全避碰并最大程度地保持隊形穩定；最后，通過仿真實驗驗證該算法在無人艇編隊路徑規劃問題中的可行性。

1 無人艇編隊路徑規劃環境模型建立

無人艇編隊在自主航行過程中通過智能感知系統實時定位并探測障礙物信息，本文所解決的無人艇編隊路徑規劃問題如圖1 所示。領航艇L以速度V 按預定航線第一象限角平分線航行，若干跟隨艇為F1，F2，…，Fn，無人艇智能感知系統探測到突發障礙物區域B1和B2。對于該種情形，本文基于RRT 算法設計一種編隊路徑規劃算法，使無人艇編隊在航行過程中保持隊形穩定，并對影響無人艇按規劃航跡點航行的突發障礙物進行自動規避，最大程度地保持編隊形狀穩定。

圖1 無人艇編隊路徑規劃環境情形Fig.1 The situation of USV formation path planning

2 無人艇編隊保形規劃技術研究

2.1 經典RRT 算法

經典RRT 算法由LaValle 于1998 年提出［7］，現有的研究表明，RRT 算法與A*算法、隨機路線圖（PRM）算法、混合整數線性規劃（MILP）算法相比，收斂速度顯著加快，具有更高的優化效能［8-11］，因此在無人機、無人車等運載器上得到了廣泛的應用［12-14］。經典RRT 算法構造方式是以給定的初始點為隨機樹根節點，根據當前環境快速有效地搜索可行域空間，通過產生隨機點，將搜索導向空白區域并添加隨機樹節點直至目標點，最后通過反向搜索得到路徑點。

2.2 “非嚴格保形修正向量”與“非嚴格保形控制圓”設計

經典RRT 算法中延伸子節點的構造方式為在任務空間C 內隨機選擇一個隨機目標點；從隨機樹當前所有的葉節點中選出一個離該隨機目標點最近的節點，稱之為臨近節點；然后從臨近節點的方向延伸一個步長為S 的距離，從而得到該延伸子節點。該種構造方式適用于單智能體在位姿空間的全局路徑規劃情形，但由于無人艇編隊航行有著隊形保持穩定的需求，因此經典算法中延伸子節點不可控的構造方式不適用于無人艇編隊全局保形規劃技術，需要對其進行改進。針對此問題，本文提出一種非嚴格控制圓，以實現無人艇編隊的非嚴格保形的全局路徑規劃，同時引進非嚴格保形修正向量提高保形精度，加速算法收斂。

設某時刻t 跟隨艇智能感知系統探測到領航艇航行到位置點PL（xt，yt），各跟隨艇的實時位置為PW（xW，yW），將PL代入編隊位置函數Xs解算得到該時刻各跟隨艇嚴格保形坐標點PF（xF，yF），此時，本文提出的無人艇編隊路徑規劃RRT 算法將開始起作用，各跟隨艇以自身實時位置為根節點，并行構造搜索樹，實現無人艇編隊的非嚴格保形路徑規劃。由于各跟隨艇搜索樹構造原理相同，本文為了敘述方便，以某單艘跟隨艇進行算法闡述。

由經典RRT 算法的EXTEND 步驟［7］，該時刻跟隨艇延伸子節點Pn（xn，yn）的計算公式為：

式中：xr，yr為該迭代過程中隨機樹生成的隨機目標點Pr的橫縱坐標；S 為無人艇單步探索步長。采用向量表示為

式中：PW，n為點PW到點Pn的位移；PW，r為點PW到點Pr的位移。由于經典算法中延伸子節點Pn依賴于隨機點Pr的生成而構造，為了實現Pn能夠以更大概率作為該跟隨艇保形航路點，首先引進非嚴格保形修正向量AR對Pn進行坐標修正。AR的定義如式（3）所示。在AR作用下，延伸子節點Pn及其坐標不再滿足式（1）和式（2），而由向量表達式（5）及式（6）給出：

式中：Pn，F為點Pn到點PF的位移；PW，r為點PW到點Pr的位移；PW，n′為點PW到點的位移；PF為編隊位置函數解算得到的該時刻下的跟隨艇嚴格保形坐標點；d 為點Pn與該時刻下的跟隨艇嚴格保形坐標點PF（XF，YF）之間的歐氏距離；ω為保形修正系數；U為計算過程中的過渡向量。

式（3）表明，完成經典RRT 算法下的延伸后，在原始延伸的基礎上添加由Pn到PF連線方向上ω作用的AR，讓搜索樹有朝著嚴格保形坐標點生長的趨勢。式（4）為Sigmoid 函數的數學表達式，該函數圖像如圖2 所示。

圖2 Sigmoid 函數圖像Fig.2 Graph of Sigmoid function

由圖2 可知，sigmoid 函數圖像位于x 軸正半軸部分，具有如下特性：函數值會隨著輸入d 的增大而增大，但增大的幅度會逐漸減小，最后趨近于1。表現在跟隨艇保形上則是當原始延伸子節點距離該時刻嚴格保形點越大時，AR作用越強；但是作用的幅度不會使向嚴格保形點趨近的向量的模超過單步探索步長而帶來過度修正的問題。該過程幾何上如圖3 所示。

圖3 非嚴格保形修正向量作用示意Fig.3 The action of non-strict conformal correction vector

式中，k為保形精度權值，可根據無人艇編隊實際任務下的精度需求賦值。

圖4 嚴格保形控制圓作用示意Fig.4 The action of non-strict conformal control circular area

3 無人艇編隊局部自主避碰技術

無人艇編隊在智能航行過程中可能會遭遇突發障礙物，傳統的局部自主避障規劃方法一般為進行局部修正以達到對障礙物的自主避碰。由于無人艇編隊各單艇在實施避碰動作時同時具有保形需求，有較大可能出現可行區域非常狹窄的情形，若仍然采用經典RRT 算法延伸子節點的方法，會出現算法隨機搜索陷入死區的情況。針對此問題，本文提出可調節避碰圓與障礙物修正向量的概念，在一定程度上放大可行區域，從而保證算法的正常運行，具體敘述如下：

當無人艇編隊智能感知系統探測到突發障礙物對某單艇航行產生威脅時，此時將觸發本文提出的障礙物修正向量，使搜索樹有著遠離障礙物延伸的趨勢，以提高算法的搜索效率，減少延伸失敗的次數。障礙物修正向量R定義為式（9）。在R的作用下，延伸子節點Pn及其坐標不再滿足式（1）和式（2），而由向量表達式（11）給出：

式中：PO為障礙物中心點；PO，W為點PO到點PW的位移；λ為調節修正強度的權值；V為計算過程產生的過渡向量。式（11）表明，當無人艇編隊感知到突發障礙物時，在原始延伸的基礎上添加由PO到PW連線方向上根據λ動態調節的R，讓搜索樹延伸有著遠離突發障礙物的趨勢，如圖5所示。

圖5 障礙物修正向量作用示意Fig.5 The action of obstacle correction vector

同時，本文根據前述基于RRT 算法的無人艇編隊保形規劃技術，提出一種可調節避碰圓的概念，使無人艇編隊不僅能實現對突發障礙物的有效避碰，而且能實現最大程度的保形。若式（11）計算得到的Pn

式中：(xO，yO)為突發障礙物中心點PO坐標；RO為突發障礙物區域半徑。為了實現無人艇編隊跟隨艇對突發障礙物的避碰，引入了碰撞檢測環節，由式（11）計算生成Pn' 后將不代入式（8）進行判定，而首先轉入式（12）進行碰撞檢測。若不滿足式（12），則轉回式（1）重新生成隨機點與計算延伸子節點進行判定，若滿足，則意味著該點在障礙物區域外，不會與突發障礙物發生碰撞；同時，為了實現在避碰安全條件下最大程度地保形，規定還需滿足式（13）才可認為該周期搜索樹延伸成功。

式中，Rr為可調節避碰圓半徑，實際工程中，可根據障礙物安全區域與保形精度要求選取，如圖6所示。

圖6 可調節避碰圓作用示意Fig.6 The action of adjustable collision avoidance circle

4 仿真結果與分析

為驗證改進RRT 算法的有效性，使用Microsoft Visual Studio 2017 開發環境編寫C++程序進行仿真實驗。本文考慮高速無人艇避碰問題，選取文獻［15］所述USV 進行仿真實驗，編隊中各USV 航速均為20 kn，RRT 算法中S 依據文獻［15］設為10，在該假設條件下，無人艇編隊的單步航行周期約為1 s。

仿真實驗中，假設領航艇L 的航行軌跡為二維坐標系第一象限角平分線從點（0，0）引出的射線，領航艇初始位置為（5 2，5 2），為了測試算法對各種避碰環境的通用性，在無人艇編隊覆蓋的地圖區域由C++隨機函數生成障礙物區域的圓心坐標與尺寸，障礙物設定的發現時間在一個單步航行周期，即每1 s 無人艇編隊智能感知系統刷新當前航行態勢，探測出影響正常航行的障礙物。進行10 個連續單步航行周期下的各跟隨艇對領航艇的保形路徑規劃仿真實驗，即生成10 組在突發障礙物約束條件下的無人艇編隊連續航行路徑規劃點，編隊結構選取為由3 艘USV 組成的等腰直角三角形結構。在編隊路徑規劃仿真實驗中，記1#跟隨艇初始位置為（0，5 2），2#跟隨艇初始位置為（5 2，0），根據2.2 節所述，非嚴格保形控制圓中k 取0.2，即保形精度為跟隨艇的0.2S。

分別進行是否引入AR，R以及同時引入2 種改進方式的仿真實驗對比。經典RRT 算法與改進RRT 算法編隊路徑點對比情況如圖7 所示。改進RRT 算法規劃出的對障礙物避碰周期下的編隊路徑點如圖8 所示。考慮改進前、后的算法均為隨機算法，每組對比重復進行8 次生成10 組連續路徑點的實驗，統計得到位置誤差對比與算法運行時間對比如表1 所示。比較RRT 算法改進前后的指標，統計結果如表2 所示。每個實驗序號中，定義某跟隨艇平均保形誤差ε為

圖7 編隊路徑點對比Fig.7 Formation path point comparison

圖8 改進RRT 算法在障礙物避碰周期下的編隊路徑點Fig.8 Improved RRT algorithm for formation path points in obstacle avoidance period

表1 重復實驗的位置誤差與算法運行時間對比Table 1 Conformal error and running time comparison for repeated experiments

表2 RRT 算法改進前后算法指標統計對比Table 2 Comparison of algorithm indexes before and after the improvement

式中：xFi，yFi分別為第i 個單步航行周期下的理論嚴格保形點橫、縱坐標；xni，yni分別為第i個單步航行周期下的算法規劃出的路徑點橫、縱坐標；n為算法運行的周期數，取10，該式可一定程度體現規劃算法的編隊保形精度。

綜合分析以上實驗結果，可以得出以下結論：

1）引入AR后，跟隨艇的保形平均誤差相比經典RRT 算法減小了7.3%，同時總計算規劃時間得到了大幅降低，這歸因于該向量使搜索樹在每個單步航行周期內都有著向理論上的嚴格保形點生長延伸的趨勢，從而使子延伸節點能夠以更大的概率落在非嚴格保形控制圓區域內，從而減少了搜索樹延伸失敗的次數，降低了算法消耗；同時，AR也使延伸子節點坐標根據與嚴格保形點的歐氏距離進行了修正，從而降低了跟隨艇路徑規劃點的保形平均誤差，提高了無人艇編隊的保形精度。

2）引入R后，總計算規劃時間相比經典RRT算法同樣得到了大幅度降低，而且算法總計算規劃時間方差也低于原算法，這表明引入該向量后，不僅算法效能得到了提升，穩定性也得到了大幅提升，這歸因于該向量在無人艇編隊智能感知系統探測到障礙物從而被觸發后，使無法正常航行的某跟隨艇對應的路徑搜索樹有著遠離障礙物中心點的趨勢，從而降低了延伸子節點落入障礙物區域的概率；同時，由于該情形下的可行區域一般較狹窄，該向量既一定程度保留了經典RRT 算法的隨機性，又對延伸子節點坐標進行修正，指引搜索樹向第3 節所述可調節避碰圓區域生長，從而提高了算法的穩定性，降低了搜索陷入死區的概率。

3）結合表1、表2 與圖7，改進算法同時融合AR與R后，發現跟隨艇的保形平均誤差相比經典RRT 算法減小了12.4%，總計算規劃時間降低了76.9%，總計算規劃時間方差得到了進一步降低，這表明完整改進算法兼有保形精度高、算法效能高及算法穩定好的優點，表現在既能連續規劃出較穩定的編隊結構的路徑點，對突發障礙進行如圖8 所示的避碰，同時耗時少，多次運行穩定。

5 結 語

本文將RRT 算法應用于無人艇編隊路徑規劃問題上，針對無人艇編隊形狀穩定問題，在RRT算法擴展環節提出了一種非嚴格保形修正向量與非嚴格保形控制圓區域。非嚴格保形修正向量使搜索樹有朝著嚴格保形坐標點生長的趨勢，使延伸子節點有著更大概率得到保留；非嚴格保形控制圓區域能有效控制編隊保形精度并調節計算量。針對突發障礙物與非嚴格保形規劃點沖突問題，在RRT 算法碰撞檢測環節提出障礙物修正向量與可調節避碰圓區域，使無人艇安全避碰并最大程度地保持隊形穩定，有效處理了避碰這一航行安全性問題與保形的任務要求之間的平衡問題。本文所提算法耗時少、穩定性強、保形精度高，在實際工程中有一定的應用空間與意義。