黃壹凡，胡立坤，薛文超

（廣西大學(xué) 電氣工程學(xué)院，南寧 530004）

0 概述

機(jī)器人學(xué)聚集了多種學(xué)科最先進(jìn)的研究成果，一直以來都是科學(xué)技術(shù)領(lǐng)域的研究熱點(diǎn)。路徑規(guī)劃算法作為移動(dòng)機(jī)器人的重要組成部分，是移動(dòng)機(jī)器人運(yùn)行研究的重點(diǎn)問題［1］。

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

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

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

1 RRT-Connect 算法

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

RRT-Connect 隨機(jī)樹生長(zhǎng)過程如圖1 所示，具體步驟如下：

圖1 RRT-Connect 隨機(jī)樹生長(zhǎng)過程Fig.1 Random tree growth process of RRT-Connect

1）初始化起始點(diǎn)、終點(diǎn)和障礙物，將起始點(diǎn)、終點(diǎn)坐標(biāo)信息加入到隨機(jī)樹，并在機(jī)器人的C 空間中生成一個(gè)隨機(jī)點(diǎn)Xrand。

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

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

2 改進(jìn)的RRT-Connect 算法

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

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

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

圖2 2 種算法樹結(jié)構(gòu)比較Fig.2 Tree structure comparison of two algorithms

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

2.2 轉(zhuǎn)角約束

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

圖3 Tree1 和Tree2 轉(zhuǎn)角約束Fig.3 Corner constraints of Tree1 and Tree2

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

2.3 動(dòng)態(tài)步長(zhǎng)優(yōu)化

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

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

動(dòng)態(tài)步長(zhǎng)表達(dá)式如式（7）所示。

2.4 兩樹連接處理

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

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

為使生成路徑更符合實(shí)際，在進(jìn)入連接階段作以下處理：

1）針對(duì)連接點(diǎn)的父節(jié)點(diǎn)、祖代點(diǎn)的處理

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

圖5 連接點(diǎn)連接結(jié)構(gòu)Fig.5 Connection structure of connection points

2）針對(duì)連接點(diǎn)的處理

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

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

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

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

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

圖6 同父節(jié)點(diǎn)重連的情況Fig.6 Reconnection of parent node

2）安全距離

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

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

2.5 改進(jìn)算法流程

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

圖7 改進(jìn)RRT-Connect 算法流程Fig.7 Procedure of improved RRT-Connect algorithm

步驟1初始化地圖，設(shè)置起止點(diǎn)、動(dòng)態(tài)步長(zhǎng)、安全距離等參數(shù)。

步驟2生成隨機(jī)點(diǎn)，確定離Tree1 最近點(diǎn)，確定步長(zhǎng)大小。

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

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

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

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

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

3 實(shí)驗(yàn)與結(jié)果分析

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

2.60 GHz，RAM 4 GB。

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

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

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

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

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

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

表1 簡(jiǎn)單環(huán)境下3 種算法的實(shí)驗(yàn)數(shù)據(jù)Table 1 Experimental data of three algorithms in simple environment

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

圖8 簡(jiǎn)單環(huán)境實(shí)驗(yàn)結(jié)果Fig.8 Experiment results in simple environment

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

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

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

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

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

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

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

圖9 復(fù)雜環(huán)境實(shí)驗(yàn)結(jié)果Fig 9 Experimental results in complex environment

為進(jìn)一步驗(yàn)證改進(jìn)算法的平滑度情況，對(duì)原RRT-Connect 和改進(jìn)RRT-Connect 兩種算法分別在復(fù)雜環(huán)境中進(jìn)行10 次仿真，比較連接角度數(shù)、規(guī)劃路徑最大角度數(shù)、最大轉(zhuǎn)角大于設(shè)定值60°的個(gè)數(shù)，對(duì)比數(shù)據(jù)如表3 所示。

表3 算法改進(jìn)前后平滑度仿真結(jié)果對(duì)比Table 3 Comparison of smoothness simulation results before and after algorithm improvement

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

4 結(jié)束語

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