宣志瑋，毛劍琳，張凱翔

（1.昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院，云南昆明 650500；2.昆明理工大學(xué)機(jī)電工程學(xué)院，云南昆明 650500）

1 引言

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

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

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

2 問題描述

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

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

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

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

圖1 CBS算法對(duì)沖突處理的兩層結(jié)構(gòu)

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

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

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

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

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

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

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

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

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

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

本文地圖建模時(shí)采用柵格法建模.每個(gè)柵格與相鄰柵格的距離為1.

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

圖2 剖分散點(diǎn)選擇

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

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

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

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

圖4 剖分點(diǎn)二次篩選

3.2 基于可視點(diǎn)的A*算法

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

圖5 機(jī)器人節(jié)點(diǎn)拓展

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

圖6 LEADTOC算法流程

4 仿真結(jié)果

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

4.1 算法求解效率對(duì)比

4.1.1 簡(jiǎn)單測(cè)試算例

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

表1 簡(jiǎn)單算例仿真結(jié)果

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

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

4.1.2 復(fù)雜測(cè)試算例

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

圖8 復(fù)雜算例3種算法節(jié)點(diǎn)拓展數(shù)量對(duì)比

表2 復(fù)雜算例仿真結(jié)果

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

4.2 更多測(cè)試算例

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

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

圖9 測(cè)試地圖集

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

表4 Benchmark測(cè)試結(jié)果

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

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

4.3 動(dòng)態(tài)沖突場(chǎng)景仿真測(cè)試

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

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

圖10 動(dòng)態(tài)沖突物仿真結(jié)果

5 結(jié)論

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