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

#基于改進(jìn)粒子群算法的智能機(jī)器人路徑規(guī)劃摘要:針對(duì)機(jī)器人路徑規(guī)劃問題，為了使機(jī)器人能在較短的時(shí)間里，經(jīng)過最短的路徑，且較平滑的從起始位置運(yùn)動(dòng)到目標(biāo)位置。本文采用粒子群群智能算法，首先，用柵格法建模，然后用粒子群算法尋優(yōu)。針對(duì)粒子群算法局部尋優(yōu)能力差的缺點(diǎn)，提出一種非線性動(dòng)態(tài)調(diào)整慣性權(quán)重的改進(jìn)粒子群路徑規(guī)劃算法。該算法將柵格法與粒子群算法進(jìn)行有效結(jié)合，在路徑長(zhǎng)度的基礎(chǔ)上引入安全度和平滑度概念，建立動(dòng)態(tài)調(diào)整路徑長(zhǎng)度的適應(yīng)度函數(shù)。與傳統(tǒng)的粒子群算法相比，實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法具有較強(qiáng)的安性實(shí)時(shí)性及尋優(yōu)能力。關(guān)鍵詞:智能機(jī)器人；路徑規(guī)劃；柵格法；粒子群算法0引言路徑規(guī)劃是智能機(jī)器人導(dǎo)航的最基本環(huán)節(jié)之一，它是指智能機(jī)器人在具有障礙物的工作環(huán)境中，按照某一性能指標(biāo)(如距離、時(shí)間、能量等)，不間斷地利用所攜帶的傳感器去認(rèn)知周圍的環(huán)境，讀取障礙物的大小、位置和距離，不斷地感知環(huán)境信息和周圍障礙物的變化，搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或近似最優(yōu)的安全、無(wú)碰撞路徑。根據(jù)智能機(jī)器人對(duì)環(huán)境信息的已知程度，路徑規(guī)劃可分為兩類：一類是環(huán)境信息已知的全局路徑規(guī)劃，另一類是環(huán)境信息未知或部分已知的局部路徑規(guī)劃。目前，常用的路徑規(guī)劃方法主要有粒子群(ParticleSwarmOptimization，PSO)算法、人工勢(shì)場(chǎng)法、柵格法、神經(jīng)網(wǎng)絡(luò)法和蟻群算法等。相比其他算法而言，粒子群算法具有收斂速度快、設(shè)置參數(shù)少、實(shí)現(xiàn)簡(jiǎn)單等特點(diǎn)，近年來(lái)受到很多學(xué)者的重視，并成功應(yīng)用于許多領(lǐng)域。但粒子群算法本身還存在著一些缺陷，如局部尋優(yōu)能力差、速度和位置更新公式不夠完善等問題，嚴(yán)重影響了路徑規(guī)劃的計(jì)算效率和可靠性。在對(duì)粒子群算法的深入研究中，國(guó)內(nèi)外學(xué)者對(duì)其固有缺陷提出了各種改進(jìn)方法，主要通過引入固定的慣性權(quán)重和學(xué)習(xí)因子等方法對(duì)速度更新公式進(jìn)行修改，有所改觀但并不完美。針對(duì)上述問題，本文采用柵格法建立環(huán)境模型，以粒子群算法為基本演化算法，引入非線性動(dòng)態(tài)調(diào)整慣性權(quán)重和改進(jìn)適應(yīng)度函數(shù)的方法對(duì)粒子群算法加以改進(jìn)[2],將粒子群算法直接運(yùn)用到柵格法中，得到智能機(jī)器人全局最優(yōu)路徑。1粒子群算法基本思想粒子群算法的背景是“人工生命”“人工生命”是來(lái)研究具有某些生命基本特征的人工系統(tǒng)。人工生命包括兩方面的內(nèi)容。其一是研究如何利用計(jì)算技術(shù)研究生物現(xiàn)象；其二研究如何利用生物技術(shù)研究計(jì)算問題。我們現(xiàn)在關(guān)注的是第二部分的內(nèi)容?，F(xiàn)在已經(jīng)有很多源于生物現(xiàn)象的計(jì)算技巧。例如，人工神經(jīng)網(wǎng)絡(luò)是簡(jiǎn)化的大腦模型；遺傳算法是模擬基因進(jìn)化過程的?，F(xiàn)在我們討論另一種生物系統(tǒng)一社會(huì)系統(tǒng)。更確切的是，在由簡(jiǎn)單個(gè)體組成的群落與環(huán)境以及個(gè)體之間的互動(dòng)行為。也可稱做“群智能”(SwarmIntelligence)。這些模擬系統(tǒng)利用局部信息從而可能產(chǎn)生不可預(yù)測(cè)的群體行為，它們都用來(lái)模擬魚群和鳥群的運(yùn)動(dòng)規(guī)律，主要用于計(jì)算機(jī)視覺和計(jì)算機(jī)輔助設(shè)計(jì)。在計(jì)算智能(ComputationalIntelligence)領(lǐng)域有兩種基于群智能的算法。蟻群算法(AntColonyOptimization)和粒子群算法(ParticleSwarmOptimization)。前者是對(duì)螞蟻群落食物采集過程的模擬。已經(jīng)成功運(yùn)用在很多離散優(yōu)化問題上。粒子群優(yōu)化算法(PSO)也是起源對(duì)簡(jiǎn)單社會(huì)系統(tǒng)的模擬。最初設(shè)想是模擬鳥群覓食的過程，但后來(lái)發(fā)現(xiàn)PSO是一種很好的優(yōu)化工具。如前所述，PSO模擬鳥群的捕食行為。設(shè)想這樣一個(gè)場(chǎng)景：一群鳥在隨機(jī)搜索食物。在這個(gè)區(qū)域里只有一塊食物。所有的鳥都不知道食物在那里。但是他們知道當(dāng)前的位置離食物還有多遠(yuǎn)。那么找到食物的最優(yōu)策略是什么呢？最簡(jiǎn)單有效的就是搜尋目前離食物最近的鳥的周圍區(qū)域。PSO從這種模型中得到啟示并用于解決優(yōu)化問題。在PSO中，每個(gè)優(yōu)化問題的解都是搜索空間中的一只鳥。我們稱之為“粒子”所有的粒子都有一個(gè)由被優(yōu)化的函數(shù)決定的適應(yīng)值(Fitnessvalue)，每個(gè)粒子還有一個(gè)速度決定他們飛翔的方向和距離。然后粒子們就追隨當(dāng)前的最優(yōu)粒子在解空間中搜索。PSO初始化為一群隨機(jī)粒子(隨機(jī)解)。然后通過迭代找到最優(yōu)解。第i個(gè)微粒表示為X=(x,x,,x)，然后粒子們就跟蹤兩個(gè)“極值”在ii1i2iD解空間中搜索。第一個(gè)就是粒子本身所找到的最優(yōu)解，這個(gè)解叫做個(gè)體極值P,best

記為P=(p,p,,p);另一個(gè)極值是群體所有微粒經(jīng)歷過的最好位置(有最TOC\o"1-5"\h\zii1i2iD好的適應(yīng)度)，這個(gè)極值就是全局極值g，記為P=(,p，…,p)。另外也bestgg1g2gD可以不用整個(gè)種群而只是用其中一部分作為粒子的鄰居，那么在所有鄰居中的極值就是局部極值。微粒i的速度用V=(v,v,...,v)表示，而每一個(gè)粒子的位置ii1i2iD就是一個(gè)潛在的解。在每一次迭代計(jì)算中，粒子的第d維(1<d<D)根據(jù)如下公式來(lái)更新自己的速度和位置:Vn+1二WVn+Vn+1二WVn+clrand()ididid-Xnid)+c2rand()cgd—Xn(1)id(2)(2)Xn+1=Xn+Vn+1

ididid其中，rand()是［0,1］范圍內(nèi)變化的隨機(jī)函數(shù)；c1和c2是正實(shí)數(shù)，稱作學(xué)習(xí)因子(LearningFactors),用來(lái)調(diào)節(jié)每次迭代的步長(zhǎng)；w為慣性權(quán)重(InertiaWeight)；n=1,2,…為迭代次數(shù)；此外，微粒的速度V被一個(gè)最大速度V所限制。如果idmax當(dāng)前對(duì)微粒的加速導(dǎo)致它在某維的速度超過該維的最大速度V，則該維的速度max被限制為該維最大速度V。max公式(1)的第1部分為動(dòng)量項(xiàng)，該項(xiàng)系數(shù)w隨著迭代逐步減小可漸進(jìn)地加強(qiáng)局部搜索能力；第2部分為“認(rèn)知(Cognition)”部分，表示微粒自身的思考；第3部分為“社會(huì)(Social)”部分，表示微粒間的信息共享與相互合作。2環(huán)境模型的建立在二維平面上對(duì)智能機(jī)器人的工作環(huán)境用改進(jìn)的柵格法進(jìn)行建模。環(huán)境建模的有效性對(duì)于機(jī)器人能否高效地規(guī)劃和避障具有關(guān)鍵性的作用，柵格法的優(yōu)點(diǎn)是建模方便迅速，能運(yùn)用多個(gè)柵格的拼合來(lái)代表多種形狀的障礙物，以減少可行區(qū)域由障礙物建模造成的損失。但該方法也存在不足，采用傳統(tǒng)柵格的序列進(jìn)行路徑規(guī)劃時(shí)，當(dāng)柵格粒度越小，雖然障礙物的表示越精確，但同時(shí)會(huì)占用大量的存儲(chǔ)空間和延長(zhǎng)規(guī)劃時(shí)間，算法的搜索范圍將按指數(shù)增加；當(dāng)柵格粒度太大，規(guī)劃的路徑會(huì)很不精確。本文將采用一種改進(jìn)的柵格法進(jìn)行建模［牛，為保證機(jī)器人能夠在環(huán)境模型中無(wú)碰撞地運(yùn)動(dòng)，將機(jī)器人模型簡(jiǎn)化為一個(gè)很小的質(zhì)點(diǎn)，把機(jī)器人的實(shí)際尺寸折算進(jìn)障礙物的面積內(nèi)，根據(jù)機(jī)器人的實(shí)際尺寸將障礙物的邊界向外擴(kuò)展。如果某一柵格中存在障礙物，則定義此黑色柵格為障礙柵格，表示為1;反之白色為自由柵格，表示為0。用多個(gè)柵格的拼合來(lái)代表不同形狀的障礙物，自由柵格則被合并成為機(jī)器人的可達(dá)區(qū)域，且柵格的大小以對(duì)障礙物表示的精確度為準(zhǔn)。本文在對(duì)空間進(jìn)行建模時(shí)，對(duì)空間內(nèi)的障礙物做如下處理，實(shí)際障礙物如圖1(a)所示，處理后障礙物如圖1(b)所示：不滿一個(gè)柵格時(shí)算一個(gè)柵格；障礙物的空凹部分和這個(gè)障礙物算成一個(gè)整體障礙物，避免局部死區(qū)的出現(xiàn)，這叫作障礙物的合并；把地圖的邊界當(dāng)成障礙物來(lái)處理。圖1障礙物形狀處理圖3改進(jìn)粒子群算法的路徑規(guī)劃在建立的空間模型中，利用粒子群算法直接找出一條最優(yōu)路徑。粒子群算法中的每一個(gè)粒子都有一個(gè)解，也就是一條路徑，粒子群算法從眾多的解中尋找一個(gè)最優(yōu)解形成最優(yōu)路徑。每個(gè)粒子的優(yōu)化函數(shù)是粒子所代表的路徑長(zhǎng)度，在這里具體指的是每一個(gè)粒子所跨過的格數(shù)。3.1粒子的有效性粒子的有效性是指粒子中任意兩個(gè)相鄰的元素之間的矩形區(qū)域能自由連通，中間不能有障礙物。即滿足約束條件且所經(jīng)過的柵格為自由柵格的粒子為有效粒

子，只要粒子中任意兩個(gè)相鄰的元素不能自由連通或柵格中同一行(同一列)迂回出現(xiàn)兩次路徑，該粒子就無(wú)效。圖2中三幅圖表示的是粒子p從第k次迭代到第k+1次有效路徑的實(shí)例，圖3中三幅圖表示的是粒子p從第k次迭代到第k+1次無(wú)效路徑的實(shí)例。P(k}■\咻+1)1^!(c)相鄰元素中間連通圖2粒子有效路徑圖PtA)PtA)1A*HZ■■■*W1J(a)相鄰元素?zé)o法連通(b)相鄰元素?zé)o法上下迂回連通（c）相鄰元素?zé)o法左右迂回連通圖3粒子無(wú)效路徑圖3.2粒子適應(yīng)度函數(shù)適應(yīng)度函數(shù)是粒子群優(yōu)化算法中的一個(gè)很重要的因素，它決定了算法能否收斂。本文在以路徑長(zhǎng)度作為適應(yīng)度函數(shù)的基礎(chǔ)上加入安全度、平滑度，對(duì)所有參數(shù)進(jìn)行加權(quán)平均凸，以滿足復(fù)雜環(huán)境下機(jī)器人路徑規(guī)劃的要求。1）規(guī)劃的路徑盡可能短，路徑的長(zhǎng)度可由下式表示:f=fv（x一x）2+（y一y）2ivi+iii+iii=1式中：f表示一個(gè)粒子中所有相鄰頂點(diǎn)之間的直線距離的和，（x,y）表示粒子1ii當(dāng)前的坐標(biāo)，（x,y）表示粒子下一位置的坐標(biāo)。i+1i+12）引入懲罰函數(shù)，提高路徑的安全度。當(dāng)機(jī)器人與障礙物相撞時(shí)，在機(jī)器(3)人的路徑長(zhǎng)度上引入一個(gè)懲罰函數(shù)，粒子碰撞的障礙物越多，施加的懲罰越大,使得此路徑生成的概率越小。懲罰函數(shù)表示為:—M2k=1(4)式中：N表示粒子從起始點(diǎn)到終點(diǎn)的直線路徑上障礙物的個(gè)數(shù)之和，M表示給定的一個(gè)較大常數(shù)項(xiàng)。3）引入路徑平滑度。機(jī)器人在運(yùn)動(dòng)過程中走對(duì)角路線可縮短路徑長(zhǎng)度以節(jié)約時(shí)間，但改變機(jī)器人的運(yùn)動(dòng)方向也會(huì)耗費(fèi)時(shí)間，為了使機(jī)器人完成任務(wù)所耗費(fèi)的時(shí)間最短，本文引入路徑平滑度公式如下:(5)式中：n表示粒子所走對(duì)角路徑中機(jī)器人轉(zhuǎn)角45°的次數(shù),1角路徑中機(jī)器人轉(zhuǎn)角90°的次數(shù)，r表示機(jī)器人的半徑。n表示粒子所走直2綜合f、f、f所得123適應(yīng)度函數(shù)為:(6)式中Q、P、Y為各自函數(shù)的加權(quán)因子，為大于等于0的任意實(shí)數(shù)。通過調(diào)整Q、P、丫可以調(diào)節(jié)f、f、f在適應(yīng)度函數(shù)中所占的比重。當(dāng)Q=1,P二0，123Y二0時(shí)，轉(zhuǎn)化為常規(guī)的僅以路徑長(zhǎng)度作為適應(yīng)度函數(shù)的數(shù)學(xué)模型。式中f和2f作為懲罰判斷函數(shù)和平滑度修正函數(shù)，其取值一般小于f的數(shù)量級(jí)。313.3非線性動(dòng)態(tài)調(diào)整慣性權(quán)重的PSO算法標(biāo)準(zhǔn)粒子群算法中，前期該算法容易陷入局部極值，產(chǎn)生早熟收斂。同時(shí),由于迭代后的速度呈線性遞增趨勢(shì)，到后期無(wú)法進(jìn)行較細(xì)的局部搜索，無(wú)法達(dá)到完全避碰的路徑規(guī)劃要求。針對(duì)以上缺陷，以Kennedy和Eberhart為代表的專家學(xué)者引入慣性權(quán)重w的粒子群算法(ParticleSwarmOptimizationalgorithmwithWeight，WPSO)[i],以限制迭代后期由于速度過快而無(wú)法進(jìn)行精細(xì)局部搜索。引入初期，取固定值w可有效改善這一缺陷，隨后又提出了線性遞減規(guī)律改變慣性權(quán)重的粒子群算法(ParticleSwarmOptimizationalgorithmwithLinearlyDecreasingWeight，LDWPSO)，具體計(jì)算公式[5]如下:w=ww=wmax-(w-w)?—kmaxmink(7)max式中：ww表示慣性權(quán)重的最大值和最小值,k表示當(dāng)前迭代次數(shù)，k表maxminmax示最大迭代次數(shù)。專家提出的改進(jìn)對(duì)求最優(yōu)化解有所改善，但不明顯。本文將動(dòng)態(tài)非線性遞減慣性權(quán)重的思想引入粒子群算法中，公式如下:wk+1=wk+1=w+-w)?minkmin'k-k、—maxIk丿max(8)式中：w為當(dāng)前迭代所得的值，其初始值為w;k表示當(dāng)前迭代次數(shù)；k表TOC\o"1-5"\h\zkmaxmax示最大迭代次數(shù)。慣性權(quán)重w從初始值w隨著n的不同值以及迭代變化k+1maxw的值，非線性下降，當(dāng)k=0時(shí)w=w;當(dāng)k=k時(shí)減小到最小值w。kk+1maxmaxmin根據(jù)個(gè)體粒子和粒子群所處的不同區(qū)域，慣性權(quán)重按不同的非線性指數(shù)n下降。當(dāng)個(gè)體粒子和粒子群處于遠(yuǎn)離個(gè)體極值和全局極值的中心區(qū)域時(shí)，取n=1.1，由式(8)可知，慣性權(quán)重下降速度減慢，這樣慣性權(quán)重在這一階段具有較大值，粒子群將以較快的速度飛向群體最優(yōu)位置；當(dāng)個(gè)體粒子和粒子群逐漸靠近目標(biāo)最優(yōu)值時(shí)，進(jìn)入中心范圍后，取n=0.9，由式（8）可知，慣性權(quán)重下降加快，粒子在最優(yōu)值所在區(qū)域?qū)?yōu)化目標(biāo)進(jìn)行更加細(xì)致的搜索。中心區(qū)域的范圍界限為個(gè)體粒子和全局粒子所發(fā)現(xiàn)的距個(gè)體極值和全局極值最大距離的中間位置。4仿真結(jié)果與分析本文利用Matlab7.0在IntelCore2主頻1.86GHz計(jì)算機(jī)上進(jìn)行了仿真實(shí)驗(yàn)。仿真參數(shù)選擇如下：種群規(guī)模即粒子個(gè)數(shù)M=50,粒子最大速度v=15,max加速因子c=c=2算法最大迭代次數(shù)k=200，最大慣性權(quán)重w=0.9，最小12maxmax慣性權(quán)重w二0.4，固定的慣性權(quán)重設(shè)定為w=0.7，對(duì)于動(dòng)態(tài)的慣性權(quán)重，慣min性權(quán)重W隨著迭代次數(shù)k的增加非線性從0.9減少到0.4。利用柵格模型對(duì)粒子群算法的智能機(jī)器人路徑規(guī)劃進(jìn)行仿真，復(fù)雜障礙物環(huán)境模型下的仿真結(jié)果如圖所示。從仿真結(jié)果可以看出，智能機(jī)器人從開始點(diǎn)“S”運(yùn)動(dòng)到終止點(diǎn)（目標(biāo)點(diǎn)）“E”，兩種算法都能實(shí)現(xiàn)尋優(yōu)和避障能力。但PSO機(jī)器人路徑規(guī)劃易陷入局部最優(yōu)，在避障中過多地考慮障礙物帶來(lái)的影響，而忽略了實(shí)時(shí)性；而WPSO機(jī)器人路徑規(guī)劃相比PSO路徑規(guī)劃能跳出局部最優(yōu)，卻沒有考慮障礙物和機(jī)器人邊緣的影響，機(jī)器人路徑中的安全性下降;本文算法針對(duì)兩種算法存在的缺陷，考慮障礙物邊緣和機(jī)器人轉(zhuǎn)角次數(shù)的影響，引入路徑長(zhǎng)度的安全度和平滑度，行50次，表1記錄了復(fù)雜環(huán)境下基本粒子群算法（PSO）與優(yōu)化后的粒子群算法（WPSO）最優(yōu)路徑長(zhǎng)度及仿真時(shí)間的結(jié)果。表1仿真結(jié)果對(duì)比表算法算法路徑長(zhǎng)度仿真時(shí)間最優(yōu)路徑均值方差均值方差PSO30.67837.571128.5140.374.2e-3WPSO28.63636.345113.0670.342.4e-3

從表I可以看到，兩種算法中WPSO算法的最優(yōu)路徑最短，由于引入了非線性慣性權(quán)重及新的適應(yīng)度函數(shù)，但在機(jī)器人路徑規(guī)劃應(yīng)用中優(yōu)化后的算法的收斂速度大于原始算法，且最優(yōu)路徑最短，則運(yùn)行時(shí)間最短?？偟膩?lái)說(shuō)，優(yōu)化后的算法無(wú)論最優(yōu)路徑長(zhǎng)度還是運(yùn)行時(shí)間都優(yōu)于原始算法。5小結(jié)本文以柵格法為環(huán)境建模方法，直接將粒子群算法運(yùn)用到智能機(jī)器人路徑規(guī)劃中，可在起點(diǎn)與終點(diǎn)之間得到一條簡(jiǎn)單安全的最優(yōu)路徑。計(jì)算機(jī)仿真證明該方法不僅可以找到最優(yōu)路徑，而且算法過程簡(jiǎn)單，容易實(shí)現(xiàn)，比傳統(tǒng)粒子群算法能得出較優(yōu)結(jié)果。參考文獻(xiàn)KennedyJ,EberhartR.Particleswarmoptimization[A].in:Proceedingsofthe4thIEEEInternationalConferenceonNeuralNetworks[C]，Piscataway:IEEEServiceCenter,1995,pp.1942-1948.GarnierS,GautraisJ,TheraulazG.Thebiologicalprinciplesofswarmintelligence[J].SwarmIntelligen