基于時間的多機器人協(xié)調(diào)避碰算法研究

李 波，易 潔

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

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

對于共享工作空間的多機器人系統(tǒng)的無碰路徑規(guī)劃，需要解決以下兩個主要問題：一是對于給定的任務(wù)，機器人相對于靜態(tài)障礙物的無碰路徑規(guī)劃；二是多機器人相互之間的無碰協(xié)調(diào)運動路徑規(guī)劃[1]。

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

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

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

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

1 背景知識

1.1 柵格法

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

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

1.2 A*算法

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

公式表示為：

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

(1)

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

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

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

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

2 基于時間的多機器人協(xié)調(diào)避碰算法

2.1 單機器人路徑規(guī)劃

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

2.1.1 環(huán)境建模

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

圖1 障礙物的合并與拓展

記單個機器人的大小為s×s，D是機器人系統(tǒng)R在二維平面上的矩形有限運動區(qū)域，其內(nèi)部分布著有限個靜態(tài)障礙物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為有靜態(tài)障礙物的柵格集，則不存在障礙物的空間為C=A-B。

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

(2)

(3)

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

2.1.2改進的A*算法

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

現(xiàn)將機器人模型簡化為質(zhì)點，機器人路徑簡化為線。

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

相應(yīng)流程如圖2所示。

程序主體：

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

圖2 改進A*算法流程

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

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

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

步驟4 結(jié)束程序。

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

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

2.2 多機器人協(xié)調(diào)避碰

2.2.1 多機器人系統(tǒng)的碰撞點集

該多機器人系統(tǒng)中所有機器人的靜態(tài)碰撞路徑點集合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 基于時間的避碰系統(tǒng)

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

偽代碼如下：

2. Fori:=1 ToN

4. Fori:=2 ToN

5. Forj:=1 Toi-1

12. Else

3 仿真實驗

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

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

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

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

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

圖5 使用改進A*算法和傳統(tǒng)A*算法的耗時對比

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

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

圖8 最終路徑序列總耗時

4 結(jié)束語

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

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