基于改進(jìn)RRT 算法的無人艇編隊路徑規(guī)劃技術(shù)

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

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

0 引 言

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

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

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

1 無人艇編隊路徑規(guī)劃環(huán)境模型建立

無人艇編隊在自主航行過程中通過智能感知系統(tǒng)實時定位并探測障礙物信息，本文所解決的無人艇編隊路徑規(guī)劃問題如圖1 所示。領(lǐng)航艇L以速度V 按預(yù)定航線第一象限角平分線航行，若干跟隨艇為F1，F(xiàn)2，…，F(xiàn)n，無人艇智能感知系統(tǒng)探測到突發(fā)障礙物區(qū)域B1和B2。對于該種情形，本文基于RRT 算法設(shè)計一種編隊路徑規(guī)劃算法，使無人艇編隊在航行過程中保持隊形穩(wěn)定，并對影響無人艇按規(guī)劃航跡點航行的突發(fā)障礙物進(jìn)行自動規(guī)避，最大程度地保持編隊形狀穩(wěn)定。

圖1 無人艇編隊路徑規(guī)劃環(huán)境情形Fig.1 The situation of USV formation path planning

2 無人艇編隊保形規(guī)劃技術(shù)研究

2.1 經(jīng)典RRT 算法

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

2.2 “非嚴(yán)格保形修正向量”與“非嚴(yán)格保形控制圓”設(shè)計

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

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

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

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

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

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

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

圖2 Sigmoid 函數(shù)圖像Fig.2 Graph of Sigmoid function

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

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

式中，k為保形精度權(quán)值，可根據(jù)無人艇編隊實際任務(wù)下的精度需求賦值。

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

3 無人艇編隊局部自主避碰技術(shù)

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

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

式中：PO為障礙物中心點；PO，W為點PO到點PW的位移；λ為調(diào)節(jié)修正強(qiáng)度的權(quán)值；V為計算過程產(chǎn)生的過渡向量。式（11）表明，當(dāng)無人艇編隊感知到突發(fā)障礙物時，在原始延伸的基礎(chǔ)上添加由PO到PW連線方向上根據(jù)λ動態(tài)調(diào)節(jié)的R，讓搜索樹延伸有著遠(yuǎn)離突發(fā)障礙物的趨勢，如圖5所示。

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

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

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

式中，Rr為可調(diào)節(jié)避碰圓半徑，實際工程中，可根據(jù)障礙物安全區(qū)域與保形精度要求選取，如圖6所示。

圖6 可調(diào)節(jié)避碰圓作用示意Fig.6 The action of adjustable collision avoidance circle

4 仿真結(jié)果與分析

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

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

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

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

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

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

表2 RRT 算法改進(jìn)前后算法指標(biāo)統(tǒng)計對比Table 2 Comparison of algorithm indexes before and after the improvement

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

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

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

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

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

5 結(jié) 語

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