動態沖突搜索的多無人車路徑規劃算法

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

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

0 引言

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

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

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

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

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

1 多無人車路徑規劃

1.1 問題描述

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

1.2 數學模型

(1)

(2)

(3)

(4)

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

2 沖突搜索算法

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

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

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

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

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

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

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

圖1 多無人車路徑示意圖

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

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

3 動態沖突搜索算法

3.1 ECBS算法

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

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

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

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

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

(5)

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

3.2 混合A*搜索算法

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

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

圖4 節點擴展示意圖

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

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

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

(6)

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

3.3 動態焦點搜索算法

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

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

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

(7)

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

4 仿真實驗及結果分析

4.1 仿真實驗設置

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

表1 無人車參數設置

4.2 算法性能對比

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

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

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

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

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

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

圖8 算法計算時間曲線

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

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

圖9 路徑成本曲線

5 結論

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

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