基于粒子滤波器的移动机器人定位和地图创建研究进展

文章编号:1002 0446(2007)03 0281 09基于粒子滤波器的移动机器人定位和地图创建研究进展*余洪山,王耀南(湖南大学电气与信息工程学院,湖南长沙 410082)摘 要:首先,对粒子滤波器的原理和研究进展进行了综述.然后,介绍了基于粒子滤波器的移动机器人定位研究进展.其次,给出了粒子滤波器在移动机器人地图创建领域的最新成果.最后,对粒子滤波器在移动机器人研究领域的未来发展方向进行了展望.关键词:粒子滤波器;蒙特卡洛定位;移动机器人地图创建;移动机器人定位;移动机器人同步地图创建和定位中图分类号: TP24 文献标识码: AA R eview on M obile R obot L ocalizati on and M ap buildi ng A l gorith m sBased on Particle FiltersYU H ong shan,WANG Y ao nan(Colle g e o f E lectri ca l and Infor ma tion Eng i neering,H unan Universit y,Chang sha410082,Ch i na)Abstract:F i rstl y,the research progress and princ i p l e o f particle filters a re overv ie w ed.Secondly,t he progress o fm ob ile robot locali zati on based on parti c le filte rs i s descri bed.T hird l y,the recent w orks o f pa rtic l e filters in m ap bu ildi ng f o r mob ile robots are presented.F i nall y,the future d i recti ons o f pa rtic l e filters in m ob ile robot are su mm ar i zed.K eyword s:parti c le filte r;M onte Carlo l o ca li za ti on;mob ile robot m ap bu il d i ng;mob ile robot localizati on;SLAM1 引言(Introduction)粒子滤波器(partic le filter)是一种基本统计工具,其核心是基于贝叶斯采样估计的顺序重要采样(Sequenti a l I m portance Sa m pli n g,S I S)滤波思想,通常也称之为Bootstrap滤波器、蒙特卡洛滤波器、Conden sation算法和Surv i v a l o f the Fittest算法,开始成功应用于目标跟踪、语音识别、移动机器人定位、地图创建、故障诊断、统计分析等领域[1~8].粒子滤波器具有可逼近任意概率分布的特性,并且计算简单方便,与传统卡尔曼滤波器方法、马尔可夫算法相比,具有其特定的优越性.De llaert等[9]和Fox等[10]分别独立提出将粒子滤波器应用于移动机器人定位研究中,即蒙特卡洛定位算法(M onte Carlo Localization,MCL).此后算法被研究人员广泛采用和扩展,迅速成为继EKF模型、马尔可夫模型后移动机器人定位领域的一个新的研究热点[11].在此基础上,研究人员将粒子滤波器引入地图创建研究,提出了一系列移动机器人同步地图创建和定位方案,如FastSL AM算法[12,13]、粒子滤波器和其他智能计算方法的复合地图创建方法等,得到了移动机器人地图创建研究人员的广泛认可.本文拟对粒子滤波器在移动机器人定位、地图创建等应用领域的最新研究进行综述,分析和总结该类算法的优缺点和可能研究方向.2 粒子滤波器原理和研究进展(The research progress and principle of particlefilters)粒子滤波器的研究源于H a mm ersley等[2]提出的基本SI S方法.1993年Gor don等[4]提出了一种新的基于SIS的Bootstrap非线性滤波方法,奠定了粒子滤第29卷第3期 2007年5月机器人 ROBOT V o.l29,N o.3M ay,2007*基金项目:国家自然科学基金资助项目(60375008);教育部博士点基金资助项目(20030532004);湖南大学优秀博士论文创新基金资助项目(521218006).收稿日期:2006-07-03波算法的基础,随后粒子滤波器的研究取得了迅速发展,代表性的如Liu 等[14]提出的连续重要性采样方法、K itaga w a 等[15]提出的蒙特卡洛滤波器和平滑器方法、Isard 等[16]提出的Condensati o n 算法、Crisan 等[17]提出的连续时间滤波器方法等.上述研究为粒子滤波器算法提供了坚实的理论基础和研究框架,并使粒子滤波器的研究逐步走向应用.2.1 粒子滤波器原理及关键技术如图1所示,粒子滤波器通过粒子集和粒子对应权值组成的随机采样数据集合s(k )表示相应的概率分布p (x k z k ),以有限样本点的求和运算取代积分运算,从而获得状态最小方差估计.用数学语言描述如下:对平稳随机过程,假定k -1时刻系统的后验概率密度为p (x k -1z k -1),依据一定原则选取n 个随机样本点,k 时刻获得测量信息后,经过状态和时间更新过程,n 个粒子的后验概率密度可近似为p (x k z k )[9].随着粒子数目的增加,粒子的概率密度函数逐渐逼近状态的概率密度函数,粒子滤波估计即达到最优贝叶斯估计的效果[5,6].粒子滤波算法摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约条件,并在一定程度上解决了粒子数匮乏问题,因此近年来该算法在许多领域得到成功应用.图1 粒子滤波器算法单次迭代处理对应的概率密度和粒子集[9]F ig .1 The probab ilit y dens ities and parti c le sets f o r one ite ra tion o f the particle filters a l go rith m [9]假设通过M 次迭代处理,采样集合s (k )可精确逼近实际概率分布.在每个时刻t ,定义随机测量数据{x (m)1:n ,w (m )n }M m =1,其中x (m)n 表示时刻n 的第m 个粒子,w (m)n 为相应粒子的权值,x (m )1:n 是信号的第m 个采样轨迹.如果这些粒子集均根据观测量z 1:n 和基于概率分布p (x 1:n z 1:n )的采样轨迹而获取,则基于式(1)近似相应的概率分布:p (x 1:n z 1:n )Mm=1w(m)n(x 1:n -x(m )1:n)(1)粒子滤波器包括三部分:1)生成粒子集(采样步骤);2)粒子权值计算(重要性步骤);3)重采样.2.1.1 生成粒子集粒子集x (m)n是根据如式(2)所示的重要性概率密度函数 (x n )提取生成的,通过迭代处理可计算得到粒子的权值,如式(3)所示.(x 1:n )= (x 1z 1)!nk=1 (x k x 1:k-1,z 1:k )(2)x (m )n~ (x n x (m )n-1,z 1:n )(3)重要性概率分布 (x n x (m )n -1,z 1:n )在粒子滤波器设计中扮演着非常重要角色,因为它负责生成表示期望概率密度的粒子集.如果提取的粒子集是在概率密度较小的区域内,则根据粒子集和相关权值获得的估计值也会很小,则对信号的后续跟踪处理可能会发散.反之,如果在概率密度非常高的区域提取粒子集,则粒子滤波器的性能会大大增强.有人提出p (x n x (m )1:n -1,z n )是最优重要性函数,但缺陷在于难以采样和对粒子集权值进行更新,因为需要积分运算[5],因此通常采用次优方案,如局部线性化、基于无先导变换的高斯近似法、基于辅助粒子滤波器的两步骤获取方法[18]等.2.1.2 重要性步骤重要性步骤包括两步:权值的计算和归一化.令重要性函数如式(2)所示,则权值更新方式如下:w*(m )n=w(m)n-1p (z n x (m )n )p (x (m)n x (m )n-1)(x (m )n x (m)1:n-1,z 1:n )(4)归一化处理如下:w(m )n=w *(m )nMj=1w *(j)n (5)2.1.3 重采样粒子滤波器的一个重要问题是粒子集权值的退化,即随着时间的增长,一部分权值变得非常大,而其余的部分则变得微不足道.重采样就是要剔除较小权值的采样,从而集中于显著权值的采样进行处理.采样过程中使用的标准算法有多种[6],如残差重采样、分支校正、系统重采样和带有拒绝控制的采样方法.通常,基本的随机重采样算法步骤如下[5,8].(1)从x (m )n中按照与标准归一化重采样函数(m)n成比例的概率分别独立提取x~(i (m ))n,其中m =1,∀,M 和i (m )=1,∀,M.与这些采样对应的新权值分别为:w~(i (m ))n=w (m )ni(m)(6)(2)返回新的随机测量数据:282 机器人2007年5月{x~(i (m ))n,w~(i (m ))n}Mi (m )=1(7)这里i(m)代表重采样后粒子集在内存中的索引号.上述粒子滤波器算法的表示具有一定程度上的通用性.例如,粒子滤波器常规直接采样方法是选择 (m)n =w (m )n .当没有重采样处理时,对应的 (m)n =1/M .辅助粒子滤波器重采样方法是设置a (m)n =w (m )np (z n +1!(m )n +1)和 (x n )=p (x n x(m )n -1),其中!(m )n为平均值,即与概率密度p (x n x (m )n -1)相关的数据变量.某些重采样算法(例如RR ),利用数组复制因子r (m )替代数组索引i (m ),复制因子表示在重采样处理中每个粒子被复制的次数.然后重采样过程根据 (m)n 采样r(m),其对应的支持量由粒子集x(m )n定义.重采样通过集中粒子集到高后验概率分布的区域以提高对未来状态的估计,但是由于提高了估计的方差,所以降低了当前估计的精度[7].因此,重采样的实施必须小心,可预先估计进行重采样的必要性[8].2.2 粒子滤波器算法存在的问题与研究热点最新的关于粒子滤波器算法的研究主要集中体现在重要性函数的设计、降低计算复杂度条件下的重采样策略、降低计算复杂度条件下的次优算法、粒子滤波器的收敛性结论等方面[1,5~7,19].对于粒子滤波器而言,粒子数匮乏是其主要缺陷,即随着迭代次数增加,粒子丧失多样性的现象.Doucet 等[20]从理论上证明了SI S 算法出现粒子数匮乏现象的必然性,而最有效的解决方法是选择重要性函数和采用重采样方法.为解决粒子数匮乏问题,研究人员也提出了很多针对状态空间模型的改进算法,如辅助变量粒子滤波算法[18]、局部线性化方法[21]、拒绝采样方法[22]等.在重采样改进方法上,H iguch i 等[23]提出通过引入遗传算法和进化算法,增加重采样过程中粒子的多样性,Fox 等[24]则根据滤波性能动态调整粒子数.3 基于粒子滤波器的移动机器人定位(M obile robot localization based on particlefilters)Dellaert 等[9]和Fox 等[10]最初将粒子滤波器算法应用于移动机器人定位,形成了一个新的移动机器人定位研究方向###蒙特卡洛定位算法.在此基础上,研究人员针对算法的计算复杂度、实时性、可靠性等方面做了进一步研究,并开始广泛应用于基于声纳、激光和视觉传感器等类别传感器的移动机器人系统定位中,成为继EKF 模型、HMM 模型之后新的移动机器人定位模型.3.1 标准MCL 算法原理和特性3.1.1 MCL 算法原理和步骤MCL 定位算法集成了机器人感知模型和运动模型[5,9~11,25],利用N 个加权随机采样或粒子集合S ={s i ,w i i =1,∀,N }表示机器人位姿后验估计概率B el(X ),算法基本原理可表示为:B el(x t )=p (x d 0,∀,t )(8)B el(X ) S (9)这里x t 为t 时刻对应的状态,d 0,∀,t 表示从时间0到t 的数据.t 时刻机器人位姿的概率分布如式(10)所示:B el(x t )=p (x o t ,u t-1,o t-1,u t-2,∀,o 0)=∀p (o t x t )∃p (x t x t-1,u t-1)B el(x t-1)d x t-1(10)其中o 0,∀,t ,u t -1,∀,0分别对应移动机器人传感器测量数据和运动控制的测量数据,条件概率p (x t x t -1,u t -1)为机器人运动模型,p (o t x t )代表机器人感知模型,∀为归一化常量.方程(10)为MCL 算法的基础.机器人运动过程中,不断生成机器人位姿的采样集合,根据粒子滤波器实现对机器人状态的预测和更新,通过多次迭代处理来精确逼近位姿的后验分布估计.利用MCL 算法进行移动机器人全局定位,主要可以分为采样、预测、更新和权值归一化四个步骤,详细步骤见图2,其中初始位姿概率在机器人所在空间范围内呈均匀分布,加权值统一为1/m [26].(1)根据B el(x -1)采集状态X t -1,按照由重要性因子p it -1规定的权值#it -1从表示B el(x -1)的采样集合S t -1中随机抽取采样x it -1.(2)预测:根据上次运算获取的状态集合S t -1和机器人运动控制量的测量信息预测当前机器人的位姿状态p (x t x t -1,u t -1),对于状态集合S t -1中的每个采样x it -1,根据p (x x it -1,u t -1),抽取一个采样x %it -1.通过上述操作,获取一个新的集合S %t 来近似预测分布p (x k dk -1),此时集合S %t 并没有考虑t 时刻任何传感器测量值.(3)更新阶段:在此阶段,我们考虑传感器测量值o t ,并对S %t 中的每个采样值进行加权处理,其权值为#it =p (o x %ik ),即给定o t 时预测值x %ik 的可信度.(4)权值归一化:对于N 个采样,分别对其权值进行归一化处理,获得t 时刻的采样集合S t ={〈x it ,283第29卷第3期余洪山等: 基于粒子滤波器的移动机器人定位和地图创建研究进展w it 〉i =1,2,∀,N p },从而获得关于Bel(x )的采样近似.1.输入:S t -1={〈x (i)t -1,w (i)t -1〉i =1,∀,N p }表示信任度Be l(x t -1),控制测量变量u t -1,观测量y t 2.S t :=∃, :=0 //初始化 3.For i :=1,∀,N p do//生成N p 个采样4.//重采样:从先前的信任度中获取状态从离散分布中根据S t -1的权值获取索引为j 的采样 5.//采样:预测下一状态基于x (j)t -1和u t -1,按p (x t x t -1,u t -1)获取采样x i t6.w (i)t :=p (y t x (i)t ) //计算重要性权值 7. := +w (i)t //更新归一化因子 8.S t :=S t &{〈x (i)t ,w (i)t 〉}//新采样插入采样集 9.end do10.for i :=1,∀,N p do//权值归一化处理11.w (i)t :=w (i)t/ 12.end do 13.ret u rn S t图2 M CL 算法基本原理和步骤[26]F i g .2 T he basic pr i nciple and steps o fM CL a l gor ith m[26]从而q t :=p (x t x t -1,u t -1)∋B el(x t -1)对应重要性采样后的预测分布,用于近似期望后验概率分布:p (o t x t )p (x t u t-1,x t-1)B el(x t-1)p (o t d 0,∀,t-1,u t-1)(11)上述更新和迭代处理步骤如图3所示,图中黑点表示机器人位姿的采样分布.从图3(a)~(c )可以看出,机器人位姿从随机分布逐渐收敛,最终收敛于实际机器人位姿,收敛速度与采样数目直接相关:O (1/m ).3.1.2 基本MCL 算法的优点和特性MCL 为在线算法,可作为Any ti m e 算法应用,而定位精度与时间相关,采样集合的尺寸在计算精度和计算复杂度之间达到一种平衡.相对于其他定位方法,基于采样表示MCL 方法的优点如下[9,10]:1)与卡尔曼滤波器方法相比,可以表示多模分布,并可实现机器人的全局定位;2)与基于栅格的马尔可夫定位方法相比,能以相当高的频率集成测量数据;3)与固定尺寸栅格单元的马尔可夫定位方法相比,具有更高的定位精度,原因在于采样中对应的状态表示没有被离散化;4)易于实施.(a)算法初始化 (b )定位处理中 (c)成功定位图3 用于全局定位的M CL 算法处理过程[10]F ig .3 The process ofM CL a l gor it hm for g l obal l o ca liza ti on [10]然而算法也存在不足,其原因在于估计的随机性.例如,如果采样集的尺寸较小,机器人可能仅因为MCL 没能够生成正确位置的一个采样而导致失去对其位置的跟踪.算法也不适用于机器人绑架问题,因为一旦机器人处于绑架状态,则可能在机器人新位姿附近没有合适的采样.因此,当传感器不足够准确时,上述基本的MCL 算法性能会急速下降.极端情况下,常规M CL 算法在传感器信息无噪声干扰时也会失败.3.2 改进MCL 算法为避免和减少常规M CL 算法的缺陷,研究人员提出了多种改进方案.Thrun 等[25]提出了混合MCL,算法综合了常规M CL 和双M CL 方法.算法通过交换MCL 算法中预测分布和重要性权值因子的角色,转换M CL 的采样过程,其中常规MCL 首先利用里程计估计新位姿,然后利用传感器测量数据评价这次采样的重要性;双MCL 方法利用最近传感器测量数据估计位姿,然后利用里程计评价估计值与机器人先前概率值和里程计数据的符合程度.上述每种方法均不是完美的,但是复合起来,效果非常好.特别是,当采样集合尺寸较小时(例如50个采样),混合MCL 算法效果良好,相比先前的M CL 算法,可以更快地284机器人2007年5月从机器人绑架问题中恢复出来.然而粒子滤波器表示能力的提高,是以高计算复杂性为代价的.如何实现粒子滤波器的在线、实时估计引来了新的研究课题.Kwok等[26]提出了一种实时粒子滤波器,解决由于计算资源的限制带来的局限性.方法不再舍弃传感器测量数据,而是将滤波器更新过程中到来的所有采样放入不同的观测集合中.这里,实时粒子滤波器利用混合采样集合代表状态空间分布,因此避免了由于独立采样数目的不足导致的滤波器分歧问题.混合分量的权值通过采用蒙特卡洛近似梯度实现最优化处理,以减少混合表示引入的近似误差,最大程度上接近最优后验分布.粒子滤波器算法执行每次更新的时间复杂度与估计所需的采样数目成线性关系.因此人们对采样的有效使用进行了研究,以保持采样集合为合理尺寸.Thr un等[25]提出在后验估计中加入观测采样,从而动态控制采样集合数目,但是该方法需要可有效生成采样的传感器模型.G ilks等[27]提出引用MC M C (M arkov ChainM onte Carlo)步骤提高基于采样表示的后验估计性能;V lassis等[28]提出采用辅助粒子滤波器,通过加入一个步骤减少预测分布和目标实际分布之间的误匹配,从而减少重要性权值变化,提高重要性采样的效率.Fox等指出,在估计步骤中自适应选择采样集合数目可大大提高粒子滤波器的效率,并给出Kull back Leibler距离(Ku ll b ack Le i b ler distance,KLD)采样解决方案[5].KLD采样方法的主要思想在于限制粒子滤波器的采样表示引入的近似误差,如果采样概率集中于一小部分状态空间,则选择小数目采样,而如果状态不确定性较高,则选择大数目采样. Kw ok[26,29]将KLD采样原理用于其提出的实时粒子滤波器算法中,形成了自适应实时粒子滤波器,进一步提高了算法的执行效率.3.3 其他MCL算法研究M CL定位方法不仅在测距类移动机器人系统中取得成功,在基于视觉传感器的机器人系统中也取得了成功,该类算法通常称之为Condensation算法. Dellaert等[9]将标准的M CL算法应用于装载有视觉传感器的移动机器人系统,解决了卡尔曼滤波器无法实现高不确定性环境下的定位的难题.V lassis 等[28]为解决图像的高维传感器观测和位置观测模型问题,使用基于NN(N earest N eighbour)条件概率估计的逆非参数观测模型,以解决图像遮挡和机器人绑架问题,方法成功应用于基于全景摄像机的室外移动机器人自主定位.Jensfelt等[30]通过TBF算法提取机器人环境中的有效路标特征,然后利用Con densation算法实现机器人的实时动态定位.Lenser 等[31]提出当机器人丢失时,在MCL算法中加入传感器采样,即传感器重置定位算法,并成功应用于Robo Cup99索尼行走机器人系统中,可在有限计算能力下实现机器人的鲁棒定位.W o lf等[32]提取环境图像不变特征作为路标,根据环境地图为数据库中的每个图像抽取可能视点集合,然后结合蒙特卡洛定位算法实现对机器人的可靠定位与跟踪.L i n aker 等[33]利用移动机器人装载全景摄像机执行基于外观的实时全局定位处理.算法直接对全景摄像机图像进行处理,生成低维旋转恒定特征向量.利用这些特征向量,粒子滤波器实现移动机器人位姿的精确连续估计.4 基于粒子滤波器的移动机器人地图创建(M obile robot map building based on particle filters)M onte m erl o等[12,13]首先提出了FastSLAM方法,该方法将粒子滤波器和扩展卡尔曼滤波器相集成,提供了一种新的移动机器人同步定位与地图创建(SLAM)方案,引起了广泛关注.目前,基于粒子滤波器的SLAM研究主要分为Fast S LAM算法及其改进算法研究、粒子滤波器算法与其他智能算法的复合算法研究、其它基于粒子滤波器的地图创建方法. 4.1 FastSLA M算法及其改进M ur phy[34]研究发现,如果知道机器人的确切路径,则路标位置的确定可分解为K个独立的估计问题,每一问题对应于一个路标,并由此提出一种有效的算法用于栅格地图的学习.在此基础上,M onte m er lo等[12]提出了Fast S LAM解决方案.该方法将SLAM 问题分解为机器人定位问题和基于机器人位姿估计的路标集合估计问题.方法利用改进的粒子滤波器估计机器人路径的后验分布,每个粒子拥有K个卡尔曼滤波器,用于路径估计条件下的K个路标位置估计,具体算法如下.4.1.1 标准FastSLA M算法FastSLAM算法具体包括3个步骤[12]:1)首先采集新的位姿,扩展对机器人路径的后验估计;2)更新观测路标估计,在此过程中Fast S LAM算法只需要表示机器人路标的两个参数,而基于E KF的SLAM算法需要2K+3(K为路标数目)个参数;3)计算采样权值,进行重新采样处理.285第29卷第3期余洪山等: 基于粒子滤波器的移动机器人定位和地图创建研究进展Fast S LAM 算法同样采用概率方法表示位姿运动模型p (s t u t ,s t -1)和观测模型p (z t s t ,%,n t ),其中s t 表示t 时刻位姿,u t 代表机器人控制量,%=(%1,%2,∀,%k )代表环境路标,z t 代表t 时刻的观测量,n t 代表t 时刻观测到路标的索引号.此时SLAM 问题就是基于观测量z t =z 1,∀,z t 和控制量u t=u 1,∀,u t 确定所有路标%和位姿s t 位置的过程.如果已知机器人路径s t和相关性变量n t,则所有路标的估计均是相对独立的,这也是Fast S LAM 算法的基础.如果数据关联性已知,则FastSLAM 可表示为:p (s t,%z t,u t,n t)=p (s tz t,u t,n t)(!kp (%s t,z t,u t,n t)(12)从而算法将SLAM 分解为机器人路径s t的后验估计问题和基于路径估计的K 个环境路标位置的估计问题.Fast S LAM 算法利用Rao B lackw ellized 粒子滤波器进行路径估计p (s tz t,u t,n t).路标位姿估计p (%k s t,z t,u t,n t)利用卡尔曼滤波器实现,每个不同路标采用独立的滤波器.由于粒子滤波器中的任意一个粒子具有自己的局部路标估计,因此对于M 个粒子集和K 个路标,将对应KM 个卡尔曼滤波器.Fast S LAM 算法中M 个粒子滤波器的任一粒子的结构如下,即关于路径和路标位置的完全后验估计:S [m ]t=〈s t,[m],![m ]1,t ,&[m]1,t,∀,![m ]N,t ,&[m ]N ,t〉(13)这里![m ]k和&[m ]k分别表示第K 个路标%k 的均差和协方差,s [m ]t代表t 时刻第m 个采样,它根据p (s tu t ,s [m ]t -1)进行增进式估计.新的采样集合S t 的预测概率分布为p (s tzt -1,u t,nt -1).根据新的观测信息,其每个采样的权值的计算为:w [m]t =∀p (z t s t,[m ],z t-1,n t )(14)为提高算法处理效率,Fast S LAM 算法用树结构表示路标位置的不确定性,并在此基础上完成采样集合的更新处理,最终算法复杂度降低为O (M l o g K ),其中M 为粒子滤波器的数目,K 为路标数目,从而大大快于基于EKF 的SLAM 算法.4.1.2 FastSLA M 算法的局限性与改进方案在上述FastSLAM 中,位姿s [m ]t根据机器人运动控制量对应的预测分布进行估计,而没有考虑t 时刻获取的观测值z t ,算法通过重采样处理对新测量信息进行集成.这种处理存在一些问题,例如当机器人运动误差大于测量误差时,位姿的采样将落入低可能性区域范围,然后在以高概率进行重采样时将终止处理.而在实际机器人系统中,运动误差相当高,因此有必要对上述算法进行改进,提高采样的效率.M on te m erl o 等[13]提出了Fast S LAM 2 0算法,即在运动量u t 和观测值z t 基础上进行位姿的采样处理:s [m]t~p (s t s t-1,[m],u t,z t,n t)(15)相应地预测分布也改为p (s t s t -1,[m ],u t,z t,n t).对观测路标的估计处理与原来的算法相同.尽管算法在位姿采样中考虑了新的观测信息,根据权值进行重新采样依然是必须的,因为采样集合并不一定能够与期望值匹配,采样权值如下:w [m]t )p (s t st -1,[m ],u t,zt -1,n t).实验证明,FastSLAM 2 0算法相对于FastSLAM 1 0算法具有更强的鲁棒性,并从理论上首次证明了算法的收敛性.图4为基于Fast S LAM 2 0算法的移动机器人室外地图创建结果,其中图中点代表基准点、实线为FastSLAM 2 0算法的计算地图,点划线为基于GPS 的测量地图.将图4中的3幅子图进行对比,可以看出算法具有很好的收敛性.(a)原始机器人里程计信息 (b)Fast SLAM 2 0算法结果(M =1) (c)基于动态特征管理的Fast SLA M 2 0图4 F ast SLAM 2 0算法应用于V ictor i a P ark 基准数据集[13]F i g .4 F ast SLAM 2 0appli ed to t he V i c t o ria P ark bench m ark data set [13]286 机器人2007年5月上述两种FastSLAM方法均假定特征间的数据关联是已知的.然而实际环境特征存在有很大的不确定性,M onte m erlo提出基于最大相似度方法估计每个粒子的关联变量:n[m]t=arg m ax ntp(z t s[m]t,n t),从而不同的粒子将对应不同的值,并且可能对应不同的n[m]t路标,从而有效解决了传统EKF方法对应的错误关联问题[12,13,35].N ieto等[36]在此基础上对Fast S LAM的实时性数据关联问题进行了深入研究,并应用于多机器人系统的实时地图创建中.上述FastSLAM算法,仅用于特征地图创建.H ahne l等[37]提出的Fast S LAM算法直接利用原始激光测距数据替代路标的地图表示,从而使数据关联处理更为简单.Ranganathan等[38]采用MC MC采样方法进行拓扑空间的构建,以扩展贝叶斯概率模型框架.为实现MC MC对拓扑空间的采样,每个拓扑被认为是关于路标测量数据集合的一个子分区;然后对子分区进行采样,作为给定传感器测量值后的拓扑空间中目标的后概率分布.上述方法一定程度上证实了粒子滤波器算法对各种地图表示法的适应性.4.2 混合粒子滤波器在地图创建中的应用除Fast S LAM方法外,粒子滤波器算法与其他智能算法的复合地图创建算法研究也取得重要进展. L i[39]提出一种基于粒子滤波器和进化机制的移动机器人同时定位与地图创建的方法.方法基于生物物种的竞争进化机制,将粒子滤波器扩展为进化粒子滤波器(Co E vo l u ti o n Partic le Filter,CEPF).在CEPF 中,粒子集分为多个种群,分别表示机器人位姿或路标.通过多个子类的进化演变处理,从而可以同时估计多个独立假设.粒子滤波器的数目根据种群增长模型可动态调整.此外,利用进化计算中的交叉、变异操作,种群间的进化可驱动粒子集向期望后验概率较大的区域移动,从而少量的粒子集即可很好地代表期望概率分布,以实现精确后验概率估计.基于进化计算的特性,方法相对于EKF和传统粒子滤波器算法具有更强的鲁棒性,并且可实现采样集合大小的自适应调整.M asson等[40]提出基于扩展卡尔曼滤波器和蒙特卡洛算法集成的大规模室外环境下移动机器人同步定位与地图创建的解决方案.但是与Fast S LAM算法不同,M asson等利用CEKF(Co m pressed Ex tended K al m an F ilter)解决常规条件下的SLAM问题;当机器人位姿误差的累积增大,E KF滤波器的单模概率分布特性无法满足工作需要时,算法采用粒子滤波器进行数据关联处理,将机器人不确定问题转化为定位问题.当成功实现定位处理后,重新采用CEKF 算法执行SLAM处理.算法有效地集成了CE KF的计算优势和M on te C arlo算法的定位能力,在大规模室外环境中得到的试验结果证明了方法的有效性和可行性.Thrun等[41]提出将最大相似度地图创建算法和MCL定位技术相融合,在线解决大规模环路环境下的高精度地图的创建.由于增进式最大相似度方法存在很大的缺陷,特别是在环行闭合区域时,累计误差已经增至非常大,从而难以构建局部连续的地图.为解决这个问题,方法采用复合估计处理,即对最大相似度算法进行扩展,引入第二个估计器采用粒子滤波器进行机器人位姿(非地图)的完全后估计.算法成功地用于多机器人系统的三维环境地图创建. 4.3 基于粒子滤波器的其它地图创建算法与FastSLAM和前面的混合粒子滤波器地图创建算法不同,Yuen等[42]提出了一种SMC SLAM(Se quentialM onte Carlo SLAM)算法,该算法全部采用粒子滤波器实现SLAM处理,其中一个粒子滤波器作为状态滤波器∋0估计机器人位姿,L个粒子滤波器作为参数滤波器∋1,∀,L估计环境障碍物.在实施过程中,算法采用Generic粒子滤波器对状态和参数滤波器进行估计,根据权值因子的累积函数分布进行采样,并且当有效粒子的数目低于某一阈值时,才执行重新采样处理.实验结果验证了方法的可行性,但是由于环境特征存在感知误差,地图创建部分的准确性还有待进一步提高.Kanto r等[43]提出将蒙特卡洛方法应用于基于主动路标的机器人系统定位和地图创建,此时机器人只能获取自身与路标之间的距离信息,而不包含任何状态与识别信息.由于机器人与主动路标之间无需可视路径,并且完全避免了数据关联问题,因此通过蒙特卡洛方法,无需精确放置信号塔,基于机器人系统的里程计信息和路标距离信息,即可高效实现移动机器人同步定位和地图创建.多机器人系统协作进行未知环境下的同步定位与地图创建是当前SLAM的重要研究方向之一, Rek leitis等[44]引入第二台机器人作为辅助从而减少机器人探索过程中移动机器人位姿的不确定性,采用粒子滤波器进行建模和减少累计里程计误差,实现精确大规模地图创建.Thrun等[41]将增进式地图创建和MCL的融合算法扩展至多个机器人平台,通过协作处理,成功生成关于环境的单一地图.多机器287第29卷第3期余洪山等: 基于粒子滤波器的移动机器人定位和地图创建研究进展。