朱奇光　夏翠萍　陳衛(wèi)東　陳　穎

1.燕山大學(xué)，秦皇島，0660042.河北省特種光纖與光纖傳感重點(diǎn)實(shí)驗(yàn)室，秦皇島，066004

基于混沌優(yōu)化MPSO的移動(dòng)機(jī)器人FastSLAM算法研究

朱奇光1,2夏翠萍1陳衛(wèi)東1,2陳穎1

1.燕山大學(xué)，秦皇島，0660042.河北省特種光纖與光纖傳感重點(diǎn)實(shí)驗(yàn)室，秦皇島，066004

針對(duì)移動(dòng)機(jī)器人快速同時(shí)定位和地圖創(chuàng)建(FastSLAM)中粒子退化問題，提出一種基于混沌優(yōu)化的中值導(dǎo)向粒子群優(yōu)化(MPSO)算法。該算法在粒子估計(jì)過程中引入觀測(cè)信息，調(diào)整粒子的提議分布，提高位置預(yù)測(cè)的準(zhǔn)測(cè)性?；煦鐑?yōu)化MPSO算法采用兩步優(yōu)化策略，首先通過中值導(dǎo)向加速度來改進(jìn)粒子的進(jìn)化速度，有效地克服粒子退化問題，改善算法的收斂性;然后針對(duì)粒子耗盡問題，在MPSO優(yōu)化算法中引入混沌搜索算法來尋找全局最優(yōu)位置，驅(qū)散聚集在局部最優(yōu)的粒子群，使其向全局最優(yōu)位置靠近，擴(kuò)大解空間的范圍，從而保持種群的多樣性。仿真和實(shí)時(shí)數(shù)據(jù)證明了該方法正確、可行。

快速同時(shí)定位和地圖創(chuàng)建；提議分布；中值導(dǎo)向粒子群優(yōu)化；中值導(dǎo)向加速度；混沌

0　引言

移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)是指移動(dòng)機(jī)器人在未知環(huán)境中運(yùn)動(dòng)時(shí)，構(gòu)建增量式地圖的同時(shí)自我定位的過程[1]。擴(kuò)展卡爾曼濾波(extend Kalman filter, EKF)通過增量式數(shù)據(jù)來評(píng)估機(jī)器人位姿和環(huán)境特征位置的聯(lián)合后驗(yàn)概率，被廣泛應(yīng)用于移動(dòng)機(jī)器人SLAM中。但基于EKF的SLAM存在兩個(gè)明顯的缺陷[2]：一是其計(jì)算復(fù)雜度高，導(dǎo)致特征數(shù)目不能太多，數(shù)據(jù)處理也極其復(fù)雜；二是如果數(shù)據(jù)關(guān)聯(lián)一旦錯(cuò)誤則EKF濾波器將發(fā)散。針對(duì)此局限，Montemerlo等[2-3]提出了快速同時(shí)定位與地圖創(chuàng)建(FastSLAM)算法，將SLAM后驗(yàn)概率降解為定位問題和基于機(jī)器人位姿的K個(gè)獨(dú)立環(huán)境特征的評(píng)估問題。但在標(biāo)準(zhǔn)FastSLAM方法中，基于序列重要性采樣(sequential impotance sampling, SIS)存在粒子退化問題[4]，重采樣雖然可以解決退化問題，但由于權(quán)重大的采樣多次被復(fù)制，權(quán)重小的采樣被忽略，所以這樣又導(dǎo)致了粒子的耗盡現(xiàn)象。目前很多學(xué)者致力于改進(jìn)FastSLAM算法，Yoon等[5]提出了模糊粒子濾波器算法，該方法可以克服樣本中干擾粒子對(duì)預(yù)測(cè)結(jié)果的影響，從而提高預(yù)測(cè)結(jié)果的精度，但是實(shí)現(xiàn)起來比較復(fù)雜。劉利枚等[6]將粒子群優(yōu)化(particle swarm optimization, PSO)的思想引入到粒子濾波中，有效地改善了粒子的退化問題，但不能有效地解決粒子的耗盡問題。

針對(duì)FastSLAM中粒子退化耗盡問題，本文提出了一種基于混沌MPSO(median-oriented PSO)優(yōu)化的FastSLAM算法，該算法利用機(jī)器人的觀測(cè)值對(duì)預(yù)測(cè)采樣過程進(jìn)行優(yōu)化，從而增強(qiáng)位置預(yù)測(cè)的準(zhǔn)確性，并且通過調(diào)整該算法的結(jié)構(gòu)，有效地解決了粒子退化問題。針對(duì)粒子的耗盡問題，采用混沌搜索算法對(duì)其進(jìn)行優(yōu)化，可以在迭代中產(chǎn)生許多局部最優(yōu)解的鄰近點(diǎn)， 以此來幫助惰性粒子逃離局部最優(yōu)點(diǎn)， 進(jìn)而能夠快速搜尋到全局最優(yōu)點(diǎn)，很好地保持了粒子的多樣性。

1　改進(jìn)的粒子群優(yōu)化算法

PSO算法[7]是一種基于群體智能的全局優(yōu)化算法，它利用m個(gè)粒子組成的粒子群在n維目標(biāo)空間中搜索最優(yōu)解。粒子i具有兩個(gè)屬性:位置xi=(xi1,xi2,…,xin)T與速度vi=(vi1,vi2,…,vin)T，其中xi為一個(gè)潛在解。粒子曾經(jīng)歷過的最好位置(個(gè)體最優(yōu)位置)為ppbest=(pp1,pp2,…,ppn)T和整個(gè)群體曾經(jīng)歷過的最好位置(全局歷史最優(yōu)位置)為pgbest=(pg1,pg2,…,pgn)T。每次迭代中，粒子i第d維的速度和位置按下式更新:

vid(t+1)=w(t)vid(t)+C1r(pid(t)-xid(t))+

C2r(pgd(t)-xid(t))

(1)

xid(t+1)=xid(t)+vid(t+1)

(2)

d=1,2,…,ni=1,2,…,m

式中，t為當(dāng)前進(jìn)化代數(shù)；r為分布于[0,1]之間的隨機(jī)數(shù)；C1、C2為加速常數(shù)；w(t)為慣性權(quán)重；xid(t)為當(dāng)前位置；pid為局部最優(yōu)位置；pgd(t)為全局最優(yōu)位置。

1.1中值導(dǎo)向PSO算法

在PSO中，粒子群在追逐最優(yōu)粒子時(shí)，越接近最優(yōu)粒子，其速度越小，粒子群表現(xiàn)出強(qiáng)烈的趨同性，容易陷入局部最優(yōu)，特別當(dāng)似然函數(shù)呈多峰狀態(tài)時(shí)，會(huì)使大量粒子聚集在次優(yōu)位置，無法到達(dá)最優(yōu)位置，使得其并不能有效地改善粒子退化與枯竭現(xiàn)象，從而影響算法的精確度。為了避免粒子的退化枯竭現(xiàn)象，本文對(duì)PSO算法的結(jié)構(gòu)進(jìn)行了優(yōu)化和改進(jìn)，從而提出了一種中值導(dǎo)向的粒子群優(yōu)化(median-oriented particle swarm optimization, MPSO)算法。

該算法如下:

vid(t+1)=vid(t)+Mid(t)

(3)

其中,Mid(t)代表中值導(dǎo)向加速度，其定義如下:

Mid(t)=ai(t)[r(pid(t)-pmd(t)-xid(t))+

r(pgd(t)-pmd(t)-xid(t))]

(4)

則每次迭代中，粒子i第d維的更新速度公式為

vid(t+1)=vid(t)+ai(t)[r(pid(t)-pmd(t)-

xid(t))+r(pgd(t)-pmd(t)-xid(t))]

(5)

其中，pmd(t)為d維空間中粒子當(dāng)前中間位置，ai(t)為加速系數(shù)，且

(6)

(7)

其中，fi(t)為第i個(gè)粒子適應(yīng)度，fmax(t)和fmed(t)分別為當(dāng)前粒子適應(yīng)度最大值和適應(yīng)度中間值:

fmax=max(fj(t))

(8)

fmed(t)=median(fj(t))

(9)

由經(jīng)典牛頓力學(xué)可知，一個(gè)向量粒子在Δt時(shí)間內(nèi)受到恒定的加速度時(shí)有

(10)

式中，x1和x2分別為最初位置和最后位置；a和v1分別為粒子的加速度和初速度。

把式(5)代入式(2)中，并根據(jù)牛頓經(jīng)典力學(xué)公式(式(10))可推出每次迭代中，粒子i第d維的更新位置公式:

xid(t))+r(pgd(t)-xid(t))]

(11)

在粒子群優(yōu)化算法中，中值導(dǎo)向加速度扮演了一個(gè)關(guān)鍵角色，不僅提高了算法的收斂速度，而且還可以幫助粒子逃離局部最優(yōu)。當(dāng)粒子遠(yuǎn)離機(jī)器人真實(shí)位姿時(shí)，改進(jìn)后的粒子群算法將有助于粒子向機(jī)器人真實(shí)的位置移動(dòng)，從而提高機(jī)器人位置估計(jì)的精確性。在標(biāo)準(zhǔn)的PSO中，隨著優(yōu)化的進(jìn)行，線性遞減的慣性權(quán)重w可以平衡粒子局部開發(fā)能力和全局探測(cè)能力[8]。而在MPSO中，中值導(dǎo)向加速度可以更好地權(quán)衡粒子的開發(fā)和探測(cè)能力。

1.2利用混沌對(duì)MPSO進(jìn)行優(yōu)化

混沌具有遍歷性、隨機(jī)性和規(guī)律性等特點(diǎn)，能夠在一定范圍內(nèi)按照自身的規(guī)律不重復(fù)地遍歷所有的狀態(tài)，具有易跳出局部最優(yōu)、搜索速度快、全局漸近收斂等優(yōu)點(diǎn)[9]。而在MPSO算法中，隨著搜索迭代的進(jìn)行，種群多樣性不斷損失，使得算法有可能過早收斂，因此在MPSO算法中引入混沌搜索算法來尋找全局最優(yōu)位置，驅(qū)散聚集在局部最優(yōu)的粒子群，使其向全局最優(yōu)位置靠近，擴(kuò)大解空間范圍，從而保持種群的多樣性，提高算法的精確度[10]。

本文采用適應(yīng)度方差作為判斷粒子陷入局部最優(yōu)的準(zhǔn)則。設(shè)粒子群的數(shù)量為n，fi為第i個(gè)粒子的適應(yīng)度，favg為粒子群的平均適應(yīng)度， 則適應(yīng)度方差的標(biāo)準(zhǔn)值為

(12)

粒子群體的適應(yīng)度方差為

(13)

其中，ρ2反映粒子群體的收斂程度，ρ2越小，群體越接近收斂。根據(jù)實(shí)際情況設(shè)定一收斂閾值a，當(dāng)ρ2

本文采用分段Logistic混沌映射初始化各粒子[11]:

(14)

其中，xid(t+1)?(0,1)，初值xid(t)取0.345，u是控制參量，當(dāng)u=4時(shí)，系統(tǒng)完全處于混沌狀態(tài)。與一般的Logistic或Tent混沌映射相比，式(14)產(chǎn)生的混沌序列在(0,1)間分布對(duì)稱性較好，具有良好的隨機(jī)性和對(duì)初值敏感性，并且在生成混沌序列時(shí)無需進(jìn)行擾動(dòng)運(yùn)算，使算法具有很高的效率。

利用式(14)產(chǎn)生的混沌序列進(jìn)行混沌搜索:

pgd=xgd+R(2xid(t)-1)

(15)

其中,R為搜索半徑，采用2種不同搜索半徑進(jìn)行搜索，一種是以pgd為中心進(jìn)行的混沌搜索:

R1=η pgd

其中，η為混沌搜索系數(shù)。

另一種是以原點(diǎn)為中心的混沌搜索:

R2=ρ(xmax-xmin)

其中，xmax和xmin分別為解空間上下界，ρ為混沌搜索的收縮因子，以減小混沌搜索的范圍，增加搜索精度，本文取ρ=1.1。

2　基于混沌MPSO的FastSLAM算法

基于混沌MPSO的FastSLAM算法流程步驟如下:

(1)預(yù)測(cè)。根據(jù)提議分布對(duì)當(dāng)前粒子集進(jìn)行預(yù)測(cè)采樣，獲得下一時(shí)刻的粒子st～p(st|st-1,ut)，其中，st～p(st|st-1,ut)為符合馬爾可夫的建議分布函數(shù)。

(2)混沌MPSO優(yōu)化。

③用式(13)產(chǎn)生的混沌序列進(jìn)行混沌搜索:

④通過引用適應(yīng)度函數(shù)ffitness來判斷機(jī)器人預(yù)測(cè)位置的優(yōu)化程度并計(jì)算路標(biāo)觀測(cè)的預(yù)測(cè)值z(mì)tpred:

(16)

(17)

(3)權(quán)重計(jì)算。通過下式計(jì)算粒子的權(quán)重：

(18)

通過混沌MPSO優(yōu)化，粒子集在計(jì)算權(quán)重前就更加接近機(jī)器人的真實(shí)位置，從而使得權(quán)重計(jì)算更能體現(xiàn)粒子的分布情況，重采樣過程更加有效，加速了粒子集的收斂，為下一時(shí)刻機(jī)器人的位置預(yù)測(cè)提供了一個(gè)更好的初始值。

3　實(shí)驗(yàn)研究及其分析

(a)標(biāo)準(zhǔn)的FastSLAM2.0地圖

(b)APSO-VI FastSLAM算法地圖

(c)混沌MPSO FastSLAM算法地圖圖1　各種算法的匹配地圖

為了驗(yàn)證所提出的基于混沌MPSO的FastSLAM算法的有效性，對(duì)基于混沌MPSO的FastSLAM算法、APSO-VI FastSLAM算法[12]以及標(biāo)準(zhǔn)的FastSLAM 2.0算法進(jìn)行比較。移動(dòng)機(jī)器人的仿真環(huán)境為80 m×80 m的平面矩形區(qū)域，環(huán)境中有37個(gè)特征點(diǎn)和11個(gè)路徑點(diǎn)，分別用“*”和“o”表示。如圖1所示。圖1a為采用標(biāo)準(zhǔn)的FastSLAM2.0算法得到的地圖，圖1b為采用APSO-VI FastSLAM算法得到的地圖，圖1c為采用基于混沌MPSO的FastSLAM算法得到的地圖。圖2a、圖2b分別為基于APSO-VI FastSLAM算法和混沌MPSO FastSLAM算法的特征點(diǎn)估計(jì)標(biāo)準(zhǔn)差誤差圖。

(a)APSO-VI FastSLAM算法

(b)混沌MPSO FastSLAM算法圖2　兩種算法的特征點(diǎn)估計(jì)標(biāo)準(zhǔn)差誤差圖

從圖1可知，在特征點(diǎn)估計(jì)上，圖1a明顯比圖1b和圖1c契合度低，而圖1c局部的特征點(diǎn)估計(jì)明顯比圖1b契合度高;同時(shí)移動(dòng)機(jī)器人在自身位姿路徑估計(jì)上，圖1a明顯沒有圖1b和圖1c路徑估計(jì)契合度高，對(duì)比圖1b、圖1c可知，在y軸(0,40 m)坐標(biāo)上圖1c比圖1b的路徑估計(jì)契合度明顯要高。這意味著無論基于機(jī)器人自身位姿估計(jì)還是特征估計(jì)，基于混沌MPSO FastSL-AM算法的建圖準(zhǔn)確度最高。從圖2可知，基于 APSO-VI FastSLAM算法的特征點(diǎn)估計(jì)誤差為0.44 m，而基于混沌MPSO FastSLAM算法的特征點(diǎn)估計(jì)誤差為0.28 m，較之前者特征匹配度提高了36.36%，因此基于混沌MPSO FastSLAM算法明顯改善了機(jī)器人在特征估計(jì)和自身位姿估計(jì)上的精確性。

為進(jìn)一步說明算法的性能，下面分別給出三種算法在x軸、y軸方向的定位誤差，如圖3所示。圖3a、圖3b和圖3c分別表示為標(biāo)準(zhǔn)的FastSLAM2.0算法定位方向誤差、APSO-VI FastSLAM算法定位方向誤差和混沌MPSO FastSLAM定位方向誤差。由圖3可知，標(biāo)準(zhǔn)的FastSLAM算法x和y方向的誤差最大值分別為1.8 m和1.15 m， APSO-VI FastSLAM算法x方向和y方向的誤差最大值分別為0.6 m和0.5 m，基于混沌MPSO的FastSLAM算法x方向和y方向的誤差最大值分別為0.45 m和0.3 m。 基于混沌MPSO的FastSLAM算法相對(duì)其他兩種算法在x方向上誤差分別降低了75%和25%，y方向上誤差分別降低了74%和40%，可知基于混沌MPSO的FastSLAM算法性能得到了很大的改善。

(a)標(biāo)準(zhǔn)的FastSLAM2.0定位方向誤差

(b)APSO-VI FastSLAM算法定位方向誤差

(c)混沌MPSO FastSLAM定位方向誤差圖3　各種算法的定位誤差圖

表1　有效樣本比較結(jié)果

4　結(jié)論

常規(guī)的FastSLAM2.0算法由于在位置預(yù)測(cè)過程中沒有考慮最新的觀測(cè)數(shù)據(jù)，因此影響地圖的精度，同時(shí)該算法存在粒子退化和耗盡問題。針對(duì)以上問題，提出一種基于混沌MPSO的FastSLAM算法。該算法在粒子估計(jì)過程中引入觀測(cè)信息，調(diào)整粒子的提議分布，增加位置預(yù)測(cè)的準(zhǔn)測(cè)性。通過中值導(dǎo)向加速度來改進(jìn)粒子的進(jìn)化速度，不僅提高了算法的計(jì)算速度，而且有效地克服了粒子的退化問題，改善了算法的收斂性，提高了建圖的精度。針對(duì)粒子耗盡問題，在MPSO優(yōu)化算法中引入混沌搜索算法來找全局最優(yōu)位置，驅(qū)散聚集在局部最優(yōu)的粒子群，使其向全局最優(yōu)位置靠近，擴(kuò)大解空間的范圍，從而保持種群的多樣性。實(shí)驗(yàn)仿真表明，該方法很大的提高了建圖的精確性。

[1]陳衛(wèi)東, 劉要龍, 朱奇光. 基于改進(jìn)雁群PSO算法的模糊自適應(yīng)擴(kuò)展卡爾曼濾波的SLAM算法[J]. 物理學(xué)報(bào)，2013, 62(17): 170506.

ChenWeidong,LiuYaolong,ZhuQiguang.FuzzyAdaptiveExtendedKalmanFilterSLAMAlgorithmBasedontheImprovedWildGeesePSOAlgorithm[J].ActaPhys.Sin., 2013, 62(17): 170506.

[2]MontemerloM,ThrunS.SimultaneousLocalizationandMappingwithUnknownDataAssociationUsingFastSLAM[C]//RoboticsandAutomation.ProceedingICRA’03.IEEEInternationalConference.Germany, 2003: 1985-199.

[3]MontemerloM,ThrunS.FastSLAM:aFactoredSolutiontoSimultaneoustoSimultaneousLocationandLocalizationandMapping[C]//ArtificialIntelligence,ProceedingsoftheNationnalConference.Canasa, 2002: 593-598.[4]DoucetA,GodsillS,AndrieuC.OnSequentialMonteCarloSamplingMethodsforBayesianFilter[J].StatisticsandComputing, 2000, 10(3): 197-208.[5]YoonCY,CheonMK,ParkM.ObjectTrackingfromImageSequencesUsingAdaptiveModelsinFuzzyParticleFilter[J].InformationSciences, 2013, 253(20): 74-99.[6]劉利枚, 蔡自興. 基于改進(jìn)的粒子群優(yōu)化的FastSLAM方法[J]. 高技術(shù)通訊, 2011, 21(4): 422-427.

LiuLimei,CaiZixing.AnApproachtoFastslamBasedonImprovedParticleSwarmOptimizationAlgorithm[J].ChineseHighTechnologyLetters, 2011, 21(4): 422-427.[7]KennedyJ,EberhartR.ParticleSwarmOptimization[C]//ProceedingsoftheIEEEInternationalConfernceonNeuralNetworks.Perth,Australia, 1995: 1941-1948.

[8]ZahraB,SitiMHS,ShafaatunnurH.MPSO:Median-orientedParticleSwarmOptimization[J].AppliedMathematicsandComputation, 2013, 219(11): 5817-5836.

[9]張學(xué)良，溫淑花，李海楠，等. 基于Tent映射的混沌粒子群優(yōu)化算法及其應(yīng)用[J]. 中國(guó)機(jī)械工程，2008，19(17): 2108-2112.

ZhangXuelang,WeiShuhua,LiHainan,etal.ChaoticParticleSwarmOptimizationAlgorithmBasedonTentMapping[J].Chinamechanicalengineering, 2008,19(17): 2108-2112.[10]IndranilP,AnnaK,SaptarshiD,etal.ChaosSuppressioninaFractionalOrderFinancialSystemUsingIntelligentRegroupingPSOBasedFractionalFuzzyControlPolicyinthePresenceofFractionalGaussianNoise[J].NonlinearDyn., 2012, 70 (4): 2445-2461.[11]張薇, 謝紅梅, 王保平. 一種新型的分段logistic混沌擴(kuò)頻通信算法[J]. 計(jì)算機(jī)科學(xué), 2013, 40(1): 59-62.

ZhangWei,XieHongmei,WangBaopin.NovelPiecewiseLogisticChaoticSpreadSpectrumCommunicationAlgorithm[J].ComputerScience, 2013, 40(1): 59-62.

[12]XuGang.AnAdaptiveParameterTuningofParticleSwarmOptimizationAlgorithm[J].AppliedMathematicsandComputation, 2013, 219: 4560-4569.

(編輯王艷麗)

Research on Mobile Robot FastSLAM Based on Chaos Optimization of MPSO Algorithm

Zhu Qiguang1,2Xia Cuiping1Chen Weidong1,2Chen Ying1

1.Yanshan University,Qinhuangdao,Hebei,066004 2.The Key Laboratory for Special Fiber and Fiber Sensor of Hebei Province,Qinhuangdao,Hebei,066004

Aiming at the particle degradation problem of an mobile robot FastSLAM a chaos optimization MPSO based algorithm was proposed. The algorithm incorporated the newest observation information into the prediction of particle, adjusted the proposal distribution of the particles, and the accuracy of prediction of a robot’s position was enhanced. The MPSO was solved by a sequential two-step optimization strategy. Firstly, the speed of evolution of particle was improved by the median-oriented acceleration, the particle degradation effectively was overcome, the convergence of the algorithm was improved. Then, focusing on the depletion of the particle, the chaos search algorithm optimization algorithms was introduced to MPSO global optimal position to disperse gathered at local optimum particle swarm to the global optimum location close to broaden the scope of the solution space, thus maintaining the population the diversity of simulation. The experimental results prove that the improved method is correct and feasible.

fast simultaneous location and mapping(FastSLAM); proposal distribution; media-oriented particle swarm optimization(MPSO); median-oriented acceleration; chaos

2014-01-15

國(guó)家自然科學(xué)基金資助項(xiàng)目(61201112,61172044)；河北省自然科學(xué)基金資助項(xiàng)目(F2013203250, F2012203169)；河北省普通高等學(xué)校青年拔尖人才計(jì)劃資助項(xiàng)目(BJ2014056)；燕山大學(xué)青年教師自主研究計(jì)劃資助項(xiàng)目(14LGA013)

TP183DOI：10.3969/j.issn.1004-132X.2015.05.004

朱奇光，男，1978年生。燕山大學(xué)信息科學(xué)與工程學(xué)院副教授。主要研究方向?yàn)闄C(jī)器人智能控制和多傳感器信息融合及應(yīng)用。發(fā)表論文30余篇。夏翠萍，女，1989年生。燕山大學(xué)信息科學(xué)與工程學(xué)院碩士研究生。陳衛(wèi)東，男，1971年生。燕山大學(xué)信息科學(xué)與工程學(xué)院教授。陳穎，女，1980年生。燕山大學(xué)電氣工程學(xué)院副教授。