姜利光+甘屹+孫福佳

摘要：針對(duì)隨機(jī)擴(kuò)展樹(shù)算法在未知空間中進(jìn)行路徑規(guī)劃擴(kuò)展時(shí)隨機(jī)性大，且擴(kuò)展的樹(shù)節(jié)點(diǎn)在整個(gè)環(huán)境空間中搜索過(guò)于均勻等問(wèn)題，提出一種基于變參數(shù)閾值的隨機(jī)擴(kuò)展樹(shù)算法。改進(jìn)后的算法在路徑規(guī)劃中，針對(duì)具體情況選取參數(shù)閾值作下一步擴(kuò)展，使得每次擴(kuò)展都有著一定的概率性偏向目標(biāo)；同時(shí)設(shè)定可變參數(shù)閾值，避免了陷入局部極小值，有效解決了隨機(jī)性大和搜索均勻問(wèn)題。通過(guò)Matlab仿真實(shí)驗(yàn)，驗(yàn)證了該算法的可行性和有效性。

關(guān)鍵詞：隨機(jī)擴(kuò)展樹(shù)算法；路徑規(guī)劃；參數(shù)閾值

DOIDOI：10.11907/rjdk.171936

中圖分類(lèi)號(hào)：TP312

文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-7800（2017）012-0067-03

Abstract：A stochastic extension tree algorithm based on variable parameter threshold is proposed to solve the large random of extension tree algorithm in the unknown space and the searching uniformity of extended tree nodes in the whole environment space. And then the improved algorithm is applied to the path planningto select the parameters for the next expansion aiming at the specific situation， so that each expansion has a certain probability of bias target； Meanwhile， the variable parameter threshold is set to avoid falling into the local minimum，which effectively solves the problem of large random and searching uniformity. Finally， the feasibility and validity of the algorithm are verified by MATLAB simulation experiment.

Key Words：stochastic expansion tree algorithm； path planning； parameter threshold

0 引言

路徑規(guī)劃技術(shù)是移動(dòng)機(jī)器人研究領(lǐng)域的一個(gè)重要組成部分。其目的是機(jī)器人按照某一性能（時(shí)間、路程、耗能）搜索出一條由起始位置節(jié)點(diǎn)到目標(biāo)位置節(jié)點(diǎn)之間的最優(yōu)或次優(yōu)無(wú)碰路徑。路徑規(guī)劃主要分為：完全已知的全局路徑規(guī)劃、完全未知或部分未知的局部路徑規(guī)劃[1]。已有研究多是關(guān)于全局已知的靜態(tài)環(huán)境中的路徑規(guī)劃。但在實(shí)際工作中，環(huán)境中的障礙物位置、大小信息都是未知或部分已知，有時(shí)障礙物的位置還會(huì)隨著時(shí)間的變化而不斷移動(dòng)，增加了路徑規(guī)劃中的建模難度[2]。

隨機(jī)擴(kuò)展樹(shù)（Rapidly-exploring Random Tree，RRT）算法采用隨機(jī)采樣的規(guī)劃方法，無(wú)需對(duì)環(huán)境空間進(jìn)行建模，避免了預(yù)處理[3]。但由于RRT算法是基于隨機(jī)采樣的機(jī)制，導(dǎo)致在路徑規(guī)劃時(shí)擴(kuò)展的樹(shù)節(jié)點(diǎn)均布于整個(gè)空間內(nèi)，產(chǎn)生大量的無(wú)效搜索。針對(duì)該問(wèn)題，提出通過(guò)重復(fù)使用前一個(gè)周期的隨機(jī)樹(shù)信息對(duì)RRT算法進(jìn)行改進(jìn)，如ERRT算法[4]、DRRT算法[5]等均在一定程度上提高了RRT算法的穩(wěn)定性，但RRT算法所采用的固有規(guī)劃方式限制了其進(jìn)一步應(yīng)用。本研究采用RRT算法進(jìn)行機(jī)器人路徑規(guī)劃，以避免復(fù)雜的空間建模，并提出通過(guò)引入變參數(shù)閾值引導(dǎo)隨機(jī)樹(shù)在擴(kuò)展樹(shù)節(jié)點(diǎn)時(shí)以一定的概率偏向目標(biāo)點(diǎn)，避免了搜索點(diǎn)過(guò)于平均的問(wèn)題。此外，參數(shù)閾值存在可變性，使得算法避免收斂于局部極小值。

1 RRT算法原理

RRT算法以機(jī)器人移動(dòng)的開(kāi)始點(diǎn)作為樹(shù)的初始根節(jié)點(diǎn)，利用隨機(jī)采樣原則，選定一個(gè)隨機(jī)節(jié)點(diǎn)。根據(jù)機(jī)器人移動(dòng)約束條件（如步長(zhǎng)、角度），在所選擇的節(jié)點(diǎn)上避開(kāi)障礙物進(jìn)行擴(kuò)展，直到產(chǎn)生一個(gè)新節(jié)點(diǎn)，并將此新節(jié)點(diǎn)添加到搜索樹(shù)中，以此為樹(shù)節(jié)點(diǎn)作下一步擴(kuò)展，依次重復(fù)上述過(guò)程，直至找到所要尋找的目標(biāo)點(diǎn)才停止搜索。RRT算法擴(kuò)展方式如圖1所示。

圖1中，C表示擴(kuò)展環(huán)境空間，X0表示初始點(diǎn)，Xi表示隨機(jī)點(diǎn)，Xj表示離隨機(jī)點(diǎn)最近的一個(gè)樹(shù)節(jié)點(diǎn)，Xi為在Xi和Xj的連線上以步長(zhǎng)Δ為單位截取的新節(jié)點(diǎn)。

該算法在搜索過(guò)程中要保證隨機(jī)采樣使得機(jī)器人向未知區(qū)域擴(kuò)展，并在搜索過(guò)程中不斷推進(jìn)搜索樹(shù)生長(zhǎng)，同時(shí)使得樹(shù)節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離不斷接近，最終該目標(biāo)點(diǎn)為樹(shù)上的一個(gè)節(jié)點(diǎn)或者在最后的節(jié)點(diǎn)附近（小于一個(gè)步長(zhǎng)距離），即完成搜索任務(wù)[6]。

定義X0為初始位姿構(gòu)建隨機(jī)樹(shù)T，搜索步驟如下：①在空間C中，從初始點(diǎn)X0出發(fā)先建立一顆遍歷樹(shù)T；②在所處的區(qū)域內(nèi)隨機(jī)選擇一個(gè)位姿點(diǎn)Xi；③找出T中距離Xi最近的節(jié)點(diǎn)Xj，然后選擇控制輸入集U中的選擇輸入u∈U（如速度、角度等）作用在Xj，使得機(jī)器人沿著Xi直至Xj；④由Xj朝Xi方向擴(kuò)展一定的距離Δ到Xk（Δ為步長(zhǎng)），并且Xk屬于該自由空間中的點(diǎn)；⑤選擇能使Xj到Xi距離最近的控制輸入u為最佳輸入，依次重復(fù)該過(guò)程直到新生成的某個(gè)點(diǎn)與目標(biāo)點(diǎn)（Xm）的距離小于該步長(zhǎng)，至此隨機(jī)樹(shù)構(gòu)建完畢。

2 變參數(shù)閾值RRT算法

2.1 偏向目標(biāo)算法模型

RRT算法在全局規(guī)劃中隨機(jī)選取節(jié)點(diǎn)，能夠擴(kuò)大搜索空間，但這種隨機(jī)選擇也會(huì)導(dǎo)致路徑規(guī)劃中的盲目性，不僅使得路徑解發(fā)散，同時(shí)也會(huì)耗費(fèi)相當(dāng)長(zhǎng)的算法搜索時(shí)間。特別是在障礙物較少的局部空間中，隨機(jī)性會(huì)將路徑搜索點(diǎn)均勻遍布于空間內(nèi)，延長(zhǎng)路徑解收斂時(shí)間。為了提升RRT算法效率，研究者[7-8]提出采用偏向目標(biāo)RRT算法（Bias-goal，Bg-RRT）。隨機(jī)函數(shù)生成樹(shù)節(jié)點(diǎn)之前，先按照平均概率分布方式隨機(jī)獲取一個(gè)概率值，以一定的概率性將目標(biāo)點(diǎn)作為牽引點(diǎn)，在一定程度上解決擴(kuò)展節(jié)點(diǎn)時(shí)隨機(jī)性大的問(wèn)題。具體步驟為：取參數(shù)閾值q，表示該算法認(rèn)定目標(biāo)點(diǎn)Xm為隨機(jī)點(diǎn)Xi的幾率。如果q的值大于算法產(chǎn)生的概率值p，就選取目標(biāo)點(diǎn)Xm為Xi。如果q的值小于算法產(chǎn)生的概率值p，就由隨機(jī)函數(shù)Random_Configuration（）隨機(jī)產(chǎn)生Xi。經(jīng)過(guò)大量研究發(fā)現(xiàn)，加入?yún)?shù)閾值q后，該算法可以使得擴(kuò)展樹(shù)快速地向目標(biāo)點(diǎn)生長(zhǎng)，但過(guò)多偏向目標(biāo)點(diǎn)時(shí)會(huì)使擴(kuò)展樹(shù)生長(zhǎng)陷入局部最優(yōu)[9-10]。

2.2 Vpt-RRT算法

3 實(shí)驗(yàn)仿真與分析

仿真實(shí)驗(yàn)所用的計(jì)算機(jī)處理器為Intel（R） CORE（TM）i5-2400 CPU @ 3.10GHz 3.10GHz，顯卡為Intel（R） HD Graphics，內(nèi)存為4.00G，采用MATLABR2014a工具編程實(shí)現(xiàn)。

3.1 參數(shù)閾值設(shè)定

Bg-RRT算法中，參數(shù)閾值q的值域?yàn)閇0，1]。以q=0.1取值間隔為0.1，進(jìn)行不同參數(shù)閾值q的路徑規(guī)劃仿真實(shí)驗(yàn)。仿真實(shí)驗(yàn)中選取相同的步長(zhǎng)，以仿真結(jié)果中擴(kuò)展的總節(jié)點(diǎn)數(shù)、路徑平均規(guī)劃時(shí)間作為性能指標(biāo)。對(duì)每一個(gè)參數(shù)閾值q，分別做多次實(shí)驗(yàn)，通過(guò)統(tǒng)計(jì)分析實(shí)驗(yàn)到第10次時(shí)結(jié)果趨于收斂；故取10次實(shí)驗(yàn)的平均值作為該q值的Bg-RRT算法運(yùn)行結(jié)果，仿真實(shí)驗(yàn)結(jié)果如圖3、圖4所示。

由圖3可知，隨著參數(shù)閾值q值的增大，Bg-RRT算法中隨機(jī)樹(shù)擴(kuò)展的總節(jié)點(diǎn)數(shù)先減小后增大再減小，總體呈現(xiàn)下降趨勢(shì)。由圖4可知，路徑規(guī)劃時(shí)間總體上隨著參數(shù)閾值q的增大先減小后增大。通過(guò)分析圖3、圖4可知，參數(shù)閾值q對(duì)Bg-RRT算法而言十分重要，直接影響路徑規(guī)劃相關(guān)性能指標(biāo)。q值選取得是否合理直接影響最終路徑規(guī)劃的優(yōu)劣。q值較小時(shí)，隨機(jī)樹(shù)擴(kuò)展的樹(shù)節(jié)點(diǎn)增多，說(shuō)明規(guī)劃中偏向目標(biāo)搜索的趨勢(shì)不明顯，隨機(jī)樹(shù)會(huì)向其它空白區(qū)域進(jìn)行無(wú)效擴(kuò)展。q取值較大時(shí)，隨機(jī)樹(shù)以較大的概率選取目標(biāo)節(jié)點(diǎn)進(jìn)行擴(kuò)展，更傾向朝著目標(biāo)點(diǎn)生長(zhǎng)，擴(kuò)展的樹(shù)節(jié)點(diǎn)較少；但由于隨機(jī)節(jié)點(diǎn)選擇單一，規(guī)避障礙物的能力減弱，甚至陷入局部極小值。因此需要花費(fèi)更多的時(shí)間躲避障礙，進(jìn)而增加了規(guī)劃時(shí)間。

通過(guò)以上仿真分析可知，本文中的qmax可取0.7，此時(shí)擴(kuò)展樹(shù)有較大的偏目標(biāo)趨勢(shì)，且路徑規(guī)劃時(shí)間相對(duì)較少，在無(wú)障礙空間內(nèi)能以較大的概率朝著目標(biāo)搜索。而對(duì)于qmin，取0.1時(shí)，算法規(guī)劃時(shí)間、擴(kuò)展的樹(shù)節(jié)點(diǎn)相對(duì)較多。在路徑規(guī)劃中，q值的選取根據(jù)定義視具體情況而定，合理完成RRT算法的路徑規(guī)劃。

3.2 仿真實(shí)驗(yàn)

機(jī)器人移動(dòng)的仿真環(huán)境范圍設(shè)置為100*100單位格，整個(gè)環(huán)境空間分為障礙區(qū)域和非障礙區(qū)域，其中空白處表示無(wú)障礙區(qū)域，黑色圓表示障礙物。仿真環(huán)境中設(shè)定機(jī)器人移動(dòng)的起始點(diǎn)坐標(biāo)為（5，5），目標(biāo)點(diǎn)坐標(biāo)為（95，95）。

采用RRT算法、Bg-RRT算法和Vpt-RRT算法分別在大型障礙物環(huán)境、存有狹窄通道環(huán)境、復(fù)雜障礙環(huán)境下進(jìn)行對(duì)比實(shí)驗(yàn)。各種環(huán)境中的障礙物大小、數(shù)量均隨機(jī)生成。

圖5為3種算法在大型障礙物環(huán)境下的路徑規(guī)劃結(jié)果，其中“+”表示隨機(jī)樹(shù)在規(guī)劃過(guò)程中擴(kuò)展的樹(shù)節(jié)點(diǎn)，線條表示由起始點(diǎn)到目標(biāo)點(diǎn)的路徑規(guī)劃擬合線。圖6為狹窄通道環(huán)境下3種算法的路徑規(guī)劃結(jié)果，仿真過(guò)程設(shè)定迭代次數(shù)為500次時(shí)，RRT算法會(huì)出現(xiàn)無(wú)法成功規(guī)劃的現(xiàn)象，Vpt-RRT算法則能夠在每次仿真中快速找到一條有效路徑。復(fù)雜障礙物環(huán)境下的路徑規(guī)劃如圖7所示。

由圖5-圖7可看出，本文提出的Vpt-RRT算法在不同的障礙環(huán)境中都能合理地規(guī)劃出一條有效路徑。

為了更清楚地體現(xiàn)變參數(shù)閾值RRT算法的優(yōu)越性，在以上3組實(shí)驗(yàn)中分別用RRT算法、Bg-RRT算法和Vpt-RRT算法各進(jìn)行100次仿真實(shí)驗(yàn)，取平均值作對(duì)比。表1和表2分別為路徑規(guī)劃過(guò)程中的總節(jié)點(diǎn)數(shù)和有效節(jié)點(diǎn)數(shù)。

由表1、表2可知，Vpt-RRT算法在3種不同環(huán)境下路徑規(guī)劃過(guò)程中擴(kuò)展的總節(jié)點(diǎn)數(shù)和有效節(jié)點(diǎn)數(shù)均優(yōu)于其它兩種算法，特別是在存有狹窄通道的環(huán)境下，這說(shuō)明Vpt-RRT算法能夠大大減少擴(kuò)展的樹(shù)節(jié)點(diǎn)，并且該算法具有很好的避障性能和尋優(yōu)能力。

4 結(jié)語(yǔ)

本文針對(duì)基本隨機(jī)擴(kuò)展樹(shù)算法在路徑規(guī)劃中隨機(jī)性大、搜索方式過(guò)于平均、搜索效率低下等問(wèn)題，分析其原因在于隨機(jī)點(diǎn)在全空間分布過(guò)于均勻。鑒于此，提出了Vpt-RRT算法，在原算法中通過(guò)增加變參數(shù)閾值，視具體環(huán)境確定閾值大??；引導(dǎo)擴(kuò)展中樹(shù)節(jié)點(diǎn)的選取，使節(jié)點(diǎn)有偏向目標(biāo)點(diǎn)的趨勢(shì)，避免了擴(kuò)展中的無(wú)效搜索，使得隨機(jī)樹(shù)能夠朝著目標(biāo)點(diǎn)快速地?cái)U(kuò)展生長(zhǎng)，解決了隨機(jī)點(diǎn)分布過(guò)于平均的缺點(diǎn)；并且由于每次擴(kuò)展時(shí)參數(shù)閾值都具有可變性，因而避免了陷入局部極小值。最后在3種不同環(huán)境下，分別對(duì)RRT算法、Bg-RRT算法與本文提出的Vpt-RRT算法進(jìn)行仿真實(shí)驗(yàn)。結(jié)果表明，在不同的仿真環(huán)境中，本文提出的算法均具有更好的可行性和有效性。

參考文獻(xiàn)：

[1] 魏唯.智能規(guī)劃方法中啟發(fā)式搜索策略的研究[D].長(zhǎng)春：吉林大學(xué)，2013.

[2] 朱大奇，顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述[J].控制與決策，2010，25（7）：961-967.

[3] 張煜，任保安，陳璟.基于改進(jìn)RRT算法的預(yù)警機(jī)實(shí)時(shí)航跡規(guī)劃[J].計(jì)算機(jī)仿真，2016（9）：106-112.

[4] 馮林，賈菁輝.基于對(duì)比優(yōu)化的RRT路徑規(guī)劃改進(jìn)算法[J].計(jì)算機(jī)工程與應(yīng)用，2011，47（3）：210-213，228.

[5] 宋金澤，戴斌，單恩忠，等.一種改進(jìn)的RRT路徑規(guī)劃算法[J].電子學(xué)報(bào)，2010，38：225-228.

[6] 杜明博，梅濤，陳佳佳，等.復(fù)雜環(huán)境下基于RRT的智能車(chē)輛運(yùn)動(dòng)規(guī)劃算法[J].機(jī)器人，2015（4）：443-450.

[7] 黃炳強(qiáng)，曹廣益.基于人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃研究[J].計(jì)算機(jī)工程與應(yīng)用，2006（27）：26-28.

[8] FRAGKOPOULO CHRISTOS，GRAESER AXEL.Extended algorithm with dynamic N-dimensional cuboid domains[C].Proceedings of the International Conference on Optimisation of Electrical and Electronic Equipment，OPTIM，2010：851-857.

[9] GARRIDO SANTIAGO，BLANCO DOLORES，MORENO LUIS，et al.Improving RRT motion trajectories using VFM [C].IEEE 2009 International Conference on Mechatronics，ICM，2009.

[10] 王濱，金明河，謝宗武，等.基于啟發(fā)式的快速擴(kuò)展隨機(jī)樹(shù)路徑規(guī)劃算法[J].機(jī)械制造，2007，45（12）：1-4.

（責(zé)任編輯：孫 娟）