孫豐財， 張亞楠， 史旭華

(寧波大學(xué) 信息科學(xué)與工程學(xué)院,浙江 寧波 315000)

改進(jìn)的快速擴(kuò)展隨機(jī)樹路徑規(guī)劃算法*

孫豐財， 張亞楠， 史旭華

(寧波大學(xué) 信息科學(xué)與工程學(xué)院,浙江 寧波 315000)

針對快速擴(kuò)展隨機(jī)樹(RRT)路徑規(guī)劃算法缺乏穩(wěn)定性和偏離最優(yōu)解的問題，提出了一基于RRT的偏向性路徑搜索算法(m-RRT)。m-RRT采用生成隨機(jī)點向量組的形式對隨機(jī)點選取策略進(jìn)行了優(yōu)化，改善快速擴(kuò)展隨機(jī)樹的不確定性，減少不必要的擴(kuò)展，而加快向目標(biāo)位置搜索的速度，且得到的路徑優(yōu)于RRT算法的結(jié)果。通過其在二維平面路徑規(guī)劃和三維機(jī)械臂路徑規(guī)劃的測試,表明其具有一定的應(yīng)用價值。

路徑規(guī)劃； 機(jī)械臂； 快速擴(kuò)展隨機(jī)樹算法； 避障； 機(jī)器人操作系統(tǒng)

0 引 言

機(jī)器人路徑規(guī)劃即通過某些性能(如時間、距離、能量)指標(biāo)來規(guī)劃出一條從初始位置到目標(biāo)位置的最優(yōu)或者較優(yōu)的線路。目前，機(jī)械臂路徑規(guī)劃方法主要有細(xì)胞分解法、人工勢場法、隨機(jī)路標(biāo)法(probabilistic roadmap，PRM)和快速擴(kuò)展隨機(jī)樹(rapidly-exploring random tree，RRT)法等[1～3]。

針對傳統(tǒng)RRT算法的缺陷，Burget F，Bennewitz M等人[4]采用雙向隨機(jī)搜索樹(Bi2RRT) 搜索算法，提高了搜索效率，由于該算法較原始RRT算法有更好的收斂性，因此，在目前路徑規(guī)劃中很常見。Melchior N A[5]提出的粒子RRT算法考慮了地形的不確定性，保證了在不確定性環(huán)境下隨機(jī)搜索樹的擴(kuò)展。Kuffner J J等人[6]提出了RRT2connect算法，使得節(jié)點的擴(kuò)展效率大大提高；同時王道威等人[7]采用動態(tài)步長改善了RRT的不確定性，馮林等人[8]通過對比優(yōu)化的方法使得在復(fù)雜環(huán)境下的規(guī)劃穩(wěn)定性增強(qiáng)，宋金澤等人[9]通過添加非完整性約束及雙向多步擴(kuò)展方式提高了搜索效率。

本文針對RRT算法搜索缺乏穩(wěn)定性和偏離最優(yōu)解等缺點，對RRT算法進(jìn)行了相應(yīng)的改進(jìn)，通過實驗發(fā)現(xiàn)改進(jìn)算法在二維環(huán)境和三維環(huán)境均能取得較好的效果。因此,算法對二維、三維環(huán)境的路徑規(guī)劃，尤其是機(jī)械臂的路徑規(guī)劃具有一定的參考價值。

1 RRT算法流程

1.1 RRT算法

以節(jié)點qstart為初始點，qgoal為目標(biāo)點，建立搜索樹T，其初始化和擴(kuò)展過程步驟如下:

1)選擇初始點qstart為根節(jié)點。

2)在位形空間中隨機(jī)選取采樣點qrand。

3)在已經(jīng)產(chǎn)生的搜索樹上，搜索一個距離采樣點qrand最近的節(jié)點qnearest。

4)在qnearest和qrand的連線上，以一定的步長r從qnearest出發(fā)創(chuàng)造一個新的節(jié)點qnew。如果從qnearest到qnew的擴(kuò)展過程中無碰撞發(fā)生，則將這個新的節(jié)點qnew加入到搜索樹中，并同時將從qnearest到qnew的邊插入到搜索樹中；反之，則無擴(kuò)展發(fā)生。

重復(fù)上述步驟，在用戶定義約束的限制下，不斷計算qnew與qgoal的距離，直至其小于給定的距離閾值。

圖1 RRT算法核心步驟

RRT算法的隨機(jī)采樣擴(kuò)張?zhí)匦?，?dǎo)致其收斂到目標(biāo)點位置的速度比較慢，耗費的空間資源比較大，同時搜索出的路徑往往偏離最短路徑；為了提高算法的效率和性能，需不斷對該算法進(jìn)行改進(jìn)。

1.2 改進(jìn)后的RRT算法

針對擴(kuò)展節(jié)點的選取，提出了改進(jìn)RRT算法，即m-RRT算法。

首先，m-RRT算法采用大多數(shù)變異RRT算法的通用策略，在標(biāo)準(zhǔn)的RRT算法基礎(chǔ)上加入了偏向策略，即在一定的小概率情況下將隨機(jī)采樣節(jié)點qrand設(shè)置為目標(biāo)點，以促進(jìn)搜索因子盡快趨向目標(biāo)點，以提高搜索效率。

其次，與標(biāo)準(zhǔn)RRT算法步驟(2)不同，m-RRT算法每次隨機(jī)采樣m個節(jié)點，按照與目標(biāo)點的距離排序構(gòu)成隨機(jī)序列，將距目標(biāo)點最近的點作為隨機(jī)點qrand開始探索，如果發(fā)現(xiàn)發(fā)生碰撞導(dǎo)致無法擴(kuò)展，則需取次距離點進(jìn)行計算；當(dāng)隨機(jī)序列中首次未碰撞的節(jié)點執(zhí)行完成，或者隨機(jī)序列為空之后再次隨機(jī)生成m個采樣節(jié)點，繼續(xù)上述步驟。

算法減少了不必要的探索方向，一定幾率地減少了再次遇到障礙物的可能性，保留了對目標(biāo)位置的全局趨向性；同時，依次嘗試保證了節(jié)點有能力跳出障礙區(qū)域，避免陷入局部，有利于快速尋找到目標(biāo)點。本文通過實驗對比m-RRT算法的搜索時間效率和結(jié)果路徑優(yōu)劣性，選擇每輪隨機(jī)采樣個數(shù)m=4，此時算法性能表現(xiàn)良好。m-RRT算法主要流程如下：

1)初始化起點qstart和目標(biāo)點qgoal并在位形空間內(nèi)建立障礙物模型。

2)在位形空間中隨機(jī)選取采樣點qrand,并擴(kuò)展新生成節(jié)點qnew直至到達(dá)目標(biāo)節(jié)點qgoal。

a.在空間中隨機(jī)采樣4個位置節(jié)點，并按照離目標(biāo)點的距離排序存入qrand_arr隨機(jī)序列，選取離目標(biāo)點距離最近點設(shè)為節(jié)點qrand。

(1)

機(jī)械臂距離公式(各關(guān)節(jié)角度距離其目標(biāo)角度之和)

(2)

b.按照標(biāo)準(zhǔn)RRT算法步驟選擇隨機(jī)樹T中離隨機(jī)采樣點qrand最近的節(jié)點為qnearest，按照一定步長進(jìn)行生成擴(kuò)展節(jié)點qnew，同時檢測從節(jié)點qnearest到節(jié)點qnew是否與障礙物發(fā)生碰撞，如果發(fā)生碰撞，則在隨機(jī)采樣點qrand_arr隨機(jī)序列中選擇次位的節(jié)點成為新的隨機(jī)節(jié)點qrand。

3)如果沒有發(fā)生碰撞則在此方向繼續(xù)擴(kuò)展節(jié)點并收入隨機(jī)樹中，繼續(xù)沿此方向擴(kuò)展，直至發(fā)生碰撞，此時，舍棄隨機(jī)序列qrand_arr，重新生成4個隨機(jī)點構(gòu)成新的隨機(jī)序列。

4)當(dāng)目標(biāo)點qgoal與新生成的節(jié)點qnew距離小于或等于步長時，將qnew=qgoal，此時如無碰撞則程序結(jié)束，若有碰撞則繼續(xù)步驟(2)。

5)將得到的路徑點進(jìn)行保存，得出結(jié)果。

改進(jìn)后算法在二維平面路徑規(guī)劃中可以大量減少規(guī)劃時間，并且整棵樹的生長偏向于目標(biāo)點，隨著樹的生長更容易找出更好的路徑；在三維空間中，不需耗費巨大的空間資源，同時，m-RRT算法由于樹的趨向性生長避免了對許多無意義空間的探索，減少了標(biāo)準(zhǔn)RRT算法執(zhí)行時間，相比于RRT-Start等追求接近最優(yōu)解的算法，其規(guī)劃結(jié)果也很理想。

2 實驗對比

2.1 二維有障礙環(huán)境下算法效果

2.1.1 簡單環(huán)境下算法比較

如圖2所示，實驗分別對piz-RRT算法[10]、標(biāo)準(zhǔn)RRT算法、RRT-star算法[11]、m-RRT算法進(jìn)行了比較，其中m-RRT算法給出了當(dāng)隨機(jī)序列節(jié)點數(shù)m取值分別為2，4，6時的運(yùn)行效果。演示圖均為左側(cè)圓形點位置為起始點右側(cè)圓形點位置為目標(biāo)點進(jìn)行規(guī)劃。在簡單環(huán)境下，對比4種算法效果發(fā)現(xiàn)m-RRT算法擴(kuò)展節(jié)點大量減少，樹的生成有一種趨向性，空間資源浪費很少；同時通過不同隨機(jī)序列節(jié)點數(shù)m的取值，可以明顯發(fā)現(xiàn)規(guī)劃路徑慢慢趨向于穩(wěn)定和更優(yōu)化。

圖2 簡單環(huán)境下4種算法比較

2.1.2 復(fù)雜環(huán)境下算法比較

復(fù)雜環(huán)境中，不僅障礙物數(shù)量增多，起始點與目標(biāo)點也不易直接搜尋，很大程度提高了路徑規(guī)劃的難度。如圖3所示，同樣將以上4種算法進(jìn)行比較，其中，m-RRT算法給出了當(dāng)隨機(jī)序列節(jié)點數(shù)m分別取值為2，4，6時的運(yùn)行效果。演示圖均為左側(cè)圓形點位置為起始點，右側(cè)圓形點為止為目標(biāo)點進(jìn)行規(guī)劃。通過對比可以發(fā)現(xiàn)即使環(huán)境變得復(fù)雜，m-RRT算法依然能夠利用很少的擴(kuò)展到達(dá)目標(biāo)位置，不僅消耗空間資源少，規(guī)劃路徑也具有明顯的優(yōu)勢。

圖3 復(fù)雜環(huán)境下4種算法比較

通過以上實驗可以發(fā)現(xiàn)：對于m-RRT算法，不同m的取值使得規(guī)劃路徑慢慢趨向于穩(wěn)定和更優(yōu)化，但經(jīng)過對比發(fā)現(xiàn)這種路徑的優(yōu)化是通過犧牲時間效率達(dá)到的，并且當(dāng)m節(jié)點數(shù)增加太多也會使m-RRT算法的優(yōu)勢降低，通過綜合對比最終選用m=4，此時路徑規(guī)劃算法無論規(guī)劃結(jié)果或者規(guī)劃耗時均較為滿意，視為最佳方案。

2.1.3 二維路徑規(guī)劃效率對比

以上實驗的4種算法中，從耗費空間資源的角度，RRT-star算法和標(biāo)準(zhǔn)RRT算法耗費最多，而m-RRT算法與piz-RRT算法相差不多，相比前兩者，在空間資源的消耗上有明顯的優(yōu)化，付出的空間代價很小。從規(guī)劃結(jié)果來看，其他3種算法的隨機(jī)性較m-RRT算法強(qiáng)，m-RRT算法很大程度上降低了隨機(jī)程度。同時，通過表1中所示的5種不同環(huán)境下(從第一組到第五組障礙物依次增多)，4種算法的路徑規(guī)劃耗時對比，可以看出：RRT-star算法時間復(fù)雜度成倍提升，其次為標(biāo)準(zhǔn)RRT算法，這兩者在時間的消耗上遠(yuǎn)大于piz-RRT和m-RRT算法?？梢?，隨著環(huán)境復(fù)雜度的增大，m-RRT也同樣獲得了很好的效果。實驗證明了m-RRT算法規(guī)劃出的路徑更趨向于平穩(wěn)，在大多數(shù)情況下優(yōu)于其他3種算法，且規(guī)劃路徑較為緊湊，也更接近于理想路徑。

表1 4種改進(jìn)RRT算法效率對比 s

2.2 三維有障礙環(huán)境下機(jī)械臂路徑規(guī)劃算法效果

2.2.1 機(jī)械臂路徑規(guī)劃演示

如圖4所示為運(yùn)用m-RRT算法的路徑規(guī)劃運(yùn)行效果。圖(a)中綠色機(jī)械臂為起始位置，橘色機(jī)械臂代表目標(biāo)位置，圖4(b)～圖4(f)依次給出了一次規(guī)劃的結(jié)果，整體運(yùn)行路線如圖4(g)和圖4(h)所示?？梢钥闯觯涸谟姓系K物的環(huán)境下運(yùn)用m-RRT算法能夠完整地解決此類路徑規(guī)劃問題，將機(jī)械臂移至側(cè)面的行為完全沒有接觸到障礙物，并且機(jī)械臂先運(yùn)動到上方，依次順著箱體表面移動到下方，效果理想，證明了m-RRT算法在三維機(jī)械臂的路徑規(guī)劃中的有效性。

圖4 三維環(huán)境下改進(jìn)算法路徑規(guī)劃結(jié)果

2.2.2 機(jī)械臂路徑規(guī)劃效率對比

圖5所示為2種不同三維環(huán)境下標(biāo)準(zhǔn)RRT算法和m-RRT算法的效率比較，分為不同組別，每組取平均值進(jìn)行比較，可以看出：在障礙物較多的情況下2種RRT算法在解決路徑規(guī)劃問題的效率有較大差異，標(biāo)準(zhǔn)RRT算法耗時較多且并不穩(wěn)定，m-RRT算法相對更加穩(wěn)定，且不論環(huán)境是否復(fù)雜，其執(zhí)行效率相對優(yōu)越。同時，在三維機(jī)械臂路徑規(guī)劃實驗中發(fā)現(xiàn)RRT-star等算法效率較差，耗時過久且時常無法找到解，并不適用于此類規(guī)劃。

圖5 復(fù)雜環(huán)境和簡單環(huán)境情況下效率對比

3 結(jié)束語

通過以上二維平面和三維機(jī)械臂路徑規(guī)劃研究發(fā)現(xiàn)，m-RRT算法明顯提高了RRT路徑規(guī)劃的穩(wěn)定性，并且有利于規(guī)劃出接近于最優(yōu)路徑的較優(yōu)路徑，相對于標(biāo)準(zhǔn)RRT算法，其效率更高，無論在算法效率還是路徑質(zhì)量，均體現(xiàn)出了很大的優(yōu)越性。同時m-RRT算法在三維機(jī)械臂路徑規(guī)劃上也能夠取得較好的效果。實驗表明：m-RRT算法在路徑規(guī)劃上具有一定的應(yīng)用價值。

[1] 王海英,蔡向東,尤 波,等.基于遺傳算法的移動機(jī)器人動態(tài)路徑規(guī)劃研究[J].傳感器與微系統(tǒng),2007,26(8):32-34.

[2] 王春華,邱立鵬,潘德文.改進(jìn)蟻群算法的機(jī)器人焊接路徑規(guī)劃[J].傳感器與微系統(tǒng),2017,36(2):75-77.

[3] 張云峰,馬振書,孫華剛，等.基于改進(jìn)快速擴(kuò)展隨機(jī)樹的機(jī)械臂路徑規(guī)劃[J].火力與指揮控制,2016,41(5):25-30.

[4] Burget F,Bennewitz M,Burgard W.BI^2RRT:An efficient sampling-based path planning framework for task-constrained mobile manipulation[C]∥IEEE/RSJ International Conference on Inteligent Robots & Systems,2016.

[5] Melchior N A,Simmons R.Particle RRT for path planning with uncertainty[C]∥IEEE International Conference on Robotics & Automation,2007:1617-1624.

[6] Kuffner J J,Lavalle S M.RRT-connect:An efficient approach to single-query path planning[C]∥IEEE International Conference on Robotics & Automation,2000:995-1001.

[7] 王道威,朱明富,劉 慧.動態(tài)步長的RRT 路徑規(guī)劃算法[J].計算機(jī)技術(shù)與發(fā)展,2016,26(3):105-107.

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

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

[10] 徐 娜,陳 雄,孔慶生,等.非完整約束下的機(jī)器人運(yùn)動規(guī)劃算法[J].機(jī)器人,2011,33(6):666-672.

[11] Islam F,Nasir J,Malik U,et al.RRT-smart:Rapid convergence implementation of RRT towards optimal solution[C]∥International Conference on Mechatronics and Automation,2012:1651-1656.

史旭華，女，通訊作者，教授，碩士生導(dǎo)師，主要從事模式識別與智能算法的研究工作，E—mail:928111455@qq.com。

Improved rapidly-exploring random tree path planning algorithm*

SUN Feng-cai， ZHANG Ya-nan， SHI Xu-hua

(College of Information Science and Engineering,Ningbo University,Ningbo 315000,China)

Because the rapidly-exploring random tree(RRT)path planning algorithm is unstable and not optimal,propose a biased path search strategy which is calledm-RRT.By generating a random point vectors to optimize the strategy of random points selection,the uncertainty of RRT searching can be improved,meanwhile,the expansion of searching tree can also be reduced.So,it can improve the searching speed to the destination,and the path got bym-RRT is better than that by RRT.Through the two-dimensional path planning and three-dimensional manipulator path planning,the results show that it has certain application value.

path planning;manipulator; rapidly exploring random tree(RRT)algorithm; obstacle avoidance; robot operating system(ROS)

10.13873/J.1000—9787(2017)09—0129—03

2017—07—12

浙江省自然科學(xué)基金資助項目 (LY14F030004)；浙江省科技計劃資助項目 (2015C31017)；寧波市自然科學(xué)基金資助項目(2016A610092)

TP 24

A

1000—9787(2017)09—0129—03

孫豐財(1992-)，男，碩士研究生，研究方向為模式識別與智能算法。