改進(jìn)Informed-RRT*的動態(tài)環(huán)境路徑規(guī)劃算法

王楊斌，章 偉，王為科，胡 陟

(上海工程技術(shù)大學(xué)機(jī)械與汽車工程學(xué)院，上海 201000)

0 引言

科技的快速發(fā)展和自動化水平的不斷提高使得機(jī)器人在日常生活中得到了越來越廣泛的應(yīng)用，機(jī)器人路徑規(guī)劃技術(shù)作為機(jī)器人控制的核心技術(shù)也得到了越來越多的關(guān)注。路徑規(guī)劃是機(jī)器人根據(jù)特定的環(huán)境條件與任務(wù)目的找出一條從起點到終點的最優(yōu)路線的技術(shù)。過去的幾十年里，學(xué)者們提出了許多路徑規(guī)劃算法，并在實際應(yīng)用中取得了良好的效果，如人工勢場法[1]、快速探索隨機(jī)樹算法[2]、遺傳算法[3]、A*算法[4]、強(qiáng)化學(xué)習(xí)[5]等。根據(jù)對環(huán)境信息的把握程度可將路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃根據(jù)所有地圖信息進(jìn)行路徑規(guī)劃，局部路徑規(guī)劃根據(jù)局部位置的障礙物分布情況，從當(dāng)前節(jié)點出發(fā)選擇到達(dá)某一點的最優(yōu)路徑。

基于采樣策略的路徑規(guī)劃因其便利性和快捷性而廣受關(guān)注,快速探索隨機(jī)樹(Rapidly-exploring Random Tree，RRT)算法[6]便是其中的典型代表。其通用性強(qiáng)，能夠在不同維度下解決機(jī)器人的空間路徑規(guī)劃問題，具備非常高的探索成功率，但缺陷也十分明顯，一是在復(fù)雜空間搜索速度較慢;二是所得路徑通常不是其最優(yōu)解，且距離最優(yōu)路徑偏差較大;三是在動態(tài)環(huán)境下使用時具有局限性。為解決傳統(tǒng)RRT算法的不足，國內(nèi)外學(xué)者們分別對RRT算法做出了改進(jìn)。王坤等[7]采用雙向搜索策略和自適應(yīng)步長方法改進(jìn)了雙向快速擴(kuò)展隨機(jī)樹(Rapidly Exploring Random Tree Connect,RRT-Connect)算法，能夠在起點和目標(biāo)點同時生成兩棵隨機(jī)樹從而提高了搜索效率;KARAMAN等[8]提出了RRT*算法，當(dāng)?shù)螖?shù)足夠多時，可找到一條最優(yōu)或接近最優(yōu)的路徑，但使得搜索時間大大延長。GAMMELL等[9]提出了Informed快速擴(kuò)展隨機(jī)樹(Informed-RRT*)算法，引入了橢圓約束采樣空間進(jìn)行采樣優(yōu)化，大大縮短了搜索時間，但在動態(tài)環(huán)境下容易與障礙物發(fā)生碰撞。

針對復(fù)雜多變環(huán)境下機(jī)器人的避障問題，提出一種基于Informed-RRT*的改進(jìn)算法，使得機(jī)器人能夠適用于動態(tài)環(huán)境下的避障。首先，通過Informed-RRT*算法在靜態(tài)環(huán)境下進(jìn)行初始全局路徑規(guī)劃，得到一系列路徑分支。使用橢圓空間采樣減少傳統(tǒng)RRT*算法探索的冗余性，加快收斂速度。引入自適應(yīng)變步長策略，當(dāng)父節(jié)點附近的障礙物個數(shù)較多時選擇較小的步長生成新節(jié)點，反之選擇較大的步長生成新節(jié)點。其次，機(jī)器人沿著全局路徑運(yùn)動時，根據(jù)環(huán)境中動態(tài)障礙物的位置判斷得到被障礙物阻斷的路徑分支，刪除不可行路徑并獲取局部路徑規(guī)劃起點和終點。根據(jù)人工勢場法對截斷路徑重規(guī)劃，生成新的局部避障路徑，將局部路徑合并到全局路徑中獲得新的避障路徑。在環(huán)境簡單的局部區(qū)域使用人工勢場法，能夠很好地克服陷入局部極小值的缺點。最后，在仿真環(huán)境下對算法的性能和可行性進(jìn)行驗證。

1 問題描述

本文將機(jī)器人路徑規(guī)劃過程需要的概念具體化，并給出相關(guān)定義。假設(shè)機(jī)器人已知全局環(huán)境信息，并能夠?qū)崟r獲取環(huán)境信息變化。假設(shè)D?Rn,為全局環(huán)境空間，Dobs表示空間中被障礙物所占據(jù)的空間，包括動態(tài)障礙物和靜態(tài)障礙物所占的空間，Dfree表示自由空間，機(jī)器人能夠在此空間自由抵達(dá)任何位置，xstart∈Dfree,表示路徑規(guī)劃起始點，xgoal∈Dfree,表示路徑規(guī)劃目標(biāo)點，σ:(0,n)∈D,表示能夠通過環(huán)境空間將xstart和xgoal連接起來的數(shù)量為n的路徑點，s表示除起點和終點以外的某一個路徑點，這一系列點共同構(gòu)成路徑規(guī)劃的可行路徑。評判路徑的成本函數(shù)表示為c(x),當(dāng)成本函數(shù)最小值時，便可取得最優(yōu)路徑，其值為c*，路徑可表達(dá)為

σ=arg{c(σ)|σ(0)=xstart,σ(n)=xgoal,?s∈[0,n],σ(s)∈Dfree}。

(1)

算法生成的隨機(jī)樹由T=(V,E)表示，V∈Dfree，表示隨機(jī)樹中所有節(jié)點的集合，E∈Dfree，表示所有的分支。

2 基本算法原理

2.1 RRT*算法

RRT*算法具備搜索完備性和漸近優(yōu)化性，很好地解決了路徑規(guī)劃的最優(yōu)解問題，在能找到可行全局路徑的前提下，通過不斷迭代，可以找出近似最優(yōu)路徑。

RRT*算法包括以下幾個關(guān)鍵步驟：

1)根據(jù)Sample函數(shù)在空間Dfree采樣得到隨機(jī)點xrand；

2)根據(jù)Nearest函數(shù)確定隨機(jī)點xrand和最近的節(jié)點xnearest；

3)根據(jù)Steer函數(shù)生成一個自xnearest向xrand一個步長距離的新節(jié)點xnew；

4)根據(jù)Near函數(shù)獲取隨機(jī)樹中一定半徑下節(jié)點集Xnear；

5)根據(jù)Parent函數(shù)計算得到節(jié)點集Xnear中與xnew節(jié)點成本最小點xmin，并作為xnew的父節(jié)點xparent；

6)將xnew并入隨機(jī)樹；

7)不斷重復(fù)上述步驟得到路徑，根據(jù)Rewire函數(shù)對路徑進(jìn)行迭代優(yōu)化，計算路徑成本c，得到最終優(yōu)化路徑。

在多迭代次數(shù)足夠的情況下，基于采樣法的RRT*算法能夠得到近似最優(yōu)路徑，但存在一定的局限性：1)在復(fù)雜空間搜索速度較慢，得到近似最優(yōu)路徑需要較多的迭代次數(shù)；2)在動態(tài)環(huán)境下使用時具有局限性。

2.2 人工勢場法

人工勢場法基本思想是通過環(huán)境建立引力場和斥力場的混合勢場，通過判斷勢場下降方向來搜尋移動機(jī)器人的運(yùn)動路徑。目標(biāo)點機(jī)器人產(chǎn)生引力，障礙物對機(jī)器人產(chǎn)生斥力，所有力場疊加后產(chǎn)生的合力方向決定機(jī)器人的運(yùn)動方向。引力勢場模型為

(2)

由引力勢場函數(shù)求得負(fù)梯度函數(shù)得出引力

Fa(X)=-▽Uatt(X)

(3)

其中：Uatt為引力力場；Ka為引力系數(shù)；Xg表示目標(biāo)點的位置。

斥力場函數(shù)由障礙物和機(jī)器人的距離決定，假設(shè)障礙物位置坐標(biāo)為X。

(4)

式中：Urep為斥力場；η為斥力增益系數(shù)；ρ(X,X0)表示機(jī)器人與障礙物的距離；ρ0表示障礙物的最大影響范圍，當(dāng)機(jī)器人離障礙物的距離大于ρ0時，則障礙物對機(jī)器人沒有影響。

(5)

人工勢場對機(jī)器人所產(chǎn)生的力可表示為

F(X)=Fatt(X)+∑Frep(Xi)

(6)

式中:Fatt為目標(biāo)點對機(jī)器人所產(chǎn)生的引力;Frep為障礙物對機(jī)器人的斥力。兩者共同組成合力F控制移動機(jī)器人運(yùn)動方向。但是，在一些特殊情況，人工勢場存在陷入局部極小值、目標(biāo)不可達(dá)的問題。

3 路徑規(guī)劃算法性能分析

在全局路徑規(guī)劃中，本文采用Informed-RRT*算法進(jìn)行全局路徑規(guī)劃。相比RRT*算法，其優(yōu)勢在于橢圓采樣策略[10]，將采樣空間限制在一個橢圓空間內(nèi)部，提升了全局路徑搜索效率，但在生成初始路徑時，依然缺乏應(yīng)對復(fù)雜環(huán)境的能力。引入自適應(yīng)變步長策略，根據(jù)父節(jié)點附近的障礙物個數(shù)選取合適步長生成新節(jié)點xnew，克服路徑迭代次數(shù)過多的缺點，提升搜索效率。另外，在實際情況下，Informed-RRT*算法規(guī)劃的路徑只考慮靜態(tài)障礙物的影響，動態(tài)障礙物可能會出現(xiàn)在路徑上阻礙機(jī)器人運(yùn)動的情況，為保證機(jī)器人能夠順利抵達(dá)目標(biāo)點，需要對路徑進(jìn)行局部動態(tài)規(guī)劃。本文針對Informed-RRT*算法在動態(tài)環(huán)境中的不適用性問題，引入路徑重規(guī)劃節(jié)點選擇策略，使用人工勢場法解決了與動態(tài)障礙物碰撞的問題。

3.1 橢圓空間采樣策略

Informed-RRT*算法的優(yōu)勢在于：當(dāng)RRT*算法探索出一條路徑后將采樣空間限制在一個橢圓空間內(nèi)部，并以起始點和目標(biāo)點為焦點。根據(jù)橢圓的性質(zhì)可知，橢圓內(nèi)的點與兩焦點的距離長度之和小于橢圓上的點與兩焦點的距離長度之和。在橢圓內(nèi)，迭代采樣后新生成的路徑長度小于原先的路徑長度，從而不斷優(yōu)化。隨著迭代次數(shù)不斷增加，橢圓空間不斷縮小，路徑長度也不斷縮短，最終得到最優(yōu)路徑。橢圓空間采樣策略在復(fù)雜環(huán)境或者高維空間的情況下使用，效率提升更加顯著，能夠以相對更少迭代次數(shù)發(fā)現(xiàn)最優(yōu)的路徑。

Informed-RRT*算法的核心為其橢圓空間采樣策略，可以通過對單位圓xball～U(Xball)中均勻分布的樣本進(jìn)行采樣。通過矩陣變換、旋轉(zhuǎn)、平移得到橢圓采樣樣本子集xellipse～U(Xellipse)。以xstart和xgoal作為橢圓焦點，表達(dá)式為

xellipse=Lxball+xcentre

(7)

(8)

3.2 自適應(yīng)步長策略

Informed-RRT*算法根據(jù)現(xiàn)實環(huán)境情況對整個空間進(jìn)行規(guī)劃，構(gòu)建從起點xstart到終點xgoal的可行路徑樹。通過迭代生成的隨機(jī)節(jié)點xrand，找到距離隨機(jī)樹最近的節(jié)點xnearest，以固定步長S向xnearest，xrand方向進(jìn)行生長，生成新節(jié)點xnew。新節(jié)點xnew為

(9)

現(xiàn)實空間環(huán)境種類具有多樣性，而步長在一定程度上決定著探索效率，設(shè)置過短，在障礙物較少的空曠地區(qū)搜索冗余度過大;設(shè)置過長，在障礙物較多的密集地區(qū)探索效率低下。針對這一問題，對Informed-RRT*算法進(jìn)行改進(jìn)，提出自適應(yīng)步長策略，該策略以機(jī)器人附近障礙物數(shù)量來確定步長S。設(shè)立障礙物數(shù)量參數(shù)q和閾值p，表示機(jī)器人附近障礙物數(shù)量。當(dāng)障礙物數(shù)量q等于零時，按照2倍初始步長S0生成新節(jié)點，當(dāng)障礙物數(shù)量q小于或等于閾值p時，按照初始步長S0生成新節(jié)點，當(dāng)障礙物數(shù)量q大于閾值p時，按照0.5倍初始步長S0生成新節(jié)點，即

(10)

自適應(yīng)步長策略能夠根據(jù)不同的環(huán)境更好地擴(kuò)展新節(jié)點，減少迭代次數(shù)，增強(qiáng)算法的探索能力。

3.3 路徑重規(guī)劃節(jié)點選擇策略

改進(jìn)的Informed-RRT*算法通過生成一棵快速隨機(jī)樹來探索整個空間，生成的最優(yōu)路徑由一系列點組成的樹枝構(gòu)成。當(dāng)動態(tài)障礙物出現(xiàn)在機(jī)器人行進(jìn)的路徑上時，阻止機(jī)器人繼續(xù)運(yùn)動。針對該問題，引入路徑重規(guī)劃策略，對全局路徑進(jìn)行裁剪，刪除受動態(tài)障礙物影響的路徑，進(jìn)行局部路徑規(guī)劃，選取局部路徑規(guī)劃的起點和終點。節(jié)點選擇策略如圖1所示。

圖1 節(jié)點選擇策略Fig.1 Node selection strategy

若機(jī)器人檢測到只有一段路徑分支被障礙物阻斷，選擇節(jié)點策略如圖1(a)所示，機(jī)器人運(yùn)動到V(i+1)時檢測到有動態(tài)障礙物在E(i+1)路徑分支上，刪除E(i+1)分支，并將V(i+1)和V(i+2)分別設(shè)立為局部路徑規(guī)劃的起點和終點。若機(jī)器人檢測到只有多段路徑分支被阻斷，選擇節(jié)點策略如圖1(b)所示，機(jī)器人運(yùn)動到V(i+1)時檢測到有動態(tài)障礙物在E(i+1)和E(i+2)路徑分支上，刪除E(i+1)和E(i+2)分支，并將V(i+1)和V(i+3)分別設(shè)立為局部路徑規(guī)劃的起點和終點。

3.4 局部動態(tài)避障方法

人工勢場法具有良好的實時性和動態(tài)性，在動態(tài)環(huán)境避障方面表現(xiàn)出優(yōu)越的性能,缺點為在相對復(fù)雜的環(huán)境下可能會存在陷入極小值和目標(biāo)不可達(dá)的問題。根據(jù)路徑重規(guī)劃節(jié)點選擇策略選擇出局部路徑重規(guī)劃的起點xstart和終點xgoal，限定了局部路徑規(guī)劃的空間，只對受障礙物影響的路徑進(jìn)行重規(guī)劃，僅考慮動態(tài)障礙物以及附近的靜態(tài)障礙物的影響，很大程度地簡化了空間環(huán)境，降低了陷入極小值的風(fēng)險。通過多次仿真對比效果，設(shè)定合理的引力系數(shù)Ka和斥力增益系數(shù)η，能較好地解決因勢場設(shè)定不合理引起的目標(biāo)不可達(dá)的問題。

人工勢場偽代碼如下。

V←{xstart};E←?;T=(V,E)

xnow←xstart

foriters

iflthreashold

xparent←xnow;

Fvec←Fatt+∑Frep;

xnow←xnow+δFvec·step

iters←iters+1;

V← ∪{xnow};

E←E∪{(xnow,xparent)};

V←∪{xnow};

E←E∪{(xnow,xgoal)};

returnT。

4 仿真驗證

使用200×200 單位長度為m的二維地圖，驗證所提出的算法在全局路徑規(guī)劃中的有效性。地圖尺寸橫坐標(biāo)范圍為(-100 m,100 m)，縱坐標(biāo)范圍為(-100 m,100 m)，xstart坐標(biāo)為(-80 m,-80 m)，xgoal坐標(biāo)為(50 m,50 m)，為驗證算法的性能，將RRT*算法、Informed-RRT*算法與改進(jìn)的Informed-RRT*算法進(jìn)行比較，3種算法的路徑規(guī)劃結(jié)果如圖2所示。圖中，圓形為障礙物，綠色路徑為算法生成的隨機(jī)樹，紅色路徑為最終生成的路徑，黑色三角形為最后的采樣點。

圖2 3種算法路徑規(guī)劃圖Fig.2 Planning paths of the three algorithms

由圖2(b)可知，Informed-RRT*算法通過橢圓采樣能夠限制住采樣范圍。由圖2(c)可知，在自適應(yīng)步長改進(jìn)的Informed-RRT*算法的路徑規(guī)劃中，探索樹在左下角較為空曠的環(huán)境中步長較大，在中間障礙物較為密集的環(huán)境中步長較短。相比Informed-RRT*算法，改進(jìn)算法在較為空曠的環(huán)境中探索范圍更大。

通過對上述3種算法進(jìn)行多次仿真，結(jié)果如圖3所示。

圖3 3種算法效果對比圖Fig.3 Comparison chart of the three algorithms

由圖3可知，通過自適應(yīng)步長改進(jìn)的Informed-RRT*算法在降低迭代次數(shù)方面有著較好的效果。經(jīng)過20次實驗后，RRT*算法的平均迭代次數(shù)為325，Informed-RRT*算法的平均迭代次數(shù)為352，改進(jìn)的Informed-RRT*算法的平均迭代次數(shù)為246。改進(jìn)算法迭代次數(shù)相對于原有算法平均迭代次數(shù)優(yōu)化了30%左右。改進(jìn)的Informed-RRT*算法有效地提高了收斂速度和采樣效率。

改進(jìn)算法動態(tài)避障仿真如圖4所示。路徑成本函數(shù)值為202.52 m。動態(tài)障礙物由(x,y,z)表示，(x,y)為坐標(biāo)，z為障礙物半徑，3個坐標(biāo)分別為(40 m,10 m,6 m),(-10 m,-20 m,4 m),(-50 m,-50 m,6 m)。根據(jù)線段檢測可知障礙物影響路徑分支為:l1=[(39.05,38.93),(35.17,23.99)],l2=[(-23.24,-26.12),(-3.80,-15.9)],l3=[(-56.08,-49.80),(-39.52,-33.89)],單位為m。對以上3處路徑分支重規(guī)劃，生成局部路徑(圖4(b)中藍(lán)色路徑)。將原有被障礙物阻斷的路徑去除，替換為局部路徑，生成新的全局路徑(圖4(c))。計算新的路徑成本函數(shù)值為220.35 m，路徑長度增加約8%。

圖4 改進(jìn)算法動態(tài)避障圖Fig.4 Dynamic obstacle avoidance of the improved algorithm

綜上所述，本文提出的自適應(yīng)變步長策略對Informed-RRT*算法的全局路徑規(guī)劃效率有一定的提升，在20次仿真中，相較于Informed-RRT*算法，改進(jìn)算法獲得初始路徑的迭代次數(shù)少了30%左右。通過與人工勢場法結(jié)合的改進(jìn)算法，利用橢圓約束采樣和自適應(yīng)步長策略提高了路徑收斂速度,并獲得成本較低的全局路徑，在動態(tài)變化的環(huán)境中，能夠?qū)崟r地根據(jù)障礙物的位置進(jìn)行局部路徑規(guī)劃，實現(xiàn)了避障的目的，具有良好的實用性。

5 結(jié)束語

路徑規(guī)劃是當(dāng)前機(jī)器人在復(fù)雜環(huán)境下運(yùn)動的關(guān)鍵技術(shù)，Informed-RRT*算法作為一種采樣法能夠在靜態(tài)環(huán)境下快速、有效地獲取機(jī)器人的全局路徑。針對機(jī)器人在運(yùn)動過程中遇到環(huán)境復(fù)雜程度和動態(tài)障礙物問題，提出一種自適應(yīng)步長的改進(jìn)Informed-RRT*和人工勢場法的改進(jìn)算法，引入實驗將改進(jìn)的算法應(yīng)用于機(jī)器人路徑規(guī)劃，并通過實驗和經(jīng)驗方法確定參數(shù)。仿真結(jié)果表明,改進(jìn)算法有效地減少了全局路徑規(guī)劃的迭代次數(shù)，提高了收斂速度，獲得近似最優(yōu)路徑，并能夠在動態(tài)環(huán)境中實現(xiàn)避障和路徑的有效重規(guī)劃，驗證了算法的適用性。