第41卷第7期 浙江大学学报（工学版） Vol.41 No.7 2007年7月 Journal of Zhejiang University(Engineering Science) Jul.2007 基于MCMC粒子滤波的机器人定位 许士芳，谢立，刘济林 (浙江大学信息科学与工程学院，浙江杭州310027) 摘要：针对基于传统粒子滤波的机器人定位方法存在粒子退化的问题，提出了基于马尔科夫蒙特卡罗(MCMC) 粒子滤波的机器人定位方法.将传统的粒子滤波算法与典型的MCMC方法一Metropolis Hastings(MH)抽样算 法有机结合，并应用于机器人定位研究中.试验结果表明，MCMC方法可以有效抑制粒子退化问题.与基于传统粒 子滤波的机器人定位方法相比，该方法降低了定位误差均值和定位误差最大值，取得了更高的定位精度，有效地解 决了机器人定位这一非线性非高斯状态估计问题， 关键词：马尔科夫蒙特卡罗方法；粒子滤波；机器人定位：Metropolis Hastings抽样，粒子退化 中图分类号：TP242 文献标识码：A 文章编号：1008-973X(2007)07-1083-05 Robot localization based on MCMC particle filter XU Shi-fang,XIE li,LIU Ji-lin (College of Information Science and Engineering,Zhejiang University,Hangzhoy 310027,China) Abstract:Robot localization based on the Markov Chain Monte Carlo (MCMC)particle filter was proposed to solve the problem that robot localization based on the simple particle filter suffers from severe sample degeneracy.The standard MCMC method,Metropolis Hastings (MH)sampling,was incorporated into the filtering framework,and was applied to the robot localization problem.Experimental results showed that the MCMC particle filter can increase the sample variety and reduce sample degeneracy.Robot locali- zation based on the MCMC particle filter is much more accurate,compared with robot localization based on the simple particle filter. Key words:Markov Chain Monte Carlo (MCMC)method;particle filters;robot localization;Metropolis Hastings(MH)sampling;sample degeneracy 机器人定位是移动机器人研究的重要内容之粒子滤波(particle filter,.P℉)算法可以克服上述缺 一，它是一个非精确传感器信息序列融合的过程，也 陷，在处理非线性非高斯问题中显示了强大的生命 是一个非线性非高斯状态的在线估计过程). 力2]，目前粒子滤波算法在机器人定位中已获得了 针对非线性系统估计问题，常用的滤波方法包 成功应用1：4)，但在精确度方面仍有不足，粒子退化 括卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)和un- 是其主要原因之一， scented卡尔曼滤波(UKF)等.这些算法均要求观 粒子退化是指在重采样过程后，少数粒子被大 测噪声和过程噪声为独立不相关的高斯白噪声，但 量复制，而大多数粒子没有很多后代，使得粒子群丧 在实际情况下，观测噪声并不一定是高斯白噪声，因 失多样性的现象[.随着迭代次数增加，少数粒子的 此，上述算法在实际应用中均存在不同程度的缺陷.重要性权值趋近于1，而其他大多数粒子的重要性 收璃日期：2006-02-19. 浙江大学学报（工学版）网址：www,journals..ju.edu.cn/eng 基金项目：国家自然科学基金重点资助项目(60534070)，浙江省重点国际科技合作资助项目(2005C14008). 作者简介：许士芳(1982一)，女，山东德州人，博士生，主要从事机器人定位、视摄编解码、图像处理等方面的研究 E-mail:shifang_xu@yahoo.com.cn 通讯联系人：刘济林，男，教授，博导，E-mail:iul@zu,cdu.cn 万方数据
第41卷第7期 2007年7月 浙 江 大 学 学 报(工学版) Journal of Zhej iang University(Engineering Science) V01．41 No．7 Jul．2007 基于MCMC粒子滤波的机器人定位 许士芳，谢 立，刘济林 (浙江大学信息科学与工程学院，浙江杭州310027) 摘 要：针对基于传统粒子滤波的机器人定位方法存在粒子退化的问题，提出了基于马尔科夫蒙特卡罗(MCMC) 粒子滤波的机器人定位方法．将传统的粒子滤波算法与典型的MCMC方法——Metrop01is Hastings(MH)抽样算 法有机结合，并应用于机器人定位研究中．试验结果表明，MCMC方法可以有效抑制粒子退化问题．与基于传统粒 子滤波的机器人定位方法相比，该方法降低了定位误差均值和定位误差最大值，取得了更高的定位精度，有效地解 决了机器人定位这一非线性非高斯状态估计问题． 关键词：马尔科夫蒙特卡罗方法；粒子滤波；机器人定位；Metropolis Hastings抽样}粒子退化 中图分类号：TP242 文献标识码：A 文章编号：1008—973X(2007)07—1083—05 Robot localization based on MCMC particle filter xu Shi—fang，XIE li，LIU Ji—lin (College of Information Science and Engineering，Zhejiang University，Hangzhou．310027，China) Abstract：Robot localization based on the Markov Chain Monte Carlo(MCMC)particle filter was proposed to solve the problem that robot 10calization based on the simple particle filter suffers from severe sample degeneracy．The standard MCMC method，Metropolis Hastings(MH)sampling，was incorporated into the filtering framework，and was applied to the robot localization problem．Experimental results showed that the MCMC particle filter can increase the sample variety and reduce sample degeneracy．Robot locali— zation based on the MCMC particle filter is much more accurate，compared with robot localization based on the simple particle filter． Key words：Markov Chain Monte Carlo(MCMC)method；particle filters；robot localization；Metropolis Hastings(MH)sampling；sample degeneracy 机器人定位是移动机器人研究的重要内容之 一，它是一个非精确传感器信息序列融合的过程，也 是一个非线性非高斯状态的在线估计过程u J． 针对非线性系统估计问题，常用的滤波方法包 括卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)和un— scented卡尔曼滤波(UKF)等．这些算法均要求观 测噪声和过程噪声为独立不相关的高斯自噪声．但 在实际情况下，观测噪声并不一定是高斯白噪声．因 此，上述算法在实际应用中均存在不同程度的缺陷． 粒子滤波(particle filter，PF)算法可以克服上述缺 陷，在处理非线性非高斯问题中显示了强大的生命 力[2-3]．目前粒子滤波算法在机器人定位中已获得了 成功应用口’4‘7‘，但在精确度方面仍有不足，粒子退化 是其主要原因之一． 粒子退化是指在重采样过程后，少数粒子被大 量复制，而大多数粒子没有很多后代，使得粒子群丧 失多样性的现象[8]．随着迭代次数增加，少数粒子的 重要性权值趋近于1，而其他大多数粒子的重要性 收稿日期：2006—02—19． 浙江大学学报(工学版)网址：www．journals．zju．edu．cn／eng 基金项目：国家自然科学基金重点资助项目(60534070)；浙江省重点国际科技合作资助项目(2005C14008)． 作者简介：许士芳(1982一)，女，山东德州人，博士生，主要从事机器人定位、视频编解码、图像处理等方面的研究 E-mail：shifang_xu@yahoo．corn．cn 通讯联系人：刘济林，男，教授，博导．E-mail：liujt@zju．edu．cn 万方数据
1084 浙江大学 学报（工学版） 第41卷 权值减小为0.由于重采样过程偏好那些“合适的” w(o)=p(yxo)p(xo)/q(zo.y). (6) 粒子，少数重要性权值趋近于1的粒子被大量复制， 可以采用按照概率分布q(·|y)抽样得到的 而其他大多数粒子被抛弃，从而引起粒子退化问题。 粒子来近似表示E(g(xo,:),即 为解决粒子滤波的粒子退化问题，本文结合 E(g(xo)= MCMC方法，在传统的粒子滤波过程中添加MH ∑g,(x)，(x)/N/ 抽样步骤[8]，增加了粒子群的多样性，抑制了粒子 退化问题；把MCMC粒子滤波应用到机器人定位 [/N-立. 中，有效地提高了定位精确度. 式中：心，(x)是归一化后的重要性权值 对于马尔可夫过程，如果当前状态和未来的观 1 MCMC粒子滤波算法描述 测值无关，则使得重要性函数满足 q(zoy)=q(zo--1)q(-1),(8) 1.1蒙特卡罗方法 可以得到 蒙特卡罗方法的核心思想：按照后验概率分布 0,=0- p(y:x)p(x,|x-1) (9) 随机抽取若干个粒子，用这些离散粒子之和来近似 q(x,xot-1,y1） 式(9)给出了由t一1时刻重要性权值心-1更新得到 表示连续概率密度函数的值，即 t时刻重要性权值心，的方法. 1）-23ed/N (1) 以上给出的滤波方法有一个严重的局限性：粒 式中：p(x:|y:)是后验概率密度函数的近似值， 子重要性权值的方差随着时间的推移而增大，严重 (x68:i=1,…,N}是符合后验概率的随机抽样， 降低了算法精确度.重采样方法山可以有效解决这 6(·)是delta函数， 一问题，其主要思想是去除权值小的粒子，复制权值 蒙特卡罗方法可以用来近似随机变量函数的期 大的粒子.常用的重采样方法有采样重要性重采样 望值，随机函数g(xo)的期望值为 (sampling-importance resampling,SIR)、残差重采 样(residual resampling)和最小方差采样(minimum E(g.(xo))=g (xo)p(o yo.)dzo.(2) variance sampling)等[)， 任何形如式(2)的表达式都可近似表示为 1.3MCMC粒子滤波 Eg,万=之g,(/N. 重要性函数的选择对粒子滤波精确度有很大影 (3) 响.Kitagawa2证明了最优重要性函数为q(x, 式中：粒子x符合独立同分布.根据大数定理可以 xo-1y)=(x,xo-1,y1:).但采用最优重要性 得到Eg,a》二B(g,(》.当()的 函数时，需要计算p(x工o1y)的积分，计算复 杂度较大.Liu等人别提出可以用概率分布p(x,| 后验概率方差有界，即var)（g,(xo:)<∞时， 根据中心极限定理可得 x-1)近似p(x:xo:-1,少)，用p(x,x-1)作为重 Nw.[E(g.(x万-E(g(xo)门 要性函数，比p(x:xo4-1,y1)更容易进行抽样运 算.从应用角度出发4-刀，多数重要性函数都采用 N(O,var(g ())) (4) 次优算法，即q(x,|xo-1,y1)=p(x|x-1),这是 1.2粒子滤波基本算法 在滤波精确度和时间复杂度之间的折衷 由1.1节知，可用有限个粒子的和来近似表示 粒子滤波算法的可行性建立在下面2个假设的 后验概率分布，但有时很难直接按照后验概率来抽 基础之上[们. 样.为解决这一问题，可根据一个已知的、方便抽样 1)蒙特卡罗假设：能够用离散粒子之和 的函数q(xo|)来抽样.函数q(xo:|y1)被称为 重要性函数，且 ∑，0(dz)/N来足够精确地近似连续概率密 E(g,(xot)）= 度函数p(xo:|y1t): g.()p(zoy) (oy)do 2)重要性采样假设：能够按照某个合适的重要 性函数分布进行采样，并且能够保证重要性采样的 Eg ((o))/E(()).(5) 精确度 式中：E1)表示概率密度函数为g(x:h)时 如果上述2个条件有一个不满足，就会严重影 的函数期望值；，(x0,)为重要性权重，有 响粒子滤波的精确性 万方数据
浙 江 大 学 学 报(工学版) 第41卷 权值减小为0．由于重采样过程偏好那些“合适的” 粒子，少数重要性权值趋近于1的粒子被大量复制， 而其他大多数粒子被抛弃，从而引起粒子退化问题． 为解决粒子滤波的粒子退化问题，本文结合 MCMC方法，在传统的粒子滤波过程中添加MH 抽样步骤[8。1 0|，增加了粒子群的多样性，抑制了粒子 退化问题；把MCMC粒子滤波应用到机器人定位 中，有效地提高了定位精确度． MCMC粒子滤波算法描述 1．1蒙特卡罗方法 蒙特卡罗方法的核心思想：按照后验概率分布 随机抽取若干个粒子，用这些离散粒子之和来近似 表示连续概率密度函数的值，即、 。 N 5(x呲I 呲 Yo：t)一∑峨(dx。)／NP yo(dxo／N． )一∑峨： 。 ． (1) ．i一1 ： 式中：多(z。：。I Y。：。)是后验概率密度函数的近似值， {z撼；i=1，…，N)是符合后验概率的随机抽样， 艿(·)是delta函数． 蒙特卡罗方法可以用来近似随机变量函数的期 望值，随机函数戤(z吣)的期望值为 r E(g。(zo：。))一l g。(Xo：，p(xo：。I yo：。)dx蚴．(2) J 任何形如式(2)的表达式都可近似表示为 硬瓦丽一∑g。(z鼢)／N． (3) l=1 式中：粒子z挖符合独立同分布．根据大数定理可以 得到瓦丽≮≥E(g。(‰。))．当g。(‰。)的 后验概率方差有界，即var小l，。)(gr(z。：t))<Cx3时， 根据中心极限定理可得 N“2·[E(gt瓦一O：t万E(g；(z呲))]净 N(O，varp(．忆)(g。(z㈨)))． (4) 1．2粒子滤波基本算法 由1．1节知，可用有限个粒子的和来近似表示 后验概率分布，但有时很难直接按照后验概率来抽 样．为解决这一问题，可根据一个已知的、方便抽样 的函数q(x。：。Iyb)来抽样．函数q(x。：。Iy¨)被称为 重要性函数，且 E(g。(Xo：。))一 Jg。(z。：z)弓糍q(z。：t y，：c)d_。：z= E(．Yl：t)(岱(Xo：t)砜(Xo：t))／E(Ⅵ／≯(觋(Xo：t))． (5) 式中：E扣}扎，，表示概率密度函数为q(z㈨l y¨)时 的函数期望值；W。(z。：。)为重要性权重，有 W。(zo：。)一夕(y1：。fz。：。)p(z。：。)／q(x。：。fyl：。)． (6) 可以采用按照概率分布q(·1 y㈨)抽样得到的 粒子来近似表示E(g。(z吣))，即 E(g：(z。))一 [妻“《：)Wt(X撼)／N]／ l∑g。(z撼 ∞／N I／ P N 1 N l∑W。(z黝／N l一∑g：(《：)面。(z黝．(7) 式中：W～。(z搀)是归一化后的重要性权值． 对于马尔可夫过程，如果当前状态和未来的观 测值无关，则使得重要性函数满足 q(xo：，J Yl：；)一g(zo：，一1 y】：，一1)g(z。J zo：：一】，Y1：，)，(8) 可以得到 P(y：lz。)p(z。lz。一。) q(xf lzo：川，Y¨)‘ (9) 式(9)给出了由￡一1时刻重要性权值WH更新得到 t时刻重要性权值W。的方法． 以上给出的滤波方法有一个严重的局限性：粒 子重要性权值的方差随着时间的推移而增大，严重 降低了算法精确度．重采样方法[1u可以有效解决这 一问题，其主要思想是去除权值小的粒子，复制权值 大的粒子．常用的重采样方法有采样重要性重采样 (sampling—importance resampling，SIR)、残差重采 样(residual resampling)和最小方差采样(minimum variance sampling)等o81． 1．3 MCMC粒子滤波 重要性函数的选择对粒子滤波精确度有很大影 响．Kitagawa[121证明了最优重要性函数为q(z。} zot一-，y¨)一P(五t X喊一，，Y¨)．但采用最优重要性 函数时，需要计算P(z。J z㈦一，，Y¨)的积分，计算复 杂度较大．Liu等人[1胡提出可以用概率分布P(z。j zH)近似P(z：l z。，，，y¨)，用P(z。J zH)作为重 要性函数，比乡(z。J z。，，，Y功)更容易进行抽样运 算．从应用角度出发[4_7]，多数重要性函数都采用 次优算法，即q(黾l z。，。，y¨)一P(z。l zH)，这是 在滤波精确度和时间复杂度之间的折衷． 粒子滤波算法的可行性建立在下面2个假设的 基础之上L6J． 1)蒙特卡罗假设：能够用离散粒子之和 ∑：：。疋：：：(dx0：。)／N来足够精确地近似连续概率密 度函数P(z吣lyi：。)； 2)重要性采样假设：能够按照某个合适的重要 性函数分布进行采样，并且能够保证重要性采样的 精确度． 如果上述2个条件有一个不满足，就会严重影 响粒子滤波的精确性． 万方数据
第7期 许士芳，等：基于MCMC粒子滤波的机器人定位 1085 在t时刻经过重采样后，得到了用来表示后验 响应信号的内容由特征点的D惟一确定，通过计算 概率分布p(xo|y1)的N个粒子.由于重采样过程 请求信号发出时间和响应信号接受时间之差，得到 偏好那些“合适的”粒子，使得粒子滤波能够跟踪时 机器人和对应特征点之间的距离. 变系统的后验概率分布，但这也会使某些粒子没有 用机器人的笛卡儿坐标和航向角表示它的状态 后代，而另外一些粒子有很多后代.最糟糕的情况是 向量.在t时刻的状态向量记为X,=[x,y,0],其 某一粒子的后代个数为N,即重采样后的所有粒子 中(x,y:)为机器人在笛卡儿坐标系中的位置，8，为 都是某一粒子的后代，这就会引起严重的粒子退化， 机器人的航向角.本次试验中的非线性状态方程为 影响滤波精度，因此，需要采取措施，使得在不影响 Tx,+△D,cos6,7 粒子群概率分布的前提下增加粒子群的多样性，从 X+1= y,+△D,sina, 十wa=f(X,,4)十w,(10) 而解决粒子退化问题。 9.+△0 针对上述粒子退化问题，一个有效的解决方法 式中：w:是过程噪声：△D,是由里程表测得的在t和 是对每个粒子引入马尔可夫蒙特卡罗(MCMC)方 t+1时刻之间的行驶路程：△是由陀螺仪测得的 法[们.如果粒子服从后验概率p(xo:|y1t),那么实 航向角的变化；4，为t时刻的控制变量，，=[△D, 施核为K(xo|xo)的马尔科夫链变换之后，在保证 △0]T. K(xo4|xo)p(xot|y1)=p(x0ty:)的前提 在t时刻，机器人和特征点(x6,y)之间距离的 测量方程为 下，仍然可以得到一组满足既定后验概率分布的粒 子群，而且这组新的粒子群可能移动到了状态空间 h(X,[x,h]T)=√(x-x)2+(y-)+.(11) 中更为有利的位置.上述马尔科夫变换不影响粒子 式中：，为测量噪声，在定位之前由试验统计得到 群的概率分布，利用变换后的粒子群近似后验概率 ,的概率分布模型. 密度函数，变换后的近似误差不会大于变换前，因 2.2定位方法 此，可以结合任何标准的MCMC方法（如Gibbs抽 本文利用MCMC粒子滤波对机器人进行定 样、MH抽样等)来提高粒子群的多样性.MCMC方 位，具体算法如下： 法也可以被看作在有限的混合分布∑，K(红：| 1)初始化：按照初始状态概率分布函数p(x。) 在状态空间中随机抽取N个粒子，得到{X°；=1， x)/N中进行抽样.Gilks等人ao]证明了这种MC …,N}. MC抽样方法的收敛性. 2)对于t=1,2,…,循环操作以下5个步骤，直 在粒子滤波中引人MH抽样的具体过程如下： 到测量结束 1)按照均匀概率分布从区间[0,1]中抽样得到 (1)运动估计：对于集合{X”1:i=1,…,N}中的 门限值u,即u~Uo.1. 每个粒子，根据里程表和陀螺仪的数据4-：，按照重 2)按照概率p(x|x巴)抽样得到x,0,即 要性函数q(X|X-1,h1)=p(X|X-)抽样，从而 x,一p(xx1). 得到运动估计后的粒子集合{；i=1,…,N), 3)如果u<min{1,p(y.xo)/p(y:lx)},那 (2)粒子赋权：根据贝叶斯准则，按照测量数据 么就接受xn,即x=x;否则丢弃x0,保留 h,对运动估计后的X集合中的每个粒子赋予重 重采样的粒子0，即x=. 要性权值w=p(h),并归一化为 2机器人定位试验 0=w0/∑w”, (12) 从而得到对当前时刻位置估计的粒子集合S= 2.1问题描述 {(,0)}. 本文使用文献[7]中所提到的数据.机器人上安 (3)权重抽样：从S中按照重要性权值对粒子进 装有里程表、陀螺仪和射频收发器，里程表和陀螺仪 行抽样，记抽样后所得粒子集合为X;i=1,,N}. 分别用来测量机器人的行驶路程和航向角的变化： (4)利用MCMC方法进行抽样：模拟马尔可夫 射频收发器用来接受环境中固定特征点处射频收发 蒙特卡罗过程，对于权重抽样后所得集合中每个粒 器的信号，每个特征点都有一个惟一的编号(ID). 子再进行MH轴样，记MH抽样后得到的粒子集合 在机器人移动过程中，不断发出射频请求信号，在其 为{X”;i=1,…,N},此时把每个粒子的重要性权 有效距离内的特征点都会响应这个请求信号，每个 重都赋值为1/N,即0=1/N;i=1,…,N. 万方数据
第7期 许士芳，等：基于MCMC粒子滤波的机器人定位 在t时刻经过重采样后，得到了用来表示后验 概率分布P(z。：。lY¨)的N个粒子．由于重采样过程 偏好那些“合适的”粒子，使得粒子滤波能够跟踪时 变系统的后验概率分布．但这也会使某些粒子没有 后代，而另外一些粒子有很多后代．最糟糕的情况是 某一粒子的后代个数为N，即重采样后的所有粒子 都是某一粒子的后代，这就会引起严重的粒子退化， 影响滤波精度．因此，需要采取措施，使得在不影响 粒子群概率分布的前提下增加粒子群的多样性，从 而解决粒子退化问题． 针对上述粒子退化问题，一个有效的解决方法 是对每个粒子引入马尔可夫蒙特卡罗(MCMC)方 法[8]．如果粒子服从后验概率P(z㈦I Y㈨)，那么实 施核为K(x。：。Iz吣)的马尔科夫链变换之后，在保证 r ． K(x。：。1 z。：。)夕(zo：。l Y1：。)一p(xo：。l Yl：。)的前提 J 下，仍然可以得到一组满足既定后验概率分布的粒 子群，而且这组新的粒子群可能移动到了状态空间 中更为有利的位置．上述马尔科夫变换不影响粒子 群的概率分布，利用变换后的粒子群近似后验概率 密度函数，变换后的近似误差不会大于变换前．因 此，可以结合任何标准的MCMC方法(如Gibbs抽 样、MH抽样等)来提高粒子群的多样性．MCMC方 法也可以被看作在有限的混合分布≥：2，K(x吣I z把)／N中进行抽样．Gilks等人[1叨证明了这种MC— MC抽样方法的收敛性． 在粒子滤波中引入MH抽样的具体过程如下： 1)按照均匀概率分布从区间[o，1]中抽样得到 门限值“，即M～己，『o，1]． 2)按照概率P(五f z霉。)抽样得到z。“”，即 Xt““--p(x。I z罂1)． 3)如果u}． (3)权重抽样：从S一中按照重要性权值对粒子进 行抽样，记抽样后所得粒子集合为{x≯；i一1，…，N)． (4)利用MCMC方法进行抽样：模拟马尔可夫 蒙特卡罗过程，对于权重抽样后所得集合中每个粒 子再进行MH抽样，记MH抽样后得到的粒子集合 为{墨”；i一1，…，N)，此时把每个粒子的重要性权 重都赋值为1／N，即训；。一1／N；i一1，…，N． 万方数据
1086 浙江大学学 报（工学版） 第41卷 (5)输出：t时刻的状态估计为 表1是分别利用传统粒子滤波定位方法和本文提出 x=∑w“xn, (13) 的MCMC粒子滤波定位方法所得结果的误差.其 t时刻的方差估计为 中E和Em分别表示垂直于路径方向的误差均值 B,=∑w(x0-X,)r(x0-x).(14 和误差最大值，E和Em分别表示平行于路径方向 的误差均值和误差最大值.由表1可知，本文提出的 2.3试验结果 MCMC粒子滤波定位方法与传统粒子滤波算法相 本文实验结果如图1所示.图中菱形表示环境 比，误差有所降低，取得了更高的定位精度， 中的特征点，实线表示估计路径，虚线表示真实路 径，通过里程表和陀螺仪得到惯导信息，结合射频收 表1试验结果比较 发器所测得的距离信息，利用本文提出的MCMC Tab.1 Comparison of experimental results m 粒子滤波定位方法估计得到机器人路径.真实路径 粒子滤波类型E Ep. Eem 传统粒子滤波 0.6353.5261.8845.197 利用精度为2cm的全球定位系统(GPS)信号得到. MCMC粒子滤波 0.5443.5211.7335.056 本次试验设置粒子数为1000，即用1000个粒子的 加权和来估计机器人的当前位置， 3 图2(a)和(b)分别为采用传统粒子滤波定位方 结语 法和MCMC粒子滤波定位方法时某时刻各粒子在 本文提出的MCMC粒子滤波定位方法有效解 机器人环境坐标中的分布情况.其中实线代表机器 人路径，加号表示机器人的真实位置，空心圆圈表示 决了机器人定位这一非线性非高斯状态估计问题. 估计位置，实心点表示1000个粒子的位置.图2(a) 和传统的粒子滤波算法相比，MCMC粒子滤波有效 抑制了粒子退化问题.试验结果表明本文提出的定 和(b)中估计位置和真实位置之间的距离分别为 0.773和0.501m.图2(a)中粒子分布非常集中，粒 位方法提高了机器人定位的精确度， 与其他滤波方法（例如卡尔曼滤波等）相比，粒子 子群丧失了多样性，粒子退化严重，估计误差较大； 图2(b)中粒子群分布相对分散，和图2(a)相比粒子 滤波算法的计算复杂度较大.在本文工作的基础上， 退化问题得到了抑制，有利于粒子群移向机器人的 通过若干改进，如通过动态更新粒子集合的粒子数 真实位置，估计误差较小. 目，可以降低算法的计算复杂度.如何进一步提高粒 比较估计路径和真实路径，得到垂直于路径方 子滤波算法的实时处理能力是今后需要开展的工作， 向和平行于路径方向的误差平均值和误差最大值. 参考文献(References): [1]THRN S,FOX D,BURGARD W.et al.Robust Monte 10 Carlo localization for mobile robots J.Artificial Intel- ligence,2001,128(1/2):99-114. [2]DOUCET A,FREITAS N,GORDON N.Sequential 10 Monte Carlo methods in practice [M].New York: -15 Springer,2001. 215-10-50510 x/m [3]DOUCET A,GODSILL S,ANDRIEU C.On sequential 图1MCMC粒子滤波机器人定位结果 Monte Carlo sampling methods for Bayesian filtering Fig.1 Results of MCMC particle filter location [J].State Compute,2000,10(3):197-208. -9.5r [4]FOX D.KLD sampling:adaptive particle filters [J].In- -9.5m -10.0 -10.0 ternational Journal of Robotics Research,2003.22 (12): -10.5 -10.5 985-1003. -11.0 -11.0 [5]KWOK C,FOX D,MEILA M.Adaptive real-time par- -11. -11. 3.54.04.55.0 3.54.04.55,0 ticle filters for robot localization [C/Proceedings of x/m x/m (a)传统粒子滤波 IEEE International Conference on Robotics and Automa- (b)MCMC粒子滤波 tion.Taipei,China:IEEE,2003:2836-2841. 图2粒子群分布 [6]REKLEITIS I M.Cooperative localization and multi-ro- Fig.2 Particles distribution bot exploration [D.Montreal,Canada:McGill Univer- 万方数据
浙 江 大 学 学 报(工学版) 第41卷 (5)输出：t时刻的状态估计为 五=∑兰，硼凇％ (13) t时刻的方差估计为 P。一≥：2，硼∥(x≯一置)T(x；。一五)．(14) 2．3试验结果 本文实验结果如图1所示．图中菱形表示环境 中的特征点，实线表示估计路径，虚线表示真实路 径．通过里程表和陀螺仪得到惯导信息，结合射频收 发器所测得的距离信息，利用本文提出的MCMC 粒子滤波定位方法估计得到机器人路径．真实路径 利用精度为2 am的全球定位系统(GPS)信号得到． 本次试验设置粒子数为1 000，即用1 000个粒子的 加权和来估计机器人的当前位置． 图2(a)和(b)分别为采用传统粒子滤波定位方 法和MCMC粒子滤波定位方法时某时刻各粒子在 机器人环境坐标中的分布情况．其中实线代表机器 人路径，加号表示机器人的真实位置，空心圆圈表示 估计位置，实心点表示1 000个粒子的位置．图2(a) 和(b)中估计位置和真实位置之间的距离分别为 0．773和0．501 In．图2(a)中粒子分布非常集中，粒 子群丧失了多样性，粒子退化严重，估计误差较大； 图2(b)中粒子群分布相对分散，和图2(a)相比粒子 退化问题得到了抑制，有利于粒子群移向机器人的 真实位置，估计误差较小． 比较估计路径和真实路径，得到垂直于路径方 向和平行于路径方向的误差平均值和误差最大值． 图1 MCMC粒子滤波机器人定位结果 Fig．1 Results of MCMC particle filter location x／m (a)传统粒子滤波 x／m (b)MCMC粒子滤波 图2粒子群分布 Fig．2 Particles distribution 表1是分别利用传统粒子滤波定位方法和本文提出 的MCMC粒子滤波定位方法所得结果的误差．其 中E，。和E。分别表示垂直于路径方向的误差均值 和误差最大值，E。。和E。。分别表示平行于路径方向 的误差均值和误差最大值．由表1可知，本文提出的 MCMC粒子滤波定位方法与传统粒子滤波算法相 比，误差有所降低，取得了更高的定位精度． 表1试验结果比较 Tab．1 Comparison of experimental results rn 粒子滤波类型 Ev。 E，。 E，。 E。。 传统粒子滤波0．635 3．526 1．884 5．197 MCMC粒子滤波0．544 3．521 1．733 5．056 3 结语 本文提出的MCMC粒子滤波定位方法有效解 决了机器人定位这一非线性非高斯状态估计问题． 和传统的粒子滤波算法相比，MCMC粒子滤波有效 抑制了粒子退化问题．试验结果表明本文提出的定 位方法提高了机器人定位的精确度． 与其他滤波方法(例如卡尔曼滤波等)相比，粒子 滤波算法的计算复杂度较大．在本文工作的基础上， 通过若干改进，如通过动态更新粒子集合的粒子数 目，可以降低算法的计算复杂度．如何进一步提高粒 子滤波算法的实时处理能力是今后需要开展的工作． 参考文献(References)： [1]THRN S，FOX D，BURGARD W，et a1．Robust Monte Carlo localization for mobile robots[J]．Artificial Intel— ligence，2001，128(1／2)：99—114． [2]DOUCET A，FREITAS N，GORDON N．Sequential Monte Carlo methods in practice[M]．New York： Springer，2001． [3]DOUCET A，GODSILL S，ANDRIEU C．On sequential Monte Carlo sampling methods for Bayesian filtering EJ]．State Compute，2000，10(3)：197—208． [4]FOX D．KLD sampling：adaptive particle filters[J]．In— ternational Journal of Robotics Research，2003，22(12)： 985—1003． E5-1 KWOK C，FOX D，MEILA M．Adaptive real—time par— ticle filters for robot localization[C]∥Proceedings of IEEE International Conference on Robotics and Automa— tion．Taipei，China：IEEE，2003：2836—2841． [6]REKLEITIS I M．Cooperative localization and multi—ro— bot exploration[D]．Montreal，Canada：McGill Univer— 万方数据
第7期 许士芳，等：基于MCMC粒子滤波的机器人定位 1087 sity,2003. dynamic Bayesian models [C]//Medical Research Coun- [7]DEREK K.Range only robot localization and SLAM cil.Cambridge,UK:[s.n.,1998. with radio [D].Pittsburgh:Carnegie Mellon Universi- [11]GORDON N J,SALOMND DJ,SMITH A F M.No- ty,2004. vel approach to nonlinear and non-Gaussian Bayesian [8]VAN DER MERWE R,DOUCET A,DE FREITAS N, state estimation [J.IEEE Proceedings on Radar and et al.The unscented particle filter [R.Cambridge. Signal Processing,1993,140(2):107-113. UK:Cambridge University,2000. [12]KITAGAWA G.Monte Carlo filter and smoother for non [9]ANDRIEU C,FREITAS N,DOUCET A.Sequential Gaussian nonlinear state space models []Journal of MCMC for Bayesian model selection [C]/IEEE Higher Computational and Graphical Statistics,1996,5(1):1-25. Order Statistics Workshop.Ceasarea,Israel:IEEE, [13]LIU J S,CHEN R.Sequential Monte Carlo methods 1999:130-134. for dynamic systems [J].Journal of the American Sta- [10]GILKS W R,BERZUINI C.Monte Carlo inference for tistical Association,1998,93(443):1032-1044. (上接第1082页) 保持了较高的时钟频率和系统性能，又获得了较好 -14. 的应用适应性.通过在CK520仿真开发平台上定量 [5]潘国振，王界兵，严晓液.高性能嵌人式CPU特殊指令 分析TLB的失效率并对面积和速度各指标进行折 单元的设计与实现[J门.浙江大学学报：工学版，2005， 衷，定制设计的多级TLB结构MMU系统性能基 39(2):211-215. 准测试结果达到单级全相联设计水平，物理实现中 PAN Guo-zhen,WANG Jie-bing,YAN Xiao-lang.De- 集成多级TLB结构MMU的CK520处理器时钟颜 sign and implementation of special instruetions unit in 率达到230MHz以上，面积仅增加7.6%. high performance embedded CPU [J].Journal of Zhe- jiang University:Engineering Science,2005,39(2):211 参考文献(References): -215. [1]AUSTIN T M,SOHI G S.High-bandwidth address [6]张字宏，王界兵，严晓浪，等.标志预访问和组选择历史 translation for multiple-issue processors [C]//Proceed- 相结合的低功耗指令cache[J刀.电子学报，2004,32 ings of the 23rd Annual International Symposium on (8):1287-1290. ComputerArchitecture.Philadelphia,US:[s.n.], ZHANG Yu-hong,WANG Jie-bing,YAN Xiao-lang, 1996:158-167. et al.Pre-visiting tag and keeping way history to reduce [2]Tensilica Inc.Xtens microprocessor overview hand- power in instruction cache [J].Chinese Journal of Elec- book:a summary of the Xtens microprocessor data tronics,2004,32(8):1287-1290. book [EB/OL].[2006-01-15].http://www.tensilica. [7]HENNESSY J L,PATTERSON D A.Computer archi- com/products/white_papers.htm tecture:a quantitative approach [M].3rd ed.San Fran- [3]MANNE S,KLAUSTER A,GRUNWALD D,et al.Low cisco:Morgan Kaufmann,2002. power TLB design for high performance microprocessors [8]JACOB B L,MUDGE T N.A look at several memory [R].Colorado.US:University of Colorado,1997. management units,TLB refill mechanisms and page ta- [4]GUTHAUS M,RINGENBERG J,ERNST D,et al. ble organizations [C]/ACM the 8th International Con- MiBench:a free commercially representative embedded ference on Architectural Support for Programming Lan- benchmark suite [C]//IEEE 4th Annual Workshop on guages and Operating Systems.San Jose,US:ACM, Workload Characterization.Austin,US:IEEE,2001:3 1998:295-306. 万方数据
第7期 许士芳，等：基于MCMC粒子滤波的机器人定位 1087 slty，2003． E7]DEREK K．Range only robot localization and SLAM with radio I-D]．Pittsburgh：Carnegie Mellon Universi— ty，2004． [8]VAN DER MERWE R，DOUCET A，DE FREITAS N， et a1．The unscented particle filter[R]．Cambridge， UK：Cambridge University，2000． r9]ANDRIEU C，FREITAS N，DOUCET A．Sequential MCMC for Bayesian model selection[c]∥IEEE Higher Order Statistics Workshop．Ceasarea，Israel：IEEE， 1999：130—134． [10]GILKS W R，BERZUINI C．Monte Carlo inference for dynamic Bayesian models[c]∥Medical Research Council．Cambridge，UK：Is．n．]，1998． [11]GORDON N J，SALOMND DJ，SMITH A F M．No— vel approach tO nonlinear and non～Gaussian Bayesian state estimation I-J]．IEEE Proceedings on Radar and Signal Processing，1993，140(2)：107—113． [12]KITAGAWA G．Monte Carlo filter and smoother for nonGaussian nonlinear state space models[J]．Journal of Computational and Graphical Statistics，1996，5(1)：1—25． [13]LIU J S，CHEN R．Sequential Monte Carlo methods for dynamic systems[J]．Journal of the American Sta— tistical Association，1998。93(443)：1032—1044． ◆¨_◆Ⅲ◆_lI¨●-l～●-¨’◆¨◆¨◆1¨_◆¨l一●-l"●，¨◆㈣◆·1m◆¨_◆”◆¨◆-I。◆·H¨◆m●一‘¨_◆Ⅲ◆¨◆¨_●IJ-◆一{¨_◆¨_◆，1¨_●{r|◆…h◆u¨◆¨◆¨_●¨●¨_●m●-…◆·_I (上接第1082页) 保持了较高的时钟频率和系统性能，又获得了较好 的应用适应性．通过在CK520仿真开发平台上定量 分析TLB的失效率并对面积和速度各指标进行折 衷，定制设计的多级TLB结构MMU系统性能基 准测试结果达到单级全相联设计水平．物理实现中 集成多级TLB结构MMU的CK520处理器时钟频 率达到230 MHz以上，面积仅增加7．6％． 参考文献(References)： [1-1 AUSTIN T M，SOHI G S．High—bandwidth address translation for multiple-issue processors[C]∥Proceed— ings of the 23rd Annual International Symposium on ComputerArchitecture．Philadelphia， US： Is．n．]， 1996：158—167． [2-1 Tensilica Inc．Xtensa@microprocessor overview hand～ book：a summary of the Xtensa④microprocessor data book[EB／OL]．[2006一01一is]．http t}j ww哪．tensilica． corn／products／white——papers．htm [3]MANNE S，KLAUSTER A，GRUNWALD D，et a1．Low power TLB design for high performance microprocessors [R]．Colorado，US：University of Colorado，1997． [4]GUTHAUS M，RINGENBERG J，ERNST D，et a1． MiBench：a free commercially representative embedded benchmark suite Ec]}{IEEE 4th Annual Workshop on Workload Characterization．Austin，US：IEEE，2001：3 一14． Is]潘国振，王界兵，严晓浪．高性能嵌入式CPU特殊指令 单元的设计与实现[J]．浙江大学学报：工学版，2005， 39(2)：211—215． PAN Guo～zhen，WANG Jie—bing，YAN Xiao—lang．De— sign and implementation of special instructions unit in high performance embedded CPU[J]．Journal of Zhe· jiang University：Engineering Science，2005，39(2)：211 —215． F6]张宇宏，王界兵，严晓浪，等．标志预访问和组选择历史 相结合的低功耗指令cache[J]．电子学报，2004，32 (8)：1287—1290． ZHANG Yu—hong，WANG Jie-bing，YAN Xiao—lang， et a1．Pre—visiting tag and keeping way history tO reduce power in instruction cache[J]．Chinese Journal of Electronics，2004，32(8)：1287—1290． [7]HENNESSY J L，PATTERSON D A．Computer archi— tecture：a quantitative approach[M]．3rd ed．San Fran— cisco：Morgan Kaufmann，2002． [8]JACOB B L，MUDGE T N．A look at several memory management units，TLB refill mechanisms and page ta— ble organizations Ec]ff ACM the 8th International Con· ferenee on Architectural Support for Programming Lan‘ guages and Operating Systems．San Jose，US：ACM， 1998：295—306． 万方数据