一種雙階段多智能體路徑規劃算法

李慶華, 王佳慧, 李海明, 馮 超*

(1.齊魯工業大學(山東省科學院)電子信息學院(大學物理教學部)， 濟南 250353； 2.濟南市人機智能協同工程實驗室， 濟南 250353；3.齊魯工業大學(山東省科學院)電氣工程與自動化學院， 濟南 250353 )

隨著無人化工作的復雜性、動態性增強，多智能體路徑規劃問題逐漸成為研究熱點，諸多學者對這一領域做了有益的探索[1-2]。多智能體路徑規劃算法的定義為：在響應規劃請求時，要求每個智能體均可規劃出連接各自起點和終點的路徑，同時確保智能體之間無碰撞，且與障礙物無碰撞。根據多智能體協調控制方式可分為耦合式方法和解耦式方法。

耦合式方法是將所有智能體視為一個整體對其進行路徑規劃，尋找無碰撞路徑。Ferreira等[3]提出一種雙向通訊鏈路的思想，通過建立包含地層幾何形狀及其位置的勢能場來實現水下多機器人的協調。Liu等[4]提出一種基于A-Star(A*)算法和包含環境表示的遺傳算法實現多機器人任務分配和路徑規劃。耦合式方法[5]需要共享空間，并將多智能體的信息傳遞給一個系統進行控制，不需要特定的協作算法，但該類算法普遍存在配置空間維度高、計算復雜的問題。

解耦式方法是將每個智能體視為獨立個體，通過構建協作算法修改已規劃的獨立路徑，從而獲取無碰撞路徑。Settembre等[6]提出了一種分散的協作態勢評估方法，當一個機器人局部認為某個特定的計劃應該被執行時，就會發送該計劃的建議給其團隊成員，該算法成功地平衡了協作感知和共享大量信息的成本。晁永生等[7]提出一種兩階段解耦方法，首先利用A*算法為每個智能體規劃出在靜態環境下無碰撞的路徑，再通過修改不同機器人的沖突路徑實現機器人間的協調配合。該方法提高了搜索效率，但該方法的不足在于A*算法不能保證找到一條最短路徑[8]。解耦式方法存在智能體間相互干涉，協作算法可實現局部最優，但無法確保全局最優[8]。

目前常用的路徑規劃算法主要有：通過模擬自然進化過程搜索最優解的遺傳算法；通過搜索勢能函數下降方向尋找路徑的人工勢場法(artifical potential field method, APFM)；基于啟發函數搜索的A*算法；基于采樣搜索的概率路線圖(probabilistic roadmap，PRM)方法。諸多學者對路徑規劃算法繼續探索[9-10]。在規劃路徑時，遺傳算法具有良好的全局搜索能力，但搜索速度較慢[11]；人工勢場法能夠得到最優路徑且路徑平滑，但其易陷入局部最優[8]；A*算法基于啟發式函數可快速地導向目標節點[12],但其路徑并不是最優[8]。PRM算法是概率完備的但不是最優[13]。相對于其他算法，快速擴展隨機樹(rapidly exploring random trees，RRT)算法[14]計算復雜度較低，在不使用配置空間中障礙物顯式信息的情況下找到解決方案，但它不能確保路徑搜索的漸進最優[15]。

針對此問題，Karaman等[15]提出了RRT*算法，通過增加選擇父節點和重布線過程，保證快速找到初始路徑，并隨著樣本數量的增加對路徑進行優化。該方法具有概率完備性，同時確保了路徑搜索的漸近最優。RRT*算法在理論上可以得到一個最優解，但在有限時間內由于算法收斂速度慢，最優解的計算不能在有限時間內完成。Nasir等[16]引入智能采樣和路徑優化策略，提出了RRT*-SMART(smart rapidly-exploring random trees star)算法，加快算法的收斂速度。RRT*-SMART算法在找到起點和終點間可行路徑后，從子節點開始不斷判斷是否可直接連接先輩節點。在優化過程中，障礙物附近如果出現無法直接優化的錨點，在錨點附近增加采樣，找到更優路徑，但該算法的環境自適應性差。Noreen等[17]提出基于連通區域、目標有界采樣和路徑優化的RRT*-AB(RRT*-adjustable bounds)算法。在連通區域的邊界上進行目標偏置有界抽樣，尋找初始路徑。這種采樣策略降低了RRT算法的采樣盲目性。一旦找到路徑，通過節點抑制和集中有界抽樣的方法逐步優化路徑。張偉民等[18]提出一種基于目標約束采樣和目標偏置擴展的改進RRT*(goal-bias constrained sampling and extending RRT*，GCSE-RRT*)算法。在采樣階段設置目標偏置概率，在一定概率下選擇目標點作為采樣點，在隨機采樣時對采樣點位置約束，提高采樣階段的目標導向性；擴展新節點時由隨機點和目標點共同決定，加快算法搜索速度。相對來說，GCSE-RRT*算法提高了節點利用率，占用內存更少，并且運行時間更短，但規劃出的路徑不是最優。

現基于回溯思想提出一種改進的RRT*(back tracking RRT*，BT-RRT*)算法，并結合自適應局部避障策略解決多智能體的路徑規劃問題。算法分為兩個階段，全局路徑規劃階段，各智能體在不考慮其他智能體的前提下，在靜態環境中規劃各自無碰撞路徑；在協作避障階段，智能體沿著已規劃好的路徑移動，基于自適應局部避障策略，實時對動態障礙物和其他智能體進行避障。

1 基礎知識

給定工作空間W，其中障礙物區域為Wobs，無障礙物區域為Wfree。在RRT*算法中，構建的樹為T=(V,E)，其中V為節點集合，E為連接節點的邊集合。定義Ψ(q1,q2)為節點q1、q2間的路徑，c(q1,q2)為隨機樹中兩個節點q1、q2之間的路徑成本，c(q,T)為起始點朝向節點q的路徑成本，路徑成本用歐氏距離來計算：

(1)

式(1)中：(x1,y1)、(x2,y2)為節點q1、q2的坐標。

定義1：父節點的階。將距離新節點最近的節點定義為1階父節點，則上一父節點為其2階父節點。則經m次回溯后，新節點可回溯至其m階父節點。

1.1 RRT*算法

擴展樹以起始點qinit為根節點開始擴展，首先在整個搜索空間中采取隨機的方式生成隨機點qrand，然后遍歷當前已有的樹節點，從中尋找距離qrand最近的節點qnearest，在點qnearest處向qrand延伸步長p后得到新節點qnew。如果新節點qnew在無障礙區域，則以新節點為圓心，r為半徑(r>0)，得到落在這個區域內的節點的集合Q={qnearest,q1,q2}，如圖1(a)所示。如圖1(b)所示，遍歷集合內所有點，選擇起始點qinit到達新節點qnew時的路徑成本最小的節點為新節點qnew的最佳父節點，從而進行重新布線。最后為集合內其他節點進行重新布線，進一步使得隨機樹節點間連接的代價盡量小，如圖1(c)所示，選擇節點q1、q2的最佳父節點，達到優化效果。起始點朝向節點qnew的成本代價與qnew、q2之間的路徑成本的和小于起始點朝向節點q1的成本代價與q1、q2之間的路徑成本的和，因此節點q2的最佳父節點為qnew，如圖1(d)所示。

圖1 RRT*算法重新布線過程

1.2 BT-RRT*算法

RRT*算法是在RRT算法的基礎上添加一個尋找最佳父節點的過程，并對隨機樹重新布線，從而在給定環境中生成較優路徑。為了縮短RRT*算法的路徑代價和計算成本，提出了回溯布線法。表1給出了BT-RRT*算法的主要步驟，在給定無障礙物區域中隨機采樣一個節點qrand，由Nearest函數得到隨機樹T中距離隨機點qrand最近的節點qnearest，再由Steer函數獲得沿qrand方向上距離節點qnearest長度為p的節點qnew，確定新節點后，若新節點在無障礙區域，將使用Rewire函數對隨機樹回溯布線，經過多次迭代后，逐漸從無障礙區域優化出一條較優路徑。

表1 BT-RRT*算法

表2給出回溯布線的過程，一旦確定新節點qnew在無障礙物區域，將對從起始點qinit到新節點qnew的路徑回溯布線，被檢查節點qc從qnearest的父節點qnearest-parent開始向起始點qinit移動，檢查每個節點與新節點qnew間的路徑Ψ(qc,qnew)是否在無障礙區域，直到無沖突條件失敗，停止回溯，新節點qnew的最佳父節點qnew-parent為回溯路徑中最后一個無碰撞節點。圖2所示為回溯布線前后樹結構的變化。如圖2(b)所示，基于三角形不等式的概念對路徑重新布線，邊e3總是小于e1和e2的和，因此存在一條較短的路徑e3，重新布線后的路徑如圖2(c)所示。

由定義1可知，點1為1階父節點，點2為2階父節點

表2 回溯布線

1.3 回溯布線的復雜度

推論1： 對于任一新節點qnew，隨機變量A表示每次需回溯的次數，m表示不發生碰撞的次數，新節點可回溯至其m階父節點，則有P(A1-(1/2)m。

證明： 假設障礙物在工作空間內是均勻分布的，則單次回溯與環境產生無碰撞的概率P(A=1)=1/2。則有

P(A=m)=(1/2)m

(2)

P(A1-(1/2)m

(3)

定義2(可忽略函數[19])：存在一個函數f:N→R，如果滿足以下條件：對于整數c>0來說，存在一個正整數nc，對于n>nc的整數來說，有關于n的多項式|f(n)|<1/nc，則該函數為一個可忽略函數，常用negl(n)表示。

推論2：對于每個新節點qnew可回溯次數A<10的概率接近1。因此A/n=negl(n)，其中n為全部可回溯的節點數量。所以BT-RRT*算法的回溯布線過程中所增加的碰撞檢測復雜度可被忽略，其整體復雜度小于RRT*算法的復雜度。

2 多智能體路徑規劃

數學符號采用通用的表示方法，表3給出了多智能體路徑規劃中特殊符號的含義。

表3 符號含義

2.1 動態環境下多智能體路徑規劃算法

首先，在不考慮其他智能體的前提下，在靜態環境中為各智能體規劃無碰撞路徑。然后，各智能體以固定步長ε沿其無碰撞路徑從起點向目標點移動。在各智能體移動過程中，結合距離傳感器獲得的局部信息和避障策略完成局部避障，其中局部避障包括對動態障礙物的避碰和對其他智能體的避碰。

圖3為算法的具體步驟，首先初始化有關參數，包括各智能體的起始點Si、目標點Gi、探測半徑R及移動步長ε。在靜態環境中利用BT-RRT*算法規劃各自無碰撞路徑Pathi，此時不考慮其他智能體。各智能體在以步長ε執行路徑過程中利用距離傳感器獲得其與動態障礙物和其他智能體之間的距離Li，同時實時記錄各智能體的位置。若距離傳感器探測范圍內存在障礙物，則根據局部避障策略避開障礙物，否則，各智能體在原規劃路徑勻速移動。若所有智能體達到目標點，則算法終止。

圖3 多智能體路徑規劃算法

2.2 自適應局部避障策略

定義3(任務優先級)：在仿真實驗中任務優先級預先自定義，智能體所承擔的任務越重要，智能體的優先級越高。

智能體間的協作以局部路徑重規劃方式為主。距離傳感器的探測半徑為

R=Mn/10

(4)

其中環境尺寸為Mn×Mn像素。傳感器的探測半徑越大局部避障效果越好，但增加重規劃時間。

根據任務重要程度確定智能體的協作優先級，將其他智能體視為動態障礙物，若兩智能體間距離小于距離傳感器探測半徑。則根據各智能體的優先級和局部避障策略完成智能體間協作。優先級高的智能體在原路徑向目標點移動，優先級較低的根據避障策略完成局部避碰。

圖4為局部避障過程，利用距離傳感器獲得各智能體與前方動態障礙物的距離Li，將動態障礙物的避碰分為兩個階段，當R/2

圖4 局部避障算法

3 仿真實驗

對提出的算法在多智能體全局路徑規劃時的有效性進行仿真驗證。實驗空間尺寸為200×200像素。實驗過程中將智能體看作質點處理。仿真實驗平臺及配置為：MATLAB R2018b，64位Windows10，處理器Intel(R) Core(TM) i5-8265U，CPU主頻1.6 GHZ。

3.1 全局路徑規劃階段

在基于采樣思想所構造的RRT*路徑規劃算法中，算法主要由兩部分組成：首先構造快速擴展隨機樹用以表示地圖，快速擴展隨機樹是由多個無碰撞節點，及節點間的邊構成，節點數量由RRT*算法的迭代參數決定。隨后基于該樹形結構，依據代價函數進行路徑搜索獲取規劃后的路徑。

定義4(路徑節點)：在RRT*算法中，構成規劃路徑的無碰撞節點定義為路徑節點。

定義5(路徑節點數量)：任意一條路徑是由該路徑上的無碰撞快速擴展隨機樹節點，及相鄰節點間的無碰撞邊構成，這些無碰撞節點的個數稱為該條路徑的路徑節點數量。

在全局路徑規劃階段，使用RRT*算法、GCSE-RRT*算法和BT-RRT*算法在3種不同環境下對多智能體初始路徑規劃進行仿真對比。圖5～圖7分別表示2個智能體、3個智能體、4個智能體路徑規劃結果，圖中均已標明各個智能體的起始點、目標點，其中黑色為障礙物區域。

圖5 2個智能體路徑規劃結果

圖6 3個智能體路徑規劃結果

圖7 4個智能體路徑規劃結果

從路徑長度、規劃時間兩方面進行對比，對比得到的仿真結果如表4所示。智能體數量為2時，相同環境中BT-RRT*算法比RRT*算法節省23.7%的時間，路程縮短13.4%；比GCSE-RRT*RRT*算法節省5.9%的時間，路程縮短5.2%。智能體數量為3時，相同環境中BT-RRT*算法比RRT*算法節省29%的時間，路程縮短16.4%；比GCSE-RRT*RRT*算法節省10.4%的時間，路程縮短14.1%。智能體數量為4時，相同環境中BT-RRT*算法比RRT*算法節省31%的時間，路程縮短10%；比GCSE-RRT*RRT*算法節省10%的時間，路程縮短5.5%。

表4 算法性能比較

基于回溯布線思想在添加新節點向起始點回溯，不僅降低了路徑成本，也降低了路徑點的數量。在路徑規劃過程中路徑點以坐標的形式實時保存，占用一定的內存數量。給定環境尺寸為200×200像素，單個坐標(x,y)占用內存為16 bit，則整體算法占用內存為bn=16Pnbit，其中Pn為路徑節點數量。在相同環境下BT-RRT*算法相較于RRT*算法和GCSE-RRT*算法路徑點明顯減少。因此BT-RRT*算法占用內存較少。BT-RRT*算法不受環境、智能體數量的約束，隨智能體數量的增加，其在路徑長度，規劃時間、路徑節點方面均表現出優勢，因此本文提出的算法適用于多種不同環境，獲得的路徑更短，搜索路徑時間更短。

3.2 動態環境下多智能體路徑規劃仿真

對提出的動態環境下多智能體路徑規劃算法的有效性進行仿真驗證，如圖8所示，環境尺寸為200×200像素，其中，動態障礙物在環境中以隨機的方向移動，智能體移動步長為2，距離傳感器探測半徑為20單位像素。根據定義3，4個智能體優先級定義為：智能體1>智能體2>智能體3>智能體4，說明智能體1的所承擔的任務更重要。在靜態環境下，智能體的全局規劃路徑用藍色虛線表示，在路徑執行過程中，各智能體根據避障策略避開動態障礙物和其他智能體，各智能體的最終路徑用紅色實線表示。實驗表明，智能體可以避開動態障礙物，同時避開其他智能體，獲得較優路徑。

圖8 動態環境下多智能體路徑規劃結果

4 結論

對多智能體路徑規劃的研究主要集中在以下兩點。

(1)提出了一種雙段多智能體路徑規劃算法。全局規劃階段提出一種改進RRT*算法(BT-RRT*)，其優勢是基于回溯布線方法，減少無效父節點。隨著樣本數量的增加，算法規劃出的路徑更優化，收斂速度更快，且不影響算法復雜度。在已知環境下快速地尋找一條無碰撞路徑。

(2)多智能體協作避障階段，根據提出的避碰策略，實現對動態障礙物和其他智能體的避碰，完成多智能體的協作規劃。

實驗表明，該算法能夠有效地引導多智能體從起始位置移動到目標位置，且不與環境中的障礙物發生碰撞。