改進雙向快速搜索隨機樹的無人艇路徑規劃

趙貴祥, 周 健, 李云淼, 王晨旭,*

(1. 天津大學海洋科學與技術學院, 天津 300110; 2. 江蘇自動化研究所, 北京 100036)

0 引 言

水面無人艇(unmanned surface vehicles, USV)作為一種智能化海洋裝備,具有便攜性好、操縱性強等優點,常被用來執行近海水域的海洋調查和監測任務,以及海上應急和救助等工作[1]。由于USV適用于復雜的工況,則要求其具有更強的自主性,而路徑規劃是USV自主系統的重要基礎[2]。

根據環境信息的知悉情況,路徑規劃可分為局部路徑規劃和全局路徑規劃,前者適用于USV的局部避碰,后者能在已知環境中提前規劃一條安全可靠的路徑[3]。全局路徑規劃主要包括A-star算法[4]、遺傳算法[5]、粒子群優化算法[6]、概率路線圖(probabilistic road maps, PRM)算法[7]和快速搜索隨機樹(rapidly-exploring random tree, RRT)算法。RRT算法具有模型簡單、搜索速度快且具有概率完備性等優勢,在解決路徑規劃的問題上得到了廣泛的應用[8-10]。雙向RRT[11](bi-directional RRT, BI-RRT)是由兩個隨機搜索樹同時擴展的RRT版本,提高了算法收斂速度。然而,BI-RRT仍生成大量冗余的隨機點,帶來了搜索效率低、路徑拐點多等缺點[12-13]。

為了提高算法的搜索速度,國內外學者進行了大量的改進和研究,向金林等[14]通過動態步長的策略對BI-RRT算法進行了改進,改進后路徑質量更好,算法收斂時間更短。徐秉超等[15]采用預生長和基于歐氏距離的隨機點篩選機制,提高了BI-RRT算法的搜索效率。Ma等[16]提出了概率平滑的BI-RRT算法,通過引入目標偏置策略和節點校正機制減少了碰撞概率,提高了算法的搜索速度。Maseko等[17]通過知情采樣和路徑優化來加速基于基本RRT和優化RRT(RRT star, RRTstar)的搜索速度。為減少RRT規劃路徑的拐點,Karaman等[18]提出了RRTstar算法,通過迭代的方式不斷更新父節點和重新布線,以確保漸進式的優化。Qureshi等[19]為了提高算法的收斂速度,在RRTstar中加入了人工勢能場算法。Akgun等[20]在RRTstar的基礎上提出了改進的雙向RRTstar(bi-directional-RRTstar, BI-RRTstar)算法,提高了搜索效率。Qureshi等[21]隨后提出智能BI-RRTstar(intelligent BI-RRTstar, IB-RRTstar)算法,通過引入智能樣本、插入啟發式算子,使算法快速收斂到最優的路徑。RRTstar類的算法能夠減少路徑拐點和提高平滑度,但增加了時間成本。Liu等[22]提出一種基于貝塞爾曲線平滑和目標偏向的BI-RRT,提高了路徑的平滑度。然而,上述的改進算法都是單一的,并未同時解決搜索效率低、路徑拐點多等問題,改進的效果受到一定的影響。

為此,在上述研究的基礎上對BI-RRT進行改進,提出了搜索樹擴展策略和路徑平滑策略。首先采取極度貪心的思想,提高了兩顆隨機樹的連接概率,并采用高斯偏置隨機點的采樣方法,降低了搜索的盲目性,選擇啟發式的節點擴展策略使隨機樹的擴展更具導向性;其次,對節點擴展進行角度約束,對生成的路徑進行剪枝和3次B樣條優化處理,最終得到一條平滑的路徑。

1 研究基礎

1.1 環境模型描述

USV全局路徑規劃需要尋找一條從起始點到目標點的無碰撞路徑。首先,需要將已經獲得的靜態環境信息進行柵格化,并進一步進行二值化處理。在此過程中,障礙物被表示為黑色,用數字1表示,可行水域被表示為白色,用數字0表示。為了簡化模型,通常將USV視為質點,并對障礙物進行膨脹處理。通常使用一個形狀為圓形或正方形的卷積核對障礙物進行膨脹,通過對原始圖像進行掃描和卷積計算來實現。如果得到的結果全部為1,則無需進行膨脹,否則需要進行膨脹。這種方法不僅適用于凸形障礙物,也適用于凹形障礙物,如圖1所示。圖像膨脹的公式定義如下:

圖1 障礙物膨脹示意圖Fig.1 Schematic diagram of obstacle expansion

A⊕B={x|(B)X∩A≠?}

(1)

式中:A為需要膨脹的圖像;B為卷積核; ⊕為閔可夫斯基運算符。

1.2 BI-RRT算法原理

BI-RRT是RRT算法的進化版。它從起點xstart和終點xgoal開始,分別構建兩棵搜索樹,直到兩棵樹相遇,最終實現路徑規劃。具體步驟如下:如圖2所示,首先,對第一棵樹進行擴展。在搜索空間中以一定概率選擇任意隨機點xrand作為采樣點,然后在搜索樹上找到距離xrand最近的節點xnear,并以一定步長向xrand的方向擴展得到新的節點xnew。接下來,檢查xnear和xnew之間是否有障礙物,若無障礙物,則將xnew添加到搜索樹中,否則放棄xnew。第二棵樹的擴展步驟與第一棵樹相同。兩棵樹不斷交替擴展,直至它們之間的距離小于設定的閾值。此時,從每棵樹的xnew節點開始,向各自的xnear節點回溯,生成路徑的節點。最后,通過連接這些節點的線段生成路徑。

圖2 BI-RRT示意圖Fig.2 Schematic diagram of the BI-RRT

雖然BI-RRT相比RRT搜索速度更快,但BI-RRT的擴展方向缺少導向性和啟發性,冗余的擴展降低了路徑的搜索效率。此外,傳統BI-RRT算法生成了大量冗余節點,導致得到的路徑拐點較多。本文針對上述問題,對傳統BI-RRT算法進行改進,以提高路徑搜索效率和路徑的平滑度。

2 改進的BI-RRT算法

2.1 搜索樹擴展策略

2.1.1 引入貪心思想

為了提高搜索效率,本文引入貪心思想,通過兩種規則對傳統BI-RRT算法進行改進:規則一,在搜索樹擴展之前,首先判斷起始點和目標點之間是否有障礙物。如果沒有障礙物,則直接連接起始點和目標點,生成路徑;規則二,在搜索樹得到新節點后開始判斷是否可以與另一棵樹上最近的節點相連接。如果可以直接連接,則完成路徑搜索。

2.1.2 高斯偏置隨機點生成策略

傳統的BI-RRT算法在路徑搜索時存在目標導向性不足的問題,因為其采樣點分布比較均勻,覆蓋了整個搜索空間。這種情況導致搜索樹進行了大量的盲目擴展,浪費了大量的計算資源,同時也降低了算法的收斂速度。為了改善這個問題,本文提出一種基于高斯采樣的BI-RRT算法,該算法能夠在一定的概率下以高斯分布的方式出現在目標點附近。改進后隨機點xrand的求取如下:

(2)

式中:rand為地圖中的隨機點;p為(0,1)之間的隨機數;η,λ為目標偏向閾值;f(x,μ,B)為二元高斯聯合分布密度函數,其中x為隨機點向量,μ為均值向量,B為協方差矩陣。求取公式如下:

(3)

在北東坐標系下,當協方差矩陣B的正對角線的值相同時,若反對角線的值為負,則二維高斯分布的圖像的長軸方向與坐標系橫軸的正方向夾角為135°;若反對角線的值為零,則不存在偏角;若反對角線的值為正,則二維高斯分布的圖像的長軸方向與坐標系橫軸正方向的夾角為45°,具體如圖3所示。

圖3 不同協方差矩陣下隨機點的分布Fig.3 Random point distribution with different covariance matrixes

為了提高采樣點的利用率,將二維高斯分布的圖像的長軸方向與起始點和目標點連線方向垂直。為方便分析,對隨機點進行旋轉變換。如圖4所示,θ1為起始點和目標點的連線與水平軸的正方向的夾角;θ2為高斯分布的圖像的長軸與起始點和目標點連線的夾角;θ3為隨機點需要旋轉的角度。

圖4 坐標系轉換示意圖Fig.4 Schematic diagram of coordinate system conversion

(4)

(5)

(6)

圖5 高斯隨機點分布示意圖Fig.5 Distribution schematic diagram of Gaussian random points

2.1.3 啟發式節點擴展方法

傳統BI-RRT最近節點xnear的選擇僅考慮與隨機點xrand的距離,因此xnear的選擇也過于隨機,這也是導致搜索效率較低的原因之一。根據A-Star算法中的啟發式路徑代價的思想,本文對最近節點xnear進行改進,xnear的計算如下:

xnear=min(Cost(xtree,xstart)+dis(xtree,xgoal))

(7)

式中:Cost(xtree,xstart)為搜索樹上的節點至起始點的路徑之和;dis(xtree,xgoal)為搜索樹上的節點到目標點的距離。常用的距離計算公式主要包括歐氏距離和曼哈頓距離,為進一步提高計算速度,本文通過曼哈頓距離計算搜索樹和目標點之間的距離,如下所示:

dis(x,y)=|x(1)-y(1)|+|x(2)-y(2)|

(8)

2.2 路徑平滑策略

2.2.1 轉角約束和連接點約束

傳統的BI-RRT算法在節點擴展時由于未考慮角度約束,通常會出現大角度的連接方式。此外,兩顆搜索樹的連接處也常不能平滑過渡,也會出現大角度的轉折,因此傳統的BI-RRT算法無法滿足USV的運動學要求。為解決這一問題,本文設計了角度約束的策略,設USV最大轉彎角度為θmax,在生成新節點xnew后,計算新擴展路徑與前一段路徑的夾角θ4。若θ4小于θmax,則記錄并保存新擴展的節點,否則,刪掉新擴展的節點。θ4的計算公式如下:

(9)

式中:A為xnear節點到xnew節點的向量;B為xparent節點到xnear節點的向量;xparent節點為xnear節點的父節點。

為解決傳統BI-RRT算法在兩棵樹在連接點處出現大角度連接的問題,本文在連接處同樣進行角度約束。如圖6所示,在節點擴展無障礙的情況下,當滿足θ5≤θmax且θ6≤θmax時兩棵樹連接,路徑搜索成功。θ5和θ6的計算公式分別如下:

圖6 搜索樹平滑連接示意圖Fig.6 Schematic diagram of smooth connection of search tree

(10)

(11)

2.2.2 搜索樹剪枝優化

為減少冗余的節點,需要對得到的初始路徑進行剪枝處理。改進的BI-RRT生成的初始路徑為P(i)=[P(0),P(2),P(3),P(4),…,P(s)];在已知初始路徑節點P(i)的情況下,從起始點xstart至目標點xgoal進行路徑剪枝。具體步驟如下:

步驟 1輸入初始路徑的節點P(i)。P(i)=[P(0),P(2),P(3),P(4),…,P(s)]。

步驟 2將P(0)作為優化起始點P-start,Pnew=[P(0)],判斷P(0)與P(s)是否可以直接連通,若可以連通則優化程序結束,否則執行下一步驟。

步驟 3依次判斷P(0)與P(s-1)是否可以連通,若可以連通,將P(i)視為優化起始點P-start。否則,s=s-1,不斷循環此過程,直至P(0)和P(i)可以連通,并將P(i)視為優化起始點P-start,不斷循環此過程。

步驟 4判斷P-start和P(s)是否可以連通。若可以連通,則結束優化程序,Pnew=[P(0),P(i),…,P(s)]。否則,s=s-1,直到P-start和P(s)可以連通,將P(s)賦值于P-start,并返回步驟3。路徑剪枝的過程順序如圖7中步驟①～步驟⑤所示。

圖7 路徑剪枝示意圖Fig.7 Schematic diagram of path cuttings

2.2.3 3次B樣條優化

經過上述的改進,路徑的節點減少了,但不能滿足USV航行的實際情況,因此還需要將得到的路徑進行平滑處理。3次B樣條曲線具有二階連續導數,因此可以滿足速度和加速度對連續性的要求,更便于對USV進行控制與路徑跟蹤。本文選擇3次B樣條對得到的路徑進行進一步處理,當有m+1個控制點時,3次B樣條的計算公式如下:

(12)

其中,基函數Bi,3的求法如下:

(13)

2.3 算法流程

本文介紹了一種基于改進BI-RRT算法的USV路徑規劃算法。具體步驟如下:

步驟 1確定起始點xstart和目標點xgoal,對地圖進行二值化處理,將不可航行區域表示為黑色,用數字1表示,將可航行水域表示為白色,用數字0表示,對障礙物進行膨脹處理。

步驟 2判斷xstart和目標點xgoal之間的連線是否有障礙物。若無障礙物,則路徑搜索成功,否則執行步驟3。

步驟 3首先對第一棵搜索樹進行節點擴展。通過基于高斯分布的目標偏置確定隨機點x1rand,并通過啟發式代價確定x1near。

步驟 4判斷x1near與x1rand的連線與上一擴展節點是否滿足角度約束。若滿足,則執行步驟5,否則返回步驟3。

步驟 5從x1near向x1rand方向延長步長δ的距離,判斷連線是否可視。若可視,得到確定x1new,否則返回步驟3。

步驟 6判斷x1new與另一棵樹上最近的節點之間是否有障礙物。若無障礙物,路徑搜索完成,否則執行步驟7。

步驟 7接下來進行第二棵樹的節點擴展。通過基于高斯分布的目標偏置確定隨機點x2rand,并通過啟發式代價確定x2near。

步驟 8判斷x2near與x2rand的連線與上一擴展節點是否滿足角度約束。若滿足,則執行步驟9,否則返回步驟7。

步驟 9從x2near向x2rand方向延長步長δ的距離,判斷連線是否可視。若可視,記錄該點為x2new,否則返回步驟7。

步驟 10判斷x2new與第一棵樹上最近的節點之間是否有障礙物。若無障礙物,則執行步驟11,否則返回步驟7。

步驟 11判斷步驟10的連接是否滿足角度約束。若滿足,則路徑搜索完成,否則返回步驟7。

步驟 12對初始路徑進行剪枝和3次B樣條優化處理。改進的算法流程如圖8所示。

圖8 路徑剪枝示意圖Fig.8 Schematic diagram of path cuttings

3 實驗結果與分析

為了驗證改進的BI-RRT算法的有效性,本文通過計算機進行仿真實驗。實驗中,將全局路徑規劃的起始點設置為(30,30),目標點設為(470,470),步長設為30,USV的船長設為5 m,最大角度變換不超過90°。將USV的船長設為卷積核的長度,對柵格化的地圖進行膨脹處理。地圖中的黑色代表障礙物,白色代表可行水域。在狹窄通道水域和復雜水域的環境中,分別使用改進BI-RRT、BI-RRT、BI-RRTstar和IB-RRTstar進行仿真實驗。其中,BI-RRTstar和IB-RRTstar算法的最大迭代次數設為1 500。

在狹窄通道水域環境中,對BI-RRT、BI-RRTstar、IB-RRTstar以及改進的BI-RRT算法分別進行了100次實驗,每次試驗結果都具有隨機性,因此隨機選取了每種算法的一次實驗結果進行比較,比較結果如圖9所示。在BI-RRT算法中,由于隨機點缺乏目標導向性,導致生成了大量冗余的節點,而搜索樹的擴展和連接處也存在大角度轉折,因此路徑的拐點較多,如圖9(a)中的橢圓1和橢圓2處所示。BI-RRTstar在BI-RRT的基礎上進行了大量的父節點更新和重連接的操作,減少了路徑的拐點,但大量的迭代過程需要更多的時間,如圖9(b)所示。IB-RRTstar同樣需要進行大量的迭代,結果如圖9(c)所示。改進的BI-RRT算法在未進行后期優化的情況下,路徑較原始的BI-RRT算法更加平滑,擴展節點更少,搜索樹的擴展和連接處也不存在大角度轉折,從而減少了路徑的拐點,如圖9(d)中較粗的折線所示。在剪枝優化后,改進的BI-RRT算法進一步減少了路徑的拐點,如圖9(d)中較細的折線所示。最終,在改進的BI-RRT算法的基礎上進行了3次B樣條優化處理,生成的路徑不存在拐點,如圖9(e)所示。表1展示了狹窄通道水域環境中各算法100次實驗的結果。

表1 狹窄通道水域中仿真數據對比Table 1 Comparison of simulation data in narrow-passage waters

圖9 狹窄通道水域中的對比實驗Fig.9 Comparative experiments in narrow-passage waters

在復雜環境水域中,同樣進行了100次BI-RRT、BI-RRTstar、IB-RRTstar以及改進的BI-RRT算法的實驗。隨機選取了每種算法的一次實驗結果進行比較,如圖10所示。實驗結果顯示,BI-RRT算法存在大量冗余的擴展,搜索樹的擴展和連接處存在大角度轉折,路徑的拐點較多,如圖10(a)中標注的橢圓1和橢圓2處。BI-RRTstar的實驗結果如圖10(b)所示,IB-RRTstar的實驗結果如圖10(c)所示。雖然路徑平滑度得到了改進,但消耗了大量時間。改進BI-RRT的結果如圖10(d)所示,較粗的折線表示未進行后期優化的結果,而較細的折線表示剪枝優化后的結果。改進后的算法搜索樹的節點較少,路徑的轉角大和拐點多的問題得到了有效的改善。圖10(e)表示改進BI-RRT的結果,優化的路徑平滑無拐點。上述4種算法的100次仿真實驗結果如表2所示。

表2 復雜環境水域中仿真數據對比Table 2 Comparison of simulation data in complex waters

圖10 復雜環境水域中對比實驗Fig.10 Comparative experiments in complex waters

(1) BI-RRT算法的全局路徑規劃中采樣點過多,搜索效率較低。改進的BI-RRT算法采用了極度貪心算法和高斯偏置隨機點采樣,并引用了啟發式節點擴展,從而提高了路徑的搜索效率。表1和表2的數據顯示,改進后的BI-RRT算法采樣點最少,時間最短,相對于改進前平均時間減少了40.5%,隨機采樣點數減少了65.0%。

(2) BI-RRT算法的全局路徑規劃存在較多的拐點,路徑不平滑,不能適用于欠驅動的USV的路徑規劃。改進的算法通過節點擴展和搜索樹連接的角度約束,有效減少了全局路徑的拐點數。隨后,改進算法采用剪枝策略和3次B樣條進行路徑優化和平滑處理,優化后的路徑是一條平滑的曲線,能夠滿足USV的運動學要求。表1和表2的數據顯示,改進后的BI-RRT算法路徑拐點最少,平滑度最高。

(3) 表1和表2的數據顯示,相對于BI-RRT算法,BI-RRTstar、IB-RRTstar和改進的BI-RRT算法的平均路徑長度分別減少了23.5%、24.3%和24.0%。BI-RRTstar算法和IB-RRTstar算法可以得到相對較短的路徑。若只考慮規劃的路徑長度,IB-RRTstar算法得到的路徑較本文改進算法得到的路徑更短,但IB-RRTstar算法需要大量的迭代,增加了算法的耗時。此外,IB-RRTstar算法得到的路徑仍存在一些拐點,不能滿足USV的運動學要求。相比之下,改進的算法既能夠減少算法的時間、滿足USV運動學的約束,同時也能夠減少路徑的長度。

4 結 論

為了解決傳統的BI-RRT算法在全局路徑規劃時搜索效率低、路徑拐點較多等問題,本文提出一種改進的USV全局路徑規劃算法。通過與BI-RRTstar、IB-RRTstar以及BI-RRT進行仿真對比實驗,得出以下結論:

(1) 采取極度貪心、高斯偏置隨機點采樣以及啟發式的節點擴展方法可以縮短路徑搜索時間,提高算法的搜索效率。改進的BI-RRT相對于改進前,平均時間減少了40.5%,隨機采樣點減少了65.0%。

(2) 通過對節點擴展和搜索樹連接進行角度約束,將生成的路徑剪枝并進行3次B樣條優化處理,生成的路徑可以滿足USV的運動學要求、減少路徑拐點數量、增加路徑的平滑度、縮短路徑長度。相對于改進前,改進的BI-RRT平均路徑減少了24.0%。

在未來的研究中,應考慮環境因素,例如風浪流等,并研究多智能體之間的協調避碰、結合局部路徑規劃,以滿足USV海上自主航行的實際情況。