符 強,寧永科,紀元法,孫希延

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

1 引言

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

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

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

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

2 改進RRT算法

2.1 傳統(tǒng)RRT算法

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

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

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

圖2 RRT算法整體流程圖

2.2 改進RRT算法

2.2.1 智能采樣

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

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

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

2.2.2 路徑優(yōu)化

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

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

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

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

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

3.1 機器人運動模型

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

(1)

3.2 速度采樣

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

圖6 DWA算法速度空間圖

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

(2)

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

(3)

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

(4)

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

Vr=Vs∩Va∩Vd

(5)

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

圖7 前向軌跡預測原理示意圖

圖8 前向軌跡預測過程和結果示意圖

3.3 評價函數(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ù)加權系數(shù);Path(v,w)用于計算軌跡偏離全局路徑距離;Heading(v,w)用于計算方位角;Goal(v,w)用于計算軌跡對應目標點距離;Occ(v,w)用于計算軌跡距障礙物距離。

4 融合算法

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

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

5 實驗與分析

5.1 改進RRT算法仿真

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

本文將改進RRT算法與傳統(tǒng)RRT算法,RRT-Star算法, 雙向RRT(RRT-connect)算法進行仿真對比,所有算法在地圖中都設置相同的起點位置坐標為(2,2),目標位置坐標為(49,24)。為了保證所有的算法都能收斂且搜索到較優(yōu)路徑,在本文規(guī)格為50m×30m地圖中,設置迭代次數(shù)為10000次,能夠滿足實驗需求。仿真地圖大小不僅僅局限于本實驗設置大小,迭代次數(shù)應該根據(jù)實驗地圖大小合理設置,保證算法能夠搜索到較優(yōu)路徑。另外,由于RRT算法在搜索路徑過程中存在隨機采樣特性,路徑結果并不唯一,本文選擇其中一個路徑規(guī)劃結果作為仿真結果進行對比,如圖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)境復雜度逐漸增加,能更好的測試改進RRT算法相比RRT、RRT-Star和RRT-connect算法的優(yōu)越性。因為RRT算法在搜索路徑過程中是進行隨機采樣的,所以在實驗過程中,對三種算法分別進行10次試驗,取平均值得到表1數(shù)據(jù)。

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

環(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

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

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

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

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

圖14 環(huán)境地圖

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

圖15 實驗測試小車

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

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

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

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

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

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

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

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

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

6 總結

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