基于RRT算法的無人艇航線重規(guī)劃方法

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

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

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

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

1 航線重規(guī)劃方法

1.1 整體流程

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

圖1 航線重規(guī)劃算法流程

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

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

表1 BP_RRT算法偽代碼

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

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

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

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

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

1.3 隨機樹分割方法

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

圖2 隨機樹分割過程示意

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

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

對于一個包含有M個節(jié)點的隨機樹(T)，根據(jù)障礙物和隨機樹，利用FIND_DEAE_NODE函數(shù)確定失效節(jié)點，并將這些節(jié)點的Parent屬性設置為0。循環(huán)遍歷隨機樹中的所有節(jié)點，查找父節(jié)點Parent屬性為0的節(jié)點，并將這些節(jié)點的Parent屬性也設置為0，直到隨機樹中所有節(jié)點的Parent屬性不再改變?yōu)橹埂?/p>

1.4 航線平滑步驟

對于一個包含N個節(jié)點的航線(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之間所有節(jié)點，令Ustart=Ugoal，Ugoal=UN。

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

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

表3 航線平滑處理偽代碼

2 無人艇航線重規(guī)劃仿真實驗

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

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

2.1 可行性實驗

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

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

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

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

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

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

2.2 對比實驗

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

2.2.1 算法耗時

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

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

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

表4 廣州港航線重規(guī)劃耗時統(tǒng)計特例 s

2.2.2 重規(guī)劃航線長度

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

圖7 航線長度對比

3 新增障礙物位置的影響

3.1 仿真實驗

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

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

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

然而在廣州港(1/2)L處和大連港(2/3)L處卻出現(xiàn)耗時突增的現(xiàn)象。在廣州港任務空間中由(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 算法耗時突增現(xiàn)象分析

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

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

圖9 任務空間通道口示意

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

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

4 結論

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

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

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