唐嘉寧,顏 衡,陳云浩,和雪梅,閆搏遠

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

0 引言

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

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

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

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

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

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

1.1 問題描述

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

1.2 數(shù)學模型

(1)

(2)

(3)

(4)

式(1)表示最小化多無人車路徑總代價,Ci表示無人車ri的路徑成本;式(2)表示一個目標點只能分配給一個無人車;式(3)表示一輛無人車只能對應唯一的目標點;式(4)表示不允許2輛無人車在同一時間出現(xiàn)在同一位置。

2 沖突搜索算法

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

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

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

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

2) 一組無人車的路徑Nsol。該組路徑通過底層搜索得到,每輛無人車ai的路徑必須滿足當前節(jié)點的Ncon。

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

以圖1為例,圖中無人車r1要移動到g1,無人車r2要移動到g2。

圖1 多無人車路徑示意圖

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

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

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

3.1 ECBS算法

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

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

圖3 搜索多無人車路徑基本流程框圖

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

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

(5)

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

3.2 混合A*搜索算法

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

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

圖4 節(jié)點擴展示意圖

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

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

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

(6)

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

3.3 動態(tài)焦點搜索算法

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

混合A*焦點搜索算法在多無人車路徑規(guī)劃時,不僅能夠規(guī)劃出更符合運動學約束的平滑路徑,還能減少沖突的發(fā)生,提高搜索路徑的效率。

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

(7)

式中:li表示無人車ri的路徑距離;ti為無人車ri由起點到達目標點的時間;α和β分別為路徑距離和時間的加權系數(shù)。

4 仿真實驗及結果分析

4.1 仿真實驗設置

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

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

4.2 算法性能對比

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

圖6 隨機障礙物環(huán)境20輛無人車不同時間點的軌跡

圖7表示8輛無人車在算法改進前后求解的路徑質量對比。

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

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

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

圖8 算法計算時間曲線

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

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

圖9 路徑成本曲線

5 結論

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

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