一種移動機器人路徑規(guī)劃新算法

朱宏輝，王嘉豪

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

0 引言

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

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

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

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

1 RRT*算法原理

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

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

圖1 隨機樹節(jié)點擴展

圖2 隨機樹擴展k次后

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

圖4 隨機樹重布線

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

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

表1 RRT*算法偽代碼

2 RRT*算法改進

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

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

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

圖6 局部逆序試連

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

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

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

3 實驗與結(jié)果分析

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

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

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

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

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

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

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

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

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

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

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

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

圖12 兩種算法實驗數(shù)據(jù)對比結(jié)果

表2 實驗結(jié)果對比分析表

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

圖13 X2BOT V2.0實物圖

圖14 移動機器人路徑規(guī)劃

4 結(jié)束語

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