多移動機(jī)器人3階段解耦路徑規(guī)劃

唐銘偉， 宋栓軍

(西安工程大學(xué) 機(jī)電工程學(xué)院， 陜西 西安 710613)

多機(jī)器人集群作業(yè)系統(tǒng)正在逐漸取代單機(jī)器人作業(yè)系統(tǒng)[1]。與單機(jī)器人相比，多機(jī)器人集群作業(yè)時，各機(jī)器人之間可以協(xié)調(diào)配合，從而提升工作效率[2-3]。但是在多機(jī)器人集群系統(tǒng)中，機(jī)器人若在安全時間內(nèi)到達(dá)同一節(jié)點(diǎn)，則會發(fā)生路徑?jīng)_突、系統(tǒng)死鎖等情況。因此，如何合理地解決這些問題，是當(dāng)前機(jī)器人領(lǐng)域的一個重要研究方向[4]。

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

1 模型構(gòu)建

1.1 環(huán)境建模

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

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

2) 環(huán)境地圖由N*N個柵格構(gòu)成，設(shè)置閾值時間ΔT，規(guī)定在該時段內(nèi)不同的機(jī)器人不能到達(dá)同一柵格節(jié)點(diǎn)。

環(huán)境模型如圖1所示，各柵格節(jié)點(diǎn)在坐標(biāo)系中都有相對應(yīng)的序號，取其中心點(diǎn)坐標(biāo)為該節(jié)點(diǎn)的坐標(biāo)。由圖1可知，當(dāng)柵格邊長取值為1，柵格序號與坐標(biāo)可用公式(1)進(jìn)行轉(zhuǎn)換：

(1)

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

圖1 柵格地圖Figure 1 Grid map

1.2 模型構(gòu)建

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

(2)

式中：vi為機(jī)器人Ri的移動速度，δ為暫停的次數(shù)。

1.3 算法設(shè)計

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

1.3.1參數(shù)優(yōu)化

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

(3)

(4)

式中：A,B,C,D,E和F為常數(shù)，Nc為當(dāng)前迭代次數(shù)，k為總迭代次數(shù)。

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

(5)

式中：Q0為初始信息素強(qiáng)度，Lb為本次迭代最優(yōu)路徑，LB為之前所有迭代中的最優(yōu)路徑，λ為調(diào)整參數(shù)。

1.3.2修改啟發(fā)函數(shù)

傳統(tǒng)蟻群算法采用從當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j之間距離的倒數(shù)作為啟發(fā)函數(shù)ηij。但是在柵格環(huán)境中，螞蟻在尋路時，由于正反饋?zhàn)饔貌幻黠@，容易陷入局部最優(yōu)。針對這種情況，引入路徑指導(dǎo)函數(shù)，如公式(6)所示：

(6)

式中djG為下一可選節(jié)點(diǎn)j與目標(biāo)點(diǎn)G之間的距離。

新的啟發(fā)函數(shù)為

(7)

在路徑指導(dǎo)函數(shù)的作用下，可以加快算法的收斂速度，為機(jī)器人快速地規(guī)劃出較優(yōu)的全局路徑。

2 多機(jī)器人沖突預(yù)判與消解

2.1 沖突檢查

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

(8)

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

若滿足該公式，表示二者沿著初始路徑移動時，不會在安全時間內(nèi)到達(dá)該交叉點(diǎn)Wi，j，不會發(fā)生路徑?jīng)_突，稱該交叉點(diǎn)為偽沖突點(diǎn)Mi，j。若不滿足該公式，表示二者將會發(fā)生路徑?jīng)_突，需要進(jìn)行路徑協(xié)調(diào)。

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

2.2 沖突消解

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

1) 暫停策略

對機(jī)器人Ri進(jìn)行新的安全判定，判斷公式為

(9)

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

2) 更新策略

機(jī)器人Ri將沖突節(jié)點(diǎn)Zi視為障礙物柵格，系統(tǒng)重新為其規(guī)劃出一條從起始點(diǎn)至目標(biāo)點(diǎn)的路徑，再與其他機(jī)器人的路徑進(jìn)行安全判斷。若不會產(chǎn)生新的沖突節(jié)點(diǎn)，則令Ri沿著新規(guī)劃的路徑移動；若產(chǎn)生新的沖突節(jié)點(diǎn)，則將新的沖突點(diǎn)視為障礙物柵格，重新對Ri進(jìn)行全局路徑規(guī)劃。

分別使用上述2種方法進(jìn)行路徑協(xié)調(diào)，比較二者所需時間，選擇時耗較少的方法進(jìn)行路徑協(xié)調(diào)。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 改進(jìn)蟻群算法的實(shí)驗(yàn)與分析

為了驗(yàn)證改進(jìn)蟻群算法的高效性，使用MATLAB軟件進(jìn)行實(shí)驗(yàn)仿真。在相同的環(huán)境下，分別使用改進(jìn)的蟻群算法和傳統(tǒng)蟻群算法(ACO)進(jìn)行路徑規(guī)劃。參數(shù)選擇如表1所示，尋得最優(yōu)路徑如圖2所示(圖中S表示起始點(diǎn)，G表示目標(biāo)點(diǎn))，算法收斂曲線如圖3所示，實(shí)驗(yàn)結(jié)果數(shù)據(jù)如表2所示。

圖2 最優(yōu)路徑對比Figure 2 Comparison of optimal paths

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

表1 算法參數(shù)選擇

表2 仿真結(jié)果數(shù)據(jù)

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

3.2 多機(jī)器人路徑規(guī)劃

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

在實(shí)驗(yàn)1中，機(jī)器人集群的初始路徑和協(xié)調(diào)路徑分別如圖4～5所示，實(shí)驗(yàn)結(jié)果數(shù)據(jù)如表3所示。

圖4 20*20環(huán)境下初始路徑規(guī)劃Figure 4 Initial path planning in 20*20 environment

圖5 20*20環(huán)境下協(xié)調(diào)路徑規(guī)劃Figure 5 Coordinated path planning in 20*20 environment

表3 實(shí)驗(yàn)1數(shù)據(jù)結(jié)果

在實(shí)驗(yàn)2中，機(jī)器人集群的初始路徑和協(xié)調(diào)路徑分別如圖6～7所示，實(shí)驗(yàn)結(jié)果數(shù)據(jù)如表4所示。

圖7 25*25環(huán)境下協(xié)調(diào)路徑規(guī)劃Figure 7 Coordinated path planning in 25*25 environment

表4 實(shí)驗(yàn)2數(shù)據(jù)結(jié)果

4 結(jié)論

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

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

2) 課題組所提出的路徑協(xié)調(diào)策略可以有效地檢測和消解多機(jī)器人之間的路徑?jīng)_突問題，減少了機(jī)器人繞行距離和等待時間，保證了多機(jī)器人集群系統(tǒng)的運(yùn)行效率和可靠性。

課題組所提出的路徑規(guī)劃方法還存在一定的局現(xiàn)性，優(yōu)化目標(biāo)較為單一，今后將考慮多個目標(biāo)進(jìn)行優(yōu)化，同時考慮工作環(huán)境中存在動態(tài)障礙物的情況，提高機(jī)器人工作的安全性。