蘭世豪，韓濤，黃友銳，徐善永

(安徽理工大學(xué) 電氣與信息工程學(xué)院， 安徽 淮南 232000)

0 引言

隨著煤礦開采智能化、無人化的發(fā)展，煤礦移動機(jī)器人在煤礦運輸、救援、勘測等方面得到了快速發(fā)展[1]。良好的自主導(dǎo)航技術(shù)是煤礦移動機(jī)器人在煤礦復(fù)雜、未知環(huán)境中正常工作的保障，而自主導(dǎo)航技術(shù)的研究與路徑規(guī)劃算法的研究密不可分，局部路徑規(guī)劃算法是應(yīng)對復(fù)雜動態(tài)環(huán)境的良好解決辦法[2]。

經(jīng)典的局部路徑規(guī)劃算法有bug避障算法[3]、人工勢場法[4]、增強(qiáng)向量場直方圖法[5-6]等，它們在簡單環(huán)境中可以實時避開障礙物，但面對狹長走廊或密集障礙物時，算法會出現(xiàn)抖動或陷入局部極值，導(dǎo)致規(guī)劃路徑不合理和安全度降低。且以上算法規(guī)劃出路徑后，需根據(jù)規(guī)劃好的路徑計算移動機(jī)器人行駛的速度，才能用于實際移動機(jī)器人運動，不能直接產(chǎn)生移動機(jī)器人行駛所需的速度。為此，D.Fox等[7]提出了動態(tài)窗口算法(Dynamic Window Algorithm，DWA)，該算法在考慮機(jī)器人動力學(xué)約束和環(huán)境因素的基礎(chǔ)上,直接產(chǎn)生一組機(jī)器人下一時刻行駛的避障速度，但是在復(fù)雜環(huán)境中要輸出準(zhǔn)確最優(yōu)速度，需要對速度空間進(jìn)行多組均勻等分采樣并評價比較，這就導(dǎo)致算法評價的速度過慢且無法產(chǎn)生最優(yōu)速度。P. Saranrittichai等[8]提出了區(qū)域動態(tài)窗口算法，將算法模擬軌跡上的障礙物擴(kuò)大到臨近軌跡，增加了機(jī)器人搜索附近障礙物的能力，避免了機(jī)器人撞到一些靠近軌跡但不在軌跡上的障礙物，該算法具有良好的魯棒性，但難以解決面對復(fù)雜障礙物時規(guī)劃路徑實時性差的問題。A. Maroti等[9]提出了一種考慮機(jī)器人自身尺寸和U型障礙物開口寬度關(guān)系的改進(jìn)動態(tài)窗口算法，在一定程度上解決了局部極小值問題，但在機(jī)器人行駛速度過快時，算法不能及時輸出最優(yōu)速度，導(dǎo)致機(jī)器人易陷入U型障礙物內(nèi)。王永雄等[10]提出了自適應(yīng)參數(shù)動態(tài)窗口算法，算法評價函數(shù)中的速度權(quán)值可隨環(huán)境自適應(yīng)變化，以使機(jī)器人獲取最佳的運動速度，有效提升了算法的評價速度和機(jī)器人穿越復(fù)雜環(huán)境的能力，但在復(fù)雜環(huán)境中，機(jī)器人自身速度過快時，規(guī)劃軌跡的速度相對較慢，甚至?xí)霈F(xiàn)繞行或碰撞[11]。

針對上述問題，本文提出了一種基于膜計算(Membrane Computing，MC)和粒子群(Particle Swarm Optimization，PSO)的煤礦移動機(jī)器人動態(tài)窗口算法(MCPSO-DWA)。該算法將DWA中煤礦移動機(jī)器人的速度限制空間轉(zhuǎn)換為坐標(biāo)空間，將煤礦移動機(jī)器人的速度坐標(biāo)看作粒子位置，利用粒子群算法將速度空間從均勻等分采樣變?yōu)殡S機(jī)采樣，減少采樣速度的組數(shù)，再利用膜計算的膜內(nèi)更新和膜間交流規(guī)則對粒子群進(jìn)行優(yōu)化，加快種群尋優(yōu)速度。利用DWA中的適應(yīng)度函數(shù)對采樣的粒子進(jìn)行評價并比較，根據(jù)種群最優(yōu)和每個粒子的歷史最優(yōu)更新粒子位置和速度[12-14]，當(dāng)種群迭代次數(shù)或粒子適應(yīng)度值達(dá)到一定條件時，輸出粒子位置，即煤礦移動機(jī)器人下一時刻最佳運動速度，從而使煤礦移動機(jī)器人面對復(fù)雜環(huán)境時能快速規(guī)劃出更加合理的路徑。

1 基于膜計算的粒子群算法

1.1 粒子群算法

粒子群(PSO)算法是一種基于群體協(xié)作的隨機(jī)搜索算法，通過粒子間的不斷協(xié)作和種群迭代來尋找最優(yōu)解。每個粒子根據(jù)自身的位置和相應(yīng)的適應(yīng)度函數(shù)來評價自身的適應(yīng)度值，根據(jù)粒子的適應(yīng)度值來確定每個粒子的歷史最優(yōu)位置和種群最優(yōu)的粒子位置，并利用它們來更新粒子位置和速度[15]。更新后粒子的位置和速度如下:

(1)

(2)

式中:xie(n+1)和xie(n)分別為種群在第n+1次和第n次迭代時第i個粒子在e維的位置；vie(n+1)和vie(n)分別為種群在第n+1次和第n次迭代中第i個粒子在e維的速度；w為慣性權(quán)重[16]，wmin和wmax分別為慣性權(quán)重的最小值和最大值，N為最大迭代次數(shù)；c1和c2為學(xué)習(xí)因子；r1和r2為[0,1]之間的隨機(jī)數(shù)；pie(n)和pge(n)分別為種群第n次迭代時第i個粒子在e維的歷史最優(yōu)位置和種群在e維上的歷史最優(yōu)位置。

1.2 基于膜計算的粒子群算法

圖1 MCPSO工作流程Fig.1 Work flow of MCPSO

基于膜計算的并行計算能力，m個基本膜內(nèi)的粒子同時進(jìn)行搜索并產(chǎn)生各基本膜內(nèi)的全局最優(yōu)，再通過比較得到種群的全局最優(yōu)?；谀び嬎愕牧Ｗ尤核惴梢栽诒ＷC粒子尋優(yōu)精度的同時提高種群尋優(yōu)速度。

2 基于膜計算和粒子群的動態(tài)窗口算法

2.1 傳統(tǒng)動態(tài)窗口算法

傳統(tǒng)動態(tài)窗口算法(DWA)是根據(jù)煤礦移動機(jī)器人在環(huán)境中的位置和自身速度約束，將煤礦移動機(jī)器人的局部軌跡規(guī)劃轉(zhuǎn)換為速度選擇問題，選取煤礦移動機(jī)器人最優(yōu)軌跡對應(yīng)的速度來驅(qū)動機(jī)器人做下一時刻的運動[20-21]。

煤礦移動機(jī)器人先根據(jù)自身速度和環(huán)境的約束對自身采樣速度進(jìn)行一定的限制。煤礦移動機(jī)器人的速度限制用速度集表示為

Va={(v,ω)|0≤v≤vmax,-ωmax≤ω≤ωmax}

(3)

式中：v，ω分別為煤礦移動機(jī)器人的線速度和角速度；vmax，ωmax分別為煤礦移動機(jī)器人的最大線速度和角速度。

受電動機(jī)力矩影響，煤礦移動機(jī)器人存在最大加減速度，所以，在固定時間間隔θt內(nèi)，煤礦移動機(jī)器人允許的最大速度變化范圍為

(4)

考慮煤礦移動機(jī)器人的安全性，在以最大加減速度下進(jìn)行速度變化行駛過程中，煤礦移動機(jī)器人要與障礙物保持安全的距離。要讓煤礦移動機(jī)器人在碰撞到障礙物之前停止運動，其速度必須在一定的范圍內(nèi)。

(5)

式中dist(v,ω)為煤礦移動機(jī)器人當(dāng)前模擬軌跡與障礙物之間的距離。

最終煤礦移動機(jī)器人的行駛速度集合為

Vf=Va∩Vg∩Vz

(6)

速度采樣后，對速度空間以固定的步進(jìn)進(jìn)行速度采樣，并對采樣的速度進(jìn)行評價，評價函數(shù)為

H(v,ω)=α×heading(v,ω)+β×dist(v,ω)+γ×velocity(v,ω)

(7)

式中：heading(v,ω)為煤礦移動機(jī)器人以當(dāng)前采樣速度行駛到達(dá)模擬軌跡末端時的煤礦移動機(jī)器人朝向與目標(biāo)點之間的角度差；velocity(v,ω)為采樣速度中的線速度；α，β，γ為3個權(quán)值參數(shù)，評價速度之前先將目標(biāo)函數(shù)的3個參數(shù)歸一化處理為[0,1]之間的參數(shù)，避免某個參數(shù)權(quán)重較大而影響速度的評價質(zhì)量。

通過評價函數(shù)H(α=0.4,β=0.2,γ=0.4)對機(jī)器人的速度空間[(0,2)m/s;(-50,50)rad/s]以步進(jìn)(0.01 m/s,0.2 rad/s)進(jìn)行評價和比較，最大H值對應(yīng)的(v,ω)即為煤礦移動機(jī)器人下一時刻行駛的線速度和角速度?；贒WA的煤礦移動機(jī)器人運動軌跡如圖2所示，圖中黑色正方形和圓形為經(jīng)過膨化后的障礙物。

從圖2可看出，煤礦移動機(jī)器人在面對復(fù)雜稠密障礙物時無法穿越并出現(xiàn)了繞行情況，增加了煤礦移動機(jī)器人的總行駛路程。

圖2 基于DWA的煤礦移動機(jī)器人運動軌跡Fig.2 Mine mobile robot trajectory based on DWA

2.2 基于膜計算和粒子群的動態(tài)窗口算法

DWA在復(fù)雜環(huán)境中規(guī)劃路徑不合理，規(guī)劃效率低，其原因為算法評價函數(shù)對其速度采樣空間評價的速度太慢，無法及時產(chǎn)生最優(yōu)速度，導(dǎo)致煤礦移動機(jī)器人穿越稠密障礙物時規(guī)劃路徑不合理。為此，本文提出了基于膜計算和粒子群的動態(tài)窗口算法(MCPSO-DWA)，利用MCPSO對DWA中速度空間進(jìn)行采樣并評價，最終得出煤礦移動機(jī)器人下一時刻行駛的最優(yōu)速度。

2.2.1 算法設(shè)計

將煤礦移動機(jī)器人的速度限制區(qū)域Vf轉(zhuǎn)換為坐標(biāo)區(qū)域，以v和ω為橫坐標(biāo)和縱坐標(biāo)建立坐標(biāo)系，如圖3所示，其中(vmin,vmax)和(-ωmax,ωmax)分別為煤礦移動機(jī)器人的限制速度范圍，陰影部分S為煤礦移動機(jī)器人的速度限制區(qū)域。將S轉(zhuǎn)換為粒子群算法中粒子的搜索區(qū)域，其中(vmin,vmax)和(-ωmax,ωmax)為粒子搜索邊界，將S中坐標(biāo)點(v,ω)轉(zhuǎn)換為粒子對應(yīng)的位置X。粒子在S中不斷更新位置和速度，最終輸出最優(yōu)粒子對應(yīng)的位置(v,ω)，即MCPSO-DWA輸出的最優(yōu)行駛速度。

圖3 速度限制區(qū)域坐標(biāo)Fig.3 Coordinate of speed limit area

初始化一個單層膜結(jié)構(gòu)，其中包括1個表層膜和m個基本膜。在區(qū)域S中，初始化選取Q個粒子，每個粒子的位置代表機(jī)器人的一個速度位置坐標(biāo)，粒子位置和速度的初始化公式為

X=[(vmax,ωmax)-(vmin,-ωmax)]×r+(vmin,ωmax)

(8)

V=[(vmax,ωmax)-(vmin,ωmax)]×0.1×(2r-1)

(9)

式中：X和V分別為粒子初始位置和速度；r為[0,1]之間的隨機(jī)數(shù)。

將Q個粒子均勻分配到m個基本膜當(dāng)中，每個基本膜內(nèi)含有D個粒子，第k(k=1,2,…,m)個基本膜內(nèi)的第d(d=1,2,…,D)個粒子的初始速度為Vkd，初始位置為Xkd。利用評價函數(shù)(式(7))對各基本膜內(nèi)的粒子進(jìn)行評價，根據(jù)每個粒子對應(yīng)評價函數(shù)的得分比較得出每個基本膜內(nèi)的全局最優(yōu)，將各基本膜內(nèi)全局最優(yōu)對應(yīng)的粒子輸出到表層膜中進(jìn)行比較，得到種群全局最優(yōu)，各基本膜內(nèi)粒子根據(jù)表層膜返回的種群全局最優(yōu)和自身歷史最優(yōu)進(jìn)行位置和速度的更新，整個種群不斷迭代更新，直到種群輸出最優(yōu)粒子的位置，即煤礦移動機(jī)器人下一時刻的行駛速度。煤礦移動機(jī)器人不斷根據(jù)單位采樣周期內(nèi)輸出的最優(yōu)速度進(jìn)行行駛，并判斷是否到達(dá)終點。

2.2.2 算法流程

MCPSO-DWA流程如圖4所示。

圖4 MCPSO-DWA流程Fig.4 MCPSO-DWA flow

(1) 根據(jù)煤礦移動機(jī)器人自身動力學(xué)約束和傳感器接收的環(huán)境信息產(chǎn)生煤礦移動機(jī)器人的速度限制區(qū)域Vf。

(2) 將速度限制區(qū)域Vf轉(zhuǎn)換為坐標(biāo)區(qū)域。

(3) 初始化單層膜結(jié)構(gòu)，其結(jié)構(gòu)包括1個表層膜和m個基本膜；在速度限制區(qū)域Vf中隨機(jī)選取Q個粒子，根據(jù)式(8)和式(9)產(chǎn)生粒子的位置和速度，并均勻分配到m個基本膜中，表層膜為空；設(shè)定學(xué)習(xí)因子c1,c2和慣性權(quán)重wmin,wmax，最大迭代次數(shù)N，適應(yīng)度函數(shù)閾值δ。

(8) 膜內(nèi)種群不斷迭代更新，直到達(dá)到最大迭代次數(shù)或者種群全局最優(yōu)Gbest的適應(yīng)度值達(dá)到δ，停止迭代，輸出此時Gbest對應(yīng)粒子的位置坐標(biāo)，即煤礦移動機(jī)器人下一時刻行駛的避障速度，否則返回步驟(3)。

(9) 執(zhí)行輸出的避障速度，并判斷煤礦移動機(jī)器人是否達(dá)到目標(biāo)點，若達(dá)到，則停止煤礦移動機(jī)器人運動，否則返回步驟(1)，繼續(xù)搜索下一時刻的最優(yōu)避障速度，直到達(dá)到終點。

2.2.3 算法應(yīng)用

MCPSO-DWA改進(jìn)了傳統(tǒng)DWA速度空間中速度的采樣方式，利用MCPSO變均勻等分采樣為隨機(jī)采樣，將采樣的速度模擬為煤礦移動機(jī)器人下一時刻的行駛軌跡，再結(jié)合煤礦移動機(jī)器人接收到的復(fù)雜環(huán)境信息，對每條模擬軌跡進(jìn)行評價，多次迭代并產(chǎn)生煤礦移動機(jī)器人下一時刻行駛的最優(yōu)速度。要達(dá)到煤礦移動機(jī)器人根據(jù)連續(xù)時間段間隔內(nèi)輸出的最優(yōu)速度進(jìn)行路徑規(guī)劃，就要求算法在固定時間間隔內(nèi)及時產(chǎn)生煤礦移動機(jī)器人下一時刻的最優(yōu)行駛速度，煤礦移動機(jī)器人用最優(yōu)速度行駛，并按固定時間間隔進(jìn)行一次最優(yōu)行駛速度的更新，使煤礦移動機(jī)器人在連續(xù)時間間隔內(nèi)都可以采用最優(yōu)速度行駛，實現(xiàn)實時的局部避障規(guī)劃，以保證煤礦移動機(jī)器人在煤礦復(fù)雜多變的環(huán)境中安全高效行駛。

3 實驗結(jié)果及分析

為驗證MCPSO-DWA的有效性，分別在不同復(fù)雜度的環(huán)境中，對比MCPSO-DWA和DWA的性能，對煤礦移動機(jī)器人行駛的路程、時間和穿越密集障礙物的能力等進(jìn)行比較和分析。

3.1 實驗參數(shù)設(shè)定

利用Python 3.7進(jìn)行實驗仿真，構(gòu)建的基本地圖如圖2所示，在此基礎(chǔ)上驗證煤礦移動機(jī)器人遇到障礙物時規(guī)劃路徑的能力。

MCPSO-DWA的相關(guān)參數(shù)配置見表1，在速度坐標(biāo)區(qū)域中共選取20個粒子，并均勻分配到4個基本膜中。根據(jù)煤礦移動機(jī)器人的實際情況配置DWA參數(shù),見表2，對障礙物進(jìn)行半徑為0.5 m的膨化處理。

表1 MCPSO-DWA參數(shù)配置Table 1 Parameters configuration of MCPSO-DWA

表2 DWA參數(shù)配置Table 2 Parameters configuration of DWA

3.2 仿真實驗與分析

為驗證MCPSO-DWA在不同場景的適應(yīng)能力，先在4種不同復(fù)雜度環(huán)境的地圖上對算法性能進(jìn)行驗證，4種環(huán)境的復(fù)雜度依次增加，在環(huán)境3和環(huán)境4中，障礙物密集度不斷提升。煤礦移動機(jī)器人在4種環(huán)境中利用DWA和MCPSO-DWA規(guī)劃的最優(yōu)路徑對比如圖5所示；煤礦移動機(jī)器人在4種環(huán)境中按照MCPSO-DWA和DWA規(guī)劃路徑行駛時的總步數(shù)與每步評價次數(shù)對比如圖6所示。MCPSO-DWA運行步數(shù)和每步迭代次數(shù)的關(guān)系如圖7所示。

從圖5(a)、圖5(b)可看出，在環(huán)境1和環(huán)境2中,DWA和MCPSO-DWA都可以穿越障礙物，規(guī)劃出比較合理路徑。從圖6(a)、圖6(b)可看出，在環(huán)境1和環(huán)境2中規(guī)劃路徑時，DWA迭代的總步數(shù)分別為58和75，由表2中DWA相關(guān)參數(shù)所決定，故每步對應(yīng)評價次數(shù)均為800；在環(huán)境1和環(huán)境2中規(guī)劃路徑時，MCPSO-DWA迭代的總步數(shù)分別為64和70，每步對應(yīng)評價次數(shù)為種群在速度坐標(biāo)區(qū)域迭代搜索時，當(dāng)種群中Gbest的值達(dá)到要求閾值，種群所迭代的次數(shù)n乘以粒子數(shù)Q的值。從圖7可看出MCPSO-DWA運行步數(shù)和每步迭代次數(shù)的關(guān)系，其中由環(huán)境1和環(huán)境2對應(yīng)折線可得MCPSO-DWA運行每步的迭代次數(shù)為5～30，結(jié)合圖6(a)和6(b)可看出，每步評價次數(shù)為100～600，

(a) 環(huán)境1中2種算法規(guī)劃路徑對比

(a) 環(huán)境1中2種算法規(guī)劃步數(shù)與評價次數(shù)對比

低于DWA每步評價次數(shù)。從圖5(c)和圖5(d)可看出， MCPSO-DWA規(guī)劃出的路徑明顯比DWA規(guī)劃出的路徑更加合理，面對稠密障礙物， MCPSO-DWA規(guī)劃的路徑可以安全穿越；從圖6(c)和圖6(d)可看出，MCPSO-DWA迭代總步數(shù)分別為76和124，也分別少于DWA迭代總步數(shù)83和165。同時從圖7中的環(huán)境3、環(huán)境4對應(yīng)折線和圖6(c)、圖6(d)可看出， MCPSO-DWA運行每步的迭代次數(shù)為10～35，每步評價次數(shù)為200～700，也均少于DWA每步800次的評價次數(shù)。

圖7 MCPSO-DWA規(guī)劃步數(shù)與每步迭代次數(shù)的關(guān)系Fig.7 Relationship between the number of planning steps and the number of iterations per step of MCPSO-DWA

表3給出了DWA和MCPSO-DWA在以上環(huán)境中規(guī)劃路徑的數(shù)據(jù)對比。從表3可看出，對于簡單的環(huán)境1， MCPSO-DWA規(guī)劃路徑的時間和總步數(shù)雖然不占優(yōu)勢，但規(guī)劃路徑的長度比DWA要短8.77%；對于一般復(fù)雜度的環(huán)境2和環(huán)境3， MCPSO-DWA規(guī)劃路徑用時分別比DWA規(guī)劃路徑用時要少8.66%和15.53%，并且規(guī)劃路徑的長度也比DWA規(guī)劃路徑長度短2.30%和6.71%；對于復(fù)雜環(huán)境4， MCPSO-DWA相比DWA在總步數(shù)減少41步的情況下，規(guī)劃時間減少31.55%，規(guī)劃路徑長度減少10.02%。

表3 不同環(huán)境規(guī)劃路徑數(shù)據(jù)對比Table 3 Data comparison of planning paths in different environments

由表3中DWA和MCPSO-DWA規(guī)劃路徑長度和用時可知煤礦移動機(jī)器人在不同場景中的行駛速度，見表4。從表4可知，在簡單環(huán)境1中，煤礦移動機(jī)器人采用DWA規(guī)劃路徑時行駛速度比MCPSO-DWA快0.12 m/s，但在復(fù)雜度增加的后3種環(huán)境中，煤礦移動機(jī)器人利用MCPSO-DWA規(guī)劃路徑時行駛速度分別比DWA快0.05，0.08，0.19 m/s。這表明MCPSO-DWA在復(fù)雜環(huán)境中能規(guī)劃出更合理的路徑，同時可以提升煤礦移動機(jī)器人的導(dǎo)航效率。

表4 不同環(huán)境行駛速度對比Table 4 Comparison of driving speed in different environments

多種路徑規(guī)劃算法在含有U型障礙物的環(huán)境中難以逃離U型開口，陷入局部極值。鑒于自適應(yīng)動態(tài)窗口算法(Adaptive-DWA，A-DWA)在復(fù)雜環(huán)境中規(guī)劃路徑合理性和適應(yīng)性強(qiáng)的特點，本文在相同含有U型障礙物環(huán)境中利用MCPSO-DWA，A-DWA和 DWA進(jìn)行路徑規(guī)劃，并對比其合理性，用以驗證MCPSO-DWA在特殊U型障礙物場景中的適應(yīng)性。在含有U型障礙物環(huán)境中3種算法規(guī)劃的最優(yōu)路徑對比如圖8所示。從圖8可看出，DWA和A-DWA在運行過程中面對U型障礙物時未能繞行，在U型障礙物內(nèi)發(fā)生碰撞， MCPSO-DWA可以成功繞過U型開口，到達(dá)終點。這說明MCPSO-DWA在不同復(fù)雜度環(huán)境中都可以進(jìn)行合理的路徑規(guī)劃，不僅可縮短規(guī)劃路徑的長度，還可明顯減少規(guī)劃路徑總步數(shù)和時間。

圖8 U型障礙物環(huán)境中3種算法規(guī)劃的最優(yōu)路徑對比Fig.8 Comparison of optimal planning paths of three algorithms in U-shaped obstacle environment

4 結(jié)語

針對煤礦移動機(jī)器人采用傳統(tǒng)DWA在復(fù)雜環(huán)境中規(guī)劃路徑不合理和規(guī)劃速度慢的問題，提出了基于膜計算和粒子群的煤礦移動機(jī)器人動態(tài)窗口算法(MCPSO-DWA)，該算法通過基于膜計算的粒子群算法對煤礦移動機(jī)器人的速度限制區(qū)域進(jìn)行優(yōu)化，提高了速度采樣的隨機(jī)性和規(guī)劃路徑的合理性，煤礦移動機(jī)器人可根據(jù)連續(xù)時間段間隔內(nèi)輸出的最優(yōu)速度進(jìn)行路徑規(guī)劃。實驗結(jié)果表明，在復(fù)雜環(huán)境規(guī)劃路徑時，與傳統(tǒng)DWA相比，MCPSO-DWA在降低規(guī)劃步數(shù)和每步評價次數(shù)的同時，可縮短7%～10%規(guī)劃路徑長度和9%～32%的規(guī)劃時間，并可適應(yīng)含U型障礙物的特殊環(huán)境。