基于稀疏節(jié)點與雙向插值的RRT*改進算法

王國安，姜春英，陶廣宏，葉長龍

(沈陽航空航天大學機電工程學院，遼寧沈陽 110136)

0 前言

隨著機器人技術的不斷發(fā)展，路徑規(guī)劃作為智能機器人技術發(fā)展的關鍵，得到了高速的發(fā)展。路徑規(guī)劃研究的問題可以描述為：在地圖環(huán)境已知或部分已知的情況下，規(guī)劃一條起點到目標點且與障礙物無碰撞的有效路徑。根據地圖環(huán)境的狀況，路徑規(guī)劃算法可以分為全局路徑規(guī)劃和局部路徑規(guī)劃。常用的路徑規(guī)劃算法主要有以下幾種：圖搜索算法[1]、人工勢場法[2]、基于隨機采樣的算法[3]、智能算法[4-6]和融合深度學習的算法[7]等。

RRT算法是基于采樣的算法中典型代表之一[8]。RRT算法通過隨機采樣在空間中獲取一組離散點，經過碰撞檢測，進而形成一條無碰撞路徑。與傳統(tǒng)路徑規(guī)劃算法相比，RRT算法具有概率完備[9]、易于實現(xiàn)等優(yōu)點，但由于沒有考慮路徑的長度問題，往往生成的路徑較為曲折，不能保證結果為最優(yōu)路徑或次優(yōu)路徑。因此，眾多學者也為之開展了許多探索與研究。一方面，為了加快算法收斂速度，文獻[10]提出了RRT-Connect算法，其改進點在于同時從起點和目標點建立隨機樹并擴展，同時也提出了一種多樹擴展思想，加快了算法收斂速度；文獻[11]中的Bias-RRT算法通過生成隨機樹的隨機樣本的概率分布影響算法的收斂速度，將目標點以更高的概率添加到分布中，這種方法極大地提高了算法向目標點的擴展速度；另一方面，為了得到最優(yōu)或近優(yōu)路徑，文獻[12]提出了RRT*算法，核心在于選擇最優(yōu)父節(jié)點和重連兩個優(yōu)化過程，進而通過不斷迭代得到最優(yōu)解，但此算法仍然存在計算時間長的不足；文獻[13]和文獻[14]分別提出了Informed RRT*算法和RRT*-SMART算法，前者提出超球概念來縮小節(jié)點采樣范圍，后者則通過選擇關鍵節(jié)點來限制節(jié)點采樣范圍，均可以在一定程度上加快收斂速度；文獻[15]提出了隨機樹被動生長方法，與引力函數相結合，提高了算法的運行速度；文獻[16]提出的Q-RRT*算法，通過三角不等式可大量跳過路徑中不必要的冗余節(jié)點，進而極大地縮短了路徑長度，達到次優(yōu)路徑。文獻[17]提出了一種目標偏置擴展和CantmullRom樣條插值的雙向RRT*路徑規(guī)劃算法，縮短路徑的同時對路徑進行了平滑處理。文獻[18]提出一種基于跳點搜索(JPS)策略的RRT*算法，通過跳點搜索策略減少尋路過程中計算節(jié)點的數量，提高了RRT*算法的搜索效率，加快了收斂速度。

以上述文獻為基礎，針對RRT*算法存在的精度低、環(huán)境適應性差等問題，本文作者提出了一種基于稀疏節(jié)點與雙向插值的RRT*改進算法，并通過仿真實驗驗證了該算法的有效性。

1 問題與方法描述

1.1 問題描述

路徑規(guī)劃中，一般把整個空間定義為X。根據與障礙物之間的狀態(tài)，又劃分為Xobs和Xfree，Xobs代表障礙物區(qū)域；Xfree代表無障礙物區(qū)域，且Xfree=X/Xobs。將(X，xstart，xgoal)定義為一個路徑規(guī)劃問題，其中xstart∈Xfree為初始狀態(tài)，xgoal?Xfree為目標區(qū)域。在此前提下，通過已知的(X，xstart，xgoal)，得到一條從xstart到xgoal的無碰撞路徑。

1.2 標準RRT*算法

RRT*算法是通過樹T=(V，E)的形式來探索空間，其中V是節(jié)點的集合，E是節(jié)點之間連接關系的集合。RRT*算法首先通過隨機采樣函數Sample()在地圖Map上得到隨機點xrand；使用Nearest(V,xrand)尋找，獲得距離xrand最近的點xnearest；如果xrand與xnearest的距離大于ε，則以步長ε擴展新節(jié)點xnew，否則使用原距離擴展新節(jié)點xnew；如果xnew和xnearest之間與障礙物未發(fā)生碰撞，則找出xnew的鄰域Vnear，通過ChooseParent(Vnear,xnew)函數在鄰域Vnear中重新選擇xnew的父節(jié)點，若Vnear中存在點使得xstart到xnew的路徑距離更短，則替換xnew的父節(jié)點，否則不操作；再通過Rewire(Vnear,xnew)函數進行重新布線，若Vnear中的點的父節(jié)點改為xnew可以縮短路徑，則進行父節(jié)點更新，否則不操作。迭代優(yōu)化過程賦予RRT*算法漸進最優(yōu)性[12]。RRT*算法的偽代碼如算法1所示。

算法1 RRT*

Input:xstart,xgoal, Map

Output:T=(V,E)

1: fori=1 toKdo

2:xrand=Sample();

3:xnearest=Nearest(V,xrand);

4:xnew=NewNode(xnearest,xrand,ε);

5: If CollisionFree(xnew,xnearest, Map)then

6:Vnear←Near(T,xnew,rnear);

7:xparent←ChooseParent(Vnear,xnew);

8:T←E(xnew,xparent);

9:T←Rewire(Vnear,xnew)

10: end if

11: end for

2 改進RRT*算法

在RRT*算法中，采樣點在空間均勻采樣，使得算法向目標點擴展速度慢；并且，在迭代過程中，隨機樹節(jié)點大量增加，算法計算量明顯增大，導致算法效率低下。針對RRT*算法存在的收斂速度慢、效率低等問題，本文作者提出了基于稀疏節(jié)點與雙向插值的RRT*改進算法。

改進算法在找到初始路徑前，首先依據目標偏向策略確定好采樣點；再根據步長獲取新節(jié)點；通過稀疏節(jié)點法提前判斷新節(jié)點是否滿足擴展要求，避免算法在局部區(qū)域過度搜索；在找到初始路徑后，通過雙向插值優(yōu)化，對路徑進行優(yōu)化迭代；在達到迭代次數上限或運行時間上限后，視為達到收斂目標，終止迭代。改進的RRT*算法流程如圖1所示。

圖1 改進算法流程

2.1 目標偏向采樣

鑒于RRT*算法所獲得的采樣點存在隨機性與盲目性，使得算法搜索效率低，本文作者采用目標偏向采樣[11]，使隨機樹在采樣過程中，以一定的概率朝著目標點生長。目標偏向采樣的具體內容為：首先，設置目標偏向概率閾值Q(Q∈(0，1))， 采用高斯隨機數Qrand(Qrand∈(0，1))與上述概率閾值Q相比較。當Qrand

2.2 稀疏節(jié)點法

在RRT*算法中，經過多次迭代，隨機樹節(jié)點擴展的隨機性會使得局部區(qū)域被節(jié)點重復擴展，造成局部區(qū)域內節(jié)點過多。若對節(jié)點擴展不加以限制，將導致算法迭代時間增多，算法效率下降。故本文作者采用稀疏節(jié)點法對節(jié)點擴展加以限制，避免節(jié)點在局部區(qū)域內過度搜索、重復擴展，增強算法的搜索效率，以此來縮短獲得初始路徑的時間。

如圖2所示，對節(jié)點xnew進行稀疏節(jié)點篩選，判斷該節(jié)點是否為可擴展節(jié)點。經過目標偏向采樣得到xrand后，先得到其最近節(jié)點xparent；再以xparent為父節(jié)點，ε為步長，向xrand方向擴展得到xnew；此時，以xnew為中心，以ε/2為半徑畫圓，若圓內存在其他已擴展節(jié)點，則xnew未通過節(jié)點篩選；而以xnew2為中心，以ε/2為半徑畫圓，圓內無其他節(jié)點，則xnew2通過篩選。

圖2 稀疏節(jié)點法示意

此改進算法中，稀疏節(jié)點法的偽代碼如算法2所示。

算法2 Choosepoint(T,xnew)

1:xnewnear=Nearest(V,xnew);

2: if |xnew-xnewnear|<ε/2 then

3: return False;

4: else

5: return True

6: end if

2.3 雙向插值優(yōu)化

通過RRT*算法得到的初始路徑往往較為曲折，同時依然存在大量的冗余節(jié)點。因此，需對該路徑進行優(yōu)化處理，以便獲得更加平穩(wěn)的路徑。鑒于此，本文作者采用三角不等原理與雙向插值算法對路徑進行優(yōu)化，以獲得最優(yōu)的路徑規(guī)劃結果。

該優(yōu)化過程存在兩個核心過程：(1)重新選擇路徑中節(jié)點的父節(jié)點，去除路徑中的冗余節(jié)點，該過程定義為算法3中的最優(yōu)父節(jié)點函數FindbestParent(xnode)；(2)通過雙向插值函數創(chuàng)造一個新的節(jié)點，用以代替路徑中節(jié)點的父節(jié)點，使父節(jié)點所處的位置更優(yōu)，該過程被定義為算法3中的雙向插值函數SimDic(xnode,xallow, Parent(xnode))。

在最優(yōu)父節(jié)點函數中，以路徑中的某一點xchild為例，定義xchild的最優(yōu)父節(jié)點為xpbest，xpbest的父節(jié)點為Parent(xpbest)。如圖3所示，在尋找最優(yōu)父節(jié)點中，xchild需依次遍歷路徑上xchild之前的路徑節(jié)點，在遍歷過程中，需進行碰撞檢測，若xchild與xpbest之間的路徑未發(fā)生碰撞，則繼續(xù)向前遍歷節(jié)點Parent(xpbest)，此時，xchild與Parent(xpbest)之間的路徑發(fā)生碰撞，則遍歷終止，選擇xpbest作為xchild的最優(yōu)父節(jié)點。

圖3 最優(yōu)父節(jié)點示意

在雙向插值函數中，設置參數D為二分法的終止條件，β為算法4中xallow與xgparent之間的距離。

在得到最優(yōu)父節(jié)點xpbest后，以xchild、xpbest和Parent(xpbest)為支點，運用雙向插值函數，創(chuàng)建新節(jié)點xallow。如圖4(a)所示，先以xchild為支點，向xpbest和Parent(xpbest)的連線上，運用二分法與碰撞檢測，得到替代節(jié)點xforbid；如圖4(b)所示，再以Parent(xpbest)為支點，向xforbid和xchild的連線上，在多次二分法尋找后，觸發(fā)終止條件，則替代節(jié)點xallow為xforbid。

圖4 雙向插值示意

雙向插值優(yōu)化全過程可總結為：在得到xchild的最優(yōu)父節(jié)點xpbest后，運用雙向插值函數得節(jié)點xallow，若經過優(yōu)化，則以xallow替換xpbest；同時更新xchild的父節(jié)點信息。

路徑優(yōu)化函數PDpath(T)函數的整體流程如算法3所示；雙向插值函數的偽代碼如算法4所示。

算法3 PDpath(T)

1:xchild=xgoal;

2:xpbest=Findbest Parent(xgoal);

3: whilexpbest≠xstartdo

4:xallow=SimDic(xchild,xpbest, Parent(xpbest));

5:xallow=SimDic(Parent(xpbest),xallow,xchild);

6: ifxallow≠xpbestthen

7:V(xpbest)=V(xallow);

8: end if

9:E=E∪E(xchild,xpbest);

10:xchild=xpbest;

11:xpbest=FindbestParent(xchild);

12: end while

13: ReturnT;

算法4 SimDic(xnode,xallow,xgparent)

1: while Distance(xallow,xgparent)>Ddo

2:xmid=(xallow+xgparent)/2;

3: if CollisionFree(xmid,xnoed)then

4:xallow=xmid;

5: else

6:xgparent=xmid;

7: end if

8: end while

9: Returnxallow;

3 仿真實驗

3.1 稀疏節(jié)點仿真實驗

對RRT*算法和運用稀疏節(jié)點的RRT*算法進行仿真分析，通過仿真實驗，驗證稀疏節(jié)點法在得到初始路徑時間和降低隨機樹節(jié)點個數的有效性。

在狹窄、U形環(huán)境中，分別對兩種算法進行仿真分析。如圖5所示，運用稀疏節(jié)點的RRT*算法中局部區(qū)域中的隨機樹節(jié)點密度更小，稀疏節(jié)點法避免了局部區(qū)域過度搜索。地圖大小為500像素×500像素，步長為20，分別運行50次，得到獲取初始路徑的時間tfind、隨機樹節(jié)點個數T和迭代次數K，并取平均值得表1。由表1可知，稀疏節(jié)點法在尋找初始路徑方面，有利于縮短得到初始路徑的時間，降低隨機樹節(jié)點個數。

表1 算法對比結果

圖5 算法比較

3.2 文中改進算法仿真實驗

仿真實驗配置為：64位Window10，處理器Intel(R)Core(TM)i7-7500U，CPU主頻2.7 GHz，內存8 GB。

為了對比文中改進算法的性能，對RRT*、Informed-RRT*、Q-RRT*和文中所述改進算法在3種仿真環(huán)境中進行算法驗證，如圖6所示，地圖大小均為500像素×500像素。3種環(huán)境下分別將4種算法各運行100次，使用以下4個參數的平均值對算法進行評價：發(fā)現(xiàn)初始路徑時間tfind、發(fā)現(xiàn)次優(yōu)路徑時間t5%、初始路徑長度linit以及算法路徑長度lpath。

圖6 仿真地圖

改進算法中參數及變量設置：步長ε設置為20像素，搜索半徑rnear為40像素，目標偏向概率Q設置為0.2 ，雙向插值距離系數D設為2像素。

3.2.1 仿真環(huán)境一

仿真環(huán)境一中，xstart設置為[50，50]像素，xgoal設置為[450，450]像素。算法仿真結果如圖7所示。

圖7 仿真環(huán)境一仿真結果

由圖7可知：在仿真環(huán)境一，文中改進算法路徑最優(yōu)，貼合障礙物邊緣；其次是Q-RRT*算法的規(guī)劃結果；其他兩種算法路徑較為曲折，存在較多冗余節(jié)點。仿真環(huán)境一下算法對比結果如表2所示，算法規(guī)劃的路徑長度如圖8所示。

表2 仿真環(huán)境一的路徑規(guī)劃結果

圖8 仿真環(huán)境一的路徑長度

由表2可知：文中改進算法性能最好，相較于Q-RRT*算法，tfind縮短了71.7%，雖然初始路徑linit較長，但次優(yōu)路徑時間t5%縮短了26%；相較于RRT*算法和Informed-RRT*算法，tfind縮短了48.2%和45.5%，t5%縮短了76.5%和67.1%。如圖8所示：改進算法得到初始路徑的時間最短，并且路徑收斂時間最短。

綜上所述：在仿真環(huán)境一，文中改進算法相較于其他3種算法，tfind、t5%最短；相同時間下，算法收斂時間最快，路徑最短。

3.2.2 仿真環(huán)境二

仿真環(huán)境二中，xstart設置為[50，50]像素，xgoal設置為[450，450]像素，算法仿真結果如圖9所示。

圖9 仿真環(huán)境二仿真結果

由圖9可知：在仿真環(huán)境二下，文中改進算法性能優(yōu)于其他3種算法。仿真環(huán)境二下算法對比結果如表3所示，算法規(guī)劃的路徑長度如圖10所示。

表3 仿真環(huán)境二的路徑規(guī)劃結果

圖10 仿真環(huán)境二的路徑長度

由表3可知：文中改進算法性能最好，相較于Q-RRT*算法，tfind縮短了68.4%，次優(yōu)路徑時間t5%縮短了47.2%；相較于RRT*算法和Informed-RRT*算法，tfind縮短了48.4%和46.7%，t5%縮短了72.5%和67.5%。

圖10中，RRT*算法和Informed-RRT*算法的曲線幾乎重合，這是由于后者的橢球采樣優(yōu)化在迷宮地圖中難以發(fā)揮作用；文中改進算法路徑收斂時間最短。

綜上所述，相較于其他3種算法，文中改進算法得到初始路徑的時間最短，路徑收斂效果最好。

3.2.3 仿真環(huán)境三

仿真環(huán)境三中，xstart設置為[50，50]像素，xgoal設置為[450，450]像素。算法仿真結果如圖11所示。

圖11 仿真環(huán)境三的仿真結果

由圖11可知：在仿真環(huán)境三下，文中改進算法性能優(yōu)于其他3種算法。仿真環(huán)境三下算法對比結果如表4所示，算法路徑長度如圖12所示。

圖12 仿真環(huán)境三的路徑長度

由表4可知：文中改進算法性能最好，相較于Q-RRT*算法、RRT*算法和Informed-RRT*算法，tfind分別縮短了73.8%、55.6%和56.3%，次優(yōu)路徑時間t5%分別縮短了45.9%、70.4%和63%。由圖12可知：文中改進算法在收斂速度和路徑長度上有較高提升。

4 路徑規(guī)劃實驗

基于ROS機器人操作系統(tǒng)，在移動機器人上對文中改進算法進行驗證。機器人采用Ubuntu 16.04系統(tǒng)，ROS版本為Kinetic，采用單線激光雷達進行障礙物檢測。如圖13所示，為改進算法的路徑規(guī)劃圖。

圖13 改進算法路徑規(guī)劃圖

驗證過程如圖14所示，圖中顯示了機器人運動的位置。

圖14 實際路徑規(guī)劃結果

結果顯示：機器人路徑規(guī)劃結果精度較高，機器人可沿著當前規(guī)劃路徑進行無碰撞運動。說明改進算法具有較好的優(yōu)化能力，對機器人路徑規(guī)劃具有實用價值。

5 結論

提出一種基于稀疏節(jié)點與雙向插值的改進RRT*算法。該算法運用稀疏節(jié)點法縮短了初始路徑尋找時間；在后續(xù)迭代中，使用雙向插值優(yōu)化路徑節(jié)點，有效縮短次優(yōu)路徑時間。

在3種環(huán)境下的仿真結果表明：文中改進算法在tfind和t5%上，其平均值分別縮短了約61%和約59%。然后，將所提算法運用在實際機器人路徑規(guī)劃，實驗結果表明：改進算法有一定應用價值。文中算法在規(guī)劃過程中提高了尋路質量和效率，收斂速度更快。