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

黃壹凡，胡立坤，薛文超

（廣西大學 電氣工程學院，南寧 530004）

0 概述

機器人學聚集了多種學科最先進的研究成果，一直以來都是科學技術領域的研究熱點。路徑規劃算法作為移動機器人的重要組成部分，是移動機器人運行研究的重點問題［1］。

路徑規劃的基本任務是找到一條從起始位置到目標位置的無碰撞路徑［2］，并且使得這條路徑能夠具有距離短、用時少等優點。在傳統路徑規劃方法中：A*算法［3-4］和D*算法［5］在實時規劃或者局部規劃中都有較好的性能，但兩種算法需要的計算量較大，特別在三維空間中計算量劇增；人工勢場法［6］易于實現的優點使其能被廣泛應用，但容易陷入局部極小值；蟻群算法［7］可以獲得全局最優解且有較強魯棒性，但計算量大、收斂時間長。

快速擴展隨機樹（Rapidly exploring Random Tree，RRT）算法采用隨機采樣方法搜索［8］，具有很強的搜索性，但是該算法的隨機性也導致其搜索效率較低。為提高RRT 算法的搜索效率，偏 向RRT［9］、RRT-Connect［10］、雙 向RRT［11］等改進算法被相繼提出，這些算法通過目標偏向、雙樹拓展等改進加快了收斂算法速度，但仍存在較大轉折、繞遠等問題。為得到最優路徑，一些算法相繼提出：RRT*［12-13］采用漸進優化思想改進了由基本RRT 產生的并非概率最優解問題；Bi-RRT*［14］采用雙樹拓展同時結合RRT-Connect 和啟發式思想，在保證漸進最優的同時加快了收斂速度；Quick-RRT*［15］在重選父節點時擴大潛在父節點選擇范圍，以此減少路徑不必要的拐彎來縮短路徑長度；文獻［16］通過重選父節點的方法改進RRT 算法來縮短規劃路徑；文獻［17］提出結合目標引力思想的變步長搜索方法提高搜索效率；文獻［18］借鑒人工勢場思想引導隨機樹更有效生長；文獻［19］通過施加轉角約束、兩樹連接處處理等一系列改進，提高規劃路徑質量。

雖然上述優化方法用于機器人路徑規劃時都取得了較好的效果，但RRT 的強隨機性仍然使所得路徑存在曲折、繞遠、連接不平滑等現象，這也將限制移動機器人的實際工作效率。本文針對RRT-Connect 算法進行改進，通過設置多種約束優化新節點生成規則，進一步提高移動機器人規劃路徑的質量。

1 RRT-Connect 算法

1999 年，LAVALLE 提出RRT 算法［8］，基本思想是向隨機的目標點拓展隨機樹上的最近點來探索搜索空間，通過不斷迭代最終找到可行路徑，但是RRT 算法的拓展具有隨機性，導致收斂速度慢。針對這一不足，2000 年，LAVALLE 和KUFFNER 提出RRT-Connect 算法，通過在起始點和目標點建立兩棵隨機樹交替向彼此方向拓展，同時引入貪婪策略，進一步加快收斂速度。

RRT-Connect 隨機樹生長過程如圖1 所示，具體步驟如下：

圖1 RRT-Connect 隨機樹生長過程Fig.1 Random tree growth process of RRT-Connect

1）初始化起始點、終點和障礙物，將起始點、終點坐標信息加入到隨機樹，并在機器人的C 空間中生成一個隨機點Xrand。

2）在Tree1 上找到距Xrand歐氏距離最小的點，記為Xnearest1，方向指向Xrand，朝該方向生長一定步長Sstep得到Xnew1。如果Xnew1不滿足C 空間中的障礙物約束，則舍棄該點重新生成隨機點；如果滿足，則把生長后的樹枝和端點注入到樹上。

3）計算Tree2 上距Xnew1歐氏距離最小的點，記為Xnearest2，方向指向Xnew1，朝該方向生長一定步長得到Xnew2。若通過碰撞檢測，則將Xnew2注入Tree2 中，同時啟動貪婪策略子程序，繼續往Xnew1方向生長，直到遇到障礙物或者兩樹最近距離小于閾值后停止；若遇到障礙物，則返回主程序交換兩樹并重復上述采樣過程，若兩樹距離小于連接閾值，則返回連接點信息，路徑搜索結束。

2 改進的RRT-Connect 算法

2.1 考慮祖代點的重選父節點環節

傳統雙向RRT 算法在生成新節點后立即進入另一棵樹生成新節點，這容易生成不必要的節點，并導致繞遠現象的發生。為了使路徑趨于漸進最優解，本文基于文獻［14-15］的設計思想，在原RRT-Connect 拓展的基礎上增加重選父節點環節。

圖2（a）所示為RRT*的重選父節點環節，和傳統的RRT 算法一樣生成Xnew點后，以特定的半徑r搜索Xnew附近的頂點為父節點候選集，然后迭代計算Near 圓內點到起始點距離成本，選取最小成本的點作為Xnew的父節點，以此達到漸進優化的效果，而如果要加快收斂速度，則需要增大搜索半徑r，這會導致計算量呈指數級增加。相比于RRT*的增大搜索半徑r，本文改進算法通過自定義父節點的深度范圍來擴大新節點的潛在父節點候選集，使求得新節點到起始點更短距離路徑的概率增加，而計算量卻沒有增加太多。改進RRT-Connect 算法的重選父節點環節如圖2（b）所示。

圖2 2 種算法樹結構比較Fig.2 Tree structure comparison of two algorithms

上述改進主要得益于三角不等式原理和共享同一父節點。當生成Xnew并以特定的半徑r搜索其附近的頂點時，同時求出圓內頂點的父節點和祖節點。由三角不等式原理可知，在考慮Near圓父節點后，只要新節點與圓外父節點碰撞檢測通過，就可得到該新節點更低路徑成本，由此求得新節點更短路徑成本概率增加；而通過擴大父候選集的改進，Near 圓內的點已傾向于共享同一父節點，因此，改進后Xnew擴大了父節點候選集，而候選集頂點總數卻不會增加太多，另由于舍棄了RRT*中的Rewire 過程，因此收斂時間相比于RRT*不會增加太多。上述過程考慮到深度為2 的潛在父節點，因此，也稱其為考慮祖代點的重選父節點環節。

2.2 轉角約束

為使規劃路徑更平滑，對每一個拓展的新節點添加轉角約束。圖3為Tree1、Tree2的轉角約束示意圖。

圖3 Tree1 和Tree2 轉角約束Fig.3 Corner constraints of Tree1 and Tree2

假設最大轉角約束為θ。對于Tree1，當0 ≤α≤θ時，轉角約束通過，可將新節點注入Tree1中，接著進入Tree2的拓展，當θ＜α≤180°時則舍棄該新節點，并重新生成采樣點；對于Tree2，當0 ≤β≤θ時，轉角約束通過，并將新節點注入Tree2 中，接著進入貪婪拓展，當θ＜α≤180°時則舍棄該新節點，并退出拓展子程序進入交換兩樹環節。

2.3 動態步長優化

在原RRT-Connect 算法中，兩樹通過交換迭代拓展生成可行路徑，如在圖3 中：對于Tree1，生成隨機點Xrand后計算最近鄰節點Xnear1，然后以固定步長往Xrand方向拓展一步，計算表達式如式（5）所示；對于Tree2，生成Xnew1點后計算得到Xnew1距離Tree2 最近的點Xnear2，然后以固定步長Sstep往Xnew1方向拓展一步或者多步，計算表達式如式（6）所示。

原RRT-Connect 算法拓展步長固定，收斂速度慢，生成路徑轉折多，特別經過改進即考慮祖代點的重選父節點環節后，由于考慮到轉角約束，導致在連接處連接失敗的概率增加，進而增加了收斂時間。受文獻［19-21］關于動態步長優化的啟發，本文結合改進算法的實際情況，提出一種動態步長優化方法：當兩樹距離Dtree小于設定閾值σ1時，強制小步長，以此增加兩樹連接成功率，該步驟執行優先級為I 級；當兩樹距離大于設定閾值σ1時，認為兩樹還未進入連接狀態，步長由機器人與障礙物距離決定；當機器人與障礙物距離Dobs大于設定閾值σ2時，認為機器人處于空曠環境，采用最大步長；相反，當機器人與障礙物距離小于設定閾值σ2時，認為機器人處于障礙物環境，采用中步長，也即默認步長，當默認步長較小時，小步長與默認步長可取等值，該步驟執行優先級為II 級。

動態步長表達式如式（7）所示。

2.4 兩樹連接處理

傳統RRT-Connect 算法在連接時，只要新節點與另一棵樹的距離小于指定閾值，即將兩點直接連接，這會導致連接處出現轉角過大的問題，有時甚至出現180°轉角，主要有圖4 所示的T 形連接和V 形連接兩種情況。

圖4 兩種連接方式Fig.4 Two connection modes

為使生成路徑更符合實際，在進入連接階段作以下處理：

1）針對連接點的父節點、祖代點的處理

該過程依次對祖代點、父節點進行碰撞檢測和角度檢測，檢測通過即可連接。如圖5 所示，首先判斷Xnew1到Xparent2、Xnear2的可連接性，即先計算α3、β3。若角度約束符合設定值，則對Xnew1到Xparent2進行碰撞檢測，都通過則返回Xnew1坐標以及該點在兩樹的父節點Xnear1、Xparent2序號到主程序中；若上述約束不通過，則判斷Xnear2的可連接性；若以上兩種情況都不通過，則進行針對連接點Xconnect2的連接處理。

圖5 連接點連接結構Fig.5 Connection structure of connection points

2）針對連接點的處理

當φ＞δ時，計 算α1、β1。第1 類相遇：α1＞δ且β1＞δ，只有V 形連接存在此情況，判為有效相遇。第2 類相遇：α1＜δ或β1＜δ，只有T 形連接存在此情況，判為無效相遇，進入同父節點重連處理。

當φ＜δ時，檢 測α1、β1。第3 類相遇：α1＞δ且β1＞δ，只有V 形連接存在此情況，引入安全距離處理。第4 類相遇：α1＜δ或β1＜δ，只有T 形連接存在此情況，判為無效相遇，舍棄。

下文簡要闡述同父節點重連、安全距離這兩個概念。

1）同父節點重連

在第2 類相遇情況下，為減少搜索次數，增加連接成功率，提出同父節點重連概念，即考慮重新連接與連接點共享父節點的點。如圖6 所示，Xconnect2為達到連接閾值的連接點，通過計算其父節點Xnear2共享該父節點的點信息，即Y1、Y2節點。若檢測α1、β1都大于最小夾角約束，則判Y1為有效相遇點。

圖6 同父節點重連的情況Fig.6 Reconnection of parent node

2）安全距離

在第3 類相遇情況下，V 形連接，需要考慮機器人最小移動距離，即安全距離η。當Xnew1、Xconnect2兩點距離小于連接閾值而大于η時，可判斷為有效相遇，小于η則判為無效相遇，舍棄。

此外，判為有效相遇后，通過碰撞檢測即可定為有效連接點。Tree2 連接Tree1 與Tree1 連接Tree2 類似。

2.5 改進算法流程

改進RRT-Connect 算法流程如圖7 所示，具體步驟如下：

圖7 改進RRT-Connect 算法流程Fig.7 Procedure of improved RRT-Connect algorithm

步驟1初始化地圖，設置起止點、動態步長、安全距離等參數。

步驟2生成隨機點，確定離Tree1 最近點，確定步長大小。

步驟3生成Tree1 新節點。若通過碰撞檢測則判斷是否與Tree2 連接，若能連接則跳至步驟7，不能連接則進行步驟4，若不通過碰撞檢測則重新采樣。

步驟4考慮祖代點的父節點重選，檢測能否滿足轉角約束和碰撞檢測，都滿足則重連處理，不滿足則返回重新采樣。

步驟5確定Tree2 拓展步長，同時生成Tree2新節點，檢測是否與Tree1 連接，能連接則跳至步驟7，不能則考慮祖代點的父節點重選。

步驟6貪婪拓展，同時連接檢查，能連接則跳至Step7，不能則考慮祖代點的父節點重選，循環步驟6 直至檢測不通過，交換兩樹返回步驟2。

步驟7若連接則進行連接處理程序，結束進程，返回路徑信息path，不能連接則交換樹，返回步驟2。

3 實驗與結果分析

實驗仿真在Matlab2018a 上進行，實驗環境配置為：Window10，處理器Intel?CoreTMi5-3230M CPU@

2.60 GHz，RAM 4 GB。

為驗證改進算法的性能，采用簡單環境和復雜環境兩種地圖，仿真地圖大小為500 cm×500 cm，起點坐標（10，10），終點坐標（490，490），障礙物由多個圓形區域組成，分別對Bias-BiRRT、原始RRT-Connect、改進RRT-Connect 這3 種算法進行50 次仿真，并將相應的平均搜索時間、平均采樣點數、平均路徑長度等進行對比分析，同時在復雜環境下，對改進前后算法進行平滑度分析實驗。其中，Bias-BiRRT、原始RRT-Connect 拓展步長10 cm，改進RRT-Connect 拓展最大步長20 cm，默認步長10 cm，改進RRT-Connect最大轉角設置為60°。

3.1 簡單環境仿真

簡單環境仿真結果如表1 和圖8 所示，具體分析如下：

1）在規劃效率方面，由表1 可知，在簡單環境中，改進算法固定步長時的平均迭代次數為147.18次，比Bias-BiRRT、RRT-Connect算法分別降低43%、19%，而平均規劃時間為2.81 s，和RRT-Connect 算法消耗的時間2.70 s 相差不大。由這兩項數據可得，引入改進重選父節點環節及設置多種約束后，改進算法的計算量和RRT-Connect 算法相比沒有增加很多，而迭代次數變少，規劃效率更高。

2）在路徑長度優化效果方面，由表1 可知，改進算法固定步長時的平均路徑長度為694.10 cm，相比Bias-BiRRT 和RRT-Connect 算法分別減少127.55 cm、59.65 cm。可以看出，引入考慮祖代點的重選父節點環節能夠起到減小路徑長度的效果，提高規劃路徑質量。

3）在動態步長優化方面，由表1 可知，改進算法動態步長規劃時間相比固定步長減小0.8 s，而路徑長度幾乎不變。由該數據可知，改進算法動態步長策略以較大步長通過空曠區域，而以較短步長規劃障礙物區域、兩樹連接區域，總體上加快了改進算法收斂速度，減少了添加轉角約束、父節點重連所消耗的時間。

表1 簡單環境下3 種算法的實驗數據Table 1 Experimental data of three algorithms in simple environment

4）在圖8（a）～圖8（d）中，細線為拓展樹樹枝，粗線為規劃路徑，可以看出，在簡單環境下，RRT-Connect算法的整條路徑較Bias-BiRRT 曲折變少，但也還存在一定的繞遠情況，而改進算法規劃路徑更直、拐彎更少，兩拓展樹也能較好地連接，路徑質量顯著提高。

圖8 簡單環境實驗結果Fig.8 Experiment results in simple environment

3.2 復雜環境仿真

3 種算法在復雜環境中的實驗結果如表2 和圖9所示，可見其與在簡單環境中實驗結果整體趨勢一致，主要有以下特點

1）在路徑規劃效率方面，由表2 可知，在復雜環境中，改進算法固定步長時平均規劃時間相較于簡單環境只增加了0.1 s，平均迭代次數也大致相等，而RRT-Connect算法平均規劃時間增加了0.44 s，平均迭代次數增加39.9 點，出現了繞遠現象，這表明環境變復雜后改進算法規劃效率仍然能保持較高水平。

2）在路徑長度優化效果方面，由表2 可知，改進算法固定步長時的平均路徑長度為696.28 cm，與簡單環境相比，雖然路徑點數多了2.64 點，但規劃路徑長度基本一致，再次表明考慮祖代點的重選父節點能夠使規劃路徑趨于最優。

表2 復雜環境下3 種算法實驗數據Table 2 Experimental data of three algorithms in complex environment

3）在動態步長優化方面，改進算法動態步長相比于固定步長，路徑總長變化不大，但是平均規劃時間、迭代次數分別減少22%、17%，相比于簡單環境的減小幅度變小，這是由環境的復雜程度決定的，但總體上仍加快了算法的收斂速度。

4）由圖9（a）～圖9（d）可知，在復雜環境中，改進算法相比Bias-BiRRT 和RRT-Connect 算法轉折明顯減少，繞遠現象減小，RRT-Connect 算法規劃的路徑存在多個超過90°轉角，而改進算法更趨于平滑。

圖9 復雜環境實驗結果Fig 9 Experimental results in complex environment

為進一步驗證改進算法的平滑度情況，對原RRT-Connect 和改進RRT-Connect 兩種算法分別在復雜環境中進行10 次仿真，比較連接角度數、規劃路徑最大角度數、最大轉角大于設定值60°的個數，對比數據如表3 所示。

表3 算法改進前后平滑度仿真結果對比Table 3 Comparison of smoothness simulation results before and after algorithm improvement

由表3 可知：RRT-Connect 算法大于最大轉角60°的平均個數為7.2 個，并且出現3 個超過100°的轉角，而改進算法通過最大轉角約束，將大于60°轉角個數限制為0 個：在連接角度方面，RRT-Connect 算法出現3 次轉角過大的情況，而改進算法則全部維持在設定范圍內。從以上數據可知，通過引入轉角約束、連接處理等環節后，改進算法規劃的路徑在連接處能平滑連接，剔除急轉拐角，規劃路徑質量更高，符合移動機器人的安全快速運行需求。

4 結束語

本文提出一種改進的RRT-Connect 算法，通過引入考慮祖代點的重選父節點環節，優化部分路徑長度，并設計動態步長優化策略減小收斂時間。此外，還通過設置轉角約束和添加連接處理單元，提高規劃路徑平滑度。實驗結果表明，改進算法比原算法規劃路徑質量更高，收斂速度更快。由于本文研究只在靜態障礙物場景下進行，未考慮到動態障礙物，因此下一步將融合局部路徑規劃算法組成混合算法，并在現有環境下隨機加入動態障礙物，進一步提高路徑規劃環境的真實性和改進算法的適用性。