基于RRT*改進的移動機器人路徑規劃算法

梁永豪，陳秋蓮，王成棟

(廣西大學 計算機與電子信息學院，廣西 南寧 530004)

0 引 言

機器人路徑規劃，是通過算法在有障礙物的環境中找到一條從起點到目標點的無碰撞路徑。路徑規劃算法作為移動機器人的關鍵技術之一，受到國內外學者的廣泛關注。

目前，路徑規劃算法主要包括遺傳算法[1]、粒子群算法[2]、人工勢場法[3]、基于網格的算法[4]和基于采樣的算法[5，6]?；诓蓸拥乃惴ㄍㄓ眯员容^強，受環境維度和復雜度的約束較小[7]，是當前最流行的路徑規劃算法之一?？焖贁U展隨機樹(RRT)[8]是基于采樣算法的典型代表，依靠隨機采樣和碰撞檢測模塊，在給定的空間內生成一組離散的點。雖然RRT可以快速找到路徑，但它不能確保找到的路徑是最優的。RRT*[9]是RRT算法的變體，在添加新節點時引入兩個優化模塊：重選父節點和重布線。優化模塊確保在采樣點趨于無窮時，RRT*算法找到最優解。

RRT*是RRT算法發展的一個里程碑，吸引了很多學者，主要從兩個方面對算法的性能進行改進：采樣策略和樹擴展策略。采樣策略主要是通過優化采樣點，加速算法收斂。如知情采樣Informed-RRT*[10]限制采樣點在橢圓形采樣區域內；目標偏置[11]和人工勢場法[12]利用目標點的信息來指導新節點生成方向；Bi-RRT*[13]則采用雙樹拓展的同時引入啟發式思想，加快收斂速度。樹擴展策略則是對擴展樹本身的結構進行優化，使路徑長度盡可能短。如Quick-RRT*[14]使用三角不等式來優化重選父節點和重布線模塊，縮短路徑長度；F-RRT*[15]在Quick-RRT*的基礎上以創造父節點替代重選父節點；KB-RRT*[16]通過添加運動學約束，減少不必要的節點生長，獲取滿足運動學約束，路徑更短的擴展樹。但這些改進依舊存在不足，如F-RRT*路徑規劃的效率還有不少的提升空間，同時在復雜障礙物環境下會產生較多的冗余節點；目標偏置在局部最優環境或者迷宮下，性能顯著下降；人工勢場法在多障礙物環境下計算量大，效率比較低。

針對RRT*中存在的盲目搜索、路徑較長和冗余節點多而導致路徑規劃效率低，本文提出一種融合樹擴展策略和采樣策略的改進RRT*算法(AF-RRT*)。采樣策略方面，采用動態步長，減少冗余節點；利用自適應探索，減少采樣的盲目性，加快路徑規劃速度，同時避免陷入局部最優；樹擴展策略方面，引入創造父節點，并與采樣策略有效融合，減少路徑長度。

1 相關工作

1.1 RRT算法

RRT的搜索過程類似于樹不斷地向四周擴散生長。起點xstart為擴展樹G的根節點，樹中節點用集合V表示，節點間的連接用集合E存儲。在自由空間隨機采樣節點xrand引導樹的生長方向。找到離隨機點最近的節點xnearest，在xnearest和xrand連線上以步長eta生成新節點xnew。若最近鄰節點與新節點之間的路徑與障礙物無碰撞，則將新節點加入樹中，否則重新采樣。重復上述過程，直到新節點擴散到目標點，完成整個搜索，返回該節點到根節點的連線作為路徑。偽代碼如算法1所示：

Algorithm1:RRT()

Input:xstart,xgoal,Map,K,eta,

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) xnew← NewState(xnearest,xrand,eta);

(6)ifCollisionFree(xnew,xnearest)then

(7) V ← V ∪ {xnew};

(8) E ← E ∪ {(xnearest,xnew)};

(9)ifDistance(xgoal,xnew) <λthen

(10)returnG=(V,E);

(11)endif

(12)endif

(13)endfor

其中函數SampleFree()在自由空間內生成隨機節點；Nearest()利用歐式距離選擇樹節點中離隨機節點最近的節點；NewState()從兩點連線方向上擴展固定步長，得到新節點；CollisionFree()判斷給定兩點間的連線是否在自由空間。

1.2 RRT*算法

RRT的隨機擴展性使算法穩定性差，無法收斂至全局最優。RRT*在樹擴展過程中，確定隨機點位置后，對新加入搜索樹的新節點，增加重選父節點和重布線操作。

RRT*的偽代碼如算法2所示，第(7)行和第(10)行分別為重選父節點ChooseParent()和重布線Rewire()。通過隨機點的增加實現路徑的漸進優化，當隨機點數量趨于無窮時能夠獲得最優解。

Algorithm2:RRT*()

Input:xstart,xgoal,Map,K,eta,Rnear

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) xnew← NewState(xnearest,xrand,eta);

(6)ifCollisionFree (xnew,xnearest)then

(7) xmin← ChooseParent(xnew,V,Rnear);

(8) V ← V ∪ {xnew};

(9) E ← E ∪ {(xmin,xnew)};

(10) G ← Rewire(V,E,xnew,Rnear);

(11)ifDistance(xgoal,xnew) <λthen

(12)returnG=(V,E);

(13)endif

(14)endif

(15)endfor

(1)重選父節點ChooseParent()函數：以新節點xnew為圓心，產生以Rnear為半徑的鄰域Xnear，以鄰域內的節點為備選父節點。首先計算xnew與鄰域內所有節點連接后的權值大小，若鄰域內中存在節點xmin，使以xmin為父節點的xnew權值最小，則刪除xnew與原父節點xnearest的邊，在樹中新增xnew與xmin的邊集，如圖1(a)所示。

圖1 RRT*的樹擴展

(2)重布線Rewire()函數：重布線則是在RRT采樣后加入了剪枝過程，優化新節點附近的舊節點。若鄰域內存在節點的權值大于以xnew為父節點到該節點的權值，則剪掉這些節點與原父節點的連線，以xnew作為新的父節點，更新鄰域內其它節點的路徑長度，如圖1(b)所示。

2 AF-RRT*算法

RRT*算法能漸進達到最優，但存在隨機性過大、路徑代價大和冗余節點等不足，計算量隨節點數量的增加而急劇增加，需要較長的時間才能逼近最優解。本文提出一種融合樹擴展策略和采樣策略的AF-RRT*算法，提高路徑規劃效率。

2.1 AF-RRT*算法描述

AF-RRT*在RRT*基礎上，進行3方面的改進：①動態步長，減少由于震蕩現象導致的冗余節點；②自適應探索，減少采樣的隨機性；③創造父節點代替RRT*的重選父節點，使擴展樹貼近障礙物生長，減短路徑長度。偽代碼如算法3所示：

Algorithm3:AF-RRT*()

Input:xstart,xgoal,Map,K,Ddich,eta,Rnear,Cnow,Ccol,Pgoal,Prand

Output:G=(V,E)

(1) V ← {xstart},E ← ?;

(2)Fori=1 to Kdo

(3) xrand← SampleFree(i);

(4) xnearest← Nearest(V,xrand);

(5) Eta ← DSS(xnearest,xgoal,eta);

(6) xnew← AES(xrand,xgoal,xnearest,Eta,&Cnow,Ccol,Pgoal,Prand);

(7)ifxnew≠ xrandthen

(8) xreachest← FindReachest(G,xnearest,xnew,xstart);

(9) xcreate←CreateNode(G,xreachest,xnew,Ddich,xstart);

(10)ifxcreate≠ ?then

(11) V ← V ∪ {xcreate,xnew};

(12) E←E∪{(Parent(xreachest),xcreate),(xcreate,xnew)};

(13)else

(14) V ← V ∪ {xnew};

(15) E ← E ∪ {(xreachest,xnew)};

(16)endif

(17) G ← Rewire(V,E,xnew,Rnear);

(18)ifDistance(xgoal,xnew) <λthen

(19)returnG = (V,E);

(20)endif

(21)endif

(22)endfor

其中函數DSS()根據當前最鄰近節點與目標點的距離動態調整步長；AES()根據擴展樹各節點位置，進行自適應探索，生成新節點；FindReachest()搜索與新節點連線不經過障礙物的最遠祖先節點；CreateNode()生成創造節點，在障礙物邊緣連接最遠祖先節點和新節點。

算法在第(1)、第(2)行初始化擴展樹，沒達到最大迭代次數時，進入循環。

第(3)～第(7)行產生新節點。確定隨機點xrand與最近節點xnearest后，DSS()根據xnearest與目標點的距離計算動態步長，AES()使用當前步長生成新節點xnew。

第(8)～第(16)行是創造父節點的過程，分為兩步。FindReachest()找到新節點xnew的最遠祖先節點xreachest。CreateNode()根據xnew和xreachest的位置，在障礙物邊緣生成創造節點xcreate。若xcreate不為空，則將xnew和xcreate加入擴展樹；否則將xnew加入擴展樹。

第(17)～第(22)行是重布線和算法終止條件。

2.2 動態步長

RRT*算法生成新節點xnew后，判斷其是否在目標點xgoal的λ鄰域，若是，則找到一條可行路徑。為了保證算法的效率，通常取步長為eta>λ。但是，若xnearest與xgoal的距離在(λ，eta)之間，使用固定步長進行擴展會造成目標點附近的震蕩。大地圖中采用大擴展步長時震蕩現象尤為明顯，會增加路徑規劃時間和冗余節點。DSS()函數通過動態步長來解決震蕩問題。每次擴展，先計算xnearest和xgoal的距離Dis，若Dis>eta時，則擴展步長不變；否則，將擴展步長調整為Dis。

2.3 自適應探索

擴展樹新節點的探索可以分為強探索和弱探索。強探索狀態下擴展樹往隨機節點生長，增加多樣性；弱探索狀態下擴展樹往目標點生長，縮短搜索時間。AES()函數可根據擴展樹的探索狀態生成新節點。

Function:AES()

Input:xrand,xgoal,xnearest,Eta,Cnow,Ccol,Ccont,Pgoal,Prand

Output:xnew

(1) xnew← xrand;

(2) P ← Estatus(Cnow,Ccol,Pgoal,Prand);

(3) xpnew← xnearest+P*(xgoal-xnearest)*Eta+(1-P)*(xrand-xnearest)*Eta

(4)ifCollisionFree (xnew,xnearest)then

(5) xnew← xpnew;

(6)else

(7) xpnew← xnearest+(1-P)*(xgoal-xnearest)*Eta+P*(xrand-xnearest)*Eta

(8)ifCollisionFree (xnew,xnearest)then

(9) xnew← xpnew;

(10)else

(11) Cnow← Cnow+1

(12)endif

(13)endif

(14)returnxnew

其中Estatus()為擴展樹的探索狀態函數。初始狀態為弱探索，Cnow為擴展樹新節點生成失敗次數。Cnow除以Ccol取整，若為奇數，則返回值為Prand，擴展樹進入強探索狀態；否則，返回值為Pgoal，擴展樹進入弱探索狀態。Prand+Pgoal=1。調整參數Ccol，可控制擴展樹探索狀態切換的速度。

在確定P值后，新節點由目標點和隨機點共同確定。目標點和隨機點兩個方向的權值分別是P和(1-P)，進行矢量合成確定新節點的擴展方向和擴展距離。

新節點坐標由式(1)確定，其中Eta為擴展步長

(1)

弱探索狀態時，擴展樹偏向目標點生長，如圖2黑色平行四邊形所示。若新節點xnew和最近點xnearest的連線不經過障礙物，則擴展成功；否則，交換隨機點和目標點的權值，使樹偏向隨機點擴展，如圖2灰色平行四邊形所示。再次進行碰撞檢測，若通過則擴展成功；否則失敗次數加1，重新采樣。通過交換目標點和隨機點兩個方向的權值，快速繞開障礙物。

圖2 新節點擴展的自適應探索

2.4 創造父節點

RRT*在現有的樹節點中重選父節點，如果鄰域內路徑代價普遍比較大，父節點重選作用不大。增大鄰域半徑可以擴大父節點選擇范圍，但鄰域內節點數量增加會顯著增加計算量。本文以創造父節點代替重選父節點，在障礙物邊緣創造一個節點作為新節點的父節點，使擴展樹貼近障礙物生長，降低路徑代價。創造父節點由兩個步驟來完成：查找最遠祖先節點和創造新節點。

(1)查找最遠祖先節點：偽代碼見函數FindReachest()。把最鄰近節點賦給xreachest。判斷xreachest的父節點與xnew的連線是否在自由空間，若是則把xreachest的父節點賦給xreachest，繼續搜索其父節點。否則返回當前xreachest。

Function:FindReachest()

Input:G,xnew,xnearest,xstart

Output:xreachest

(1) xreachest← xnearest;

(2)Whilexreachest≠ xstartdo

(3)ifCollisionFree(xnew,Parent(xreachest))then

(4) xreachest← Parent(xreachest);

(5)else

(6)returnxreachest

(7)endif

(8)endwhile

(9)returnxreachest

由于只搜索xnew的祖先節點，不搜索xnew周圍的節點，搜索節點數量大幅度減少，降低了搜索時間。

(2)創造新節點：在xreachest及其父節點之間的連線上找到節點xallow，使得xallow與xnew的連線L位于障礙物邊緣。在連線L上找到一點xcreate，使xcreate可同時連接xnew和xreachest的父節點，如圖3粗實線路徑所示。從圖3中可以明顯看出粗實線路徑比虛線路徑短。與重選父節點的路徑(圖1(a))相比，路徑長度更是短得多。

圖3 創造父節點過程

創造新節點過程的偽代碼見函數CreateNode()，第(4)～第(11)行找到節點xallow確定連線L的位置，第(13)～第(20)行找到節點xcreate。其中參數Ddich用來控制新節點與障礙物的逼近程度。Ddich越小，路徑長度越短，計算時間越多。調整Ddich的大小，可以平衡路徑長度和時間代價。

Function:CreateNode()

Input:G,xreachest,xnew,Ddich,xstart

Output:xcreate

(1) xallow← xreachest;

(2)ifxreachest≠ xstartthen

(3) xforbid← Parent(xreachest);

(4)WhileDistance(xallow,xforbid) >Ddichdo

(5) xmid← (xallow+xforbid)/2;

(6)ifCollisionFree(xnew,xmid)then

(7) xallow← xmid;

(8)else

(9) xforbid← xmid;

(10)endif

(11)endwhile

(12) xforbid← xnew;

(13)WhileDistance(xallow,xforbid) >Ddichdo

(14) xmid← (xallow+xforbid)/2;

(15)ifCollisionFree(Parent(xreachest),xmid)then

(16) xallow← xmid;

(17)else

(18) xforbid← xmid;

(19)endif

(20)endwhile

(21)endif

(22)ifxallow≠ xreachestthen

(23) xcreate← xmid;

(24)else

(25) xcreate← ?;

(26)endif

(27)returnxcreate

3 實 驗

仿真實驗通過python實現，實驗環境為：Windows10，Intel(R) Core(TM) i7-5500U，主頻2.40 GHz，內存8 G。地圖中S點為起點，T點為終點，黑色矩形為障礙物，白色為自由區域，黑色細線是算法在路徑規劃中生成的擴展樹。為驗證算法的有效性，在4個不同障礙物的全局環境，分別與原始算法RRT*、F-RRT*進行對比實驗。地圖大小均為640×480，公共參數設置：初始步長eta=40，目標點鄰域λ=15，搜索半徑Rnear=45，Ddich的取值為2，Ccol的取值為50，Prand取值為0.2，Pgoal取值為0.8。

3.1 仿真實驗

3.1.1 簡單障礙物環境

簡單障礙物環境如圖4所示，只有一個矩形障礙物。3種算法分別進行100次路徑規劃所得的平均數據，如表1所示。

表1 簡單障礙物環境下的對比

圖4 簡單障礙物環境的路徑規劃結果

在擴展節點數和路徑規劃時間這兩個指標上，AF-RRT*明顯優于RRT*和F-RRT*。AF-RRT*擴展節點數比RRT*少75.25%，路徑規劃時間少90.19%，路徑長度短11.52%。AF-RRT*與F-RRT*的路徑長度接近，但是擴展節點數減少77.43%，路徑規劃時間減少90.48%。

3.1.2 迷宮障礙物環境

地圖上中有兩個平行的矩形組成一個簡單迷宮，如圖5所示。3種算法100次路徑規劃所得的平均數據見表2。AF-RRT*的路徑長度與F-RRT*算法接近，擴展節點數量和路徑規劃時間遠少于其它兩種算法。AF-RRT*相比于F-RRT*，擴展節點數減少41.30%，路徑規劃時間減少63.25%。

表2 迷宮障礙物環境下的對比

圖5 迷宮障礙物環境的路徑規劃結果

3.1.3 凹形障礙物環境

地圖中有一個凹形障礙物，如圖6所示。3種算法100次路徑規劃所得的平均數據見表3。

表3 凹形障礙物環境下的對比

圖6 凹形障礙物環境的路徑規劃結果

AF-RRT*相比于F-RRT*，擴展節點數減少48.43%，路徑規劃時間減少69.21%。

3.1.4 復雜障礙物環境仿真

地圖上中有許多各種不同類型的矩形障礙物，如圖7所示。

圖7 復雜障礙物環境的路徑規劃結果

3種算法在復雜障礙物環境下分別進行100次路徑規劃，平均數據見表4。在擴展節點數和路徑規劃時間這兩指標上，AF-RRT*同樣明顯優于RRT*和F-RRT*。與F-RRT*算法相比，AF-RRT*的擴展節點數減少72.45%，路徑規劃時間減少85.29%。

表4 復雜障礙物環境下的對比

3.1.5 仿真實驗分析

從結果對比可以看出，F-RRT*跟RRT*算法存在著無效搜索和在目標點震蕩的問題。在多障礙物環境下，由于擴展樹需要頻繁繞過障礙物，F-RRT*探索無效區域時在障礙物邊緣大量地創造新節點，這會使擴展節點數和路徑規劃時間大幅度上升。本文算法AF-RRT*利用自適應探索策略和動態步長針對性減少了在盲目搜索和擴展樹震蕩的過程中創造的新節點，使得擴展節點數和路徑規劃時間大大減少。普通環境下，如簡單障礙物和復雜障礙物環境，AF-RRT*的路徑規劃時間比F-RRT*減少80%～90%；在局部最優或者迷宮類環境下，如迷宮障礙物和凹形障礙物，AF-RRT*的路徑規劃時間也比F-RRT*減少60%～70%。

3.2 消融實驗

為進一步檢驗AF-RRT*算法各組成部分的有效性，保持相同參數不變，在復雜障礙物環境對AF-RRT*算法進行消融實驗，如圖8所示。

圖8 AF-RRT*消融實驗的路徑規劃結果

無創造父節點、無自適應探索策略、無動態步長的AF-RRT*和完整的AF-RRT*在多障礙物環境下分別進行100次路徑規劃，平均數據見表5。無創造父節點樹擴展策略的AF-RRT*算法路徑長度較長；無自適應探索策略的AF-RRT*算法，擴展節點數較多和路徑規劃時間較長；無動態步長的AF-RRT*擴展節點數較多。說明創造父節點在使擴展樹緊貼障礙物生長，自適應探索策略在減少盲目搜索，動態步長在減少冗余節點，是有效的。其中自適應探索策略發揮了較為關鍵的作用，對提高AF-RRT*算法的性能做出了較大的貢獻。

表5 AF-RRT*消融實驗數據

4 結束語

本文以RRT*算法為基礎，在樹擴展策略和采樣策略兩方面作了改進。引入創造父節點的樹擴展策略，改變RRT*樹擴展的過程，使擴展樹緊貼障礙物生長，縮短了路徑長度。通過自適應探索策略，縮短了路徑規劃的時間，同時避免陷入局部最優。通過動態步長減少了擴展樹在目標點震蕩。仿真實驗結果表明，AF-RRT*在路徑規劃效率和路徑質量上相較于RRT*和F-RRT*具有較明顯的優越性。消融實驗驗證了各改進點的有效性。