阮曉鋼， 劉少達， 朱曉慶

(1.北京工業(yè)大學信息學部， 北京 100124； 2.計算智能與智能系統(tǒng)北京市重點實驗室， 北京 100124)

路徑規(guī)劃是機器人自主導航技術中一個關鍵性難題，本質(zhì)上是指在當前環(huán)境空間中移動機器人依據(jù)一定的性能指標，如路徑長度、收斂時間、消耗能量等，找到從起始點到目標點的最佳或次佳的無障礙通路[1-2]. 機器人可沿規(guī)劃路徑運動至目標點. 傳統(tǒng)的路徑規(guī)劃算法，如圖搜索算法、人工勢場法、動態(tài)窗口法(dynamic window approach，DWA)、蟻群優(yōu)化算法等，在低維度空間中具有一定的優(yōu)勢[3-7]，但都須事先對環(huán)境空間進行建模且計算復雜度與機器人自由度和環(huán)境維度呈指數(shù)關系：因此，不適合解決多自由度機器人在高維空間中的路徑規(guī)劃問題.

快速探索隨機樹(rapid exploring random tree,RRT)是解決高維空間中路徑規(guī)劃問題的一種流行且行之有效的方法[8]. 該算法通過對隨機采樣點進行碰撞檢測的方式搜索整個環(huán)境空間，避免了對空間中障礙物的建模，因此，計算復雜度較低，適合解決高維動態(tài)多障礙物環(huán)境空間中路徑規(guī)劃問題[9]. RRT算法計算成本低且搜索速度快，自從提出以來就引起了眾多學者的關注. 但是，該算法仍存在以下缺點[10]：1) 采樣點的隨機性導致算法的收斂速度較慢，路徑規(guī)劃效率較低. 2) 在具有狹窄通道的U形環(huán)境中算法容易陷入局部極小點. 3) 該算法是概率完備性算法，生成的路徑一般不是最優(yōu)且較為曲折.

為了解決上述問題，研究者提出了許多改進算法以改進路徑搜索過程[10-14]. 其中隨機概率目標快速探索隨機樹(probability goal RRT,PGRRT)算法和雙向快速探索隨機樹(bidirectional RRT,BRRT)算法在路徑生成效率上取得了顯著進步. PGRRT算法中隨機樹以一定概率向目標點方向生長以提高路徑規(guī)劃效率. BRRT算法[13]可在起始點和目標點生成2棵隨機樹，同時進行路徑搜索，加快了算法的收斂速度，提高了機器人在具有狹窄通道的環(huán)境中逃離局部極小點的概率.

為了進一步提高路徑的生成效率和算法逃離局部極小點的概率，本文提出一種改進的基于BRRT的簡單高效的自適應啟發(fā)式多快速探索隨機樹(adaptive heuristic multiple rapid exploring random tree,AHMRRT)路徑規(guī)劃算法. 該算法在原有BRRT算法的基礎上引入了多隨機樹構建策略和自適應啟發(fā)式目標偏置因子. 多隨機樹構建策略使算法可在起始點、目標點和子目標點生成4棵隨機樹，并同時進行擴展搜索，相較于BRRT算法進一步提高了路徑生成效率和算法逃離局部極小點的概率. 同時，AHMRRT算法在自適應啟發(fā)式目標偏置因子的作用下可根據(jù)環(huán)境中障礙物分布情況自適應地改變單棵隨機樹新節(jié)點的生成策略，提高了機器人執(zhí)行路徑規(guī)劃任務中的智能性. 在自由空間，該算法可以迅速向目標節(jié)點擴展以提高搜索效率；在多障礙物空間，AHMRRT算法將調(diào)用隨機采樣函數(shù)增加隨機樹生長的隨機性以防止機器人陷入局部最優(yōu)點.

1 術語定義

為了更加方便、準確地描述PGRRT、BRRT、AHMRRT算法，做出以下定義[15].

S=xsource,G=xgoal,M=xsub：S是路徑規(guī)劃任務的起始點，G為全局目標點，M為基于多隨機樹構建策略選擇的處于空閑空間中的子目標點.

T=(V,E)：T代表算法生成的擴展隨機樹；隨機樹T中的頂點存儲于集合V中；連接頂點的邊關系存儲于集合E中.

xs=SampleFree(i)：該函數(shù)可以在當前地圖空間中生成獨立均勻分布的隨機采樣點.

xnew=Steer(xnear,xs,ρ)：該函數(shù)為新節(jié)點生成函數(shù)，以xnear、xs和擴展步長ρ作為輸入，返回樹T的一個新節(jié)點xnew，其中‖xnew-xnear‖≤ρ.ρ表示隨機樹的增長率，ρ較大時隨機樹擴展更快.

FreeCheck(X,x,y)：該函數(shù)以地圖空間集合和2個節(jié)點作為輸入，返回1則表示節(jié)點及其連線處于空閑空間，返回0則表示節(jié)點或其連線處于障礙物中.

2 PGRRT算法和BRRT算法

2.1 PGRRT算法

RRT算法是一種單查詢路徑規(guī)劃算法，以起始點作為根節(jié)點，通過迭代生成子節(jié)點的方式生成一棵探索隨機樹，當子節(jié)點進入目標點區(qū)域即可規(guī)劃出從起始點到目標點的連續(xù)無障礙路徑.

RRT算法偽代碼如下.

算法1RRT算法

1:V←S;E←?;

2: while True do

3:xs←Sample(i),i=i+1;

4:xnear←Nearst(T,xs);

5:xnew←Steer(xnear,xs,ρ);

6: if FreeCheck(X,xnear,xnew)=1 then

7:V←{xnew}∪V;E←{xnear,xnew}∪E;

8: if dist(xnew,G)≤εthen

break;

8: end if

10:returnT=(V,E);

初始時刻V={S},E=?，在每次迭代過程中，SampleFree函數(shù)生成一個隨機采樣點，Nearst函數(shù)找到當前隨機樹中距離采樣點歐氏距離最近的頂點xnear. 然后，通過Steer函數(shù)生成新節(jié)點xnew，判斷該節(jié)點是否有效，若有效則添加到隨機擴展樹中，同時判斷該節(jié)點是否進入目標區(qū)域. 若到達目標區(qū)域，則路徑規(guī)劃成功；否則繼續(xù)迭代執(zhí)行[16].

新節(jié)點的生成策略是RRT算法中的關鍵步驟. 傳統(tǒng)RRT算法中新節(jié)點的位置由采樣節(jié)點唯一確定. 新節(jié)點的位置為

(1)

式中：xnew.posX、xnew.posY、xnear.posX、xnear.posY分別為新節(jié)點和最近節(jié)點的橫縱坐標；θ為xnear與xs連線與X軸的夾角. 隨機樹擴展過程如圖1所示.

圖1 RRT算法的新節(jié)點生成Fig.1 New node generation of RRT algorithm

為了提高RRT的路徑規(guī)劃效率，PGRRT在SampleFree函數(shù)中引入?yún)?shù)概率閾值p，在每次迭代過程中得到一個0～1.0的隨機概率q.當0

2.2 BRRT算法

其實他們潛意識里早就知道，從許多許多年前開始，他們就注定了是要互相陪伴的人，不然為什么這么多年來誰也沒有跟其他人戀愛過，誰也沒有遺忘過誰。

BRRT算法在路徑搜索速度和效率方面有了大幅度提高. 相較于RRT算法，BRRT算法具有以下優(yōu)勢[17-18]：首先，BRRT算法的擴展步長更長，使得隨機樹長得更快. 其次，T1、T2兩棵樹彼此向?qū)Ψ浇惶鏀U展，而不是采用隨機擴展的方式，在一定程度上降低了搜索的盲目性. 但是，BRRT仍存在一定的問題，當起始點與目標點處于具有U形障礙物的環(huán)境中時，因為T1、T2不斷向?qū)Ψ竭M行擴展，所以，可能造成算法陷入局部極小點，增加路徑規(guī)劃時間，降低算法的路徑規(guī)劃效率，甚至導致算法失敗.

3 AHMRRT算法

為了進一步提高BRRT算法的搜索速度和效率以及機器人逃離局部極小點的概率. 本文提出基于BRRT算法的AHMRRT算法.

3.1 新節(jié)點生成策略

通過在單棵隨機樹中引入自適應啟發(fā)式目標偏置因子，AHMRRT算法中新節(jié)點的位置將不再僅由隨機采樣點唯一確定. 新節(jié)點位置公式為

(2)

式中：σ為自適應啟發(fā)式目標偏置因子；α為目標點G和最近節(jié)點xnear的連線與X軸的夾角. AHMRRT算法中單棵隨機樹擴展過程如圖2所示. 通過改變偏置因子σ的值可以在隨機樹生長過程中改變新節(jié)點的生成策略.

圖2 AHMRRT算法的新節(jié)點生成Fig.2 New node generation of AHMRRT algorithm

3.2 自適應調(diào)整策略

自適應調(diào)整策略具體為：偏置因子σ的值可根據(jù)環(huán)境中障礙物的分布情況自適應進行改變. 在隨機樹生長的初始時刻，自適應啟發(fā)式偏置因子σ為0.1，首先通過擴展第一步得到xnew，新節(jié)點在隨機采樣點和目標點的作用下向目標點方向快速擴展. 如果新節(jié)點沒有與障礙物發(fā)生碰撞并且未連接到其他樹，則隨機樹將繼續(xù)使用目標偏置策略擴展新節(jié)點. 因此，在探索自由空間Xfree時隨機樹的擴展更具有目標導向性，提高了算法的搜索速度和效率. 當新節(jié)點與障礙物發(fā)生碰撞新節(jié)點無效時，將σ設置為0，此時采用隨機擴展的方式生成新節(jié)點，即在多障礙空間中提高新節(jié)點生成的隨機性以防止機器人陷入局部極小點而降低算法收斂時間. 添加自適應啟發(fā)式目標偏置因子既避免了新節(jié)點陷入局部極小點，又提高了隨機樹的搜索效率，減少了冗余搜索.

3.3 多隨機樹構建策略

為了實現(xiàn)4棵隨機樹同時生長以提高搜索效率，需要在當前空間中選擇子目標點作為第3、4棵樹的根節(jié)點.

子目標點選擇策略具體為：為降低算法復雜度，首先將起始點與目標點的中點及其鄰域點作為子目標點的候選點，計算公式為

(3)

(4)

然后依次檢測xmed、xmed1、xmed2、xmed3、xmed4是否與障礙物發(fā)生碰撞，選擇第1個有效候選點作為子目標點構建第3、4棵快速擴展隨機樹. 式(4)中δ為鄰域半徑，依據(jù)地圖尺寸大小設置δ值. 本文中地圖尺寸為500×500像素，δ=80.

如果以上5個候選節(jié)點均與障礙物發(fā)生碰撞，則AHMRRT算法舍棄子目標點的選取，僅在起始點和目標點區(qū)域構建2棵隨機樹. 候選點示意圖如圖3所示.

圖3 候選節(jié)點示意圖Fig.3 Diagram of candidate node

圖4為AHMRRT算法擴展示意圖. 圖中：紅色實心圓點是起始點S；黑色實心圓點是目標點G.T1和T2相向生長，T3和T4相向生長，4棵樹同時進行擴展搜索. 4棵樹相連后，路徑規(guī)劃成功. 紅色折線為規(guī)劃出的路徑. 綠色實心圓點為子目標點M.

圖4 AHMRRT算法擴展示意圖Fig.4 Expansion diagram of AHMRRT algorithm

4 仿真實驗

4.1 仿真實驗場景

為了驗證AHMRRT算法的高效性且可以有效逃離路徑規(guī)劃過程中極易出現(xiàn)的局部極小點問題，在圖5所示的500×500像素的簡單、標準、復雜、U形環(huán)境中將AHMRRT算法與PGRRT、BRRT算法進行了比較. 仿真實驗中使用的硬件平臺是ThinkPad T460，Intel Core(TM)i5-6200U 2.4 GHz CPU，RAM 8GB，軟件平臺是matlab2015.

圖5 實驗場景Fig.5 Experimental scene

4.2 簡單、標準、復雜環(huán)境下的對比實驗

實驗參數(shù)：擴展步長ρ=15；概率閾值p=0.1；自適應啟發(fā)式偏置因子的初始值σ=0.1;起始點S=(20,20)，目標點G=(480,480). 在仿真實驗過程中，針對每種環(huán)境用PGRRT、BRRT算法和AHMRRT算法各進行50次實驗.

圖6是AHMRRT算法與PGRRT、BRRT算法在簡單、標準、復雜環(huán)境下的仿真實驗結果圖.

圖6 實驗結果對比Fig.6 Comparison of experimental results

從圖6可以直觀地看出：在簡單和標準環(huán)境中，AHMRRT算法相較于PGRRT算法和BRRT算法搜索冗余度大幅度降低，搜索效率顯著提高；在復雜環(huán)境中，AHMRRT算法相較于PGRRT算法搜索冗余度大大降低，搜索效率顯著提升，但是與BRRT算法相比搜索效率提升并不明顯.

除了以圖6的形式對實驗結果進行了直觀的可視化比較，本文還對AHMRRT、PGRRT、BRRT三種算法在簡單、標準、復雜環(huán)境下的實驗結果進行了定量分析. 定量分析指標為算法中隨機樹搜索總分支數(shù)，路徑規(guī)劃消耗時間即算法收斂時間. 50次實驗中各種算法搜索的平均分支數(shù)和算法平均收斂時間如表1所示. 從定量分析數(shù)據(jù)可以看出，AHMRRT算法相較于PGRRT、BRRT算法在簡單、標準環(huán)境下執(zhí)行路徑規(guī)劃任務具有顯著優(yōu)勢，搜索冗余度更低，搜索速度更快，搜索效率更高. 在簡單、標準環(huán)境下AHMRRT算法在50次仿真實驗中搜索的平均分支數(shù)和路徑搜索耗時遠低于PGRRT、BRRT算法，相較于PGRRT算法搜索冗余度降低67.71%，算法規(guī)劃時間僅需PGRRT算法的36.76%；相較于BRRT算法搜索冗余度降低37.67%，算法規(guī)劃時間僅需BRRT算法的55.93%. 搜索冗余度大幅度降低，路徑規(guī)劃效率顯著提高. 在復雜環(huán)境中，AHMRRT算法相較于PGRRT算法搜索冗余度降低36.32%；算法規(guī)劃時間僅需PGRRT算法的54.39%；搜索冗余度大幅度降低，路徑規(guī)劃效率顯著提高. 然而在復雜環(huán)境中，AHMRRT算法相較于BRRT算法并無優(yōu)勢，造成該結果的原因?qū)⒃?.4節(jié)進行分析.

表1 量化分析Table 1 Quantitative analysis

4.3 局部極小點逃離實驗

為驗證AHMRRT算法在具有狹窄通道的環(huán)境中的有效性和高效性，本文設計了局部極小點逃離實驗. 實驗場景如圖5(d)所示. 起點為S=(160,160)，目標點為G=(360,360);實驗中對RRT、BRRT算法和AHMRRT算法各進行50次仿真實驗.

實驗結果如圖7所示. PGRRT、BRRT、AHMRRT算法的平均消耗時間分別為39.69、30.39、14.74 s. AHMRRT算法的路徑規(guī)劃時間僅需PGRRT算法的37.13%，僅需BRRT算法的48.50%，算法搜索效率大幅度提高；PGRRT、BRRT、AHMRRT算法搜索的平均分支數(shù)分別為573、482、310. AHMRRT算法的搜索冗余度相較于PGRRT算法降低45.90%，相較于BRRT算法降低35.68%. 實驗結果表明，AHMRRT算法可有效逃離路徑規(guī)劃任務中易出現(xiàn)的局部極小點問題.

圖7 U形障礙逃離實驗Fig.7 Experiments of U-shaped obstacle escape

4.4 實驗分析

從以上仿真實驗結果可以看出，在簡單和標準場景中的AHMRRT算法明顯優(yōu)于PGRRT、BRRT算法. 此外，該算法可有效地避免在路徑規(guī)劃過程中容易出現(xiàn)的局部極小點問題.

AHMRRT算法搜索冗余度降低的原因是算法中引入了自適應啟發(fā)式目標偏置因子. 該因子使算法可根據(jù)空間中的障礙物分布情況自適應地改變新節(jié)點的生成策略. 在空閑區(qū)域中生成新節(jié)點將更具有方向性. 當遇到障礙物時，將增加新節(jié)點生成的隨機性，以避免陷入局部最優(yōu). 動態(tài)變化的偏置因子不僅降低了搜索的分支數(shù)，而且確保了AHMRRT算法的概率完整性.

AHMRRT算法的收斂速度大幅度提高的原因是該算法基于多隨機樹構建策略可構造出4棵隨機樹進行同時搜索，大大提高了路徑生成的效率，也提高了算法逃離局部極小點的概率.

在復雜環(huán)境中，AHMRRT算法搜索效率相較于BRRT算法提升并不明顯，原因在于本文中的隨機樹構建策略為了降低算法復雜度，直接將起始點與目標點的中點及其鄰域選為第3節(jié)點的候選區(qū)域，該方法適用于地圖中間無障礙物的簡單、標準環(huán)境. 然而在復雜環(huán)境中通過本文的隨機樹構建策略選擇的第3節(jié)點易處于具有狹窄通道的約束區(qū)域，算法在逃離約束區(qū)域的過程中反而增加了算法的收斂時間和隨機樹總分支數(shù).

5 結論

1) 本文提出了一種基于BRRT算法的改進路徑規(guī)劃算法——AHMRRT算法. AHMRRT算法基于多隨機樹構建策略可以同時構建4棵隨機樹，并通過引入自適應啟發(fā)式目標偏置因子使算法可自適應地調(diào)整新節(jié)點的生成策略. 實驗結果表明，AHMRRT算法在簡單、標準環(huán)境中具有更高的搜索效率和更低的冗余搜索且可避免在路徑規(guī)劃過程中容易出現(xiàn)的局部極小點問題.

2) 在多隨機樹構建策略中為降低算法復雜度，選取起始點與目標點的中點及其鄰域作為子目標點的候選區(qū)域，在復雜環(huán)境中可能并不適用. 針對復雜環(huán)境中子目標點的選取還需要進一步研究，以提高AHMRRT算法的泛化能力.