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

1、第 2 9卷第 3期 2 0 0 7 年5 月 机器人R O B O T V o 1 2 9 N o 3 Ma y，2 0 07 文章编号：1 2-0 4 4 6(2 0 0 7)0 3-0 2 8 1-0 9 基于粒子滤波器的移动机器人定位和地图创建研究进展 余 洪 山，王耀 南(湖南大学 电气与信息工程学院，湖南 长沙4 1 0 0 8 2)摘要：首先，对粒子滤波器 的原理 和研究进展进行 了综述 然后，介绍 了基于粒 子滤波 器的 移动机器 人定位 研究进展 其次，给出 了粒子滤波器在移动机器人地 图创建领域的最新成果 最后，对粒 子滤波 器在 移动机器人 研究 领域的未来发展方向进行了

2、展望 关键词：粒子滤波器；蒙特卡洛定位；移动机器人地图创建；移动机器人定位；移动机器人同步地图创建和定 位 中图分类 号：T P 2 4 文献标识码：A A Re v i e w o n M o b i l e Rob o t Lo c a l i z a t i o n a n d M a p b ui l d i n g Al g o r i t h m s Ba s e d o n Pa r t i c l e Fi l t e r s YU Ho n g-s h a n，W ANG Ya o n a n (C o l l e g e ofE l e c t r ic a l a n d

3、 I n f o r ma t i o n E n g i n e e r i n g，H u n a n U n i v e r s i t y，C h a n g s h a 4 1 0 0 8 2，C h i n a)Abs t r a c t：Fi r s t l y，t h e r e s e a r c h pr o g r e s s a n d princ i p l e o f pa r t i c l e f i l t e rs a r e o v e r v i e we dS e c o n dl y，t h e p r o gre s s o f mo b i l

4、e rob o t l o c a l i z a t i o n b a s e d o n p a r t i c l e fi l t e r s i s d e s c ri b e d T h i r d l y，t h e r e c e n t wo r k s o f p a r t i c l e fi l t e rs i n ma p b u i l d i n g f o r no b i l e r o b o t s are p r e s e n t e d F i n all y，t h e f u t u r e d i r e c t i o n s o f

5、 p a rti c l e fi l t e rs i n mo b i l e r o b o t are s u mma r i z e d Ke ywo r ds：p a r t i c l e fil t e r；Mo nt e Ca r l o l o c ali z a t i o n；mo b i l e r o b o t ma p b u i l di ng；mo b i l e r o b ot l o c ali z a t i o n；S LAM 1 引言(I n t r o d u c t i o n)粒子滤波 器(p a r t i c l e f i l t e

6、 r)是一 种基本 统计工 具，其核心是基于贝叶斯 采样 估计 的顺序重要采样(S e q u e n t i a l I mp o rt a n c e S a mp l i n g，S I S)滤波思想，通常 也称之为 B o o t s t r a p滤波器、蒙特 卡洛滤波器、C o n d e n s a t i o n算法和 S u r v i v a l o f t h e F i t t e s t 算法，开始成功应 用于 目标跟踪、语音识别、移动机器人定 位、地 图创 建、故障诊断、统计分析等领域 粒子滤波器具有 可逼近任意概率分 布 的特性，并且计算简单方便，与传统卡尔曼滤波

7、器方法、马尔 可夫算法相 比，具有其特定 的优越性 D e l l a e rt等 和 F o x等。分别独立提出将粒子滤波器应用 于移动机 器人定位研究 中，即蒙特卡洛定位算法(M o n t e C a r l o L o c a l i z a t i o n，M C L)此后算法被研究人员广泛采用和 扩展，迅速成为继 E K F模型、马尔可夫模型后移动机 器人定位领域的一个新的研究热点 在此基础上，研究人员将粒子滤波器引入地 图创建研究，提 出了 一系列 移 动机 器人 同步地 图创建 和 定位 方 案，如 F a s t S L A M算 法，B 、粒子滤波器和其他智能计算方 法 的

8、复合地图创建方法等，得到 了移动机器人 地图 创建研究人员 的广泛认 可 本 文拟对粒子 滤波器在 移动机器人定位、地图创建 等应用领域 的最新研究 进行综述，分析和总结该类算法 的优缺点 和可能研 究方向 2 粒 子 滤 波器 原理 和 研 究进 展(T h e r e-s e a r c h pr o g r e s s a nd p r i nc i p l e o f p a r t i c l e fi l t e r s)粒子滤波器的研究源于 H a m m e r s l e y 等 提出的 基本 S I S方法 1 9 9 3年 G o r d o n等 提出了一种新的 基于

9、S I S的 B o o t s t r a p 非线性滤波方法，奠定了粒子滤 基金项 目：国家 自然科学基金 资助项 目(6 0 3 7 5 0 0 8)；教育部博士点基金 资助项 目(2 0 0 3 0 5 3 2 0 0 4)；湖南 大学优 秀博士论文 创新基金 资助 项 目(5 2 1 2 1 8 0 0 6)收稿 日期：2 0 0 6 0 70 3 维普资讯 http:/ 2 8 2 机器人 2 0 0 7年 5月 波算 法的基 础，随后粒子滤波器的研 究取得 了迅速 发展，代表性的如 L i u等 提 出的连续 重要性 采样 方法、K i t a g a w a等 提 出的蒙特卡洛

10、滤波器和平滑 器方法、I s a r d等 提出的 C o n d e n s a t io n算法、C r i s a n 等 提出的连续时间滤波器方法等 上述研究为粒 子滤波器算法提供 了坚实的理论基础和研究框架，并使粒子滤波器的研究逐步走向应用 2 1 粒子滤波器原理及关键技术 如图 1所示，粒子滤波器通过 粒子集和粒子 对 应权值组成的随机采样数据集合 S(k)表示相应的概 率分布P()，以有限样本点的求和运算取代积分 运算，从而获得状态最小方差估计 用数学语言描述 如下：对平稳随机过程，假定 七一1时刻系统的后验概 率密度为 P(一。)，依据一定原则选取 n个随机 样本点，k时刻获得

11、测量信 息后，经过状态和时间更 新过程，n个粒子的后验概率密度可近似为 P(J：)随着粒子数 目的增加，粒子的概率密度 函数逐 渐逼近状态 的概率密度 函数，粒子滤波估计 即达到 最优贝叶斯估计的效果 粒子滤 波算法摆脱 了解 决非线性滤波问题时随机量必须满足高斯分布的制 约条件，并在一定程度上解决了粒子数匮乏问题，因 此近年来该算法在许多领域得到成功应用 图 1 粒子滤波器算法单次迭代处理对应的概率密度和粒子集 F i g 1 T h e p r o b a b i l i t y d e n s i t i e s a n d p a r t i c l e s e t s f o r o

12、 n e i t e r a t i o n o f t h e p a r t i c l e fi l t e r s a l g o r i t h m 假设通过 次迭代处理，采样集合 s(k)可精确 逼近实际概率分布 在 每个时刻 t，定 义随机测量数 据 ，：。，其 中 表示时刻 n的第 m个粒 子，为相应粒子的权值，：是信号的第 m个采 样轨迹 如果这些粒子集均根据观测量 和基于概 率分布p(：f。：)的采样轨迹而获取，则基于式(1)近似相应的概率分布：M|P(I Z l：n)一 6(一 ：)(1)粒子滤波器包括三部分：1)生成粒子集(采样步 骤)；2)粒子权值计算(重要性步骤)；

13、3)重采样 2 1 1 生成粒子集 粒子集 是根据如式(2)所示 的重要性概率 密度函数 7 ()提取生成的，通过迭代处理可计算得 到粒子的权值，如式(3)所示 一 仃(：)=仃(。)兀仃(。：一 ，。：)(2)7 (f，。：)(3)重要性概率分布7 r(I，。：)在粒子滤波器 设计 中扮演着非常重要角色，因为它负责生成表示 期望概率密度的粒子 集 如果提取的粒子集是在概 率密度 较小 的区域 内，则根据粒 子集 和相关权值获 得的估计值也会很小，则对信号的后续跟踪处理可 能会发散 反之，如果在概率 密度非常高的区域提取 粒子集，则粒子滤波器的性能会大大增强 有人提出p(。，)是最优重要性 函

14、数，但 缺陷在于难 以采样 和对粒 子集权值进行更新，因为 需要积分运算 J，因此通常采用次优方案，如局部线 性化、基于无先导变换的高斯近似法、基于辅助粒子 滤波器的两步骤获取方法 等 2 1 2 重要性步骤 重要性步骤包括两步：权值 的计算和归一化 令 重要性函数如式(2)所示，则权值更新方式如下：(4)7 丁 xn I 1：n 一 1，0 1：n，归一化处理如下：，(m)：=_(5)n L，JW n 2 1 3 重采样 粒子滤波器的一个重要问题是粒子集权值的退 化，即随着时 间的增长，一部分权值变得非 常大，而 其余 的部分则变得微不足道 重采样就是要剔除较 小权值 的采样，从而集 中于显

15、著权值 的采样进行处 理 采样过程中使用的标准算法有多种，如残差重 采样、分支校正、系统重采样和带有拒绝控制 的采样 方法 通常，基本的随机重采样算法步骤如下 (1)从 中按照 与标 准 归一 化重 采样 函数 成比例的概率分别独立提取 x ，其 中 m=1，和 i J 1，与这些采样对应的新权值 分别为：W：O f(m)(6)(2)返 回新的随机测量数据：维普资讯 http:/ 第 2 9卷第 3期 余洪山等：基于粒子滤波器的移动机器人定位和地图创建研究进展 2 8 3 x ，”M (7)这里 i 代表重采样后粒子集在 内存 中的索引号 上 述粒子滤 波器算 法 的表示 具有 一定程 度上

16、的通用 性 例如，粒子滤波器常规直接采样方法是选择=当没有重采样处理时，对应 的=1 辅 助粒子滤波器重采样方法是设置 n：州=：驯 P(+f：)和7 r()=p(l：)，其中 为平均值，即 与概率密度 P(：)相关 的数据变量 某些重采样 算法(例如 R R)，利用数组复制因子 r 替代数组索 引 i ，复制因子表示在重采样处理 中每个粒子被复 制的次数 然后重采样过程根据 采样 r ，其对 应的支持量 由粒子集 定义 重采样通过集 中粒子集到高后验概率分布 的区 域以提高对 未来状态 的估 计，但 是 由于提高 了估 计 的方差，所 以降低 了当前估 计的精度 因此，重采 样的实施必须小心

17、，可预先估计进行重采样 的必要 性 2 2 粒子滤波器算法存在的问题与研究热点 最新的关于粒子滤波器算法 的研究 主要集 中体 现在重要性函数的设 计、降低计算复杂度条件下的 重采样策略、降低计算 复杂度条件下 的次优算法、粒 子滤波器的收敛性结论等方面 对于粒子滤波器 而言，粒子数 匮乏是其主要缺 陷，即随着迭代次数增加，粒子丧失多样性 的现象 D o u c e t 等 加 从理论上证明了 S I S算法 出现粒子数匮 乏现象 的必然性，而最有效 的解决方法是选择重要 性函数和采用重采样方法 为解决粒子数匮乏问题，研究人员也提出 了很多针对状态空间模型的改进算 法，如辅 助 变量 粒 子

18、滤 波算 法、局 部 线 性 化方 法、拒绝采样 方法 等 在重采样 改进方 法上，H i g u c h i 等 提 出通过引入遗传算法和进化算法，增 加重采样过程 中粒子 的多样性，F o x等 则 根据滤 波性能动态调整粒子数 3 基于粒子滤波器 的移动机器人定位(Mo b i l e r o bo t l o c a l i z a t i o n ba s e d o n p a r t i c l e fi l t e r s)D e l l a e r t 等 和 F o x等 叫最初将粒子滤波器算 法应用于移动机器人定位，形成 了一个新 的移动机 器人定位研究方 向蒙特卡 洛定

19、 位算 法 在 此基 础上，研究人 员针对算法 的计算复杂度、实时性、可 靠性等方面做 了进 一步研究，并开始广泛应 用于基 于声纳、激光和视觉传感 器等类别传感器 的移动机 器人系统定位 中，成为继 E K F模 型、H MM模 型之后 新的移动机器人定位模型 3 1 标准 MC L算法原理和特性 3 1 1 MCL算法原理和步骤 MC L定位算法集成 了机器人感知模型和运动模 型。，利用 个加权随机采样或粒子集合 S=s ，加 l=1，N 表示机器人位姿后验估计概率 B e l()，算法基本原理可表示为：B e l()=P(l d。)(8)B e l(X)S (9)这里 为 t 时刻对应的

20、状态，d 表示从u f 3 0到 t 的数据 t 时刻机器人位姿 的概率分布如式(1 0)所 示：B e l(f)=P(l 0 f，M f 一 1，0 ，M f 一 2，0 o)=(0 )Jp(，M )e f(，)_ l J (1 0)其中 o。“。分别对应移动机器人传感器测量 数据 和运 动控制 的测量数 据，条件 概率 P(一 ，“)为机器人运动模型，P(O，)代表机器人感知模 型，7 7 为归一化常量 方程(1 0)为 MC L算法 的基础 机器人运动过程 中，不 断生成机器人位姿 的采样集 合，根据粒 子滤波器实现对机器人状态的预测和更 新，通过 多次迭代处理来精确逼近位姿的后验分布

21、估计 利用 M C L算法进行移动机器人 全局定位，主要 可以分为采样、预测、更 新和权值归一化 四个步骤，详细步骤见图 2，其 中初始位姿概率在机器人所在空 间范围内呈均匀分布，加权值统一为 1 m 拍 (1)根据 B e l(一1)采集状态，按照 由重要 性 因子P：一。规定的权值：一，从表示 B e l(一1)的采样 集合 S 中随机抽取采样 z：(2)预测：根据上次运算获取 的状态集合 s 和 机器人运动控制量的测量信息预测 当前机器人 的位 姿状态P(，一 ，“，)，对于状态集合 s 中的每个 采样：一 ，根据 P(一 ，“)，抽取一个采样 z 通过上述操作，获取一个新 的集合 s

22、来近似 预测分 布P(l d )，此时集合 s 并没有考虑 t 时刻任何传 感器测量值(3)更新阶段：在此阶段，我们考虑传感器测量 值 0 ，并对 s 冲 的每个采样值进行加权处理，其权值 为 l-p(o l)，即给定o 时预测值 的可信度(4)权值归一化：对 于 个采样，分别对其权值 进行归一化处理，获得 t 时刻的采样集合 S =(：，维普资讯 http:/ 机器人 2 0 0 7年 5月)f i=1，2，从而获得关于B e l(x)的采样近 似 1，输入：5 =(，)J i：1，表示信任度(一 )，控制测量变量 一 l，观测量 Y 2 S ：=西，d=0 初始化 3 F o r i：1，

23、d o 生成 个采样 4 重采样：从先 前的信任度 中获取状态 从离散分布 中根据 s 的权值获取索引为 的采样 5 采样：预测下一状态 基于 和“，按 p(一 l 一 )获取采样：6 “=P(Y 。)计算 重要性权值 7 O t：=O t+”更新归一化因子 8 S ：=S u(，0)新采样插入采样集 9 e n d d 0 1 0 f o r ：1，d o 权值归一化处理 1 1 ：1 2 e n d d o l 3 e t u 7 1 S 图 2 M C L算法基本原理和步骤 F i g 2 T h e b a s i c p r i n c i p l e a n d s t e p s

24、 o f MCL a l g o ri t h m 从而 q ：=p(一。，“)B e l()对应重要 性采样后 的预测分布，用于近似期望后验概率分布：P(o f)P(f“一】，一 1)B e l(一 1)，、厂 上述更新 和迭代处理 步骤如图 3所示，图中黑 点表示机器人位姿 的采样分布 从 图 3(a)(e)可 以看出，机器人位姿从随机分布逐渐 收敛，最终收敛 于实际机器人位姿，收敛速度与采样数 目直接相关：O(1 -)3 1 2 基本 MC L算法的优点和特性 MC L为在线算法，可作为 A n y-t i m e算法应用，而 定位精度与时间相关，采样集合的尺寸在计算精度 和计算复杂度之

25、间达到一种平衡 相对于其他定位 方法，基于采样表示 MC L方法的优点如下，m ：1)与 卡尔曼滤波器方法相 比，可以表示多模分 布，并可实 现机器人的全局定位；2)与基于栅格 的马尔可夫定 位方法相比，能以相当高的频率集成测量数据；3)与 固定尺寸栅 格单元 的马尔可夫定位方法相 比，具有 更高的定位精度，原因在于采样 中对应 的状态表示 没有被离散化；4)易于实施(a)算法初始化(b)定位处理中 (c)成功定位 图3 用于全局定位的 MC L算法处理过程 F i g 3 T h e p r o c e s s o f MC L alg o rit h m f o r g l o b al

26、l o c ali z a t i o n Io 然而算法也存在不足，其原 因在于估计的随机 性 例如，如果采样集的尺寸较小，机器人可能仅 因 为 MC L没能够生成正确位 置的一个采样而导致失 去对其位置 的跟踪 算法也不适用于机器人绑架 问 题，因为一旦机器人处于绑架状态，则可能在机器人 新位姿附近没有合适 的采样 因此，当传感器不足够 准确时，上述基本 的 MC L算 法性能会急速下降 极 端情况下，常规 MC L算法在传感器信息无噪声干扰 时也会失败 3 2 改进 MC L算法 为避免和减少常规 MC L算法 的缺陷，研究人员 提出了多种改进方案 T h r u n等 提出了混合 MC

27、 L，算法综合 了常规 MC L和双 MC L方法 算法通过交换 MC L算法中预测分布和重要 性权值 因子 的角色，转 换 MC L的采样过程，其 中常规 MC L首先利用里程计 估计新位姿，然后利用传感器测量数据评价这次采 样的重要性；双 MC L方法利用最 近传感器测量数据 估计位姿，然后利用里程计评 价估计值与机器人先 前概率值和里程计数 据的符合程度 上述每种方法 均不是完美的，但是复合起来，效果非常好 特别是，当采样集合尺寸较小时(例如 5 0个采样)，混合 M C L 算法效果 良好，相 比先前 的 MC L算法，可 以更快地 维普资讯 http:/ 第 2 9卷第 3期 余洪山

28、等：基于粒子滤波器的移动机器人定位和地图创建研究进展 2 8 5 从机器人绑架问题中恢复出来 然而粒子滤波器表示能力 的提高，是 以高计算 复杂性为代价 的 如何实现粒子滤波器的在线、实时 估计 引来 了新 的研究 课题 K w o k等 提出 了一种 实时粒子滤波器，解决 由于计算资源 的限制带来 的 局限性 方法不再舍弃传感器测量数据，而是将 滤波 器更新过程 中到来的所有采样放入不同的观测集合 中 这里，实时粒子滤波器利用混合采样集合代表状 态空间分布，因此避免 了由于独立采样数 目的不足 导致 的滤波器分歧 问题 混合分量的权值通过采用 蒙特卡洛近似梯度 实现最优化处理，以减少混合表

29、示引入 的近似误差，最大程度上接近最优后验分布 粒子滤波器算法执行每次更新的时间复杂度与 估计所需的采样数 目成线性关 系 因此人们对采样 的有效使用进行 了研究，以保持采样集合为合理尺 寸 T h r u n等 提出在后验估计 中加入观测采样，从 而动态控制采样 集合数 目，但是该方法需要可有效 生成采样的传感器模型 G i l k s 等 提出引用 M C M C (M a r k o v C h a i n Mo n t e C a r l o)步骤提高基于采样表示 的后验估计性能；V l a s s i s 等 提 出采用辅 助粒子滤 波器，通过加入 一个步骤减少预测分布和 目标实际

30、分布之问的误匹配，从而减少重要性权值变化，提高 重要性采样 的效率 F o x等指出，在估计步骤中自适应选择采样集合 数 目可大 大提高 粒子 滤波 器 的效 率，并给 出 K u l 1 b a c k L e i b l e r 距离(K u l l b a c k L e i b l e r d i s t a n c e，K L D)采 样解决方案I S K L D采样方法 的主要思想在于限制 粒子滤波器的采样表示 引入 的近似误 差，如果采样 概率集 中于一小部分状态空间，则选择小数 目采样，而如果状 态不 确 定 性 较 高，则 选 择 大 数 目采 样 K w o k 将 K L

31、 D采样原理用于其提 出的实时粒子 滤波器算法中，形成 了 自适应实时粒子滤波器，进一 步提高了算法的执行效率 3 3 其他 MC L算法研究 MC L定位方法不仅在测距类移动机器人系统 中 取得成 功，在基于视觉 传感器 的机器人系统 中也取 得 了成功，该类算法通常称之为 C o n d e n s a t i o n算法 D e l l a e r t 等_ 9 将标准的 MC L算法应用于装载有视觉 传感器的移动机器人系统，解决了卡尔曼滤波器无 法实现 高 不确 定 性 环 境下 的定 位 的 难题 V l a s s i s 等 为解决图像的高维传感器观测和位置观测模 型问题，使用基

32、于 N N(N e a r e s t N e i g h b o u r)条件概率 估计 的逆非参数观测模 型，以解决 图像遮挡和机器 人绑架 问题，方法成功应用于基 于全景摄像机的室 外移动机器人 自主定位 J e n s f e h等_ 3。通过 T B F算法 提取机器人环境 中的有效路标特征，然后利用 C o n d e n s a t i o n算法实现 机器人 的实 时 动态定 位 L e n s e r 等_ 3 提出当机器人丢失时，在 MC L算法中加入传感 器采样，即传感 器 重 置定 位 算 法，并 成 功 应 用 于 R o b o C u p 9 9索尼行走机器人系统

33、 中，可在有限计 算 能力下实现机器人的鲁棒定位 Wo l f 等 提取环境 图像不变特征作为路标，根据环境地 图为数据库 中 的每个图像抽取可能视点集合，然后结合蒙特卡洛 定位算法实现对机器人 的可靠定位 与跟踪 L i n a k e r 等_ 3 利用移动机器人装载全景摄像机执行基 于外 观的实时全局定位处 理 算法直接对全景摄像机 图 像进行处理，生成低维旋转恒定特征向量 利用这些 特征 向量，粒子滤波器实现移 动机器人位姿的精确 连续估计 4 基于粒子滤波器的移动机器人地图创建(Mo b i l e r o b o t ma p-b u i l d i n g b a s e d o n p a r t i c l e fi l t e r s)M o n t e me r l o等 1 2,1 3 首先提出了 F a s t S L A M方法，该方法将粒子滤波器和扩展卡尔曼滤