基于時間的多機器人協調避碰算法研究

李 波，易 潔

(重慶理工大學， 重慶 400054)

隨著機器人技術的發展及生產實踐的需求，單機器人系統已經不能滿足人們的實際需求。而與單機器人構成的系統相比，多機器人系統可以通過機器人之間的相互協調合作提高工作效率，提升工作性能，增強容錯能力。因此，對多機器人系統的研究已成為目前的研究熱點，但同時多機器人系統也使系統復雜程度增加。多機器人系統的算法問題包含了路徑規劃、碰撞檢測、避障策略等，其中最重要的問題是多個機器人如何在互斥的工作區域中工作而不沖突。

對于共享工作空間的多機器人系統的無碰路徑規劃，需要解決以下兩個主要問題：一是對于給定的任務，機器人相對于靜態障礙物的無碰路徑規劃；二是多機器人相互之間的無碰協調運動路徑規劃[1]。

路徑規劃是機器人研究中的主要研究內容之一。機器人的路徑規劃問題，就是在其工作空間中搜索一條從起始狀態到目標狀態能避開障礙物的最優路徑，其一般步驟主要包括環境建模和路徑搜索。先根據采集到的環境信息構建路徑搜索空間，一般可以簡化為二維模型，目前的表示方法可以大致分為3類：柵格表示、幾何信息表示和拓撲圖表示[2]。柵格表示法是將整個工作環境按照相同的大小劃分成若干小方格，即柵格，同時對于每個柵格分別進行說明是否存在障礙物。其特點是：創建與維護較容易，但當柵格數量增大時，內存消耗非常大，且實時處理變困難。幾何法是使用更為抽象的幾何特征對環境進行描述，這種方法便于位置估計和目標識別，但幾何信息的提取需要對感知信息做額外的處理，且需要有大量感知信息的支撐才能得出結果，較麻煩[3]。拓撲圖將環境表示為一張拓撲意義中的圖，圖中的節點對應于環境中的一個特征狀態、地點。節點間存在的直接連接路徑相當于圖中連接節點的弧，它能實現快速的路徑規劃，而當存在兩個很相似的地方時，這種方法很難確定是否為同一個節點。

目前，常見的單個機器人系統的路徑搜索避障算法有：A*算法[4]、人工勢場法[5]、遺傳算法[6]等。這些算法依舊存在許多問題，例如A*算法轉折次數多，使得機器人按規劃路徑行動變得非常困難；人工勢場法在狹小的區域當中容易產生抖動；遺傳算法還不完善，人工因素對結果有很大的影響。對于多機器人系統的路徑規劃方法的研究，大部分是從單個機器人系統的路徑搜索避障算法擴展而來的，主要可分成三大類：傳統方法(人工勢場法等)、智能優化算法(遺傳算法、強化學習等)和其他方法(動態規劃、模糊控制等)。

人工勢場法是將機器人系統在工作空間中的運動看作受到了一種虛擬的人工受力場的作用后的運動，障礙物對機器人產生斥力，而目標狀態點對之產生引力，引力和斥力由算法產生相應的勢，但是和單機器人系統一樣，也存在著目標點不可達和易陷入徘徊抖動的問題，以及對于動態環境規劃能力不足的問題。文獻[7]在原有引力勢場和斥力勢場的基礎上增加了填平勢場，并規定只有當機器人處于局部極小點時才啟用，解決了目標點不可達和易陷入徘徊抖動的問題，但是對于動態環境規劃能力不足的問題并未得到解決。在強化學習算法中，主要是將機器人系統通過馬爾科夫建模轉換成一個強化學習可以解決的問題，再通過強化學習進行求解，從而得到路徑規劃，達到避障的目的。其優點主要是：不需要構建精確的環境模型，并且可以一次性將避障、路徑規劃等問題解決。其缺點是：當處于動態環境中，會不可避免地出現學習策略隨狀態、動作的維數呈指數增長的現象導致“維數災難”，同時由于其不存在一個明確的標簽，在機器人和環境之間的交互完全是采用試探的方法，導致強化學習的學習速度比較慢。文獻[8]提到可以通過分層強化學習來解決“維數災難”；文獻[9]提出一種新的基于ERL算法的非線性動態系統最優控制方法，提高了非線性系統的學習效率。雖然路徑得到了優化，但是學習效率并沒有得到顯著的提升，且算法的復雜性依舊存在。

在本文中使用改進的柵格法來對環境進行建模，然后再在構建好的環境模型中利用對轉折次數多這一缺點進行改進后的A*算法，在不考慮機器人相互碰撞、只考慮靜態障礙物的前提下得到單個機器人位形空間的靜態無碰運動規劃；在給定各機器人任務路徑的前提下，使用基于時間的多機器人協調避碰規劃算法實現系統間的無沖突連續路徑搜索，即先求得多機器人系統的碰撞點集，然后通過計算時間來調節機器人到達碰撞點時是否真正產生碰撞，如果產生則避讓。該方法采用解耦法進行研究，同時仿真實驗結果：說明該方法能有效并快速得到多機器人系統的路徑規劃。

1 背景知識

1.1 柵格法

柵格法最先由W.E.Howeden于1986年提出。在進行路徑規劃時，W.E.Howeden采用柵格表示地圖，在處理障礙物的邊界時，避免了復雜的計算。當要更精確地表示地圖，即柵格粒度越小時，會占用大量的存儲空間，并且實時處理會變得困難[10]。

柵格模型使用形狀為二維矩形的柵格表示環境，由于柵格法的特性，如何選取單個柵格的大小將直接影響路徑規劃算法的性能。若柵格選得過小，環境地圖的精度高，但處理速度慢；柵格選得過大，決策的速度快，但規劃路徑可能不精確。

1.2 A*算法

A*算法[11]是一種靜態路網中求解最短、路最有效的直接搜索方法，也是許多其他問題的常用啟發式算法。

公式表示為：

f(a)=g(a)+h(a)

(1)

其中：f(a)是從初始位形經由位形節點a到目標位形的估計代價；g(a)是在位形空間中從初始位形到位形節點的實際代價，即為到達位形節點a已經消耗了的代價；h(a)是從位形節點a到目標位形的最佳路徑的估計代價。

從公式中可以看出：啟發函數h(a)的設置將直接影響A*算法的路徑規劃性能。如果h(a)經常比從a移動到目標的實際代價小(或者相等)，則A*保證能找到一條最短路徑。h(a)越小，A*擴展的結點越多，運行就越慢。

如果h(a)精確地等于從a移動到目標的代價，則A*將僅僅尋找最佳路徑而不擴展別的任何結點，這會運行得非常快。盡管這不可能在所有情況下發生，但仍可以在一些特殊情況下讓它們精確地相等。只要提供完美的信息，A*會運行得很完美。

如果h(a)有時比從a移動到目標的實際代價高，則A*不能保證找到一條最短路徑，但它運行得更快。

2 基于時間的多機器人協調避碰算法

2.1 單機器人路徑規劃

第一階段是在忽略機器人間沖突的前提下，規劃出各個機器人與環境的無碰路徑。該問題屬于機器人運動規劃的基本問題，即規劃處給定機器人的初始位形到目標位形的無碰路徑。

2.1.1 環境建模

本文使用柵格法表示環境，記單個柵格的大小為單個機器人的大小，并且此時將障礙物簡化為矩形。當障礙物不滿一個柵格時將其擴展為一個柵格，并且不規則形狀障礙物進行合并或擴展柵格形成規則形狀即矩形(如圖1所示)。這樣，可以使規劃路徑準確，且決策速度快。

圖1 障礙物的合并與拓展

記單個機器人的大小為s×s，D是機器人系統R在二維平面上的矩形有限運動區域，其內部分布著有限個靜態障礙物b0,b1,…，bn。以D的左上角為坐標原點O，橫向為X軸，縱向為Y軸，建立一個柵格坐標系，X、Y軸的最大值為Xmax、Ymax，以單個機器人的大小作為一個單位柵格，其中bi(i=1,2,…,n)占一個或多個柵格。記a∈D為任意柵格，A為D中所有a的集合，同時B=(b0,b1,…,bn)?A為有靜態障礙物的柵格集，則不存在障礙物的空間為C=A-B。

所以序號為i的柵格ai的坐標(xi,yi)可用下列公式確定：

(2)

(3)

現在規劃的目的是使機器人由任意起點abegin無碰地到達任意終點aend，begin≠end且abegin,aend∈C。

2.1.2改進的A*算法

A*算法雖然廣泛應用于移動機器人的自主路徑規劃中，但是A*算法給出的規劃路徑轉折點過多，不夠平滑。故可以設置，若當前節點在父節點的某一方向，則在f(a)值相同的情況下優先選取當前節點同一方向的下一節點。又由于已知起點abegin和終點aend的坐標，例如在起點右邊，即abegin的x值比aend小，則先不計算當前節點左邊節點的f(a)值，只比較右邊和上下節點的f(a)值。只考慮x值的大小。

現將機器人模型簡化為質點，機器人路徑簡化為線。

在搜索路徑過程中設置：開啟列表openSet——尋路過程中的待檢索節點列表；關閉列表closeSet——不需要再次檢索的節點列表；追溯表comeFrom——存儲父子節點關系的列表，用于追溯生成路徑。

相應流程如圖2所示。

程序主體：

步驟1 比較abegin、aend的x值大小，若abegin的x值比aend小，則接下來相鄰節點不包括當前節點左節點；當abegin的x值比aend大，則接下來再也不考慮當前節點的右節點。將起點abegin加入開啟列表openset。

圖2 改進A*算法流程

步驟2 尋找開啟列表openset中f(a)值最小的節點，設為當前點anow；開啟列表openset中移出當前點anow；關閉列表closeset中加入當前點anow。

步驟3 對當前點的相鄰節點aneighbor，如果它不可通過或者已經在關閉列表中，略過。否則：若它不在開啟列表中，加入開啟列表中；若在開啟列表中，進行g(a)值判定，若此路徑G值比之前路徑小，則此相鄰點的父節點為當前點，同時更新g(a)與f(a)值。反之，則保持原來的節點關系與g(a)、f(a)值。

當目標點aend在開啟列表或當前開啟列表為空，則轉到步驟4；否則，轉到步驟2。

步驟4 結束程序。

當目標點aend在開啟列表中，此時有路徑生成，此時由aend節點開始逐級追溯上一級父節點，直到追溯到開始節點abegin，此時各節點即為路徑。

當開啟列表為空，此時沒有路徑。

2.2 多機器人協調避碰

2.2.1 多機器人系統的碰撞點集

該多機器人系統中所有機器人的靜態碰撞路徑點集合Q=Q1∪Q2∪…QN，算法偽代碼如下：

1. Fori:=1 ToN-1

2. Forj:=i+1 ToN

5. else

6. Continue

7. Sort (Qk)

8.Q=Q1∪Q2∪…QN

2.2.2 基于時間的避碰系統

依次計算出機器人到達每一個碰撞點的時間，判定碰撞點是否真會相撞，再將所有要等待的機器人相應列出，最后得到每一個機器人的運行序列。

偽代碼如下：

2. Fori:=1 ToN

4. Fori:=2 ToN

5. Forj:=1 Toi-1

12. Else

3 仿真實驗

使用Matlab仿真軟件，假設在10×10的D區域中，有容積為3的機器人系統R={R1,R2,R3}，令單個機器人大小為1×1，即單個柵格大小也為1×1，3個機器人以相同的速度v勻速前進。分別給定起點abegin和終點aend的坐標，使用A*算法求得機器人R的路徑點集合，如圖3所示(黑色為障礙點，綠色為起點，黃色為行進路徑點)。

圖3 使用傳統A*算法機器人R的路徑點集合

使用改進的A*算法求得機器人R的路徑點集合如圖4所示(黑色為障礙點，綠色為起點，黃色為行進路徑點)。

圖4 使用改進A*算法機器人R的路徑點集合

可以明顯看出：拐點得到了有效的減少，路徑更為平滑，同時得到結果的耗時也有些許優化，如圖5所示(其中improve_A為改進的A*算法)。

圖5 使用改進A*算法和傳統A*算法的耗時對比

圖6 使用改進的A*算法機器人Rk(k=1,2,3)的路徑點集合

圖7 機器人Rk(k=1,2,3)相對應的無碰序列

圖8 最終路徑序列總耗時

4 結束語

本文提出了通過調節機器人到達碰撞點時間的基于柵格環境離線解耦法，能夠有效解決在共享區域空間時多機器人系統的無碰運動規劃問題。該方法由兩階段實現，第一階段使用改進的A*算法得到單個機器人的靜態無碰運動序列；第二階段通過調節機器人抵達碰撞節點柵格的時間來實現機器人之前的無碰運動。

但本文算法的計算量較大，對于大型多機器人系統具有一定的局限性。今后研究將從以下幾個方面進行改進：① 改進算法提高運算效率；② 為了獲得多機器人系統的最優解決方案，增加優化指標，如時間、能耗等。