基于粒子滤波的移动机器人SLAM算法

1、第40卷第1期2010年1月 东南大学学报(自然科学版)JOURNALOFSOUTHEASTUNIVERSITY(NaturalScienceEdition) Vo.l40No.1Jan.2010doi:10.3969/.jissn.1001-0505.2010.01.022基于粒子滤波的移动机器人SLAM算法涂刚毅 金世俊 祝雪芬 宋爱国(东南大学仪器科学与工程学院,南京210096)摘要:针对FastSLAM算法对传感器精度要求较高,不适用于方向性差的超声传感器问题,提出了一种基于超声概率栅格地图环境特征点提取匹配的移动机器人粒子滤波同时定位与地图创建(SLAM)算法.该算法可分解为机器人

2、位姿估计和环境路标估计2个部分.基于蒙特卡罗定位原理利用粒子滤波算法对机器人运动轨迹进行估计;在建立全局超声概率栅格地图的基础上,利用概率栅格地图环境特征提取算法对环境路标坐标进行估计.实验证明,该算法较好地解决了超声测距传感器由于散射角大带来的特征点估计不准的问题,对环境路标和机器人轨迹的估计都比较准确.并对移动机器人累计误差进行了有效的补偿,减少了由于累积误差造成的移动机器人轨迹扭曲失真.关键词:移动机器人;同步定位与地图创建;粒子滤波器;栅格地图中图分类号:TP242 文献标志码:B 文章编号:1001-0505(2010)01 0117 06ParticlefilterSLAMmeth

3、odformobilerobotTuGangyi JinShijun ZhuXuefen SongAiguo(SchoolofInstrumentScienceandEngineering,SoutheastUniversity,Nanjing210096,China)Abstract:FocusingonthelimitedapplicationofFastSLAMalgorithmduetoitshighaccuracyrequir mentforsensorandunfitnessforultrasonicsensorwithpoordirectivity,asimultaneouslo

4、calizationandmapping(SLAM)algorithmutilizingparticlefilterisprovided,whichisbasedonultrasonicprobabilitygridmapfeaturepointsextractionandmatching.Thisalgorithmconsistsofrobotposeesti mationandenvironmentlandmarkestimation.ParticlefiltersareappliedtoestimatetherobottrajectoryaccordingtoMonteCarlometh

5、ods.Basedonglobalultrasonicprobabilisticgridmap,theenvironmentlandmarkpositionisobservedbyenvironmentfeatureextractionalgorithm.Theeffectivenessoftheproposedalgorithmisvalidatedbyexperimentalresul.tTheestimationaccuracyoffeaturepointposi tionaswellasenvironmentlandmarkareimproved.Thecumulativeerroro

6、ftherobotiscompensatedeffectivelyandthemobilerobottrajectorydistortionisreduced.Keywords:mobilerobo;tsimultaneouslocalizationandmapping;particlefilter;gridmap移动机器人的定位和地图创建是机器人领域的研究热点.在目前的研究中,针对已知环境的自主定位研究已有一些实用的解决方法.但在未知环境下机器人位置不确定时,如何将定位和地图创建结合,利用自身的定位来创建地图,同时利用地图进行自主定位和导航,即移动机器人的同时定位与地图创建(simultan

7、eouslocalizationandmapping,SLAM)问题还有待进一步研究.SLAM被认为是移动机器人实现真正全自主的核心问题.粒子滤波(particlefilter,PF)算法是基于蒙特卡罗和贝叶斯估计的滤波方法,也称为顺序蒙特卡洛方法(sequentialMonteCarlomethods,SMC),基本思想是采用一组从概率密度上随机抽取的加权样本来近似后验概率密度,可用于对非线性运动模收稿日期:2009 06 17. 作者简介:涂刚毅(1981 ),男,博士生;金世俊(联系人),男,博士,副教授,jinsj263.ne.t基金项目:教育部高等学校科技创新工程重大项目培育资金项目

8、(708045)、江苏省 六大高峰人才!资助项目(06 D 031)、东南大学优秀青年教师基金资助项目(4022001004).引文格式:涂刚毅,金世俊,祝雪芬,等.基于粒子滤波的移动机器人SLAM算法J.东南大学学报:自然科学版,2010,40(1):117 122.10.jiss0505.2010.p(xuk,xk-1)p(xk-1zk-1,uk-1)dxk-1(1)型进行估计.考虑到机器人位姿和环境特征的独立性,Montemerlo等提出了一种基于Rao Black wellized粒子滤波(Rao Blackwellizedparticlefil ter,RBPF)的FastSLAM算

9、法1 2k式中,为归一化常量;p(zkxk)为传感器测量值概率分布,即机器人观测模型;p(xkuk,xk-1)表示移动机器人在施加运动控制量uk后,位姿更新为xk的概率,称为机器人运动模型.SLAM问题被分解成机器人运动轨迹的粒子滤波估计和路标位置的扩展卡尔曼滤波(extendedKalmanfilter,EKF)估计2个过程.在采用EKF对路标进行估计时,假定高斯分布对于在探测过程中出现的异常点敏感,错误的匹配将产生严重的后果.FastSLAM算法对传感器精度要求较高,适用于方向性较好的激光测距传感器.超声测距传感器由于散射角大,测量精度低,观测不可靠,不适用于该算法.针对这一问题,本文将高

10、斯超声模型建图算法32 粒子滤波SLAM算法2 1 粒子滤波SLAM算法理论基础在SLAM中机器人状态向量xk应同时包含机器人的位姿信息sk和环境信息m,即xk=sk,mT式中,sk=(xk,yk,!k)表示移动机器人在全局坐标系下的位置坐标xk,yk和角度值;m=m1,(,mN表示N个环境路标坐标集.基于PF的SLAM方法的基本思想是:根据所有时刻环境感知信息z=z1,(,zk和机器人位姿更新信息u=u1,(,uk,估计机器人运动轨迹s=s1,(,sk的后验概率密度p(skkkk与粒子滤波算法4相结合,提出了一种基于超声概率栅格地图环境特征点提取匹配的移动机器人粒子滤波SLAM算法.利用粒子

11、滤波算法对机器人运动轨迹进行估计,利用概率栅格地图对路标坐标进行估计.算法基本框架可概括为环境感知、特征匹配、状态更新、地图更新4个过程.环境感知.通过超声测距传感器获取环境信息,建立全局超声概率栅格地图.#特征匹配.将当前测量值与超声概率栅格地图中环境路标进行匹配.状态更新.结合PF算法原理,对机器人粒子集的粒子位姿及权值进行更新,计算机器人粒子集概率中心位姿,作为机器人估计位姿.%地图更新.结合蒙特卡罗定位原理,根据机器人估计位姿和超声测量值对概率栅格地图和环境路标信息进行更新.z,u,n)、kkk环境地图和机器人轨迹的联合后验概率密度pkkkk1(s,mz,u,n).式(1)可等效表示为

12、p(s,mz,u,n)= p(s p(skkkkkkz,u,n)p(mNkkks,z,u,n)=ikkkkkz,u,n)kkk)p(mi=1s,z,u,n)(2)kkkk式中,n表示在k时刻所观测到的环境特征点信息,n=n1,n2,(,nk.如式(2)所示,SLAM问题可被分解为机器人位姿估计和基于机器人位姿估计的环境路标估计2个过程.2 2 算法设计结合PF算法基本原理算法如下.7k1 蒙特卡罗定位蒙特卡罗定位5 6,基于PF的SLAM(MonteCarlolocalization,MCL)的理论基础是贝叶斯滤波(Bayesianfilter,BF)理论.其核心思想是通过递归方式以目前所接收

13、到的所有数据为条件,对状态变量xk进行估计.移动机器人数据可以分为2类:环境感知信息zk= k, k,其中, k,k分别为超声传感器测距和方位角信息;机器人位姿更新信息uk= sk, !k,其中, sk为里程计增量信息, !k为角速度陀螺仪角度积分变化量.将zk作为系统的观测向量,后验概率密度p(xkzk,uk)可表示为如下贝叶斯概率模型形式p(7TT(i)(i)输入:机器人初始坐标s0和粒子数总数Num.输出:机器人运动轨迹及环境地图.根据初始坐标,随机生成由Num个粒子s0、粒子权值w0组成的粒子集s0=s0,w0i=1,(,Num.#预测机器人位姿(i)k-18 9(i)(i).根据k-

14、1时刻粒子集(i)sk-1中的每个粒子s及运动模型p(sk计算k时刻粒子sk.根据观测模型p(zkk.(i)(i)(i)uk,sk-1),(i)(i)sk)计算粒子sk对应:p(的特征点坐标,并将其与环境路标集中的某一路标第1期(i)涂刚毅,等:基于粒子滤波的移动机器人SLAM算法k(xL-xk/)+(yL-y/k)=y-ykL-!kxL-x119%对粒子权值wk进行更新,即wk(i)=wk-1p(zkNum(i)sk)(i)(3)(9)归一化处理为w10(i)k=w(i)k(i)k+wi=1(i)k(4)式中,(xL,yL)为特征点坐标;(xk/,yk/)为传感器中心坐标.理想情况下,基于粒

15、子sk的观测模型p(zksk)与移动机器人观测模型相同.若传感器中心(x/)与粒子坐标(xk,yk)的偏差为L,与k,y/k粒子观测模型p(zk计算公式为z(i)L,k(i)(i)(i)(i)(i)(i),根据粒子权值w样服粒子退化现象.环境地图更新对路标mi进行更新.2 3 粒子位姿估计2对粒子集sk进行重采,剔除权值小的粒子,复制权值大的粒子,克.利用p(mis,z,u,n)kkkksk)对应的环境特征点坐标(i)(i)以sk来表示粒子集合,粒子集中的每一个粒子s.Sk表示机器人可能的位姿,即Sk=s,(,si=1,(,Numsk(i)(1)k(i)k(i)k=xL,ky(i)L,(i)(

16、i=x/+ + kkkcos(!ky/+ + kksin(!(i)(i)(i)k(i)(i)k(i)k(i)k(i)=(5)(6)(7)xk+Lcos!+ kcos(!+ kkky(i)(i)k=(xk,yk,!)ksk=i=+1skwNum(i)(i)k(i)(i)(i)+Lsin!+ ksin(!+ k)(10)式中,zL,k为与粒子sk对应的特征点坐标.2 5 特征点坐标估计及匹配超声传感器由于存在散射角大、方向性差的问题,其测距信息存在较大的不确定性,需要根据一定的模型对其进行处理.常见的模型有中线模型和平2,11均分布弧线模型.本文中采用了高斯分布模型.该模型认为障碍物可能存在于发散

17、角圆弧上的任一点,以发射角中间线距离R为中心点,呈正态分布;同时在半径R的圆弧空间内没有障碍物存在.根据高斯分布模型及贝叶斯公式,可建立概率栅格地图.环境中的特征点被映射为栅格地图中的大概率区域.环境特征点坐标mi计算式如下:mi=Cxy.#式中,i表示粒子编号.随着移动机器人运动,粒子集sk的位姿应根据位姿更新信息uk进行同步更新.在理想状态下,粒子sk位姿更新模型p(sk(i)(i)uk,sk-1)应和移动机(i)器人运动模型相同.但实际情况由于考虑到里程计、角速度陀螺仪积分存在误差,所以在粒子位姿更新时加入里程计噪声分量N sk和角度噪声分量N sk.同时,为了避免由于粒子退化以及加入重

18、采样算法后出现的粒子分布过度收敛等问题而导致的定位精度降低,在进行位姿更新时加入位姿随机高斯发散增量Nx,Ny,N!.粒子位姿更新模型p(sk(i)+(P(OCxy)PCxy)uk,sk-1)如下式所示:xk-1(i)(i)( sk+N sk)cosk!k+N !kNxN(8)Cxy.#P(OCxy)(11)sk(i)=(i)yk-1+( sk+N sk)sin!k+Ny式中,#为特征点可能所在区域;PCxy为栅格Cxy中心坐标;P(OCxy)为栅格被占据的概率.机器人在感知新的观测值zk=( k, k)后,将其与已知环境特征点集m中的特征点进行匹配.在环境路标相距较远时,可依据最近邻居(ne

19、arestneighbor,NN)数据关联准则,即选取距离观测值最近的环境路标作为其匹配点.环境路标间距离相距越远,误匹配率越低,即f(sk,m)=mm.mind(mi,zL,k)i!k-(i)2 4 环境特征点坐标估计机器人运动及观测模型如图1所示,可表述为(i)(i)(12)(i)式中,d(mi,zL,k)表示与粒子sk对应的特征点坐标zL,k与环境路标mi之间距离;函数f(sk,m)返回值为粒子sk与环境特征点集m中最近的特征点距离.设Th为最大匹配误差阈值,若f(sk,m)图 (i)(i)(i)(i)(i)h,.1202 6 粒子权值更新东南大学学报(自然科学版) 第40卷式移动机器人

20、,在室外自然环境中完成了SLAM(i)若zL,k与mi匹配成功,可对粒子权值wk进行更新,依据式(2)可计算得到mi对应的观测值( ).当前时刻测量值zk= k, kk, k.令超声测距误差 k= k- k,超声测量角度误差 k= k- k. k主要误差源是机器人角度误差 !k,可认为 k0 !k. !k被认为符合高斯分布,则 ,!).测距误差也被认为符合高斯分kN(M!布, , ).选取误差项 kN(M k, k作为测量增量对粒子权值进行更新,则p(zk如下1222(i)(i)(i)(i)T(i)算法实验.移动机器人选取真实环境中的树作为绝对路标,在约8m18m的区域做连续绕圈实验.与结构化

21、的室内环境相比,室外环境更具有一般性,因而对本文提出的SLAM算法的检验提出了更高的要求. 探索者!机器人配备有超声测距传感器、角速度陀螺仪、电子罗盘、里程计等传感器,其特点是采用了单超声传感器扫描的设计方案.通过将一个超声传感器加装在舵机上,利用舵机旋转,带动超声传感器在前方1802方向上以11 252间隔进行扫描,弥补了超声传感器数量不足的缺陷,可替代超声环设计.虽然此设计方案在系统实时性方面不及传统方案,但在系统复杂度、成本、质量和体积方面具有显著优势,适合室外环境工作.图2为实验环境及超声概率栅格地图,通过对比,采用基于高斯超声模型算法2sk)计算式(13)(i):(i)p(zksk)

22、=GM!,2( k)GM,2( k)!式中,函数G为高斯概率计算公式.将式(13)代入式(3)则可根据当前测量值zk对粒子集权值进行更新.建立的概率栅3 实验结果实验载体采用了自主研制的 探索者!号四轮格地图较为准确地反映了环境特征.图2 实验环境及超声概率栅格地图选取真实环境中的树作为绝对路标,概率栅格地图中与其对应的环境路标概率中心坐标如表1所示.表1 环境路标坐标 cm路标123456概率中心(-117.5,44.7)(11.3,219.5)(88,96)(163.2,288.4)(174,-70.6)(268.6,122.3)实际中心(-115,48)(13,225)(91,101)(

23、165,293)(180,-73)(271,118)误差间距4.145.755.834.936.464.92考虑到测量误差、栅格细化不够等原因,可认为环境路标概率中心也与实际坐标近似一致.图3为部分时刻移动机器人粒子集分布变化图.图中, 椭圆!表示路标或粒子分布的3不确.图3 粒子分布图在未探测到有效声纳数据时,粒子位姿根据移动机器人轨迹进行运动更新.由于存在位姿随机高斯发,第1期涂刚毅,等:基于粒子滤波的移动机器人SLAM算法121利用获得的有效超声测距数据对粒子权值进行更新、重采样后,权值小的粒子被剔除,权值大的粒子被复制,粒子集合收敛,分布方差减小,如图4所示.图5(a)中,由于移动机器

24、人在运动过程中里程计、角速度陀螺仪等传感器累计误差在不断累积,轨迹偏差越来越大,特征点分布呈发散趋势.图5(b)为利用PF SLAM算法处理后获得的机器人粒子集概率中心轨迹图.由于粒子分布更新时存在随机增量,概率中心轨迹存在一定的扰动.通过不断将当前测量值与环境路标进行匹配,可以对累计误差进行有效修正.(a)粒子X坐标分布修正曲线如图6所示,由图可见.移动机器人累计误差得到了有效的补偿,角度等误差发散的趋势得到了抑制.(b)粒子Y坐标分布(a)X坐标(c)粒子角度分布图4 方差变化曲线(b)Y坐标移动机器人顺时针连续行驶6圈,原始实验数据与处理后的实验结果分别如图5(a)和(b)所示.(c)角

25、度图6 修正曲线4 结语实验证明,本文提出的基于超声概率栅格地图环境特征点提取匹配的移动机器人PF SLAM算(a)移动机器人原始轨迹法,对移动机器人累计误差进行了有效的补偿,解决了超声测距传感器由于散射角大带来的特征点估计不准的问题,对环境路标和机器人轨迹的估计都比较准确.今后的研究将主要集中在继续提高SLAM算法效率和精度方面.参考文献(References)1MontemerloM,ThrunS.SimultaneouslocalizationandmappingwithunknowndataassociationusingFastSLAM(b)移动机器人处理后轨迹C3ProcIEEEI

26、ntConfRoboticsandAutomation.ii,:.5122东南大学学报(自然科学版) 第40卷rativedistributedperceptionJ.Robot,2008,30(3):210 216.(inChinese)7ArulampalamS,MaskellS,GordonN,eta.lAtutorialonparticlefiltersforon linenon linear/non GaussianBayesiantrackingJ.IEEETransactionsonSignalProcessing,2002,50(2):174 188.8LiuXian en,M

27、aBojun,QiNingning,eta.lAsonardatabasedparticlefilteringlocalizationmethodformobilerobotC/Proceedingsofthe7thWorldCongressonIntelligentControlandAutomation.Chongqing,China:IEEEPress,2008:3920 3924.9LiuGuocheng,WangYongj.iMulti sensormovingtargettrackingusingparticlefilterC/Proceedingsofthe2007IEEEInt

28、ernationalConferenceonRoboticsandBiomimetics.673.10GiorgioG,CyrillS,WolframB.ImprovinggridbasedSLAMwithrao blackwellizedparticlefiltersbyadaptiveproposalsandselectiveresamplingC/ProcIEEEIntConfRoboticsandAutomation.Barce lona,Spain:IEEEPress,2005:2443 2448.11BankD,KampkeT.High resolutionultrasonicenvironmentimagingJ.IEEETransactionsonRobot ics,2007,23(2):370 381.12IoannisRekleitis.AparticlefiltertutorialformobilerobotlocalizationTR CIM 04 02R.Montrea,lQuebec,Canada:CentreforIntelligentMachines,McGillUniversity,2004.San