宋林憶，嚴華

（四川大學(xué)電子信息學(xué)院，成都 610065）

0 引言

隨著人類科技的不斷發(fā)展與進步，移動機器人越來越多地被運用到我們的工作當中。路徑規(guī)劃算法[1]的研究是當前機器人研究的一個重要分支，它指移動機器人在當前地圖中能夠找出一條合適的從起點到終點的軌跡，并且能夠繞過障礙物。

目前已有許多成熟的理論，如勢場法[2]、A*算法[3]和遺傳算法[4]等，快速探索隨機樹算法（RRT）[5]亦是其中之一。RRT算法效率高并且可以運用于未知地圖中，然而RRT算法依舊存在著一些不足，如生成的路徑成本高而不優(yōu)，路徑要在全地圖中來搜索和采樣，從而增加了很多不相關(guān)節(jié)點，增加了存儲器需要的內(nèi)存，并且所需時間相應(yīng)增長，收斂速率緩慢[6]。

由于上述不足的存在，國際上很多學(xué)者紛紛對這些進行了研究，并提出了很多改善后的RRT算法，以適應(yīng)不同的應(yīng)用場景。Kuffne和Lavalle聯(lián)合發(fā)表了RRT-connect算法[7]，第二年，提出了一種雙向搜索樹[8]。C.Urmson和R.Simmon發(fā)表了一個基于啟發(fā)式的偏向算法[9]，它將RRT向目標點的位置偏移，這樣RRT算法就可以得到一個較優(yōu)的規(guī)劃路徑。D.Ferguson和A.Stentz嘗試著運行RRT算法數(shù)次來降低路徑成本[10]，多次運行后也能收斂到相對較優(yōu)的路徑，同時算法多次運行后得到的路徑成本較小。S.Karaman和E.Frazzoli提出了RRT*算法[11]，它改變了原始RRT算法中挑選父節(jié)點的操作過程，并使用成本函數(shù)挑選出擴展結(jié)點域中成本最低的一個點作為父節(jié)點。然后，每經(jīng)過一次迭代后重新連接樹上已經(jīng)存在的節(jié)點，進而確保計算復(fù)雜度和漸進最優(yōu)解。

然而，RRT*算法的缺點在于它的最優(yōu)化是用降低收斂速度和增加搜索持續(xù)時間為代價的。文獻[12]引入了目標偏置策略，它讓隨機生成樹有目的地向目標點延伸，但是目標偏置策略會導(dǎo)致一些隨機性缺失，削弱避障能力，并有可能導(dǎo)致陷入局部最小值，文獻[13]提出了規(guī)避步長延伸法，它可以使隨機樹合理地遠離障礙區(qū)域，避免陷入局部最小值。

以上方法雖然可以合理遠離障礙區(qū)，可是在得到最優(yōu)解上需要迭代次數(shù)過多，并且節(jié)點數(shù)過大，無法滿足在低存儲量的環(huán)境下的需要。因此，本文提出的Modified Informed-RRT*（MI-RRT*）算法在目標偏置策略的基礎(chǔ)上，運用規(guī)避步長的延伸法，使快速擴展隨機樹在延伸時避開障礙物，并運用橢圓采樣來減少得到最優(yōu)解所需要的迭代次數(shù)，從而使得算法快速收斂，同時限制了最大節(jié)點數(shù)，降低了其所需的內(nèi)存大小，能夠運用更少的時間，快速收斂到一條相對成本較低的路徑。然后經(jīng)過相關(guān)實驗證明了所提出算法的合理和可行性。

1 RRT＊算法

由于RRT算法收斂速度慢等不足，Karamans跟Frazzolie改善了RRT算法的節(jié)點相連的操作，并發(fā)表出RRT*算法。與原始RRT算法區(qū)別在于，RRT*算法改變了父節(jié)點選擇的操作過程，新增節(jié)點的同時檢測采樣點附近的節(jié)點，并在添加新節(jié)點后確定是否有成本更低的路徑，如果有，則用新節(jié)點替換舊節(jié)點來對路徑進行修正。

如圖1所示，RRT*算法分為兩個階段：

圖1 RRT*擴展算法原理圖

第一階段：用最優(yōu)解把xnew連到樹上。

選擇xnew的辦法與RRT是一樣的，接著以xnew為圓心，以r為半徑畫圓。獲得的所畫圓內(nèi)的全部點，全部放入集合Znear。然后，迭代計算全部xnew→xnear+xnear→xstart的成本，選最小的那個與xnew鏈接。到此第一階段結(jié)束。

第二階段：驗證經(jīng)過xnew到達xnear另外的點的成本有沒有比以前更小，若有的話，則連接新線。如圖xnew向另外兩個xnear連虛線。從經(jīng)過xnew到達xnear，要是比從xstart到xnear的所用代價更低，就應(yīng)用新邊連接，移除舊邊。

RRT*算法的主要步驟如算法1所示。

算法 1 RRT*（xstart,xgoal）

1:V←xstart;E←Φ;T←(V,E);i←0

2:while i

3:xrand←Sample(i)

4:xnearest←NearestNode(xrand,T)

5:xnew←Steer(xnearest,xrand)

6:if CollisionCheck(xnearest,xnew)then

7:T←InsertNode(xnew)

8:xnear←NearNodes(xnew,T)

9:xparent←ChooseBestParent(xnew,xnear,T)

10:OptimizeVertices(xparent,xnew,T)

11:end if

12:end while

RRT*算法優(yōu)化了RRT算法形成的路徑，留住了RRT算法的概率完備性的特征，增大了收斂速率，即經(jīng)過較少的迭代生成代價較少的路徑。

但是，RRT*算法需要在全地圖中來搜索和采樣，當區(qū)域環(huán)境中存有許多障礙之時，不小心就會陷入部分最小值，然后造成路徑規(guī)劃成功率變低。產(chǎn)生了大量結(jié)點，而且計算量隨樣本數(shù)量的增加而增加，在具有有限存儲器的嵌入式系統(tǒng)中會產(chǎn)生阻礙，最后優(yōu)化整個采樣區(qū)域中的路徑是困難的。針對RRT*算法的這些問題，提出了一種改進的RRT*算法，通過目標偏置，自適應(yīng)步長延伸，修改結(jié)點、橢圓采樣等方法對RRT*算法進行改進。

2 改進RRT＊算法的改進策略

下面分別對4個改進策略展開解釋。

2.1 目標偏置模型

以文獻[13]為參考，文章中先根據(jù)均勻概率分布獲取到一個隨機數(shù)p，當p小于預(yù)設(shè)的閾值pmax時,則選擇目標點xgoal為采樣點，否則，根據(jù)原始方法在全局區(qū)域中隨機獲取采樣點。文獻[13]顯著減少了RRT*算法的隨機性，不必在全地圖中來搜索和采樣，加速了RRT*向目標點的擴展。

然而，要是p值過大，擴展樹延伸到最終目標點的趨勢很強，但是隨機樹生長的隨機性受到限制，并且難以繞開障礙區(qū)域，導(dǎo)致路徑規(guī)劃的失??；要是p值過小，隨機樹具有很強的隨機性，經(jīng)過多次增長后，它才可以達到最終目標點并獲得可行路徑。然而，所獲得的路徑將存在繞遠行為，這降低了移動機器人實際操作的效率。為了盡可能縮短路徑，采用了一種不一樣采樣規(guī)則，一次選取2個臨時目標點，其中臨時目標點xrand1為最初的目標點，另一個臨時目標點xrand2為空白區(qū)域隨機選取。根據(jù)采樣點xrand2選擇最近的節(jié)點作為xnear。

影響RRT*算法規(guī)劃速度的主要環(huán)節(jié)是選擇xnear的過程，因為該過程需要遍歷當前隨機樹的所有節(jié)點并依次計算與xrand的距離。臨時目標點采樣規(guī)則與步長調(diào)整規(guī)則相結(jié)合，以盡可能朝向目標點延伸。當擴展新的節(jié)點時，在節(jié)點xnear處優(yōu)先朝向目標點xrand1延伸步長ε，并且當難以延伸時，將步長調(diào)整為0.5ε再次嘗試擴展新節(jié)點。如果擴展再次失敗，它將向臨時目標點xrand2擴展，此過程無需重新選取xnear。

2.2 動態(tài)步長

在原始RRT*中，如果設(shè)置的步長很小，則隨機樹的增長速度將過慢，這將影響得到解決方案的速度；如果設(shè)置的步長較大，在障礙較多的環(huán)境下，新節(jié)點將難以在該環(huán)境下延伸，同時降低了得到解決方案的效率。為了解決這個缺陷，本文引入了一種動態(tài)步長的策略。如圖2所示，Di為距離xrand最近的障礙邊緣的距離，εmin，εmax為設(shè)定好的最小，最大步長。

圖2動態(tài)步長延伸策略

根據(jù)式1計算出步長：

用這個公式計算出的步長保持在最小和最大步長之間。在靠近障礙物的情況下，傳統(tǒng)固定步長生長方法可能會無法通過障礙檢測，這將會降低路徑規(guī)劃效率，而本文提出的動態(tài)步長則可以在障礙物附近生成有效的采樣點。

在遠離障礙物的情況下，傳統(tǒng)固定步長生長受限于固定步長，導(dǎo)致隨機樹在空曠區(qū)域生長緩慢，而本文的動態(tài)步長可以按照最大步長來生長，增快RRT*的生長速度，隨之減少了找到初始解決方案的時間。

2.3 限制RRT★節(jié)點數(shù)量

隨著解決方案的優(yōu)化，樹中的節(jié)點數(shù)量會無限增長。要是我們在具有存儲量有限的嵌入式系統(tǒng)內(nèi)實現(xiàn)RRT*算法時，將會出現(xiàn)問題。

針對節(jié)點數(shù)量無限增長的問題，我們引入了去除滿足一定條件的節(jié)點來限制最大節(jié)點數(shù)[14]。在達到固定數(shù)量的節(jié)點后，它繼續(xù)對樹進行優(yōu)化。具體地說，改進的RRT*使用RRT*的骨架，并用節(jié)點移除過程來豐富它。最初，樹生長與RRT*相同，直到達到最大數(shù)量的節(jié)點M。如果此時沒有一條到達目標點的可行解決方案，則必須重新啟動算法。然后，每當添加新的節(jié)點時，改進的RRT*就刪除一個節(jié)點。

圖3節(jié)點插入、重新布線和刪除

圖3通過簡單的2D圖描述了改進的RRT*中添加新節(jié)點的過程。當且僅當從初始狀態(tài)到當前狀態(tài)的成本降低并且樹中至少有一個沒有子節(jié)點或僅有一個子節(jié)點的節(jié)點，才添加新節(jié)點。要添加新節(jié)點，將從樹中刪除一個不擁有子節(jié)點或僅有一個子節(jié)點的節(jié)點。要是有幾個沒有子節(jié)點的結(jié)點，則任意選擇當中的一個結(jié)點。因為要是節(jié)點沒有子節(jié)點，也就表示其不在通往目標狀態(tài)的路徑上。

第一次執(zhí)行結(jié)點刪除過程是在父節(jié)點優(yōu)化期間。一旦xnear中的節(jié)點是xnear中另一個節(jié)點的唯一子節(jié)點，并且起始點經(jīng)過xnew到這個子節(jié)點的累積路徑成本低于原始路徑，則子節(jié)點將重新連接，作為xnew的子節(jié)點并刪掉父節(jié)點。我們觀察到存在無法添加節(jié)點的情況，因為在xnear中沒有要刪除子節(jié)點的節(jié)點，它可能會降低RRT*的收斂速率。為了緩解這種情況，采用全局節(jié)點移除過程。節(jié)點移除在整個樹中搜索沒有子節(jié)點的結(jié)點，并隨機刪除一個結(jié)點。要是在節(jié)點優(yōu)化和節(jié)點移除中不能找到要被刪掉的節(jié)點，則在樹中刪掉xnew。

2.4 橢圓采樣

之前RRT*算法的采樣是對全部狀態(tài)空間來均勻采樣，從而產(chǎn)生了大而密集的采樣空間。然而，在得到初始路徑再改善到成本最小路徑時，樹到各自樹的根的所用成本大于現(xiàn)在找到成本最小路徑的采樣點，對最優(yōu)解決方案的生成沒有多少幫助。傳統(tǒng)RRT*算法對上述采樣點進行了許多的操作，上述采樣點也使用了一部分的存儲量，同時也影響了算法的效率。

針對RRT*算法的這個不足，文獻[15]提出了一種算法，運用橢圓采樣來降低無效采樣點的數(shù)量。改進RRT*試圖通過直接獲取由n維橢球約束的起點和目標點之間的區(qū)域中的樣本來最小化路徑的成本。用這種方式，改進RRT*僅將解決方案的路徑限制在橢圓體內(nèi)，即啟發(fā)式采樣域，而不是整個自由空間，從而提高收斂速度并增加找到不易找到的路徑的概率。它是RRT*算法的簡單擴展，它不做任何額外的假設(shè)，并且與使用啟發(fā)式偏見的其他算法不同，它不會擴展無法改進解決方案的節(jié)點。但其初始橢圓子集由RRT*全局搜索生成，整體時間花銷大，結(jié)合本文目標偏置和動態(tài)步長延伸可以較好的解決初始路徑花銷大這個問題。

如圖4所示，這是一個橢圓的采樣視圖，cmin是xgoal和xstart之間的真正距離。cmax是到現(xiàn)在為止找到的最佳路徑的成本。當cmax接近真實距離時，橢球會縮小。找到目標后，所有樣本都從這個橢圓形區(qū)域內(nèi)取出。

圖4橢圓采樣視圖

通過從一個單位n維球中第一次采樣xball，從Xf均勻地采樣點xf，然后計算以下過程：

其中C被定義為下面這個式子：

U,是酉矩陣，可以通過奇異值分解（SVD）即U∑VT=M獲得，M定義為：

這里l1T表示單位矩陣的第一列的和：

其中L定義為：

L在這里表示xstart和xgoal之間的實際距離，通常由歐幾里得距離決定。

xcentre是橢圓體的中心，用于平移橢圓體，定義如下：

與原始RRT*的不同之處在于，在找到路徑之后，算法通過只聚焦于開始和目標中的范圍來減少搜索空間，同時逐漸縮小橢圓體以找到最低成本的路徑。由于其集中搜索，該算法對規(guī)劃問題的維度和領(lǐng)域的依賴性較小，并且能夠更快地找到更好的拓撲不同路徑。它還能夠在比RRT*更嚴格的公差范圍內(nèi)找到解決方案，并且計算量相等，并且在沒有障礙物的情況下，可以在有限時間內(nèi)找到機器零點內(nèi)的最優(yōu)解決方案。

圖5無障礙物時橢圓采樣

如圖5所示，起始狀態(tài)和目標狀態(tài)分別顯示為綠色和紅色，相隔100個單位。當前解決方案以紅色突出顯示，橢圓形采樣域顯示為灰色虛線用于說明。隨著解決方案的改進，采樣域逐漸變小，從而可以產(chǎn)生最優(yōu)解的效果。圖（a）顯示了第59回迭代完第一個解，（b）第175回迭代完和（c）第1142回迭代完的最終解，此時橢圓已變化為為起點和目標之間的線。

3 實驗仿真與結(jié)果分析

為了證明所提算法的可行性,將該算法在具有障礙的二維簡單環(huán)境中運行了實驗來證明，再比較了RRT，改進的RRT*[13]，以及MI-RRT*算法的規(guī)劃結(jié)果，仿真所用硬件環(huán)境為Win7系統(tǒng)，Intel Core i5-7500CPU@3.40GHz，8GB 內(nèi)存。

在二維簡單障礙環(huán)境下，RRT算法，改進的RRT*算法和MI-RRT*算法在不同的Map下的實驗。如圖所示，其中圖6-圖8為RRT算法、改進的RRT*算法和MI-RRT*算法的運行情況，算法的迭代次數(shù)從左到右依次增加。圖中的方塊是障礙物，左側(cè)是起點，右側(cè)圓圈是終點，曲線為最終路徑。RRT,改進RRT*和MIRRT*的半徑r都設(shè)為1.5，改進的RRT*最大節(jié)點設(shè)置為N=3000。實驗進行10次，計算其平均值。

通過對三種算法運行情況的比較，本文的所提算法有更高的收斂速率，而且得到的解決方案更優(yōu)，在經(jīng)過多次迭代后，MI-RRT*算法生成的分支和路徑比改進的RRT*算法更優(yōu)。并且能夠看出，在引入限制節(jié)點數(shù)量后，MI-RRT*算法生成的路徑耗費的節(jié)點有了限制。同時，能夠清楚地看到，該算法不但在運用橢圓采樣后，采樣點大量變少，并且也將采樣點收斂到成本最小路徑。

表1列出的是在Map1-Map3路徑規(guī)劃成功之后獲得的數(shù)據(jù)，表中的數(shù)字是算法在每個Map運行10次取平均值。在Map1中，MI-RRT*在相近路徑代價情況下，用更少節(jié)點節(jié)省了80.7%的時間，在Map2中，節(jié)省了44.0%的時間，在Map3中，節(jié)省了78.9%的時間。通過實驗數(shù)據(jù)可以觀測出，RRT*相比RRT可以在搜到更優(yōu)路徑，MI-RRT*相比改進的RRT*在更少的節(jié)點數(shù)的情況下能用更少時間的搜索到更優(yōu)的路徑。

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

為了使對比效果更明顯，實驗選擇了一些障礙，使得起點和終點之間的真正最短路徑存在狹窄的間隙。演示了算法的性能，觀察了算法的收斂性，并對算法分別進行了2200、3000、5000和7000次迭代。最好的路徑是通過中間的狹窄間隙。圖中9-11算法的迭代次數(shù)從左到右依次增加，可以看到，改進的RRT*在7000次迭代時找到了一條穿過窄間隙的路徑，而MI-RRT*在3000次迭代的時候找到了一條穿過窄間隙的更好路徑，并且耗費的結(jié)點更少。RRT從不通過中間的窄間隙向目標發(fā)送路徑。

圖6 Map1下三種算法對比

圖7 Map2下三種算法對比

圖8 Map3下三種算法對比

圖9

4 結(jié)語

本文提出了基于改進的RRT*移動機器人路徑規(guī)劃算法，而且運用仿真實驗驗證了其可行性與優(yōu)勢，得到以下結(jié)論：

（1）改進RRT*算法，初始路徑生成時，采用目標偏置和動態(tài)步長延伸，使得到初始解決方案的速率和質(zhì)量得到提升。

（2）采用橢圓采樣，增進了本文提出的MI-RRT*算法的收斂速率。

（3）進行結(jié)點控制，減少了生成樹所占用的存儲量大小。