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

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

0 引 言

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

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

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

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

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

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

2 無人艇編隊(duì)保形規(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ùn)載器上得到了廣泛的應(yīng)用［12-14］。經(jīng)典RRT 算法構(gòu)造方式是以給定的初始點(diǎn)為隨機(jī)樹根節(jié)點(diǎn)，根據(jù)當(dāng)前環(huán)境快速有效地搜索可行域空間，通過產(chǎn)生隨機(jī)點(diǎn)，將搜索導(dǎo)向空白區(qū)域并添加隨機(jī)樹節(jié)點(diǎn)直至目標(biāo)點(diǎn)，最后通過反向搜索得到路徑點(diǎn)。

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

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

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

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

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

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

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

式（3）表明，完成經(jīng)典RRT 算法下的延伸后，在原始延伸的基礎(chǔ)上添加由Pn到PF連線方向上ω作用的AR，讓搜索樹有朝著嚴(yán)格保形坐標(biāo)點(diǎn)生長的趨勢。式（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é)點(diǎn)距離該時(shí)刻嚴(yán)格保形點(diǎn)越大時(shí)，AR作用越強(qiáng)；但是作用的幅度不會使向嚴(yán)格保形點(diǎn)趨近的向量的模超過單步探索步長而帶來過度修正的問題。該過程幾何上如圖3 所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖7 編隊(duì)路徑點(diǎn)對比Fig.7 Formation path point comparison

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

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

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

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

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

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

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

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

5 結(jié) 語

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