劉 沖,劉本學(xué),呂 桉,李 霞

(鄭州大學(xué)機械與動力工程學(xué)院,鄭州 450001)

0 引言

隨著各行業(yè)對自動化要求的不斷提高,諸如掃地機器人、物流機器人等室內(nèi)移動機器人的應(yīng)用越來越廣泛。路徑規(guī)劃是保證移動機器人完成各項任務(wù)的前提,是移動機器人研究領(lǐng)域的一個重要基礎(chǔ)性問題[1]。路徑規(guī)劃是指根據(jù)已知的環(huán)境地圖等條件,在滿足特定條件的情況下,規(guī)劃出一條從起點到終點的無碰撞路徑[2]。路徑規(guī)劃算法的性能優(yōu)劣對于機器人能否順利完成任務(wù)有較大的影響。常見的路徑規(guī)劃算法包括Dijkstra算法、A*算法、RRT算法[3]、RRT*算法、遺傳算法、蟻群算法等。Dijkstra算法一定能找到環(huán)境中的最短路徑,但是路徑規(guī)劃過程中遍歷的節(jié)點過多,計算復(fù)雜,算法效率較低。A*算法在Dijkstra算法的基礎(chǔ)上設(shè)置了估價函數(shù),提高了算法的搜索效率。遺傳算法的計算量較大,對于高維度問題難以處理和優(yōu)化。

RRT算法是一種概率完備、結(jié)構(gòu)簡單且具有靈活搜索能力的路徑規(guī)劃算法。具有較快的擴展和搜索速度,適用于各種復(fù)雜環(huán)境下的路徑規(guī)劃。但因其隨機采樣的機制而存在節(jié)點利用率低、路徑復(fù)雜度高等問題[4-5]。針對RRT算法的不足,許多學(xué)者提出了優(yōu)化方案。POHL[6]提出了雙向擴展隨機樹算法(Bidirectional RRT),通過構(gòu)造兩棵分別以起點和終點為根節(jié)點的隨機樹來完成路徑規(guī)劃,提高了算法的收斂速度。JR等[7]將貪心算法應(yīng)用于Bi-RRT算法,提出了RRT-Connect算法,提高了隨機樹的擴展效率。WANG等[8]提出了一種基于強化學(xué)習(xí)的LM-RRT算法,提高了隨機樹的局部空間探索能力,使機器人在狹窄通道的路徑規(guī)劃效率得到提高。QI等[9]提出了MOD RRT*算法,通過對生成的路徑進行重新規(guī)劃,提高了路徑的質(zhì)量。李金良等[10]通過改變RRT算法的度量函數(shù)并添加角度約束,減少了路徑規(guī)劃過程中的搜索范圍,提高了算法的效率。王碩等[11]通過引入動態(tài)權(quán)重系數(shù)使樹盡可能地在向目標(biāo)點進行擴展的同時又能夠及時地避開障礙物,減少了冗余節(jié)點數(shù)。張玉偉等[12]提出了IBI-RRT*算法,通過引入基于狀態(tài)子集直接采樣的反向擴展策略,使機器人在障礙物邊界區(qū)時能夠獲得接近最優(yōu)路徑成本的可行路徑。

本文通過優(yōu)化采樣點選擇策略,并提出了可變步長的隨機樹擴展策略,以及對初始路徑平滑處理3個方面對RRT算法進行改進。使用MATLAB進行仿真分析,在搭載ROS系統(tǒng)的移動機器人上進行實驗,驗證了改進RRT算法的優(yōu)越性與可靠性。

1 RRT算法介紹

在RRT算法中,設(shè)置機器人所在狀態(tài)空間為X,起點為Xstart,終點為Xgoal。算法初始化時將起點加入隨機樹中。每次采樣開始時,從狀態(tài)空間中隨機選取采樣點Xrandom,依次遍歷當(dāng)前隨機樹中的所有節(jié)點,找到距Xrandom距離最短的節(jié)點Xnearest,從Xnearest起向Xrandom擴展一個步長得到新節(jié)點Xnew。若Xnearest與Xnew之間的路徑?jīng)]有障礙物,則將Xnew加入隨機樹中,否則重新開始采樣,直至隨機樹擴展到終點Xgoal。最后,從Xgoal回溯至Xstart,即可得到完整的路徑。算法的流程如圖1所示。

圖1 RRT算法流程

2 改進RRT算法

2.1 基于動態(tài)概率的采樣點選擇策略

RRT算法總是在機器人狀態(tài)空間中隨機選取采樣點,雖然具有概率完備性,但在實際搜索路徑的過程中,這種隨機選取采樣點的策略會使大量的采樣點最終不會出現(xiàn)在規(guī)劃的路徑中,導(dǎo)致計算資源的浪費。為了減少采樣過程的隨機性,URMSON等[13]提出了目標(biāo)偏向RRT(Goal-biased RRT)算法。該算法按式(1)所示方式選取采樣點。

(1)

式中:Xrandom為采樣點,Xgoal為終點,P為隨機數(shù),0

Goal-biased RRT算法既保留了RRT算法的概率完備性,也提高了隨機樹的擴展速度。但Goal-biased RRT算法中概率閾值Pbias為固定值。當(dāng)采樣點在障礙物附近時,Goal-biased RRT算法會使機器人不易繞開障礙物。若繼續(xù)采用目標(biāo)點作為擴展點,機器人會陷入局部極小值,增加路徑規(guī)劃的時間。在實際路徑規(guī)劃過程中,一般將采樣次數(shù)設(shè)置為有限值。這就有可能導(dǎo)致路徑規(guī)劃失敗,如圖2所示。

圖2 路徑規(guī)劃失敗

本文提出了基于動態(tài)概率的采樣點選擇策略:在算法開始時設(shè)置概率閾值初始值為Pini。當(dāng)隨機樹擴展到障礙物附近時,則認(rèn)為其已陷入局部極小值,此時通過式(2)計算Pbias的值。隨著采樣點落入障礙物次數(shù)的增加,Pbias的值會逐漸減小。此時的采樣策略會增加隨機樹逃脫障礙物附近區(qū)域的概率,也就會更快的跳出局部極小值。

(2)

式中:Pbias為概率閾值,Pini為概率閾值的初始值,n為上一次采樣成功至此次采樣期間,采樣失敗的次數(shù)。

在如圖2所示的環(huán)境地圖中,取不同的概率閾值初始值Pini,分別對Goal-biased RRT算法與基于動態(tài)概率采樣的RRT算法進行仿真分析。以采樣點數(shù)量、路徑長度作為評價指標(biāo),記錄1000次仿真的平均值,仿真結(jié)果如圖3所示。

(a) 概率閾值初始值Pini對路徑長度的影響 (b) 概率閾值初始值Pini對采樣點數(shù)量的影響圖3 概率閾值初始值Pini對兩種算法性能的影響

由圖3a可知,概率閾值初始值Pini的大小對兩種算法規(guī)劃得到的路徑長度都有影響。隨著Pini的增大兩種算法規(guī)劃的路徑長度變短,這是由于Pini值越大,采樣點越具有目標(biāo)偏向性。在Pini<0.8時兩種算法所規(guī)劃出的路徑長度差在1%以內(nèi)?？梢哉J(rèn)為兩種算法在路徑長度這一評價指標(biāo)上性能大致相同。由圖3b可知,Goal-biased RRT算法的采樣點數(shù)量始終大于基于動態(tài)概率的RRT算法,并且隨著Pini值的增大,前者的采樣點數(shù)量急劇增加。過多的采樣點會導(dǎo)致算法的計算時間增加,降低路徑規(guī)劃的速率。說明基于動態(tài)概率采樣的RRT算法能提高路徑規(guī)劃的效率。

2.2 基于可變步長的隨機樹擴展策略

RRT算法中隨機樹的擴展步長為固定值。步長過大時,會增加Xnew與Xnearest之間的路徑經(jīng)過障礙物的概率,降低采樣的成功率,導(dǎo)致采樣次數(shù)增加、算法的效率降低;步長過小時,不僅會增加采樣的次數(shù),還會使路徑過于曲折,不利用機器人移動。

本文提出了可變步長的隨機樹擴展策略。在算法初始化時,設(shè)置最小步長Slim和基本步長S兩個參數(shù),兩參數(shù)的關(guān)系如式(3)所示。

S=n×Slim

(3)

式中:n為距離因子,其值由環(huán)境地圖的大小確定。

在未遇到障礙物時,隨機樹的擴展步長等于S。在遇到障礙物時,采用變步長的策略進行擴展。其主要思路如下:當(dāng)Xnearest與Xnew兩點之間的路徑經(jīng)過障礙物時,并不是直接舍棄Xnew開始下一次采樣。而是將Xnearest與Xnew之間的路徑分為n+1份,每一段的端點分別設(shè)置為Xi,如圖4所示。若存在某一個Xi使得Xnearest與Xi之間的路徑不經(jīng)過障礙物且Xnearest與Xi+1之間的路徑經(jīng)過障礙物,則將Xi作為新的節(jié)點加入隨機樹中。按照此規(guī)則,在圖4中應(yīng)將X3作為新的節(jié)點加入隨機樹中?；诳勺儾介L的隨機樹擴展策略提高了采樣點的利用效率,有利于減少采樣的次數(shù),提高算法的收斂速度。

圖4 可變步長的隨機樹擴展策略

圖4中各節(jié)點坐標(biāo)的計算式如式(4)和式(5)所示:

xi=x0+i×Slim×cosθ

(4)

yi=y0+i×Slim×sinθ

(5)

式中:xi、yi分別為節(jié)點Xi的橫、縱坐標(biāo),x0、y0分別為節(jié)點Xnearest的橫、縱坐標(biāo),θ為節(jié)點Xnearest與節(jié)點Xnew連線與水平正方向的夾角。

2.3 初始路徑優(yōu)化

RRT算法按照從Xgoal回溯至Xstart的方法生成路徑。最終生成的路徑中會包含許多不必要的采樣點。一方面會增加最終路徑的長度,另一方面會導(dǎo)致路徑過于曲折,不利于機器人移動。采用如下方法對機器人的路徑進行優(yōu)化:從起點Xstart開始,依次檢查Xstart與初始路徑中的各個節(jié)點Xi之間是否存在障礙物。如果不存在障礙物,則將Xi的父節(jié)點變更為Xstart;如果存在障礙物,則用Xi的父節(jié)點代替Xstart繼續(xù)遍歷,直至遍歷至Xgoal。路徑優(yōu)化的示意圖如圖5所示。優(yōu)化前的路徑為Xstart→X1→X2→X3→Xgoal。優(yōu)化后的路徑為Xstart→X2→Xgoal。

圖5 初始路徑優(yōu)化

圖5中相鄰節(jié)點之間的轉(zhuǎn)角過大,不利用機器人移動,需要進行路徑平滑處理。貝塞爾曲線是一種連續(xù)光滑的曲線,具有曲率連續(xù),控制結(jié)構(gòu)簡單等特點[14]。本文使用五次貝塞爾曲線對路徑進行平滑處理。五次貝塞爾曲線和6個控制點之間的關(guān)系如式(6)所示。五次貝塞爾曲線如圖6所示。

圖6 五次貝塞爾曲線

B5(t)=(1-t)5P0+5t(1-t)4P1+10t2(1-t)3P2+
10t3(1-t)2P3+5t4(1-t)P4+t5P5

(6)

式中:t為控制參數(shù),t∈[0,1];P0～P5為6個控制點。

3 基于MATLAB的仿真分析

為驗證本文提出算法的性能,在圖2所示環(huán)境地圖中,對RRT算法、Goal-biased RRT算法、改進RRT算法進行仿真分析。仿真使用計算機的處理器為AMD R5-4650G,仿真軟件為MATLAB R2019a。

設(shè)置起點坐標(biāo)為(10,10),終點坐標(biāo)為(80,90),概率閾值初始值Pini設(shè)置為0.2。采用路徑長度、采樣點數(shù)量、路徑規(guī)劃時間作為評價指標(biāo)進行仿真。每種算法分別進行1000次路徑規(guī)劃,取平均值作為仿真結(jié)果。圖7為3種算法規(guī)劃出的路徑,3種算法的仿真數(shù)據(jù)如表1所示。

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改進RRT算法圖7 仿真環(huán)境下3種算法規(guī)劃的路徑

表1 3種算法仿真數(shù)據(jù)對比

由圖7可知,3種算法都規(guī)劃出了一條從起點到終點的無障礙路徑。由于RRT算法的采樣策略不具有方向性,RRT算法規(guī)劃過程中的采樣點的數(shù)量明顯多于Goal-biased RRT算法和改進RRT算法。并且RRT算法規(guī)劃的路徑的長度也比Goal-biased RRT算法和改進RRT算法長。由于改進RRT算法采用了變步長的采樣點擴展策略以及路徑優(yōu)化,改進RRT算法規(guī)劃出的路徑長度也明顯短于RRT算法和Goal-biased RRT算法,并且路徑更為平滑。

由表1可知,改進RRT算法相比于RRT算法和Goal-biased RRT算法,規(guī)劃路徑的長度分別減少了23.09%和15.45%。規(guī)劃路徑的時間分別減少了87.16%和46.55%。由于Goal-biased RRT算法的概率閾值為固定值,在遇到障礙物后可能會陷入局部極小值,增加了路徑規(guī)劃的時間。

4 基于ROS平臺的實驗驗證

為驗證算法在實際環(huán)境中的性能,將算法應(yīng)用于如圖8所示的基于ROS的移動機器人。該移動機器人硬件組成為樹莓派4B、思嵐A1激光雷達(dá)、STM32開發(fā)板。分別在實驗室環(huán)境和Gazebo虛擬環(huán)境中進行實驗。圖9a為實驗室環(huán)境,圖9b為Gazebo軟件中的虛擬環(huán)境。

圖8 基于ROS的移動機器人

(a) 實驗室環(huán)境 (b) Gazebo環(huán)境圖9 實驗環(huán)境

使用Gmapping算法建立二維地圖,在可視化工具Rviz中設(shè)置相同的起點和終點,分別在兩個場景中使用3種算法各進行100次路徑規(guī)劃。3種算法的路徑規(guī)劃結(jié)果如圖10和圖11所示。實驗數(shù)據(jù)如表2和表3所示。

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改進RRT算法圖10 實驗室環(huán)境中3種算法規(guī)劃的路徑

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改進RRT算法圖11 Gazebo環(huán)境中3種算法規(guī)劃的路徑

表2 實驗室環(huán)境中3種算法實驗數(shù)據(jù)對比

表3 Gazebo環(huán)境中3種算法實驗數(shù)據(jù)對比

由表2和表3可知,在真實環(huán)境的實驗中,改進RRT算法在路徑長度、采樣次數(shù)、規(guī)劃時間3項指標(biāo)中,均有更好的性能,與仿真得到的結(jié)論一致。同時考慮機器人在真實環(huán)境中的移動過程,由圖10和圖11可知,改進RRT算法規(guī)劃出的路徑具有更少的轉(zhuǎn)折點,更有利于機器人的移動。進一步驗證了算法的有效性和優(yōu)越性。

5 結(jié)論

針對RRT算法采樣時間長,規(guī)劃路徑曲折等問題,提出了一種改進RRT算法。通過采用基于動態(tài)概率的采樣點選擇策略和可變步長的隨機樹擴展策略,減少了采樣點數(shù)量,提高了隨機樹的搜索效率,最后使用五次貝塞爾曲線對初始路徑進行優(yōu)化,使最終生成的路徑更適合機器人移動。仿真和實驗結(jié)果均表明改進RRT算法能有效提高機器人路徑規(guī)劃的效率,并且規(guī)劃的軌跡更符合機器人運動學(xué)規(guī)律。