基于改進(jìn)RRT算法的垃圾收集車路徑規(guī)劃研究

顧海蛟， 宋 宇

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春 130012)

0 引 言

隨著現(xiàn)代技術(shù)的發(fā)展，無人化時(shí)代已然來臨，原本需要人工才能完成的簡(jiǎn)單和危險(xiǎn)的工作被機(jī)器所代替，比如現(xiàn)在快遞業(yè)的無人寄取、大疆科技的無人機(jī)，以及常見的無人售貨機(jī)等。就無人駕駛的垃圾回收車而言，決定其性能優(yōu)劣的實(shí)質(zhì)是內(nèi)部的動(dòng)力驅(qū)動(dòng)系統(tǒng)和外部的路徑規(guī)劃問題。其中路徑規(guī)劃問題直接決定著工作效率和是否能夠完成所要執(zhí)行任務(wù)的關(guān)鍵。因此，一種合理與高效的路徑規(guī)劃算法是實(shí)現(xiàn)其工作性能最大化的保證。

常用的路徑規(guī)劃算法有圖搜索法、人工勢(shì)場(chǎng)法、遺傳算法、RRT算法等。圖搜索法是在已知環(huán)境和障礙物信息構(gòu)造從起點(diǎn)到目標(biāo)點(diǎn)的可行路徑[1]，主要分為深度優(yōu)先和廣度優(yōu)先兩個(gè)方向。在Prim和Dijkstar最短路徑算法中采用了廣度優(yōu)先的思想，但規(guī)劃效果對(duì)啟發(fā)式函數(shù)依賴性太強(qiáng)，較好的啟發(fā)函數(shù)需要靠試湊來獲得[2]。人工勢(shì)場(chǎng)法是將整體抽象成一個(gè)引力場(chǎng)，目標(biāo)點(diǎn)對(duì)移動(dòng)物體產(chǎn)生“引力”，障礙物對(duì)移動(dòng)物體產(chǎn)生“斥力”，最后通過“合力”控制物體的運(yùn)動(dòng)方向，但規(guī)劃效果得到的往往不是全局最優(yōu)路徑[3]。遺傳算法是一種仿生物算法，隨著一代又一代的“遺傳”,逐漸找到最優(yōu)路徑，容易出現(xiàn)過早的收斂和停滯現(xiàn)象。RRT算法是一種增量式采樣的搜索方法，在應(yīng)用中不需要任何參數(shù)，具備良好的使用性能，它利用增量遞增方法構(gòu)建搜索樹，逐漸提高分辨能力，而無需設(shè)置任何參數(shù)與函數(shù)。

文中主要對(duì)垃圾回收車的路徑規(guī)劃進(jìn)行研究，假設(shè)在高檔小區(qū)每個(gè)家庭的垃圾都在對(duì)應(yīng)規(guī)定的地方投放，而無人回收垃圾車在預(yù)先標(biāo)記的垃圾點(diǎn)自主地將垃圾收集到一起處理，就會(huì)考慮到小車路徑規(guī)劃問題，只要將小車路徑規(guī)劃好，效率就會(huì)大大提高。在路徑規(guī)劃算法中,RRT算法在應(yīng)用中需要參數(shù)少，具有良好的使用功能，容易實(shí)現(xiàn)。借鑒改進(jìn)RRT算法的基礎(chǔ)上，提出一種新的、易于實(shí)現(xiàn)的改進(jìn)RRT算法的路徑規(guī)劃方法。傳統(tǒng)RRT算法在設(shè)定好出發(fā)點(diǎn)和目標(biāo)點(diǎn)的位置后，只需要在一定的迭代次數(shù)后就會(huì)找到一條從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的路徑，但速度會(huì)比較慢，而在改進(jìn)后的雙向隨機(jī)搜索樹是從起始點(diǎn)和目標(biāo)點(diǎn)并行生成兩棵RRT樹，直至兩棵樹相遇，這樣會(huì)縮短搜索過程用時(shí)，減少計(jì)算機(jī)的計(jì)算用時(shí)，但會(huì)在規(guī)劃的路徑中出現(xiàn)障礙盲區(qū)，在實(shí)際允許的情形下加入“改線”機(jī)制，改進(jìn)后的算法縮短了路徑長(zhǎng)度，達(dá)到節(jié)省能耗的目的。

1 RRT算法概述

RRT算法是一種基于隨機(jī)采樣的樹結(jié)構(gòu)搜索算法，以空間給定的起點(diǎn)qstart出發(fā)，通過在給定的封閉空間中隨機(jī)采樣,并且引導(dǎo)搜索樹的生長(zhǎng)。當(dāng)樹的節(jié)點(diǎn)進(jìn)入目標(biāo)區(qū)域內(nèi),并且找到終點(diǎn)qgoal時(shí)算法結(jié)束，然后回溯到起始節(jié)點(diǎn),即可得到所規(guī)劃的路徑。用有向圖表示路徑G=(u,E)，一條規(guī)劃的路徑就是一系列坐標(biāo)點(diǎn)的順序連接(u1,u2,u3,…,un)，u1=qstart,un=qgoal。同時(shí)(ui,ui+1)∈E,1≤i≤n-1表示邊。實(shí)質(zhì)就是使用采樣點(diǎn)來擴(kuò)展圖G，之后就是一條從起點(diǎn)到終點(diǎn)的路徑。

RRT算法流程如下：

1.U←{qstart}，E←φ，i←0

2.While i

3.G′←(u,E)

4.qrand←Sample(i)，i←i+1

5.(U,E)←extend(G′,qrand)

6.G←(G′,qrand)

7.end while

RRT算法的簡(jiǎn)易圖解如圖1所示。

圖1 RRT算法的簡(jiǎn)易圖解

最初，擴(kuò)展圖G的坐標(biāo)點(diǎn)集只有初始節(jié)點(diǎn)qstart，邊集E為空集。然后進(jìn)入迭代,即進(jìn)入While循環(huán)，迭代次數(shù)為N，設(shè)置為新的擴(kuò)展圖G′，Sample(i)用來采樣一個(gè)新的點(diǎn)qrand，利用新的點(diǎn)來拓展圖G，最后到達(dá)目標(biāo)點(diǎn)qgoal，結(jié)束循環(huán)，此時(shí),通過RRT算法找到從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的規(guī)劃路徑[4-7]。

2 RRT-Connect算法概述

RRT-Connect算法是在RRT算法的基礎(chǔ)上加上了雙樹雙向抖索的引導(dǎo)策略，并且在擴(kuò)展路徑的方式基礎(chǔ)上加入了貪婪策略,用來加快搜索速度。

RRT-Connect算法的簡(jiǎn)易圖解如圖2所示。

圖2 RRT-Connect算法的簡(jiǎn)易圖解

圖2中RRT-Connect算法在搜索路徑時(shí)，從起始點(diǎn)與目標(biāo)點(diǎn)兩端同時(shí)進(jìn)行路徑的搜索，直到兩段路徑相遇，即全局搜索路徑結(jié)束，得到規(guī)劃路徑軌跡。

3 算法”改線“機(jī)制

經(jīng)過RRT-Connect算法規(guī)劃得到的路線將會(huì)在障礙物的邊緣出現(xiàn)障礙盲區(qū)，這一部分盲區(qū)主要是由起始點(diǎn)與目標(biāo)點(diǎn)之間進(jìn)行雙向的樹形搜索存在的不足引起的，不能同時(shí)兼顧搜索時(shí)間短與路徑長(zhǎng)度短的優(yōu)點(diǎn)，針對(duì)上述問題引入”改線機(jī)制“，其原理是三角形的三邊原理，如圖3所示。

圖3 三角形三邊原理

假設(shè)三角形的三條邊長(zhǎng)度分別為a，b，c，其中a，b兩邊的夾角為α，其對(duì)應(yīng)的邊為c。三角形兩邊之和大于第三邊，a+b>c。文中就是利用這一原理，在規(guī)劃的路徑中引入改線機(jī)制，使得規(guī)劃后的路徑實(shí)際運(yùn)動(dòng)距離更短。

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

仿真是在Matlab2014上進(jìn)行的，在采樣點(diǎn)給定次數(shù)4 000次進(jìn)行的實(shí)驗(yàn)。

起點(diǎn)坐標(biāo)(5，5)，終點(diǎn)坐標(biāo)(95，95)通過RRT-Connect算法找到路徑。

采樣次數(shù)2 000次時(shí)，部分采樣點(diǎn)下生成的路徑規(guī)劃如圖4所示。

圖4 部分采樣點(diǎn)生成路徑規(guī)劃

采樣次數(shù)4 000次時(shí)，全部采樣點(diǎn)下生成的路徑規(guī)劃如圖5所示。

圖5 全部采樣點(diǎn)生成路徑規(guī)劃

全部采樣點(diǎn)生成的路徑長(zhǎng)度比部分采樣點(diǎn)生成的路徑長(zhǎng)度少4.25%。

4.1 改線仿真實(shí)驗(yàn)

在控制其它變量不變，只改變采樣點(diǎn)數(shù)的情況下進(jìn)行仿真實(shí)驗(yàn)，下面將進(jìn)行改線，其中改線是在圖5障礙盲區(qū)的路徑段進(jìn)行改線，改線后在滿足垃圾收集車實(shí)際運(yùn)動(dòng)軌跡的情形下路徑規(guī)劃如圖6所示。

圖6 改線后的路徑規(guī)劃

圖6相較于圖5，生成的路徑長(zhǎng)度減少4.45%。

4.2 仿真數(shù)據(jù)對(duì)比

算法結(jié)果對(duì)比見表1。

表1 算法結(jié)果對(duì)比

改進(jìn)后的算法路徑在保證采樣點(diǎn)數(shù)，運(yùn)行時(shí)間一樣的情況下長(zhǎng)度減少了。達(dá)到了減少實(shí)際運(yùn)行路徑長(zhǎng)度的目的。

5 結(jié) 語

提出的基于改進(jìn)RRT-Connect算法的垃圾收集車路徑規(guī)劃算法，通過利用三角形三邊定理進(jìn)行“改線”，得出的實(shí)際運(yùn)行路徑長(zhǎng)度相較于原始算法更短，能達(dá)到減少耗能的目的。仿真結(jié)果表明，改進(jìn)后的算法路徑長(zhǎng)度相較于原始算法的路徑長(zhǎng)度縮短了4.45%，有很大的利用價(jià)值。