基于雙粒子群算法的礦井搜救機(jī)器人路徑規(guī)劃

封碩，謝廷船，康靖，李建良

(長安大學(xué) 工程機(jī)械學(xué)院， 陜西 西安 710064)

0 引言

礦井發(fā)生事故后，往往受災(zāi)現(xiàn)場情況復(fù)雜，盲目下井救援可能會造成二次傷亡，而且在狹窄環(huán)境下也很難找到被困人員[1]。利用機(jī)器人救災(zāi)為礦井救援帶來了新方向。礦井搜救機(jī)器人能夠探測現(xiàn)場溫度、有毒氣體和煙霧濃度等，能把環(huán)境數(shù)據(jù)傳送給外界，以供救援人員參考，制定合理的援救方案[2]。為了使礦井搜救機(jī)器人可以在復(fù)雜地形中及時避障并完成環(huán)境探測，效率高、距離短的路徑規(guī)劃至關(guān)重要[3]。

目前，國內(nèi)外學(xué)者所研制的礦井搜救機(jī)器人都具備一定的避障能力，已經(jīng)將傳統(tǒng)的進(jìn)化算法,如蟻群算法、遺傳算法等應(yīng)用于礦井搜救機(jī)器人路徑規(guī)劃中[4-5]。相比這些算法，粒子群算法收斂更快、資源更節(jié)省。但是在障礙物復(fù)雜多變的礦井里，傳統(tǒng)的粒子群算法應(yīng)用于路徑規(guī)劃還存在一些不足，如易陷入局部最優(yōu)、迭代后期個體最優(yōu)解易波動,導(dǎo)致求解精度低、算法前期收斂速度慢等問題，嚴(yán)重影響了礦井搜救機(jī)器人路徑規(guī)劃的效率和可靠性[6]。吳高超[7]采用等分可行空間逐段求解的方法建立了礦井搜救機(jī)器人避障模型，以步長作圓，以粒子的位置來表示運(yùn)動角度，再利用粒子群算法對粒子位置進(jìn)行優(yōu)化，進(jìn)而獲得更有利的避障角度。但是在障礙物繁多的環(huán)境下，該方法成功率較低，未能發(fā)揮出粒子群算法搜索能力強(qiáng)的特點(diǎn)。為了解決上述粒子群算法存在前期收斂速度慢、后期求解精度低的問題，本文引入添加非線性學(xué)習(xí)因子和動態(tài)速度權(quán)重的方法對粒子群算法加以改進(jìn)。針對礦井搜救機(jī)器人在復(fù)雜路況下路徑規(guī)劃成功率低的問題，提出了一種新的路徑規(guī)劃算法，首先將障礙物膨脹化處理為規(guī)則化多邊形，建立環(huán)境模型，再以雙改進(jìn)粒子群算法作為路徑尋優(yōu)算法，當(dāng)傳感器檢測到礦井搜救機(jī)器人正前方一定距離內(nèi)有障礙物時，開始運(yùn)行雙改進(jìn)粒子群算法，然后評估2種粒子群算法得到的路徑是否符合避障條件，若均符合避障條件,則選取最短路徑作為最終路徑，最后得到礦井搜救機(jī)器人在整個路況模型中的最優(yōu)行駛路徑。

1 改進(jìn)粒子群算法

1.1 標(biāo)準(zhǔn)粒子群算法

粒子群優(yōu)化算法(Particle Swarm Optimization，PSO)是一種模擬鳥群覓食行為的仿生智能優(yōu)化算法。PSO算法用隨機(jī)解初始化一群隨機(jī)粒子，通過迭代找到最優(yōu)解，在每一次迭代中，每個粒子通過跟蹤個體極值和全局極值來更新自己。種群迭代過程中，個體粒子不斷更新其在解空間中的速度，以控制個體的飛行方向及距離，不斷向個體極值和全局極值方向迭代[8]。

粒子群算法常規(guī)的數(shù)學(xué)模型有速度和位置更新模型，表達(dá)式分別為

vk+1(i,d)=ωvk(i,d)+c1r1[qk(i,d)-

xk(i,d)]+c2r2[gk(i,d)-xk(i,d)]

(1)

xk+1(i,d)=xk(i,d)+vk+1(i,d)

(2)

式中:vk(i,d),qk(i,d),xk(i,d)和gk(i,d)分別表示算法第k次迭代時粒子i的飛行速度矢量、個體極值、個體值及全局極值的第d維分量；ω為慣性因子，反映粒子的運(yùn)動習(xí)慣，代表粒子有維持先前運(yùn)動的趨勢；c1和c2為學(xué)習(xí)因子，也稱加速因子，c1值越大,代表該粒子向個體極值逼近趨勢越強(qiáng)，c2值越大,代表粒子向全局極值逼近趨勢越強(qiáng)；r1和r2為[0,1]范圍內(nèi)的隨機(jī)數(shù)[9]。

1.2 改進(jìn)的粒子群算法

1.2.1 非線性學(xué)習(xí)因子法(CPSO)

式(1)中的c1、c2分別表示粒子向個體極值和全局極值的加速能力，過大或過小的學(xué)習(xí)因子都不利于粒子群算法迭代。本文提出一種隨著迭代次數(shù)變化而做非線性變化的學(xué)習(xí)因子。

(3)

(4)

式中：c1max,c1min為c1的最大值和最小值；c2max,c2min為c2的最大值和最小值；n為當(dāng)前時刻迭代代數(shù)；M為總迭代代數(shù)。

1.2.2 速度權(quán)重法(PPSO)

由式(2)可看出，標(biāo)準(zhǔn)粒子群算法中粒子位置只與前一時刻的位置和當(dāng)前時刻的速度有關(guān)，若算法后期某次迭代速度過快，個體極值則會過度偏離全局極值，結(jié)果波動較大，導(dǎo)致求解精度低。本文提出了一種加入速度權(quán)重的方法,粒子位置更新公式為

xk+1(i,d)=xk(i,d)+pvk+1(i,d)

(5)

(6)

1.3 算法性能

算法針對不同難易程度的適應(yīng)度函數(shù)得出的數(shù)據(jù)結(jié)果能夠直觀反映該算法的性能優(yōu)劣。為了比較2種改進(jìn)算法在簡單和復(fù)雜地形下的性能優(yōu)劣，選用了2種擁有不同復(fù)雜度地形的適應(yīng)度函數(shù)(也稱為測試函數(shù))做仿真實(shí)驗(yàn)[10]。

首先實(shí)驗(yàn)利用粒子群算法求取2個函數(shù)的最小值，再通過比較求解精度和速度得出算法性能優(yōu)劣。實(shí)驗(yàn)分別以Sphere、Rosenbrock函數(shù)作為適應(yīng)度函數(shù)，分別對應(yīng)f1、f2。

(7)

(8)

(x1,x2,…,xn)為n維坐標(biāo)系下的點(diǎn)坐標(biāo)。f1是一個單峰函數(shù)，只有一個峰頂，在坐標(biāo)為(0,0,…,0)處取得最小值0；f2函數(shù)在坐標(biāo)為(1,1,…,1)處取得最小值0，其最小值位置隱藏在一個平滑、狹長的拋物線形山谷地形內(nèi)，使得算法很難求取該位置[11-13]。

算法種群規(guī)模為50，迭代次數(shù)為25。每個適應(yīng)度函數(shù)均用標(biāo)準(zhǔn)PSO,CPSO,PPSO三種算法進(jìn)行實(shí)驗(yàn)，仿真結(jié)果如圖1、圖2所示。

圖1 Sphere函數(shù)尋優(yōu)實(shí)驗(yàn)

設(shè)定當(dāng)?shù)^程中算法適應(yīng)度值在[0，0.01]范圍時稱為收斂階段，適應(yīng)度值在(0.01，+∞)范圍時稱為初始階段，初次進(jìn)入收斂階段的迭代代數(shù)設(shè)為z，其對應(yīng)的適應(yīng)度值為f。提取圖1、圖2中z和f的值(表1)，并用z值大小表示迭代速度的快慢。

圖2 Rosenbrock函數(shù)尋優(yōu)實(shí)驗(yàn)

表1 算法初次進(jìn)入收斂階段數(shù)據(jù)

比較表1中數(shù)據(jù)可知，針對測試函數(shù)Sphere，PPSO收斂速度最快，其次是CPSO，PSO收斂速度最慢；針對測試函數(shù)Rosenbrock，收斂速度最快的是CPSO，其次是PSO，而PPSO收斂速度最慢。

針對每個測試函數(shù)，使用PSO,CPSO,PPSO算法做30次仿真實(shí)驗(yàn)，每種算法運(yùn)行結(jié)束后得到的最終適應(yīng)度值作為一次仿真實(shí)驗(yàn)結(jié)果。每個算法在以Sphere,Rosenbrock為適應(yīng)度函數(shù)下均能得出30個實(shí)驗(yàn)結(jié)果，對這些結(jié)果求最大值、最小值、平均值、標(biāo)準(zhǔn)差，結(jié)果見表2、表3。

分析表2、表3可得，針對擁有復(fù)雜地形的Rosenbrock函數(shù)，PPSO得到的最大值、最小值、平均值以及標(biāo)準(zhǔn)差均最小，表明其求得的最優(yōu)解精度最高、波動最小，尋優(yōu)效果比PSO和CPSO更好。針對呈單峰地形的Sphere函數(shù)，CPSO算法求得的最優(yōu)解精度最高、波動最小，效果最優(yōu)，PPSO次之,并且差距較小，標(biāo)準(zhǔn)PSO尋優(yōu)效果最差。綜合來看，CPSO收斂速度快于PSO，PPSO的尋優(yōu)效果優(yōu)于PSO。

表2 Sphere函數(shù)對3種算法的測試數(shù)據(jù)

表3 Rosenbrock函數(shù)對3種算法的測試數(shù)據(jù)

具體分析如下：

CPSO中，隨著迭代代數(shù)的增加，式(3)中c1逐漸減小且下降速率增大，式(4)中c2逐漸減小且增長速率減小。算法前期c1值長久保持在較大值，而c2值較小，促進(jìn)了粒子向當(dāng)前個體極值迭代，使得粒子能夠發(fā)散出去。而到算法后期，c1和c2值與前面情況相反，促進(jìn)了粒子向當(dāng)前全局極值迭代，使得粒子更快地向最優(yōu)值逼近。以上2種情況都能夠加快算法迭代速度[14]。

2 障礙物模型建立及路徑規(guī)劃

2.1 障礙物模型建立

在解決礦井搜救機(jī)器人路徑規(guī)劃問題中，正確的環(huán)境建模對于路徑規(guī)劃和搜救機(jī)器人能否完成避障具有關(guān)鍵性的作用[15]。在實(shí)際應(yīng)用中，采用基于激光雷達(dá)的同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)技術(shù)構(gòu)建環(huán)境地圖。圖3(a)是掃描出的二維柵格障礙物，圖3(b)是將圖3(a)中柵格進(jìn)行膨脹化后得到的規(guī)則障礙物。

(a) 柵格化障礙物

(b) 膨脹化障礙物

2.2 路徑規(guī)劃

建立好路況地圖后，礦井搜救機(jī)器人的路徑規(guī)劃還面臨一個難題，就是在相對空曠路段機(jī)器人單步行駛距離(步長)大則效率高，在狹窄路段步長小則路徑規(guī)劃成功率高。針對上述問題，提出了基于2個改進(jìn)粒子群算法(CPSO+PPSO)相結(jié)合的路徑規(guī)劃方法。機(jī)器人單次避障模型如圖4所示。

路徑規(guī)劃步驟如下：

(1) 如圖4(a)所示，在t時刻機(jī)器人位置處Pt建立坐標(biāo)系，PtE與x軸正方向夾角為αt。當(dāng)傳感器未檢測到PtE方向上有障礙物時，機(jī)器人沿著PtE方向行駛。

(2) 當(dāng)傳感器檢測到PtE方向上有障礙物時，在極坐標(biāo)系下建立以步長h為半徑的圓(圖4(b))，種群中每個個體的值代表著路徑的走向。運(yùn)行CPSO算法，首先初始化種群中的個體值，根據(jù)式(1)不斷更新個體值，某次更新后的預(yù)估位置為K點(diǎn)，當(dāng)前位置為O點(diǎn)，然后判斷OK是否穿過障礙物邊界，若沒有穿過邊界，根據(jù)適應(yīng)度函數(shù)f3取小原則選擇出粒子群個體極值和全局極值，否則重新分配粒子位置，最終得出避障最優(yōu)角u。判斷最優(yōu)角u對應(yīng)的OK路徑和障礙物邊界是否相交，若不相交，結(jié)束當(dāng)前算法，得出K點(diǎn)的f3，否則賦值K點(diǎn)f3=10 000。CPSO算法適應(yīng)度函數(shù)為

(a) 未檢測到障礙物

(b) CPSO算法路徑

(c) PPSO算法路徑

f3=h+D1

(9)

式中D1為粒子更新后的位置對應(yīng)的K點(diǎn)到終點(diǎn)E的距離。

(3) CPSO算法結(jié)束后，運(yùn)行PPSO算法。PPSO算法路徑如圖4(c)所示，在極坐標(biāo)系中，將機(jī)器人運(yùn)動分解為10步，步長為h/10。具體迭代過程和步驟(2)中CPSO算法相似，不同的是每次更新的是10個個體值,且算法運(yùn)行一次求出的是10個全局極值。10個全局極值對應(yīng)10段路徑角度，分別為e(1),e(2),…,e(10)，其中,e(1)對應(yīng)著礦井搜救機(jī)器人O-A行駛路線，e(10)對應(yīng)著I-J路線。則該算法得到的最優(yōu)路徑為O—A—B—C—D—E—F—G—H—I—J。若該路徑與障礙物邊界不相交，得出J點(diǎn)的f4，否則賦值J點(diǎn)f4=10 000。PPSO適應(yīng)度函數(shù)為

f4=L+D2

(10)

(11)

式中：L為粒子i更新后的位置對應(yīng)的10段路徑長度之和；D2為J點(diǎn)到終點(diǎn)E的距離；θ為粒子i前后2個維度值之差，是一個角度差,θ=x(i,j)-x(i,j-1)。

(4) 比較CPSO和PPSO算法結(jié)果對應(yīng)的適應(yīng)度值f3和f4，選擇適應(yīng)度值小的路徑作為礦井搜救機(jī)器人的最終路徑。

3 仿真結(jié)果與分析

建立井下障礙物地圖，對改進(jìn)的雙粒子群算法進(jìn)行仿真實(shí)驗(yàn)。算法程序中參數(shù)設(shè)置如下：

礦井搜救機(jī)器人運(yùn)動起點(diǎn)為S(60,60)，終點(diǎn)為E(250,250)，步長h=4 dm。

PPSO算法：迭代代數(shù)為30，其他參數(shù)均與CPSO算法相同。

為了便于分析和觀察CPSO和PPSO算法結(jié)合的過程，給出了礦井搜救機(jī)器人的路徑規(guī)劃仿真以及2個位置路徑選取細(xì)節(jié)，如圖5—圖7所示。

圖5 路徑規(guī)劃仿真結(jié)果

圖6中2種算法均完整運(yùn)行3次，最終礦井搜救機(jī)器人路徑為A—C—D—E，路徑選取流程如下：

(1) CPSO和PPSO分別得出2條路徑，即虛線AC和折線AB，由式(9)計算出C點(diǎn)處的f3，由式(10)得出B點(diǎn)處的f4，且f3

圖6 圖5中第1處路徑

圖7 圖5中第2處路徑

(2) CPSO和PPSO分別得出虛線CF和折線CD，由于虛線CF與障礙物邊界相交，所以F處f3=10 000，D處f4小于f3，取折線CD作為最終路徑。

(3) CPSO和PPSO分別得出虛線DG和折線DE，E處f3小于G處f4，取折線DE作為最終路徑。

圖7中，地形較為開闊，CPSO和PPSO各運(yùn)行了6次，實(shí)取路徑均是CPSO計算生成的虛線線段，機(jī)器人行走的路線為A—B—C—D—E—F—G。

根據(jù)此次仿真數(shù)據(jù)得出，礦井搜救機(jī)器人按照2.2節(jié)步驟(1)走了56步，CPSO和PPSO算法均運(yùn)行了24次。算法得出的終點(diǎn)為(248.19，248.18)，由于到E點(diǎn)的距離小于機(jī)器人步長，因此最后一步是直達(dá)E點(diǎn)。所以，理論路徑長度為

L1|dm=(56+24)×4+

(12)

按照Matlab仿真的數(shù)據(jù)計算出路徑長度為325.41 dm，耗時223.35 s。算法中粒子每走一步，需要將粒子極坐標(biāo)系下坐標(biāo)轉(zhuǎn)換成直角坐標(biāo)系下坐標(biāo)，再加上Matlab數(shù)據(jù)有效位有限和距離公式誤差等原因，導(dǎo)致理論路徑長度和仿真計算長度不等，本文均以仿真數(shù)據(jù)計算的路徑長度為準(zhǔn)。

分別采用PSO,PSO+PSO和CPSO+PPSO算法進(jìn)行路徑規(guī)劃仿真,仿真次數(shù)為50，測試數(shù)據(jù)見表4。從表4可知，CPSO+PPSO的最優(yōu)路徑最短、成功率最高、平均路徑長度最短，運(yùn)行時間雖然最長,但是與其他2種算法差距較小。實(shí)驗(yàn)結(jié)果表明，改進(jìn)的雙粒子群算法和路徑規(guī)劃模型的有效結(jié)合，提高了路徑規(guī)劃成功率，縮短了路徑長度。

表4 3種方案的測試數(shù)據(jù)

4 結(jié)論

(1) 針對礦井搜救機(jī)器人路徑規(guī)劃問題，提出了基于雙粒子群算法的路徑規(guī)劃方法。利用基于激光雷達(dá)的SLAM技術(shù)建立柵格化障礙物模型，再經(jīng)過膨脹化處理形成規(guī)則圖形，并投影到二維坐標(biāo)系中，構(gòu)建井下環(huán)境模型。

(2) 通過改進(jìn)學(xué)習(xí)因子和添加動態(tài)速度權(quán)重提高了粒子群算法的收斂速度，降低了最優(yōu)解波動幅度。CPSO算法粒子步長大，適用于相對開闊地帶尋找路徑。PPSO算法粒子步長小，擅長在障礙物形狀復(fù)雜多變地帶尋找路徑。2種算法共同尋找路徑的方案提高了路徑規(guī)劃效率,在復(fù)雜路段中能夠?qū)ふ业阶顑?yōu)路徑。

(3) 在構(gòu)建的環(huán)境模型中進(jìn)行3種算法的仿真實(shí)驗(yàn)，結(jié)果證明,CPSO和PPSO結(jié)合的雙粒子群算法提高了路徑規(guī)劃成功率，縮短了路徑長度。