改進(jìn)雙向快速搜索隨機(jī)樹(shù)的無(wú)人艇路徑規(guī)劃

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

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

0 引 言

水面無(wú)人艇(unmanned surface vehicles, USV)作為一種智能化海洋裝備,具有便攜性好、操縱性強(qiáng)等優(yōu)點(diǎn),常被用來(lái)執(zhí)行近海水域的海洋調(diào)查和監(jiān)測(cè)任務(wù),以及海上應(yīng)急和救助等工作[1]。由于USV適用于復(fù)雜的工況,則要求其具有更強(qiáng)的自主性,而路徑規(guī)劃是USV自主系統(tǒng)的重要基礎(chǔ)[2]。

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

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

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

1 研究基礎(chǔ)

1.1 環(huán)境模型描述

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

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

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

(1)

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

1.2 BI-RRT算法原理

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

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

雖然BI-RRT相比RRT搜索速度更快,但BI-RRT的擴(kuò)展方向缺少導(dǎo)向性和啟發(fā)性,冗余的擴(kuò)展降低了路徑的搜索效率。此外,傳統(tǒng)BI-RRT算法生成了大量冗余節(jié)點(diǎn),導(dǎo)致得到的路徑拐點(diǎn)較多。本文針對(duì)上述問(wèn)題,對(duì)傳統(tǒng)BI-RRT算法進(jìn)行改進(jìn),以提高路徑搜索效率和路徑的平滑度。

2 改進(jìn)的BI-RRT算法

2.1 搜索樹(shù)擴(kuò)展策略

2.1.1 引入貪心思想

為了提高搜索效率,本文引入貪心思想,通過(guò)兩種規(guī)則對(duì)傳統(tǒng)BI-RRT算法進(jìn)行改進(jìn):規(guī)則一,在搜索樹(shù)擴(kuò)展之前,首先判斷起始點(diǎn)和目標(biāo)點(diǎn)之間是否有障礙物。如果沒(méi)有障礙物,則直接連接起始點(diǎn)和目標(biāo)點(diǎn),生成路徑;規(guī)則二,在搜索樹(shù)得到新節(jié)點(diǎn)后開(kāi)始判斷是否可以與另一棵樹(shù)上最近的節(jié)點(diǎn)相連接。如果可以直接連接,則完成路徑搜索。

2.1.2 高斯偏置隨機(jī)點(diǎn)生成策略

傳統(tǒng)的BI-RRT算法在路徑搜索時(shí)存在目標(biāo)導(dǎo)向性不足的問(wèn)題,因?yàn)槠洳蓸狱c(diǎn)分布比較均勻,覆蓋了整個(gè)搜索空間。這種情況導(dǎo)致搜索樹(shù)進(jìn)行了大量的盲目擴(kuò)展,浪費(fèi)了大量的計(jì)算資源,同時(shí)也降低了算法的收斂速度。為了改善這個(gè)問(wèn)題,本文提出一種基于高斯采樣的BI-RRT算法,該算法能夠在一定的概率下以高斯分布的方式出現(xiàn)在目標(biāo)點(diǎn)附近。改進(jìn)后隨機(jī)點(diǎn)xrand的求取如下:

(2)

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

(3)

在北東坐標(biāo)系下,當(dāng)協(xié)方差矩陣B的正對(duì)角線的值相同時(shí),若反對(duì)角線的值為負(fù),則二維高斯分布的圖像的長(zhǎng)軸方向與坐標(biāo)系橫軸的正方向夾角為135°;若反對(duì)角線的值為零,則不存在偏角;若反對(duì)角線的值為正,則二維高斯分布的圖像的長(zhǎng)軸方向與坐標(biāo)系橫軸正方向的夾角為45°,具體如圖3所示。

圖3 不同協(xié)方差矩陣下隨機(jī)點(diǎn)的分布Fig.3 Random point distribution with different covariance matrixes

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

圖4 坐標(biāo)系轉(zhuǎn)換示意圖Fig.4 Schematic diagram of coordinate system conversion

(4)

(5)

(6)

圖5 高斯隨機(jī)點(diǎn)分布示意圖Fig.5 Distribution schematic diagram of Gaussian random points

2.1.3 啟發(fā)式節(jié)點(diǎn)擴(kuò)展方法

傳統(tǒng)BI-RRT最近節(jié)點(diǎn)xnear的選擇僅考慮與隨機(jī)點(diǎn)xrand的距離,因此xnear的選擇也過(guò)于隨機(jī),這也是導(dǎo)致搜索效率較低的原因之一。根據(jù)A-Star算法中的啟發(fā)式路徑代價(jià)的思想,本文對(duì)最近節(jié)點(diǎn)xnear進(jìn)行改進(jìn),xnear的計(jì)算如下:

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

(7)

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

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

(8)

2.2 路徑平滑策略

2.2.1 轉(zhuǎn)角約束和連接點(diǎn)約束

傳統(tǒng)的BI-RRT算法在節(jié)點(diǎn)擴(kuò)展時(shí)由于未考慮角度約束,通常會(huì)出現(xiàn)大角度的連接方式。此外,兩顆搜索樹(shù)的連接處也常不能平滑過(guò)渡,也會(huì)出現(xiàn)大角度的轉(zhuǎn)折,因此傳統(tǒng)的BI-RRT算法無(wú)法滿足USV的運(yùn)動(dòng)學(xué)要求。為解決這一問(wèn)題,本文設(shè)計(jì)了角度約束的策略,設(shè)USV最大轉(zhuǎn)彎角度為θmax,在生成新節(jié)點(diǎn)xnew后,計(jì)算新擴(kuò)展路徑與前一段路徑的夾角θ4。若θ4小于θmax,則記錄并保存新擴(kuò)展的節(jié)點(diǎn),否則,刪掉新擴(kuò)展的節(jié)點(diǎn)。θ4的計(jì)算公式如下:

(9)

式中:A為xnear節(jié)點(diǎn)到xnew節(jié)點(diǎn)的向量;B為xparent節(jié)點(diǎn)到xnear節(jié)點(diǎn)的向量;xparent節(jié)點(diǎn)為xnear節(jié)點(diǎn)的父節(jié)點(diǎn)。

為解決傳統(tǒng)BI-RRT算法在兩棵樹(shù)在連接點(diǎn)處出現(xiàn)大角度連接的問(wèn)題,本文在連接處同樣進(jìn)行角度約束。如圖6所示,在節(jié)點(diǎn)擴(kuò)展無(wú)障礙的情況下,當(dāng)滿足θ5≤θmax且θ6≤θmax時(shí)兩棵樹(shù)連接,路徑搜索成功。θ5和θ6的計(jì)算公式分別如下:

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

(10)

(11)

2.2.2 搜索樹(shù)剪枝優(yōu)化

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

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

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

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

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

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

2.2.3 3次B樣條優(yōu)化

經(jīng)過(guò)上述的改進(jìn),路徑的節(jié)點(diǎn)減少了,但不能滿足USV航行的實(shí)際情況,因此還需要將得到的路徑進(jìn)行平滑處理。3次B樣條曲線具有二階連續(xù)導(dǎo)數(shù),因此可以滿足速度和加速度對(duì)連續(xù)性的要求,更便于對(duì)USV進(jìn)行控制與路徑跟蹤。本文選擇3次B樣條對(duì)得到的路徑進(jìn)行進(jìn)一步處理,當(dāng)有m+1個(gè)控制點(diǎn)時(shí),3次B樣條的計(jì)算公式如下:

(12)

其中,基函數(shù)Bi,3的求法如下:

(13)

2.3 算法流程

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

步驟 1確定起始點(diǎn)xstart和目標(biāo)點(diǎn)xgoal,對(duì)地圖進(jìn)行二值化處理,將不可航行區(qū)域表示為黑色,用數(shù)字1表示,將可航行水域表示為白色,用數(shù)字0表示,對(duì)障礙物進(jìn)行膨脹處理。

步驟 2判斷xstart和目標(biāo)點(diǎn)xgoal之間的連線是否有障礙物。若無(wú)障礙物,則路徑搜索成功,否則執(zhí)行步驟3。

步驟 3首先對(duì)第一棵搜索樹(shù)進(jìn)行節(jié)點(diǎn)擴(kuò)展。通過(guò)基于高斯分布的目標(biāo)偏置確定隨機(jī)點(diǎn)x1rand,并通過(guò)啟發(fā)式代價(jià)確定x1near。

步驟 4判斷x1near與x1rand的連線與上一擴(kuò)展節(jié)點(diǎn)是否滿足角度約束。若滿足,則執(zhí)行步驟5,否則返回步驟3。

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

步驟 6判斷x1new與另一棵樹(shù)上最近的節(jié)點(diǎn)之間是否有障礙物。若無(wú)障礙物,路徑搜索完成,否則執(zhí)行步驟7。

步驟 7接下來(lái)進(jìn)行第二棵樹(shù)的節(jié)點(diǎn)擴(kuò)展。通過(guò)基于高斯分布的目標(biāo)偏置確定隨機(jī)點(diǎn)x2rand,并通過(guò)啟發(fā)式代價(jià)確定x2near。

步驟 8判斷x2near與x2rand的連線與上一擴(kuò)展節(jié)點(diǎn)是否滿足角度約束。若滿足,則執(zhí)行步驟9,否則返回步驟7。

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

步驟 10判斷x2new與第一棵樹(shù)上最近的節(jié)點(diǎn)之間是否有障礙物。若無(wú)障礙物,則執(zhí)行步驟11,否則返回步驟7。

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

步驟 12對(duì)初始路徑進(jìn)行剪枝和3次B樣條優(yōu)化處理。改進(jìn)的算法流程如圖8所示。

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

3 實(shí)驗(yàn)結(jié)果與分析

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

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

表1 狹窄通道水域中仿真數(shù)據(jù)對(duì)比Table 1 Comparison of simulation data in narrow-passage waters

圖9 狹窄通道水域中的對(duì)比實(shí)驗(yàn)Fig.9 Comparative experiments in narrow-passage waters

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

表2 復(fù)雜環(huán)境水域中仿真數(shù)據(jù)對(duì)比Table 2 Comparison of simulation data in complex waters

圖10 復(fù)雜環(huán)境水域中對(duì)比實(shí)驗(yàn)Fig.10 Comparative experiments in complex waters

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

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

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

4 結(jié) 論

為了解決傳統(tǒng)的BI-RRT算法在全局路徑規(guī)劃時(shí)搜索效率低、路徑拐點(diǎn)較多等問(wèn)題,本文提出一種改進(jìn)的USV全局路徑規(guī)劃算法。通過(guò)與BI-RRTstar、IB-RRTstar以及BI-RRT進(jìn)行仿真對(duì)比實(shí)驗(yàn),得出以下結(jié)論:

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

(2) 通過(guò)對(duì)節(jié)點(diǎn)擴(kuò)展和搜索樹(shù)連接進(jìn)行角度約束,將生成的路徑剪枝并進(jìn)行3次B樣條優(yōu)化處理,生成的路徑可以滿足USV的運(yùn)動(dòng)學(xué)要求、減少路徑拐點(diǎn)數(shù)量、增加路徑的平滑度、縮短路徑長(zhǎng)度。相對(duì)于改進(jìn)前,改進(jìn)的BI-RRT平均路徑減少了24.0%。

在未來(lái)的研究中,應(yīng)考慮環(huán)境因素,例如風(fēng)浪流等,并研究多智能體之間的協(xié)調(diào)避碰、結(jié)合局部路徑規(guī)劃,以滿足USV海上自主航行的實(shí)際情況。