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

黃壹凡，胡立坤，薛文超

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

0 概述

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

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

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

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

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 中，同時啟動貪婪策略子程序，繼續(xù)往Xnew1方向生長，直到遇到障礙物或者兩樹最近距離小于閾值后停止；若遇到障礙物，則返回主程序交換兩樹并重復上述采樣過程，若兩樹距離小于連接閾值，則返回連接點信息，路徑搜索結束。

2 改進的RRT-Connect 算法

2.1 考慮祖代點的重選父節(jié)點環(huán)節(jié)

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

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

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

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

2.2 轉角約束

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

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

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

2.3 動態(tài)步長優(yōu)化

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

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

動態(tài)步長表達式如式（7）所示。

2.4 兩樹連接處理

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

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

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

1）針對連接點的父節(jié)點、祖代點的處理

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

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

2）針對連接點的處理

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

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

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

1）同父節(jié)點重連

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

圖6 同父節(jié)點重連的情況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初始化地圖，設置起止點、動態(tài)步長、安全距離等參數。

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

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

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

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

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

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

3 實驗與結果分析

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

2.60 GHz，RAM 4 GB。

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

3.1 簡單環(huán)境仿真

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

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

2）在路徑長度優(yōu)化效果方面，由表1 可知，改進算法固定步長時的平均路徑長度為694.10 cm，相比Bias-BiRRT 和RRT-Connect 算法分別減少127.55 cm、59.65 cm?？梢钥闯?，引入考慮祖代點的重選父節(jié)點環(huán)節(jié)能夠起到減小路徑長度的效果，提高規(guī)劃路徑質量。

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

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

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

圖8 簡單環(huán)境實驗結果Fig.8 Experiment results in simple environment

3.2 復雜環(huán)境仿真

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

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

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

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

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

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

圖9 復雜環(huán)境實驗結果Fig 9 Experimental results in complex environment

為進一步驗證改進算法的平滑度情況，對原RRT-Connect 和改進RRT-Connect 兩種算法分別在復雜環(huán)境中進行10 次仿真，比較連接角度數、規(guī)劃路徑最大角度數、最大轉角大于設定值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 次轉角過大的情況，而改進算法則全部維持在設定范圍內。從以上數據可知，通過引入轉角約束、連接處理等環(huán)節(jié)后，改進算法規(guī)劃的路徑在連接處能平滑連接，剔除急轉拐角，規(guī)劃路徑質量更高，符合移動機器人的安全快速運行需求。

4 結束語

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