基于蟻群粒子群融合的多機(jī)器人協(xié)同路徑規(guī)劃算法

黃 駿，朱 強(qiáng)

(蕪湖職業(yè)技術(shù)學(xué)院 智能制造學(xué)院，安徽 蕪湖 241006)

隨著科技的不斷發(fā)展，智能機(jī)器人被應(yīng)用到工業(yè)、物流業(yè)、電力行業(yè)等越來越多的領(lǐng)域。規(guī)劃科學(xué)合理的避障路徑是決定智能機(jī)器人作業(yè)效率及作業(yè)執(zhí)行效果的關(guān)鍵[1]。多機(jī)器人在協(xié)同作業(yè)過程中，易發(fā)生相互碰撞，極大影響作業(yè)效率。李洋等[2]基于自適應(yīng)步長(zhǎng)快速擴(kuò)展隨機(jī)樹的方法提出了一種雙臂機(jī)器人協(xié)同路徑規(guī)劃算法，提高了碰撞檢測(cè)的有效性。董朝瑞等[3]設(shè)計(jì)了一種基于預(yù)約格柵方法的多機(jī)器人動(dòng)態(tài)路徑規(guī)劃算法，提高了倉(cāng)儲(chǔ)機(jī)器人協(xié)同作業(yè)的系統(tǒng)效率。彭湘等[4]通過引入半自由柵格和半障礙柵格概念，融合蟻群算法得到了多地形情況下移動(dòng)機(jī)器人的最佳路徑規(guī)劃。劉劍等[5]以揀貨智能機(jī)器人為研究對(duì)象，結(jié)合所設(shè)定的能耗與移動(dòng)間距等目標(biāo)函數(shù)，構(gòu)建出3層布谷鳥模型，進(jìn)而得到機(jī)器人協(xié)同路徑規(guī)劃。該方法雖然可以快速得到不同類型食品揀貨機(jī)器人規(guī)劃作業(yè)路徑，卻不能完全避免機(jī)器人間的碰撞，協(xié)同規(guī)劃效果不佳。萬逸飛等[6]以降低機(jī)器人運(yùn)行耗時(shí)、提升路徑平滑度等為研究目標(biāo),采用多目標(biāo)及協(xié)同算法,對(duì)多目標(biāo)函數(shù)的協(xié)調(diào)路徑規(guī)劃問題進(jìn)行求解,得到了多機(jī)器人協(xié)同路徑規(guī)劃。此方法所規(guī)劃的協(xié)同路徑雖然在避障與避碰撞方面均表現(xiàn)良好,但所用算法的運(yùn)算過程過于煩瑣,整體規(guī)劃效率較低。

粒子群算法是通過對(duì)鳥群搜索食物的過程實(shí)施仿真而產(chǎn)生的一種全局性優(yōu)化算法，該算法具有搜索速度快、全局搜索性能強(qiáng)、平穩(wěn)性好等特點(diǎn)，但針對(duì)組合優(yōu)化問題的處理效果稍差[7]。而蟻群算法是通過對(duì)蟻群搜索食物的過程實(shí)施仿真的一種算法，同時(shí)也是一種通過概率搜索最優(yōu)路徑的啟發(fā)式全局優(yōu)化算法。該算法搜索最優(yōu)路徑時(shí)主要依據(jù)路徑的信息素與啟發(fā)函數(shù)2種參數(shù)值[8-9]。蟻群算法具有收斂性能好、魯棒性及搜索準(zhǔn)確性高等特點(diǎn)，尤其適用于處理組合優(yōu)化問題，其缺點(diǎn)主要為搜索效率稍差、運(yùn)算量高、易陷入局部最優(yōu)等[10]。為此，本研究通過融合粒子群算法與蟻群算法，獲得蟻群粒子群融合算法，在暫不考慮機(jī)器人間干擾的情況下，確定各機(jī)器人全局最優(yōu)避障路徑，再結(jié)合多機(jī)器人協(xié)同作業(yè)避碰規(guī)則，規(guī)劃出免碰撞的最佳機(jī)器人間協(xié)同路徑，為多機(jī)器人協(xié)同作業(yè)效率與安全性的提升提供保障。

1 多機(jī)器人協(xié)同路徑規(guī)劃算法

多機(jī)器人協(xié)同路徑規(guī)劃要解決的主要問題是避障路徑規(guī)劃和協(xié)同作業(yè)避碰，最終獲得可避障與避碰的多機(jī)器人最優(yōu)協(xié)同路徑。

1.1 多機(jī)器人協(xié)同作業(yè)的環(huán)境建模與機(jī)器人約束條件設(shè)定

1.1.1 建立多機(jī)器人協(xié)同作業(yè)環(huán)境的柵格模型

在規(guī)劃多機(jī)器人協(xié)同路徑之前，先選用柵格法構(gòu)建多機(jī)器人協(xié)同作業(yè)環(huán)境模型[11]。在建模之前，對(duì)相關(guān)條件進(jìn)行設(shè)定：

1)各機(jī)器人均以1.18 m/s的速度移動(dòng)；

2)機(jī)器人的實(shí)際規(guī)格不予以考量；

3)以各個(gè)柵格的中點(diǎn)坐標(biāo)作為各自柵格的坐標(biāo)；

4)當(dāng)障礙物的大小小于單個(gè)柵格時(shí)，也視其為一個(gè)柵格；

5)機(jī)器人的作業(yè)運(yùn)動(dòng)環(huán)境為二維空間。

按上述設(shè)定條件創(chuàng)建多機(jī)器人協(xié)同作業(yè)環(huán)境柵格模型，其結(jié)構(gòu)如圖1所示。

在圖1中，白色柵格與黑色柵格分別代表自由區(qū)域與障礙物區(qū)域，各機(jī)器人作業(yè)運(yùn)動(dòng)時(shí)需避開全部障礙物區(qū)域，在自由區(qū)域內(nèi)完成作業(yè)運(yùn)動(dòng)。其中，柵格j與二維空間內(nèi)的坐標(biāo)(xj，yj)相對(duì)應(yīng)，將柵格空間內(nèi)按著橫、縱坐標(biāo)方向的行、列柵格數(shù)量設(shè)定為Mx與My。將圖內(nèi)左上角第1個(gè)柵格設(shè)定為首個(gè)柵格，其坐標(biāo)為(0.5,5.5);將首個(gè)柵格右側(cè)的柵格依次設(shè)定為柵格2～6,并按條件設(shè)定柵格的坐標(biāo)。每個(gè)柵格坐標(biāo)同其序號(hào)之間的關(guān)聯(lián)式為

(1)

式中,mod(j/Mx)與cell(j/My)分別為j/Mx的余數(shù)和j/My的整數(shù)。

柵格經(jīng)過二維平面化處理后,經(jīng)由各機(jī)器人自身傳感器實(shí)時(shí)采集作業(yè)環(huán)境信息,通過膨化操作處理作業(yè)環(huán)境內(nèi)的各個(gè)障礙物,令其占據(jù)若干個(gè)柵格[12-13]。設(shè)Ai(x0,y0)與Qi(x1,y1)為第i個(gè)機(jī)器人的運(yùn)動(dòng)初始點(diǎn)與終點(diǎn)坐標(biāo),以柵格信息為依據(jù),在兩點(diǎn)之間為機(jī)器人查詢符合約束條件的最佳路徑。按此方法獲取全部機(jī)器人的最佳路徑。

1.1.2 設(shè)定機(jī)器人約束條件

在規(guī)劃?rùn)C(jī)器人路徑的過程中，需設(shè)定位移、避障、結(jié)束運(yùn)動(dòng)等約束條件[14]。設(shè)Bi為第i個(gè)機(jī)器人的路徑，它包含多個(gè)柵格序號(hào)，即Bi=(bi1，bi2，…，bik)，其中，k為路徑中的總柵格數(shù)。該機(jī)器人最短路徑長(zhǎng)度Di為

(2)

式中，n為機(jī)器人的總數(shù)量。

機(jī)器人約束條件設(shè)定如下：

1)位移約束條件：此約束條件是對(duì)機(jī)器人的運(yùn)動(dòng)方向?qū)嵤┘s束，設(shè)定其僅可朝向左、右、前、后4個(gè)方向產(chǎn)生平面位移運(yùn)動(dòng)與暫停，且單次位移運(yùn)動(dòng)只能選擇一個(gè)方向。約束條件表示為

(3)

式中：(xc，yc)為機(jī)器人行走至第c步時(shí)所對(duì)應(yīng)的坐標(biāo)，l為常數(shù)。

2)避障約束條件：該約束條件是對(duì)機(jī)器人距障礙物的間距實(shí)施約束，約束條件表示為

(4)

式中，(xe，ye)為障礙物柵格的坐標(biāo)。

3)結(jié)束運(yùn)動(dòng)約束條件：該約束條件約定機(jī)器人抵達(dá)目標(biāo)點(diǎn)時(shí)才可以結(jié)束路徑的搜索。約束條件表示為

(5)

由公式(2)可知，如果機(jī)器人由c-2步至c-1步的過程中所行走的步數(shù)為1，且不考慮是向前還是向后運(yùn)動(dòng)，那么，當(dāng)機(jī)器人由c-1步至c步時(shí)，既能夠待在原地不動(dòng)(也就是步數(shù)為0)，也能夠繼續(xù)運(yùn)動(dòng)1步。但是，如果機(jī)器人由c-2步至c-1步的過程中的步數(shù)為0時(shí)，即機(jī)器人待在原地不動(dòng)，則此時(shí)機(jī)器人留在此處。

1.2 基于蟻群粒子群融合的多機(jī)器人避障路徑規(guī)劃過程

1.2.1 粒子群算法下多機(jī)器人最優(yōu)參考路徑預(yù)規(guī)劃

初始階段任意分布在解區(qū)間的一組粒子構(gòu)成了算法的起始狀態(tài)，其作用是為了提高后續(xù)迭代的效率及效果。在通過粒子群算法預(yù)規(guī)劃各機(jī)器人最優(yōu)參考路徑過程中，可采取對(duì)各粒子的速度與位置持續(xù)更新的方式獲得最優(yōu)解，即各機(jī)器人的最優(yōu)參考路徑。粒子位置與速度更新的表達(dá)式為

(6)

式中:Xij(t)為第(i,j)個(gè)粒子的初始位置,Xij(t+1)為t+1時(shí)刻更新后的位置;Vij(t)與Vij(t+1)為粒子的初始速度與更新之后的速度;μ為加權(quán)系數(shù);σ1與σ2為學(xué)習(xí)因子;rand(·)為隨機(jī)數(shù),且其取值范圍為(0,1);hB與Bi j,B分別為當(dāng)前粒子群的全局最優(yōu)解與第(i,j)個(gè)粒子的最優(yōu)解。

在建立的柵格模型內(nèi),確定障礙物、終點(diǎn)、初始點(diǎn)的坐標(biāo),并將各粒子視為對(duì)應(yīng)機(jī)器人,對(duì)全部粒子的位置與速度、粒子群算法的各參數(shù)均進(jìn)行初始化處理。運(yùn)用公式(6)持續(xù)更新各粒子的位置與速度,連續(xù)迭代至最高迭代次數(shù),μ值緩慢降低,實(shí)現(xiàn)全局最優(yōu)解及個(gè)體最優(yōu)解的反復(fù)更新調(diào)整。得到的最優(yōu)解即為各機(jī)器人的最優(yōu)參考路徑,從而完成了各機(jī)器人的路徑預(yù)規(guī)劃。

1.2.2 蟻群算法下多機(jī)器人全局最優(yōu)避障路徑

(7)

在規(guī)劃路徑過程中，螞蟻柵格路徑的啟發(fā)函數(shù)和信息素濃度這2類參數(shù)值可正向決定螞蟻選擇此柵格作為可行柵格的概率。在初始柵格處放置螞蟻，在公式(7)的基礎(chǔ)上，螞蟻由所處柵格j向之后柵格j+1運(yùn)動(dòng)的概率Pj+1的計(jì)算公式為

(8)

式中：δj為啟發(fā)函數(shù)，γ為期望啟發(fā)因子，Ji為螞蟻所處柵格j的全部后續(xù)可行柵格集合，m為該集合的總數(shù)量。由此，2個(gè)柵格之間路徑的啟發(fā)函數(shù)δj，j+1的計(jì)算公式為

(9)

式中：Lj，j+1為所處柵格j與可行柵格j+1的間距，ω1為L(zhǎng)j，j+1的權(quán)重系數(shù)，Lj+1，Q為可行柵格j+1距目標(biāo)柵格Q的距離，ω2為L(zhǎng)j+1，Q的權(quán)重系數(shù)。

(10)

(11)

最佳路徑上的信息素濃度與迭代次數(shù)成正比關(guān)系，當(dāng)達(dá)到最高迭代次數(shù)之后，結(jié)束路徑搜索過程，確定全局最優(yōu)避障路徑。

1.3 基于作業(yè)避碰規(guī)則的多機(jī)器人協(xié)同路徑規(guī)劃

為有效防止各機(jī)器人相互之間的碰撞，需進(jìn)一步規(guī)劃出多機(jī)器人協(xié)同避碰路徑，實(shí)現(xiàn)多機(jī)器人的協(xié)同作業(yè)。為實(shí)現(xiàn)此目標(biāo)，需設(shè)定多機(jī)器人協(xié)同作業(yè)避碰規(guī)則[15]，并以此規(guī)則得到最佳或者接近最佳的路徑組合，即多機(jī)器人最佳協(xié)同路徑，最終實(shí)現(xiàn)多機(jī)器人協(xié)同路徑規(guī)劃目標(biāo)。

(12)

由此，根據(jù)式(2)可完成多機(jī)器人協(xié)同路徑規(guī)劃。

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

以某變電站的多個(gè)智能巡檢機(jī)器人為例，運(yùn)用本文所提算法對(duì)此類機(jī)器人巡檢作業(yè)中的協(xié)同路徑實(shí)施規(guī)劃，通過規(guī)劃結(jié)果檢驗(yàn)本文所提算法的實(shí)際應(yīng)用性能。

從該變電站的智能巡檢機(jī)器人中抽選出2個(gè)尺寸規(guī)格相同的機(jī)器人A和B,在MATLAB 2017軟件環(huán)境下進(jìn)行仿真實(shí)驗(yàn),實(shí)驗(yàn)關(guān)鍵參數(shù)設(shè)定見表1。

表1 實(shí)驗(yàn)關(guān)鍵參數(shù)設(shè)定

在上述實(shí)驗(yàn)條件下,先通過本文所提算法創(chuàng)建2個(gè)實(shí)驗(yàn)機(jī)器人協(xié)同作業(yè)環(huán)境的柵格模型(圖2)。

2.1 路徑規(guī)劃效果測(cè)試

使用本文算法中的基礎(chǔ)粒子群算法預(yù)規(guī)劃出各機(jī)器人的最優(yōu)參考路徑。預(yù)規(guī)劃效果如圖3所示。

注：灰色柵格—機(jī)器人的初始點(diǎn)與目標(biāo)點(diǎn)；灰色實(shí)線—機(jī)器人A的路徑；黑色虛線—機(jī)器人B的路徑。

以路徑預(yù)規(guī)劃結(jié)果為基礎(chǔ)，繼續(xù)融合蟻群算法繼而獲得各機(jī)器人的全局最優(yōu)避障路徑，結(jié)果如圖4所示。

由圖3和圖4可知，預(yù)規(guī)劃最優(yōu)參考路徑與全局最優(yōu)避障路徑均能夠有效避開所有障礙柵格，但預(yù)規(guī)劃的2個(gè)機(jī)器人的最優(yōu)參考路徑的長(zhǎng)度均高于融合蟻群算法規(guī)劃所得的路徑?？梢姡疚乃崴惴ㄒ?guī)劃所得的最優(yōu)避障路徑更為理想。

由于運(yùn)用本文所提算法所得的2個(gè)機(jī)器人全局最優(yōu)避障路徑存在重合碰撞路徑，所以依據(jù)作業(yè)避碰規(guī)則得到2個(gè)機(jī)器人的最終協(xié)同路徑規(guī)劃結(jié)果(圖5)。

由圖5可知,運(yùn)用本文所提算法最終得到的2個(gè)機(jī)器人協(xié)同路徑中,通過令機(jī)器人A在黑色實(shí)線路徑上[坐標(biāo)范圍為(14.5,13.5)～(16.5,13.5)]折返一次,可將2個(gè)機(jī)器人的重合碰撞路徑消除掉,同時(shí)未改變最優(yōu)路徑,從而實(shí)現(xiàn)了多機(jī)器人協(xié)同路徑規(guī)劃。

2.2 耗時(shí)測(cè)試

為進(jìn)一步檢驗(yàn)本文所提算法的綜合性能，對(duì)規(guī)劃過程中各階段的耗時(shí)進(jìn)行統(tǒng)計(jì)，結(jié)果見表2。

表2 規(guī)劃過程中各階段耗時(shí)統(tǒng)計(jì)結(jié)果 單位：ms

由表2可知，本文所提算法對(duì)2個(gè)機(jī)器人路徑規(guī)劃的整體耗時(shí)結(jié)果相差不大，且平均耗時(shí)低于200 ms。

3 結(jié)語

在多機(jī)器人共同作業(yè)情況下，科學(xué)合理的多機(jī)器人協(xié)同路徑是保障作業(yè)效率與效果的關(guān)鍵。本文以粒子群算法預(yù)規(guī)劃所得最優(yōu)參考路徑作為蟻群算法的信息素加強(qiáng)值，實(shí)現(xiàn)了多個(gè)機(jī)器人最優(yōu)全局避障路徑的規(guī)劃；在此基礎(chǔ)上，為避免協(xié)同作業(yè)時(shí)多個(gè)機(jī)器人之間產(chǎn)生的相互碰撞，引入作業(yè)避碰規(guī)則，消除了各機(jī)器人協(xié)同作業(yè)時(shí)的碰撞路徑，獲得了最終多機(jī)器人協(xié)同路徑。實(shí)驗(yàn)仿真結(jié)果表明，本文所提算法規(guī)劃效率較高，能保障多機(jī)器人協(xié)同路徑規(guī)劃的時(shí)效性，綜合性能表現(xiàn)較好。