基于改進RRT算法的避障路徑規劃

馮 垚，周志峰，沈亦純，王立端

（1. 上海工程技術大學 機械與汽車工程學院，上海 201620； 2. 上海衛星工程研究所，上海 200240；3. 上海司南衛星導航技術股份有限公司，上海 201801）

隨著機器人技術的快速發展，機器人運動過程中的避障問題受到了學者們的廣泛關注。高效的路徑規劃可以提高機器人的執行效率。因此，研究如何降低避障路徑規劃的時間成本和長度成本具有重要意義。

近年來，國內外學者針對機器人的路徑規劃算法做了大量研究。Fan等[1]提出的人工勢場法引入了力場原理，即：將障礙物附近區域定義為斥力場，將目標點附近區域定義為引力場?；谠撍惴ㄒ巹澋穆窂诫m能實現有效避障，但當引力和斥力的合力為0 N 時，易陷入局部最小值，導致無法規劃出一條無碰撞路徑。Dolgov等[2]提出的A*算法是一種圖形遍歷算法，該算法在簡單環境中易找到最優路徑，但隨著環境復雜程度的提高，每一個節點處的運算量增大，導致路徑規劃效率大幅下降[3-4]。相較于上述算法，LaValle等[5]提出的快速搜索隨機樹（rapid‐ly-exploring random tree, RRT）算法具有在空間內隨機快速采樣的特點，且無需建模，具備空間搜索的完整性[6-7]。但是，由于搜索方向的隨機性，RRT算法的收斂速度慢，且在復雜環境中會產生大量無效節點，導致所規劃路徑的平滑度較差[8-10]。

針對RRT算法在路徑規劃效率上的不足，許多學者對其進行了改進優化。Yuan 等[11]提出的Bias-RRT 算法是在RRT 算法的基礎上引入目標偏置策略，利用偏置概率使隨機樹朝目標點方向拓展。在障礙物較少的環境中，Bias-RRT算法能快速規劃出無碰撞路徑，但在復雜環境中過大的偏置概率會降低規劃速度[12-13]。Karaman 等[14]提出的RRT*算法在RRT算法的基礎上增加了重選父節點以及重新布線兩大策略，以增加時間成本的方式來規劃一條接近最優的路徑。Luo等[15]基于RRT*算法提出了一種In‐formed-RRT*算法，通過將采樣區域限制在橢圓內的方式對規劃路徑進行優化。相比于RRT*算法，Informed-RRT*算法獲取漸進最優路徑的速度有所提升[16]。Kuffner 等[17]提 出 了RRT-Connect 算 法，該算法是在RRT算法的基礎上以目標點為根節點增加了1棵隨機樹，2棵隨機樹同時朝對方進行生長。相較于RRT 算法，RRT-Connect算法在路徑規劃速度上有所提升，但其規劃的路徑仍不夠平滑[18-21]。

綜上所述，已有研究雖通過改進使RRT算法在路徑規劃效率上得到了一定程度的提升，但仍存在環境適應性差、收斂速度慢和規劃路徑質量差等問題。為解決上述問題，筆者提出了一種新的改進RRT算法。首先，在傳統RRT算法中引入地圖復雜程度評估策略，以計算得到最適合相應地圖的步長和偏置概率；然后，利用采樣區域動態更新策略和采樣點優化策略來提高采樣點的有效性及質量，以實現在保留傳統RRT算法隨機性的同時獲取漸進最優采樣點，從而確保隨機樹朝目標點附近正向生長；最后，基于節點重連策略，在重新連接初始路徑節點后進行碰撞檢測，以刪除冗余節點，使得避障路徑更加優化。

1 RRT算法

圖1 RRT算法原理Fig.1 Principle of RRT algorithm

令M表示地圖，T表示隨機樹，k表示迭代次數，則RRT算法的偽代碼如下：

2 改進RRT算法

2.1 地圖復雜程度評估策略

傳統RRT算法不能充分利用地圖中的障礙物信息來自適應調整步長，導致即使面向障礙物較少的簡單地圖，也難以快速規劃出一條漸進最優的路徑。Bias-RRT算法采用目標偏置策略，通過設定偏置概率P來選取目標點作為采樣點，在簡單地圖中可以有效提高路徑規劃的收斂效率[11]。但偏置概率P的取值采用主觀定義，當面向不同環境時，Bias-RRT算法的適用性不足，尤其是對于障礙物較多的復雜地圖，過大的偏置概率P會增加路徑規劃的時間成本，有時甚至可能會導致路徑規劃失敗[12-13]。

此外，傳統RRT算法中隨機樹生長的步長為定值，而同一步長無法適應不同地圖。當在障礙物較少的簡單地圖中進行路徑規劃時，若步長過短，則會增加節點數量，導致規劃時間延長。當在障礙物較多的復雜地圖中進行路徑規劃時，若步長過長，則會增大與障礙物碰撞的概率，導致難以得到一條無碰撞的路徑。

針對上述問題，本文提出了一種地圖復雜程度評估策略，即基于地圖中的障礙物信息計算地圖復雜程度系數C1，從而得到最合適的偏置概率P和步長S。假設地圖的復雜程度主要與地圖中障礙物的總面積及分布情況有關，且兩者對應的權重相等（均為0.5），則地圖的復雜程度系數C1可表示為：

式中：Aobstacle表示障礙物面積；Amap表示地圖面積；Dobstacle表示障礙物分布情況，將地圖劃分為100個均等格子，障礙物所占格子數與100之比即為障礙物的分布情況。

地圖復雜程度系數C1越趨近于1，說明地圖的復雜程度越高，則偏置概率P和步長S的取值應越小。合適的步長S應考慮偏置概率P以及起點到目標點的距離。本文將步長S設為偏置概率P與起點到目標點的距離之積。為確定偏置概率P的最佳取值，在傳統RRT 算法中引入偏置概率P和步長S，并將偏置概率P分別取為1-C1、(1-C1)2、(1-C1)3和(1-C1)4，在同一地圖中分別進行50 次路徑規劃仿真實驗，整理相關數據并對比，結果如表1所示。由表1 可知，當偏置概率P過大時，會導致路徑規劃失??；當偏置概率P過小時，偏置效果變差，導致路徑規劃的時間成本增加；當偏置概率P=(1-C1)3時，路徑規劃的時間成本和長度成本均最低。綜上，偏置概率P和步長S的計算式可表示為：

表1 引入不同偏置概率時RRT算法的路徑規劃結果對比Table 1 Comparison of path planning results of RRT algo‐rithm with different bias probabilities

2.2 采樣區域動態更新策略

傳統RRT算法規劃的無碰撞路徑通常不具有方向性，甚至可能會出現逆向生長，導致路徑規劃的時間成本和長度成本過高。出現上述問題的主要原因在于傳統RRT算法會在無效區域內進行采樣，使得隨機樹生長出過多的無效樹枝。為了避免該情況發生，本文提出了采樣區域動態更新策略，其核心思想是隨著隨機樹的生長，不斷向目標點所在區域縮小采樣范圍，以逐漸逼近目標點。

隨機樹節點的正向生長是指朝當前節點與目標點之間的區域生長，可不斷縮小當前節點與目標點的距離，即有效生長；逆向生長是指朝當前節點與起點之間的區域生長，會不斷擴大當前節點與目標點的距離，即無效生長。如圖2所示，在二維地圖中，根據隨機樹上最新增加的節點Qnew，將搜索空間分為2塊區域，其中目標點所在區域定義為有效區域，即區域Ⅱ，另一塊區域定義為無效區域，即區域I。在下一次隨機采樣時，僅在區域Ⅱ內進行采樣，隨著隨機樹節點的增加，有效區域不斷向目標點收縮，這樣可以保證隨機樹正向生長。但如圖2(a)所示，當節點Qnew生長到障礙物附近且正向生長基本被障礙物阻擋時，由于僅可在有效區域內采樣，RRT算法易陷入局部無解，使得隨機樹無法繼續生長。為解決該問題，在動態更新采樣區域的基礎上引入節點拒絕策略。當隨機樹在當前節點經過50次迭代后仍然無法繼續生長時，即認為該節點無效，將其從隨機樹中刪除，如圖2(b)所示的節點Qvoid即為無效節點。此時，隨機樹的有效采樣區域為基于無效節點的父節點Qparent分割得到的區域Ⅱ，隨機樹在更新后的有效采樣區域內重新開始生長，如圖2(c)所示。

圖2 采樣區域動態更新示意Fig.2 Schematic diagram of dynamic update of sam‐pling area

2.3 采樣點優化策略

傳統RRT算法在地圖中隨機生成的采樣點不具有導向性，導致隨機樹的樹枝生長得雜亂無章。大量低質量采樣點的產生，不僅會增加路徑規劃的時間成本，而且會導致節點冗余，使得路徑規劃的長度成本增加。

“我家種了四畝葡萄，在家正要吃早飯，聽說這邊要舉行爭霸賽，我飯都沒吃，去園子里摘了幾串葡萄就過來了?！币晃恍諒埖拇蠼愀嬖V記者，威縣葡萄的種植面積非常大，幾乎家家戶戶都有種，小到一兩畝，大到幾十畝不等。種植的葡萄種類主要以巨峰為主，紅寶石、維多利亞等品種并存?！斑^來參賽不是說一定要當葡萄王，就是想證明一下自家的葡萄種的不比他們的差，不信你嘗嘗我家葡萄多甜。這葡萄就像自己孩子似的，誰不想讓人夸夸自己孩子呢，你說是不是？”張大姐一邊聊，一邊邀請記者品嘗。

為此，本文設計了一種采樣點優化策略。如圖3所示，在每一次采樣時，同時隨機產生3個待定采樣點Qrand1、Qrand2、Qrand3并構成集合，計算集合中每個待定采樣點到目標點的距離，選取距離目標點最近的待定采樣點作為最終采樣點。該優化方法的核心思想在于：從待定采樣點集合中選取距離目標點最近的采樣點來確定節點生長的方向，以保證隨機樹朝目標點附近生長。通過提高采樣點的質量，既避免了隨機樹生長的長度成本過高，又保留了RRT算法搜索方向的完整性。

圖3 采樣點優化過程示意Fig.3 Schematic diagram of sampling point optimiza‐tion process

2.4 節點重連策略

基于上述策略改進的RRT算法雖能快速規劃得到初始路徑，但該初始路徑存在節點冗余現象，導致路徑的彎折次數較多。為解決該問題，本文提出了節點重連策略，即通過對初始路徑上的節點進行重新連接來減少轉折點，從而優化路徑。

節點重連策略的原理如圖4所示。圖中：虛線所示路徑為未引入節點重連策略的改進RRT算法規劃的初始路徑，該路徑由初始節點集合{Q1,Q2,Q3,Q4,Q5,Q6}中的節點依次連接而成，其中Q1和Q6分別代表起點和目標點。引入節點重連策略后，將目標點Q6作為隨機樹的根節點，基于Q6依次連接初始節點集合中的各個節點并進行碰撞檢測。通過檢測發現，線段Q6Q1和線段Q6Q2均會與障礙物發生碰撞，但Q6直接連接Q3時沒有發生碰撞，故將Q3加入優化節點集合。然后，以Q3作為碰撞檢測的起點，依次連接初始節點集合中的各個節點并進行新一輪的碰撞檢測。結果顯示，Q3直接連接Q1時沒有發生碰撞，故將Q1加入優化節點集合。當起點被加入到優化節點集合中時，表明節點重連結束。將優化節點集合中的節點按順序依次連接，獲得優化路徑，如圖4中實線所示路徑。相較于初始路徑而言，節點重連后路徑的節點數量明顯減少，有效降低了路徑規劃的長度成本。

3 仿真實驗與結果分析

為驗證本文改進RRT算法在避障路徑規劃中的可行性及其規劃效率的提升效果，分別利用傳統RRT 算法、Bias-RRT 算法、RRT-Connect 算法和改進RRT算法進行路徑規劃仿真并對比。本文仿真環境為Intel(R) Core(TM) i5-7200U CPU @ 2.50 GHz，其內存為4 GB。各RRT 算法均采用Python3.6 和MATLAB 2017a軟件來實現。

3.1 二維空間仿真

為驗證本文改進RRT 算法在二維空間中規劃避障路徑的可行性，分別利用傳統RRT 算法、Bias-RRT算法、RRT-Connect算法和改進RRT算法在3種復雜程度不同的二維地圖中進行50次路徑規劃仿真實驗。其中：二維地圖的大小均為18 cm×18 cm；路徑的起點為(2，2) cm，終點為(17，17) cm。在本文中，傳統RRT 算法、Bias-RRT 算法、RRTConnect 算法的固定步長均取1.5 cm；Bias-RRT 算法的偏置概率取20%。

4 種RRT 算法在3 種二維地圖中的路徑規劃效果分別如圖5 至圖7 所示。從圖5(a)、圖6(a)、圖7(a)中可以看出，傳統RRT算法生長的隨機樹沒有方向性，產生了大量無效節點，且規劃的路徑較為曲折。從圖5(b)、圖6(b)、圖7(b)中可以看出，偏置概率取20%的Bias-RRT算法在簡單環境中可以使隨機樹的生長具有方向性，但在復雜環境中無法實現。從圖5(c)、圖6(c)、圖7(c)中可以看出，RRT-Connect算法中2棵隨機樹的雙向生長雖提高了路徑規劃效率，但仍存在不少無效節點且路徑質量較差。從圖5(d)、圖6(d)、圖7(d)中可以看出，本文的改進RRT算法基于對地圖復雜程度的自動評估得到的步長相比于其他3種RRT算法的固定步長具有明顯優勢，尤其在復雜程度不高的地圖1 和地圖2中，合適的偏置概率和步長大幅提高了路徑規劃效率；在復雜程度較高的地圖3中，當隨機樹生長到障礙物附近無法繼續正向生長時，節點拒絕策略有助于隨機樹擺脫局部無解，有效地提高了路徑規劃效率。綜上所述，相較于其他3種RRT算法，本文的改進RRT算法生長的隨機樹的樹枝大幅減少，采樣點數量減少且均向目標點收縮，經過節點重連后獲得的無碰撞路徑的質量明顯優于其他RRT算法。

圖5 二維地圖1中4種RRT算法的路徑規劃效果對比Fig.5 Comparison of path planning effect of four RRT algorithms in 2D map 1

圖6 二維地圖2中4種RRT算法的路徑規劃效果對比Fig.6 Comparison of path planning effect of four RRT algorithms in 2D map 2

圖7 二維地圖3中4種RRT算法的路徑規劃效果對比Fig.7 Comparison of path planning effect of four RRT algorithms in 2D map 3

對4 種RRT 算法在3 種復雜程度不同的地圖中的路徑規劃數據（耗時、節點數、路徑長度）進行均值處理并對比，結果分別如表2至表4所示。

表2 二維地圖1中4種RRT算法的路徑規劃數據對比Table 2 Comparison of path planning data of four RRT al‐gorithms in 2D map 1

表3 二維地圖2中4種RRT算法的路徑規劃數據對比Table 3 Comparison of path planning data of four RRT al‐gorithms in 2D map 2

表4 二維地圖3中4種RRT算法的路徑規劃數據對比Table 4 Comparison of path planning data of four RRT al‐gorithms in 2D map 3

由表2可得，相較于傳統RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進RRT算法在復雜程度較低的地圖1中的路徑規劃耗時分別下降了96.24%，90.08%，81.93%，路徑長度分別縮短了18.89%，13.74%，16.12%。

由表3可得，相較于傳統RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進RRT算法在復雜程度一般的地圖2中的路徑規劃耗時分別下降了93.28%，88.52%，62.29%，路徑長度分別縮短了26.64%，21.66%，13.44%。

由表4可得，相較于傳統RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進RRT算法在復雜程度較高的地圖3中的路徑規劃耗時分別下降了93.11%，92.49%，73.41%，路徑長度分別縮短了24.25%，23.05%，21.16%。

綜上所述，相較于傳統RRT算法、Bias-RRT算法和RRT-Connect算法，在復雜程度不同的二維地圖中，本文的改進RRT算法的避障路徑規劃速度更快，且路徑的節點更少，長度更短。

3.2 三維空間仿真

為驗證本文的改進RRT算法在更加復雜的三維空間中規劃路徑的可行性，分別利用傳統RRT 算法、Bias-RRT 算法、RRT-Connect 算法和改進RRT算法在三維地圖中進行50次路徑規劃仿真實驗。其中，三維地圖的大小為100 cm×100 cm ×100 cm，起點為(0，0，0) cm，終點為(100，100，100) cm。在本文中，傳統RRT 算法、Bias-RRT 算法、RRTConnect 算法的固定步長取5 cm，其中Bias-RRT 算法的偏置概率取20%。

4 種RRT 算法在三維地圖中的路徑規劃效果如圖8所示。通過對比可知，在三維地圖中，傳統RRT算法因搜索空間擴大且隨機樹的生長不具有方向性，生長出了大量的無效樹枝，其規劃的路徑質量較差。偏置概率取20%的Bias-RRT算法的隨機樹生長具有一定的導向性，但仍存在較多冗余節點。相較于Bias-RRT算法，RRT-Connect算法中2棵隨機樹的雙向生長更具有導向性，但也存在冗余節點，最終的避障路徑質量較差。與上述3種RRT算法相比，本文的改進RRT算法具有更適合三維地圖的步長以及其隨機樹生長的導向性更優，使得無效節點的數量大大減少，經節點重連后得到的避障路徑也更加平滑。

圖8 三維地圖中4種RRT算法的路徑規劃效果對比Fig.8 Comparison of path planning effect of four RRT algorithms in 3D map

將4種RRT算法在三維地圖中的路徑規劃數據（耗時、節點數、路徑長度）進行均值處理并對比，結果如表5 所示。由表5 可知，相較于傳統RRT 算法、Bias-RRT算法和RRT-Connect算法，本文的改進RRT算法在三維地圖中的路徑規劃耗時分別下降了99.74%，90.77%，88.47%，路徑長度分別縮短了26.71%，14.43%，13.19%。

表5 三維地圖中4種RRT算法的路徑規劃數據對比Table 5 Comparison of path planning data of four RRT al‐gorithms in 3D map

3.3 機械臂避障路徑規劃仿真

為驗證本文提出的改進RRT 算法在機械臂避障路徑規劃中的適用性，在MATLAB 2017a 軟件中開展仿真實驗，本文以六自由度機械臂為研究對象。首先，對機械臂的工作空間進行分析，若起點和目標點均在工作空間內，則可直接開展機械臂末端的避障路徑規劃。機械臂末端避障路徑的規劃過程為：先利用改進RRT 算法規劃初始避障路徑，并將初始避障路徑分解為若干路徑點；然后，將路徑點的坐標依次代入機械臂的逆運動學方程，以求解得到8組關節轉角。若某路徑點對應的8組關節轉角中有滿足機械臂各連桿與障礙物無碰撞且各連桿之間無碰撞的關節轉角，則認為該路徑點有效。但若8組關節轉角均不能滿足上述要求，則將該路徑點的前一個路徑點作為新的起點，重新規劃路徑并進行連桿碰撞檢測，直到獲得一條無碰撞路徑。

基于改進RRT算法的機械臂末端避障路徑規劃結果（規劃50次）如圖9所示。其中：起點為(30，40，50) cm，終點為(-40，30，30) cm，圓球表示障礙物，實線為機械臂末端的避障路徑。與此同時，分別利用傳統RRT 算法、Bias-RRT 算法、RRTConnect 算法對機械臂末端的避障路徑規劃50 次（起點、終點以及障礙物位置等均相同），并對4種RRT 算法的避障路徑規劃數據進行均值處理和對比，結果如表6所示。結果表明，利用本文的改進RRT算法對機械臂末端的避障路徑進行規劃時，所得路徑的質量更高，時間成本更低，驗證了其適用性。

圖9 基于改進RRT算法的機械臂避障路徑規劃結果Fig. 9 Obstacle avoidance path planning result of robotic arm based on improved RRT algorithm

表6 基于4種RRT算法的機械臂避障路徑規劃數據對比Table 6 Comparison of obstacle avoidance path planning data of robotic arm based on four RRT algorithms

4 結 論

針對傳統RRT算法在避障路徑規劃時存在效率低的問題，本文提出了一種新的改進RRT算法，該算法引入了以下4個策略：1）地圖復雜程度評估策略，即根據地圖中的障礙物信息，計算適合相應地圖的步長和偏置概率；2）采樣區域動態更新策略，令RRT算法僅在有效區域內采樣，以確保隨機樹正向生長；3）采樣點優化策略，從待定采樣點集合中選取距離目標點最近的采樣點作為最終采樣點，提高了隨機樹朝目標點附近生長的概率；4）節點重連策略，由于初始規劃路徑的彎折次數較多，通過對初始路徑節點進行優選和重新連接，可去除冗余節點，實現了路徑優化。最后，通過在二維、三維地圖中以及對機械臂開展避障路徑規劃仿真實驗，驗證了改進RRT 算法對環境的適應性比傳統RRT 算法、Bias-RRT 算法、RRT-Connect 算法強，其規劃無碰撞路徑的耗時更短，質量更高且更適用于機械臂。