基于偏置采樣和包圍優化的移動機器人路徑規劃方法

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

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

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

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

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

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

1 WB-BIT*方法

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

1.1 函數定義

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

1.2 WB-BIT*方法主要模塊

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

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

圖1 知情集采樣區域Fig.1Informed set sampling region

rBIT*≥

(1)

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

在路徑規劃過程中，知情集是通過當前最優路徑長度決定采樣區域的大小，而更小的采樣區域能夠提供更精細的搜索.因此，為了通過減小采樣區域實現更加精細的搜索和更短路徑的獲取，本文設計了一種基于當前路徑包圍障礙物的路徑優化策略.該優化策略主要流程如算法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的路徑優化策略在路徑長度有更新時執行.策略開始執行后，當前路徑根據自定義變量被離散為均勻配置，由目標點至起始點將路徑包圍至障礙物周圍.算法2偽代碼中的LocalPath()函數表示節點之間連線的碰撞檢測，|Xpath|表示路徑點集合的基數，若路徑中包含N個點，則Xpath={x1,x2,…,xN}.此外，算法2中U是路徑連接的離散化程度，根據當前連接半徑與自定義變量的比例來確定，能在路徑點連接大于連接半徑時更精細地進行路徑優化.

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

圖2 路徑包圍優化過程Fig.2Wrapping process of path optimization

1.4 基于路徑指導的偏置采樣

路徑優化策略能夠加速當前路徑長度的減少，獲得一條高質量路徑.當環境中存在多條路徑時，則需要保持對潛在更優新路徑的探索.因此，本文設計了一種采樣策略，利用當前路徑點相關的啟發式函數值計算搜索區域，執行偏置采樣.

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

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

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

(2)

(3)

其次取各點啟發式值中最大值H(xmax)作為偏置采樣的參數，使用H(xmax)作為橢圓長軸的長度，起始點xstart和目標點xgoal作為焦點建立一個橢圓的偏置采樣區域，如圖3所示.所建立的偏置采樣區域屬于知情集的子集，在路徑數量不止一條的情況下，從這個較小子集中進行采樣，更有可能找到改善路徑和加快算法收斂的采樣點.然而，由于該采樣區域是通過現有路徑進行估計的，所含信息不如知情集充足，可能得到的是局部最優路徑[19].因此，為了確保WB-BIT*方法的采樣均勻性而保證漸進最優性，偏置采樣需要與知情集采樣結合，并引入偏置比α∈(0,1)來平衡這一采樣過程(如算法3第7行).當已有可行路徑時，在偏置區域生成樣本的概率為α,在知情集中進行采樣的概率則為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 理論分析

本小節對WB-BIT*方法進行理論分析.

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

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

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

定理2漸進最優性.當采樣至無窮個樣本時，WB-BIT*方法漸進收斂到給定路徑規劃問題的最優解的概率是1，即：

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

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

Xnear,k={xj∈Xsamples|j

rRRT*}.

圖4 仿真實驗環境Fig.4Experiment environments

采樣點xk從這些鄰點中選擇能夠使得xk的當前實際gT(xk)最小化的狀態進行連接，接著經重布線考慮其余鄰點能否通過連接xk使各自當前gT(xother)降低.

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

Xnear,k={x∈Yj|j

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

(4)

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

2 仿真實驗與分析

2.1 實驗環境設置

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

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

2.2 實驗結果與分析

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

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

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

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

表1 不同地圖仿真實驗結果

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

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

3 結 論

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