CBS框架下面向復雜地圖的低拓展度A*算法

宣志瑋，毛劍琳，張凱翔

（1.昆明理工大學信息工程與自動化學院，云南昆明 650500；2.昆明理工大學機電工程學院，云南昆明 650500）

1 引言

隨著移動機器人的應用范圍越來越廣泛，移動機器人的應用環(huán)境也變得越來越復雜、多變，因此面向復雜地圖的移動機器人路徑規(guī)劃問題成為機器人領域［1］、視頻游戲領域［2］、無人機領域［3］、機器人編隊系統(tǒng)領域［4］的研究熱點.目前針對機器人路徑規(guī)劃的求解算法有基于A*（A Star）的算法［5］，基于約簡（Reduction-Based）的算法［6］，也有基于搜索框架的算法［7，8］，基于采樣的算法［9，10］，人工勢場法［11］，蟻群算法［12，13］等.

基于沖突的搜索算法（Conflict-Based Search，CBS）是求解多機器人路徑規(guī)劃（Multi-Agent Path Finding，MAPF）的一種重要框架［7］，由兩層結構組成，低層結構采用A*算法完成單機器人路徑規(guī)劃；高層結構進行多機器人路徑的沖突判斷，并向低層提供沖突點和沖突時間信息.常用A*類算法作為低層尋路算法求解路徑，為了提高算法求解效率，其中有Tie-Breaking 策略［14，15］，通過減少搜索過程中拓展節(jié)點的數(shù)量來提升運行效率.A*算法處理大規(guī)模任務時仍然會拓展大量節(jié)點，這在計算上是不可行的［16］.

針對低層算法在面對大地圖和復雜障礙物時求解效率低的問題，本文提出帶障礙處理的Delaunay 三角剖分優(yōu)化和分段A*算法相結合的思想，對CBS 框架下低層路徑規(guī)劃問題進行求解，以此完成復雜障礙環(huán)境中高效地路徑優(yōu)化.

2 問題描述

2.1 Conflict-Based Search(CBS)框架描述

A*算法在求解MAPF 問題時，狀態(tài)空間與機器人數(shù)量呈指數(shù)關系［6］，當機器人數(shù)量為1 時，狀態(tài)空間只與地圖大小成線性關系.CBS框架將MAPF問題分解為一個帶約束限制的單機器人尋路問題.CBS 框架定義如下所示：

給定地圖G=(V，E)，V為圖中的頂點集合，E為圖中的邊集合.約束元組信息(ai，v，t)表示機器人ai禁止在時刻t占據(jù)頂點v，v∈V.每個機器人最后生成的路徑都滿足對其添加的約束.

CBS的核心思想是為沖突機器人增加一組約束，直到找到能滿足約束的路徑.CBS 算法是兩層結構算法.在高層，進行全局沖突檢測并添加約束；低層則更新機器人路徑以滿足新的約束信息.如圖1所示.

圖1 CBS算法對沖突處理的兩層結構

2.2 低層機器人路徑規(guī)劃問題描述

機器人的最優(yōu)路徑規(guī)劃問題為依據(jù)某個或某些優(yōu)化準則（如工作代價最小、行走路徑最短、行走時間最短等），在給定工作空間中找到一個從起始點到目標點的無障礙最優(yōu)路徑.

給定柵格地圖G和機器人a，機器人起點為s和終點為g，經(jīng)過時間為T，時間離散化，機器人每次移動時間為一個單位時間，允許向四鄰域范圍內的自由柵格進行節(jié)點拓展，最后即可生成最終路徑P為一組路徑序列P=(s0，p1，…，pT-1，gT)，注意，該路徑集合P為無固定障礙路徑.

在CBS 框架下，低層求解器采用考慮時空信息的標準A*算法進行求解.標準A*算法在地形簡單、范圍較小的地圖中尋路求解路徑質量高，且在該框架下，A*算法僅與地圖規(guī)模是線性關系，但隨著機器人數(shù)量的增加，算法仍然會拓展數(shù)量眾多的節(jié)點，這對求解效率的提升帶來了阻礙.

本文通過計算幾何和優(yōu)化啟發(fā)式函數(shù)選擇節(jié)點策略，兩者相結合提高CBS 框架下低層算法的求解效率，加速MAPF的求解過程.

3 帶障礙處理Delaunay 三角剖分優(yōu)化的低拓展度A*算法

為降低A*算法所需的節(jié)點擴展度從而提升求解效率，本文提出帶障礙處理Delaunay 三角剖分優(yōu)化的低拓展度A*算法（Low Extension A*algorithm under Delaunay Triangulation with Obstacle Constraints，LEADTOC），采用Delaunay 三角剖分進行地圖和固定障礙物的處理［17］，然后采用最短路徑算法和可視化方法獲得無固定障礙的連通路徑，最后采用分段A*算法規(guī)劃出優(yōu)化路徑.

3.1 帶障礙處理的Delaunay 三角剖分及無固定障礙路徑優(yōu)化

Delaunay 三角剖分是指把散點集合C剖分成不均勻的三角形網(wǎng)格.對于平面內的給定散點有以下2 個特點：空圓性（在Delaunay 三角剖分中任意三角形的外接圓范圍內不會有其他點存在），該特性保證了Delaunay 三角形的唯一性；最大化最小角特性（剖分后三角形的最小角最大）.

通過縮放因子的靈活選擇可控制生成三角剖分地圖邊和節(jié)點的數(shù)量，對特定地圖選取恰當?shù)目s放因子可在保證求解質量的前提下，進一步優(yōu)化求解效率，縮放因子一般取為5～8.以250×250 大小的地圖為例，取縮放因子為7，可將大地圖近似縮放為35×35 大小規(guī)模的地圖.

本文地圖建模時采用柵格法建模.每個柵格與相鄰柵格的距離為1.

對于給定地圖，首先進行剖分散點的選擇，選出兩部分的點：包圍障礙物邊緣的點和二維平面內的均勻取點.包圍障礙物邊緣的散點，這一部分的散點距離最近障礙物的距離為1，即該部分選取的每一個散點與其最近的障礙物的距離均為1，如圖2所示；第二部分散點根據(jù)縮放規(guī)模在二維柵格地圖均勻取點，點間距為縮放因子所確定的縮放規(guī)模，縮放規(guī)模為5就是在橫縱坐標兩個維度每隔5個柵格取一個點（不包括處于障礙物內的點），剖分散點由以上兩部分點構成.

圖2 剖分散點選擇

如圖3 所示，根據(jù)散點生成的第一次三角形，其中部分三角形的邊有穿越障礙物的情況.通過最近點搜索算法，將三角形的重心設置為參考點，障礙物點作為查詢點，篩選出包圍查詢點的三角形.后續(xù)將篩選出的三角形的每兩個頂點間進行線性插值，插值后的狀態(tài)點有一個及其以上的點處于障礙物柵格則視為這兩個頂點之間的邊不可見，將三角形組成的不可見邊進行刪除，最后生成基于三角剖分的無障礙連通圖GD（Graph with Constrained Delaunay Triangulation）.

圖3 帶障礙物處理的三角剖分

隨后，對無障礙連通圖GD采用最短路徑算法計算從起點s到終點g的最短路徑，可獲得機器人a在圖GD上的路徑集合P=(s，v1，…，vn，g)，vi（i=1，…，n）為中間路徑點，s，g，vi∈V.

根據(jù)兩點間直線距離最短原理，采用可視性判斷對路徑集合P從起點s到終點g進行優(yōu)化，將當前節(jié)點與序號大于當前節(jié)點的最大序號可視節(jié)點連接，連接后將當前序號最大的可視節(jié)點視為起點與其后續(xù)節(jié)點繼續(xù)做可視性分析，最終得到優(yōu)化后的路徑集合.如圖4 所示，通過最短路徑算法得到路徑集合P=(s，v1，v2，v3，v4，g)，經(jīng)過可視性優(yōu)化后，得到路徑集合Popt=(s，v4，g).第一次生成的路徑點集合包含冗余節(jié)點，通過可視性原則對路徑集合P進行優(yōu)化，刪除冗余節(jié)點，得到二次篩選節(jié)點集合Popt，低層尋路算法按照二次篩選路徑點集合Popt進行分階段尋路.

圖4 剖分點二次篩選

3.2 基于可視點的A*算法

在可視優(yōu)化路徑的基礎上，采用A*算法依據(jù)可視點進行分段尋路，即對于機器人a由初始任務Task=(s，g)替換為分階段尋路任務Taskopt=(Popt).需說明的是，在每個階段，機器人在柵格地圖采用A*算法進行四鄰域搜索，即給定起點的情況下，向鄰近無障礙柵格搜索，如圖5 所示，機器人可以向上、左、右三個直線方向上的鄰近空白區(qū)域拓展節(jié)點，但無法向下拓展節(jié)點，由此，避開障礙物.

圖5 機器人節(jié)點拓展

綜上所述，整個改進算法如圖6所示，由地圖處理、分階段尋路、沖突檢測三部分組成.在帶約束的三角剖分地圖基礎上，進行圖搜索和經(jīng)過點的可視性優(yōu)化，然后進行分階段A*算法尋路.在CBS 算法框架下，算法不僅能快速找到最優(yōu)路徑，同時能夠滿足約束條件，最后生成一條無沖突的路徑.

圖6 LEADTOC算法流程

4 仿真結果

為測試所提出的LEADTOC 算法性能，本文采用文獻［18］中給出的標準算例集，該算例集給出不同規(guī)模和復雜度的地圖，以及機器人在地圖中的起點和終點，由此進行的測試具有可復現(xiàn)性和可對比性.對比算法為考慮時空信息的標準A*算法［5］和在擴展度方面有優(yōu)勢的Tie-Breaking A*算法［14］，在本文中Tie-Breaking 策略取為：h=(1+d)×h，其中，h是啟發(fā)式函數(shù)，為曼哈頓距離，d為Tie-Breaking 值，是一個很小的數(shù)字，本文A*算法、Tie-Breaking A*算法和LEADTOC 算法的d值分別取為0、0.001、0.001.所有算例結果均為20 次獨立運行的平均值.測試采用的性能參數(shù)為：擴展節(jié)點數(shù)量，最終路徑長度，求解時間（完成每次規(guī)劃所需的時間）.所有運行均在matlab2020b環(huán)境下，11th Gen Intel（R）Core（TM）i5-1135G7@2.40 GHz和16 GB 內存容量的計算機配置下完成.

4.1 算法求解效率對比

4.1.1 簡單測試算例

本算例為地圖den312d下的測試，機器人起點坐標為（16，60），終點坐標為（55，37）.3 種算法的路徑規(guī)劃結果如圖7 所示，其中紅色線為最終規(guī)劃的路徑，藍色區(qū)域為算法運行過程中擴展的節(jié)點.由圖7 可見，本文提出的LEADTOC 算法和Tie-Breaking A*算法拓展節(jié)點數(shù)量都明顯少于A*算法.3種算法的性能結果見表1.

表1 簡單算例仿真結果

圖7 簡單算例3種算法節(jié)點拓展數(shù)量對比

由表1 可見，3 種算法規(guī)劃的路徑長度相同.在求解時間方面，本文算法高于Tie-Breaking A*算法.這是由于本文方法采用了三角剖分進行地圖預處理和無障礙連通圖規(guī)劃，導致計算時間稍長于Tie-Breaking A*算法.但經(jīng)過預處理后依然可以通過降低節(jié)點擴展量來減少求解時間，所以求解時間仍能比A*算法低.

4.1.2 復雜測試算例

圖8 給出了復雜地圖den520d 下的路徑規(guī)劃結果，其中，機器人起點坐標為（146，50），終點坐標為（10，183），起點和終點間的障礙物較為曲折復雜.表2 則給出各算法的性能指標結果.

圖8 復雜算例3種算法節(jié)點拓展數(shù)量對比

表2 復雜算例仿真結果

由測試結果可見，在復雜地圖上本文算法均可在拓展節(jié)點數(shù)量和計算時間上明顯少于A*算法和Tie-Breaking A*算法.這說明本文算法采用的三角剖分優(yōu)化雖然在地圖預處理和無障礙連通圖的生成方面有時間代價，但是當?shù)貓D場景較為復雜時，該時間代價遠低于節(jié)點擴展帶來的時間消耗，故可較大提升算法的柵格路徑規(guī)劃效率.

4.2 更多測試算例

為了進一步驗證本文算法的求解效率和求解質量，與A*算法和Tie-Breaking A*算法對比，對文獻［18］中的10 個地圖進行測試.圖9 給出了若干典型地圖樣貌，由圖可見不同復雜程度和復雜類型的障礙情況.表3 給出每個測試地圖可用算例數(shù)量，可用算例數(shù)量指在MATLAB 笛卡爾坐標系下的可用算例數(shù)量，表4給出了10個算例下的算法性能結果.

表3 測試地圖可用算例數(shù)量

圖9 測試地圖集

由表4 可得，LEADTOC 算法在節(jié)點拓展數(shù)量上為A*算法的5.3%～52.2%，路徑長度為A*算法的98.1%～102.2%，平均求解時間為A*算法的44.3%.

表4 Benchmark測試結果

LEADTOC 算法在不規(guī)則復雜地形表現(xiàn)良好，在規(guī)則地形如Room 地形的求解質量稍差，LEADTOC 算法在大地形、復雜地形能發(fā)揮出改進算法的求解效率的優(yōu)勢.

下面是上述算例的結果分析說明：如表5 所示，以A*算法為基準，基準設置為1，分別對比了A*比A*、Tie-Breaking A*比A*、LEADTOC 比A*在節(jié)點拓展數(shù)量、路徑長度、運行時間三方面的算法性能對比情況.由結果可得LEADTOC 算法在節(jié)點數(shù)量拓展對比，運行時間對比均低于另外2 種算法，在路徑長度對比方面，長度基本持平.

4.3 動態(tài)沖突場景仿真測試

本節(jié)仿真實驗地圖為Benchmark 中的den520d，機器人和動態(tài)沖突的參數(shù)設置見表6.

如圖10（a）所示，在不考慮動態(tài)沖突時，機器人在規(guī)劃路徑上將分別與動態(tài)沖突物1和2在沖突點相撞.對此，本文算法將沖突碰撞信息轉化為約束傳遞給機器人，由機器人規(guī)劃出一條無沖突路徑，行進過程中可安全避開2 個動態(tài)沖突，成功到達終點，如圖10（b）所示.可見，本文算法能很好地處理動態(tài)沖突物的碰撞信息，并規(guī)劃出機器人的無沖突路徑.

圖10 動態(tài)沖突物仿真結果

5 結論

低層路徑規(guī)劃質量是影響多機器人路徑規(guī)劃的重要因素之一，對此，本文提出LEADTOC 算法，其中引入帶障礙處理的三角剖分方法，并提出縮放因子以靈活地適應不同類型的地圖且同時降低地圖數(shù)據(jù)量，由此獲得無固定障礙的路徑引導.進一步采用可視化分段策略，令具有動態(tài)沖突處理能力的A*算法依相鄰可視點進行分段路徑規(guī)劃，該策略可大大降低A*算法在路徑探索時的節(jié)點拓展度.仿真結果表明，復雜地形下，LEADTOC 算法的節(jié)點拓展數(shù)量和存儲空間需求量較低，在不降低求解質量的情況下能較好地提升求解效率.后續(xù)將對CBS 框架下多機器人的沖突判斷和分解策略的優(yōu)化展開進一步研究.