一種雙階段多智能體路徑規(guī)劃算法

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

(1.齊魯工業(yè)大學(xué)(山東省科學(xué)院)電子信息學(xué)院(大學(xué)物理教學(xué)部)， 濟(jì)南 250353； 2.濟(jì)南市人機(jī)智能協(xié)同工程實(shí)驗(yàn)室， 濟(jì)南 250353；3.齊魯工業(yè)大學(xué)(山東省科學(xué)院)電氣工程與自動化學(xué)院， 濟(jì)南 250353 )

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

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

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

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

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

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

1 基礎(chǔ)知識

給定工作空間W，其中障礙物區(qū)域?yàn)閃obs，無障礙物區(qū)域?yàn)閃free。在RRT*算法中，構(gòu)建的樹為T=(V,E)，其中V為節(jié)點(diǎn)集合，E為連接節(jié)點(diǎn)的邊集合。定義Ψ(q1,q2)為節(jié)點(diǎn)q1、q2間的路徑，c(q1,q2)為隨機(jī)樹中兩個節(jié)點(diǎn)q1、q2之間的路徑成本，c(q,T)為起始點(diǎn)朝向節(jié)點(diǎn)q的路徑成本，路徑成本用歐氏距離來計算：

(1)

式(1)中：(x1,y1)、(x2,y2)為節(jié)點(diǎn)q1、q2的坐標(biāo)。

定義1：父節(jié)點(diǎn)的階。將距離新節(jié)點(diǎn)最近的節(jié)點(diǎn)定義為1階父節(jié)點(diǎn)，則上一父節(jié)點(diǎn)為其2階父節(jié)點(diǎn)。則經(jīng)m次回溯后，新節(jié)點(diǎn)可回溯至其m階父節(jié)點(diǎn)。

1.1 RRT*算法

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

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

1.2 BT-RRT*算法

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

表1 BT-RRT*算法

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

由定義1可知，點(diǎn)1為1階父節(jié)點(diǎn)，點(diǎn)2為2階父節(jié)點(diǎn)

表2 回溯布線

1.3 回溯布線的復(fù)雜度

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

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

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

(2)

P(A1-(1/2)m

(3)

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

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

2 多智能體路徑規(guī)劃

數(shù)學(xué)符號采用通用的表示方法，表3給出了多智能體路徑規(guī)劃中特殊符號的含義。

表3 符號含義

2.1 動態(tài)環(huán)境下多智能體路徑規(guī)劃算法

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

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

圖3 多智能體路徑規(guī)劃算法

2.2 自適應(yīng)局部避障策略

定義3(任務(wù)優(yōu)先級)：在仿真實(shí)驗(yàn)中任務(wù)優(yōu)先級預(yù)先自定義，智能體所承擔(dān)的任務(wù)越重要，智能體的優(yōu)先級越高。

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

R=Mn/10

(4)

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

根據(jù)任務(wù)重要程度確定智能體的協(xié)作優(yōu)先級，將其他智能體視為動態(tài)障礙物，若兩智能體間距離小于距離傳感器探測半徑。則根據(jù)各智能體的優(yōu)先級和局部避障策略完成智能體間協(xié)作。優(yōu)先級高的智能體在原路徑向目標(biāo)點(diǎn)移動，優(yōu)先級較低的根據(jù)避障策略完成局部避碰。

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

圖4 局部避障算法

3 仿真實(shí)驗(yàn)

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

3.1 全局路徑規(guī)劃階段

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

定義4(路徑節(jié)點(diǎn))：在RRT*算法中，構(gòu)成規(guī)劃路徑的無碰撞節(jié)點(diǎn)定義為路徑節(jié)點(diǎn)。

定義5(路徑節(jié)點(diǎn)數(shù)量)：任意一條路徑是由該路徑上的無碰撞快速擴(kuò)展隨機(jī)樹節(jié)點(diǎn)，及相鄰節(jié)點(diǎn)間的無碰撞邊構(gòu)成，這些無碰撞節(jié)點(diǎn)的個數(shù)稱為該條路徑的路徑節(jié)點(diǎn)數(shù)量。

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

圖5 2個智能體路徑規(guī)劃結(jié)果

圖6 3個智能體路徑規(guī)劃結(jié)果

圖7 4個智能體路徑規(guī)劃結(jié)果

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

表4 算法性能比較

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

3.2 動態(tài)環(huán)境下多智能體路徑規(guī)劃仿真

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

圖8 動態(tài)環(huán)境下多智能體路徑規(guī)劃結(jié)果

4 結(jié)論

對多智能體路徑規(guī)劃的研究主要集中在以下兩點(diǎn)。

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

(2)多智能體協(xié)作避障階段，根據(jù)提出的避碰策略，實(shí)現(xiàn)對動態(tài)障礙物和其他智能體的避碰，完成多智能體的協(xié)作規(guī)劃。

實(shí)驗(yàn)表明，該算法能夠有效地引導(dǎo)多智能體從起始位置移動到目標(biāo)位置，且不與環(huán)境中的障礙物發(fā)生碰撞。