朱宏輝，王嘉豪

(武漢理工大學(xué) 物流工程學(xué)院,武漢 430063)

0 引言

輪式機(jī)器人是通過驅(qū)動(dòng)電機(jī)帶動(dòng)輪子轉(zhuǎn)動(dòng)來使得機(jī)器人發(fā)生移動(dòng)行為的機(jī)器人，由于其具有結(jié)構(gòu)簡單、控制方便、模型易建、承重大、自重輕、行走速度快等特點(diǎn)，在生活、物流、交通等領(lǐng)域應(yīng)用最為廣泛。路徑規(guī)劃是移動(dòng)機(jī)器人完成各種功能的基礎(chǔ)，具備路徑規(guī)劃能力的移動(dòng)機(jī)器人才真正具備實(shí)用性。機(jī)器人路徑規(guī)劃是在已知自身位置和目標(biāo)點(diǎn)或者目標(biāo)區(qū)域的條件下，在機(jī)器人運(yùn)動(dòng)空間尋找一條能夠無碰撞抵達(dá)目標(biāo)點(diǎn)或者區(qū)域的線路[1-3]。路徑規(guī)劃能夠極大地?cái)U(kuò)展移動(dòng)機(jī)器人工作空間，提高移動(dòng)機(jī)器人的實(shí)際應(yīng)用價(jià)值。

傳統(tǒng)的路徑規(guī)劃算法有：Dijkstra算法、A*算法、蟻群算法、粒子群算法等。Dijkstra算法簡單，但搜索速率較慢，耗時(shí)較長。A*算法通過啟發(fā)式圖搜索方式尋找起始點(diǎn)到終點(diǎn)之間的最低代價(jià)路徑，降低了算法的復(fù)雜度，提高了算法效率，但算法對(duì)啟發(fā)式函數(shù)選取較為嚴(yán)苛，且隨著場(chǎng)景規(guī)模的增大，算法運(yùn)行空間呈指數(shù)增長。蟻群算法與粒子群算法等仿生學(xué)路徑規(guī)劃算法易與其他算法結(jié)合，但算法運(yùn)行效率低，不能滿足移動(dòng)機(jī)器人實(shí)時(shí)性要求，且容易陷入局部最小值。這些傳統(tǒng)算法效率低下，求解困難，且無法應(yīng)用于高維復(fù)雜空間[4-5]。

1998年，LaValle等提出快速擴(kuò)展隨機(jī)樹(rapidly-exploring random tree,RRT)算法[6]，它通過不斷在狀態(tài)空間隨機(jī)性選取樣本點(diǎn)并進(jìn)行選擇來規(guī)劃一條可行路徑[7]。RRT算法無需對(duì)狀態(tài)空間顯式建模，規(guī)劃速度快且能夠考慮機(jī)器人的各種約束，這種算法能夠很好解決復(fù)雜環(huán)境下的路徑規(guī)劃問題。相比其他算法而言，RRT算法適用于高維復(fù)雜空間的路徑規(guī)劃問題，且算法路徑搜索效率更高。不過，RRT 算法也存在一些缺陷，如收斂速度緩慢、路徑節(jié)點(diǎn)冗余等[8-9]。2000年，Kuffner和LaValle等提出了RRT-connect算法，提高了節(jié)點(diǎn)的擴(kuò)展效率，并于第二年提出雙向搜索樹(Bidirenctional-RRT)算法，由起點(diǎn)和終點(diǎn)同時(shí)延伸搜索樹，尋找最優(yōu)路徑。2011年,Karaman等[10]提出RRT*算法，通過優(yōu)化節(jié)點(diǎn)擴(kuò)展方法使得算法具備概率完備性，隨著采樣點(diǎn)的不斷增加，總能在空間中尋找一條由起始點(diǎn)到可到達(dá)點(diǎn)的路徑。同時(shí)，RRT*算法那還具備漸進(jìn)最優(yōu)性，通過增加采樣點(diǎn)逐步優(yōu)化規(guī)劃的路徑，但是這種算法在復(fù)雜場(chǎng)景下冗余節(jié)點(diǎn)較多，且收斂時(shí)間較長。2013年，M.Jordan和A.Perez提出了將雙向擴(kuò)展的思想同RRT*算法結(jié)合的B-RRT*(Bidirenctional-RRT*)算法，加快了算法的收斂速度，但是算法在擴(kuò)展過程中缺乏方向性并且規(guī)劃路徑距離障礙物過近[11]。2015年Qureshi A H等人使用智能樣本插入函數(shù)使得算法快速收斂到最佳路徑[12]。

針對(duì)RRT*算法在室內(nèi)應(yīng)用場(chǎng)景下規(guī)劃路徑存在較多冗余路徑點(diǎn)導(dǎo)致移動(dòng)機(jī)器人移動(dòng)過程中出現(xiàn)多次停頓及轉(zhuǎn)向的問題，本文提出一種基于局部逆序試連的路徑優(yōu)化策略的改進(jìn)RRT*算法，通過局部逆序試連法剔除冗余路徑點(diǎn)，縮短了規(guī)劃路徑長度，且提高了算法運(yùn)行速度，避免了實(shí)際應(yīng)用中冗余路徑點(diǎn)導(dǎo)致機(jī)器人在轉(zhuǎn)彎過程中出現(xiàn)多次停頓和換向現(xiàn)象。

1 RRT*算法原理

RRT* 算法是在RRT算法基礎(chǔ)上進(jìn)行了相應(yīng)的改進(jìn)，而使其保留算法概率完備性的同時(shí)，還具備漸近最優(yōu)性的顯著特征[13]。RRT算法雖然提升了路徑搜索效率，但是由于節(jié)點(diǎn)擴(kuò)展過程中的隨機(jī)采樣，導(dǎo)致路徑規(guī)劃過程的不確定性，算法運(yùn)行效率時(shí)高時(shí)低。相同起始點(diǎn)和終點(diǎn)的條件下規(guī)劃路徑差別較大，可復(fù)用性差，同時(shí)規(guī)劃路徑往往與最佳路徑偏差較大。由于RRT算法獲取的最終路徑并非全局最優(yōu)路徑，Karaman和Frazzoli提出RRT*算法來解決這個(gè)問題[14]。RRT*算法同樣采用隨機(jī)采樣方式擴(kuò)展隨機(jī)樹，但其在新產(chǎn)生路徑節(jié)點(diǎn)附近尋找更優(yōu)的父節(jié)點(diǎn)，以保證從起點(diǎn)到新路徑點(diǎn)的路徑代價(jià)為最小，然后對(duì)隨機(jī)樹進(jìn)行重布線。RRT*算法通過尋找最優(yōu)父節(jié)點(diǎn)和隨機(jī)樹重布線來不斷優(yōu)化路徑，使得整個(gè)路徑規(guī)劃過程漸進(jìn)優(yōu)化。算法的實(shí)現(xiàn)原理如下：

RRT*算法尋找節(jié)點(diǎn)的方法如下：首先將起點(diǎn)Xinit作為根節(jié)點(diǎn)放入隨機(jī)樹中，構(gòu)建初始隨機(jī)樹T1。然后，通過隨機(jī)函數(shù)在安全區(qū)域Cfree內(nèi)獲取一個(gè)隨機(jī)點(diǎn)Xrand，并在隨機(jī)樹中尋找與Xrand距離最近的節(jié)點(diǎn)Xnearest。令步長為ρ，在Xnearest與Xrand連線之間尋找一點(diǎn)Xnew，使得Xnearest與Xnew之間的歐幾里得距離為ρ。如果Xnearest與Xnew連線沒有與障礙區(qū)域Cobs相交，則將Xnew作為新的路徑節(jié)點(diǎn)放入隨機(jī)樹T中，形成新的隨機(jī)樹。重復(fù)上述過程，隨機(jī)樹擴(kuò)展過程如圖1所示。

圖1 隨機(jī)樹節(jié)點(diǎn)擴(kuò)展

圖2 隨機(jī)樹擴(kuò)展k次后

圖3 選擇最佳父節(jié)點(diǎn)

圖4 隨機(jī)樹重布線

重復(fù)上述隨機(jī)樹擴(kuò)展和優(yōu)化過程，直到新的路徑節(jié)點(diǎn)Xnew與目標(biāo)路徑點(diǎn)Xgoal距離在允許范圍內(nèi)，迭代終止。每一次迭代過程中，若隨機(jī)樹重連后任一路徑節(jié)點(diǎn)連線與障礙區(qū)域相交，則放棄隨機(jī)樹重連，直接進(jìn)入下一次迭代過程。

原始RRT*算法偽代碼如表1所示，由原理分析可知算法存如下問題：采樣過程中在全局區(qū)域隨機(jī)生成采樣點(diǎn)，導(dǎo)致在生成路徑中存在大量冗余路徑節(jié)點(diǎn)，尤其在轉(zhuǎn)向部分路徑。

表1 RRT*算法偽代碼

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

RRT*算法通過不斷尋找最佳父節(jié)點(diǎn)和隨機(jī)樹重連來逐漸優(yōu)化路徑，但是因?yàn)樵诘^程中對(duì)全局狀態(tài)空間進(jìn)行采樣，產(chǎn)生了大量沒有必要的節(jié)點(diǎn)，特別是在轉(zhuǎn)彎部分的路徑，如圖5所示。產(chǎn)生的冗余節(jié)點(diǎn)不僅降低了算法的收斂速度，還導(dǎo)致算法在實(shí)際應(yīng)用中使得機(jī)器人在運(yùn)動(dòng)拐角處產(chǎn)生多次停頓和換向運(yùn)動(dòng)，降低了機(jī)器人運(yùn)動(dòng)的流暢度。

圖5 規(guī)劃路徑中的冗余節(jié)點(diǎn)

在實(shí)際應(yīng)用場(chǎng)景中，由于RRT*算法規(guī)劃的路徑中在障礙物附近的拐角處產(chǎn)生了許多冗余節(jié)點(diǎn)，導(dǎo)致移動(dòng)機(jī)器人在拐角附近運(yùn)動(dòng)時(shí)，產(chǎn)生多次停頓并換向。這種現(xiàn)象導(dǎo)致移動(dòng)機(jī)器人的運(yùn)動(dòng)流暢性差，同時(shí)降低了機(jī)器人的運(yùn)動(dòng)效率。同時(shí)，移動(dòng)機(jī)器人頻繁停頓和轉(zhuǎn)向產(chǎn)生的機(jī)械沖擊將增加移動(dòng)機(jī)器人各機(jī)械部件的性能損耗，縮短了機(jī)器人的機(jī)械壽命。為避免這種現(xiàn)象出現(xiàn)，本文提出一種局部逆序試連的方法對(duì)RRT*算法規(guī)劃的路徑進(jìn)行優(yōu)化，通過局部逆序試連法剔除規(guī)劃路徑中的冗余路徑節(jié)點(diǎn)，從而縮短規(guī)劃路徑的長度，使得轉(zhuǎn)向部分路徑更加平滑。在實(shí)際應(yīng)用場(chǎng)景中減少移動(dòng)機(jī)器人停頓和轉(zhuǎn)向的次數(shù)，可以提高移動(dòng)機(jī)器人的運(yùn)動(dòng)流暢性，延長機(jī)器人機(jī)械壽命，縮短規(guī)劃路徑，算法流程如下：

圖6 局部逆序試連

4)將得到的更新節(jié)點(diǎn)集合替換原始的節(jié)點(diǎn)集合，并以更新節(jié)點(diǎn)集合中的節(jié)點(diǎn)替換隨機(jī)樹中原始節(jié)點(diǎn)集合中的節(jié)點(diǎn)，即完成了一次局部路徑平滑。使用下一個(gè)節(jié)點(diǎn)集合替換此時(shí)的節(jié)點(diǎn)集合，重復(fù)上述步驟，進(jìn)行下一次局部路徑平滑。

5)利用更新路徑節(jié)點(diǎn)集合中所有節(jié)點(diǎn)依次替換擴(kuò)展隨機(jī)樹T中與障礙物距離小于等于R的節(jié)點(diǎn)，將更新后的擴(kuò)展隨機(jī)樹T′中的路徑節(jié)點(diǎn)依次連接，即可得到最終的規(guī)劃路徑。

通過引入局部逆序試連路徑優(yōu)化方法，有效剔除了RRT*算法規(guī)劃路徑中的冗余路徑節(jié)點(diǎn)，縮短了規(guī)劃路徑長度。改進(jìn)后的RRT*算法在移動(dòng)機(jī)器人路徑規(guī)劃實(shí)際應(yīng)用中，能夠規(guī)劃出更接近最優(yōu)路徑的路徑，且能夠避免移動(dòng)機(jī)器人在轉(zhuǎn)向時(shí)候出現(xiàn)多次停頓和換向，使得移動(dòng)機(jī)器人運(yùn)動(dòng)更加流暢。

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

為了驗(yàn)證對(duì)原始RRT*算法改進(jìn)的有效性和性能更優(yōu)，首先需要通過仿真軟件進(jìn)行路徑規(guī)劃模擬實(shí)驗(yàn)。仿真實(shí)驗(yàn)平臺(tái)為安裝在64位Windows7系統(tǒng)(電腦配置：Intel Core處理器，主頻3.7 GHz，內(nèi)存32 GB)上的MATLAB 2018a。仿真實(shí)驗(yàn)中在不同狀態(tài)空間條件下分別使用RRT*算法和改進(jìn)后RRT*算法進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，通過分析在相同條件下兩種算法得到的規(guī)劃路徑的對(duì)比結(jié)果，驗(yàn)證算法改進(jìn)的有效性。仿真實(shí)驗(yàn)中模擬地圖大小為1 000×1 000，S表示機(jī)器人起始位置，G表示目標(biāo)位置，黑色陰影區(qū)域表示障礙區(qū)域，白色區(qū)域?yàn)榘踩珔^(qū)域。

RRT*算法的隨機(jī)采樣步長和鄰域半徑?jīng)Q定了算法的收斂速度和路徑的準(zhǔn)確度，適當(dāng)減少搜索步長和鄰域半徑可以提高算法的準(zhǔn)確性，但同時(shí)降低了搜索效率[15]。實(shí)驗(yàn)中將隨機(jī)采樣步長設(shè)置為25，鄰域半徑為50，以使得路徑規(guī)劃實(shí)驗(yàn)效果最佳。在相同狀態(tài)空間條件下，分別使用RRT*算法和改進(jìn)的RRT*算法進(jìn)行20次路徑規(guī)劃實(shí)驗(yàn)。路徑規(guī)劃過程中迭代次數(shù)越多，表示規(guī)劃路徑所用時(shí)間越長[16]，通過對(duì)比兩種算法規(guī)劃路徑所用迭代次數(shù)和規(guī)劃路徑長度可以直觀地比較兩種算法性能優(yōu)劣。

在相同狀態(tài)空間中使用不同的RRT改進(jìn)算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn)，通過對(duì)比不同路徑規(guī)劃算法的仿真實(shí)驗(yàn)結(jié)果，分析比較算法性能，仿真實(shí)驗(yàn)中各算法計(jì)算得到的規(guī)劃路徑如圖7所示。其中圖7為RRT-Connect算法規(guī)劃的路徑，由路徑規(guī)劃結(jié)果可知該算法搜索效率較高，但其規(guī)劃路徑會(huì)出現(xiàn)與理想路徑有較大的偏差的現(xiàn)象。圖8為B-RRT*算法路徑規(guī)劃仿真結(jié)果，其路徑規(guī)劃過程中搜索效率更高，且收斂速度更快，但是路徑在局部區(qū)域容易出現(xiàn)與障礙物貼近的情況，在移動(dòng)機(jī)器人實(shí)際應(yīng)用當(dāng)中容易出現(xiàn)碰撞事故，不適合直接用于室內(nèi)服務(wù)機(jī)器人的路徑規(guī)劃。圖9中RRT*算法規(guī)劃路徑與B-RRT*算法生成的路徑相似，但其生成路徑與障礙物能夠保持一定的距離，更適合移動(dòng)機(jī)器人實(shí)際使用。但是RRT*算法規(guī)劃的局部路徑存在較多冗余路徑點(diǎn)，導(dǎo)致規(guī)劃路徑曲折，同時(shí)收斂速度較慢。通過局部逆序試連方法對(duì)RRT*算法進(jìn)行優(yōu)化，可以得到冗余轉(zhuǎn)折節(jié)點(diǎn)大幅度減少的平滑路徑，并使得路徑縮短，如圖10所示。增加障礙物種類，使用改進(jìn)后的RRT*算法再次規(guī)劃移動(dòng)機(jī)器人運(yùn)動(dòng)路徑，如圖11所示。實(shí)驗(yàn)結(jié)果表明，在增加障礙物數(shù)目和種類的情況下，改進(jìn)后的RRT*算法仍能夠正確規(guī)劃路徑，且規(guī)劃路徑更接近最優(yōu)路徑。

圖7 RRT-Connect算法規(guī)劃路徑

圖8 B-RRT*算法規(guī)劃路徑

圖9 RRT*算法規(guī)劃路徑

圖10 RRT*算法與改進(jìn)算法規(guī)劃路徑對(duì)比(單一種類障礙物)

圖10和圖11中路徑1為原始RRT*算法規(guī)劃路徑，路徑2為改進(jìn)后的RRT*算法規(guī)劃路徑。由觀察可知，改進(jìn)后的RRT*算法的規(guī)劃路徑中冗路徑節(jié)點(diǎn)遠(yuǎn)少于原始RRT*算法生成路徑中冗余路徑節(jié)點(diǎn)數(shù)目，不僅有效縮短了規(guī)劃路徑的長度，同時(shí)使得規(guī)劃路徑更為平滑。

圖11 RRT*算法與改進(jìn)算法規(guī)劃路徑對(duì)比(多種類障礙物)

圖12為RRT*算法改進(jìn)前后在多種障礙類型區(qū)域下的實(shí)驗(yàn)數(shù)據(jù)對(duì)比結(jié)果折線圖。由折線圖可知，改進(jìn)的RRT*算法路徑規(guī)劃過程所需迭代次數(shù)更少，并且在任意狀態(tài)空間條件下其規(guī)劃路徑長度也更短。仿真實(shí)驗(yàn)數(shù)據(jù)統(tǒng)計(jì)結(jié)果如表2所示 ，統(tǒng)計(jì)結(jié)果表明在不同狀態(tài)空間下改進(jìn)后的RRT*算法平均迭代次數(shù)減少了38%左右，平均規(guī)劃路徑長度縮短了4%左右，算法耗時(shí)減少35%左右。

由仿真實(shí)驗(yàn)結(jié)果可知，改進(jìn)的RRT*算法保留了原始RRT*算法概率完備性和漸進(jìn)最優(yōu)的特點(diǎn)，成功剔除規(guī)劃路徑中的冗余路徑節(jié)點(diǎn)，縮短了規(guī)劃路徑的長度，使得路徑規(guī)劃結(jié)果更接近最優(yōu)路徑。

為進(jìn)一步驗(yàn)證改進(jìn)RRT*算法在實(shí)際應(yīng)用中的正確性和實(shí)用性，使用該算法進(jìn)行移動(dòng)機(jī)器人路徑規(guī)劃實(shí)驗(yàn)。實(shí)驗(yàn)所用移動(dòng)機(jī)器人為X2BOT V2.0，實(shí)驗(yàn)環(huán)境為一般室內(nèi)

圖12 兩種算法實(shí)驗(yàn)數(shù)據(jù)對(duì)比結(jié)果

表2 實(shí)驗(yàn)結(jié)果對(duì)比分析表

房間，如圖13所示。上位機(jī)為安裝Ubuntu18.04系統(tǒng)的筆記本電腦(Intel Core處理器，主頻2.6 GHz，內(nèi)存8 G)，程序運(yùn)行環(huán)境為ROS。圖14為機(jī)器人在控制軟件中的路徑規(guī)劃圖，圖中箭頭表示機(jī)器人在環(huán)境地圖中的起始位置，實(shí)心方塊表示目標(biāo)點(diǎn)，算法生成的規(guī)劃路徑由若干路徑節(jié)點(diǎn)構(gòu)成。從圖14中能夠看出，機(jī)器人規(guī)劃出的路徑較為平滑，規(guī)劃路徑中沒有多余的路徑點(diǎn)。移動(dòng)機(jī)器人在轉(zhuǎn)彎過程中，沒有出現(xiàn)停頓和多次轉(zhuǎn)向的動(dòng)作，機(jī)器人從起始點(diǎn)到目標(biāo)點(diǎn)整個(gè)移動(dòng)過程運(yùn)動(dòng)流暢。實(shí)驗(yàn)結(jié)果證明，改進(jìn)的RRT*算法通過逆序試連法剔除規(guī)劃路徑中的冗余路徑節(jié)點(diǎn)，在實(shí)際應(yīng)用中能夠有效避免移動(dòng)機(jī)器人在移動(dòng)過程出現(xiàn)多次停頓和轉(zhuǎn)向動(dòng)作，提高機(jī)器人運(yùn)動(dòng)連貫性。

圖13 X2BOT V2.0實(shí)物圖

圖14 移動(dòng)機(jī)器人路徑規(guī)劃

4 結(jié)束語

RRT*算法能夠規(guī)劃一條較為接近最短的可行路徑，但是生成路徑存在大量不必要的冗余轉(zhuǎn)折節(jié)點(diǎn)，導(dǎo)致機(jī)器人移動(dòng)不連貫和實(shí)用性差。為此，本文提出的一種改進(jìn)RRT*算法，通過引入局部逆序試連策略，能夠減少規(guī)劃路徑中的冗余節(jié)點(diǎn)，縮短路徑長度，使得規(guī)劃路徑更加平滑，更適用于室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃。仿真實(shí)驗(yàn)和機(jī)器人路徑規(guī)劃實(shí)驗(yàn)表明，改進(jìn)后的RRT*算法規(guī)劃的路徑更接近最優(yōu)路徑，且路徑規(guī)劃耗時(shí)更少。在實(shí)際應(yīng)用中，能夠有效避免移動(dòng)機(jī)器人在轉(zhuǎn)向區(qū)域出現(xiàn)多次停頓和換向，保證機(jī)器人移動(dòng)連貫性。