動(dòng)態(tài)沖突搜索的多無(wú)人車路徑規(guī)劃算法

唐嘉寧,顏 衡,陳云浩,和雪梅,閆搏遠(yuǎn)

(1.云南民族大學(xué) 電氣信息工程學(xué)院, 昆明 650000; 2.云南民族大學(xué) 無(wú)人自主系統(tǒng)研究院, 昆明 650000)

0 引言

自1980年代,無(wú)人車開始進(jìn)入人類的生活,無(wú)人車憑借高機(jī)動(dòng)性、高性能、執(zhí)行危險(xiǎn)任務(wù)不會(huì)造成人員傷亡等優(yōu)點(diǎn)被廣泛應(yīng)用于各種場(chǎng)景,為人類的生活、工作等提供了極大的便利[1]。但是在面對(duì)搶險(xiǎn)救災(zāi)、重要目標(biāo)搜索等時(shí)間要求高的任務(wù)時(shí),單無(wú)人車受到載荷、能源條件的限制,難以在有限的時(shí)間內(nèi)快速完成一些復(fù)雜的任務(wù),因此,多無(wú)人車的協(xié)同是當(dāng)前的一個(gè)研究熱點(diǎn)。

為使多無(wú)人車能夠快速應(yīng)對(duì)緊急復(fù)雜的任務(wù),需要快速生成連接無(wú)人車初始位置和目標(biāo)位置的無(wú)碰撞路徑,即多無(wú)人車路徑規(guī)劃(multi-UGV path finding)問(wèn)題。多無(wú)人車路徑規(guī)劃問(wèn)題是一個(gè)NP-hard問(wèn)題,為多無(wú)人車規(guī)劃出一組從已知起點(diǎn)到目標(biāo)終點(diǎn)的無(wú)碰撞沖突最優(yōu)路徑。目前,關(guān)于多無(wú)人車路徑規(guī)劃的方法大多是基于A*[2-3]、人工勢(shì)場(chǎng)[4]和Dijstra[5-6]等傳統(tǒng)算法進(jìn)行改進(jìn)優(yōu)化。Silver等[7]提出一種HCA*算法,根據(jù)預(yù)先定義的順序依次規(guī)劃無(wú)人車,當(dāng)無(wú)人車找到目標(biāo)路徑時(shí),此路徑將被存儲(chǔ)到全局保存表中,后續(xù)無(wú)人車在搜索路徑時(shí)不會(huì)遍歷先前發(fā)生沖突的位置。然而,隨著無(wú)人車數(shù)量增加,狀態(tài)空間呈指數(shù)級(jí)增長(zhǎng),在無(wú)人車數(shù)量多、地圖較大的場(chǎng)景,可能無(wú)法在有限時(shí)間內(nèi)找到最優(yōu)解。

除了上述傳統(tǒng)路徑規(guī)劃算法外,如蟻群算法[8]、遺傳算法[9]等啟發(fā)式算法也逐漸應(yīng)用到了多無(wú)人車路徑規(guī)劃中。另外一些學(xué)者運(yùn)用機(jī)器學(xué)習(xí)的方法解決多無(wú)人車路徑規(guī)劃問(wèn)題,Xia等[10]將神經(jīng)網(wǎng)絡(luò)用于多無(wú)人車路徑規(guī)劃問(wèn)題,以沖突半徑和每個(gè)路徑點(diǎn)到?jīng)_突中心的距離差作為神經(jīng)網(wǎng)絡(luò)的輸入,把沖突和路徑距離的加權(quán)作為神經(jīng)網(wǎng)絡(luò)的目標(biāo)函數(shù),輸出一組規(guī)避沖突且使每一條路徑距離總和最短的路徑點(diǎn)。但是以上方法都存在計(jì)算量大,求解速度慢,容易陷入局部最優(yōu)等缺點(diǎn)。

CBS算法[11-13]是一種用來(lái)求解多無(wú)人車路徑規(guī)劃問(wèn)題最優(yōu)解的框架,該算法通過(guò)底層搜索路徑,頂層消除沖突實(shí)現(xiàn)多無(wú)人車路徑規(guī)劃問(wèn)題的求解;Li等[12]提出了一種基于安全走廊約束的非線性優(yōu)化CBS算法,為每輛無(wú)人車構(gòu)建安全走廊,并對(duì)無(wú)人車的優(yōu)先級(jí)進(jìn)行排序;Sharon等[13]在CBS算法的基礎(chǔ)上提出了一種MA-CBS(Meta-Agent CBS)算法,該算法不局限于單智能體的底層搜索,而是將有沖突的智能體合并為一個(gè)小組,相比CBS算法,計(jì)算速度提升了一個(gè)數(shù)量級(jí);Barer等[14]提出的ECBS算法通過(guò)焦點(diǎn)搜索大幅度減少了運(yùn)行時(shí)間;Andreychuk等[15]提出了時(shí)間連續(xù)的CBS算法(continous-time CBS,CCBS),將某個(gè)時(shí)刻的約束改為某個(gè)時(shí)間段內(nèi)的約束;Li等[16]提出明確估計(jì)CBS(explicit estimation CBS,EECBS)算法,該算法的頂層搜索樹的擴(kuò)展由在線學(xué)習(xí)決定;Chan等[17]提出了增強(qiáng)并行CBS(ENHANCED PARALLEL CBS)算法,在ECBS的基礎(chǔ)上采用多線程進(jìn)行搜索,提高ECBS算法的效率;Li等[18]改進(jìn)了CBS算法的頂層搜索,通過(guò)推理智能體之間的沖突關(guān)系提出了2種啟發(fā)式,顯著提高了成功率;喬喬等[19]將ECBS算法中的單向搜索改為雙向搜索,以加快搜索速度。Li等[20]使用多鄰域搜索對(duì)智能體集群進(jìn)行重規(guī)劃,解決了范圍大的多智能體路徑規(guī)劃問(wèn)題。

以上研究中對(duì)CBS算法的改進(jìn)都存在搜索效率低以及生成的路徑不符合運(yùn)動(dòng)學(xué)約束等問(wèn)題,因此,本文在CBS算法的基礎(chǔ)上,對(duì)底層搜索進(jìn)行改進(jìn),提出了一種混合A*焦點(diǎn)搜索算法,使生成的路徑更符合運(yùn)動(dòng)學(xué)約束;引入次優(yōu)因子提高了CBS算法效率;并將路徑距離加權(quán)系數(shù)和時(shí)間加權(quán)系數(shù)引入CBS算法的底層路徑搜索中,以找到快且短的路徑。

1 多無(wú)人車路徑規(guī)劃

1.1 問(wèn)題描述

多無(wú)人車路徑規(guī)劃問(wèn)題是由一組k個(gè)無(wú)人車{r1,r2,…,rk}與有向圖G=(V,E)組成,每個(gè)無(wú)人車ri都有各自的起點(diǎn)si∈V和目標(biāo)點(diǎn)gi∈V。在每個(gè)時(shí)間點(diǎn)t,無(wú)人車可執(zhí)行一個(gè)移動(dòng)到相鄰頂點(diǎn)的動(dòng)作或保持當(dāng)前位置的等待動(dòng)作,這2個(gè)動(dòng)作的時(shí)間成本都是t0,但是路徑距離成本只有無(wú)人車移動(dòng)時(shí)才增加。目標(biāo)是為多無(wú)人車找出一組從起點(diǎn)si到目標(biāo)終點(diǎn)gi的無(wú)碰撞沖突最優(yōu)路徑,該路徑不僅要滿足路徑距離成本最小,還要盡量滿足時(shí)間成本累積小。

1.2 數(shù)學(xué)模型

(1)

(2)

(3)

(4)

式(1)表示最小化多無(wú)人車路徑總代價(jià),Ci表示無(wú)人車ri的路徑成本;式(2)表示一個(gè)目標(biāo)點(diǎn)只能分配給一個(gè)無(wú)人車;式(3)表示一輛無(wú)人車只能對(duì)應(yīng)唯一的目標(biāo)點(diǎn);式(4)表示不允許2輛無(wú)人車在同一時(shí)間出現(xiàn)在同一位置。

2 沖突搜索算法

沖突搜索(CBS)算法是求解多無(wú)人車路徑規(guī)劃問(wèn)題的最優(yōu)解算法框架。底層使用A*,Dijstra等普通算法進(jìn)行單機(jī)路徑搜索,為每輛無(wú)人車尋找各自的有效最優(yōu)路徑,但和傳統(tǒng)A*算法不同的是:① 需要滿足頂層搜索中添加的沖突規(guī)避約束;② 最優(yōu)路徑搜索不僅要考慮空間維度,還要考慮時(shí)間維度。

頂層遍歷底層搜索的路徑,檢查路徑之間是否存在沖突,如果存在沖突,則添加約束后重新進(jìn)行底層搜索,直到所有路徑?jīng)]有沖突為止。

CBS中頂層搜索生成一棵二叉樹(constarint tree,CT),CT中每個(gè)節(jié)點(diǎn)N都包含以下信息:

1) 對(duì)每輛無(wú)人車的約束集Ncon。CT中的根節(jié)點(diǎn)為一組空約束,子節(jié)點(diǎn)繼承父節(jié)點(diǎn)的約束,且為一輛無(wú)人車增加一組新的約束。

2) 一組無(wú)人車的路徑Nsol。該組路徑通過(guò)底層搜索得到,每輛無(wú)人車ai的路徑必須滿足當(dāng)前節(jié)點(diǎn)的Ncon。

3) 總成本Ncost。該值為Nsol中每輛無(wú)人車的路徑成本總和。

以圖1為例,圖中無(wú)人車r1要移動(dòng)到g1,無(wú)人車r2要移動(dòng)到g2。

圖1 多無(wú)人車路徑示意圖

底層搜索中兩輛無(wú)人車各自的最優(yōu)路徑為r1:和r2:,此時(shí)路徑總成本Ncost=10。因?yàn)?輛無(wú)人車在第3步時(shí)會(huì)在C3發(fā)生碰撞沖突,調(diào)用頂層搜索為無(wú)人車分別添加約束和,并生成2個(gè)子節(jié)點(diǎn),如圖2。再次調(diào)用底層搜索重新尋找滿足新約束條件的最優(yōu)路徑,其中左子節(jié)點(diǎn)生成的路徑r1:,r2的路徑保持不變;右子節(jié)點(diǎn)生成的路徑r1的路徑保持不變,r2:。2個(gè)子節(jié)點(diǎn)的路徑都沒(méi)有沖突,且路徑總成本Ncost都等于11,按照二叉樹的遍歷規(guī)則,選擇左子節(jié)點(diǎn)的Nsol為最終的路徑。若2個(gè)子節(jié)點(diǎn)都還存在沖突,則繼續(xù)展開子節(jié)點(diǎn),直到生成一組沒(méi)有碰撞的路徑。

圖2 沖突生成的二叉樹示意圖

3 動(dòng)態(tài)沖突搜索算法

3.1 ECBS算法

CBS算法在頂層搜索檢測(cè)到?jīng)_突時(shí),會(huì)根據(jù)沖突生成2個(gè)子節(jié)點(diǎn),并對(duì)2個(gè)子節(jié)點(diǎn)新增相應(yīng)的約束,在底層路徑搜索時(shí),只需要找到滿足約束的路徑,這種盲目的搜索只能處理當(dāng)前的沖突,新規(guī)劃的路徑還可能再次出現(xiàn)沖突,因此,增強(qiáng)的CBS(enhanced conflict-based search,ECBS)算法在CBS的框架基礎(chǔ)上對(duì)2層搜索進(jìn)行了優(yōu)化。

ECBS算法與CBS算法相同,底層搜索每個(gè)無(wú)人車的路徑,頂層消除無(wú)人車之間的沖突。不同的是ECBS在頂層和底層搜索中使用了焦點(diǎn)搜索,每次迭代不僅搜索一條最優(yōu)的路徑,而是搜索在恒定次優(yōu)因子ω內(nèi)的全部路徑,大大降低沖突頻率的發(fā)生,其基本流程如圖3所示。

圖3 搜索多無(wú)人車路徑基本流程框圖

焦點(diǎn)搜索生成2個(gè)節(jié)點(diǎn)列表:OPEN和FOCAL。OPEN列表是A*算法的常規(guī)列表,存儲(chǔ)當(dāng)前節(jié)點(diǎn)和所有鄰居節(jié)點(diǎn)的集合;FOCAL列表是OPEN列表的子集,存放符合下面要求的節(jié)點(diǎn):

FOCAL={n|n∈OPEN,f(n)≤ω·fmin(n)}

(5)

式中:n表示路徑節(jié)點(diǎn),fmin(n)為OPEN列表中代價(jià)估計(jì)的下界,ω為次優(yōu)因子,FOCAL列表中所有路徑節(jié)點(diǎn)的代價(jià)在ω·fmin(n)以內(nèi),FOCAL列表中的節(jié)點(diǎn)按照每個(gè)節(jié)點(diǎn)發(fā)生沖突的次數(shù)進(jìn)行增序排列,用來(lái)判斷節(jié)點(diǎn)擴(kuò)展順序的優(yōu)先級(jí)。

3.2 混合A*搜索算法

ECBS算法雖然能夠保證解決方案的同時(shí)提高搜索效率,但是在底層搜索時(shí)仍采用傳統(tǒng)的A*算法,未考慮在實(shí)際應(yīng)用場(chǎng)景中的運(yùn)動(dòng)學(xué)約束問(wèn)題,本文結(jié)合傳統(tǒng)A*算法,提出了在底層路徑搜索中使用混合A*焦點(diǎn)搜索算法,使規(guī)劃的路徑符合無(wú)人車運(yùn)動(dòng)學(xué)約束,同時(shí)有效減少了沖突發(fā)生的概率,進(jìn)而提高了算法搜索路徑的速度。

傳統(tǒng)A*算法的節(jié)點(diǎn)會(huì)選擇周圍4個(gè)或8個(gè)節(jié)點(diǎn)進(jìn)行擴(kuò)展,如圖4(a),這種節(jié)點(diǎn)擴(kuò)展方式只考慮位置,而忽略了方向角信息,因此不適合用在有運(yùn)動(dòng)學(xué)約束的無(wú)人車上。本文的混合A*算法在進(jìn)行節(jié)點(diǎn)擴(kuò)展時(shí),需要考慮運(yùn)動(dòng)學(xué)約束,所以擴(kuò)展的節(jié)點(diǎn)包含了方向角信息,通過(guò)對(duì)路徑的曲率添加約束條件來(lái)滿足無(wú)人車角速度的約束,圖4(b)為混合A*節(jié)點(diǎn)擴(kuò)展的基本方式。

圖4 節(jié)點(diǎn)擴(kuò)展示意圖

本文的混合A*繼承了傳統(tǒng)A*的一些思想,都會(huì)生成OPEN和CLOSE列表以及啟發(fā)式函數(shù)f(n)=g(n)+h(n),其流程如圖5所示。

圖5 混合A*算法流程框圖

混合A*算法中g(shù)(n)與傳統(tǒng)A*相同,均為無(wú)人車實(shí)際行駛的距離,不過(guò)h(n)需要考慮的因素有很多,如轉(zhuǎn)向角變化,因此本文中混合A*的啟發(fā)式評(píng)估代價(jià)為

(6)

式中: (sx,sy)和(gx,gy)分別為當(dāng)前節(jié)點(diǎn)n和目標(biāo)節(jié)點(diǎn)的坐標(biāo)值;kθ為偏航角的權(quán)重系數(shù);θ為無(wú)人車當(dāng)前位置的指向角度;θg為當(dāng)前節(jié)點(diǎn)n指向目標(biāo)節(jié)點(diǎn)的角度。

3.3 動(dòng)態(tài)焦點(diǎn)搜索算法

在底層搜索中,將混合A*算法中OPEN列表中滿足條件的次優(yōu)子節(jié)點(diǎn)加入FOCAL列表中,并從FOCAL列表中選擇要擴(kuò)展的節(jié)點(diǎn),當(dāng)目標(biāo)點(diǎn)出現(xiàn)在FOCAL列表某節(jié)點(diǎn)的搜索范圍內(nèi)停止搜索。因?yàn)榛旌螦*生成的子節(jié)點(diǎn)并不一定位于柵格的頂點(diǎn)或中心位置,即同一柵格內(nèi)可能有多個(gè)節(jié)點(diǎn),因此,當(dāng)同一柵格的2個(gè)子節(jié)點(diǎn)距離小于或等于2倍的無(wú)人車半徑Ri時(shí),才有可能發(fā)生沖突,否則兩輛無(wú)人車可以同時(shí)出現(xiàn)在同一柵格。

混合A*焦點(diǎn)搜索算法在多無(wú)人車路徑規(guī)劃時(shí),不僅能夠規(guī)劃出更符合運(yùn)動(dòng)學(xué)約束的平滑路徑,還能減少?zèng)_突的發(fā)生,提高搜索路徑的效率。

多無(wú)人車進(jìn)行路徑規(guī)劃時(shí),會(huì)為了找到最短路徑需等待較長(zhǎng)的時(shí)間,因此,不能僅用路徑的長(zhǎng)度作為衡量標(biāo)準(zhǔn),尤其是在重要物資投送等對(duì)時(shí)間敏感度高的任務(wù)中,路徑的時(shí)間成本同樣重要,因此,本文將路徑距離加權(quán)系數(shù)和路徑時(shí)間加權(quán)系數(shù)代入函數(shù)中,使用公式(7)衡量多無(wú)人車路徑的總成本,

(7)

式中:li表示無(wú)人車ri的路徑距離;ti為無(wú)人車ri由起點(diǎn)到達(dá)目標(biāo)點(diǎn)的時(shí)間;α和β分別為路徑距離和時(shí)間的加權(quán)系數(shù)。

4 仿真實(shí)驗(yàn)及結(jié)果分析

4.1 仿真實(shí)驗(yàn)設(shè)置

為驗(yàn)證本文提出改進(jìn)混合A*焦點(diǎn)搜索的有效性和可行性,采用ROS仿真平臺(tái),在一臺(tái)ubuntu16.04,intel i5-12400@2.5 GHz處理器,運(yùn)行內(nèi)存16GB的集顯臺(tái)式機(jī)電腦上運(yùn)行。對(duì)文獻(xiàn)[12]的CBS算法和本文提出的動(dòng)態(tài)CBS算法進(jìn)行對(duì)照實(shí)驗(yàn)。在仿真中,使用八叉樹地圖表示環(huán)境的占用情況。其中無(wú)人車的參數(shù)設(shè)置如表1所示。

表1 無(wú)人車參數(shù)設(shè)置

4.2 算法性能對(duì)比

圖6為本文方法20輛無(wú)人車在隨機(jī)障礙物環(huán)境中、不同時(shí)間點(diǎn)運(yùn)行的軌跡圖,其中圖6(a)和圖6(d)分別為無(wú)人車初始時(shí)刻的位置和無(wú)人車行駛至目標(biāo)點(diǎn)時(shí)的最終軌跡圖。地圖環(huán)境設(shè)置為8 m×8 m的正方形,環(huán)境里包含隨機(jī)生成的12個(gè)0.3 m×0.3 m的正方形障礙物,每輛無(wú)人車的起點(diǎn)和目標(biāo)點(diǎn)位置在實(shí)驗(yàn)開始之前給定,并保證不被障礙物占據(jù)。

圖6 隨機(jī)障礙物環(huán)境20輛無(wú)人車不同時(shí)間點(diǎn)的軌跡

圖7表示8輛無(wú)人車在算法改進(jìn)前后求解的路徑質(zhì)量對(duì)比。

圖7 CBS算法底層搜索使用A*路徑搜索和混合A*路徑搜索結(jié)果圖

本文提出的算法能夠有效提高多無(wú)人車路徑的求解質(zhì)量,主要體現(xiàn)在求解的路徑更符合無(wú)人車運(yùn)動(dòng)學(xué)的約束,圖7(a)為改進(jìn)前的CBS算法在底層搜索中使用A*路徑搜索,因?yàn)閭鹘y(tǒng)A*算法八向搜索和擴(kuò)展的特性,無(wú)人車的路徑為折線;圖7(b)為本文改進(jìn)后的CBS算法,在底層使用混合A*路徑搜索,根據(jù)無(wú)人車轉(zhuǎn)向角進(jìn)行節(jié)點(diǎn)擴(kuò)展,同時(shí)生成滿足無(wú)人車運(yùn)動(dòng)學(xué)約束的軌跡,規(guī)劃的路徑是平滑曲線。同時(shí)無(wú)人車的路徑重合部分也大幅減少,這也為之后的頂層沖突消解節(jié)省了時(shí)間。

為驗(yàn)證算法的效率,在相同的仿真環(huán)境中,分別對(duì)本文方法和前沿的ECBS算法各重復(fù)21次實(shí)驗(yàn),記錄每次實(shí)驗(yàn)結(jié)果并取平均值為最終的實(shí)驗(yàn)結(jié)果。圖8為不同數(shù)量無(wú)人ECBS運(yùn)行的時(shí)間曲線。

圖8 算法計(jì)算時(shí)間曲線

與前沿的ECBS方法相比,本文提出的DCBS算法搜索的路徑重合部分大幅度減少,配合次優(yōu)路徑策略和混合A*算法,無(wú)人車可選擇的路徑數(shù)量變多,無(wú)人車之間的沖突數(shù)量減少,計(jì)算速度整體提高。當(dāng)無(wú)人車數(shù)量在4—16輛時(shí),本文提出的算法計(jì)算時(shí)間減少了0.14～1.56 ms,相較于前沿的ECBS算法平均提高了31.98%;當(dāng)無(wú)人車數(shù)量超過(guò)20時(shí),計(jì)算速度的提升逐漸明顯,本文提出的算法計(jì)算速度的提升均超過(guò)50%,尤其是在32輛無(wú)人車時(shí),計(jì)算速度的提升最明顯,計(jì)算時(shí)間縮短83.71 ms,計(jì)算速度提升了71.95%。實(shí)驗(yàn)數(shù)據(jù)表明:本文提出的DCBS算法在多無(wú)人車的路徑規(guī)劃速度上有明顯提升,尤其是在無(wú)人車數(shù)量較多的情況下,提出的DCBS算法更具適用性。

為了驗(yàn)證本文提出的算法不僅在更短的時(shí)間內(nèi)為多無(wú)人車規(guī)劃出符合運(yùn)動(dòng)學(xué)約束的路徑,且路徑總成本也有降低,使用引入路徑距離權(quán)重α和時(shí)間權(quán)重β的代價(jià)判別公式(7)對(duì)路徑總成本進(jìn)行衡量,其中距離權(quán)重和時(shí)間權(quán)重分別設(shè)置為0.2和0.1。圖9為本文方法與前沿ECBS算法路徑總成本曲線,因?yàn)闊o(wú)人車數(shù)量的增加,2種方法的路徑總成本都在不斷增加,但本文方法路徑總成本普遍更低,路徑總成本平均降低7.64%。在16輛和28輛無(wú)人車時(shí),路徑總成本降低最多,分別降低了10.52和10.92;在8輛無(wú)人車時(shí),路徑總成本雖然只降低了6.66,但是因?yàn)闊o(wú)人車數(shù)量少,路徑總距離和時(shí)間都很短,路徑總成本降低了18.91%,是本文實(shí)驗(yàn)中的成本降低率的最大值。實(shí)驗(yàn)數(shù)據(jù)表明:本文提出的DCBS算法有效降低了多無(wú)人車路徑的總成本。

圖9 路徑成本曲線

5 結(jié)論

本文針對(duì)CBS算法在多無(wú)人車路徑規(guī)劃中存在求解效率低,求解路徑不符合無(wú)人車運(yùn)動(dòng)學(xué)約束等問(wèn)題,在CBS算法的基礎(chǔ)上,提出了動(dòng)態(tài)CBS(DCBS)算法,基于次優(yōu)策略和路徑平滑策略,在底層搜索中使用混合A*搜索算法并引入次優(yōu)因子,提高了路徑的規(guī)劃速度,使規(guī)劃的路徑更符合運(yùn)動(dòng)學(xué)的約束;為了解決多車路徑規(guī)劃為了找到最短路徑而等待時(shí)間過(guò)長(zhǎng)的問(wèn)題,引入路徑距離加權(quán)系數(shù)α和時(shí)間加權(quán)系數(shù)β,平衡路徑距離和時(shí)間的關(guān)系,以規(guī)劃出快且短的路徑。與前沿ECBS算法相比,本文提出的DCBS算法減少了多無(wú)人車路徑規(guī)劃的時(shí)間,規(guī)劃出的路徑更符合運(yùn)動(dòng)學(xué)約束,同時(shí)使整體路徑代價(jià)減少。

本文研究的多無(wú)人車路徑規(guī)劃是二維平面路徑規(guī)劃,未來(lái)將以多無(wú)人機(jī)三維環(huán)境中的路徑規(guī)劃為重點(diǎn)展開研究。