基于多指標動態(tài)優(yōu)先級的無人機協(xié)同路徑規(guī)劃

王祝，張夢通，張振鵬，徐廣通

1.華北電力大學(xué)（保定） 自動化系，保定 071003

2.浙江大學(xué) 湖州研究院，湖州 313002

近年來，無人機（Unmanned Aerial Vehicle，UAV）集群在物流配送、電力巡檢等領(lǐng)域發(fā)揮著越來越重要的作用［1-2］。為保證多架無人機在復(fù)雜環(huán)境中高效協(xié)同完成任務(wù)，需要對無人機集群的路徑進行科學(xué)合理的設(shè)計［3-4］。

多無人機協(xié)同路徑規(guī)劃需要在考慮協(xié)同約束的情況下，以最小代價合作完成任務(wù)為目標［5-7］，為多架無人機生成安全可行的飛行路徑。協(xié)同約束的處理是多無人機協(xié)同路徑規(guī)劃的關(guān)鍵，按照約束處理方式的不同，可分為耦合規(guī)劃和解耦規(guī)劃兩類［8］。與耦合規(guī)劃相比，解耦規(guī)劃結(jié)果雖然不能保證路徑最短，但具有顯著的效率優(yōu)勢。

優(yōu)先級規(guī)劃作為解耦規(guī)劃的主要方式［9-10］，最早由Erdmann 和Lozano-Pérez［11］提出，是指在規(guī)劃過程中優(yōu)先級低的個體規(guī)避優(yōu)先級高的個體，以實現(xiàn)集群個體之間避撞約束的解耦。目前，優(yōu)先級解耦規(guī)劃方法在自動導(dǎo)引運輸車（Automated Guided Vehicle，AGV）領(lǐng)域已得到廣泛研究［12-15］。文獻［12］采取粒子群優(yōu)化算法對多AGV 進行路徑規(guī)劃，提出基于時間優(yōu)先級的粒子迭代更新機制，加快了求解收斂速度，但由于優(yōu)先級固定，可能出現(xiàn)某個AGV 長時間等待導(dǎo)致系統(tǒng)效率變低。文獻［13］提出一種基于動態(tài)優(yōu)先級和改進蟻群算法的多AGV 路徑規(guī)劃算法，根據(jù)AGV 電池剩余電量分配優(yōu)先級，提高了AGV 電力使用效率，但仿真環(huán)境簡單，AGV規(guī)模小，當(dāng)規(guī)模增加時，算法效率難以保證。文獻［14］提出基于時間窗的優(yōu)先路徑規(guī)劃算法，計算受影響AGV 與延遲AGV 之間的相對距離來更新優(yōu)先級，解決了多輛AGV 的時延問題。但在無延遲情況下，采用動態(tài)窗口算法規(guī)劃路徑，該方法可能會得到局部極小值。文獻［15］提出事件驅(qū)動的優(yōu)先級更新邏輯，分為2 個優(yōu)先級模塊以應(yīng)對AGV 單機或編隊運動下的優(yōu)先級更新，旨在保證編隊運動及單機通過交叉路口時避免碰撞。其中，在計算通過交叉路口AGV 優(yōu)先級時，賦予更接近公共路點AGV 更高的優(yōu)先級，雖然該方法能解決當(dāng)前沖突，但降低了結(jié)果最優(yōu)性。

在多無人機協(xié)同路徑規(guī)劃方面，文獻［16］提出了一種基于耦合度的優(yōu)先級生成準則，利用無人機群潛在碰撞表示無人機間的耦合程度，提高了規(guī)劃效率，但沒考慮總路程等影響規(guī)劃質(zhì)量的因素，路徑質(zhì)量難以保證。文獻［8］將滾動規(guī)劃與優(yōu)先級規(guī)劃結(jié)合，將無人機群路徑規(guī)劃問題分解成單無人機短時域路徑規(guī)劃問題，增強了算法實時性，但優(yōu)先級固定，存在優(yōu)先級不合理導(dǎo)致集群整體規(guī)劃效率變低的情況。文獻［9］考慮飛行時間短、剩余飛行空間大的無人機成功規(guī)避其他無人機的概率更大，提出了一種飛行時間驅(qū)動的動態(tài)優(yōu)先級解耦機制，提高了收斂速度，但該機制僅考慮了飛行時間對協(xié)同規(guī)劃的影響，沒有充分考慮其他指標，如航路長度、碰撞風(fēng)險等，這些因素對多無人機協(xié)同路徑規(guī)劃的有效性和實用性也具有重要意義。因此，需要綜合考慮多種因素，以制定更加全面、準確的優(yōu)先級協(xié)同路徑規(guī)劃策略。

在優(yōu)先級協(xié)同約束處理機制的基礎(chǔ)上，單機路徑規(guī)劃算法也是影響協(xié)同路徑規(guī)劃效率的關(guān)鍵。針對個體路徑規(guī)劃，Daniel 等［17］將可視線引入A*算法，通過判斷擴展節(jié)點與父節(jié)點之間是否存在可視線，實現(xiàn)了任意角度搜索的Theta*算法。但Theta*算法要對所有遍歷過的節(jié)點進行視線檢查，因此當(dāng)柵格規(guī)模很大時，算法效率較低。為提高路徑規(guī)劃效率，Nash 等［18］對Theta*算法的搜索過程進一步改進，提出了Lazy Theta*算法。Faria 等分別提出基于稀疏網(wǎng)格［19］和基于單一網(wǎng)格［20］的Lazy Theta*算法，減少了存儲空間和計算時間。但這2 種方法僅適用于稀疏環(huán)境的路徑規(guī)劃任務(wù)，對于密集環(huán)境下的路徑規(guī)劃存在不足。徐鵬飛等［21］針對機器人的運動學(xué)特性以及環(huán)境對路徑規(guī)劃結(jié)果的影響，提出了基于環(huán)境優(yōu)化的Lazy Theta*算法，考慮環(huán)境中的動態(tài)障礙物，能夠考慮環(huán)境的動態(tài)變化。但該算法計算復(fù)雜度高，需要較長的計算時間。此外，上述工作都是針對單機的路徑規(guī)劃研究，在多機路徑規(guī)劃方面如何減少基于Theta*的協(xié)同規(guī)劃復(fù)雜度有待研究。

為了更好地權(quán)衡協(xié)同規(guī)劃效率以及路徑質(zhì)量，本文的主要工作包括以下3 點。

1） 提出了一種基于多指標動態(tài)優(yōu)先級的單邊避碰機制，克服固定優(yōu)先級存在的死鎖問題；同時，考慮影響求解效率和路徑質(zhì)量的多個指標，構(gòu)建動態(tài)優(yōu)先級的生成模型，實現(xiàn)無人機優(yōu)先級的合理分配。

2） 針對優(yōu)先級協(xié)同框架下的單機路徑規(guī)劃，提出基于擁堵權(quán)值地圖的Lazy Theta*算法，降低集群間沖突發(fā)生的可能性，提高求解速度。

3） 構(gòu)建“路徑重規(guī)劃+起點等待”的局部沖突組合規(guī)避策略，可針對沖突情況選擇合理的消解策略，以提高集群協(xié)同規(guī)劃的求解速度。

1 問題描述

本節(jié)針對多無人機協(xié)同路徑規(guī)劃問題，完成任務(wù)指標和約束條件的構(gòu)建。本文開展的無人機協(xié)同路徑規(guī)劃研究基于以下假設(shè)：

1） 無人機均以固定速度、高度飛行，因此只考慮二維平面的避障策略。

2） 無人機都能與中央控制器進行通信。

3） 處理局部沖突過程中，無人機存在一條能繞開其他無人機起點到達對應(yīng)目標點的路徑。

1.1 任務(wù)指標

通過多無人機的整體任務(wù)完成時間評判任務(wù)完成質(zhì)量。

整體任務(wù)完成時間T為無人機集群中最大任務(wù)完成時間，反映了無人機系統(tǒng)的任務(wù)完成效率，可表示為

式中：Ti為第i個無人機的任務(wù)完成時間。

1.2 協(xié)同路徑規(guī)劃約束

1） 電量約束

要保證每架無人機都能完成任務(wù)，完成任務(wù)時第i架無人機的剩余電量記為pie，剩余電量pie滿足：

式中：emin為保證無人機安全降落的最低電量。

2） 障礙規(guī)避約束

基于柵格化地圖描述，可用一定數(shù)量的柵格區(qū)域來近似障礙。在無人機飛行過程中，為了保證其能規(guī)避環(huán)境中的障礙，無人機中心與障礙物邊緣柵格中心之間距離D需滿足：

式中：L為無人機軸距；R為柵格分辨率；s為飛行安全距離。

路徑規(guī)劃中將無人機作為質(zhì)點，因此需根據(jù)無人機實際尺寸與飛行安全距離對障礙物邊緣進行膨脹，以保證無人機不會與障礙發(fā)生碰撞。

3） 機間避撞約束

為確保無人機之間不會發(fā)生碰撞，需要考慮機間碰撞約束。因此無人機中心點之間的距離d需滿足：

式中：c為機間安全距離。

2 協(xié)同路徑規(guī)劃算法

2.1 協(xié)同規(guī)劃總體流程

協(xié)同路徑規(guī)劃分為單機路徑規(guī)劃層和多無人機系統(tǒng)協(xié)調(diào)層，單機路徑規(guī)劃層為每架無人機規(guī)劃出與環(huán)境中障礙無碰撞的全局路徑，再經(jīng)過多無人機系統(tǒng)協(xié)調(diào)層完成無人機之間的協(xié)同運動。協(xié)同規(guī)劃總體流程如圖1所示。

圖1 協(xié)同路徑規(guī)劃總體流程圖Fig.1 Flow chart of cooperative path planning

單機路徑規(guī)劃層的流程說明具體見2.4 節(jié)，下面主要對多無人機系統(tǒng)協(xié)調(diào)層的關(guān)鍵步驟進行說明。

1） 路徑點對應(yīng)時刻的近似計算

在解耦框架下，速度為v=[vxvy]，時間步長為dt，第i架無人機在第k個時間步長的橫縱坐標可由式（5）計算，并將第i架無人機的橫縱坐標集合分別存放到飛行橫縱坐標集合Φx=｛x1，x2，…，xn｝，Φy=｛y1，y2，…，yn｝。

式中：β為上一時刻位置到當(dāng)前位置的方向角。

2） 沖突判斷和優(yōu)先級更新

考慮機間協(xié)同約束，每架無人機根據(jù)上一步迭代過程中其他無人機在對應(yīng)時刻的位置，來計算局部沖突。將沖突點位置以及發(fā)生沖突的無人機編號分別存放到Φpos和ΦUAV集合中。計算在同一點存在機間碰撞的無人機的優(yōu)先級，完成更新。

3） 沖突消解

在優(yōu)先級引導(dǎo)的單邊避碰機制下，首先系統(tǒng)會先解決起點沖突，然后根據(jù)預(yù)設(shè)的避障策略選擇規(guī)則，令優(yōu)先級低的無人機采取相應(yīng)避障策略。一旦完成一輪沖突消解，系統(tǒng)會進行下一輪迭代，直至獲得滿足全部協(xié)同路徑規(guī)劃約束的飛行路徑。

2.2 多指標動態(tài)優(yōu)先級更新策略

在協(xié)同規(guī)劃中，處理機間碰撞的方式直接影響到規(guī)劃效率和結(jié)果。由于每架無人機只能獲得他機上一次迭代的位置信息，在相互避碰機制下，若沖突雙方均采取路徑重規(guī)劃的策略，可能會出現(xiàn)路徑反復(fù)振蕩。為解決這個問題，采用動態(tài)優(yōu)先級引導(dǎo)的單邊避碰機制，通過不斷更新優(yōu)先級順序，使高優(yōu)先級無人機具有更大的行駛權(quán)利，避免與低優(yōu)先級無人機發(fā)生碰撞。此外，不同的優(yōu)先級計算方法決定了局部沖突發(fā)生時的不同決策方案，對規(guī)劃速度和質(zhì)量有重要影響。

本文動態(tài)優(yōu)先級的確定考慮多個指標，具體包括以下4 個。

1） 碰撞風(fēng)險

使用沖突次數(shù)作為指標衡量無人機之間的碰撞風(fēng)險。各無人機之間的沖突次數(shù)計算公式如式（6）所示：

式中：Ci(ΦUAV)為編號為i的無人機在沖突無人機集合ΦUAV中出現(xiàn)的次數(shù)即沖突次數(shù)；D(ΦUAV)為沖突無人機集合長度。遍歷ΦUAV中每個元素，若等于i，沖突權(quán)值δ為1，Ci(ΦUAV)累加，直至遍歷結(jié)束。

2） 總路程S

無人機飛行路徑的累計距離。

3） 等待時間Tw

無人機為消解機間沖突執(zhí)行等待策略所等待的時間步長。

4） 剩余路程比Sr

即發(fā)生沖突時的剩余路程Sc和總路程S的比值，其表達式為

優(yōu)先級更新策略的確定分為如下2 個部分。

1） 當(dāng)碰撞風(fēng)險不同時，無人機的碰撞風(fēng)險越大，優(yōu)先級越低。

沖突次數(shù)越多代表對應(yīng)無人機碰撞風(fēng)險越大。此時令碰撞風(fēng)險大的個體執(zhí)行規(guī)避策略有更大概率規(guī)避更多無人機。因此具有較高碰撞風(fēng)險的無人機被賦予較低的優(yōu)先級。

2） 當(dāng)碰撞風(fēng)險相同時，考慮總路程S、等待時間Tw、剩余路程比Sr，構(gòu)建綜合多指標的優(yōu)先級計算模型，如式（8）所示。其中，無人機的綜合指標Pi越大，其優(yōu)先級越高。

式中：α、β、γ為對應(yīng)指標的影響權(quán)重；、和為歸一化后的各指標取值。其計算公式分別如式（9）～式（11）所示：

為優(yōu)化任務(wù)完成效率，將更高的飛行權(quán)利分配給符合以下條件的無人機：總路程長、等待時間長、剩余路程比大?？偮烦淘介L的無人機需要更長的時間來完成任務(wù)，但其調(diào)整空間較小，因此其優(yōu)先級較高。同時，過多地讓同一無人機執(zhí)行等待策略會延長整體任務(wù)完成時間，降低系統(tǒng)執(zhí)行效率，因此等待時間越長的無人機也具有較高的優(yōu)先級。而剩余路程比大的無人機具有更多的剩余飛行距離，賦予其更高的飛行權(quán)利有助于降低整體的飛行時間。

各個指標的權(quán)重α、β、γ選用層次分析法（Analytic Hierarchy Process，AHP）來確定，具體做法如下所示。

1） 構(gòu)建如圖2所示的層次模型。

圖2 層次模型Fig.2 Hierarchical model

2） 構(gòu)造表1所示的比較判別對矩陣A。

表1 比較判別對矩陣Table 1 Pairwise comparison discriminant matrix

3） 權(quán)重計算

由各準則層對目標層權(quán)重aij根據(jù)式（12）計算特征向量的近似值Wi，再將特征向量標準化后得到式（13）所示的權(quán)重向量W。

4） 一致性檢驗

為了判斷比較判別對矩陣是否存在邏輯性矛盾，需要通過式（14）進行一致性檢驗：

式中：λmax為最大特征根；N為比較判別對矩陣階數(shù)；CI為一致性指標；平均隨機一致性指標RI可根據(jù)對比矩陣的階數(shù)給出［22］。當(dāng)隨機一致性比率CR＜0.1 時認為計算所得的權(quán)重是正確且合理的。

2.3 沖突組合規(guī)避策略

針對局部機間碰撞規(guī)避，無人機一般采取路徑重規(guī)劃策略尋找規(guī)避沖突點的路徑，受AGV采用等待策略消解機間沖突啟發(fā)，等待策略可以避免大范圍路徑重規(guī)劃從而降低計算復(fù)雜度?？紤]到多旋翼無人機能夠自主懸停，在安全點懸停等待一段時間同樣能規(guī)避沖突?；诘? 節(jié)中的假設(shè)3）可知，各個無人機的起點均可以作為安全點，因此本文采取“路徑重規(guī)劃+起點等待”的組合策略規(guī)避機間沖突。

采用路徑重規(guī)劃的方式協(xié)調(diào)能力強，但規(guī)劃耗時相對較長；采用起點等待的策略，由于無人機無需考慮避障，因此規(guī)劃耗時短，而對于圖3所示的沖突，起點等待策略下無人機飛行時間延長。因此需要結(jié)合2 種方式的特點，根據(jù)沖突情況建立策略選擇公式，可表示為

式中：θ為沖突向量之間的夾角，當(dāng)θ≥180°-ε時，執(zhí)行路徑重規(guī)劃策略，否則執(zhí)行起點等待策略；ε為容許值，可人為給定，以權(quán)衡協(xié)調(diào)能力及求解速度。

在局部碰撞檢測中，需要將每架無人機ui（i=1，2，…，n）與其他無人機uj（j≠i）路徑進行比較，即根據(jù)式（3）依次計算同一時刻Tk時ui的路徑點pki和uj的路徑點pkj之間的距離是否滿足協(xié)同約束，將不滿足協(xié)同約束的坐標集合Φpos記為：

沖突無人機集合可表示為

沖突向量之間的夾角θ，可表示為

當(dāng)θ=180°時，表示沖突無人機對沖飛行，若執(zhí)行起點等待策略，會出現(xiàn)路徑更新后依然存在沖突的情況，起點等待策略效率變低，此時就應(yīng)選擇路徑重規(guī)劃策略。

另外，有一種特別情況需要指出，如圖4所示的起點沖突，起點沖突定義為：假設(shè)UAVi的優(yōu)先級大于UAVj的優(yōu)先級，且UAVj的起點在UAVi的路徑上。

圖4 起點沖突示意圖Fig.4 Schematic diagram of start-point conflict

在多指標動態(tài)優(yōu)先級的單邊避碰機制下，由于高優(yōu)先級個體會無視優(yōu)先低個體，當(dāng)發(fā)生起點沖突時，由于UAVi的優(yōu)先級更高，UAVi無視UAVj，但s2并非是安全點，UAVj采取起點等待策略無法成功規(guī)避沖突。

因此，當(dāng)發(fā)生起點沖突時，將優(yōu)先級高個體加入待調(diào)整無人機集合，執(zhí)行沖突規(guī)避策略。

利用組合策略規(guī)避沖突偽代碼如算法1所示，具體流程如下所示。

步驟1（第1～3 行）確定規(guī)避策略

遍歷沖突無人機集合，r_flag 為重規(guī)劃標志位，當(dāng)θ≥180°-ε時，r_flag 為True，即執(zhí)行重規(guī)劃策略。否則執(zhí)行起點等待策略。

步驟2（第4～13 行）執(zhí)行重規(guī)劃策略

算法1 沖突規(guī)避偽代碼Input：待調(diào)整無人機集合Φ?adj、沖突無人機集合ΦUAV、沖突位置集合Φpos Output：飛行橫縱坐標集合Φx、Φy 1.Φwait←? ∥記錄執(zhí)行過起點等待策略的無人機集合2.for i，uav in enumerate（ΦUAV）：3.（r_flag，col_p）←select_strategy（Φpos）4. if r_flag：5. rpath_uav←Φ?adj［i］6. k←uav.index（rpath_uav）7. Col_loc←Φpos［i］［0］［k］ ∥待避開位置8. new_path←Exe_replan_strategy（rpath_uav，Col_loc） ∥重規(guī)劃9. if not new_path： ∥無法重新路徑規(guī)劃10. if Φ?adj［i］ not inΦwait：11. （Φx，Φy）←Exe_wait_strategy（Φ?adj［i］）12. else：13. （Φx［Φ?adj［i］］，Φy［Φ?adj［i］］）←draw_pos（new_path）14. else：15. if Φ?adj［i］ not in Φwait：16. （Φx，Φy）←Exe_wait_strategy（Φ?adj［i］） ∥起點等待17. if not col_detection（）［0］：18. break 19.return Φx，Φy

令連續(xù)沖突點的第一個為待規(guī)避位置，將其視為障礙后進行路徑規(guī)劃，生成新路徑new_path，若無法生成新路徑，即new_path 為空，則執(zhí)行起點等待策略規(guī)避沖突。

步驟3（第14～16 行）執(zhí)行起點等待策略

為避免在同一輪迭代過程中某無人機重復(fù)執(zhí)行起點等待策略，Φwait記錄一輪迭代中執(zhí)行過起點等待策略的無人機。判斷當(dāng)前無人機在本輪迭代是否執(zhí)行過起點等待策略，若沒有，在起點等待一個時間步長dt。

步驟4（第17～19 行）判斷是否存在沖突

處理完本輪迭代中所有沖突后，判斷系統(tǒng)是否還存在沖突，若系統(tǒng)中已無沖突，則跳出循環(huán)，否則繼續(xù)下一輪迭代。

2.4 單機路徑規(guī)劃的改進Lazy Theta*算法

在協(xié)同規(guī)劃總體流程中，每架無人機需要進行路徑規(guī)劃以達到任務(wù)點，Lazy Theta*算法［17］作為A*的改進算法，能實現(xiàn)任意角度的路徑規(guī)劃，消除了A*算法結(jié)果存在的“Z 字”抖動現(xiàn)象。

本文為了降低沖突發(fā)生的可能性，在Lazy Theta*算法基礎(chǔ)上加入擁堵權(quán)值地圖（記為Weighted Map Lazy Theta*，簡稱WM-Lazy Theta*），使無人機在單機規(guī)劃層能避開“擁堵路段”，提高飛行效率。

WM-Lazy Theta*算法流程如圖5所示，下面主要對本文改進所涉及的擁堵權(quán)值地圖更新和節(jié)點代價計算進行說明。

圖5 WM-Lazy Theta*算法流程圖Fig.5 Flowchart of WM-Lazy Theta*

1） 擁堵權(quán)值地圖初始化和構(gòu)建

將柵格地圖劃分為m個擁堵檢測區(qū)域，構(gòu)成擁堵權(quán)值地圖。擁堵檢測區(qū)域大小影響路徑規(guī)劃的效率及質(zhì)量，若將每個柵格都作為擁堵檢測區(qū)域，會增加路徑規(guī)劃復(fù)雜度；若合并過多的柵格構(gòu)成大的擁堵檢測區(qū)域，則會導(dǎo)致規(guī)劃失去更優(yōu)的路徑結(jié)果。文中的擁堵檢測區(qū)域劃分方法如下。

步驟1確定最小擁堵檢測區(qū)域

在構(gòu)建柵格地圖時，柵格分辨率與無人機軸距相同，即為L。每個柵格即為一個最小擁堵檢測區(qū)域。

步驟2設(shè)置比例因子τ

擁堵檢測區(qū)域面積為WH τ2，W和H分別為柵格地圖的寬和高。

步驟3確定擁堵檢測區(qū)域柵格數(shù)量

根據(jù)擁堵區(qū)域面積及柵格分辨率即可求得每個擁堵檢測區(qū)域包含柵格數(shù)量為WH(τ2L2)。

步驟4為每個擁堵檢測區(qū)域編號排序

從柵格地圖左上角開始，按照從左到右、從上到下的順序遍歷所有的擁堵檢測區(qū)域，分配從0 開始的連續(xù)整數(shù)作為編號。

每規(guī)劃好一條路徑后更新?lián)矶聶?quán)值地圖：記錄該路徑在每塊區(qū)域的占有時間（進入和離開時間），并給定初始擁堵權(quán)值。若第i塊區(qū)域（i=1，2，…，m）存在重復(fù)時間段，對應(yīng)的擁堵權(quán)值進行疊加。

2） 節(jié)點代價計算

在擴展節(jié)點的過程中，計算到達當(dāng)前節(jié)點的時間，查詢當(dāng)前節(jié)點所在區(qū)域的占有時間。若當(dāng)前時間在占有時間內(nèi)，則令M等于占有時間對應(yīng)的擁堵權(quán)值，否則令M=0。節(jié)點價值的評估函數(shù)表示為

式中：f為節(jié)點的代價值；g為累計代價值，表示當(dāng)前點到起點的路徑長度；h為估計代價值，表示當(dāng)前點到終點的路徑長度；w為估計代價值的權(quán)重，w越大表示節(jié)點越傾向于“接近終點”。

圖6 給出了一個權(quán)值地圖的示例。想定為一個20×20 的柵格環(huán)境，每個柵格大小為1 m×1 m，比例因子設(shè)為10，因此將2×2 的柵格劃分為一個擁堵檢測區(qū)域，飛行速度為0.5 m/s，給定10 組起始點和終點，利用WM-Lazy Theta*按序號依次規(guī)劃路徑，圖6 是規(guī)劃完成后t=10 s 時的擁堵權(quán)值地圖。若繼續(xù)添加任務(wù)點進行路徑規(guī)劃，則在現(xiàn)有擁堵權(quán)值地圖基礎(chǔ)上進行節(jié)點代價計算。

圖6 t=10 s 時擁堵權(quán)值地圖Fig.6 Congestion weighted map at t=10 s

圖7 為引入擁堵權(quán)值地圖后擴展節(jié)點示意圖。當(dāng)規(guī)劃第4 條路徑時，P點擴展的節(jié)點Pnext1、Pnext2占有時間分別為（9.4，12.2）s、（9，11）s，t=10 s 時對應(yīng)區(qū)域內(nèi)柵格的擁堵權(quán)值M=3，而在Pnext節(jié)點的占有時間（9，11）s 內(nèi)所在區(qū)域沒有被訪問過，M記為0。分別對這3 個節(jié)點計算代價值得到fnext2＞fnext1＞fnext。因此Pnext為最優(yōu)節(jié)點，達到避開擁堵區(qū)域的效果。

圖7 引入擁堵權(quán)值地圖后擴展節(jié)點Fig.7 Expanded nodes after introducing congestion weighted map

3 仿真試驗與分析

為驗證所提算法的可行性和優(yōu)勢，開展多無人機協(xié)同路徑規(guī)劃仿真對比試驗。試驗中，無人機飛行區(qū)域為20 m×12 m，設(shè)置柵格規(guī)模為50×30，使柵格分辨率與無人機軸距相同為0.4 m，每架無人機飛行速度均相同且恒定，設(shè)置為0.2 m/s。容許值ε=30°，最低安全電量emin設(shè)為5%，機間安全距離c設(shè)為0.1 m，飛行安全距離s設(shè)為0.1 m。

為模擬無人機運動過程中的電量損耗，采用下式表示t時刻的電量Et。

式中：stepw為等待步長；Es為無人機的起始電量。

3.1 協(xié)同路徑規(guī)劃算法有效性驗證

分別選取無人機數(shù)量為5、10、15，開展協(xié)同路徑規(guī)劃算法有效性試驗。擁堵權(quán)值地圖設(shè)置中，比例因子設(shè)為10，即每個區(qū)域大小為5×3。

首先確定多指標動態(tài)優(yōu)先級的指標權(quán)重。根據(jù)文獻［23］所示的標度方法構(gòu)造表2所示的比較矩陣，利用式（12）和式（13）求得各項指標權(quán)重。α=0.599 5，β=0.274 9，γ=0.122 9，再通過式（14）計算CR得CR=0.039＜0.1，通過一致性檢驗，說明該權(quán)重合理。

表2 權(quán)重計算兩兩比較矩陣Table 2 Weight calculation pairwise comparison matrix

圖8 給出了5、10、15 架無人機的協(xié)同路徑規(guī)劃結(jié)果圖。圖9 給出了3 種規(guī)模下最小距離變化曲線。針對障礙規(guī)避約束，根據(jù)仿真試驗設(shè)定可得式（3）中()L+R2+s=0.5 m。由圖9（a）可知，3 種規(guī)模下，無人機與障礙物之間的最小距離在任意時刻均大于0.5 m，因此，全局路徑滿足障礙規(guī)避約束。從圖9（b）可知，3 種規(guī)模下機間最小距離均大于無人機軸距與機間安全距離之和0.5 m，滿足協(xié)同約束。圖10 展示了3 種集群規(guī)模下完成任務(wù)的剩余電量，由圖10 可知，每個無人機的剩余電量均大于最低安全電量5%，即滿足電量約束。

圖9 3 種規(guī)模下最小距離變化曲線Fig.9 Minimum distance for three swarm-scale scenarios

圖10 3 種集群規(guī)模下完成任務(wù)的剩余電量Fig.10 Remained electricity for three swarm-scale scenarios

3.2 對比試驗

本文提出的規(guī)劃算法主要包含3 個改進部分：多指標動態(tài)優(yōu)先級、WM-Lazy Theta*和組合規(guī)避策略，為驗證每一部分的改進效果，分別開展優(yōu)先級方法對比、單機路徑規(guī)劃方法對比和規(guī)避策略對比試驗。

3.2.1 多指標動態(tài)優(yōu)先級效果分析

為了分析本文提出的多指標動態(tài)優(yōu)先級方法的求解效果，將其與飛行時間驅(qū)動的動態(tài)優(yōu)先級方法［9］進行對比。

表3 為3 種無人機規(guī)模下，2 種優(yōu)先級協(xié)同路徑規(guī)劃算法求得的的任務(wù)完成時間與規(guī)劃耗時。由表3 可知，無人機規(guī)模為5 架時，多指標動態(tài)優(yōu)先級相比飛行時間驅(qū)動的動態(tài)優(yōu)先級雖然任務(wù)完成時間略有增加，但規(guī)劃耗時減少85.0%，當(dāng)規(guī)模增加至10、15 架時，多指標方法不僅任務(wù)完成時間較飛行時間驅(qū)動方法減少了8.23、3.8 s，規(guī)劃耗時分別減少74.4%和47.8%。

表3 不同優(yōu)先級方法的求解結(jié)果對比Table 3 Comparison of results of two priority-based methods

表4 給出了5、10、15 架無人機協(xié)同規(guī)劃過程中的優(yōu)先級計算結(jié)果示例，來分析2 種優(yōu)先級確定方法對規(guī)劃結(jié)果的影響。表4 中｛i，j｝表示無人機i和無人機j存在沖突，且為無人機i優(yōu)先級小于無人機j。圖11～圖13 分別給出了3 個示例下利用飛行時間驅(qū)動方法和多指標方法計算優(yōu)先級過程中的指標值。

圖11 5 架無人機第4 次迭代時的優(yōu)先級指標值Fig.11 Priority index values at 4th iteration for 5 drones

由圖11（a）可以看出，在5 架無人機協(xié)同規(guī)劃的第4 次迭代中，對于飛行時間驅(qū)動的動態(tài)優(yōu)先級方法，由于UAV3的總飛行時間最短，因此UAV3在與其他無人機發(fā)生沖突時，總是UAV3執(zhí)行規(guī)避策略。在5 架協(xié)同規(guī)劃的后續(xù)迭代過程中，也存在上述情況，使得UAV3的等待時間到達20 s，任務(wù)完成時間至104.57 s，但由表3 可知，該規(guī)模下整體任務(wù)完成時間為112.10 s，因此該規(guī)模下整體任務(wù)完成時間并沒有受到影響。

而對于多指標動態(tài)優(yōu)先級方法，發(fā)生沖突的無人機中，UAV5與其他2 架無人機存在沖突，其余均為與其他1 架無人機發(fā)生沖突，因此UAV5碰撞風(fēng)險最大，優(yōu)先級最低。其余4 架無人機的優(yōu)先級由圖11（b）中無人機各個指標的歸一化值代入式（8）得出：P1=0.524 5，P2=0.305 7，P3=0.07，P4=0.65。設(shè)定的指標權(quán)重中，總路程與等待時間占主導(dǎo)地位且當(dāng)前迭代步中各無人機的等待時間均為0 s，因此4 架無人機優(yōu)先級的確定是由總路程確定。

由于整體任務(wù)完成時間并未受等待時間影響，此時2 種算法的主要區(qū)別在于對碰撞風(fēng)險大的無人機的處理，多指標優(yōu)先級優(yōu)先令碰撞風(fēng)險大的無人機做出規(guī)避，在任務(wù)時間上最優(yōu)性受到限制，但縮短了規(guī)劃耗時。

由圖12（a）可知，在10 架無人機協(xié)同規(guī)劃的第10 次迭代示例中，對于飛行時間驅(qū)動的動態(tài)優(yōu)先級方法，UAV5的總飛行時間最短。當(dāng)UAV5與其他無人機發(fā)生沖突，始終令UAV5執(zhí)行避障策略，導(dǎo)致UAV5等待時間達到43 s，最終使整體任務(wù)時間延長至128.05 s。而對于多指標動態(tài)優(yōu)先級方法，首先由沖突次數(shù)計算各無人機的碰撞風(fēng)險：UAV8＞UAV9=UAV1＞UAV6。若碰撞風(fēng)險不同優(yōu)先級即可確定。優(yōu)先級相同時，將圖12（b）中各指標歸一化值代入式（8）得到：P1=0.689 2、P6=0.786 6、P8=0.039 3、P9=0.845 5。多指標動態(tài)優(yōu)先級考慮了等待時間，因此即使UAV1的總路程大于UAV9，由于UAV9等待時間過長，最終計算結(jié)果P9＞P1。多指標動態(tài)優(yōu)先級解決了某架無人機長時間等待的問題，于是多指標動態(tài)優(yōu)先級的的整體任務(wù)完成時間及規(guī)劃時間較飛行時間驅(qū)動的動態(tài)優(yōu)先級都有所降低。

圖12 10 架無人機第10 次迭代時的優(yōu)先級指標值Fig.12 Priority index values at 10th iteration for 10 drones

當(dāng)無人機數(shù)量增加至15 架時，飛行時間驅(qū)動的動態(tài)優(yōu)先級會根據(jù)圖13（a）中的總飛行時間計算優(yōu)先級，持續(xù)令同一無人機做出規(guī)避策略的問題難以避免，進而發(fā)生某一無人機長時間等待的情況。多指標動態(tài)優(yōu)先級在確定UAV11碰撞風(fēng)險最大后，根據(jù)圖13（b）所示的歸一化數(shù)據(jù)計算各無人機優(yōu)先級數(shù)值：P2=0.640 2、P9=0.937 1、P11=0.563 5、P14=0.061 5、P15=0.476 5，更加準確地量化了各無人機之間的協(xié)調(diào)性，因此多指標動態(tài)優(yōu)先級在整體任務(wù)完成時間和規(guī)劃耗時也均優(yōu)于飛行時間驅(qū)動的動態(tài)優(yōu)先級。

圖13 15 架無人機第22 次迭代時的優(yōu)先級指標值Fig.13 Priority index values at 22nd iteration for 15 drones

可見，多指標動態(tài)優(yōu)先級方法相比飛行時間驅(qū)動的動態(tài)優(yōu)先級方法，不僅可以提高多無人機系統(tǒng)的協(xié)調(diào)能力，還能夠提高多無人機協(xié)同路徑規(guī)劃的計算效率。

3.2.2 WM-Lazy Theta*算法效果分析

為了評估WM-Lazy Theta*算法在協(xié)同規(guī)劃中的性能表現(xiàn)，在不同無人機規(guī)模下，將其與Lazy Theta*進行對比試驗。試驗中，將柵格地圖劃分為100 塊區(qū)域，在每個固定大小的擁堵檢測區(qū)域內(nèi)，有3 架及以上無人機在同一時間段經(jīng)過則認定為“擁堵區(qū)域”。

表5 給出了在無人機規(guī)模分別為5 架、10 架、15 架時，2 種單機規(guī)劃算法下的整體任務(wù)完成時間及規(guī)劃耗時。由表5 可知，雖然在整體完成任務(wù)時間上會略有增加，但WM-Lazy Theta*方法的求解速度較Lazy Theta*方法分別提高了59.6%、67.3%、65.0%。對比表明WM-Lazy Theta*通過考慮不同區(qū)域的擁堵情況，能夠更好地協(xié)調(diào)不同無人機之間的沖突，從而更快速地完成多機系統(tǒng)規(guī)劃全局路徑規(guī)劃。

表5 不同單機規(guī)劃算法的求解結(jié)果對比Table 5 Comparison of results of different UAV planning algorithms

圖14 展示了15 架無人機協(xié)同時2 種算法所規(guī)劃路徑對應(yīng)的擁堵區(qū)域結(jié)果。由圖14 可知，WM-Lazy Theta*方法中“擁堵區(qū)域”相比Lazy Theta*大幅減少，且根據(jù)擁堵持續(xù)時間統(tǒng)計，改進算法對應(yīng)“擁堵區(qū)域”的持續(xù)時間降低了72.1%。上述對比結(jié)果表明，WM-Lazy Theta*方法在規(guī)劃時能夠是使得無人機盡可能避開“擁堵路段”，擁有更快的協(xié)同規(guī)劃求解速度。

圖14 15 架無人機的擁堵區(qū)域?qū)Ρ菷ig.14 Comparison of congestion area for 15 UAVs

3.2.3 組合策略效果分析

為驗證組合策略對規(guī)劃的性能影響，在3 種無人機規(guī)模下，開展組合策略、重規(guī)劃策略、起點等待策略對比試驗。表6 為3 種規(guī)模下3 種算法的任務(wù)完成時間和規(guī)劃耗時對比。通過表6 可知，僅有5 架無人機時，3 種策略下無人機整體任務(wù)完成時間幾乎相同。然而，隨著規(guī)模的增加，只執(zhí)行等待策略的無人機的整體任務(wù)完成時間明顯高于另外2 種策略。因此在整體任務(wù)完成時間方面，重規(guī)劃策略最佳，而組合策略和起點等待策略的整體任務(wù)完成時間分別增加了2.81%、1.70%、1.60% 和4.70%、16.3%、28.5%。由于執(zhí)行等待策略的時間復(fù)雜度更低，因此在規(guī)劃耗時上，起點等待策略表現(xiàn)出最優(yōu)的性能，較組合策略、重規(guī)劃策略分別提高86.4%、85.5%、50.0%和90.5%、90.4%、70.3%。

表6 3 種規(guī)避策略的求解結(jié)果對比Table 6 Comparison of results of three avoidance strategies

通過上述分析可知，安全點等待策略求解速度快，但協(xié)調(diào)能力差，重規(guī)劃策略的協(xié)調(diào)能力好，但求解速度慢，而組合策略算法的協(xié)調(diào)能力與重規(guī)劃策略相當(dāng)?shù)那闆r下，求解速度得到提高。在避障過程中，求解速度和協(xié)調(diào)能力存在一定矛盾關(guān)系。求解速度快但可能會在協(xié)調(diào)能力上犧牲一定的表現(xiàn)，而協(xié)調(diào)能力強可能導(dǎo)致求解速度較慢。因此，在選擇避障策略時，需要綜合考慮實際場景的需求，以確定具體采用哪種算法。如果在緊急情況下需要快速應(yīng)對，那么起點等待策略可能是更好的選擇；而如果需要較高的協(xié)調(diào)能力，那么重規(guī)劃策略算法可能更為合適。在一些需要求解速度和協(xié)調(diào)能力平衡的場景下，組合策略算法則可能成為一個更好的選擇。

4 結(jié) 論

本文針對多無人機協(xié)同路徑規(guī)劃提出了一種基于優(yōu)先級的解耦規(guī)劃方法，主要包括：① 發(fā)展了基于多指標動態(tài)優(yōu)先級的單邊避碰機制，并構(gòu)建了動態(tài)優(yōu)先級生成模型，實現(xiàn)了無人機優(yōu)先級的合理分配；② 提出了基于擁堵權(quán)值地圖的Lazy Theta*算法，降低全局路徑中發(fā)生集群間沖突的概率，提高求解速度；③ 構(gòu)建了“路徑重規(guī)劃+起點等待”的局部沖突組合規(guī)避策略，在保證協(xié)調(diào)能力的同時，提高了求解速度。最后通過對比試驗驗證了在不同無人機規(guī)模下，多指標動態(tài)優(yōu)先級與WM-Lazy Theta*均提高了規(guī)劃質(zhì)量及求解速度，組合規(guī)避策略在保證協(xié)調(diào)能力與路徑重規(guī)劃策略相當(dāng)?shù)那闆r下，實現(xiàn)了求解速度提升，證明了本文提出方法的有效性。