基于改進(jìn)粒子群算法的電動(dòng)機(jī)器人全局路徑規(guī)劃

陳玲玲，張占奎，鐘元辰，張忠瑞

(1.吉林化工學(xué)院信息與控制工程學(xué)院，吉林 吉林 132000；2.國(guó)網(wǎng)盤(pán)錦供電公司，遼寧 盤(pán)錦 124000；3.國(guó)網(wǎng)遼寧省電力有限公司，遼寧 沈陽(yáng) 110006)

自主導(dǎo)航的電動(dòng)機(jī)器人應(yīng)用范圍包括工業(yè)、軍事、救援、太空、農(nóng)業(yè)和家庭[1]，在這些實(shí)際應(yīng)用中，電動(dòng)機(jī)器人的智能能力決定其能否在碰撞移動(dòng)下順利完成任務(wù)。在這些智能能力中，地圖建模和路徑規(guī)劃是最智慧、最重要的特征。路徑規(guī)劃是智能機(jī)器人和車(chē)輛自主運(yùn)行的關(guān)鍵要素，是指在一個(gè)有障礙物的運(yùn)動(dòng)環(huán)境中，機(jī)器人參考傳感器掃描到的環(huán)境信息，尋找1條不觸碰障礙物的幾何路徑，按照這條路徑能夠安全移動(dòng)到目標(biāo)位置，其要素包括：①存在能夠移動(dòng)到終點(diǎn)的安全路線；②路徑不碰到障礙物且長(zhǎng)度盡可能??；③耗時(shí)少；④無(wú)碰撞。路徑規(guī)劃算法一般包括環(huán)境建模和路徑搜索，每次規(guī)劃任務(wù)都會(huì)有多條到達(dá)目標(biāo)點(diǎn)的路徑，但是最佳路徑的計(jì)算取決于一些決策標(biāo)準(zhǔn)，例如路徑長(zhǎng)度、航行時(shí)間或能量最小化。除了避免碰撞的安全性和效率之外，評(píng)估路徑的標(biāo)準(zhǔn)還可以是其平滑性和可行性、計(jì)算路徑所需的計(jì)算資源。許多優(yōu)化算法已被用于路徑規(guī)劃問(wèn)題[2-3]。這些算法分為經(jīng)典方法、元啟發(fā)式方法2種。通常，由于計(jì)算花費(fèi)時(shí)間較長(zhǎng)且無(wú)法處理環(huán)境中偶然性情況，經(jīng)典方法不常用于實(shí)時(shí)移動(dòng)路徑規(guī)劃。與不能很好解決復(fù)雜環(huán)境問(wèn)題的經(jīng)典算法相比，元啟發(fā)式算法易于實(shí)現(xiàn)，具有處理環(huán)境中存在的不確定性的能力[4-5]。

為了有效解決這個(gè)問(wèn)題，學(xué)者們提出了不同的方法和思路。Mohanata提出了一種新穎的基于Petri-Net模型與遺傳算法(genetic algorithm, GA)相結(jié)合的方法，構(gòu)成了集成導(dǎo)航控制器[6]。Oleiwi開(kāi)發(fā)了一種將蟻群優(yōu)化算法與GA相結(jié)合的混合方法，使用蟻群優(yōu)化算法獲得無(wú)障礙的次優(yōu)路徑，然后作為GA的初始路徑[7]。ZHANG B提出了一種受鴿子啟發(fā)的算法來(lái)規(guī)劃UCAV的三維路徑[8]。李志錕等[9]對(duì)信息素分配和信息素含量的方法進(jìn)行優(yōu)化，并在引入權(quán)重參數(shù)改進(jìn)轉(zhuǎn)移概率公式。

粒子群優(yōu)化算法(particle swarm optimization，PSO)是由Kenned博士和Eberhart教授通過(guò)觀察群體動(dòng)物捕食行為發(fā)明的一個(gè)進(jìn)化計(jì)算方法[10]，靈感來(lái)自于自然生物的運(yùn)動(dòng)表征，研究員發(fā)現(xiàn)個(gè)體之間通過(guò)某些簡(jiǎn)單的交互方式能夠完成整個(gè)群體的繁雜活動(dòng)，局部最優(yōu)通過(guò)某種搜索規(guī)則和個(gè)體之間相互作用產(chǎn)生全局最優(yōu)解。PSO最初用于解決非線性連續(xù)優(yōu)化問(wèn)題，主要應(yīng)用之一是機(jī)器人的路徑規(guī)劃[11-12]。由于PSO需要配置的參數(shù)較少，并且數(shù)學(xué)模型較為簡(jiǎn)單，不涉及比較復(fù)雜的數(shù)學(xué)計(jì)算等優(yōu)點(diǎn)，吸引了很多學(xué)者的關(guān)注并對(duì)其進(jìn)行研究和改進(jìn)，以方便實(shí)際應(yīng)用。然而，標(biāo)準(zhǔn)的PSO存在著提前收斂和全局收斂缺乏保證的缺點(diǎn)，這限制了其在實(shí)際系統(tǒng)中的應(yīng)用，于是很多學(xué)者對(duì)其進(jìn)行了改進(jìn)。文獻(xiàn)[13]引入多個(gè)因子參數(shù)動(dòng)態(tài)調(diào)整算法參數(shù)，并使用魚(yú)群的跳躍機(jī)制增加粒子的多樣性。XIE M將最大化采集數(shù)據(jù)量、路徑安全性和最短行駛距離作為優(yōu)化目標(biāo)，并使用最小-最大歸一化方法計(jì)算適應(yīng)度[14]。Girija提出了將人工勢(shì)場(chǎng)和粒子群結(jié)合的混合算法[15]，該方法將PSO與有源濾波器相結(jié)合，提高了在高障礙物密度環(huán)境下確定可行且代價(jià)最小路徑的速度，并且能夠在較短的時(shí)間里，規(guī)劃出安全的可行駛路徑。本文提出了將粒子適應(yīng)度差值作為算法的反饋信號(hào)來(lái)動(dòng)態(tài)調(diào)節(jié)慣性權(quán)重和引入隨機(jī)因子的改進(jìn)粒子群算法(improved particle swarm optimization，I-PSO)，為自主電動(dòng)機(jī)器人或車(chē)輛在具有許多不同形狀和大小的靜態(tài)障礙物環(huán)境中找到從起點(diǎn)到目的地的無(wú)碰撞全局路徑。

1 環(huán)境模型

對(duì)運(yùn)動(dòng)空間抽象出模型是執(zhí)行路徑規(guī)劃算法的前提，其作用是將機(jī)器人的工作空間映射成路徑規(guī)劃算法可以使用的抽象空間。將工作空間抽象成合理的模型能夠方便計(jì)算機(jī)存儲(chǔ)和計(jì)算，并且可以縮短優(yōu)化算法探索的時(shí)間。為了方便對(duì)空間環(huán)境建立數(shù)學(xué)模型，本文采用的是根據(jù)柵格法建立的二維地圖模型。將周?chē)h(huán)境看成二維平面，將平面進(jìn)行等面積的柵格化處理，空間中無(wú)障礙物位置對(duì)應(yīng)的單元格，用白色填充，障礙物位置對(duì)應(yīng)的單元格，用黑色填充，柵格中的數(shù)字代表機(jī)器人通行該位置所需要的能量值。目標(biāo)是在基于柵格的地圖上找到1條能夠使機(jī)器人安全通行并最終到達(dá)目標(biāo)點(diǎn)的路徑點(diǎn)集合，且這個(gè)集合組成的路徑是所有能到達(dá)目標(biāo)位置的路徑中長(zhǎng)度最短的。路徑規(guī)劃中需要考慮的重點(diǎn)是所選路徑既不能在障礙物位置，也不能與障礙物位置交叉。地圖模型如圖1所示。

圖1 地圖模型

為了保證機(jī)器人不與障礙物發(fā)生碰撞，單元格的大小應(yīng)綜合考慮實(shí)際環(huán)境空間和機(jī)器人的尺寸大小，并且在構(gòu)建柵格地圖時(shí)需要對(duì)空間中的物體及機(jī)器人活動(dòng)區(qū)域邊界進(jìn)行放大，障礙物的大小應(yīng)向外延伸到機(jī)器人自身物理半徑與安全距離之和，這樣在路徑規(guī)劃時(shí)就可以把機(jī)器人看作質(zhì)點(diǎn)。

2 路徑規(guī)劃數(shù)學(xué)模型

規(guī)劃路徑的目的是尋找1條滿足多個(gè)約束條件的路徑曲線。本文將最短路徑和障礙物碰撞風(fēng)險(xiǎn)作為量化尋找路徑的標(biāo)準(zhǔn)。

2.1 路徑長(zhǎng)度函數(shù)

在路徑規(guī)劃方向，最短路徑意味著最小化出發(fā)位置到目標(biāo)位置的路徑長(zhǎng)度，使用歐幾里得距離計(jì)算2個(gè)位置坐標(biāo)的長(zhǎng)度。

(1)

式中：(xi(t),yi(t))代表粒子i在t時(shí)刻的坐標(biāo)位置。

在迭代中，如果第i個(gè)粒子的位置與目標(biāo)點(diǎn)和起點(diǎn)之間的距離最短，則被選為最佳點(diǎn)。路徑規(guī)劃算法計(jì)算出中間節(jié)點(diǎn)、起始位置和目標(biāo)位置之間的連線長(zhǎng)度的累加。

(2)

式中：f1是最短路徑的目標(biāo)函數(shù)；(xs,ys)是規(guī)劃任務(wù)的起點(diǎn)坐標(biāo)；(xg,yg)是規(guī)劃任務(wù)的終點(diǎn)坐標(biāo)。

2.2 碰撞風(fēng)險(xiǎn)

機(jī)器人最重要的目標(biāo)是不能與工作環(huán)境中的障礙物接觸并保持一定的安全距離。因此，需要判斷得到的路線是否與障礙物有交點(diǎn)，或者與障礙物之間的距離太近。為了判斷路徑是否可靠，本文采用下式計(jì)算路徑的碰撞風(fēng)險(xiǎn)。

(3)

(4)

(5)

式中：(xi,yi)是第i個(gè)粒子搜索到的路徑節(jié)點(diǎn)；(xod(k),yod(k))是地圖中第k個(gè)障礙物的中心坐標(biāo)。

基于以上2個(gè)數(shù)學(xué)模型，構(gòu)造包含避免與其路徑上的障礙物碰撞和最小化機(jī)器人規(guī)劃出的路徑歐氏距離的目標(biāo)函數(shù)。函數(shù)是通過(guò)2個(gè)目標(biāo)函數(shù)的加權(quán)獲得。

F=λ1f1+λ2f2

(6)

式中：λ1、λ2分別是最短路徑權(quán)重和碰撞風(fēng)險(xiǎn)權(quán)重。

3 I-PSO的路徑規(guī)劃

PSO的消息分享方式可以看成一種協(xié)作共生的行為，也就是每個(gè)粒子都不斷探尋搜索空間，在其探尋過(guò)程中，種群中其他粒子對(duì)它的動(dòng)作也會(huì)有不同程度的影響，同時(shí)粒子還會(huì)記錄過(guò)往的最優(yōu)答案，即其探尋最佳答案的過(guò)程會(huì)同時(shí)受其他粒子影響和自身經(jīng)驗(yàn)的指引。假設(shè)在1個(gè)D維搜索空間中，存在N個(gè)粒子，每個(gè)粒子都會(huì)根據(jù)下式來(lái)迭代更新自身的坐標(biāo)和速度。

vi(t+1)=ωiυi(t)+c1r1(t)[pi(t)-xi(t)]+c2r2(t)[pg(t)-xi(t)]

(7)

xi(t+1)=xi(t)+vi(t+1)

(8)

式中：ωi是粒子i的慣性權(quán)重；pi(t)是粒子i在t時(shí)刻搜索到的最優(yōu)位置；pg(t)是t時(shí)刻搜索到的全局最優(yōu)位置；c1是設(shè)置粒子向個(gè)體最優(yōu)方向每次運(yùn)動(dòng)長(zhǎng)度的群體學(xué)習(xí)因子；c2是設(shè)置粒子向群體最優(yōu)方向每次運(yùn)動(dòng)長(zhǎng)度的群體學(xué)習(xí)因子；r1和r2為[0,1]范圍內(nèi)的均勻隨機(jī)數(shù)；vi∈[-vmax,vmax]，vmax是粒子的最大速度。

粒子的個(gè)體極值pi更新函數(shù)定義為式(9)。

(9)

式中：f(xi,(t))是適應(yīng)度值。

整個(gè)群體的全局極值pg的更新函數(shù)見(jiàn)式(10)。

pg(t)=min{f(p1(t)),f(p2(t)),…，f(pD(t))}

(10)

式中：f(p1(t))是個(gè)體極值。

3.1 慣性權(quán)重的調(diào)整

為了權(quán)衡粒子的全局探索和開(kāi)發(fā)特性，可以改變每個(gè)粒子移動(dòng)速度的慣性權(quán)重因子，而且慣性權(quán)重ω的值越大，種群的全局探索能力就會(huì)較好，當(dāng)ω的值越小時(shí)，粒子群的局部搜索能力會(huì)較強(qiáng)。因此可以通過(guò)靈活地調(diào)整ω的方式，決定種群在搜索過(guò)程中何時(shí)增大全局搜索或者加強(qiáng)局部搜索。

(11)

(12)

3.2 學(xué)習(xí)因子的調(diào)整

在種群搜索過(guò)程中，讓c1維持較大的值能不斷擴(kuò)大種群的探索區(qū)域，但是這樣會(huì)導(dǎo)致算法過(guò)慢收斂。讓c2維持比較大的值能使粒子盡可能的獲取鄰近粒子的信息，從而加快算法的尋優(yōu)速度，但是得到的解可能不是最優(yōu)的。因此加速度的取值方法對(duì)是否能夠找到最優(yōu)解有很大作用。為了讓粒子在算法初期盡量多的執(zhí)行全局搜索，在算法后期擁有顯著的收斂速度，應(yīng)在算法前期適當(dāng)增大c1且減小c2，在算法后期適當(dāng)減小c1，同時(shí)增大c2。加速因子的更新公式設(shè)計(jì)見(jiàn)式(13)和式(14)。

(13)

(14)

式中：c1b，c1e，c2b，c2e分別是粒子自身和群體學(xué)習(xí)因子的初始值和最終值。

為了降低掉入局部陷阱的概率，本文在更新粒子位置時(shí)，引入一個(gè)隨機(jī)因子，以增加粒子探索的隨機(jī)性。

Li(t)=Li(t)(1+r)

(15)

式中：Li(t)是第i個(gè)粒子在第t次的位置；r是[-0.1, 0.1]的隨機(jī)值。

3.3 PSO流程

PSO流程如圖2所示。

圖2 路徑規(guī)劃流程圖

a.設(shè)置N、D、tmax、Vmax，并初始化ω、c1b、c1e、c2b、c2e、v和x；

b.根據(jù)式(9)和式(10)計(jì)算粒子的F、p，并選出種群的pg；

c.根據(jù)式(11)—(14)，動(dòng)態(tài)調(diào)整每個(gè)粒子的ω、c1和c2。

d.根據(jù)式(7)和式(8)，計(jì)算下一次迭代粒子的v和x。

e.判斷是否達(dá)到規(guī)劃任務(wù)的終止要求。若達(dá)到，結(jié)束迭代，否則轉(zhuǎn)到步驟b。

4 仿真試驗(yàn)

使用MATLAB模擬二維柵格地圖,使用I-PSO和標(biāo)準(zhǔn)PSO規(guī)劃路徑，結(jié)果如圖3所示。為滿足PSO的收斂和穩(wěn)定條件[18]，參數(shù)設(shè)置為：ωmax=0.8，ωmin=0.01，c1b=1.5，c1e=1，c2b=1，c2e=1.5，tmax=100，N=100，D=1，環(huán)境中設(shè)置較少的障礙物。

圖3中，藍(lán)色曲線是基于標(biāo)準(zhǔn)PSO規(guī)劃的路徑，紅色曲線是I-PSO規(guī)劃的路徑，從圖3可看出在障礙物較少時(shí)，2個(gè)算法找到的路徑基本相同，都規(guī)劃出了較優(yōu)的路徑。

圖3 規(guī)劃的路徑1

圖4是2種PSO在迭代搜索最優(yōu)路徑時(shí)函數(shù)適應(yīng)度值變化圖形。I-PSO的適應(yīng)度值迭代曲線，在迭代開(kāi)始階段，其值明顯小于標(biāo)準(zhǔn)PSO的適應(yīng)度值。在迭代中期，I-PSO的適應(yīng)度值已經(jīng)趨近于穩(wěn)定?？梢钥闯鯥-PSO較標(biāo)準(zhǔn)PSO在收斂速度和收斂效果上都更有優(yōu)勢(shì)，在簡(jiǎn)單的環(huán)境下能更快的規(guī)劃出較優(yōu)的全局路徑。

圖4 適應(yīng)度值1

為了進(jìn)一步驗(yàn)證算法的可行性，增加地圖中障礙物的數(shù)量，以測(cè)試I-PSO在面對(duì)不同環(huán)境下規(guī)劃路徑的能力。搜索空間維度D=2，其他參數(shù)不變，然后進(jìn)行仿真試驗(yàn)，得到規(guī)劃路徑如圖5所示。

圖5 規(guī)劃的路徑2

圖5是改變工作環(huán)境內(nèi)障礙物的密度后，2種算法在較為復(fù)雜環(huán)境下所規(guī)劃的全局路徑。圖5中，I-PSO搜索到的安全路徑相比于標(biāo)準(zhǔn)PSO規(guī)劃的路徑更趨近于直線，且軌跡長(zhǎng)度更小。從圖5可看出，當(dāng)?shù)貓D中的障礙物較多，環(huán)境較為復(fù)雜情況下，I-PSO相比標(biāo)準(zhǔn)PSO能找到更短的安全路徑，且路徑的拐點(diǎn)角度更小。

圖6是障礙物較多時(shí)，2種算法在迭代尋找路徑時(shí)的適應(yīng)度值變化曲線。從圖6可看出，當(dāng)障礙物增多后，標(biāo)準(zhǔn)PSO在起始階段收斂性較差，且算法的穩(wěn)定性也不好，而I-PSO在起始階段就能很快找到較好的解。

圖6 適應(yīng)度值2

圖7和圖8是在障礙物密度更大、活動(dòng)空間更有局限性的環(huán)境下，2種算法規(guī)劃的路徑和對(duì)應(yīng)的適應(yīng)度值迭代變化圖形?？梢钥闯觯琁-PSO在更為復(fù)雜的環(huán)境下也能規(guī)劃出安全、平滑和長(zhǎng)度更短的全局路徑。

由上述在3種環(huán)境下仿真得到的試驗(yàn)結(jié)果表明，本文提出的I-PSO較傳統(tǒng)PSO能更快的規(guī)劃出安全且距離更短的全局路徑，并且在算法的收斂性和穩(wěn)定性上都更優(yōu)。

圖7 規(guī)劃的路徑3

圖8 適應(yīng)度值3

5 結(jié)語(yǔ)

本文使用柵格法對(duì)機(jī)器人移動(dòng)空間進(jìn)行環(huán)境建模，為了使規(guī)劃的路徑更加安全、更適合機(jī)器人移動(dòng)，建立了考慮路徑長(zhǎng)度、碰撞風(fēng)險(xiǎn)的路徑規(guī)劃數(shù)學(xué)模型。對(duì)于傳統(tǒng)PSO在規(guī)劃路徑時(shí)存在易陷入局部最優(yōu)以及收斂速度慢的問(wèn)題，本文采用將粒子適應(yīng)度差值作為算法的反饋信號(hào)來(lái)動(dòng)態(tài)調(diào)整慣性權(quán)重的方式調(diào)節(jié)種群對(duì)搜索空間的整體或部分區(qū)域的搜索需求，并且引入隨機(jī)因子盡可能避開(kāi)局部陷阱，這種方法在處理PSO過(guò)早收斂和最優(yōu)解質(zhì)量較差的問(wèn)題。MATLAB試驗(yàn)結(jié)果表明,提出的I-PSO具避障性能優(yōu)越、規(guī)劃的移動(dòng)路徑長(zhǎng)度更短、路徑更平滑等優(yōu)點(diǎn)。

然而，本文主要解決的是在靜態(tài)空間環(huán)境下的路徑規(guī)劃問(wèn)題，缺少對(duì)動(dòng)態(tài)障礙物的處理。在之后的研究中將對(duì)存在障礙物移動(dòng)的環(huán)境進(jìn)行數(shù)學(xué)建模，并將本文的算法進(jìn)行改進(jìn)，使其能在動(dòng)態(tài)工作空間中進(jìn)行規(guī)劃路徑。