多移動機器人3階段解耦路徑規劃

唐銘偉， 宋栓軍

(西安工程大學 機電工程學院， 陜西 西安 710613)

多機器人集群作業系統正在逐漸取代單機器人作業系統[1]。與單機器人相比，多機器人集群作業時，各機器人之間可以協調配合，從而提升工作效率[2-3]。但是在多機器人集群系統中，機器人若在安全時間內到達同一節點，則會發生路徑沖突、系統死鎖等情況。因此，如何合理地解決這些問題，是當前機器人領域的一個重要研究方向[4]。

NAZARAHARI等[5]通過對遺傳算法進行改進，在算法中添加碰撞消除算子，用以消除機器人之間可能發生的碰撞；但是該方法局限于障礙物較少的簡單環境中，對于障礙物較多的復雜環境則不適用。張丹露等[6]利用A*算法結合交通規則的方法，解決了機器人間的交通擁堵問題，實現多機器人的協同路徑規劃；但是其地圖局限性較大，地圖中的可行道路必須是直行的單向通道。曹其新等[7]提出了一種基于保留區域的多機器人路徑規劃方法，避免了路徑規劃時各機器人間路徑高度耦合的問題；但是該方法中各機器人需要共享位置信息，導致多機器人集群系統計算量增大。晁永生等[8]利用改進的A*算法，減少了搜索的時間，提高了系統的尋路效率。余娜娜等[9]以工作完成時間最小為目標，制定了各機器人的優先級；但是由于各機器人的獨立性較強，使用該方法尋路容易出現局部最優情況。基于此，課題組提出一種3階段多機器人解耦路徑規劃法：①利用改進傳統蟻群算法，為各機器人在靜態環境下快速規劃出一條無碰撞初始路徑[10]；②對規劃出的初始路徑進行沖突檢查；③利用不同的避碰策略消解沖突，從而在消除沖突的前提下為系統輸出一組較優的路徑組合。

1 模型構建

1.1 環境建模

為保證環境模型構建的簡潔性與連續性，課題組采用柵格法進行環境建模[11-12]。白色柵格表示自由柵格，黑色柵格表示障礙物[13]。建立二維坐標系，對柵格按照從上至下、從左至右的順序進行編號[14]，同時對機器人的運行環境進行如下處理：

1) 將障礙物輪廓擴大，擴大范圍為機器人的半徑大小，在移動過程中機器人可以視為1個質點。對障礙物進行模糊化處理時，對于不滿1個柵格的障礙物按1個障礙物處理。

2) 環境地圖由N*N個柵格構成，設置閾值時間ΔT，規定在該時段內不同的機器人不能到達同一柵格節點。

環境模型如圖1所示，各柵格節點在坐標系中都有相對應的序號，取其中心點坐標為該節點的坐標。由圖1可知，當柵格邊長取值為1，柵格序號與坐標可用公式(1)進行轉換：

(1)

式中：ceil為取整函數，mod為取余函數，a為柵格邊長，I為柵格序號。

圖1 柵格地圖Figure 1 Grid map

1.2 模型構建

在多機器人集群路徑規劃的數學模型中，需滿足連續約束、安全約束、避碰約束和終止約束[15]。機器人Ri的路徑由一組柵格坐標組成，即Li=[(xi(1),yi(1)),(xi(2),yi(2)),…,(xi(n),yi(n))]。系統工作時間取決于集群中工作時間最長的機器人，即：

(2)

式中：vi為機器人Ri的移動速度，δ為暫停的次數。

1.3 算法設計

當前機器人路徑規劃算法主要有2類：啟發式算法(Dijkstra算法、A*算法等)和仿生算法(遺傳算法、粒子群算法等)[16]。采用蟻群算法對機器人進行全局路徑規劃。針對傳統蟻群算法存在收斂速度過慢、隨機性較強等問題,課題組提出2項優化措施。

1.3.1參數優化

在傳統蟻群算法中，信息啟發因子α、期望啟發因子β、信息素揮發系數ρ、螞蟻數量m等都是非常重要的參數，其值通常取為固定值。但是在實際應用過程中，在不同時段，參數對于算法的影響也不同。為了加快算法的收斂速度，提高尋路效率，這里分別對α和β進行動態化處理，如公式(3)～(4)所示：

(3)

(4)

式中：A,B,C,D,E和F為常數，Nc為當前迭代次數，k為總迭代次數。

在螞蟻尋路過程中，信息素強度Q也起到至關重要的作用。取值越大，算法的收斂速度越快，但是會導致螞蟻尋路的空間減小，容易陷入局部最優；取值過低，算法前期的正反饋效果不明顯，使得尋路效果不佳。因此對參數Q進行自適應處理，即：

(5)

式中：Q0為初始信息素強度，Lb為本次迭代最優路徑，LB為之前所有迭代中的最優路徑，λ為調整參數。

1.3.2修改啟發函數

傳統蟻群算法采用從當前節點i到下一節點j之間距離的倒數作為啟發函數ηij。但是在柵格環境中，螞蟻在尋路時，由于正反饋作用不明顯，容易陷入局部最優。針對這種情況，引入路徑指導函數，如公式(6)所示：

(6)

式中djG為下一可選節點j與目標點G之間的距離。

新的啟發函數為

(7)

在路徑指導函數的作用下，可以加快算法的收斂速度，為機器人快速地規劃出較優的全局路徑。

2 多機器人沖突預判與消解

2.1 沖突檢查

多機器人系統R為各機器人規劃出初始路徑組LC=[Lc1,Lc2,…,Lcn]，每條路徑均為一組節點坐標的集合。若Ri的初始路徑Lci與其他機器人的初始路徑節點集合的交集為空，則機器人Ri可以按照該初始路徑安全移動；若該路徑的坐標集合與其他機器人的初始路徑的坐標集合的交集不為空，表示該初始路徑與其他機器人的初始路徑存在交叉點，此時需要對其進行安全判斷，判斷方法如下：

(8)

式中dSiWi，j(k)為Ri沿初始路徑從起始點Si至交叉點Wi,j(k)的距離。

若滿足該公式，表示二者沿著初始路徑移動時，不會在安全時間內到達該交叉點Wi，j，不會發生路徑沖突，稱該交叉點為偽沖突點Mi，j。若不滿足該公式，表示二者將會發生路徑沖突，需要進行路徑協調。

機器人Ri與其他機器人的初始路徑的交叉點集合為Wi=Wi,1∪Wi,2∪…∪Wi,n，依次對Wi中的交叉節點進行安全判斷;判斷完成之后，將不會發生路徑沖突的偽沖突節點集合Mi從Wi中去除，得到機器人Ri與其他機器人的路徑沖突節點集合Zi=(Zi,1,Zi,2,…,Zi,n)。

2.2 沖突消解

設機器人Ri與機器人Rj之間存在沖突點Zi,j(k)，比較二者到達沖突節點所需的時間，采取“先到先行，后到協調”的原則確定需要進行路徑協調的機器人。若二者同時到達沖突節點，則隨機選擇一個機器人進行路徑協調。文中采用2種協調策略：

1) 暫停策略

對機器人Ri進行新的安全判定，判斷公式為

(9)

若滿足公式(9)，表示機器人Ri采用該策略可以消解沖突，且不會產生新的沖突點。則令Ri在移動之前，在起始點處暫停ΔT后，再沿初始路徑移動；若不滿足該公式，則采用策略2)。

2) 更新策略

機器人Ri將沖突節點Zi視為障礙物柵格，系統重新為其規劃出一條從起始點至目標點的路徑，再與其他機器人的路徑進行安全判斷。若不會產生新的沖突節點，則令Ri沿著新規劃的路徑移動；若產生新的沖突節點，則將新的沖突點視為障礙物柵格，重新對Ri進行全局路徑規劃。

分別使用上述2種方法進行路徑協調，比較二者所需時間，選擇時耗較少的方法進行路徑協調。

3 實驗結果與分析

3.1 改進蟻群算法的實驗與分析

為了驗證改進蟻群算法的高效性，使用MATLAB軟件進行實驗仿真。在相同的環境下，分別使用改進的蟻群算法和傳統蟻群算法(ACO)進行路徑規劃。參數選擇如表1所示，尋得最優路徑如圖2所示(圖中S表示起始點，G表示目標點)，算法收斂曲線如圖3所示，實驗結果數據如表2所示。

圖2 最優路徑對比Figure 2 Comparison of optimal paths

圖3 收斂曲線對比Figure 3 Comparison of convergence curves

表1 算法參數選擇

表2 仿真結果數據

由表2可知，使用改進后的算法，在收斂速度和路徑長度方面，較傳統蟻群算法都有了顯著的提升，能夠較快地獲得算法的最優解，證明了改進算法的可靠性，為多機器人集群系統的路徑規劃打下了良好的基礎。

3.2 多機器人路徑規劃

為驗證先前所提出的路徑協調方法的有效性，使用MATLAB軟件進行實驗仿真，設柵格邊長為1 m，各機器人移動速度均為1 m/s，安全時間ΔT=1.5 s，移動機器人數量為3個，分別進行2組不同的實驗。

在實驗1中，機器人集群的初始路徑和協調路徑分別如圖4～5所示，實驗結果數據如表3所示。

圖4 20*20環境下初始路徑規劃Figure 4 Initial path planning in 20*20 environment

圖5 20*20環境下協調路徑規劃Figure 5 Coordinated path planning in 20*20 environment

表3 實驗1數據結果

在實驗2中，機器人集群的初始路徑和協調路徑分別如圖6～7所示，實驗結果數據如表4所示。

圖7 25*25環境下協調路徑規劃Figure 7 Coordinated path planning in 25*25 environment

表4 實驗2數據結果

4 結論

針對多機器人的路徑規劃問題，課題組提出一種具有前瞻性的3階段解耦路徑規劃法：首先，利用改進傳統蟻群算法在靜態環境下快速規劃出一條無碰撞初始路徑；然后，對初始路徑進行沖突檢查；最后，利用不同的避碰策略消解沖突從而輸出一組較優的路徑組合。利用MATLAB軟件進行了仿真實驗，實驗結果表明：

1) 改進后的蟻群算法大大提高了算法的收斂速度，可以快速地為機器人規劃出一條較優的無碰路徑。

2) 課題組所提出的路徑協調策略可以有效地檢測和消解多機器人之間的路徑沖突問題，減少了機器人繞行距離和等待時間，保證了多機器人集群系統的運行效率和可靠性。

課題組所提出的路徑規劃方法還存在一定的局現性，優化目標較為單一，今后將考慮多個目標進行優化，同時考慮工作環境中存在動態障礙物的情況，提高機器人工作的安全性。