分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃

李 靖，楊 帆，2

（1.河北工業(yè)大學 電子信息工程學院，天津 300401；2.河北工業(yè)大學 天津市電子材料與器件重點實驗室，天津 300401）

路徑規(guī)劃[1]在移動機器人控制領(lǐng)域是研究重點問題之一，路徑規(guī)劃的好壞直徑影響著機器人控制系統(tǒng)后續(xù)執(zhí)行作業(yè)任務(wù)的效率。機器人路徑規(guī)劃是根據(jù)障礙物空間地圖的信息，在滿足一定約束條件和評價指標的條件下，在線或離線規(guī)劃出從起點到目標點的行走滿意路徑。機器人路徑規(guī)劃根據(jù)環(huán)境信息掌握情況[2-6]，可以分為全局路徑規(guī)劃和局部路徑規(guī)劃，全局路徑規(guī)劃算法和局部路徑規(guī)劃算法并沒有本質(zhì)區(qū)別，若改變某些條件，二者是可以互相轉(zhuǎn)化甚至可以融合使用的。

目前路徑規(guī)劃研究中啟發(fā)式算法特別是群智能優(yōu)化算法最為流行。CONTRERAS-CRUZ 等[7]運用人工蜂群算法在自由空間中搜索可行性路徑，運用進化規(guī)則進行路徑優(yōu)化和平滑。Lamini 等[8]將遺傳算法中引用交叉算子，可以在靜態(tài)環(huán)境中兩個位置之間找到有效路徑。Shi 等[9]運用粒子群優(yōu)化蟻群算法的參數(shù)，隨后運用改進的蟻群算法進行三維空間路徑規(guī)劃。以上算法均在不同程度優(yōu)化了路徑規(guī)劃效果，但群智能優(yōu)化算法易陷入局部優(yōu)的缺陷并沒有完全解決。除此之外，大多數(shù)機器人路徑規(guī)劃大都是針對單任務(wù)目標，而對于多任務(wù)目標的路徑規(guī)劃研究較少，對于多機器人多任務(wù)目標的研究就更少，這比起單任務(wù)目標復雜。劉文兵等[10]提出了一種多無人機協(xié)同搜索多目標的路徑規(guī)劃算法，運用K-Means 聚類多任務(wù)目標將MTSP 問題轉(zhuǎn)化成TSP 問題，通過引入2-opt 算子的改進遺傳算法對多個TSP 問題進行求解規(guī)劃路徑。趙明明等[11]運用拍賣法對多任務(wù)進行分配，根據(jù)區(qū)間一致性規(guī)劃路徑使得多機器人在一定時間內(nèi)到達多目標任務(wù)。這些算法能夠完成多無人機多任務(wù)目標的路徑規(guī)劃，有一定的推廣性，但對于陸地機器人，這些算法忽略了障礙物空間的存在。

針對上述問題，本文提出了一種分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃算法。首先，根據(jù)待執(zhí)行任務(wù)的機器人的數(shù)量，通過K-Means 算法對多任務(wù)目標進行聚類，將多任務(wù)目標根據(jù)距離進行分類，每個機器人負責遍歷一類任務(wù)目標，隨后運用了引入余弦收斂因子及布谷鳥搜索算法改進灰狼優(yōu)化算法求解類內(nèi)任務(wù)目標的遍歷順序，最后通過A*法在各類內(nèi)根據(jù)遍歷順序進行避障路徑規(guī)劃。當所有的機器人都遍歷完各自類內(nèi)的任務(wù)目標后，整個系統(tǒng)的多任務(wù)目標便全部遍歷完畢，所有機器人行走的路徑即為整個系統(tǒng)的遍歷路徑，分布式任務(wù)聚類、機器人協(xié)同的搜索算法，在復雜的工作空間不但能夠很好地完成遍歷任務(wù)而且降低了計算復雜度。

1 相關(guān)理論基礎(chǔ)

1.1 K-Means 算法

K-Means 算法[12]是無監(jiān)督聚類算法，不需要有任何的先驗知識，其原理在于一種劃分的思想，以距離作為數(shù)據(jù)間相似性度量的標準，數(shù)據(jù)間的距離越小，則表明相似性越高，則可以歸為一個類簇。即在給定的樣本集中確定聚類類別數(shù)目K，隨機選取樣本集中K 個數(shù)據(jù)作為初始聚類中心，遍歷其余所有數(shù)據(jù)計算其與聚類中心的距離，分別將這些數(shù)據(jù)分配給與其最短的類簇中，隨后計算每個類簇中所有數(shù)據(jù)的均值以更新聚類中心，不斷地迭代此過程，直到滿足準則函數(shù)收斂要求。

1.2 傳統(tǒng)灰狼優(yōu)化算法（GWO）

灰狼優(yōu)化算法（grey wolf optimizer，GWO）[13]是模擬生物界灰狼捕食行為啟發(fā)所得，狼群內(nèi)具有非常嚴格的社會等級制度。在GWO 算法數(shù)學建模中，每只灰狼代表種群中1 個候選解，將最優(yōu)解視為α 狼，第2 和第3 個最佳候選解視為β 狼和δ 狼。其余的候選解視為ω 狼。在GWO 算法中搜索由α、β 和δ 引導，ω 狼跟隨這3 只狼?；依菄东C物的數(shù)學模型描述如下：

式中：t 為當前迭代次數(shù)；A 和C 為系數(shù)向量；Xp為獵物的位置向量；X 為灰狼的位置向量。A 和C 計算如式（3）、（4）所示。

式中：r1和r2的模是[0，1]之間的隨機數(shù)；a 是收斂因子，隨著迭代次數(shù)的增加從2 線性減小到0。

式中：t 為當前迭代次數(shù)；tmax為最大迭代次數(shù)。

灰狼追蹤獵物位置的數(shù)學模型描述如下：

式中：C1、C2與C3為隨機向量；Dα、Dβ和Dδ分別為α狼、β 狼和δ 狼與狼群其他成員之間的距離；Xα、Xβ與Xδ分別為α 狼、β 狼和δ 狼的位置；X 是當前狼的位置，狼群中ω 狼的位置是由α 狼、β 狼和δ 狼共同決定的。

1.3 改進的灰狼優(yōu)化算法（IGWO）

1.3.1 引入余弦收斂因子

文獻[14]指出A 和C 是兩個隨機自適應(yīng)向量以輔助GWO 算法全局搜索和局部開發(fā)。A 的取值范圍在[-a，a]，而a 線性的從2 衰減到0。當|A|>1 時進行全局搜索，若|C| > 1 則擴大搜索范圍促進全局搜索。當|A|<1 時進行局部開發(fā)，若|C|<1 則收縮搜索范圍進行攻擊獵物促進局部開發(fā)。C 是隨機確定的，在每個階段都需要搜索與開發(fā)以緩解局部最優(yōu)的局面。因此A 的大小與全局搜索和局部開發(fā)能力息息相關(guān)。A隨a 變化，而算法的收斂并不是線性的過程，式（5）中a 的線性衰減不能滿足實際情況，因此引入余弦收斂因子[15]。

式中：Tmax為最大迭代次數(shù)；t 為當前迭代次數(shù);n∈[0，1]，取af=0，a0=2。圖1 顯示了a 的變化。

圖1 收斂因子對比圖Fig.1 Comparison of convergence factor

由圖1 可以看出，余弦收斂因子a 的衰減先快后慢，在迭代初期，a 數(shù)值減小速度快，利于全局搜索，加快尋找最優(yōu)解所在的范圍，提高搜索效率。隨著迭代次數(shù)的增加，特別是到迭代后期，a 的數(shù)值減少緩慢些，目的在于精確地搜索到最優(yōu)解，有利于局部開發(fā)能力的提升，增加搜索精度。

1.3.2 引入CS 算法進行位置更新

布谷鳥搜索算法（cuckoo search，CS）[16]啟發(fā)于布谷鳥獨特的育雛方式和萊維飛行的方式。CS 算法需要遵循3 個規(guī)則：布谷鳥隨機選擇巢，一次只放一個蛋；最優(yōu)巢才會被帶到下一代且巢的數(shù)量和蛋被發(fā)現(xiàn)的概率是固定的。如果寄主鳥發(fā)現(xiàn)了布谷鳥的蛋，寄主鳥就會棄巢而建。在遵守規(guī)則的條件下，巢會隨著迭代而更新，如式（10）所示。

式中：α 為步長縮放因子；Levy（β）為萊維隨機路徑；?就是點乘運算。

萊維飛行是隨機游動，能夠算法擴大搜索范圍，由式（11）所決定[17]。

通過式（8）可以看出，GWO 算法是通過計算最優(yōu)解（α 狼的位置），隨后整個狼群均去靠近α 狼的位置，易出現(xiàn)局部最優(yōu)的情況，特別是在高維數(shù)據(jù)集中易出現(xiàn)。CS 算法利用隨機游走和萊維飛行更新巢位置，同時搜索路徑或長或短且概率幾乎相同，且方向高度隨機。因此，更容易從當前區(qū)域跳到其他區(qū)域，解決局部最優(yōu)的問題。

1.3.3 IGWO 算法的算法流程

第1 步：初始化種群的位置，初始化相關(guān)參數(shù)a，A，C 和Pa，初始化目標函數(shù)；

第2 步：根據(jù)目標函數(shù)計算種群中每個個體的適應(yīng)度值，確定出Xα、Xβ和Xδ；

第3 步：根據(jù)式（8）更新種群每個個體的位置；

第4 步：根據(jù)式（9）更新a；根據(jù)式（3）和（4）更新A 和C；

第5 步：計算種群每個個體的適應(yīng)度值；

第6 步：更新Xα、Xβ和Xδ；

第7 步：根據(jù)式（10）更新Xα、Xβ和Xδ，若產(chǎn)生隨機數(shù)大于概率Pa，則隨機更改Xα、Xβ和Xδ的位置；否則根據(jù)目標函數(shù)計算適應(yīng)度值并更新Xα、Xβ和Xδ；

第8 步：若達到終止條件，則輸出Xα；否則轉(zhuǎn)到第2 步。

1.4 A*算法

A*算法[18]是一種啟發(fā)式搜索算法，路徑規(guī)劃時對每個搜索位置進行估價，如式（12）所示。

F（n）=G（n）+H（n） （12）式中：G（n）為起點到第n 節(jié)點的實際代價；H（n）為第n 節(jié)點到目標節(jié)點的最佳路徑的估計代價。搜索過程是通過OPEN 表和CLOSED 表標識的，OPEN 表記錄了待搜索節(jié)點，CLOSED 表記錄了已搜索過節(jié)點，不斷地根據(jù)估價函數(shù)重排OPEN 表，尋找最優(yōu)節(jié)點實現(xiàn)搜索過程。

2 分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃

分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃主要分為兩個階段，即任務(wù)分配和路徑規(guī)劃，其總體框圖如圖2所示。

寬帶接收機同頻段套間幅度波動一致性，最根本關(guān)心的因素并不是接收機整個頻段波動的大小，而是同頻段接收機套間在相同的頻率，不同時刻增益波動的大小，因此我們不能用整個頻段內(nèi)的增益平坦度來作為唯一衡量同頻段套間幅度波動一致性的問題（但在設(shè)計時要盡可能的減小全頻段的增益波動）；從工作溫度來看，接收機的增益隨工作溫度的變化趨勢存在差異，影響套間幅度波動一致性；從器件互聯(lián)來看，器件間的端口駐波大小也會影響通頻帶特性，惡化帶內(nèi)波動，影響套間幅度波動一致性。

圖2 算法總框圖Fig.2 Flow diagram of algorithm

2.1 分布式協(xié)同多機器人任務(wù)分配

將待執(zhí)行任務(wù)的多機器人的數(shù)量作為K-Means聚類的k 值，對已有任務(wù)目標進行聚類，將第i（i∈[1，k]）類任務(wù)目標分配給第i 個機器人去執(zhí)行。假設(shè)任務(wù)目標集為T={T1，T2，…，Tn}，擁有待執(zhí)行任務(wù)機器人k個，聚類集合為μ={μ1，μ2，…，μk}，距離函數(shù)為

式中：Tm為第m 個任務(wù)目標；μi表示第i 類到聚類中心。

聚類中心更新公式為：

準則函數(shù)為

式中：xj表示第i 類樣本中的數(shù)據(jù)。

圖3 所示為3 個待執(zhí)行任務(wù)機器人，50 任務(wù)點3聚類的任務(wù)分配圖，任務(wù)目標分成的3 類以藍紅黃3種顏色表示。

2.2 類內(nèi)多任務(wù)目標遍歷路徑規(guī)劃

2.2.1 IGWO 算法求解多目標遍歷順序

多任務(wù)目標搜索最優(yōu)遍歷路線階段主要目的是確定多任務(wù)目標遍歷順序，為后續(xù)避障的執(zhí)行給予目標點遍歷順序預設(shè)。其問題可以描述為假設(shè)區(qū)域工作環(huán)境地圖已知，要求機器人能夠遍歷全部n 個任務(wù)點形成遍歷路徑，則路徑長度為

根據(jù)任務(wù)類標識，在類內(nèi)以式（16）作為目標函數(shù)，運用1.3.3 的算法流程進行IGWO 算法求解最優(yōu)適應(yīng)度值，得出遍歷路徑。

圖3 任務(wù)分配效果Fig.3 Effect of task assignment

圖4 所示為圖3（b）中第I 類任務(wù)遍歷順序，要求將原點作為起始點。

圖4 IGWO 算法遍歷多任務(wù)目標Fig.4 Traversing multitask target with IGWO algorithm

2.2.2 A*算法類內(nèi)避障路徑規(guī)劃

A*算法以一個圖作為輸入逐個搜索所有區(qū)域，找到從起點到已搜索區(qū)域中所有節(jié)點的最短路徑。A*算法適用于離散空間，在高分辨率地圖上，A*算法計算時間長。因此，可以降低地圖分辨率，以減少A*算法擴張搜索的計算量。將降低分辨率地圖的每個像素作為節(jié)點，每個節(jié)點都有許多無障礙物連接線作為邊，形成編碼矩陣，如圖5 所示。

圖5 連接編碼矩陣Fig.5 Connection coding matrix

圖5 中，所有可能的移動用1 表示，所有不可以的移動用0 表示；R 代表機器人當前位置。機器人搜索方向越多，意味著機器越靈活，搜索精度越高，但計算耗時會越大。

根據(jù)IGWO 求解出的類內(nèi)任務(wù)目標遍歷順序，運用A*算法避障搜索尋找可行路徑，圖6 所示為直線+對角線編碼搜索模板運用A*算法規(guī)劃出的路徑。

圖6 A*算法路徑規(guī)劃Fig.6 Path planning with A*algorithm

由圖6 可知，地圖分配率500×500 降低到100×100，以加快搜索效率，從起點坐標（10，10）到任務(wù)目標坐標（490，490）進行路徑規(guī)劃，圖6（a）中灰顏色區(qū)域為擴張搜索區(qū)域，圖6（b）中藍色線段為避障規(guī)劃出的最終路徑，路徑長度為719.8276 m。

3 實驗仿真及結(jié)果分析

3.1 IGWO 算法類內(nèi)多任務(wù)目標遍歷仿真

圖7 所示為隨機產(chǎn)生的30 個任務(wù)目標運用GWO算法與IGWO 算法求解多任務(wù)遍歷路徑規(guī)劃。

圖7 中種群數(shù)量設(shè)置為30，迭代次數(shù)為500 次，前者路徑規(guī)劃長度為5154.0973 m，后者路徑規(guī)劃長度為4892.3635 m，路徑縮短了261.7338 m，縮短比例為5.08%。

圖7 30 任務(wù)目標遍歷結(jié)果Fig.7 Traversal results of 30 task targets

圖8 為收斂曲線對比圖。

圖8 收斂曲線對比圖Fig.8 Comparison of convergence curve

從圖8 可以看出，IGWO 算法要比GWO 算法收斂速度快些，這意味著IGWO 算法規(guī)劃效果更好。

如圖9 所示為50 任務(wù)點3 聚類任務(wù)分配結(jié)果。

圖9 50 任務(wù)點3 聚類任務(wù)分配Fig.9 Task allocation of 50 task points with 3 clustering

圖9 中，藍色線段為第I 類任務(wù)遍歷，負責15 個任務(wù)目標，從原點出發(fā)路徑遍歷長度3334.3469 m，任務(wù)目標遍歷順序為0-37-12-18-19-29-31-14-10-21-33-32-28-16-34-50-0，黑色線段為第II 類任務(wù)遍歷，負責19 個任務(wù)目標，從原點出發(fā)路徑遍歷長度2195.8106 m，任務(wù)目標遍歷順序為0-24-25-13-9-35-5-43-44-23-26-41-38-27-2-22-4-39-15-48-0，紅色線段為第III 類任務(wù)遍歷，負責16 個任務(wù)目標，從原點出發(fā)路徑遍歷長度2821.7776 m，任務(wù)目標遍歷順序為0-36-30-45-46-49-8-42-3-6-1-17-20-7-47-40-11-0 遍歷路徑總長度為8351.9351 m。

3.2 A*算法路徑規(guī)劃仿真

為了驗證算法的路徑規(guī)劃性能，分別對簡單場景和復雜場景運用常用的4 種算法，即人工勢場法[19]、模糊邏輯法[20]、快速擴展隨機樹法（rapidly exploring random tree，RRT）[21]以及A*法進行仿真測試，如圖10、圖11 所示，地圖分辨率均為500×500，起點坐標設(shè)置為（50，50），目標點坐標為（400，400）。

圖10 簡單場景路徑規(guī)劃對比Fig.10 Comparison of simple scenario path planning

從圖10、圖11 可以看出,人工勢場法對給出的兩幅地圖均搜索失敗，原因在于對于圖10（a）場景，障礙物處于正前方，引力和斥力合力為0，故搜索停滯不前。圖11（a）搜索失敗原因在于目標點附近有障礙物，且障礙物產(chǎn)生的斥力大于目標產(chǎn)生的引力，故合力的方向偏離目標方向，導致搜索失敗。而模糊邏輯法、RRT 法、A*算法均能路徑規(guī)劃成功，如表1 所示。

圖11 復雜場景路徑規(guī)劃對比Fig.11 Comparison of path planning of complex scenes

表1 4 種算法路徑規(guī)劃數(shù)據(jù)對比Tab.1 Comparison of path planning data of four algorithms

從時間上看，3 種算法相差無幾，RRT 算法略快些，零點幾秒的差距對于人眼視覺毫無區(qū)別，但從規(guī)劃路徑的長度上來看，RRT 算法要優(yōu)于模糊邏輯法，而A*算法比RRT 算法更加優(yōu)越，對于圖10A*算法比RRT 算法路徑規(guī)劃長度縮短了48.6366 m，縮短比例為8.4%，而圖11 中A*算法比RRT 算法路徑規(guī)劃長度縮短了67.8911 m，縮短比例為11.2%。若以圖10與圖11 兩幅場景的路徑縮短比例均值作為算法衡量的標準，則與模糊邏輯法與RRT 算法相比，A*算法的平均路徑長度分別縮短了22.4%、9.8%。

3.3 分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃仿真

分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃如圖12 所示。

圖12（a）所示為工作區(qū)域映射到二維平面的工作地圖，紅色圓點為標識出的30 個多任務(wù)目標。假設(shè)待執(zhí)行任務(wù)機器人為2 個，要求機器人均從地圖原點出發(fā)，因此將多任務(wù)目標聚類為2 類，如圖12（b）所示，其中Ⅰ類含任務(wù)目標14 個，聚類中心為214.2857，354.28571，Ⅱ類含任務(wù)目標16 個，聚類中心為271.8750，155。隨后在類內(nèi)通過IGWO 算法求解的多任務(wù)目標的遍歷順序，Ⅰ類的遍歷順序為0-16-24-11-12-18-23-20-19-8-4-14-10-13-3-0，Ⅱ類的遍歷順序為0-1-2-25-15-21-5-28-6-17-30-9-27-7-29-22-26-0，如圖12（c）所示。根據(jù)兩類遍歷順序，運用A*算法在類內(nèi)逐個遍歷任務(wù)目標避障進行路徑規(guī)劃，如圖12（d）所示，Ⅰ類任務(wù)目標路徑規(guī)劃長度為589.7056 m，Ⅱ類任務(wù)目標路徑好長度為227.27922 m，總行駛長度為816.9848 m。

圖12 分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃Fig.12 Path planning of multi-task target traversal for distributed cooperative multi-robot

4 結(jié) 論

本文提出了一種分布式協(xié)同多機器人多任務(wù)目標遍歷路徑規(guī)劃算法，運用K-Means 算法將多任務(wù)目標聚類，分成k 類任務(wù)，每一類任務(wù)派遣一個機器人去執(zhí)行，機器人根據(jù)IGWO 算遍歷出的類內(nèi)任務(wù)目標順序，運用A*算法逐一去避障遍歷類內(nèi)任務(wù)目標，當所有的機器人都遍歷完各自的類內(nèi)的任務(wù)目標后，整個系統(tǒng)的多任務(wù)目標便全部遍歷完畢，所有機器人避障規(guī)劃路徑即為整個系統(tǒng)的路徑規(guī)劃。在本實驗任務(wù)場景中求解多任務(wù)點遍歷路徑時，IGWO 算法比GWO算法的遍歷路徑長度縮短了5.08%；在避障路徑規(guī)劃時，A*算法比模糊邏輯法與RRT 算法的平均路徑長度分別縮短了22.4%、9.8%，并能很好地完成了30 任務(wù)點2 分類的遍歷路徑避障規(guī)劃任務(wù)。這種分布式任務(wù)聚類、機器人協(xié)同搜索的算法，在復雜的工作空間不但能夠很好地完成遍歷任務(wù)，而且省去了多機器人編隊、隊形不斷變換、隊內(nèi)任務(wù)分配等環(huán)節(jié)，降低了計算復雜度，特別適用于機器人監(jiān)測巡視的場景。