馬小陸，梅 宏，王 兵，吳紫恒

（安徽工業(yè)大學(xué)電氣與信息工程學(xué)院，馬鞍山 243000）

路徑規(guī)劃是移動(dòng)機(jī)器人導(dǎo)航和控制的基礎(chǔ)，其目的是尋找一條從當(dāng)前位置無(wú)碰撞地運(yùn)動(dòng)到目標(biāo)位置，且滿足路徑最短、能量消耗最少等評(píng)價(jià)標(biāo)準(zhǔn)的路徑[1]。目前主流的路徑規(guī)劃算法可以分成三大類(lèi)，分別為傳統(tǒng)算法、啟發(fā)式算法和智能仿生算法，其中傳統(tǒng)算法包括可視圖法、人工勢(shì)場(chǎng)法[2]、模擬退火法等；啟發(fā)式算法包括Dijkstra 算法[3]、A*算法[4,5]和Floyd 算法等；智能仿生算法包括人工魚(yú)群算法、粒子群算法[6]、蟻群算法等。

快速擴(kuò)展隨機(jī)樹(shù)(Rapidly-exploring Random Tree,RRT)算法[7]是由S.M.LaVall 于1998年提出的，屬于啟發(fā)式算法。RRT 算法是依據(jù)當(dāng)前已知信息快速搜索未知狀態(tài)空間，從而具有較好的全局搜索能力，但是獲取到的最終路徑不一定是最優(yōu)路徑。針對(duì)RRT 算法存在的問(wèn)題，S.Karaman 等學(xué)者利用代價(jià)函數(shù)對(duì)RRT算法進(jìn)行優(yōu)化，提出了漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)(Rapidly-exploring Random Tree*, RRT*)算法[8]。RRT*算法從擴(kuò)展節(jié)點(diǎn)區(qū)域篩選出總代價(jià)值最小的節(jié)點(diǎn)作為父節(jié)點(diǎn)，并且每次迭代結(jié)束后都將重新連接隨機(jī)樹(shù)上的節(jié)點(diǎn)，從而確保算法尋路時(shí)的漸進(jìn)最優(yōu)性。雖然RRT*算法已經(jīng)能夠有效解決移動(dòng)機(jī)器人全局路徑規(guī)劃問(wèn)題，但是依舊存在收斂速度慢、資源消耗大、路徑轉(zhuǎn)折點(diǎn)數(shù)量過(guò)多等缺點(diǎn)，因此許多學(xué)者提出了各種優(yōu)化方法。文獻(xiàn)[9]采用人工勢(shì)場(chǎng)優(yōu)化了采樣過(guò)程，加快了算法尋路時(shí)收斂速度；文獻(xiàn)[10]引入了啟發(fā)式采樣節(jié)點(diǎn)插入算法，提高了算法的實(shí)用性和魯棒性。

本文針對(duì)RRT*算法在實(shí)際應(yīng)用中存在的問(wèn)題，提出了一種基于跳點(diǎn)搜索(Jump Point Search, JPS)尋路策略的RRT*算法。該算法以RRT*算法為基礎(chǔ)，提出了一種目標(biāo)點(diǎn)路徑尋路策略；同時(shí)，依據(jù)JPS 算法的尋路策略計(jì)算后續(xù)節(jié)點(diǎn)，提升了RRT*算法的尋路效率，且使獲取到的路徑質(zhì)量更優(yōu)。

1 相關(guān)算法

1.1 RRT*算法

RRT*算法利用獲取到的隨機(jī)采樣點(diǎn)構(gòu)建出鄰近區(qū)域節(jié)點(diǎn)分布圖，查詢(xún)周?chē)?jié)點(diǎn)是否存在一條比現(xiàn)有路徑總代價(jià)值更小的路徑，如果存在，則利用代價(jià)更小的路徑將現(xiàn)有路徑替換掉。通過(guò)上述尋路過(guò)程不斷對(duì)隨機(jī)樹(shù)進(jìn)行優(yōu)化，從而獲取漸進(jìn)最優(yōu)解。

如圖1 所示，節(jié)點(diǎn)xstart為隨機(jī)樹(shù)的根節(jié)點(diǎn)。RRT*算法在每次迭代開(kāi)始前，從給定的無(wú)障礙狀態(tài)空間內(nèi)，利用隨機(jī)采樣函數(shù)獲取隨機(jī)采樣點(diǎn)xrand。對(duì)于隨機(jī)樹(shù)上滿足條件的節(jié)點(diǎn)，利用代價(jià)函數(shù)篩選出距離節(jié)點(diǎn)xrand最近的節(jié)點(diǎn)xnearest。代價(jià)函數(shù)公式為：

其中，(x1,y1)和(x2,y2)分別表示當(dāng)前計(jì)算的兩個(gè)節(jié)點(diǎn)的坐標(biāo)。

圖1 最近節(jié)點(diǎn)的篩選Fig.1 Screening of the most recent nodes

如圖2 所示，節(jié)點(diǎn)xnearest沿著節(jié)點(diǎn)xrand方向延伸步長(zhǎng)Step 獲取到節(jié)點(diǎn)xnew，當(dāng)節(jié)點(diǎn)xnew在無(wú)障礙狀態(tài)空間Xfree內(nèi)，并且節(jié)點(diǎn)xnearest和節(jié)點(diǎn)xnew的連線不與障礙物發(fā)生碰撞時(shí)，則稱(chēng)節(jié)點(diǎn)xnew是可取的。同時(shí)，以節(jié)點(diǎn)xnew為球心，創(chuàng)建一個(gè)半徑為r的球形空間，如圖2 虛線所示，并將封閉球形空間Bxnew,r內(nèi)所有隨機(jī)樹(shù)上的節(jié)點(diǎn)作為節(jié)點(diǎn)xnew的可能父節(jié)點(diǎn)加入到鄰近節(jié)點(diǎn)集合Xnear中。球形空間Bxnew,r的公式為：

其中，i為鄰近節(jié)點(diǎn)的個(gè)數(shù)，d為維度數(shù)，γ(d)為常數(shù)。

圖2 鄰近節(jié)點(diǎn)的篩選Fig.2 Screening of neighboring nodes

如果鄰近節(jié)點(diǎn)集合Xnear為空集，則從隨機(jī)樹(shù)中篩選出距離節(jié)點(diǎn)xnew最近的節(jié)點(diǎn)添加到集合Xnear中，從而確保后續(xù)隨機(jī)樹(shù)的擴(kuò)展。集合Xnear內(nèi)節(jié)點(diǎn)的計(jì)算可分為重寫(xiě)父節(jié)點(diǎn)和隨機(jī)樹(shù)重布線兩個(gè)部分：

(1) 重寫(xiě)父節(jié)點(diǎn)

π=表示一條無(wú)循環(huán)的從節(jié)點(diǎn)n0到達(dá)節(jié)點(diǎn)nk，且經(jīng)過(guò)節(jié)點(diǎn)n1的路徑。如圖3(a)所示，將節(jié)點(diǎn)xnew和集合Xnear內(nèi)的所有節(jié)點(diǎn)連接起來(lái)，并分別計(jì)算路 徑 、 和的總代價(jià)值。如圖3(b)所示，若路徑的總代價(jià)值最小，則設(shè)節(jié)點(diǎn)xnearest為節(jié)點(diǎn)xnew的父節(jié)點(diǎn)，并且將路徑添加到隨機(jī)樹(shù)上。

圖3 父節(jié)點(diǎn)重寫(xiě)Fig.3 Override parent node

(2) 隨機(jī)樹(shù)重布線

如圖4(a)所示，將節(jié)點(diǎn)xnew和集合Xnear內(nèi)的節(jié)點(diǎn)x1、節(jié)點(diǎn)x2連接起來(lái)，并分別計(jì)算路徑、、和的總代價(jià)值，篩選出總代價(jià)值最小的路徑。如圖4(b)所示，若路徑的總代價(jià)值最小，則刪除路徑，同時(shí)將路徑添加到隨機(jī)樹(shù)上。隨機(jī)樹(shù)重布線的目的在于當(dāng)隨機(jī)樹(shù)上添加新的節(jié)點(diǎn)后，從鄰近節(jié)點(diǎn)集合內(nèi)篩選出總代價(jià)值最小的路徑替代已有路徑，從而有效減小最終路徑的總代價(jià)值。

圖4 隨機(jī)樹(shù)重布線Fig.4 Rewiring the random tree

1.2 JPS 算法

JPS 算法[11]是Harabor 等學(xué)者提出的一種啟發(fā)式路徑規(guī)劃算法。該算法在路徑規(guī)劃過(guò)程中，放棄對(duì)大部分無(wú)用節(jié)點(diǎn)的計(jì)算，只對(duì)小部分特殊節(jié)點(diǎn)進(jìn)行計(jì)算，從而有效提升了路徑規(guī)劃的速度。由于JPS 算法尋路策略的局限性，將會(huì)導(dǎo)致該算法的全局搜索能力較弱，從而搜索到的最終路徑可能不是最優(yōu)路徑。

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

RRT*算法在路徑尋優(yōu)時(shí)雖然具有漸進(jìn)最優(yōu)性，但是在實(shí)際應(yīng)用中依舊存在資源消耗大、路徑平滑度較低等缺點(diǎn)。本文針對(duì)上述問(wèn)題，以RRT*算法為基礎(chǔ)，提出了一種目標(biāo)點(diǎn)路徑尋優(yōu)策略；同時(shí)，依據(jù)JPS算法的尋路策略，提出了一種跳點(diǎn)漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)(Jump Point Rapidly-exploring Random Tree*,JPRRT*)算法。

2.1 目標(biāo)點(diǎn)路徑尋優(yōu)策略

RRT*算法為了保證尋路時(shí)的全局搜索能力，獲取到的隨機(jī)采樣點(diǎn)可能是整個(gè)無(wú)障礙物狀態(tài)空間內(nèi)的任意節(jié)點(diǎn)。如果當(dāng)前位置和目標(biāo)位置之間的環(huán)境復(fù)雜度較低時(shí)，進(jìn)行盲目的隨機(jī)樹(shù)擴(kuò)展將會(huì)降低路徑規(guī)劃的效率和浪費(fèi)內(nèi)存資源。本文針對(duì)上述問(wèn)題，提出了一種目標(biāo)點(diǎn)路徑尋優(yōu)策略。

2.1.1 相關(guān)定義

如圖5 所示，節(jié)點(diǎn)p為起始節(jié)點(diǎn)，節(jié)點(diǎn)G為目標(biāo)節(jié)點(diǎn)，此時(shí)節(jié)點(diǎn)p和節(jié)點(diǎn)G之間的環(huán)境復(fù)雜度較低，實(shí)線為節(jié)點(diǎn)p到節(jié)點(diǎn)G的一條最短路徑。在這種情況下，繼續(xù)進(jìn)行隨機(jī)樹(shù)擴(kuò)展也無(wú)法找到一條比實(shí)線總代價(jià)值更小的路徑，此時(shí)稱(chēng)路徑為目標(biāo)點(diǎn)路徑。改進(jìn)算法在尋路初期進(jìn)行目標(biāo)點(diǎn)路徑尋優(yōu)，若存在目標(biāo)點(diǎn)路徑，則結(jié)束算法尋路，并將該路徑設(shè)為最優(yōu)路徑；反之則認(rèn)為不存在目標(biāo)點(diǎn)路徑，開(kāi)始進(jìn)行后續(xù)隨機(jī)樹(shù)擴(kuò)展。

圖5 目標(biāo)點(diǎn)路徑示意圖Fig.5 Target point path diagram

本文對(duì)目標(biāo)點(diǎn)路徑尋優(yōu)策略作如下定義：

定義1利用起始節(jié)點(diǎn)p和目標(biāo)節(jié)點(diǎn)G構(gòu)成路徑規(guī)劃區(qū)域Map(p,G)，后續(xù)獲取的隨機(jī)采樣點(diǎn)N必須在此區(qū)域內(nèi)。如圖6 所示，其中白色柵格為路徑規(guī)劃區(qū)域Map(p,G)，灰色柵格在Map(p,G)外，不對(duì)其中節(jié)點(diǎn)進(jìn)行計(jì)算。

定義2若節(jié)點(diǎn)p1為節(jié)點(diǎn)p的后續(xù)擴(kuò)展節(jié)點(diǎn)，則舍去舊的路徑規(guī)劃區(qū)域Map(p,G)，并利用節(jié)點(diǎn)p1和目標(biāo)節(jié)點(diǎn)G構(gòu)成新的路徑規(guī)劃區(qū)域Map(p1,G)，后續(xù)的隨機(jī)采樣點(diǎn)N1必須在區(qū)域Map(p1,G)中。如圖7 所示，白色柵格為路徑規(guī)劃區(qū)域Map(p1,G)，灰色區(qū)域內(nèi)的節(jié)點(diǎn)不需要進(jìn)行計(jì)算。循環(huán)執(zhí)行上述尋路過(guò)程，直至擴(kuò)展到目標(biāo)節(jié)點(diǎn)G，則結(jié)束算法尋路，若當(dāng)前區(qū)域無(wú)法獲取隨機(jī)采樣點(diǎn)或當(dāng)前節(jié)點(diǎn)無(wú)法獲取后續(xù)擴(kuò)展節(jié)點(diǎn)，則稱(chēng)無(wú)目標(biāo)點(diǎn)路徑，開(kāi)始后續(xù)隨機(jī)樹(shù)擴(kuò)展。

圖6 初始路徑規(guī)劃區(qū)域構(gòu)建Fig.6 Initial path planning area construction

圖7 后續(xù)路徑規(guī)劃區(qū)域構(gòu)建Fig.7 Subsequent path planning area construction

定義3若隨機(jī)樹(shù)上最新擴(kuò)展節(jié)點(diǎn)p和目標(biāo)節(jié)點(diǎn)G的連線為水平方向或垂直方向，此時(shí)需要判斷路徑是否合理，即該路徑上是否存在障礙物節(jié)點(diǎn)，若存在障礙物節(jié)點(diǎn)，則稱(chēng)該路徑不合理，繼續(xù)進(jìn)行隨機(jī)樹(shù)擴(kuò)展；反之則稱(chēng)該路徑合理，結(jié)束路徑尋優(yōu)，并輸出搜索到的完整路徑。如圖8 所示，黑色柵格為障礙物節(jié)點(diǎn)，節(jié)點(diǎn)p和目標(biāo)節(jié)點(diǎn)G的連線為水平方向，由于路徑上存在障礙物節(jié)點(diǎn)，則該路徑不合理，繼續(xù)進(jìn)行隨機(jī)樹(shù)擴(kuò)展。

圖8 路徑合理性判斷規(guī)則Fig8 Path rationality judgment rules

2.1.2 目標(biāo)點(diǎn)路徑尋優(yōu)過(guò)程

如圖9(a)所示，節(jié)點(diǎn)p為起始節(jié)點(diǎn)，節(jié)點(diǎn)G為目標(biāo)節(jié)點(diǎn)，則當(dāng)前路徑規(guī)劃區(qū)域?yàn)镸ap(p,G)，后續(xù)隨機(jī)采樣點(diǎn)N必須在此區(qū)域內(nèi)；如圖9(b)所示，節(jié)點(diǎn)p1為節(jié)點(diǎn)p的后續(xù)擴(kuò)展節(jié)點(diǎn)，新的路徑規(guī)劃區(qū)域?yàn)镸ap(p1,G)，灰色區(qū)域內(nèi)的節(jié)點(diǎn)不需要對(duì)其計(jì)算，新的隨機(jī)采樣點(diǎn)N1在區(qū)域Map(p1,G)內(nèi)；如圖9(c)所示，節(jié)點(diǎn)p2為節(jié)點(diǎn)p1的后續(xù)擴(kuò)展節(jié)點(diǎn)，由定義3 可知，當(dāng)節(jié)點(diǎn)p2和目標(biāo)節(jié)點(diǎn)G的連線為水平方向時(shí)，此時(shí)需要判斷路徑是否合理；如圖9(d)所示，由于路徑上無(wú)障礙物節(jié)點(diǎn)，即路徑是合理的，此時(shí)說(shuō)明算法搜索到了一條最優(yōu)路徑，結(jié)束路徑尋優(yōu)，并輸出最終路徑。

圖9 目標(biāo)點(diǎn)路徑規(guī)劃Fig.9 Target point path planning

2.2 基于JPS 的節(jié)點(diǎn)擴(kuò)展策略

RRT*算法每次迭代開(kāi)始時(shí)，首先需要獲取隨機(jī)采樣點(diǎn)xrand，并且篩選出隨機(jī)樹(shù)中距離節(jié)點(diǎn)xrand最近的節(jié)點(diǎn)xnearest，最后進(jìn)行步長(zhǎng)延伸獲取新的節(jié)點(diǎn)xnew，該過(guò)程將導(dǎo)致算法尋路時(shí)計(jì)算節(jié)點(diǎn)過(guò)多，從而出現(xiàn)路徑收斂速度慢、內(nèi)存消耗大等問(wèn)題。針對(duì)上述問(wèn)題，本文提出了一種引入了JPS 搜索策略的RRT*算法，減少了算法尋路時(shí)計(jì)算的節(jié)點(diǎn)數(shù)量，其方法描述如下：

JPRRT*算法在每次迭代過(guò)程中獲取到隨機(jī)采樣點(diǎn)xrand和最近節(jié)點(diǎn)xnearest后，將根據(jù)擴(kuò)展方向的篩選規(guī)則計(jì)算出隨機(jī)樹(shù)擴(kuò)展方向，并驅(qū)使節(jié)點(diǎn)xnearest沿著擴(kuò)展方向延伸一定步長(zhǎng)，當(dāng)搜索到跳點(diǎn)時(shí)，則將跳點(diǎn)設(shè)為新的擴(kuò)展節(jié)點(diǎn)xnew。若算法達(dá)到最大迭代次數(shù)Nmax或完整路徑總代價(jià)值在N次迭代后沒(méi)有變化，則結(jié)束路徑尋優(yōu)，并輸出算法最優(yōu)解。

2.2.1 擴(kuò)展方向的篩選規(guī)則

JPRRT*算法由于只對(duì)跳點(diǎn)進(jìn)行計(jì)算，隨機(jī)樹(shù)上的節(jié)點(diǎn)大大減少，從而在隨機(jī)樹(shù)擴(kuò)展過(guò)程中，可能會(huì)出現(xiàn)多個(gè)隨機(jī)采樣點(diǎn)的最近節(jié)點(diǎn)一致，該情況可能會(huì)導(dǎo)致最近節(jié)點(diǎn)的擴(kuò)展方向被重復(fù)擴(kuò)展。針對(duì)上述問(wèn)題，隨機(jī)樹(shù)擴(kuò)展方向的篩選規(guī)則分為以下兩種情況：

(1) 擴(kuò)展方向未被計(jì)算

如圖10 所示，節(jié)點(diǎn)N為隨機(jī)采樣點(diǎn)，節(jié)點(diǎn)p為隨機(jī)樹(shù)上距離節(jié)點(diǎn)N最近的節(jié)點(diǎn)。假設(shè)任意節(jié)點(diǎn)均有八個(gè)擴(kuò)展方向，此時(shí)節(jié)點(diǎn)p的擴(kuò)展方向?yàn)楣?jié)點(diǎn)3，灰色鄰節(jié)點(diǎn)不需要計(jì)算，并且在此次擴(kuò)展結(jié)束后，將節(jié)點(diǎn)p的鄰節(jié)點(diǎn)3 設(shè)為已擴(kuò)展方向，在后續(xù)擴(kuò)展方向的篩選中，將不再對(duì)節(jié)點(diǎn)p的鄰節(jié)點(diǎn)3 進(jìn)行計(jì)算。

圖10 擴(kuò)展方向未被計(jì)算Fig.10 The extension direction is not calculated

擴(kuò)展方向的節(jié)點(diǎn)公式為：

其中，(Nx,Ny)、(px,py)和(xx,xy)分別表示隨機(jī)采樣點(diǎn)N、最近節(jié)點(diǎn)p和擴(kuò)展方向的節(jié)點(diǎn)x的坐標(biāo)。

(2) 擴(kuò)展方向已被計(jì)算

如圖11 所示，節(jié)點(diǎn)N1為新的隨機(jī)采樣點(diǎn)，節(jié)點(diǎn)p為隨機(jī)樹(shù)上距離節(jié)點(diǎn)N1的最近節(jié)點(diǎn)。假設(shè)節(jié)點(diǎn)p的鄰節(jié)點(diǎn)3 已經(jīng)被計(jì)算，需要重新篩選擴(kuò)展方向，此時(shí)將從節(jié)點(diǎn)p的鄰節(jié)點(diǎn)中篩選出距離節(jié)點(diǎn)N1最近的兩個(gè)節(jié)點(diǎn)作為待擴(kuò)展節(jié)點(diǎn)，并從待擴(kuò)展節(jié)點(diǎn)中隨機(jī)篩選一個(gè)節(jié)點(diǎn)作為擴(kuò)展方向的節(jié)點(diǎn)。由圖11 可以看出，節(jié)點(diǎn)p的所有鄰節(jié)點(diǎn)中，節(jié)點(diǎn)2 和節(jié)點(diǎn)6 滿足待擴(kuò)展節(jié)點(diǎn)的條件，然后從節(jié)點(diǎn)2 和節(jié)點(diǎn)6 中隨機(jī)篩選出一個(gè)節(jié)點(diǎn)作為擴(kuò)展節(jié)點(diǎn)x。

圖11 擴(kuò)展方向已被計(jì)算Fig.11 The extension direction has been calculated

2.2.2 鄰節(jié)點(diǎn)篩選規(guī)則

計(jì)算出擴(kuò)展節(jié)點(diǎn)x后，設(shè)節(jié)點(diǎn)p為節(jié)點(diǎn)x的父節(jié)點(diǎn)，并利用鄰節(jié)點(diǎn)篩選規(guī)則進(jìn)行節(jié)點(diǎn)預(yù)處理。鄰節(jié)點(diǎn)篩選規(guī)則可分為以下兩種情況：

(1) 無(wú)障礙物毗鄰節(jié)點(diǎn)

如圖12 所示，節(jié)點(diǎn)x的鄰節(jié)點(diǎn)均為可移動(dòng)節(jié)點(diǎn)，且任意柵格的移動(dòng)代價(jià)是一致的。由于始終存在從節(jié)點(diǎn)p到任意灰色節(jié)點(diǎn)代價(jià)值最小的路徑，且該路徑不包含節(jié)點(diǎn)x，則稱(chēng)節(jié)點(diǎn)x的灰色鄰節(jié)點(diǎn)為無(wú)用節(jié)點(diǎn)，不需要對(duì)其進(jìn)行計(jì)算。

對(duì)于直線方向移動(dòng)情況，裁剪所有滿足下式的節(jié)點(diǎn)n(n∈neighbours(x))：

其中，x表示節(jié)點(diǎn)x不會(huì)出現(xiàn)在路徑上，即該路徑不經(jīng)過(guò)節(jié)點(diǎn)x；len()函數(shù)表示路徑代價(jià)值，該函數(shù)表示當(dāng)前計(jì)算的兩個(gè)節(jié)點(diǎn)之間的距離。

對(duì)于對(duì)角線方向移動(dòng)情況，不經(jīng)過(guò)節(jié)點(diǎn)x路徑的長(zhǎng)度要絕對(duì)短于經(jīng)過(guò)節(jié)點(diǎn)x的路徑，公式為：

圖12 無(wú)障礙物毗鄰節(jié)點(diǎn)Fig.12 There are no obstacles nodes around the current node

(2) 有障礙物毗鄰節(jié)點(diǎn)

如圖13所示，節(jié)點(diǎn)x周?chē)嬖诓豢尚凶叩墓?jié)點(diǎn)時(shí)，由無(wú)障礙物毗鄰節(jié)點(diǎn)定義可知，灰色節(jié)點(diǎn)無(wú)需計(jì)算。由于節(jié)點(diǎn)p到節(jié)點(diǎn)n代價(jià)值最小的路徑，必然經(jīng)過(guò)節(jié)點(diǎn)x，則稱(chēng)節(jié)點(diǎn)n為強(qiáng)迫鄰節(jié)點(diǎn)。

強(qiáng)迫鄰節(jié)點(diǎn)的篩選公式為：

圖13 有障礙物毗鄰節(jié)點(diǎn)Fig.13 There are obstacles nodes around the current node

2.2.3 跳點(diǎn)的篩選

如果存在最小值k，使得跳點(diǎn)公式成立，且滿足任意一條下列條件，則稱(chēng)節(jié)點(diǎn)y為節(jié)點(diǎn)x的方向跳點(diǎn)[12]。跳點(diǎn)公式為：

1) 節(jié)點(diǎn)y為目標(biāo)節(jié)點(diǎn)；

2) 節(jié)點(diǎn)y擁有一個(gè)或一個(gè)以上的強(qiáng)迫鄰節(jié)點(diǎn)；

3) 若為對(duì)角線方向，同時(shí)節(jié)點(diǎn)z能夠使得即從節(jié)點(diǎn)y以方向移動(dòng)步到達(dá)節(jié)點(diǎn)z，則稱(chēng)節(jié)點(diǎn)z為節(jié)點(diǎn)y的跳點(diǎn)。

3 仿真及實(shí)驗(yàn)驗(yàn)證

3.1 仿真驗(yàn)證

為驗(yàn)證改進(jìn)算法的正確性和可行性，本文針對(duì)RRT*算法和JPRRT*算法分別進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)環(huán)境為：Windows10，Intel(R) Core(TM) i7-7700，主頻2.80 GHz，內(nèi)存12 G。實(shí)驗(yàn)平臺(tái)為：Matlab2016b。兩種算法相關(guān)參數(shù)取值為：Nmax= 5000，N= 500，Step= 20。

在仿真平臺(tái)上選取3 種不同的地圖環(huán)境進(jìn)行實(shí)驗(yàn)，如圖14、15 和16 所示。其中地圖尺寸為500×500，左上方為機(jī)器人起始位置，右下方為目標(biāo)位置，黑色區(qū)域?yàn)檎系K物柵格，白色區(qū)域?yàn)闄C(jī)器人可行走柵格，黑色折線為兩種算法規(guī)劃出的最終路徑，黑色虛線為路徑尋優(yōu)所得到的隨機(jī)樹(shù)。由圖14、15 和16 可以得出，RRT*算法獲取到的路徑平滑度較低，且存在局部最優(yōu)問(wèn)題；JPRRT*算法獲取到的路徑平滑度明顯提高，從而更適合移動(dòng)機(jī)器人的運(yùn)動(dòng)。同時(shí)由圖中虛線數(shù)量可以得出，JPRRT*算法尋路擴(kuò)展節(jié)點(diǎn)的數(shù)量遠(yuǎn)少于RRT*算法。3 種地圖仿真實(shí)驗(yàn)數(shù)據(jù)如表1 所示。

圖14 地圖1 下兩種算法實(shí)驗(yàn)路徑對(duì)比Fig.14 The experimental path comparison of the two algorithms under map 1

圖15 地圖2 下兩種算法的實(shí)驗(yàn)路徑對(duì)比Fig.15 The experimental path comparison of the two algorithms under map 2

圖16 地圖3 下兩種算法的實(shí)驗(yàn)路徑對(duì)比Fig.16 The experimental path comparison of the two algorithms under map 3

表1 兩種算法仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比Tab.1 Simulation experimental data of the two algorithms

由表1 可以看出，JPRRT*算法所規(guī)劃出的路徑長(zhǎng)度、收斂時(shí)間和擴(kuò)展節(jié)點(diǎn)數(shù)量均要優(yōu)于RRT*算法，同時(shí)路徑的轉(zhuǎn)折點(diǎn)數(shù)量更少，從而路徑更加光滑，并且隨著柵格地圖復(fù)雜度的提升，JPRRT*算法的優(yōu)越性越明顯。

3.2 實(shí)驗(yàn)驗(yàn)證

本文利用實(shí)際的基于機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)的路徑規(guī)劃實(shí)驗(yàn)結(jié)果來(lái)證明改進(jìn)算法的優(yōu)越性和可行性。移動(dòng)機(jī)器人實(shí)物如圖17所示。實(shí)驗(yàn)平臺(tái)采取兩輪差分驅(qū)動(dòng)，其中兩個(gè)前輪為萬(wàn)向輪，兩個(gè)后輪為驅(qū)動(dòng)輪。

圖17 實(shí)驗(yàn)所用移動(dòng)機(jī)器人Fig.17 Mobile robot for experiment

本文選擇實(shí)驗(yàn)室作為測(cè)試環(huán)境，先根據(jù)激光雷達(dá)信息和里程計(jì)信息使用gmapping 對(duì)環(huán)境地圖進(jìn)行構(gòu)建，并利用構(gòu)建好的柵格地圖，在相同環(huán)境下對(duì)兩種算法進(jìn)行路徑規(guī)劃實(shí)驗(yàn)。兩種算法相關(guān)參數(shù)取值為：Nmax= 10000，N= 1000，Step= 1。兩種算法實(shí)驗(yàn)結(jié)果如圖18 和圖19 所示。

由圖18 和圖19 可以得出，RRT*算法獲取到的路徑平滑度較低，將會(huì)影響移動(dòng)機(jī)器人在導(dǎo)航和控制過(guò)程中運(yùn)動(dòng)的平穩(wěn)性，甚至出現(xiàn)卡頓情況，從而降低移動(dòng)機(jī)器人的導(dǎo)航效率和使用壽命；JPRRT*算法獲取到的路徑平滑度大大提升，從而使路徑質(zhì)量更優(yōu)、更有利于移動(dòng)機(jī)器人的導(dǎo)航和控制。

圖18 目標(biāo)點(diǎn)1 下兩種算法的實(shí)驗(yàn)路徑對(duì)比Fig.18 The experimental path of the two algorithms is compared under target point 1

圖19 目標(biāo)點(diǎn)2 下兩種算法的實(shí)驗(yàn)路徑對(duì)比Fig.19 The experimental path of the two algorithms is compared under target point 2

表2 目標(biāo)點(diǎn)1 下兩種算法實(shí)驗(yàn)數(shù)據(jù)對(duì)比Tab.2 The experimental data of the two algorithms are compared under target point 1

表3 目標(biāo)點(diǎn)2 下兩種算法實(shí)驗(yàn)數(shù)據(jù)對(duì)比Tab.3 The experimental data of the two algorithms are compared under target point 2

表2 和表3 為RRT*算法和JPRRT*算法在不同目標(biāo)點(diǎn)下的實(shí)驗(yàn)數(shù)據(jù)對(duì)比。由表2 可以得出，RRT*算法和JPRRT*算法在目標(biāo)點(diǎn)1 下的尋路時(shí)間分別為154.1 ms 和114.3 ms，JPRRT*算法的路徑規(guī)劃效率提高了約25.83%。由表3 可以得出，RRT*算法和JPRRT*算法在目標(biāo)點(diǎn)2 下的尋路時(shí)間分別為76.0 ms 和59.4 ms，JPRRT*算法的路徑規(guī)劃效率提高了約21.84%。

綜上所述，相比于RRT*算法，JPRRT*算法的收斂速度更快、路徑規(guī)劃效率更高，且獲取到的路徑質(zhì)量更優(yōu)。

4 結(jié) 論

本文針對(duì)RRT*算法在移動(dòng)機(jī)器人路徑規(guī)劃任務(wù)中存在的收斂速度慢、內(nèi)存消耗大、路徑平滑度較低等缺點(diǎn)，提出了一種目標(biāo)點(diǎn)路徑搜索策略，并且依據(jù)JPS 尋路策略，提出了一種改進(jìn)的RRT*算法。仿真結(jié)果表明，JPRRT*算法不僅加快了路徑規(guī)劃時(shí)的收斂速度、降低了內(nèi)存消耗，并且搜索到的路徑要優(yōu)于RRT*算法。最后將兩種算法在相同環(huán)境下進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，結(jié)果證明JPRRT*算法是一種有效、可行的改進(jìn)算法，并且尋路效率要高于RRT*算法。