基于雙層微粒群優(yōu)化的機(jī)器人全局路徑規(guī)劃

曾現(xiàn)峰，張勇

1.中國礦業(yè)大學(xué)信息與電氣工程學(xué)院，江蘇徐州 221116

2.江蘇聯(lián)合職業(yè)技術(shù)學(xué)院徐州機(jī)電工程分院，江蘇徐州 221011

基于雙層微粒群優(yōu)化的機(jī)器人全局路徑規(guī)劃

曾現(xiàn)峰1，2，張勇1

1.中國礦業(yè)大學(xué)信息與電氣工程學(xué)院，江蘇徐州 221116

2.江蘇聯(lián)合職業(yè)技術(shù)學(xué)院徐州機(jī)電工程分院，江蘇徐州 221011

1 引言

路徑規(guī)劃是機(jī)器人能夠完成給定任務(wù)的前提，解決的問題是：在已知或未知的環(huán)境中，尋找一條從起點到終點的滿足一定優(yōu)化指標(biāo)的無碰撞路徑。根據(jù)機(jī)器人對環(huán)境的掌握狀況，已有的路徑規(guī)劃方法可分為基于模型的全局路徑規(guī)劃和基于信息檢測的局部路徑規(guī)劃，其中，全局路徑規(guī)劃的基本問題是環(huán)境建模和路徑尋優(yōu)。當(dāng)環(huán)境模型已知時，全局路徑規(guī)劃的重點在于開發(fā)高性能的搜索算法，使得規(guī)劃的路徑全局最優(yōu)。目前，用于全局路徑規(guī)劃的方法主要有基于圖論、柵格等的傳統(tǒng)方法[1]和基于進(jìn)化算法的智能方法[2-3]。這些方法各具優(yōu)點，但又都有不足之處[4-6]。針對環(huán)境的特點和優(yōu)化的性能指標(biāo)，研究不同的路徑規(guī)劃方法，具有重要的意義，是提高路徑規(guī)劃性能的有效途徑。

微粒群優(yōu)化是Kennedy等借鑒鳥群的集體捕食行為，提出的一種隨機(jī)群進(jìn)化方法[7]。相對傳統(tǒng)的優(yōu)化方法，該方法易于實現(xiàn)，可調(diào)參數(shù)少，收斂速度快，已成功應(yīng)用于函數(shù)優(yōu)化、模式識別、神經(jīng)網(wǎng)絡(luò)訓(xùn)練、數(shù)據(jù)挖掘，以及路徑規(guī)劃等領(lǐng)域。但是，利用微粒群優(yōu)化進(jìn)行機(jī)器人路徑規(guī)劃，存在局部收斂，效率低下，以及精度不高等缺點，尤其是含有非規(guī)則密集障礙物的復(fù)雜環(huán)境，優(yōu)化的路徑多與障礙物碰撞。文獻(xiàn)[8]提出一種改進(jìn)的微粒群優(yōu)化方法，先用神經(jīng)網(wǎng)絡(luò)訓(xùn)練碰撞罰函數(shù)，得到無碰撞的路徑，再用微粒群優(yōu)化求解路徑最優(yōu)問題；文獻(xiàn)[9]提出一種蟻群微粒群優(yōu)化方法，先用微粒群優(yōu)化得到蟻群優(yōu)化的初始信息素分布，再用蟻群優(yōu)化搜索全局最優(yōu)路徑。該方法有效結(jié)合了微粒群優(yōu)化和蟻群優(yōu)化的優(yōu)點，具有速度快和精度高等優(yōu)點。

針對微粒群優(yōu)化用于密集障礙物環(huán)境機(jī)器人全局路徑規(guī)劃存在的問題，本文提出一種雙層微粒群優(yōu)化方法（Two-Layer Particle Swarm Optimization，TL-PSO）。該方法通過底層微粒群優(yōu)化，得到若干最優(yōu)路徑；通過頂層微粒群優(yōu)化，在這些最優(yōu)路徑附近局部搜索，從而得到機(jī)器人的全局最優(yōu)路徑；通過對不可行路徑實施脫障操作，使其成為可行路徑。多場景的機(jī)器人路徑規(guī)劃結(jié)果表明，本文方法能夠找到機(jī)器人的全局最優(yōu)路徑。

2 問題建模

為了規(guī)劃機(jī)器人路徑，首先需要建立問題的數(shù)學(xué)模型。如圖1所示，在機(jī)器人工作空間建立兩維絕對坐標(biāo)系O-XY，其中，S為機(jī)器人的起點，G為終點，多邊形物體表示障礙物。當(dāng)起點與終點的連線與X軸不重合時，建立如下相對坐標(biāo)系S-xy：以S和G間的連線為x軸，過S點且垂直SG的直線為y軸[10]。兩坐標(biāo)系間的變換公式為：

式中，(X，Y)和(x，y)分別為點在坐標(biāo)系O-XY和S-xy的坐標(biāo)，α為坐標(biāo)軸X與x的夾角，(X0，Y0)為S在坐標(biāo)系O-XY的坐標(biāo)。

圖1 環(huán)境模型

對線段SG進(jìn)行D+1等分，并過每個等分點作SG的垂線，可以得到平行直線族(l1，…，ld-1，ld，…，lD)，該直線族與路徑的交點，即為規(guī)劃的目標(biāo)點序列(p1,…,pd-1,pd,…,pD)。

機(jī)器人路徑規(guī)劃，就是在相對坐標(biāo)系S-xy中，尋找一個目標(biāo)點序列(p1，…，pd-1，pd，…，pD)，使得構(gòu)成的路徑

式中，LSG為S和G間的距離。

機(jī)器人路徑規(guī)劃問題，可以建模為一個約束優(yōu)化問題，通過懲罰不可行路徑，得到如下無約束優(yōu)化問題：

minF(p)=L(p)·(1+N(p))（5）式中，N(p)為p與障礙物碰撞的次數(shù)。由式（5）可知，p與障礙物碰撞的次數(shù)越多，受到的懲罰程度越大，該路徑的性能越差。

3 本文方法

本文提出的基于雙層微粒群優(yōu)化的機(jī)器人全局路徑規(guī)劃的思想是：通過底層微粒群優(yōu)化，得到若干最優(yōu)路徑；通過頂層微粒群優(yōu)化，在這些最優(yōu)路徑附近局部搜索，得到機(jī)器人的全局最優(yōu)路徑；通過對不可行路徑實施脫障操作，使其成為可行路徑。

3.1 雙層微粒群優(yōu)化

對于含有密集障礙物的環(huán)境，采用微粒群優(yōu)化進(jìn)行機(jī)器人全局路徑規(guī)劃，得到的結(jié)果多為非可行路徑。即使路徑是可行的，也多為局部最優(yōu)的。通過調(diào)整微粒群優(yōu)化的參數(shù)，雖然可以在一定程度上提高規(guī)劃后路徑的性能，但是，性能的提高往往很有限。為了提高機(jī)器人路徑的全局性能，這里提出一種新的微粒群優(yōu)化方法，即雙層微粒群優(yōu)化。

該方法的思想是：微粒群優(yōu)化包含2層，即底層和頂層優(yōu)化。其中，底層微粒群優(yōu)化在決策空間廣度搜索，得到若干最優(yōu)路徑；頂層微粒群優(yōu)化基于這些最優(yōu)路徑，進(jìn)行深度搜索，得到機(jī)器人的全局最優(yōu)路徑。

傳統(tǒng)的微粒更新公式如下[7]：

式中，i=1，2，…，N，N為微粒群規(guī)模；j=1，2，…，D，D為決策空間的維數(shù)；pbest和gbest分別為微粒的個體極值點和全局極值點；c1和c2是兩個學(xué)習(xí)因子，通常取c1=c2=2；r1和r2是（0，1）之間的隨機(jī)數(shù)；ω是慣性權(quán)重，用來控制前次迭代的速度對當(dāng)前迭代速度的影響，以協(xié)調(diào)微粒的廣度和深度搜索。為了增強(qiáng)底層微粒群優(yōu)化的廣度搜索性能，更新微粒的速度時，采用下式線性遞減慣性權(quán)重：

式中，ωmax和ωmin分別為慣性權(quán)重的最大和最小值，本文取ωmax=0.9，ωmin=0.4；t和T分別為當(dāng)前和最大迭代次數(shù)。

頂層微粒群優(yōu)化基于底層微粒群優(yōu)化得到的最優(yōu)路徑，進(jìn)行深度搜索。更新微粒的速度時，慣性權(quán)重ω和學(xué)習(xí)因子c1線性遞減，學(xué)習(xí)因子c2線性遞增，其取值如下：

式中，cmax和cmin分別為學(xué)習(xí)因子的最大和最小值，本文取cmax=2.5，cmin=0.5。

3.2 脫障操作

采用微粒群優(yōu)化進(jìn)行機(jī)器人路徑規(guī)劃時，如果能夠根據(jù)機(jī)器人的起點和終點位置，以及障礙物的位置，引導(dǎo)微粒的搜索，那么，將會提高優(yōu)化解成為可行路徑的概率?；诖耍?dāng)某路徑不可行時，說明該路徑的某段與障礙物相交，對該路徑實施脫障操作，也即對微粒的相應(yīng)分量定向定步長變異，以避免機(jī)器人與障礙物碰撞。

由式（9）可知，當(dāng)ymin≥0 或ymax＞|ymin|時，整個障礙物或其大部分位于x軸的上方；相反，當(dāng)ymax＜0 或ymax≤|ymin|時，整個障礙物或其大部分位于x軸的下方。通過變異使路徑點pd和pd+1向x軸平移l，可以遠(yuǎn)離障礙物Obj，進(jìn)而使路徑成為可行的。由式（5）可知，這樣可以提高微粒的適應(yīng)值。

圖2給出了脫障操作的過程。圖中，I線為原來的路徑，由于與障礙物相交，因此，是不可行的；II線為脫障操作后得到的路徑，不再與障礙物相交，從而成為可行路徑。

圖2 脫障操作過程

需要注意的是，對微粒實施脫障操作后，如果微粒的某維，如pd，超出了取值范圍，那么，采用下式進(jìn)一步調(diào)整：

3.3 算法步驟

本文提出的基于雙層微粒群優(yōu)化的機(jī)器人全局路徑規(guī)劃方法的步驟如下：

步驟1設(shè)定微粒群優(yōu)化的參數(shù)取值；

步驟2隨機(jī)初始化底層種群；

步驟3計算每個微粒的適應(yīng)值，更新個體極值點Pbest和全局極值點Gbest；

步驟4由式（6）和（7）更新底層微粒；

步驟5判定路徑與障礙物是否相交，若是，對微粒實施脫障操作；

步驟6判定底層微粒群優(yōu)化是否滿足終止條件，若否，轉(zhuǎn)步驟3；

步驟7判定底層微粒群優(yōu)化的運行次數(shù)是否滿足要求，若否，轉(zhuǎn)步驟2；

步驟8基于底層微粒群優(yōu)化得到的最優(yōu)路徑，形成頂層微粒群優(yōu)化的初始種群；

步驟9：計算每個微粒的適應(yīng)值，更新個體極值點Pbest和全局極值點Gbest；

步驟10由式（6）和（8）更新頂層微粒；

步驟11判定路徑與障礙物是否相交，若是，對微粒實施脫障操作；

步驟12判定頂層微粒群優(yōu)化是否滿足終止條件，若否，轉(zhuǎn)步驟9；

步驟13輸出全局最優(yōu)路徑，算法結(jié)束。

4 實驗

為了驗證本文方法的有效性，將其應(yīng)用到含有密集障礙物環(huán)境的機(jī)器人路徑規(guī)劃中，并與僅含脫障操作的改進(jìn)微粒群優(yōu)化（Particle Swarm Optimization with Escape Obstacle Operator，EOO-PSO），以及標(biāo)準(zhǔn)微粒群優(yōu)化（Standard Particle Swarm Optimization，SPSO）進(jìn)行比較。

算法所需的參數(shù)設(shè)置如下：底層和頂層微粒群優(yōu)化的微粒群規(guī)模N=20，維數(shù)D=10，最大迭代次數(shù)T=100。EOO-PSO、SPSO的參數(shù)設(shè)置同底層微粒群優(yōu)化方法，但是，在EOO-PSO中利用了本文的脫障操作。實驗的運行環(huán)境為Intel Celeron M-1.6 GHz CPU，504 MB RAM的PC機(jī)，以及MATLAB7.4。

機(jī)器人的移動環(huán)境如圖3～5所示，在50 unit×50 unit的正方形環(huán)境中有12個靜態(tài)障礙物，其頂點坐標(biāo)已知，機(jī)器人的始點S和終點G的坐標(biāo)已知。始點和終點任意改變3次，運行上述3種方法各30次，圖3～5分別針對3種不同始終點的位置，給出3種方法得到的最好路徑。表1列出相應(yīng)的統(tǒng)計數(shù)據(jù)，由表1可知：

（1）對于相同的始終點位置，本文方法得到路徑的性能指標(biāo)均值最小，其次是EOO-PSO，最大的是SPSO。這說明，本文方法得到的路徑具有最好的性能指標(biāo)值，這體現(xiàn)在路徑的長度短或者與障礙物相交的路段少，這是由于本文方法不但采用了雙層微粒群優(yōu)化，而且在每層還對不可行路徑實施了脫障操作，而EOO-PSO僅對不可行路徑實施了脫障操作，缺少雙層微粒群優(yōu)化，SPSO對不可行路徑也沒有實施脫障操作。通過可行解比例也可以看出，對不可行路徑實施脫障操作是十分必要的。

圖3 始終點1不同算法所得最好結(jié)果

圖4 始終點2不同算法所得最好結(jié)果

圖5 始終點3不同算法所得最好結(jié)果

表1 不同算法得到路徑的性能

（2）對于相同的始終點位置，本文方法得到路徑的性能指標(biāo)方差最小，其次是是EOO-PSO，最大的是SPSO，這說明，本文方法的穩(wěn)定性優(yōu)于EOO-PSO和SPSO。

（3）對于相同的始終點位置，優(yōu)化機(jī)器人路徑本文方法花費的時間最多，其次是EOO-PSO，最少的是SPSO，這是容易理解的，多花費的時間主要用于雙層微粒群優(yōu)化和脫障操作上。

上述實驗結(jié)果表明，本文方法能夠找到機(jī)器人的全局最優(yōu)路徑。

5 結(jié)論

針對微粒群優(yōu)化用于密集障礙物環(huán)境機(jī)器人全局路徑規(guī)劃存在的問題，本文提出一種雙層微粒群優(yōu)化方法。該方法通過底層微粒群優(yōu)化，得到若干最優(yōu)路徑；通過頂層微粒群優(yōu)化，在這些最優(yōu)路徑附近局部搜索，從而得到機(jī)器人的全局最優(yōu)路徑；通過對不可行路徑實施脫障操作，使其成為可行路徑。三種場景的機(jī)器人路徑規(guī)劃結(jié)果表明，所提方法得到的路徑均為可行路徑，且性能指標(biāo)優(yōu)，算法的穩(wěn)定性好。

[1]劉華軍，楊靜宇，陸建峰，等.移動機(jī)器人運動規(guī)劃研究綜述[J].中國工程科學(xué)，2006，8（1）：85-94.

[2]Castillo O，Trujillo L，Melin P.Multiple objective genetic algorithms for path-planning optimization in autonomous mobile robots[J].Soft Computing，2007，11（3）：269-279.

[3]譚冠政，賀歡，Sloman A.基于ACS算法的移動機(jī)器人實時全局最優(yōu)路徑規(guī)劃[J].自動化學(xué)報，2007，33（3）：279-285.

[4]李壘，葉濤，譚民，等.移動機(jī)器人技術(shù)研究現(xiàn)狀與未來[J].機(jī)器人，2002，24（5）：475-480.

[5]王志文，郭戈.移動機(jī)器人導(dǎo)航技術(shù)現(xiàn)狀與展望[J].機(jī)器人，2003，25（5）：470-474.

[6]張捍東，鄭睿，岑豫皖.移動機(jī)器人路徑規(guī)劃技術(shù)的現(xiàn)狀與展望[J].系統(tǒng)仿真學(xué)報，2005，17（2）：439-443.

[7]Kennedy J，Eberhart R C.Particle swarm optimization[C]//Proc of IEEE Int Conf on Neural Networks．Piscataway，NJ：IEEE Press，1995：1942-1948.

[8]胡玉蘭，姜明洋，趙慧靜.基于改進(jìn)粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法研究[J].計算機(jī)工程與科學(xué)，2009，31（6）：139-141.

[9]鄧高峰，張雪萍，劉彥萍.一種障礙環(huán)境下機(jī)器人路徑規(guī)劃的蟻群粒子群優(yōu)化算法[J].控制理論與應(yīng)用，2009，26（8）：879-883.

[10]孫波，陳衛(wèi)東，席裕庚.基于粒子群優(yōu)化算法的移動機(jī)器人全局路徑規(guī)劃[J].控制與決策，2005，20（9）：1052-1060.

ZENG Xianfeng1，2,ZHANG Yong1

1.School of Information and Electrical Engineering,China University of Mining and Technology,Xuzhou,Jiangsu 221116,China
2.Electrical and Mechanical Engineering Branch of Xuzhou,Jiangsu Union Technical Institute,Xuzhou,Jiangsu 221011,China

Solving the problem of global robot path planning using Particle Swarm Optimization（PSO）has attracted various researchers in recent years,and fruitful achievements have been obtained.Previous studies,however,are hard to be applied to environments containing dense obstacles.Aiming at solving the problem of global robot path planning in environments containing dense obstacles,a two-layer PSO is presented.In this method,several optimal paths are first obtained using the bottom layer PSO.The globally optimal path is then got by local search near these optimal paths using the up layer PSO.In addition,an infeasible path becomes feasible by performing an escape obstacle operator.The proposed method is applied to solve the problem of robot path planning in various scenarios,and compared with previous methods.The experimental results confirm that the proposed method can find the globally optimal robot path.

robot;path planning;Particle Swarm Optimization（PSO）;escape obstacle

采用微粒群優(yōu)化解決機(jī)器人全局路徑規(guī)劃問題，近年來得到國內(nèi)外學(xué)者廣泛關(guān)注，并已經(jīng)取得豐碩的研究成果。但是，已有成果往往難以應(yīng)用于含有密集障礙物的環(huán)境。針對解決含有密集障礙物環(huán)境的機(jī)器人全局路徑規(guī)劃問題，提出一種雙層微粒群優(yōu)化方法。該方法通過底層微粒群優(yōu)化，得到若干最優(yōu)路徑；通過頂層微粒群優(yōu)化，在這些最優(yōu)路徑附近局部搜索，從而得到機(jī)器人的全局最優(yōu)路徑；通過對不可行路徑實施脫障操作，使其成為可行路徑。將所提方法應(yīng)用于多場景的機(jī)器人路徑規(guī)劃，并與已有方法進(jìn)行比較。實驗結(jié)果表明，該方法能夠找到機(jī)器人的全局最優(yōu)路徑。

機(jī)器人；路徑規(guī)劃；微粒群優(yōu)化；脫障

A

TP242.6

10.3778/j.issn.1002-8331.1201-0059

ZENG Xianfeng,ZHANG Yong.Global robot path planning based on two-layer particle swarm optimization.Computer Engineering and Applications,2013,49（19）：238-241.

國家自然科學(xué)基金（No.61005089）；高等學(xué)校博士學(xué)科點專項科研基金資助課題（No.20100095120016）。

曾現(xiàn)峰（1972—），男，講師，研究領(lǐng)域為智能優(yōu)化與控制，機(jī)器人路徑規(guī)劃；張勇（1979—），男，博士，講師，研究領(lǐng)域為智能優(yōu)化與控制。E-mail：xianfeng669@126.com

2012-01-06

2012-03-01

1002-8331（2013）19-0238-04

CNKI出版日期：2012-06-01http://www.cnki.net/kcms/detail/11.2127.TP.20120601.1458.048.html