基于混沌優化MPSO的移動機器人FastSLAM算法研究

朱奇光　夏翠萍　陳衛東　陳　穎

1.燕山大學，秦皇島，0660042.河北省特種光纖與光纖傳感重點實驗室，秦皇島，066004

基于混沌優化MPSO的移動機器人FastSLAM算法研究

朱奇光1,2夏翠萍1陳衛東1,2陳穎1

1.燕山大學，秦皇島，0660042.河北省特種光纖與光纖傳感重點實驗室，秦皇島，066004

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

快速同時定位和地圖創建；提議分布；中值導向粒子群優化；中值導向加速度；混沌

0　引言

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

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

1　改進的粒子群優化算法

PSO算法[7]是一種基于群體智能的全局優化算法，它利用m個粒子組成的粒子群在n維目標空間中搜索最優解。粒子i具有兩個屬性:位置xi=(xi1,xi2,…,xin)T與速度vi=(vi1,vi2,…,vin)T，其中xi為一個潛在解。粒子曾經歷過的最好位置(個體最優位置)為ppbest=(pp1,pp2,…,ppn)T和整個群體曾經歷過的最好位置(全局歷史最優位置)為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為當前進化代數；r為分布于[0,1]之間的隨機數；C1、C2為加速常數；w(t)為慣性權重；xid(t)為當前位置；pid為局部最優位置；pgd(t)為全局最優位置。

1.1中值導向PSO算法

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

該算法如下:

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

(3)

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

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維空間中粒子當前中間位置，ai(t)為加速系數，且

(6)

(7)

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

fmax=max(fj(t))

(8)

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

(9)

由經典牛頓力學可知，一個向量粒子在Δt時間內受到恒定的加速度時有

(10)

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

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

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

(11)

在粒子群優化算法中，中值導向加速度扮演了一個關鍵角色，不僅提高了算法的收斂速度，而且還可以幫助粒子逃離局部最優。當粒子遠離機器人真實位姿時，改進后的粒子群算法將有助于粒子向機器人真實的位置移動，從而提高機器人位置估計的精確性。在標準的PSO中，隨著優化的進行，線性遞減的慣性權重w可以平衡粒子局部開發能力和全局探測能力[8]。而在MPSO中，中值導向加速度可以更好地權衡粒子的開發和探測能力。

1.2利用混沌對MPSO進行優化

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

本文采用適應度方差作為判斷粒子陷入局部最優的準則。設粒子群的數量為n，fi為第i個粒子的適應度，favg為粒子群的平均適應度， 則適應度方差的標準值為

(12)

粒子群體的適應度方差為

(13)

其中，ρ2反映粒子群體的收斂程度，ρ2越小，群體越接近收斂。根據實際情況設定一收斂閾值a，當ρ2

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

(14)

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

利用式(14)產生的混沌序列進行混沌搜索:

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

(15)

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

R1=η pgd

其中，η為混沌搜索系數。

另一種是以原點為中心的混沌搜索:

R2=ρ(xmax-xmin)

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

2　基于混沌MPSO的FastSLAM算法

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

(1)預測。根據提議分布對當前粒子集進行預測采樣，獲得下一時刻的粒子st～p(st|st-1,ut)，其中，st～p(st|st-1,ut)為符合馬爾可夫的建議分布函數。

(2)混沌MPSO優化。

③用式(13)產生的混沌序列進行混沌搜索:

④通過引用適應度函數ffitness來判斷機器人預測位置的優化程度并計算路標觀測的預測值ztpred:

(16)

(17)

(3)權重計算。通過下式計算粒子的權重：

(18)

通過混沌MPSO優化，粒子集在計算權重前就更加接近機器人的真實位置，從而使得權重計算更能體現粒子的分布情況，重采樣過程更加有效，加速了粒子集的收斂，為下一時刻機器人的位置預測提供了一個更好的初始值。

3　實驗研究及其分析

(a)標準的FastSLAM2.0地圖

(b)APSO-VI FastSLAM算法地圖

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

為了驗證所提出的基于混沌MPSO的FastSLAM算法的有效性，對基于混沌MPSO的FastSLAM算法、APSO-VI FastSLAM算法[12]以及標準的FastSLAM 2.0算法進行比較。移動機器人的仿真環境為80 m×80 m的平面矩形區域，環境中有37個特征點和11個路徑點，分別用“*”和“o”表示。如圖1所示。圖1a為采用標準的FastSLAM2.0算法得到的地圖，圖1b為采用APSO-VI FastSLAM算法得到的地圖，圖1c為采用基于混沌MPSO的FastSLAM算法得到的地圖。圖2a、圖2b分別為基于APSO-VI FastSLAM算法和混沌MPSO FastSLAM算法的特征點估計標準差誤差圖。

(a)APSO-VI FastSLAM算法

(b)混沌MPSO FastSLAM算法圖2　兩種算法的特征點估計標準差誤差圖

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

為進一步說明算法的性能，下面分別給出三種算法在x軸、y軸方向的定位誤差，如圖3所示。圖3a、圖3b和圖3c分別表示為標準的FastSLAM2.0算法定位方向誤差、APSO-VI FastSLAM算法定位方向誤差和混沌MPSO FastSLAM定位方向誤差。由圖3可知，標準的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算法相對其他兩種算法在x方向上誤差分別降低了75%和25%，y方向上誤差分別降低了74%和40%，可知基于混沌MPSO的FastSLAM算法性能得到了很大的改善。

(a)標準的FastSLAM2.0定位方向誤差

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

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

表1　有效樣本比較結果

4　結論

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

[1]陳衛東, 劉要龍, 朱奇光. 基于改進雁群PSO算法的模糊自適應擴展卡爾曼濾波的SLAM算法[J]. 物理學報，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]劉利枚, 蔡自興. 基于改進的粒子群優化的FastSLAM方法[J]. 高技術通訊, 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]張學良，溫淑花，李海楠，等. 基于Tent映射的混沌粒子群優化算法及其應用[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混沌擴頻通信算法[J]. 計算機科學, 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

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

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

朱奇光，男，1978年生。燕山大學信息科學與工程學院副教授。主要研究方向為機器人智能控制和多傳感器信息融合及應用。發表論文30余篇。夏翠萍，女，1989年生。燕山大學信息科學與工程學院碩士研究生。陳衛東，男，1971年生。燕山大學信息科學與工程學院教授。陳穎，女，1980年生。燕山大學電氣工程學院副教授。