基于改進RRT與DWA融合算法的路徑規(guī)劃

符 強,寧永科,紀(jì)元法,孫希延

(桂林電子科技大學(xué)信息與通信學(xué)院,廣西 桂林 541004)

1 引言

移動機器人技術(shù)近幾年來成為了研究的熱點,它廣泛應(yīng)用于自動駕駛、醫(yī)療、運輸、農(nóng)業(yè)等領(lǐng)域[1-3],而且在疫情期間發(fā)揮了重大作用,而路徑規(guī)劃算法是研究機器人的重要關(guān)鍵技術(shù)之一。機器人路徑規(guī)劃指的是在指定的工作環(huán)境空間下,機器人能夠?qū)ふ页鲆粋€從起始位置到目標(biāo)位置的可到達的無碰撞路徑[4,5]。路徑規(guī)劃算法是實現(xiàn)移動機器人尋找路徑的重要前提,近年來,國內(nèi)外學(xué)者也在不斷的提出相關(guān)路徑規(guī)劃算法。

路徑規(guī)劃算法主要分為全局路徑規(guī)劃算法和局部路徑規(guī)劃算法[6]。全局路徑規(guī)劃算法主要包括以下幾種:基于圖搜索法、如柵格地圖法、人工勢場法、A*算法、D*算法等?；诜律鷮W(xué)的蟻群算法、遺傳算法等。局部路徑規(guī)劃算法主要有動態(tài)窗口算法等?；趫D搜索的全局路徑規(guī)劃算法雖然能夠找到最優(yōu)路徑,但是隨著環(huán)境的復(fù)雜程度增加,會導(dǎo)致算法的計算能力下降[7];基于仿生學(xué)的智能算法在搜索路徑過程中,容易陷入局部最優(yōu)。為了解決以上路徑規(guī)劃算法存在問題,有學(xué)者提出了基于隨機采樣的路徑規(guī)劃算法,如快速搜索隨機樹算法(RRT),概率路標(biāo)算法(PRM)。

快速搜索隨機樹算法(RRT)指的是在工作環(huán)境空間內(nèi),通過對環(huán)境進行不斷采樣,得到采樣節(jié)點[8],直到搜索到目標(biāo)位置為止,該算法通過對狀態(tài)空間中的采樣點進行碰撞檢測,避免了對空間的建模,能夠有效的解決高維空間和復(fù)雜約束的路徑規(guī)劃問題,與其它算法相比,RRT算法可以應(yīng)用在完整系統(tǒng),也可以應(yīng)用在非完整系統(tǒng),解決了在復(fù)雜環(huán)境下的移動機器人路徑規(guī)劃難題。但是傳統(tǒng)RRT算法存在隨機采樣特性,在狹小空間算法不容易收斂,且生成路徑并不是最優(yōu)[9]。針對以上問題,基于RRT的改進算法不斷被提出。文獻[10]提出了RRT-Connect算法,相比傳統(tǒng)RRT算法,很大程度上提高了算法搜索效率,解決了狹窄環(huán)境問題,且降低了大量無用搜索節(jié)點。文獻[11]提出了具有目標(biāo)導(dǎo)向的RRT算法,該算法改變了RRT算法的節(jié)點擴展方式,使得新擴展節(jié)點向著目標(biāo)位置前進,并對最終路徑進行了優(yōu)化,使得路徑更加平滑。文獻[12]提出了DRRT-Connect算法,該算法引入了自適應(yīng)步長函數(shù),加快了算法的搜索速度。文獻[13]提出了RP-RRT*算法,該算法將人工勢場思想引入到RRT算法的隨機采樣過程中,減少了算法的隨機采樣特性。文獻[14]提出了一種改進RRT算法,主要是優(yōu)化了隨機采樣節(jié)點方向的角度。

本文提出了一種改進融合算法,首先對RRT算法的隨機采樣進行優(yōu)化,將隨機采樣變?yōu)橹悄懿蓸?減少RRT算法的隨機采樣特性,其次利用可見圖思想對生成路徑進行優(yōu)化,減少路徑的無用節(jié)點,進一步縮短路徑,最后融合動態(tài)窗口算法解決了RRT算法不能應(yīng)用于動態(tài)環(huán)境問題。對改進前和改進后的RRT算法進行了仿真和實際環(huán)境測試。

2 改進RRT算法

2.1 傳統(tǒng)RRT算法

RRT算法是Lavalle于1998年提出的一種基于快速搜索隨機樹路徑規(guī)劃方法[15],該算法主要利用在環(huán)境空間內(nèi)進行隨機采樣,并進行無碰撞檢查,直到搜索到目標(biāo)點。RRT算法擴展節(jié)點過程如圖1所示。

圖1 RRT算法擴展節(jié)點過程示意圖

若某個采樣點達到設(shè)定目標(biāo)點范圍,就認為已經(jīng)找到了目標(biāo)位置,搜索過程結(jié)束,以此通過不斷迭代找到可行路徑。RRT算法整體流程如圖2所示。

圖2 RRT算法整體流程圖

2.2 改進RRT算法

2.2.1 智能采樣

為了進一步優(yōu)化RRT算法,將RRT算法的隨機采樣改變?yōu)橹悄懿蓸?即通過在障礙物頂點附近生成盡可能多的節(jié)點,達到優(yōu)化算法隨機采樣的目的。

智能采樣前一階段與RRT算法采樣完全一致,在后一階段有所改進,具體思想為:在RRT算法隨機采樣基礎(chǔ)上,一旦找到一條可行路徑,智能采樣將在障礙物附近尋找特殊的節(jié)點,然后以這些特殊節(jié)點設(shè)定大小的圓生成更多無碰撞的有效節(jié)點。這些圓形區(qū)域標(biāo)志了障礙物頂點的位置,如圖3(b)所示,相比圖3(a),減少了算法采樣隨機性,提高搜索效率。與RRT算法相比,在更少的時間和迭代次數(shù)下尋找到最優(yōu)路徑。

圖3 智能采樣和優(yōu)化路徑示意圖

2.2.2 路徑優(yōu)化

通過上面智能采樣搜索到所有的可到達目標(biāo)的節(jié)點,通過不斷回溯,它們彼此連接形成路徑,因為智能采樣的圓內(nèi)標(biāo)記了障礙物的頂點信息,因此從目標(biāo)位置開始,根據(jù)三角不等式原理c

圖4 三角不等式原理示意圖

因為RRT算法的采樣點是在環(huán)境空間內(nèi)隨機采樣,所以算法整體效率低,改進RRT算法加入了智能采樣和路徑優(yōu)化過程,使得算法能夠快速收斂到全局最優(yōu)解,減少算法迭代次數(shù),提高算法效率。改進RRT算法的整體流程如圖5所示。

圖5 改進RRT算法整體流程圖

3 動態(tài)窗口算法

3.1 機器人運動模型

在本文中,移動機器人采用四輪差速小車模型,只能進行前向和旋轉(zhuǎn)運動,假設(shè)倆相鄰點之間在Δ(t)時間內(nèi)做勻速直線運動,則機器人運動模型可以表示為

(1)

3.2 速度采樣

動態(tài)窗口算法通過對速度空間(v,ω)進行采樣,采樣速度空間如圖6所示,x軸代表角速度ω,y軸代表線速度v。在速度空間(v,ω)內(nèi),由于實際機器人自身約束和實際環(huán)境限制,存在vmax和ωmax,所以實際采樣空間(v,ω)可以表示為

圖6 DWA算法速度空間圖

Vs={(v,ω)|vmin≤v≤vmax,ωmin≤ω≤ωmax}

(2)

另外,移動機器人受自身電機約束影響,限制移動機器人實際可到達的速度為

(3)

基于對機器人安全考慮,必須在碰到障礙物前的安全距離內(nèi)將速度減為0,因此Va需滿足

(4)

綜上所述,實際速度采樣的動態(tài)窗口Vr大小為

Vr=Vs∩Va∩Vd

(5)

假設(shè)動態(tài)窗口算法在前向軌跡預(yù)測的Δ(t)時間內(nèi)假設(shè)速度不變,生成前向預(yù)測軌跡,如圖7和圖8所示。

圖7 前向軌跡預(yù)測原理示意圖

圖8 前向軌跡預(yù)測過程和結(jié)果示意圖

3.3 評價函數(shù)

在上面得到的若干軌跡中,通過設(shè)置合理的評價函數(shù)篩選出最優(yōu)的軌跡。具體評價函數(shù)表達式為

G(v,w)=k[αHeading(v,w)+βGoal(v,w)

+γPath(v,w)+σOcc(v,w)]

(6)

式中,參數(shù)k用于路徑平滑;α、β、γ、σ為評價函數(shù)加權(quán)系數(shù);Path(v,w)用于計算軌跡偏離全局路徑距離;Heading(v,w)用于計算方位角;Goal(v,w)用于計算軌跡對應(yīng)目標(biāo)點距離;Occ(v,w)用于計算軌跡距障礙物距離。

4 融合算法

在本文中移動機器人采用四驅(qū)移動機器小車,小車自身搭載激光雷達,利用激光雷達對工作環(huán)境空間進行不斷掃描,構(gòu)建出需要進行路徑規(guī)劃的二維柵格地圖。得到全局地圖后,首先利用本文改進RRT算法進行隨機采樣,得到初始可行路徑,篩選出障礙物附近特殊點的集合,以此集合的所有點得到設(shè)定半徑的圓形區(qū)域,通過回溯,得到初步路徑,再通過三角不等式原理對路徑進行優(yōu)化,減少路徑的拐點個數(shù),最后融合動態(tài)窗口算法解決了動態(tài)環(huán)境下路徑規(guī)劃問題,提高算法整體的實時動態(tài)性能。

在二維柵格地圖中得到全局路徑,機器人根據(jù)當(dāng)前時刻的位置、線速度和角速度,在采樣周期內(nèi)進行前向軌跡預(yù)測,得到所有的軌跡集合,丟棄所有的不滿足動態(tài)窗口內(nèi)的速度和與障礙物發(fā)生碰撞的軌跡,利用評價函數(shù)在剩下的所有軌跡中選出最優(yōu)軌跡并將軌跡對應(yīng)速度信息發(fā)送給機器人移動底盤,從而控制移動機器人移動,安全到達目標(biāo)位置。完整的融合算法流程如圖9所示。

5 實驗與分析

5.1 改進RRT算法仿真

仿真環(huán)境采用測試機器配置為intel(R) CPU2.2GHz/8G,搭載系統(tǒng)為Ubuntu18.04系統(tǒng),實驗地圖環(huán)境選擇矩形柵格地圖,分辨率為1m*1m,整個地圖大小為50m*30m,其中,給地圖中設(shè)置一部分障礙物,能更好的測試算法性能。為了測試本文改進RRT算法的有效性,將傳統(tǒng)RRT算法,RRT-Star算法,雙向RRT(RRT-connect)算法與本文改進RRT算法在相同測試環(huán)境下進行對比。本文構(gòu)建了三種不同的尺寸大小相同地圖環(huán)境,地圖環(huán)境中白色區(qū)域為無障礙物可行區(qū)域,灰色代表了障礙物所處區(qū)域。

本文將改進RRT算法與傳統(tǒng)RRT算法,RRT-Star算法, 雙向RRT(RRT-connect)算法進行仿真對比,所有算法在地圖中都設(shè)置相同的起點位置坐標(biāo)為(2,2),目標(biāo)位置坐標(biāo)為(49,24)。為了保證所有的算法都能收斂且搜索到較優(yōu)路徑,在本文規(guī)格為50m×30m地圖中,設(shè)置迭代次數(shù)為10000次,能夠滿足實驗需求。仿真地圖大小不僅僅局限于本實驗設(shè)置大小,迭代次數(shù)應(yīng)該根據(jù)實驗地圖大小合理設(shè)置,保證算法能夠搜索到較優(yōu)路徑。另外,由于RRT算法在搜索路徑過程中存在隨機采樣特性,路徑結(jié)果并不唯一,本文選擇其中一個路徑規(guī)劃結(jié)果作為仿真結(jié)果進行對比,如圖10-12所示。

圖10 環(huán)境1 RRT、RRT-Star、RRT-connect、改進RRT算法仿真圖

圖11 環(huán)境2 RRT、RRT-Star、RRT-connect、改進RRT算法仿真圖

圖12 環(huán)境3 RRT、RRT-Star、RRT-connect、改進RRT算法仿真圖

從仿真圖中可以看出,三種地圖環(huán)境的障礙物逐漸增多,環(huán)境復(fù)雜度逐漸增加,能更好的測試改進RRT算法相比RRT、RRT-Star和RRT-connect算法的優(yōu)越性。因為RRT算法在搜索路徑過程中是進行隨機采樣的,所以在實驗過程中,對三種算法分別進行10次試驗,取平均值得到表1數(shù)據(jù)。

表1 RRT、RRT-Star、RRT-connect、改進RRT算法仿真數(shù)據(jù)結(jié)果

環(huán)境算法搜索時間/s路徑節(jié)點數(shù)迭代次數(shù)路徑長度/m環(huán)境1RRT0.6413478066.71RRT-Star1.1747950055.32RRT-connect0.287720059.25改進RRT0.88380051.48環(huán)境2RRT0.3412348261.15RRT-Star1.2837950052.86RRT-connect0.196720051.89改進RRT0.84280051.89環(huán)境3RRT1.24160233279.65RRT-Star1.7572950061.12RRT-connect0.378840067.36改進RRT0.86980056.35

從仿真結(jié)果、表1和圖13試驗數(shù)據(jù)結(jié)果可以看出,三種算法均能搜索到可行路徑,其中,RRT算法雖然搜索路徑時間最短,但是路徑結(jié)果曲折,路徑包含節(jié)點數(shù)較多,且不是最優(yōu),不適合實際移動機器人執(zhí)行;RRT-Star算法相比RRT算法在搜索路徑方面花費時間較長,這是因為RRT-Star算法在RRT算法搜索到可行路徑的基礎(chǔ)上,加入了重新選擇父節(jié)點和重布線過程,算法進行不斷迭代,用來優(yōu)化路徑,雖然最終路徑結(jié)果比較平滑,但是花費時間較長,迭代次數(shù)太多;本文改進RRT算法在RRT算法的基礎(chǔ)上,加入了智能采樣,降低了算法的隨機采樣性,算法在迭代次數(shù)很少的情況下就達到收斂。然后利用三角不等式思想,通過回溯提取關(guān)鍵點,優(yōu)化路徑。改進算法在搜索時間上在可接納范圍內(nèi),由于只提取關(guān)鍵點,路徑節(jié)點數(shù)大大降低,路徑長度得到進一步優(yōu)化,從而路徑更加平滑,有利于實際移動機器人執(zhí)行。

圖13 不同環(huán)境下算法數(shù)據(jù)測試結(jié)果比較

5.2 改進RRT算法實際環(huán)境測試

在本文中實際測試機器選擇移動機器小車,裝載有激光雷達,用來獲取實際環(huán)境二維柵格地圖,如圖14所示。

圖14 環(huán)境地圖

上位機選擇Jetson Nano,系統(tǒng)為Ubuntu18.04,路徑規(guī)劃結(jié)果顯示工具選擇ROS中的RVIZ可視化工具,小車控制系統(tǒng)選擇STM32開發(fā)控制板,實際測試小車如圖15所示。在本文中,測試環(huán)境選擇室內(nèi)辦公室環(huán)境,得到全局地圖后,根據(jù)改進RRT算法獲得全局規(guī)劃路徑,然后融合動態(tài)窗口算法,解決了動態(tài)環(huán)境下路徑規(guī)劃問題。

圖15 實驗測試小車

在實際測試過程中,所有算法都采用相同的參數(shù),移動小車最大線速度和角速度分別為0.2m/s和0.5rad/s,采樣個數(shù)設(shè)置為10個,采樣間隔T=0.2s,評價函數(shù)加權(quán)系數(shù)α=0.1、β=10.0、γ=0.1、σ=0.2。圖中綠色線為路徑規(guī)劃結(jié)果。

首先在無障礙物環(huán)境下對RRT和改進RRT算法進行測試,測試結(jié)果如圖16所示。

圖16 無障礙物環(huán)境測試結(jié)果

然后在全局地圖中加入部分障礙物,融合DWA算法對RRT和改進RRT算法進行測試,測試結(jié)果如圖17-20所示。

圖17 有障礙物環(huán)境1測試結(jié)果

圖18 有障礙物環(huán)境1小車狀態(tài)輸出結(jié)果

圖19 有障礙物環(huán)境2測試結(jié)果

圖20 有障礙物環(huán)境2小車狀態(tài)輸出結(jié)果

由于在測試過程中,使用的建圖算法并不是很精確,存在建圖誤差,此外,受機器人自身電機控制偏差等因素,使得實際測試結(jié)果與實驗仿真結(jié)果往往不能保持一致。從實際測試結(jié)果可以看出,在無障礙物環(huán)境下改進RRT算法相比RRT算法路徑筆直,達到最優(yōu)。隨著障礙物的不斷增多,環(huán)境復(fù)雜度增大,雖然路徑結(jié)果有所曲折,但從最終路徑規(guī)劃結(jié)果來看,本文改進融合算法效果較好,能夠解決實際動態(tài)環(huán)境下路徑規(guī)劃問題,可以滿足實際需求。

6 總結(jié)

本文針對傳統(tǒng)的RRT算法搜索效率低、算法的隨機采樣特性導(dǎo)致規(guī)劃路徑時間長、路徑曲折且不適用于動態(tài)環(huán)境等問題,提出了一種改進融合算法,具體方式是對傳統(tǒng)的RRT算法加入智能采樣和路徑優(yōu)化。首先根據(jù)傳統(tǒng)RRT算法規(guī)劃出可行路徑,找出路徑上包含障礙物頂點附近的節(jié)點,在該節(jié)點處以一定大小的圓內(nèi)進行采樣,減少了算法的隨機采樣性,然后根據(jù)三角不等式思想,通過回溯對路徑進行優(yōu)化,使得路徑更加平滑,距離更短,最后,結(jié)合動態(tài)窗口算法(DWA),解決了動態(tài)環(huán)境下路徑規(guī)劃問題。最后通過仿真和實際環(huán)境測試驗證了本文改進融合算法的可行性。