第10卷第3期 智能系统学报 Vol.10 No.3 2015年6月 CAAI Transactions on Intelligent Systems Jun.2015 D0:10.3969/j.issn.1673-4785.201404024 网络出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150507.1135.001.html 改进RBPF的移动机器人同步定位与地图构建 罗元，余佳航，汪龙峰，王运凯 (重庆邮电大学国家信息无障碍工程研发中心，重庆400065) 摘要：传统Rao-Blackwellized粒子滤波器(RBPF)在移动机器人同步定位与地图构建(SLAM)研究中，存在算法复杂 度过高、占用内存空间过多导致实时性不理想的问题，因此提出一种改进算法。在某一特定状态的一组粒子集中，粒子 的统计特性是一致的，改进算法从中选取一个代表粒子，进行卡尔曼更新步骤，并在同一粒子集中重复使用。同时结合 Gmapping算法的建议分布和自适应重采样技术。实际PioneerⅢ移动机器人在机器人操作系统(ROS)平台上进行的 实验表明，该方法在保证栅格地图精度的同时能提高系统的实时性，降低复杂度，提高运算速度。 关键词：移动机器人；Rao-Blackwellized粒子滤波器；同步定位与地图构建(SLAM);Gmapping算法；自适应重采样技术 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2015)03-0460-05 中文引用格式：罗元，余佳航，汪龙峰，等.改进RBPF的移动机器人同步定位与地图构建[J].智能系统学报，2015,10(3)：460 464. 英文引用格式：LUO Yuan,YU Jiahang,WANG Longfeng,etal.Simultaneous localization and mapping of an improved RBPF based mobile robot [J].CAAI Transactions on Intelligent Systems,2015,10(3):460-464. Simultaneous localization and mapping of an improved RBPF based mobile robot LUO Yuan,YU Jiahang,WANG Longfeng,WANG Yunkai (National Information Accessibility Engineering Research Development Center,Chongqing University of Posts and Telecommunica- tions,Chongqing 400065,China) Abstract:As in the research of simultaneous localization and mapping(SLAM)of mobile robot applying traditional Rao-Blackwellized particle filter,the computational complexity is too high and memory space usage is too large, which causes poor real-time performance,an improved approach is proposed.Among a group of particles gathering in a particular state,the statistical properties of particles are identical.By applying the Kalman updating step to one representative particle in the group of particles,and using it repeatedly in the same group,the complexity is re- duced and arithmetic speed is improved.Combining the proposed distribution and adaptive resampling methods from the Gmapping algorithm,the results of actual experiment carried out with Pioneer IlI robot and ROS platform illus- trate that the real-time performance of the proposal could be enhanced while ensuring the quality of grid map. Keywords:mobile robot;Rao-Blackwellized particle filter;simultaneous localization and mapping (SLAM); Gmapping algorithm;adaptive resampling methods 同步定位与地图构建(simultaneous localization 的口，指移动机器人从一个未知的位置出发，在运 and mapping,SLAM)最先是由Smith等提出来 动过程中利用传感器对环境的观测递增地建立地 图，同时根据已建立的地图同步确定自己的位置。 收稿日期：2014-04-15.网络出版日期：2015-05-07. 基金项目：国家自然科学基金资助项目(51075420)：重庆市教委科学 十多年来，SLAM问题仍然是机器人研究领域的热 技术研究项目(KJ120519). 点。 通信作者：余佳航.E-mail:tracy_he_1@126.com
第 １０ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．３ ２０１５ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１５ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１４０４０２４ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１５０５０７．１１３５．００１．ｈｔｍｌ 改进 ＲＢＰＦ 的移动机器人同步定位与地图构建 罗元，余佳航，汪龙峰，王运凯 （重庆邮电大学 国家信息无障碍工程研发中心，重庆 ４０００６５） 摘 要：传统 Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 粒子滤波器（ＲＢＰＦ）在移动机器人同步定位与地图构建（ＳＬＡＭ）研究中，存在算法复杂 度过高、占用内存空间过多导致实时性不理想的问题，因此提出一种改进算法。 在某一特定状态的一组粒子集中，粒子 的统计特性是一致的，改进算法从中选取一个代表粒子，进行卡尔曼更新步骤，并在同一粒子集中重复使用。 同时结合 Ｇｍａｐｐｉｎｇ 算法的建议分布和自适应重采样技术。 实际 Ｐｉｏｎｅｅｒ ＩＩＩ 移动机器人在机器人操作系统（ＲＯＳ）平台上进行的 实验表明，该方法在保证栅格地图精度的同时能提高系统的实时性，降低复杂度，提高运算速度。 关键词：移动机器人；Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 粒子滤波器；同步定位与地图构建（ＳＬＡＭ）；Ｇｍａｐｐｉｎｇ 算法；自适应重采样技术 中图分类号：ＴＰ２４２．６ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０３⁃０４６０⁃０５ 中文引用格式：罗元，余佳航，汪龙峰，等． 改进 ＲＢＰＦ 的移动机器人同步定位与地图构建［ Ｊ］． 智能系统学报， ２０１５， １０（ ３）： ４６０⁃ ４６４． 英文引用格式：ＬＵＯ Ｙｕａｎ， ＹＵ Ｊｉａｈａｎｇ， ＷＡＮＧ Ｌｏｎｇｆｅｎｇ， ｅｔ ａｌ． Ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｏｆ ａｎ ｉｍｐｒｏｖｅｄ ＲＢＰＦ ｂａｓｅｄ ｍｏｂｉｌｅ ｒｏｂｏｔ ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（３）：４６０⁃４６４． Ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｏｆ ａｎ ｉｍｐｒｏｖｅｄ ＲＢＰＦ ｂａｓｅｄ ｍｏｂｉｌｅ ｒｏｂｏｔ ＬＵＯ Ｙｕａｎ， ＹＵ Ｊｉａｈａｎｇ， ＷＡＮＧ Ｌｏｎｇｆｅｎｇ， ＷＡＮＧ Ｙｕｎｋａｉ （Ｎａｔｉｏｎａｌ Ｉｎｆｏｒｍａｔｉｏｎ Ａｃｃｅｓｓｉｂｉｌｉｔｙ Ｅｎｇｉｎｅｅｒｉｎｇ Ｒｅｓｅａｒｃｈ ＆ Ｄｅｖｅｌｏｐｍｅｎｔ Ｃｅｎｔｅｒ， Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａ⁃ ｔｉｏｎｓ， Ｃｈｏｎｇｑｉｎｇ ４０００６５， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ａｓ ｉｎ ｔｈｅ ｒｅｓｅａｒｃｈ ｏｆ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ （ＳＬＡＭ） ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔ ａｐｐｌｙｉｎｇ ｔｒａｄｉｔｉｏｎａｌ Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ， ｔｈｅ ｃｏｍｐｕｔａｔｉｏｎａｌ ｃｏｍｐｌｅｘｉｔｙ ｉｓ ｔｏｏ ｈｉｇｈ ａｎｄ ｍｅｍｏｒｙ ｓｐａｃｅ ｕｓａｇｅ ｉｓ ｔｏｏ ｌａｒｇｅ， ｗｈｉｃｈ ｃａｕｓｅｓ ｐｏｏｒ ｒｅａｌ⁃ｔｉｍｅ ｐｅｒｆｏｒｍａｎｃｅ， ａｎ ｉｍｐｒｏｖｅｄ ａｐｐｒｏａｃｈ ｉｓ ｐｒｏｐｏｓｅｄ． Ａｍｏｎｇ ａ ｇｒｏｕｐ ｏｆ ｐａｒｔｉｃｌｅｓ ｇａｔｈｅｒｉｎｇ ｉｎ ａ ｐａｒｔｉｃｕｌａｒ ｓｔａｔｅ， ｔｈｅ ｓｔａｔｉｓｔｉｃａｌ ｐｒｏｐｅｒｔｉｅｓ ｏｆ ｐａｒｔｉｃｌｅｓ ａｒｅ ｉｄｅｎｔｉｃａｌ． Ｂｙ ａｐｐｌｙｉｎｇ ｔｈｅ Ｋａｌｍａｎ ｕｐｄａｔｉｎｇ ｓｔｅｐ ｔｏ ｏｎｅ ｒｅｐｒｅｓｅｎｔａｔｉｖｅ ｐａｒｔｉｃｌｅ ｉｎ ｔｈｅ ｇｒｏｕｐ ｏｆ ｐａｒｔｉｃｌｅｓ， ａｎｄ ｕｓｉｎｇ ｉｔ ｒｅｐｅａｔｅｄｌｙ ｉｎ ｔｈｅ ｓａｍｅ ｇｒｏｕｐ， ｔｈｅ ｃｏｍｐｌｅｘｉｔｙ ｉｓ ｒｅ⁃ ｄｕｃｅｄ ａｎｄ ａｒｉｔｈｍｅｔｉｃ ｓｐｅｅｄ ｉｓ ｉｍｐｒｏｖｅｄ． Ｃｏｍｂｉｎｉｎｇ ｔｈｅ ｐｒｏｐｏｓｅｄ ｄｉｓｔｒｉｂｕｔｉｏｎ ａｎｄ ａｄａｐｔｉｖｅ ｒｅｓａｍｐｌｉｎｇ ｍｅｔｈｏｄｓ ｆｒｏｍ ｔｈｅ Ｇｍａｐｐｉｎｇ ａｌｇｏｒｉｔｈｍ， ｔｈｅ ｒｅｓｕｌｔｓ ｏｆ ａｃｔｕａｌ ｅｘｐｅｒｉｍｅｎｔ ｃａｒｒｉｅｄ ｏｕｔ ｗｉｔｈ Ｐｉｏｎｅｅｒ ＩＩＩ ｒｏｂｏｔ ａｎｄ ＲＯＳ ｐｌａｔｆｏｒｍ ｉｌｌｕｓ⁃ ｔｒａｔｅ ｔｈａｔ ｔｈｅ ｒｅａｌ⁃ｔｉｍｅ ｐｅｒｆｏｒｍａｎｃｅ ｏｆ ｔｈｅ ｐｒｏｐｏｓａｌ ｃｏｕｌｄ ｂｅ ｅｎｈａｎｃｅｄ ｗｈｉｌｅ ｅｎｓｕｒｉｎｇ ｔｈｅ ｑｕａｌｉｔｙ ｏｆ ｇｒｉｄ ｍａｐ． Ｋｅｙｗｏｒｄｓ：ｍｏｂｉｌｅ ｒｏｂｏｔ； Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ； ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ （ ＳＬＡＭ）； Ｇｍａｐｐｉｎｇ ａｌｇｏｒｉｔｈｍ； ａｄａｐｔｉｖｅ ｒｅｓａｍｐｌｉｎｇ ｍｅｔｈｏｄｓ 收稿日期：２０１４⁃０４⁃１５． 网络出版日期：２０１５⁃０５⁃０７． 基金项目：国家自然科学基金资助项目（５１０７５４２０）；重庆市教委科学 技术研究项目（ＫＪ１２０５１９）． 通信作者：余佳航． Ｅ⁃ｍａｉｌ： ｔｒａｃｙ＿ｔｈｅ＿１＠ １２６．ｃｏｍ． 同步定位与地图构建（ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ， ＳＬＡＭ） 最 先 是 由 Ｓｍｉｔｈ 等 提 出 来 的［１］ ，指移动机器人从一个未知的位置出发，在运 动过程中利用传感器对环境的观测递增地建立地 图，同时根据已建立的地图同步确定自己的位置。 十多年来，ＳＬＡＭ 问题仍然是机器人研究领域的热 点［２］
第3期 罗元，等：改进RBP℉的移动机器人同步定位与地图构建 ·461. 目前SLAM问题的解决方法以概率估计为主， 式中：x为机器人运动轨迹，m为地图，z为观测信 主要分为基于卡尔曼滤波(KF)的算法和基于粒子 息，u为里程计信息。计算p(xI之，山14-)可以使 滤波(PF)的算法。其中，基于EKF的SLAM算 用粒子滤波算法，每一个粒子代表机器人可能的轨 法[)的研究最早开始于1986年，虽然计算速度快， 迹。对于p(m|x14之1），在求得p(x1|乙14,41-) 但此类算法存在难以解决的数据关联问题，且计算 之后可以使用卡尔曼滤波算法进行分析性计算。 量过大、精度不高。UKF不需要进行雅可比矩阵计 1.2使用RBPF进行地图构建 算、计算量小。而在实际应用时，系统噪声相关信息 如1.1小节所述，RBPF可以分为粒子滤波和卡 的不确定性都会影响UKF滤波的精度，并且主要参 尔曼滤波两部分。 数和滤波增益不能在线调整，缺乏自适应能力[)。 1.2.1粒子滤波部分 近年来，基于粒子滤波的方法正逐渐成为研究 粒子滤波是一种马尔可夫链蒙特卡罗算法，用 SLAM的热点。相对于KF及其衍生算法，PF的优 一系列样本（粒子）来估计未知状态山。通过重要 点在于能有效处理非线性非高斯分布的状态估计问 性采样原则，第i个样本的概率可以用重要性权值 题。Doucet等)提出有效解决SLAM问题的Rao w表示。 Blackwellized粒子滤波器(RBPF)方法。每一个粒 子代表机器人一种可能的运动轨迹，同时每一个粒 o(x1a）= p（x11z1,L1-1） (2) T(x11214,414-1） 子都具有自己的全局地图，它们和该粒子的轨迹相 式中：T(xz,414-)称作重要性函数或者建议 对应。因此该算法能够较好地近似移动机器人位姿 分布。归一化后的重要性权值为 和环境地图的联合概率密度。但当粒子数目增多、 a(x0) 环境地图的尺寸增大时，算法会占用大量的内存空 (3) 间，而且重采样过程中，全局地图进行拷贝需要占用 ∑(0 大量的内存空间。因此，降低计算复杂度以及减 归一化权值ω，⊙与样本{x,m⊙}共同决定 少构建精确地图所需的粒子数成为此类方法需要重 后验概率分布p(x,mlz1,41-1)。假定用N个带 点解决的问题。Griseti等[)通过改进建议分布和引 有权值的样本{(x,m0),ω0，表示后验分布 入自适应重采样机制提高该算法的执行效率。 p(x,m|乙14,1-)，则 2010年Willow Garage公司发布了开源机器人 市，(x1u,mlz1u,41-i)=∑o06g(x,m） 操作系统(robot operating system,ROS),由于其具 i=1 有点对点设计、不依赖编程语言、开源等优点，很快 (4) 在机器人领域展开学习和使用ROS的热潮〔)。在 式中：8(，(·)表示Dirac函数。如果假设用V个带 ROS中，Griseti等的算法被制作成一个名为Gmap- 有权值的样本{x9,ω，0，表示路径的后验概率 ping的SLAM功能包9)，使用激光测距仪可以建立 密度p(x|之14,41-1），则 精度较高的二维栅格地图，但其实时性仍有待提高。 (1)=三aP6gx） (5) 本文在此基础上提出了一种降低复杂度的改进算 法，在RBPF的卡尔曼更新步骤中，从一组粒子里选 由此，地图的后验概率可估算为 取一个代表粒子进行更新，避免计算所有粒子。通 fx(m31t,1t-1)= 过在配有激光测距仪的PioneerⅢ机器人上的实验 p(m) 来验证该算法的有效性。 (6) 可以使用一组N个卡尔曼滤波器进行分析性计算， 1 RBPF与地图构建 每个粒子使用一个卡尔曼滤波器] l.lRao-Blackwellized变换 标准RBPF算法中的粒子滤波由以下3步组 在SLAM问题中，Rao-Blackwellized变换可理解 成，其中⊙表示第i个粒子。 为将联合后验概率分解为对路径的后验概率和在给 1)蒙特卡罗步骤：RBPF算法中的蒙特卡罗步 定路径的情况下对地图的后验概率[0。具体分解 骤包括从先验分布中采样： 如式(1)： 0=P(x,Ix8) (7) p(x1,m|z1t,u1:-1）= 虽然计算是为了得到”，但每个粒子产都包含 p(m|x1a,z1a）·p(x1:a1z1,u1-1） (1) 了轨迹和地图的信息，分别由⑧，和三”表示
目前 ＳＬＡＭ 问题的解决方法以概率估计为主， 主要分为基于卡尔曼滤波（ＫＦ）的算法和基于粒子 滤波 （ ＰＦ） 的算法。 其中， 基于 ＥＫＦ 的 ＳＬＡＭ 算 法［３］的研究最早开始于 １９８６ 年，虽然计算速度快， 但此类算法存在难以解决的数据关联问题，且计算 量过大、精度不高。 ＵＫＦ 不需要进行雅可比矩阵计 算、计算量小。 而在实际应用时，系统噪声相关信息 的不确定性都会影响 ＵＫＦ 滤波的精度，并且主要参 数和滤波增益不能在线调整，缺乏自适应能力［４］ 。 近年来， 基 于 粒 子 滤 波 的 方 法 正 逐 渐 成 为 研 究 ＳＬＡＭ 的热点。 相对于 ＫＦ 及其衍生算法，ＰＦ 的优 点在于能有效处理非线性非高斯分布的状态估计问 题。 Ｄｏｕｃｅｔ 等［５］ 提出有效解决 ＳＬＡＭ 问题的 Ｒａｏ⁃ Ｂｌａｃｋｗｅｌｌｉｚｅｄ 粒子滤波器（ＲＢＰＦ） 方法。 每一个粒 子代表机器人一种可能的运动轨迹，同时每一个粒 子都具有自己的全局地图，它们和该粒子的轨迹相 对应。 因此该算法能够较好地近似移动机器人位姿 和环境地图的联合概率密度。 但当粒子数目增多、 环境地图的尺寸增大时，算法会占用大量的内存空 间，而且重采样过程中，全局地图进行拷贝需要占用 大量的内存空间［６］ 。 因此，降低计算复杂度以及减 少构建精确地图所需的粒子数成为此类方法需要重 点解决的问题。 Ｇｒｉｓｅｔｉ 等［７］通过改进建议分布和引 入自适应重采样机制提高该算法的执行效率。 ２０１０ 年 Ｗｉｌｌｏｗ Ｇａｒａｇｅ 公司发布了开源机器人 操作系统（ ｒｏｂｏｔ ｏｐｅｒａｔｉｎｇ ｓｙｓｔｅｍ， ＲＯＳ），由于其具 有点对点设计、不依赖编程语言、开源等优点， 很快 在机器人领域展开学习和使用 ＲＯＳ 的热潮［８］ 。 在 ＲＯＳ 中，Ｇｒｉｓｅｔｉ 等的算法被制作成一个名为 Ｇｍａｐ⁃ ｐｉｎｇ 的 ＳＬＡＭ 功能包［９］ ，使用激光测距仪可以建立 精度较高的二维栅格地图，但其实时性仍有待提高。 本文在此基础上提出了一种降低复杂度的改进算 法，在 ＲＢＰＦ 的卡尔曼更新步骤中，从一组粒子里选 取一个代表粒子进行更新，避免计算所有粒子。 通 过在配有激光测距仪的 Ｐｉｏｎｅｅｒ ＩＩＩ 机器人上的实验 来验证该算法的有效性。 １ ＲＢＰＦ 与地图构建 １．１ Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 变换 在 ＳＬＡＭ 问题中，Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 变换可理解 为将联合后验概率分解为对路径的后验概率和在给 定路径的情况下对地图的后验概率［１０］ 。 具体分解 如式（１）： ｐ（ｘ１：ｔ，ｍ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ｐ（ｍ ｜ ｘ１：ｔ，ｚ１：ｔ）·ｐ（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） （１） 式中： ｘ 为机器人运动轨迹， ｍ 为地图， ｚ 为观测信 息， ｕ 为里程计信息。 计算 ｐ（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） 可以使 用粒子滤波算法，每一个粒子代表机器人可能的轨 迹。 对于 ｐ（ｍ ｜ ｘ１：ｔ，ｚ１：ｔ） ，在求得 ｐ（ｘ１：ｔ ｜ ｚ１：ｔ， ｕ１：ｔ－１ ） 之后可以使用卡尔曼滤波算法进行分析性计算。 １．２ 使用 ＲＢＰＦ 进行地图构建 如 １．１ 小节所述，ＲＢＰＦ 可以分为粒子滤波和卡 尔曼滤波两部分。 １．２．１ 粒子滤波部分 粒子滤波是一种马尔可夫链蒙特卡罗算法，用 一系列样本（粒子）来估计未知状态［１１］ 。 通过重要 性采样原则，第 ｉ 个样本的概率可以用重要性权值 ω ｉ ｔ 表示。 ω ～ （ｘ１，ｔ） ＝ ｐ（ｘ１，ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） π（ｘ１，ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） （２） 式中： π（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） 称作重要性函数或者建议 分布。 归一化后的重要性权值为 ω （ｉ） ｔ ＝ ω ～ （ｘ （ｉ） １：ｔ ） ∑ Ｎ ｊ ＝ １ ω ～ （ｘ （ｊ） １：ｔ ） （３） 归一化权值 ω （ｉ） ｔ 与样本 ｛ ｘ （ｉ） １：ｔ ，ｍ （ｉ） ｝ 共同决定 后验概率分布 ｐ（ｘｔ，ｍ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） 。 假定用 Ｎ 个带 有权值的样本 ｛（ｘ （ｉ） １：ｔ ，ｍ （ｉ） ），ω （ｉ） ｔ ｝ Ｎ ｉ ＝ １ 表示后验分布 ｐ（ｘ１：ｔ，ｍ ｜ ｚ１：ｔ ，ｕ１：ｔ－１ ） ，则 Ｐ ? Ｎ（ｘ１：ｔ，ｍ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ∑ Ｎ ｉ ＝ １ ω （ｉ） ｔ δｘ （ｉ） １：ｔ ，ｍ（ｘ１：ｔ，ｍ） （４） 式中： δ （·）（·） 表示 Ｄｉｒａｃ 函数。 如果假设用 Ｎ 个带 有权值的样本 ｛ｘ （ｉ） １：ｔ ，ω （ｉ） ｔ ｝ Ｎ ｉ ＝ １ 表示路径的后验概率 密度 ｐ（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ，则 Ｐ ? Ｎ（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ∑ Ｎ ｉ ＝ １ ω （ｉ） ｔ δｘ （ｉ） １：ｔ （ｘ１：ｔ） （５） 由此，地图的后验概率可估算为 ｐ ? Ｎ（ｍ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ∑ Ｎ ｉ ＝ １ ω （ｉ） ｔ ｐ（ｍ ｚ１：ｔ，ｕ１：ｔ－１ ，ｘ （ｉ） １：ｔ ） （６） 可以使用一组 Ｎ 个卡尔曼滤波器进行分析性计算， 每个粒子使用一个卡尔曼滤波器［１２］ 。 标准 ＲＢＰＦ 算法中的粒子滤波由以下 ３ 步组 成，其中 ｐ ?（ｉ） ｔ 表示第 ｉ 个粒子。 １）蒙特卡罗步骤：ＲＢＰＦ 算法中的蒙特卡罗步 骤包括从先验分布中采样： ｘ ?（ｉ） ｔ ＝ Ｐ（ｘｔ ｜ ｘ （ｉ） ｔ－１ ） （７） 虽然计算是为了得到 ｘ ?（ｉ） ｔ ，但每个粒子 ｐ ?（ｉ） ｔ 都包含 了轨迹和地图的信息，分别由 μ ?（ｉ） ｔ｜ ｔ－１ 和 Σ ＾ （ｉ） ｔ｜ ｔ－１ 表示。 第 ３ 期 罗元，等：改进 ＲＢＰＦ 的移动机器人同步定位与地图构建 ·４６１·
·462. 智能系统学报 第10卷 2)序贯重要性采样步骤：包括计算式(5)中的 同位置的粒子。 p(xIz1,41-1)。如果把先验分布当作重要性函 通过在必要时更新和并在同一粒子群 数，例如π(x11乙1，山1-)=P(x,1x-1),从而可以 中重复使用，在粒子数中等或者大量时，例如100≤ 递归计算： V≤1000，时间和计算消耗能明显减小。当粒子数 o0cm9p(z,4,1z1-1,4-2,0)(8) 相对小时，例如N≤10，改进RBPF算法相对标准 式中：p(3,山，1乙1-1,414-2,0）为相似函数。在算 RBPF的优势不明显。2种算法对比如下： 法实现中，t-1时刻重采样后如果传播粒子权值相 1)标准RBPF的卡尔曼更新步聚： 同，可以省去w-1。 for i=1 to N do 3)重采样步骤：舍弃低权值粒子，保留高权值 a0←u9+ 粒子。 C(())-()) 1.2.2卡尔曼滤波部分 0←-8-8C(x)SoC(x9 通过粒子滤波部分采样之后，卡尔曼滤波 end for 负责更新地图m的均值和方差。 2)改进RBPF的卡尔曼更新步聚： u8=A(x0)u9-1+F(x0)4, 8=A(x0)9-A(x0)r+B(x0)B(x0)T forp0,更新均值u,”和方差 s0=C(x9)Σ9C(x0)T+ u0←u82+ D(x0)D(x0)(z9,91)= Σ0，C(x9)S(z,4-1)-(z,4-2) C(x0)8+G(x0)4, 0←-0C(x)'S"C9) u0=a81+ for i 2 to N do 9C(x0)rS1(z,4-i)-(z81,a92) if p(i)=p(i-1)then 0=9-8C(x)'SwC()8 u0u-w,0←-- else 式中： 4A-1兰Em,1乙1-1,41-2 u,0←-u9+ u,Em,1z14,41-1f 9C(x0)TS@(z,4-）-(z9,49-2) 王4-1c0v(m,1z1-1,41-2) 0←--况C(x)SC(x end if .主cov(m,|乙1,41-i） end for S,=cov((3,4,)1Z1-1,14-2） 此外，建议分布采用文献[7]中的 通过在均值和方差更新步骤使用卡尔曼滤波 p(x,1m91,x9,z,4,)≈ p(z,1m9,x,) 器，RBPF算法的估算精度能得到较大提高。然后， RBPF算法需要对每一个粒子使用卡尔曼滤波器， p(zI mx)d r'eL(O 当有大量粒子时计算复杂度会急剧增加。 L(=xI p(zI mx)>s 2改进RBPF算法 为间距，重采样步骤中使用自适应重采样技术，详见 [7],经过采样、计算粒子重要性权值、重采样、更新 标准RBPF中需对每一个存活粒子i,pD进行 地图等步骤实现SLAM。 卡尔曼更新，包括重复计算u,)和，然后再循 改进算法流程示意如图1所示。在t-1时刻， 环步骤重复计算逆矩阵S,(),这将导致计算量巨 采样阶段进行8~P(x-1Ix,巴)得到无权重粒子 大。在粒子滤波部分的重采样步骤中，高权值粒子 {巴，N-}。重要性采样阶段得到带权重的粒子 决定机器人更可能的位置。这一步骤中，粒子聚集 {8,©，巴}。重采样阶段，保留高权重粒子舍去低 在一些特殊位置。然后在RB变换中，粒子估计的 权重粒子得到{p9,N}。减小复杂度过程，只负 状态是离散的。这说明聚集在离散状态的粒子群分 责更新不同位置的粒子，使用一组K(K≤N)个卡 布在有限空间中。因此，所有粒子的统计特性，例如 尔曼滤波器计算山，⑧，因此减少了卡尔曼更新 ,⊙和是一致的，只需其中之一作为代表来进 的计算量。最后，在t时刻采样阶段进行一 行卡尔曼滤波步骤。最终改进RBPF算法只需用K P(x,|x)得到{p,N-}。 个卡尔曼滤波器(K<N)计算u,和)更新不
２）序贯重要性采样步骤：包括计算式（５） 中的 ｐ（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） 。 如果把先验分布当作重要性函 数，例如 π（ｘ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ Ｐ（ｘｔ ｜ ｘｔ－１ ） ，从而可以 递归计算： ω ～ （ｉ） ｔ ∝ ω ～ （ｉ） ｔ－１ ｐ（ｚｔ，ｕｔ ｜ ｚ１：ｔ－１ ，ｕｔ：ｔ－２ ，ｘ ?（ｉ） ｔ ） （８） 式中： ｐ（ｚｔ，ｕｔ ｜ ｚ１：ｔ－１ ，ｕ１：ｔ－２ ，ｘ ?（ｉ） ｔ ） 为相似函数。 在算 法实现中， ｔ － １ 时刻重采样后如果传播粒子权值相 同，可以省去 ω ｉ ｔ－１ 。 ３）重采样步骤：舍弃低权值粒子，保留高权值 粒子。 １．２．２ 卡尔曼滤波部分 通过粒子滤波部分采样 ｘ ?（ｉ） ｔ 之后，卡尔曼滤波 负责更新地图 ｍ 的均值和方差。 μ （ｉ） ｔ｜ ｔ－１ ＝ Ａ（ｘ （ｉ） ｔ ） μ （ｉ） ｔ－１｜ ｔ－１ ＋ Ｆ（ｘ （ｉ） ｔ ） ｕｔ Σ （ｉ） ｔ｜ ｔ－１ ＝ Ａ（ｘ （ｉ） ｔ ）Σ （ｉ） ｔ－１｜ ｔ－１Ａ （ｘ （ｉ） ｔ ） Ｔ ＋ Ｂ（ｘ （ｉ） ｔ ）Ｂ （ｘ （ｉ） ｔ ） Ｔ Ｓ （ｉ） ｔ ＝ Ｃ（ｘ （ｉ） ｔ ）Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ ＋ Ｄ（ｘ （ｉ） ｔ ）Ｄ （ｘ （ｉ） ｔ ） Ｔ （ｚ （ｉ） ｔ｜ ｔ－１ ，ｕ （ｉ） ｔ｜ ｔ－１ ） ＝ Ｃ（ｘ （ｉ） ｔ ） μ （ｉ） ｔ｜ ｔ－１ ＋ Ｇ（ｘ （ｉ） ｔ ） ｕｔ μ （ｉ） ｔ ＝ μ （ｉ） ｔ｜ ｔ－１ ＋ Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ （（ｚｔ，ｕｔ－１ ） － （ｚ （ｉ） ｔ｜ ｔ－１ ，ｕ （ｉ） ｔ｜ ｔ－２ ）） Σ （ｉ） ｔ ＝ Σ （ｉ） ｔ｜ ｔ－１ － Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ Ｃ（ｘ （ｉ） ｔ ）Σ （ｉ） ｔ｜ ｔ－１ 式中： μｔ｜ ｔ－１ ＝ Δ Ｅ｛ｍｔ ｜ ｚ１：ｔ－１ ，ｕ１：ｔ－２ ｝ μｔ ＝ Δ Ｅ｛ｍｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ｝ Σ ｔ｜ ｔ－１ ＝ Δ ｃｏｖ（ｍｔ ｜ ｚ１：ｔ－１ ，ｕ１：ｔ－２ ） Σｔ ＝ Δ ｃｏｖ（ｍｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） Ｓｔ ＝ Δ ｃｏｖ（（ｚｔ，ｕｔ） ｜ ｚ１：ｔ－１ ，ｕ１：ｔ－２ ） 通过在均值和方差更新步骤使用卡尔曼滤波 器，ＲＢＰＦ 算法的估算精度能得到较大提高。 然后， ＲＢＰＦ 算法需要对每一个粒子使用卡尔曼滤波器， 当有大量粒子时计算复杂度会急剧增加。 ２ 改进 ＲＢＰＦ 算法 标准 ＲＢＰＦ 中需对每一个存活粒子 ｉ， ｐ （ｉ） ｔ 进行 卡尔曼更新，包括重复计算 μ （ｉ） ｔ 和 Σ （ｉ） ｔ ，然后再循 环步骤重复计算逆矩阵 Ｓ －１（ｉ） ｔ ，这将导致计算量巨 大。 在粒子滤波部分的重采样步骤中，高权值粒子 决定机器人更可能的位置。 这一步骤中，粒子聚集 在一些特殊位置。 然后在 ＲＢ 变换中，粒子估计的 状态是离散的。 这说明聚集在离散状态的粒子群分 布在有限空间中。 因此，所有粒子的统计特性，例如 μ （ｉ） ｔ 和 Σ （ｉ） ｔ 是一致的，只需其中之一作为代表来进 行卡尔曼滤波步骤。 最终改进 ＲＢＰＦ 算法只需用 Ｋ 个卡尔曼滤波器（ Ｋ ＜ Ｎ ）计算 μ （ｉ） ｔ 和 Σ （ｉ） ｔ 更新不 同位置的粒子。 通过在必要时更新 μ （ｉ） ｔ 和 Σ （ｉ） ｔ 并在同一粒子群 中重复使用，在粒子数中等或者大量时，例如 １００ ≤ Ｎ ≤ １ ０００，时间和计算消耗能明显减小。 当粒子数 相对小时，例如 Ｎ ≤ １０，改进 ＲＢＰＦ 算法相对标准 ＲＢＰＦ 的优势不明显。 ２ 种算法对比如下： １）标准 ＲＢＰＦ 的卡尔曼更新步聚： ｆｏｒ ｉ ＝ １ ｔｏ Ｎ ｄｏ μ （ｉ） ｔ ← μ （ｉ） ｔ｜ ｔ－１ ＋ Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ （（ｚｔ，ｕｔ） － （ｚ （ｉ） ｔ｜ ｔ－１ ，ｕ （ｉ） ｔ｜ ｔ－１ ）） Σ （ｉ） ｔ ←Σ （ｉ） ｔ｜ ｔ－１ － Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ Ｃ（ｘ （ｉ） ｔ ）Σ （ｉ） ｔ｜ ｔ－１ ｅｎｄ ｆｏｒ ２）改进 ＲＢＰＦ 的卡尔曼更新步聚： ｆｏｒ ｐ （１） ｔ ， 更新均值 μ （１） ｔ 和方差 Σ （１） ｔ μ （１） ｔ ← μ （１） ｔ｜ ｔ－１ ＋ Σ （１） ｔ｜ ｔ－１Ｃ （ｘ （１） ｔ ） Ｔ Ｓ －１（１） ｔ （（ｚｔ，ｕｔ－１ ） － （ｚ （１） ｔ｜ ｔ－１ ，ｕ （１） ｔ－１｜ ｔ－２ ）） Σ （１） ｔ ←Σ （１） ｔ｜ ｔ－１ － Σ （１） ｔ｜ ｔ－１Ｃ （ｘ （１） ｔ ） Ｔ Ｓ －１（１） ｔ Ｃ（ｘ （１） ｔ ）Σ （１） ｔ｜ ｔ－１ ｆｏｒ ｉ ＝ ２ ｔｏ Ｎ ｄｏ ｉｆ ｐ （ｉ） ｔ ＝ ｐ （ｉ－１） ｔ ｔｈｅｎ μ （ｉ） ｔ ←μ （ｉ－１） ｔ ， Σ （ｉ） ｔ ←Σ （ｉ－１） ｔ ｅｌｓｅ μ （ｉ） ｔ ← μ （ｉ） ｔ｜ ｔ－１ ＋ Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ （（ｚｔ，ｕｔ－１ ） － （ｚ （ｉ） ｔ｜ ｔ－１ ，ｕ （ｉ） ｔ－１｜ ｔ－２ ）） Σ （ｉ） ｔ ←Σ （ｉ） ｔ｜ ｔ－１ － Σ （ｉ） ｔ｜ ｔ－１Ｃ （ｘ （ｉ） ｔ ） Ｔ Ｓ －１（ｉ） ｔ Ｃ（ｘ （ｉ） ｔ ）Σ （ｉ） ｔ｜ ｔ－１ ｅｎｄ ｉｆ ｅｎｄ ｆｏｒ 此外，建议分布采用文献［７］中的 ｐ（ｘｔ ｜ ｍ （ｉ） ｔ－１ ，ｘ （ｉ） ｔ－１ ，ｚｔ，ｕｔ） ≈ ｐ（ｚｔ ｜ ｍ （ｉ） ｔ－１ ，ｘｔ） ∫ ｘ＇∈Ｌ（ｉ） ｐ（ｚｔ ｜ ｍ （ｉ） ｔ－１ ，ｘ＇）ｄｘ＇ Ｌ （ｉ） ＝ ｛ｘ ｜ ｐ（ｚｔ ｜ ｍ （ｉ） ｔ－１ ，ｘ） ＞ ε｝ 为间距，重采样步骤中使用自适应重采样技术，详见 ［７］，经过采样、计算粒子重要性权值、重采样、更新 地图等步骤实现 ＳＬＡＭ。 改进算法流程示意如图 １ 所示。 在 ｔ － １ 时刻， 采样阶段进行 ｘ ?（ｉ） ｔ－１ ～ Ｐ（ｘｔ－１ ｜ ｘ （ｉ） ｔ－２ ） 得到无权重粒子 ｛ｐ ＾ （ｉ） ｔ－１ ，Ｎ －１ ｝ 。 重要性采样阶段得到带权重的粒子 ｛ｐ ＾ （ｉ） ｔ－１ ，ω ～ （ｉ） ｔ－１ ｝ 。 重采样阶段，保留高权重粒子舍去低 权重粒子得到 ｛ ｐ （ｉ） ｔ－１ ，Ｎ －１ ｝ 。 减小复杂度过程，只负 责更新不同位置的粒子，使用一组 Ｋ （ Ｋ ≤ Ｎ ）个卡 尔曼滤波器计算 μ （ｉ） ｔ 、Σ （ｉ） ｔ ，因此减少了卡尔曼更新 的计算量。 最后，在 ｔ 时刻采样阶段进行 ｘ ?（ｉ） ｔ ～ Ｐ（ｘｔ ｜ ｘ （ｉ） ｔ－１ ） 得到 ｛ｐ ＾ （ｉ） ｔ ，Ｎ －１ ｝ 。 ·４６２· 智 能 系 统 学 报 第 １０ 卷
第3期 罗元，等：改进RBPP的移动机器人同步定位与地图构建 ·463. 杂度较小，计算所需时间因此较小，较快的运动速度 采样（在1-1时刻）p%W') 下也能保证地图精度。2种算法均需120个粒子， 重要性权值{p,g} 每种算法每种速度作为1组进行10次实验，共进行 重采样{p,N} 60次实验。每组第1次实验中使用机器人实地运 KF四KF.KF网 ·卡尔曼更新以，到 行，得出地图同时将观测数据（里程计数据与激光 减小复杂度更新 数据)记录在bag文件中，其余9次实验使用该bag 采样（在时刻）{pN) 文件中的数据进行仿真，得出相应地图。bag文件 为ROS特有的文件格式，可以将各类数据信息存储 重要性权值{p:a骨 在一个文件内，在调试算法过程中使用广泛。使用 图1改进RBPF算法流程 bag文件进行算法调试可以保证每组实验的数据一 Fig.1 The flow chart of improved RBPF algorithm 致，减小误差。实验结果显示每组均出现共同特征， 选取代表性结果进行对比。图4和5为不同运动速 3实验结果及分析 度下Gmapping算法与改进算法所绘地图。 实验平台由装载URG-O4LX激光测距仪的Pio neer II-DX机器人、配有Intel双核、CPU2.l9GHz、 内存1.96GB的笔记本电脑组成，笔记本电脑安装 Ubuntu12.04操作系统与Groovy版本ROS,由笔记 本键盘控制机器人的运动，如图2所示。实验环境 为重庆邮电大学信息无障碍工程研发中心一楼，如 图3所示。 (a)0.2m/s (b)0.4m/s (c)0.8m/s 图43种不同速度下Gmapping算法所绘地图 Fig.4 Maps built with Gmapping in three different velocities 图2实验硬件平台 Fig.2 Experiment hardware platform (a)0.2m/s (b)0.4m/s (c)0.8m/s 图43种不同速度下改进算法所绘地图 Fig.5 Maps builts with improved algorithm in three different velocities 实验表明，2种算法随着机器人运动速度的增 图3实验环境 Fig.3 Experiment environment 加，均出现栅格误差增大、地图质量下降的情况。在 0.2m/s低速运动时，2种方法均能创建稳定的地 在同步定位与地图构建中，总时间消耗与机器 图；以0.4m/s运动时，Gmapping算法开始出现误 人的运动速度和环境大小有关，具体算法的时间消 差，地图出现不确定性，改进算法没有出现误差：以 耗难以确定。但可以通过控制机器人的运动速度来 0.8m/s运动时，Gmapping所绘地图误差增多，改进 衡量算法的实时性：如果算法复杂度过大，计算所需 算法也出现误差，但整体效果明显强于前者。此外， 时间因此较长，较快的运动速度将导致地图的精度 改进算法在不同速度下所建地图更为稳定，边缘更 下降，需放慢运动速度以保证地图精度：如果算法复 加清晰
图 １ 改进 ＲＢＰＦ 算法流程 Ｆｉｇ． １ Ｔｈｅ ｆｌｏｗ ｃｈａｒｔ ｏｆ ｉｍｐｒｏｖｅｄ ＲＢＰＦ ａｌｇｏｒｉｔｈｍ ３ 实验结果及分析 实验平台由装载 ＵＲＧ⁃０４ＬＸ 激光测距仪的 Ｐｉｏ⁃ ｎｅｅｒ ＩＩＩ⁃ＤＸ 机器人、配有 Ｉｎｔｅｌ 双核、ＣＰＵ ２．１９ ＧＨｚ、 内存 １．９６ ＧＢ 的笔记本电脑组成，笔记本电脑安装 Ｕｂｕｎｔｕ １２．０４ 操作系统与 Ｇｒｏｏｖｙ 版本 ＲＯＳ，由笔记 本键盘控制机器人的运动，如图 ２ 所示。 实验环境 为重庆邮电大学信息无障碍工程研发中心一楼，如 图 ３ 所示。 图 ２ 实验硬件平台 Ｆｉｇ． ２ Ｅｘｐｅｒｉｍｅｎｔ ｈａｒｄｗａｒｅ ｐｌａｔｆｏｒｍ 图 ３ 实验环境 Ｆｉｇ． ３ Ｅｘｐｅｒｉｍｅｎｔ ｅｎｖｉｒｏｎｍｅｎｔ 在同步定位与地图构建中，总时间消耗与机器 人的运动速度和环境大小有关，具体算法的时间消 耗难以确定。 但可以通过控制机器人的运动速度来 衡量算法的实时性：如果算法复杂度过大，计算所需 时间因此较长，较快的运动速度将导致地图的精度 下降，需放慢运动速度以保证地图精度；如果算法复 杂度较小，计算所需时间因此较小，较快的运动速度 下也能保证地图精度。 ２ 种算法均需 １２０ 个粒子， 每种算法每种速度作为 １ 组进行 １０ 次实验，共进行 ６０ 次实验。 每组第 １ 次实验中使用机器人实地运 行，得出地图同时将观测数据（里程计数据与激光 数据）记录在 ｂａｇ 文件中，其余 ９ 次实验使用该 ｂａｇ 文件中的数据进行仿真，得出相应地图。 ｂａｇ 文件 为 ＲＯＳ 特有的文件格式，可以将各类数据信息存储 在一个文件内，在调试算法过程中使用广泛。 使用 ｂａｇ 文件进行算法调试可以保证每组实验的数据一 致，减小误差。 实验结果显示每组均出现共同特征， 选取代表性结果进行对比。 图 ４ 和 ５ 为不同运动速 度下 Ｇｍａｐｐｉｎｇ 算法与改进算法所绘地图。 图 ４ ３ 种不同速度下 Ｇｍａｐｐｉｎｇ 算法所绘地图 Ｆｉｇ． ４ Ｍａｐｓ ｂｕｉｌｔ ｗｉｔｈ Ｇｍａｐｐｉｎｇ ｉｎ ｔｈｒｅｅ ｄｉｆｆｅｒｅｎｔ ｖｅｌｏｃｉｔｉｅｓ 图 ４ ３ 种不同速度下改进算法所绘地图 Ｆｉｇ． ５ Ｍａｐｓ ｂｕｉｌｔｓ ｗｉｔｈ ｉｍｐｒｏｖｅｄ ａｌｇｏｒｉｔｈｍ ｉｎ ｔｈｒｅｅ ｄｉｆｆｅｒｅｎｔ ｖｅｌｏｃｉｔｉｅｓ 实验表明，２ 种算法随着机器人运动速度的增 加，均出现栅格误差增大、地图质量下降的情况。 在 ０．２ ｍ ／ ｓ 低速运动时，２ 种方法均能创建稳定的地 图；以 ０．４ ｍ ／ ｓ 运动时，Ｇｍａｐｐｉｎｇ 算法开始出现误 差，地图出现不确定性，改进算法没有出现误差；以 ０．８ ｍ ／ ｓ 运动时，Ｇｍａｐｐｉｎｇ 所绘地图误差增多，改进 算法也出现误差，但整体效果明显强于前者。 此外， 改进算法在不同速度下所建地图更为稳定，边缘更 加清晰。 第 ３ 期 罗元，等：改进 ＲＢＰＦ 的移动机器人同步定位与地图构建 ·４６３·
·464. 智能系统学报 第10卷 [7]GRISETTI G,STACHNISS C,BURGARD W.Improved 4 结束语 techniques for grid mapping with Rao-Blackwellized particle 本文提出了一种改进RBPF算法用于移动机器 filters[J].Robotics,2007,23(1):34-46. 人的同步定位与地图构建，通过卡尔曼滤波更新某 [8]张建伟，张立新，胡颖，等.开源机器人操作系统一ROS 一特定状态一组粒子中的代表粒子，在必要时更新 [M].北京：科学出版社，2012：1-6. 其均值和方差，并在同一粒子群中重复使用，以降低 [9]GERKEY B.Gmapping[EB/OL].[2010-08-05].http:// 复杂度提高系统实时性。在近年来国外使用越来越 wiki.ros.org/slam_gmapping. [10]GRISETTI G,STACHNISS C,BURGARD W.Improving 广泛的机器人操作系统平台下进行实验，实验结果 grid-based slam with Rao-Blackwellized particle filters by 验证了其有效性。在后续工作中，将致力于量化降 adaptive proposals and selective resampling[C]//Proceed- 低的复杂度以及所提高的实时性。 ings of the 2005 IEEE International Conference on Robotics 参考文献： and Automation.Barcelona,Spain,2005:2432-2437. [11]DOUCET A,De FREITAS N,GORDON N.An introduc- [1]SMITH R,SELF M,CHEESEMAN P.Estimating uncertain tion to sequential Monte Carlo methods[M]//DOUCET A, spatial relationships in robotics[M]//COX I J,WILFONG DE FREITAS N,GORDON N.Sequential Monte Carlo G T.Autonomous Robot Vehicles.New York:Springer, Methods in Practice.New York:Springer,2001:3-14. 1990:167-193. [12]De FREITAS N.Rao-Blackwellised particle filtering for [2]DISSANAYAKE G,HUANG S,WANG Z,et al.A review fault diagnosis [C]//2002 IEEE Aerospace Conference of recent developments in simultaneous localization and map- Proceedings.Big Sky,USA,2002,4:1767-1772. ping[C]//6th International Conference on Industrial and 作者简介： Information Systems (ICIIS).Kandy,Sri Lanka,2011: 罗元，女，1972年生，教授，博士，主 477-482. 要研究方向为机器视觉、数字图像处 [3]SMITH R C,CHEESEMAN P.On the representation and 理。获得国家发明专利6项，重庆市科 estimation of spatial uncertainty[].The International Jour- 技进步奖1项。发表学术论文50余 篇，被SC1,EI检索20余篇。 nal of Robotics Research,1986,5(4):56-68. [4]张文玲，朱明清，陈宗海.基于强跟踪UKF的自适应 SLAM算法[J].机器人，2010,32(2)：190-195. 余佳航，男，1990年生，硕士研究 ZHANG Wenling,ZHU Mingqing,CHEN Zonghai.An a- 生，主要研究方向为移动机器人导航。 daptive SLAM algorithm based on strong tracking UKF[J]. Robot,2010.32(2):190-195. [5]DOUCET A,De FREITAS N,MURPHY K,et al.Rao- Blackwellised particle filtering for dynamic Bayesian net- works[C//Proceedings of the Sixteenth Conference on Un- certainty in Artificial Intelligence.San Francisco,USA, 汪龙峰，男，1989年生，硕士研究 生，主要研究方向为移动机器人导航。 2000:176-183. [6]QU Liping,WANG Hongjian.An overview of robot SLAM problem[C]//International Conference on Consumer Elec- tronics,Communications and Networks (CECNet).Xian- ning,China,2011:1953-1956
４ 结束语 本文提出了一种改进 ＲＢＰＦ 算法用于移动机器 人的同步定位与地图构建，通过卡尔曼滤波更新某 一特定状态一组粒子中的代表粒子，在必要时更新 其均值和方差，并在同一粒子群中重复使用，以降低 复杂度提高系统实时性。 在近年来国外使用越来越 广泛的机器人操作系统平台下进行实验，实验结果 验证了其有效性。 在后续工作中，将致力于量化降 低的复杂度以及所提高的实时性。 参考文献： ［１］ＳＭＩＴＨ Ｒ， ＳＥＬＦ Ｍ， ＣＨＥＥＳＥＭＡＮ Ｐ． Ｅｓｔｉｍａｔｉｎｇ ｕｎｃｅｒｔａｉｎ ｓｐａｔｉａｌ ｒｅｌａｔｉｏｎｓｈｉｐｓ ｉｎ ｒｏｂｏｔｉｃｓ［Ｍ］ ／ ／ ＣＯＸ Ｉ Ｊ， ＷＩＬＦＯＮＧ Ｇ Ｔ． Ａｕｔｏｎｏｍｏｕｓ Ｒｏｂｏｔ Ｖｅｈｉｃｌｅｓ． Ｎｅｗ Ｙｏｒｋ： Ｓｐｒｉｎｇｅｒ， １９９０： １６７⁃１９３． ［２］ＤＩＳＳＡＮＡＹＡＫＥ Ｇ， ＨＵＡＮＧ Ｓ， ＷＡＮＧ Ｚ， ｅｔ ａｌ． Ａ ｒｅｖｉｅｗ ｏｆ ｒｅｃｅｎｔ ｄｅｖｅｌｏｐｍｅｎｔｓ ｉｎ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐ⁃ ｐｉｎｇ［ Ｃ］ ／ ／ ６ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｄｕｓｔｒｉａｌ ａｎｄ Ｉｎｆｏｒｍａｔｉｏｎ Ｓｙｓｔｅｍｓ （ ＩＣＩＩＳ）． Ｋａｎｄｙ， Ｓｒｉ Ｌａｎｋａ， ２０１１： ４７７⁃４８２． ［３］ ＳＭＩＴＨ Ｒ Ｃ， ＣＨＥＥＳＥＭＡＮ Ｐ． Ｏｎ ｔｈｅ ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ａｎｄ ｅｓｔｉｍａｔｉｏｎ ｏｆ ｓｐａｔｉａｌ ｕｎｃｅｒｔａｉｎｔｙ［Ｊ］． Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒ⁃ ｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ， １９８６， ５（４）： ５６⁃６８． ［４］张文玲， 朱明清， 陈宗海． 基于强跟踪 ＵＫＦ 的自适应 ＳＬＡＭ 算法［Ｊ］． 机器人， ２０１０， ３２（２）： １９０⁃１９５． ＺＨＡＮＧ Ｗｅｎｌｉｎｇ， ＺＨＵ Ｍｉｎｇｑｉｎｇ， ＣＨＥＮ Ｚｏｎｇｈａｉ． Ａｎ ａ⁃ ｄａｐｔｉｖｅ ＳＬＡＭ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｓｔｒｏｎｇ ｔｒａｃｋｉｎｇ ＵＫＦ［ Ｊ］． Ｒｏｂｏｔ， ２０１０， ３２（２）： １９０⁃１９５． ［５］ ＤＯＵＣＥＴ Ａ， Ｄｅ ＦＲＥＩＴＡＳ Ｎ， ＭＵＲＰＨＹ Ｋ， ｅｔ ａｌ． Ｒａｏ⁃ Ｂｌａｃｋｗｅｌｌｉｓｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｉｎｇ ｆｏｒ ｄｙｎａｍｉｃ Ｂａｙｅｓｉａｎ ｎｅｔ⁃ ｗｏｒｋｓ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ Ｓｉｘｔｅｅｎｔｈ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｕｎ⁃ ｃｅｒｔａｉｎｔｙ ｉｎ Ａｒｔｉｆｉｃｉａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ． Ｓａｎ Ｆｒａｎｃｉｓｃｏ， ＵＳＡ， ２０００： １７６⁃１８３． ［６］ ＱＵ Ｌｉｐｉｎｇ， ＷＡＮＧ Ｈｏｎｇｊｉａｎ． Ａｎ ｏｖｅｒｖｉｅｗ ｏｆ ｒｏｂｏｔ ＳＬＡＭ ｐｒｏｂｌｅｍ［Ｃ］ ／ ／ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｎｓｕｍｅｒ Ｅｌｅｃ⁃ ｔｒｏｎｉｃｓ， Ｃｏｍｍｕｎｉｃａｔｉｏｎｓ ａｎｄ Ｎｅｔｗｏｒｋｓ （ ＣＥＣＮｅｔ）． Ｘｉａｎ⁃ ｎｉｎｇ， Ｃｈｉｎａ， ２０１１： １９５３⁃１９５６． ［７］ ＧＲＩＳＥＴＴＩ Ｇ， ＳＴＡＣＨＮＩＳＳ Ｃ， ＢＵＲＧＡＲＤ Ｗ． Ｉｍｐｒｏｖｅｄ ｔｅｃｈｎｉｑｕｅｓ ｆｏｒ ｇｒｉｄ ｍａｐｐｉｎｇ ｗｉｔｈ Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｓ［Ｊ］． Ｒｏｂｏｔｉｃｓ， ２００７， ２３（１）： ３４⁃４６． ［８］张建伟， 张立新， 胡颖， 等． 开源机器人操作系统—ＲＯＳ ［Ｍ］． 北京： 科学出版社， ２０１２： １⁃６． ［９］ＧＥＲＫＥＹ Ｂ． Ｇｍａｐｐｉｎｇ［ＥＢ／ ＯＬ］． ［ ２０１０⁃０８⁃０５］． ｈｔｔｐ： ／ ／ ｗｉｋｉ．ｒｏｓ．ｏｒｇ ／ ｓｌａｍ＿ｇｍａｐｐｉｎｇ． ［１０］ＧＲＩＳＥＴＴＩ Ｇ， ＳＴＡＣＨＮＩＳＳ Ｃ， ＢＵＲＧＡＲＤ Ｗ． Ｉｍｐｒｏｖｉｎｇ ｇｒｉｄ⁃ｂａｓｅｄ ｓｌａｍ ｗｉｔｈ Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｓ ｂｙ ａｄａｐｔｉｖｅ ｐｒｏｐｏｓａｌｓ ａｎｄ ｓｅｌｅｃｔｉｖｅ ｒｅｓａｍｐｌｉｎｇ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄ⁃ ｉｎｇｓ ｏｆ ｔｈｅ ２００５ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｂａｒｃｅｌｏｎａ， Ｓｐａｉｎ， ２００５： ２４３２⁃２４３７． ［１１］ＤＯＵＣＥＴ Ａ， Ｄｅ ＦＲＥＩＴＡＳ Ｎ， ＧＯＲＤＯＮ Ｎ． Ａｎ ｉｎｔｒｏｄｕｃ⁃ ｔｉｏｎ ｔｏ ｓｅｑｕｅｎｔｉａｌ Ｍｏｎｔｅ Ｃａｒｌｏ ｍｅｔｈｏｄｓ［Ｍ］ ／ ／ ＤＯＵＣＥＴ Ａ， ＤＥ ＦＲＥＩＴＡＳ Ｎ， ＧＯＲＤＯＮ Ｎ． Ｓｅｑｕｅｎｔｉａｌ Ｍｏｎｔｅ Ｃａｒｌｏ Ｍｅｔｈｏｄｓ ｉｎ Ｐｒａｃｔｉｃｅ． Ｎｅｗ Ｙｏｒｋ： Ｓｐｒｉｎｇｅｒ， ２００１： ３⁃１４． ［１２］ Ｄｅ ＦＲＥＩＴＡＳ Ｎ． Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｓｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｉｎｇ ｆｏｒ ｆａｕｌｔ ｄｉａｇｎｏｓｉｓ ［ Ｃ］ ／ ／ ２００２ ＩＥＥＥ Ａｅｒｏｓｐａｃｅ Ｃｏｎｆｅｒｅｎｃｅ Ｐｒｏｃｅｅｄｉｎｇｓ． Ｂｉｇ Ｓｋｙ， ＵＳＡ， ２００２， ４： １７６７⁃１７７２． 作者简介： 罗元，女，１９７２ 年生，教授，博士，主 要研究方向为机器视觉、数字图像处 理。 获得国家发明专利 ６ 项，重庆市科 技进步奖 １ 项。 发表学术论文 ５０ 余 篇，被 ＳＣＩ、ＥＩ 检索 ２０ 余篇。 余佳航，男，１９９０ 年生，硕士研究 生，主要研究方向为移动机器人导航。 汪龙峰，男，１９８９ 年生，硕士研究 生，主要研究方向为移动机器人导航。 ·４６４· 智 能 系 统 学 报 第 １０ 卷