陳彥杰，梁景林，張智星，喻 驍，王耀南

(1.福州大學(xué)機(jī)械工程及自動化學(xué)院，福建福州350108；2.廈門大學(xué)航空航天學(xué)院，福建廈門361102；3.湖南大學(xué)電氣與信息工程學(xué)院，湖南長沙410082；4.機(jī)器人視覺感知與控制技術(shù)國家工程研究中心，湖南長沙410082)

現(xiàn)如今，移動機(jī)器人已廣泛應(yīng)用于生活中的不同領(lǐng)域，例如物流運(yùn)輸[1]、服務(wù)機(jī)器人[2]、場所消毒消殺[3]等.路徑規(guī)劃是移動機(jī)器人進(jìn)行任務(wù)作業(yè)的關(guān)鍵環(huán)節(jié)，其主要作用是為機(jī)器人提供一條從起始點(diǎn)到目標(biāo)點(diǎn)的連續(xù)路徑.該路徑在保證安全無碰撞的前提下，還能滿足任務(wù)所約束的一些條件.目前，普遍使用的規(guī)劃方法包括神經(jīng)網(wǎng)絡(luò)法、蟻群算法、勢場法和隨機(jī)采樣法等[4-7].

基于采樣的規(guī)劃方法由于能夠給機(jī)器人快速地提供可行路徑，因此得到了廣泛的關(guān)注.快速探索隨機(jī)樹(rapidly-exploring random tree，RRT)[8]通過在搜索空間中持續(xù)采樣單個狀態(tài)從而增量地擴(kuò)展到目標(biāo)狀態(tài)，得到可供機(jī)器人行駛的無碰撞路徑.Klemm等[9]提出了RRT-connect，分別從起始點(diǎn)和目標(biāo)點(diǎn)擴(kuò)展雙向的搜索樹，能夠快速地找到一條可行路徑.RRT方法雖然計算效率較高但所獲得的路徑通常并非最優(yōu)，使機(jī)器人需要較多時間才能到達(dá)目標(biāo).針對這一問題，Karaman等[10]在RRT方法的基礎(chǔ)上加入了重布線過程(rewire)和路徑代價(cost)函數(shù)，對已有搜索樹中的節(jié)點(diǎn)和節(jié)點(diǎn)間連接進(jìn)行選擇改進(jìn)，使其擁有漸進(jìn)最優(yōu)的性能，得到了RRT*方法.由于RRT*方法在搜索階段覆蓋擴(kuò)展了整個空間，造成探索效率的不佳.因此，知情RRT*(informed RRT*)方法[11]設(shè)計了知情集(informed set)，用于在找到初始路徑后將采樣范圍限制至一橢圓區(qū)域，從而減小搜索范圍，使得路徑能夠更快地收斂到最優(yōu)解.快速行進(jìn)樹(fast matching tree，F(xiàn)MT*)方法[12]綜合了概率路線圖[13]和RRT，利用一批采樣點(diǎn)進(jìn)行樹狀擴(kuò)展，但FMT*容易出現(xiàn)冗余探索，從而導(dǎo)致路徑搜索性能較低.吳錚等[14]提出了一種基于方向選擇的啟發(fā)式函數(shù)，評估樣本的代價梯度，調(diào)整樣本排序，引導(dǎo)FMT*的擴(kuò)展.基于安全通道的FMT*(ST-FMT*)[15]在方法擴(kuò)展前增加了預(yù)處理生成初始路徑，而后建立安全通道進(jìn)行采樣加速算法收斂.

批處理知情搜索樹(batch informed trees，BIT*)[16]結(jié)合了Informed RRT*和FMT*的部分特點(diǎn)，通過多批次采樣進(jìn)行探索樹的擴(kuò)展.BIT*的采樣在知情集所確定的橢圓區(qū)域進(jìn)行，其采樣點(diǎn)的擴(kuò)展和連接順序由各點(diǎn)的代價估計值排列.Liu等[17]提出一種貪婪搜索策略對邊的連接順序進(jìn)行優(yōu)先處理，能更快找到初始解.Strub等[18]通過使用非對稱雙向搜索來估計適用于不同問題的啟發(fā)式，有效提高了初始路徑尋找效率.這些方法都加快了BIT*對于初始路徑的獲取，但初始路徑并非都是趨向于最優(yōu)路徑所在區(qū)域，并且對后續(xù)路徑的改進(jìn)有所欠缺.

因此，為了提高BIT*的規(guī)劃效率并加快路徑代價降低速度，本文提出了一種基于偏置采樣與包圍優(yōu)化的BIT*(wrapping-based biased BIT*,WB-BIT*)方法.該方法包括了兩個策略：1) 路徑包圍優(yōu)化，對當(dāng)前搜索所得的現(xiàn)有路徑進(jìn)行處理，使其包圍至障礙物附近，以此降低路徑代價，進(jìn)而減小知情集區(qū)域；2) 路徑指導(dǎo)偏置采樣，利用當(dāng)前路徑點(diǎn)的信息，生成偏置采樣區(qū)域加速更優(yōu)路徑的搜尋和路徑代價下降速度.最后，本文通過仿真實(shí)驗(yàn)的實(shí)現(xiàn)和結(jié)果的對比分析，驗(yàn)證了WB-BIT*方法的有效性和高效性.

1 WB-BIT*方法

為解決BIT*方法中存在因路徑代價下降速度慢導(dǎo)致規(guī)劃時間長、效率不佳的問題，本文提出了WB-BIT*方法.

1.1 函數(shù)定義

路徑規(guī)劃是指在地圖中尋找到一條安全且可行的連接起始點(diǎn)和目標(biāo)點(diǎn)的路徑.對于移動機(jī)器人，其路徑代價可等同于移動的距離，因此在本文后續(xù)章節(jié)中使用路徑長度代表路徑代價.定義X∈Rn表示為移動機(jī)器人的狀態(tài)空間，n為搜索空間的維度.BIT*方法在狀態(tài)空間里呈樹狀增量搜索，采樣點(diǎn)連接至搜索樹后成為節(jié)點(diǎn)v，兩節(jié)點(diǎn)間的連接稱為搜索樹的邊.搜索樹T包含了節(jié)點(diǎn)集合V和邊集合E，即T=(V,E).定義搜索樹T中起始點(diǎn)xstart到目標(biāo)xgoal的最優(yōu)路徑長度為cbest,當(dāng)前路徑長度為ci,未找到可行路徑時ci=∞.

1.2 WB-BIT*方法主要模塊

WB-BIT*方法的整體結(jié)構(gòu)框架具體如算法1所示，其中包含了以下幾個主要模塊：

1) 批量均勻和偏置采樣.WB-BIT*方法首先在整個搜索空間中隨機(jī)均勻采樣；當(dāng)獲得初始路徑后，以目標(biāo)點(diǎn)和起始點(diǎn)為焦點(diǎn)，初始路徑長度作為長軸長度構(gòu)建的橢圓區(qū)域稱為知情集，如圖1所示；當(dāng)知情集確定后，采樣則在此橢圓區(qū)域中進(jìn)行，該區(qū)域會隨當(dāng)前最優(yōu)路徑長度的減少而逐步縮小.本文設(shè)計了基于路徑指導(dǎo)的偏置采樣，與當(dāng)前橢圓區(qū)域的均勻采樣相結(jié)合，以此減少原始橢圓區(qū)域中可能存在的冗余樣本，同時有效利用路徑信息，提高探索路徑的效率，即算法1中第7行.

圖1 知情集采樣區(qū)域Fig.1Informed set sampling region

rBIT*≥

(1)

3) 節(jié)點(diǎn)與邊的擴(kuò)展和路徑優(yōu)化.選擇的邊(vmin,xmin)在通過啟發(fā)式和碰撞檢測的判斷后，才會進(jìn)行實(shí)際擴(kuò)展，連接并加入搜索樹中(算法1第13～23行).此時若點(diǎn)xmin已存在于搜索樹中，則認(rèn)為是樹的重布線過程，否則該點(diǎn)加入節(jié)點(diǎn)集合V，并判斷其是否能連接至目標(biāo)點(diǎn)，若能連接，即可輸出一條可行路徑及其長度.本文設(shè)計了一種路徑包圍優(yōu)化策略，每當(dāng)路徑長度發(fā)生變化時能夠利用該策略將當(dāng)前路徑快速包圍至障礙物周邊，使路徑長度快速減少，有效縮小知情集的橢圓區(qū)域，從而提高搜索精度(算法1第20行).

算法1WB-BIT*

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

2：Xunconnected←xgoal

3：QV←V;QE←?

4： repeat

5： ifQE=? andQV=? then

6：Xreuse←Prune(T,Xunconnected,ci)

7：Xsampling←Hybrid_Sample(m,xstart,xgoal,ci,Xpath)

8：Xunconnected←Xreuse∪Xsampling

9：QV←V

10： While Best_Value(QV)≤Best_Value(QE) do

11： Expand(QV,QE,ci)

12： (vmin,xmin)←Pop_Best(QE)

15：cedge←Collision_checking(vmin,xmin)

22： else

23：QV←?;QE←?

24： until STOP

25：returnT

1.3 路徑包圍優(yōu)化

在路徑規(guī)劃過程中，知情集是通過當(dāng)前最優(yōu)路徑長度決定采樣區(qū)域的大小，而更小的采樣區(qū)域能夠提供更精細(xì)的搜索.因此，為了通過減小采樣區(qū)域?qū)崿F(xiàn)更加精細(xì)的搜索和更短路徑的獲取，本文設(shè)計了一種基于當(dāng)前路徑包圍障礙物的路徑優(yōu)化策略.該優(yōu)化策略主要流程如算法2的偽代碼所示.

算法2Path_Optimization(Xpath,r)

1：i←1

2：Pnum←|Xpath|

3： Whilei<(Pnum-2)

4：d←Max(‖xi-xi+1‖2,‖xi+1-xi+2‖2)

5：U=round(10·d/r)

6：d=(xi-xi+2)/U

7： if LocalPath(xi,xi+2)∈Xfreethen

8：Xpath←Xpathxi+1

9：i=i-1

10： break

11： else

12： forj=1 to (U-1) do

13：xtemp=xi+(xi+1-xi)·j/U

14： if LocalPath(xi,xtemp,xi+2)∈Xfreethen

15：xi+1=xtemp

16： break

17： else

18： fork=1 to (U-j) do

19：xnew=xtemp+δ·j

20： if LocalPath(xi,xnew,xi+2)∈Xfreethen

21：xi+1=xnew

22： break

23：i=i+1

24： returnXpath

算法2的路徑優(yōu)化策略在路徑長度有更新時執(zhí)行.策略開始執(zhí)行后，當(dāng)前路徑根據(jù)自定義變量被離散為均勻配置，由目標(biāo)點(diǎn)至起始點(diǎn)將路徑包圍至障礙物周圍.算法2偽代碼中的LocalPath()函數(shù)表示節(jié)點(diǎn)之間連線的碰撞檢測，|Xpath|表示路徑點(diǎn)集合的基數(shù)，若路徑中包含N個點(diǎn)，則Xpath={x1,x2,…,xN}.此外，算法2中U是路徑連接的離散化程度，根據(jù)當(dāng)前連接半徑與自定義變量的比例來確定，能在路徑點(diǎn)連接大于連接半徑時更精細(xì)地進(jìn)行路徑優(yōu)化.

圖2展示了一個簡單的路徑優(yōu)化示例，圖2(a)黑色實(shí)線為原始路徑.首先擬連接xgoal和x1(藍(lán)色虛線)，當(dāng)此連接發(fā)生碰撞時，通過均勻離散化擬連接直到x′2與x1和xgoal的連接均在可行區(qū)域(圖2(b))，此時x′2替換x2作為新的連接，如圖2(c)紅色實(shí)線所示.然后重復(fù)類似過程得到x′1,最后得到圖2(d)所示的新路徑.顯然，由于三角不等式中第三邊小于其余兩邊之和，優(yōu)化后的路徑具有更短的長度.

圖2 路徑包圍優(yōu)化過程Fig.2Wrapping process of path optimization

1.4 基于路徑指導(dǎo)的偏置采樣

路徑優(yōu)化策略能夠加速當(dāng)前路徑長度的減少，獲得一條高質(zhì)量路徑.當(dāng)環(huán)境中存在多條路徑時，則需要保持對潛在更優(yōu)新路徑的探索.因此，本文設(shè)計了一種采樣策略，利用當(dāng)前路徑點(diǎn)相關(guān)的啟發(fā)式函數(shù)值計算搜索區(qū)域，執(zhí)行偏置采樣.

圖3 知情集采樣與偏置采樣Fig.3Informed sampling and biased sampling

WB-BIT*方法探索初期未找到路徑時，路徑長度視為無窮大，采樣在整個搜索空間中隨機(jī)均勻進(jìn)行.當(dāng)獲得一條可行路徑時，偏置采樣策略才會執(zhí)行.可行路徑是由各路徑點(diǎn)連接而成，可表示為Xpath={xstart,x1,x2,…,xgoal},策略首先計算各路徑點(diǎn)的啟發(fā)式值：

‖xgoal-x‖2,(x∈Xpath),

(2)

(3)

其次取各點(diǎn)啟發(fā)式值中最大值H(xmax)作為偏置采樣的參數(shù)，使用H(xmax)作為橢圓長軸的長度，起始點(diǎn)xstart和目標(biāo)點(diǎn)xgoal作為焦點(diǎn)建立一個橢圓的偏置采樣區(qū)域，如圖3所示.所建立的偏置采樣區(qū)域?qū)儆谥榧淖蛹?，在路徑?shù)量不止一條的情況下，從這個較小子集中進(jìn)行采樣，更有可能找到改善路徑和加快算法收斂的采樣點(diǎn).然而，由于該采樣區(qū)域是通過現(xiàn)有路徑進(jìn)行估計的，所含信息不如知情集充足，可能得到的是局部最優(yōu)路徑[19].因此，為了確保WB-BIT*方法的采樣均勻性而保證漸進(jìn)最優(yōu)性，偏置采樣需要與知情集采樣結(jié)合，并引入偏置比α∈(0,1)來平衡這一采樣過程(如算法3第7行).當(dāng)已有可行路徑時，在偏置區(qū)域生成樣本的概率為α,在知情集中進(jìn)行采樣的概率則為1-α.偏置采樣的偽代碼如算法3所示.

算法3Hybrid_Sample(m,xstart,xgoal,ci,Xpath)

1：Xsampling←?

2： repeat

3： ifci<∞ then

4： forx∈Xpathdo

7： ifα>rand() then

8：Xsampling←Sample_Informed(xstart,xgoal,ci)

9： else

10：Xsampling←Sample_Bias(xstart,xgoal,H(xmax))

11： else

12：Xsampling←Uniform_Sample(X)

13： until |Xsampling|=m

14： returnXsampling

1.5 理論分析

本小節(jié)對WB-BIT*方法進(jìn)行理論分析.

定理1概率完備性.對于待解決路徑規(guī)劃問題，若該問題存在解，則當(dāng)方法的迭代次數(shù)或搜索時間趨于無窮大時，獲得一條從起點(diǎn)到終點(diǎn)的可行路徑解的概率為1，即：

其中:q是采樣點(diǎn)的數(shù)量，σq是從這些采樣點(diǎn)中找到的路徑，Σ是所有可行路徑的集合.

證明WB-BIT*是基于BIT*方法的改進(jìn)，通過節(jié)點(diǎn)擴(kuò)展和連接增量地生成連續(xù)的樹.該方法在進(jìn)行規(guī)劃過程中，起始點(diǎn)xstart和目標(biāo)點(diǎn)xgoal在搜索空間中位置是已知的.在未有可行解存在的時候，該方法不斷地在整個空間中批量均勻采樣，并通過啟發(fā)式的估計值遞增地從xstart連接各采樣點(diǎn).當(dāng)規(guī)劃問題存在解時，由于批量采樣均勻地逐漸覆蓋整個搜索空間，最終xstart將通過采樣點(diǎn)無碰撞地連接至xgoal,因此找到一條可行路徑解的概率為1.基于上述論點(diǎn)，WB-BIT*具備概率完備性.

定理2漸進(jìn)最優(yōu)性.當(dāng)采樣至無窮個樣本時，WB-BIT*方法漸進(jìn)收斂到給定路徑規(guī)劃問題的最優(yōu)解的概率是1，即：

其中:q是采樣點(diǎn)的數(shù)量，σq是從這些采樣點(diǎn)中找到的路徑，c(σ*)代表理論最優(yōu)路徑長度.

證明對于一個采樣序列Xsamples={x1,x2,…,xq},WB-BIT*考慮了至少和RRT*相同的邊和連接半徑.RRT*從采樣序列中的某個樣本增量地構(gòu)建一棵搜索樹.該序列中的每個采樣點(diǎn)xk∈Xsamples考慮了連接半徑內(nèi)的所有鄰點(diǎn)：

Xnear,k={xj∈Xsamples|j

rRRT*}.

圖4 仿真實(shí)驗(yàn)環(huán)境Fig.4Experiment environments

采樣點(diǎn)xk從這些鄰點(diǎn)中選擇能夠使得xk的當(dāng)前實(shí)際gT(xk)最小化的狀態(tài)進(jìn)行連接，接著經(jīng)重布線考慮其余鄰點(diǎn)能否通過連接xk使各自當(dāng)前gT(xother)降低.

給定相同的采樣序列，WB-BIT*將采樣序列分為批量樣本，Xsamples={Y1,Y2,…,Yl}.其中每一批樣本是q個采樣點(diǎn)的集合，例如Y1={x1,x2,…,xq}.WB-BIT*通過處理該采樣序列中每一批樣本，遞增地構(gòu)建搜索樹.對于集合y∈Yk中的每個采樣點(diǎn)，均同時考慮了整個采樣序列中處于連接半徑內(nèi)的所有鄰點(diǎn)：

Xnear,k={x∈Yj|j

這些采樣點(diǎn)通過與鄰點(diǎn)的連接，將邊添加到搜索樹，使得當(dāng)前實(shí)際gT最小化，并考慮連接范圍內(nèi)其余邊連接至它的鄰域.這組邊的集合包含了RRT*在其連接范圍內(nèi)所考慮的所有邊.給定RRT*連接半徑：

(4)

結(jié)合式(1)WB-BIT*方法的連接半徑，由于WB-BIT*在任一批次樣本中的連接半徑與RRT*方法在該批次中第一個樣本的連接半徑相同，且兩者的連接半徑均單調(diào)遞減，說明了WB-BIT*至少考慮和RRT*相同的邊，同時WB-BIT*從這些邊中選擇能夠降低搜索樹中節(jié)點(diǎn)代價，并能夠提供更好的路徑連接.因此結(jié)合以上說明和Karaman等[10]對于RRT*漸進(jìn)最優(yōu)性能的論述，WB-BIT*也是具有漸進(jìn)最優(yōu)的性能.

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

2.1 實(shí)驗(yàn)環(huán)境設(shè)置

仿真實(shí)驗(yàn)在MATLAB R2018b軟件中進(jìn)行，計算機(jī)平臺為Windows 10操作系統(tǒng)，i7-7700HQ處理器和16 GB運(yùn)行內(nèi)存.為了驗(yàn)證WB-BIT*的有效性和高效性，將其分別與BIT*、FMT*、Informed RRT*方法在不同場景下進(jìn)行仿真對比.

仿真對比在3個不同場景下進(jìn)行，Map 1～3均為靜態(tài)地圖，如圖4所示.其中S代表起始點(diǎn)xstart,G代表目標(biāo)點(diǎn)xgoal.Map 1為起始點(diǎn)和目標(biāo)點(diǎn)均有環(huán)繞障礙物的場景；Map 2為多條可行路徑地圖，其中一條為最優(yōu)路徑所在區(qū)域；Map 3為復(fù)雜場景，存在許多障礙物.3個場景的理論最優(yōu)路徑長度c*分別為320,378,372 m，當(dāng)方法運(yùn)行到路徑收斂效果(當(dāng)前路徑長度/理論最優(yōu)路徑長度)小于1.05時終止，此時視作完成規(guī)劃，并同時記錄樣本數(shù)量、搜索時間、路徑長度等數(shù)據(jù).所有場景地圖中每個方法均進(jìn)行30次實(shí)驗(yàn)，并使用30次實(shí)驗(yàn)的數(shù)據(jù)平均值作為最終結(jié)果，WB-BIT*的偏置比α設(shè)置為25%.

2.2 實(shí)驗(yàn)結(jié)果與分析

圖5分別為在樣本數(shù)為500(Map 1)、500(Map 2)、1 000(Map 3)時各規(guī)劃方法的結(jié)果.紅色連線為當(dāng)前路徑，藍(lán)色橢圓邊界線為知情集區(qū)域.從圖中可以看出，各方法都能夠找到一條可行路徑，但所得路徑的長度存在差異.當(dāng)WB-BIT*找到一條路徑時，通過路徑包圍優(yōu)化策略能夠使此路徑靠近至障礙物邊緣，快速地減少路徑長度.同時偏置采樣策略能夠?qū)罄m(xù)采樣和路徑的進(jìn)一步改善起到有效作用，尤其是處于Map 2和Map 3這種存在多條可行路徑的場景中.在樣本數(shù)量的限制下，本文所提出的WB-BIT*能夠獲得比其他對比方法更優(yōu)的路徑.

圖5 不同場景地圖中各方法的規(guī)劃結(jié)果Fig.5Planning results of each algorithms in different maps

此外，將方法運(yùn)行終止時所記錄的數(shù)據(jù)，即收斂效果、路徑長度、搜索時間和樣本數(shù)量作為性能指標(biāo)進(jìn)行對比分析，如表1所示.從表1的數(shù)據(jù)中可知，當(dāng)各規(guī)劃方法收斂效果均小于1.05時，WB-BIT*在3個不同地圖中獲得趨近于理論最優(yōu)路徑的所需樣本數(shù)量和消耗時間較少.在Map 1中，環(huán)繞障礙物造成知情集橢圓區(qū)域過大，BIT*和Informed RRT*都存在冗余探索的問題，需要通過更多樣本數(shù)量才能更新到較優(yōu)路徑.WB-BIT*的路徑包圍優(yōu)化策略能夠使路徑靠近至障礙物周圍，并利用偏置采樣進(jìn)行路徑的完善，最終通過較少的樣本數(shù)量趨近于理論最優(yōu)路徑.在Map 2和Map 3中均存在多條可行路徑，WB-BIT*的策略使其能夠?qū)⑺阉鞯降穆窂介L度快速降低以減小知情集區(qū)域，同時偏置采樣的存在能夠進(jìn)行潛在更優(yōu)路徑的搜索.最終結(jié)果WB-BIT*都優(yōu)于對比的BIT*、FMT*、Informed RRT*方法.

為了進(jìn)一步分析方法的規(guī)劃效率，在圖6中給出了不同地圖下樣本數(shù)量與路徑長度的關(guān)系圖.WB-BIT*在所有地圖中樣本數(shù)量較少的情況下，路徑長度均具有較快的減少速度.以Map 1為例，WB-BIT*方法使用300個樣本，得到了距離理論最優(yōu)解1%內(nèi)的路徑長度，所需時間3.316 s(表1)，其他幾個方法則需要更多的樣本和時間才能達(dá)到相近的收斂效果.

表1 不同地圖仿真實(shí)驗(yàn)結(jié)果

綜上所述，仿真結(jié)果驗(yàn)證了WB-BIT*在不同環(huán)境下能夠高效地完成路徑規(guī)劃，且相較于同類規(guī)劃方法具有較快的路徑長度減少速度和良好的尋路效率.

圖6 不同規(guī)劃方法的路徑長度與樣本數(shù)量關(guān)系圖Fig.6Relationship between path cost and sampling numbers of different planning algorithms

3 結(jié) 論

為了解決BIT*方法存在的路徑代價降低速度慢和探索效率不佳的問題，本文提出了WB-BIT*方法.路徑包圍優(yōu)化能夠在找到可行路徑后，使路徑包圍至障礙物周邊，達(dá)到快速縮短路徑長度的目的.通過路徑中節(jié)點(diǎn)的啟發(fā)式值指導(dǎo)偏置采樣區(qū)域的建立，協(xié)助潛在更優(yōu)路徑的尋找.方法的仿真結(jié)果也驗(yàn)證了WB-BIT*的有效性和高效性.未來的工作是將WB-BIT*拓展至動態(tài)環(huán)境，提高所用策略的環(huán)境適用性.