第13卷第5期 智能系统学报 Vol.13 No.5 2018年10月 CAAI Transactions on Intelligent Systems Oct.2018 D0:10.11992/tis.201705006 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.TP.20170728.1854.002.html 量子粒子群优化下的RBPF-SLAM算法研究 伍永健，陈跃东，陈孟元 (安徽工程大学安徽省电气传动与控制重点实验室，安徽芜湖241000) 摘要：为了解决传统Rao-Blackwellized粒子滤波(RBPF)存在提议分布精度不高以及重采样过程出现的粒子 退化和多样性丢失问题，提出一种量子粒子群(QPSO)优化下的Rao-Blackwellized粒子滤波同时定位与地图构 建(RBPF-SLAM)算法。将机器人运动模型和观测模型融合作为混合提议分布，提高提议分布的精度；在重采 样过程中引入量子粒子群优化算法更新粒子位姿，根据权值划分粒子种类，引入自适应交叉变异操作，对所得 粒子集进行优化、调整，有效地防止粒子退化以及保持粒子的多样性。利用本文算法不仅用MATLAB进行仿 真实验，而且结合了旅行家2号移动机器人在机器人操作系统(ROS)上进行实际验证。结果表明，本文算法能 以较少粒子数精确估计出机器人的位姿和高精度的地图，误差和运行时间也大大降低了。 关键词：Rao-Blackwellized粒子滤波：同时定位与地图构建：提议分布：量子粒子群优化：交叉变异；移动机器人： 机器人操作系统 中图分类号：TP21,TP24文献标志码：A文章编号：1673-4785(2018)05-0829-07 中文引用格式：伍永健，陈跃东，陈孟元.量子粒子群优化下的RBPF-SLAM算法研究J.智能系统学报，2018,13(5)： 829-835. 英文引用格式：VU Yongjian,CHEN Yuedong,.CHEN Mengyuan.Research on RBPF-SLAM algorithm based on quantum- behaved particle swarm optimization[Jl.CAAI transactions on intelligent systems,2018,13(5):829-835. Research on RBPF-SLAM algorithm based on quantum-behaved particle swarm optimization WU Yongjian,CHEN Yuedong,CHEN Mengyuan (Anhui Key Laboratory of Electric Drive and Control,Anhui Polytechnic University,Wuhu 241000,China) Abstract:The traditional Rao-Blackwellized particle filter(RBPF)is associated with a low distribution accuracy as well as particle degeneracy and loss of diversity during resampling.To solve these problems,a combination of RBPF and simultaneous localization and mapping(RBPF-SLAM)algorithm based on quantum-behaved particle swarm optimiza- tion(QPSO)is proposed.A fusion of robot motion model and observation model is proposed as a hybrid distribution to improve accuracy.The QPSO algorithm updates the pose of particles in the resampling process according to the weight measurement of particle type,and an adaptive crossover and mutation operation is introduced to optimize and adjust the particle set to effectively prevent particle degradation and maintain particle diversity.To verify the effectiveness of the improved algorithm,a simulation experiment is performed on MATLAB,as well as a Voyager-II mobile robot in a ro- bot operating system(ROS).The results show that the proposed algorithm can accurately estimate the position and pose of the robot and a high precision map,and error and running time are also greatly reduced. Keywords:RBPF;simultaneous localization and map building;proposed distribution;quantum-behaved particle swarm optimization;crossover and mutation;mobile robot:ROS 收稿日期：2017-05-08.网络出版日期：2017-07-28. 基金项目：2016年度安徽高校自然科学项目(KJ2016A794): 地图构建作为机器人自主导航的基础，是指 2016年安徽工程大学研究生实践与创新基金项目 (Y040116004). 移动机器人在未知环境下依据自身携带的传感器 通信作者：伍永健.E-mail:2569513970@qq.com. 信息建立地图模型。常用的地图模型有栅格地
DOI: 10.11992/tis.201705006 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20170728.1854.002.html 量子粒子群优化下的 RBPF-SLAM 算法研究 伍永健，陈跃东，陈孟元 （安徽工程大学 安徽省电气传动与控制重点实验室，安徽 芜湖 241000） 摘 要：为了解决传统 Rao-Blackwellized 粒子滤波 (RBPF) 存在提议分布精度不高以及重采样过程出现的粒子 退化和多样性丢失问题，提出一种量子粒子群 (QPSO) 优化下的 Rao-Blackwellized 粒子滤波同时定位与地图构 建 (RBPF-SLAM) 算法。将机器人运动模型和观测模型融合作为混合提议分布，提高提议分布的精度；在重采 样过程中引入量子粒子群优化算法更新粒子位姿，根据权值划分粒子种类，引入自适应交叉变异操作，对所得 粒子集进行优化、调整，有效地防止粒子退化以及保持粒子的多样性。利用本文算法不仅用 MATLAB 进行仿 真实验，而且结合了旅行家 2 号移动机器人在机器人操作系统 (ROS) 上进行实际验证。结果表明，本文算法能 以较少粒子数精确估计出机器人的位姿和高精度的地图，误差和运行时间也大大降低了。 关键词：Rao-Blackwellized 粒子滤波；同时定位与地图构建；提议分布；量子粒子群优化；交叉变异；移动机器人； 机器人操作系统 中图分类号：TP21;TP24 文献标志码：A 文章编号：1673−4785(2018)05−0829−07 中文引用格式：伍永健, 陈跃东, 陈孟元. 量子粒子群优化下的 RBPF-SLAM 算法研究[J]. 智能系统学报, 2018, 13(5): 829–835. 英文引用格式：WU Yongjian, CHEN Yuedong, CHEN Mengyuan. Research on RBPF-SLAM algorithm based on quantumbehaved particle swarm optimization[J]. CAAI transactions on intelligent systems, 2018, 13(5): 829–835. Research on RBPF-SLAM algorithm based on quantum-behaved particle swarm optimization WU Yongjian，CHEN Yuedong，CHEN Mengyuan (Anhui Key Laboratory of Electric Drive and Control, Anhui Polytechnic University, Wuhu 241000, China) Abstract: The traditional Rao-Blackwellized particle filter (RBPF) is associated with a low distribution accuracy as well as particle degeneracy and loss of diversity during resampling. To solve these problems, a combination of RBPF and simultaneous localization and mapping (RBPF-SLAM) algorithm based on quantum-behaved particle swarm optimization (QPSO) is proposed. A fusion of robot motion model and observation model is proposed as a hybrid distribution to improve accuracy. The QPSO algorithm updates the pose of particles in the resampling process according to the weight measurement of particle type, and an adaptive crossover and mutation operation is introduced to optimize and adjust the particle set to effectively prevent particle degradation and maintain particle diversity. To verify the effectiveness of the improved algorithm, a simulation experiment is performed on MATLAB, as well as a Voyager-II mobile robot in a robot operating system (ROS). The results show that the proposed algorithm can accurately estimate the position and pose of the robot and a high precision map, and error and running time are also greatly reduced. Keywords: RBPF; simultaneous localization and map building; proposed distribution; quantum-behaved particle swarm optimization; crossover and mutation; mobile robot; ROS 地图构建作为机器人自主导航的基础，是指 移动机器人在未知环境下依据自身携带的传感器 信息建立地图模型[1]。常用的地图模型有栅格地 收稿日期：2017−05−08. 网络出版日期：2017−07−28. 基金项目：2016 年度安徽高校自然科学项目 (KJ2016A794)； 2016 年安徽工程大学研究生实践与创新基金项目 (Y040116004). 通信作者：伍永健. E-mail: 2569513970@qq.com. 第 13 卷第 5 期 智 能 系 统 学 报 Vol.13 No.5 2018 年 10 月 CAAI Transactions on Intelligent Systems Oct. 2018
·830· 智能系统学报 第13卷 图四、几何地图以及拓扑地图等。然而，地图 和粒子数的条件下获得可靠的估计，整体性能得 构建问题与定位问题紧密相关，定位的结果用于 到较大提高，能够精确估计出机器人的位姿并获 地图构建，而已经构建好的地图又能实现精确定 得高精度的地图。 位，因此同时定位与地图构建(SLAM例被提出并 受到广泛研究和应用。目前，SLAM技术大都基 1 RBPF-SLAM 于概率理论，比如卡尔曼滤波及扩展卡尔曼滤 移动机器人SLAM实质上是一个Markov链的 波、最大似然估计网、粒子滤波9和Markov定位o 过程：在一个未知环境中机器人从起始位置出发， 等。文献11]将UT和UKF运用到高斯项更新及 在运动过程中，使用里程计记录自身运动的信息 采样粒子权重计算过程中，提出一种无迹高斯混 (1=u1,2,…,山)和外部传感器获取的环境信息 合概率假设密度SLAM算法；文献[12]在雁群粒 (亿u=z1,z2,,z,估计机器人的轨迹(1u=x1,x2,…,x) 子群算法的基础上采用分数阶微积分和混沌思想 与构建增量式环境地图m,同时使用创建好的地 训练模糊自适应扩展卡尔曼滤波，从而实现同时 图及传感器的信息实现自定位。根据贝叶斯滤波 定位与地图创建；文献[I3]在基本SLAM算法的 递归原理，从概率学的角度得出SLAM的递归公 迭代过程中引入元胞自动机(CA),建立“SLAM- 式为 CA生长-重定位”的闭环作用机制。 Bel(x,m)=p(xmu)=np(m Rao-Blackwellized粒子滤波算法是目前解决 )-Bel(m SLAM问题的有效方法，它将SLAM问题分解成 机器人的位姿估计和地图估计，采用粒子滤波器 式中表示归一化常量。 和扩展卡尔曼滤波器估计概率，但仍存在算法运 SLAM中包含运动模型p(x,x-1,4-)与观测 模型p(z,x,m)两种模型。运动模型表示在给定上 行时间长，粒子退化严重等不足；此后很多改进 一时刻移动机器人轨迹x-1和控制命令u-1条件 的RBPF算法被提出，如文献[14]提出一种基于高 下，机器人获得新位姿x,的概率密度；而观测模型 斯分布的RBPF-SLAM算法，通过高斯分布分散 表示在给定移动机器人地图m与位姿x,的条件 高权重粒子获得新粒子，虽然算法能在粒子减少 下，传感器获取环境的不确定性。 的条件下保持可靠估计，但忽略对低权重粒子的 基于Rao-Blackwellized粒子滤波SLAM算法 考虑，抑制样本匮乏现象还存在不足；文献[15]提 的思想：计算机器人轨迹x和地图m的后验概率 出一种粒子群优化遗传重采样的改进RBPF-SLAM p(x1,mlz1a,1-1),将其分解为式(2)所示的轨迹估 算法，采用粒子群优化策略调整粒子集，并对权 计和地图估计两个后验概率乘积。 重较小的粒子进行变异操作，但粒子群的引入容 p(m)=p)p(m)(2) 易陷入局部最优，加上重采样中只针对权重较小 首先对机器人的轨迹进行估计，利用Rao- 粒子操作，对缓解粒子退化无法产生满意的效 Blackwellized粒子滤波器实现，其中每个粒子代 果。文献16]采用改进的提议分布并结合基于等 表机器人一条可能的行走轨迹。 级的自适应局部重采样(APRR)算法，设计了一 然后再结合观测模型对地图进行更新。将地 种基于退火参数优化混合提议分布的RBPF算 图表示为服从高斯分布的特征路标的集合，因此 法，对高权重和低权重粒子只进行复制操作，对 对地图的估计可通过特征路标估计得到，这里采 增加粒子多样性缓解粒子退化效果不佳。 用扩展卡尔曼滤波来实现。 考虑这些不足，本文从解决RBPF算法运行 因此，在粒子代表的轨迹上利用传感器实时 时间长、提议分布精度不高以及重采样过程的粒 观察获得的路标信息构成最后的地图。 子退化出发，将量子粒子群算法引入到Rao 利用Rao-Blackwellized滤波器在传感器观测 Blackwellized粒子滤波算法，提出一种QPSO- 信息与里程计信息基础下构建增量式地图的步骤 RBPF-SLAM算法，一方面在基本提议分布中加 可以分为4步： 入观测信息，使改进的提议分布更加接近真实状 1)初始化：当=0时，选取N个粒子，每个粒 态，另一方面在重采样中根据QPSO算法更新粒 子的权重为o。=1N。 子位姿，对高低权值粒子进行自适应交叉变异操 2)采样：机器人的运动模型作为提议分布π，从 作。QPSO-RBPF-SLAM保持了粒子的多样性，有 上一代粒子集合x2采样得到下一代粒子集合{x。 效缓解了粒子退化，同时算法能在减少运算时间 3)粒子权重：为了弥补采样时提议分布跟目
图 [2] 、几何地图[3]以及拓扑地图[4]等。然而，地图 构建问题与定位问题紧密相关，定位的结果用于 地图构建，而已经构建好的地图又能实现精确定 位，因此同时定位与地图构建 (SLAM[5] ) 被提出并 受到广泛研究和应用。目前，SLAM 技术大都基 于概率理论，比如卡尔曼滤波[6]及扩展卡尔曼滤 波 [7] 、最大似然估计[8] 、粒子滤波[9]和 Markov 定位[10] 等。文献[11]将 UT 和 UKF 运用到高斯项更新及 采样粒子权重计算过程中，提出一种无迹高斯混 合概率假设密度 SLAM 算法；文献[12]在雁群粒 子群算法的基础上采用分数阶微积分和混沌思想 训练模糊自适应扩展卡尔曼滤波，从而实现同时 定位与地图创建；文献[13]在基本 SLAM 算法的 迭代过程中引入元胞自动机 (CA)，建立“SLAMCA 生长–重定位”的闭环作用机制。 Rao-Blackwellized 粒子滤波算法是目前解决 SLAM 问题的有效方法，它将 SLAM 问题分解成 机器人的位姿估计和地图估计，采用粒子滤波器 和扩展卡尔曼滤波器估计概率，但仍存在算法运 行时间长，粒子退化严重等不足；此后很多改进 的 RBPF 算法被提出，如文献[14]提出一种基于高 斯分布的 RBPF-SLAM 算法，通过高斯分布分散 高权重粒子获得新粒子，虽然算法能在粒子减少 的条件下保持可靠估计，但忽略对低权重粒子的 考虑，抑制样本匮乏现象还存在不足；文献[15]提 出一种粒子群优化遗传重采样的改进 RBPF-SLAM 算法，采用粒子群优化策略调整粒子集，并对权 重较小的粒子进行变异操作，但粒子群的引入容 易陷入局部最优，加上重采样中只针对权重较小 粒子操作，对缓解粒子退化无法产生满意的效 果。文献[16]采用改进的提议分布并结合基于等 级的自适应局部重采样 (APRR) 算法，设计了一 种基于退火参数优化混合提议分布的 RBPF 算 法，对高权重和低权重粒子只进行复制操作，对 增加粒子多样性缓解粒子退化效果不佳。 考虑这些不足，本文从解决 RBPF 算法运行 时间长、提议分布精度不高以及重采样过程的粒 子退化出发，将量子粒子群算法[17]引入到 RaoBlackwellized 粒子滤波算法，提出一种 QPSORBPF-SLAM 算法，一方面在基本提议分布中加 入观测信息，使改进的提议分布更加接近真实状 态，另一方面在重采样中根据 QPSO 算法更新粒 子位姿，对高低权值粒子进行自适应交叉变异操 作。QPSO-RBPF-SLAM 保持了粒子的多样性，有 效缓解了粒子退化，同时算法能在减少运算时间 和粒子数的条件下获得可靠的估计，整体性能得 到较大提高，能够精确估计出机器人的位姿并获 得高精度的地图。 1 RBPF-SLAM u1:t = u1,u2,··· ,ut z1:t = z1,z2,···,zt x1:t = x1, x2,···, xt mt 移动机器人 SLAM 实质上是一个 Markov 链的 过程：在一个未知环境中机器人从起始位置出发， 在运动过程中，使用里程计记录自身运动的信息 ( ) 和外部传感器获取的环境信息 ( )，估计机器人的轨迹( ) 与构建增量式环境地图 ，同时使用创建好的地 图及传感器的信息实现自定位。根据贝叶斯滤波 递归原理，从概率学的角度得出 SLAM 的递归公 式为 Bel(xt , mt) = p(xt , mt |z1:t ,u1:t−1) = ηp(zt |xt " , mt )· p(xt , mt |xt−1 , mt−1 ,ut−1)·Bel(xt−1 , mt−1)dxt−1dmt−1 (1) 式中 η 表示归一化常量。 p(xt |xt−1 ,ut−1) p(zt |xt , m) xt−1 ut−1 xt xt SLAM 中包含运动模型 与观测 模型 两种模型。运动模型表示在给定上 一时刻移动机器人轨迹 和控制命令 条件 下，机器人获得新位姿 的概率密度；而观测模型 表示在给定移动机器人地图 m 与位姿 的条件 下，传感器获取环境的不确定性。 x1:t p(x1:t , m|z1:t ,u1:t−1) 基于 Rao-Blackwellized 粒子滤波 SLAM 算法 的思想：计算机器人轨迹 和地图 m 的后验概率 ，将其分解为式 (2) 所示的轨迹估 计和地图估计两个后验概率乘积。 p(x1:t , m|z1:t ,u1:t−1) = p(x1:t |z1:t ,u1:t−1)· p(m|x1:t ,z1:t ) (2) 首先对机器人的轨迹进行估计，利用 RaoBlackwellized 粒子滤波器实现，其中每个粒子代 表机器人一条可能的行走轨迹。 然后再结合观测模型对地图进行更新。将地 图表示为服从高斯分布的特征路标的集合，因此 对地图的估计可通过特征路标估计得到，这里采 用扩展卡尔曼滤波来实现。 因此，在粒子代表的轨迹上利用传感器实时 观察获得的路标信息构成最后的地图。 利用 Rao-Blackwellized 滤波器在传感器观测 信息与里程计信息基础下构建增量式地图的步骤 可以分为 4 步： ω (i) 0 = 1/N 1) 初始化：当 t=0 时，选取 N 个粒子，每个粒 子的权重为 。 π { x (i) t−1 } { x (i) t } 2) 采样：机器人的运动模型作为提议分布 ，从 上一代粒子集合 采样得到下一代粒子集合 。 3) 粒子权重：为了弥补采样时提议分布跟目 ·830· 智 能 系 统 学 报 第 13 卷
第5期 伍永健，等：量子粒子群优化下的RBPF-SLAM算法研究 ·831· 标分布的差距，需要计算每一个独立粒子的权重 量子系统的特点与粒子群算法相结合的新兴群体 ”,由重要性采样公式得出式(3)： 智能算法，将粒子群引入量子空间并确定粒子在 w9=Pr2lz,4-） 空间中的位置，通过量子束缚态描述粒子的聚集 i=1,2,…,N (3) π(x9lz1u,u-i） 性，保证了粒子在整个可行解区域的搜索，保证 4)重采样：根据式(4)计算有效粒子数，同时 收敛到全局最优解。利用QPSO算法驱使粒子快 设定阈值Nh 速地靠近于似然函数高的区域，优化调整机器人 Na-u> 位姿状态的粒子集x,则粒子位置更新如式(8) (4) 所示： 当Nea<N时，进行重采样。重采样完成后保 X(+1)=p/()+a.mbest()-X.()In1 证所有的粒子具有相同的权重。其中，心表示归 ·化权重。 p)=ft0)P0+1-ft)·P) (8) 5)地图更新：每一粒子用其轨迹x和观 4()U0,1) ft)~U0,1) 测信息zu来计算相应的p(mox,z)实现地图的 式中：X)、Xt+1)分别表示当前时刻和下一时 更新。 刻粒子的位置；p0)表示吸引子；P0表示第1次 2 QPSO-RBPF-SLAM 迭代时第i个粒子的个体最好位置；P①表示第 1次迭代时所有粒子中的全局最好位置；mbest,() 在重要性采样中，需要依据提议分布对下一 表示第1次迭代时所有粒子当前最好位置的平均 代粒子集进行采样，而基本RBPF-SLAM中常采 值；α为收缩-扩张系数。 用运动模型作为提议分布，使得粒子退化严重， 同时，为了防止粒子退化以及保持粒子的多 导致丢失权值较大的粒子从而创建的地图精度不 样性，对所得的粒子集进行优化调整，基本思想 高。为了解决提议分布精度不高的问题，结合里 是：根据权重阈值将粒子划分为高权重粒子、低 程计信息和外部传感观测信息作为混合提议分布 权重粒子以及中等权重粒子，保留中等权重粒 如式(5)所示： 子，对具有高权重和低权重的粒子进行自适应交 txmg,2za-=pempxk2u-） 叉变异操作。根据式(9)设置合适的高权重阈值 p(zm21,x2,u-) w:,和低权重阈值L,取两阈值之间的粒子作为中 (5) 等权重的粒子。 然而此混合提议分布无法直接进行采样操 2 1 作，需要目标提议分布的一个近似化正态分布实 =N'=2N (9) 现。式(6)所示的正态分布参数(，∑)通过带 选择F(X)=作为粒子的适应度函数，则 权重的蒙特卡罗采样方法估计得出。 1时刻的交叉操作与变异操作如下。 交叉操作：在高权重和低权重的粒子集中随 =∑pkmg,s) 1 机选取两个个体作为父辈粒子进行配对，按照式(10) 所示的自适应交叉率P进行交叉操作得到两个新 个体。 (6) (Pa, F'< 式中-之62为归-化因不，M表示 Pa-Pe2 P= P2+ I2F'-Fw） F'≥Fawg(10) 1+expB 常数。 Fmax -Favg 在混合提议分布下，粒子权重通过式(7)得出： 变异操作：从交叉后得到的新粒子集中随机 选择的一个父辈粒子按照式(11)所示自适应变异 u=9k∑pzm2,x)=k (7) 率Pm进行变异操作产生新粒子。 Pm: F<Fag 式中k表示常数。 Pm -Pm2 基本粒子群优化算法(PSO)由于粒子速度的 J2(F-Faw) F≥Fawg(11) 局限性而不能在整个可行空间进行搜索，无法保 1+exp Fa3x-FaVg 证算法全局收敛，故在重采样过程中引人量子粒 式中：F表示每代群体中粒子的平均适应度值； 子群算法更新粒子集。量子粒子群算法是一种将 Fm表示群体中粒子的最大适应度值；F'、F分别
ω (i) t 标分布的差距，需要计算每一个独立粒子的权重 ，由重要性采样公式得出式 (3)： ω (i) t = p(x (i) 1:t |z1:t ,u1:t−1) π(x (i) 1:t |z1:t ,u1:t−1) , i = 1,2, ··· ,N (3) Nth 4) 重采样：根据式 (4) 计算有效粒子数，同时 设定阈值 。 Neff = 1/ ∑N i=1 (ωf(i) ) 2 (4) Neff < Nth ωf(i) 当 时，进行重采样。重采样完成后保 证所有的粒子具有相同的权重。其中， 表示归 一化权重。 x (i) 1:t z1:t p(m(i) x (i) 1:t ,z1:t ) 5) 地图更新：每一粒子用其轨迹 和 观 测信息 来计算相应的 实现地图的 更新。 2 QPSO-RBPF-SLAM 在重要性采样中，需要依据提议分布对下一 代粒子集进行采样，而基本 RBPF-SLAM 中常采 用运动模型作为提议分布，使得粒子退化严重， 导致丢失权值较大的粒子从而创建的地图精度不 高。为了解决提议分布精度不高的问题，结合里 程计信息和外部传感观测信息作为混合提议分布 如式 (5) 所示： π ′ (xt m (i) t−1 , x (i) t−1 ,zt ,ut−1) = p(zt m (i) t−1 , xt)· p(xt x (i) t−1 ,ut−1) p(zt m (i) t−1 , x (i) t−1 ,ut−1) (5) N ( u (i) t , ∑(i) t ) 然而此混合提议分布无法直接进行采样操 作，需要目标提议分布的一个近似化正态分布实 现。式 (6) 所示的正态分布参数 通过带 权重的蒙特卡罗采样方法估计得出。 u (i) t = 1 η (i) ∑M j=1 xj · p ( zt m (i) t−1 , xj ) ∑(i) t = 1 η (i) ∑M j=1 p ( zt m (i) t−1 , xj ) · ( xj −u (i) t ) (xj −u (i) t )T (6) η (i) = ∑M j=1 p ( zt m (i) t−1 , xj ) 式中： 为归一化因子，M 表示 常数。 在混合提议分布下，粒子权重通过式 (7) 得出： ω (i) t = ω (i) t−1 · k ∑M j=1 p(zt m (i) t−1 , xj) = ω (i) t−1 · kη (i) (7) 式中 k 表示常数。 基本粒子群优化算法 (PSO) 由于粒子速度的 局限性而不能在整个可行空间进行搜索，无法保 证算法全局收敛，故在重采样过程中引入量子粒 子群算法更新粒子集。量子粒子群算法是一种将 { x (i) t } 量子系统的特点与粒子群算法相结合的新兴群体 智能算法，将粒子群引入量子空间并确定粒子在 空间中的位置，通过量子束缚态描述粒子的聚集 性，保证了粒子在整个可行解区域的搜索，保证 收敛到全局最优解。利用 QPSO 算法驱使粒子快 速地靠近于似然函数高的区域，优化调整机器人 位姿状态的粒子集 ，则粒子位置更新如式 (8) 所示： Xi, j(t+1)= pi, j(t)±α· mbestj(t)− Xi, j(t) ln( 1 ui, j(t) ) pi, j(t) = fj(t)· Pi, j(t)+ 1− fj(t) · Pg, j(t) ui, j(t) ∼ U(0,1) fj(t) ∼ U(0,1) (8) Xi, j(t) Xi, j(t+1) pi, j(t) Pi, j(t) Pg, j(t) mbestj(t) α 式中： 、 分别表示当前时刻和下一时 刻粒子的位置； 表示吸引子； 表示第 t 次 迭代时第 i 个粒子的个体最好位置； 表示第 t 次迭代时所有粒子中的全局最好位置； 表示第 t 次迭代时所有粒子当前最好位置的平均 值; 为收缩–扩张系数。 ωH0 ωL0 同时，为了防止粒子退化以及保持粒子的多 样性，对所得的粒子集进行优化调整，基本思想 是：根据权重阈值将粒子划分为高权重粒子、低 权重粒子以及中等权重粒子，保留中等权重粒 子，对具有高权重和低权重的粒子进行自适应交 叉变异操作。根据式 (9) 设置合适的高权重阈值 和低权重阈值 ，取两阈值之间的粒子作为中 等权重的粒子。 ωH0 = 2 N , ωL0 = 1 2N (9) F(Xi) = ω (i) 选择 t 作为粒子的适应度函数，则 t 时刻的交叉操作与变异操作如下。 Pc 交叉操作：在高权重和低权重的粒子集中随 机选取两个个体作为父辈粒子进行配对，按照式 (10) 所示的自适应交叉率 进行交叉操作得到两个新 个体。 Pc = Pc1 , F ′ < Favg Pc2 + Pc1 − Pc2 1+exp{ β [ 2(F ′ − Favg) Fmax − Favg ]}, F ′ ⩾ Favg (10) Pm 变异操作：从交叉后得到的新粒子集中随机 选择的一个父辈粒子按照式 (11) 所示自适应变异 率 进行变异操作产生新粒子。 Pm = Pm1, F < Favg Pm2 + Pm1 − Pm2 1+exp{ β [ 2(F − Favg) Fmax − Favg ]}, F ⩾ Favg (11) Favg Fmax F ′ 式中： 表示每代群体中粒子的平均适应度值； 表示群体中粒子的最大适应度值； 、F 分别 第 5 期 伍永健，等：量子粒子群优化下的 RBPF-SLAM 算法研究 ·831·
·832· 智能系统学报 第13卷 表示参与交叉操作的两个粒子中较大的适应度 改进RBPF算法得到的轨迹估计，圆形表示对应 值以及参与变异操作的粒子的适应度值；B表示 的路标估计；虚线表示利用文献[15]算法得到的 常数。 轨迹估计，三角形表示对应的路标估计：点线表 QPSO-RBPF-SLAM算法流程： 示利用RBPF算法得到的轨迹估计，黑色小点表 1)当=0时，选取N个粒子，每个粒子的权重 示对应的路标估计。 为ω。=1N;设置相关参数，即自适应交叉率P以 一真实状态 一文献[15]估计 20 -RBPF估计 改进RBPF估计 及自适应变异率Pm。 15 2)根据式（⑤）计算混合提议分布，进行采样操 10 作得出粒子集。 3)根据式(8)对粒子集{x进行QPSO迭代 优化。 4)根据式(7)计算粒子权重，并依据设定的高 101520253035404550 低权重阈值来划分粒子。 迭代步数 5)根据式(4)重采样条件，判断是否需要进行 (a)W=50 重采样操作。若需要重采样，则执行6)：否则，执 一真实状态 一文献[15]估计 -RBPF估计 …改进RBPF估计 行8) 20 15 6)保留中等权重粒子，将高权重和低权重粒 10 子根据式(10)、式(11)进行自适应交叉和变异 操作。 7)中等权重粒子和交叉变异后的粒子组成 -10 15 新粒子集进行重采样，并返回3)实现QPSO重复 20 5 101520253035404550 优化。 迭代步数 8)根据机器人轨迹x2和观测信息z1更新栅 (b)W=100 格地图。 图1机器人位姿估计 Fig.1 Pose estimation of robot 3实验 表13种算法的对比数据 Table 1 Comparison data of three algorithms 3.1仿真实验 算法 粒子数均方根误差(RMSE) 运行时间/s 为了说明本文改进算法的有效性，在MAT LAB平台进行了仿真实验。 50 2.361 0.343 RBPF 首先对机器人自身位姿估计。设置机器人实 100 2.045 0.561 际行走轨迹中真实的位姿状态，利用基本RBPF 50 2.056 0.396 文献[15] 文献[15]算法和改进RBPF在粒子数N取50和 100 1.472 0.673 100时对真实的位姿进行估计。其中，P=0.8、 50 1.974 0.350 P2=0.6、Pm1=0.1、Pm2=0.01。 改进的RBPF 100 1.386 0.647 由图1和表1可知，在粒子数相同的条件下， 改进RBPF算法的均方根误差较小，与真实状态 实际轨迹 ·实际路标 RBPF轨迹 ·RBPF路标 接近；随着粒子数的增加，虽然算法运行时间延 100 文献[15]轨迹 ·文献5]路标 90 改进RBPF轨迹改进RBPF路标 长，但估计的结果则更加接近真实状态；与RBPF 算法和文献[15]算法采用100个粒子所获得的估 0 60 计结果相比，改进的RBPF算法采用50个粒子能 50 够获得较好的估计效果，故改进的RBP℉算法能 % 30 利用较少的粒子获得可靠且较精确的估计。 2 10 其次，比较RBPF算法、文献[15]算法和改进 RBPF算法下对机器人轨迹和路标的估计结果。 0102030405060708090100 x/cm 如图2所示，设定100cm×100cm的区域，星形表 图2机器人轨迹估计和路标估计 示实际路标，红线表示实际轨迹；黑线表示利用 Fig.2 Robot trajectory estimation and road sign estimation
β 表示参与交叉操作的两个粒子中较大的适应度 值以及参与变异操作的粒子的适应度值； 表示 常数。 QPSO-RBPF-SLAM 算法流程： ω (i) 0 = 1/N Pc Pm 1) 当 t=0 时，选取 N 个粒子，每个粒子的权重 为 ；设置相关参数，即自适应交叉率 以 及自适应变异率 。 2) 根据式 (5) 计算混合提议分布，进行采样操 作得出粒子集。 { x (i) t } 3) 根据式 (8) 对粒子集 进行 QPSO 迭代 优化。 4) 根据式 (7) 计算粒子权重，并依据设定的高 低权重阈值来划分粒子。 5) 根据式 (4) 重采样条件，判断是否需要进行 重采样操作。若需要重采样，则执行 6)；否则，执 行 8)。 6) 保留中等权重粒子，将高权重和低权重粒 子根据式 (10)、式 (11) 进行自适应交叉和变异 操作。 7) 中等权重粒子和交叉变异后的粒子组成 新粒子集进行重采样，并返回 3) 实现 QPSO 重复 优化。 x (i) 1:t 8) 根据机器人轨迹 和观测信息 z1:t更新栅 格地图。 3 实验 3.1 仿真实验 为了说明本文改进算法的有效性，在 MATLAB 平台进行了仿真实验。 Pc1 Pc2 Pm1 Pm2 首先对机器人自身位姿估计。设置机器人实 际行走轨迹中真实的位姿状态，利用基本 RBPF、 文献[15]算法和改进 RBPF 在粒子数 N 取 50 和 100 时对真实的位姿进行估计。其中， =0.8、 =0.6、 =0.1、 =0.01。 由图 1 和表 1 可知，在粒子数相同的条件下， 改进 RBPF 算法的均方根误差较小，与真实状态 接近；随着粒子数的增加，虽然算法运行时间延 长，但估计的结果则更加接近真实状态；与 RBPF 算法和文献[15]算法采用 100 个粒子所获得的估 计结果相比，改进的 RBPF 算法采用 50 个粒子能 够获得较好的估计效果，故改进的 RBPF 算法能 利用较少的粒子获得可靠且较精确的估计。 其次，比较 RBPF 算法、文献[15]算法和改进 RBPF 算法下对机器人轨迹和路标的估计结果。 如图 2 所示，设定 100 cm×100 cm 的区域，星形表 示实际路标，红线表示实际轨迹；黑线表示利用 改进 RBPF 算法得到的轨迹估计，圆形表示对应 的路标估计；虚线表示利用文献[15]算法得到的 轨迹估计，三角形表示对应的路标估计；点线表 示利用 RBPF 算法得到的轨迹估计，黑色小点表 示对应的路标估计。 0 5 10 15 20 25 30 35 40 45 50 −20 −15 −10 −5 0 5 10 15 20 迭代步数 状态 真实状态 RBPF 估计 文献[15]估计 改进 RBPF 估计 真实状态 RBPF 估计 文献[15]估计 改进 RBPF 估计 (a) N=50 0 5 10 15 20 25 30 35 40 45 50 −20 −15 −10 −5 0 5 10 15 20 迭代步数 状态 (b) N=100 图 1 机器人位姿估计 Fig. 1 Pose estimation of robot 表 1 3 种算法的对比数据 Table 1 Comparison data of three algorithms 算法 粒子数 均方根误差 (RMSE) 运行时间/s RBPF 50 2.361 0.343 100 2.045 0.561 文献[15] 50 2.056 0.396 100 1.472 0.673 改进的 RBPF 50 1.974 0.350 100 1.386 0.647 0 20 30 40 50 60 70 80 90 100 10 10 20 30 40 50 60 70 80 90 100 x/cm y/cm 实际轨迹 RBPF 轨迹 文献[15]轨迹 改进 RBPF 轨迹 实际路标 RBPF 路标 文献[15]路标 改进 RBPF 路标 图 2 机器人轨迹估计和路标估计 Fig. 2 Robot trajectory estimation and road sign estimation ·832· 智 能 系 统 学 报 第 13 卷
第5期 伍永健，等：量子粒子群优化下的RBPF-SLAM算法研究 ·833· 由图2和表2可知，改进的RBPF算法在进行 子数获得更好的估计结果，使得算法整体运行时 轨迹和路标估计时所用粒子数和运行时间比RBPF 间降低。整体而言，改进的RBPF算法具有更好 算法和文献[15]算法少。在轨迹估计方面，改进 的有效性和优越性。 的RBPF算法得到的轨迹与机器人实际轨迹误差 200 较小，而RBPF算法和文献[15]算法得到的轨迹波 150 动较大；在路标估计方面，利用改进的RBPF算法 得到的路标估计与实际路标较为接近，而RBPF 100 算法和文献[15]算法得到的路标估计则在一定程 50 度上远离实际路标。因此，与RBPF算法和文献15) 算法相比，改进的RBPF算法在机器人轨迹估计 和路标估计方面能够得到更加满意的效果。 50 表23种算法的对比数据 -100 Table 2 Comparison data of three algorithms 200 -150-100-50 050100 x/m 算法 轨迹RMSE路标RMSE粒子数运行时间/s (a)RBPF仿真结果 RBPF 1.293 1.451 50 3.962 文献15] 0.976 1.154 36 2.471 200 改进的RBPF 0.737 1.047 25 1.363 150 下面利用维多利亚公园数据集对RBPF算 100 法、文献[15]算法和改进的RBPF算法的性能进一 50 步验证。由于悉尼维多利亚公园数据集并未提供 相关噪声参数的信息，故将噪声参数设置为：车 辆速度控制噪声为1.0m/s,驾驶角控制噪声为2.0°； 50 路标观测的角度噪声为2.5°，测距噪声为1.6m。 3种算法分别采用20个粒子、15个粒子和10个 -100 粒子来描述车辆轨迹和环境地图。 -200-150-100-50 050100 x/m RBPF算法、文献[15]算法和改进的RBPF算 b)文献15]仿真结果 法的仿真结果如图3所示。其中，灰色粗线表示GPS 200 路径（即真实路径），黑色细线表示估计路径，黑点 150 表示估计路标。 由图3可知，3种算法在不同程度上估计出GS 100 路径，但RBPF算法采用20个粒子得到的轨迹在 50 部分区域出现明显的不匹配现象，偏差较大；文 献[15]算法采用15个粒子得到的轨迹相比RBPF 算法不匹配现象减少：而改进的RBPF算法采用 -50 10个粒子得到的轨迹与GPS路径之间的误差较小， -100 。 吻合度更高。同时，RBPF算法和文献[I5]算法出现 -200-150-100-50050100 x/m 粒子匮乏问题而导致估计的路标个数不完全，而改 (C)改进的RBPF仿真结果 进的RBPF算法能精确地估计所有设定的路标。 由上述仿真可知，RBPF算法的提议分布缺少 图3维多利亚公园数据集仿真结果 Fig.3 Simulation results based on Vitoria Park data 观测信息且所有粒子都有参与重采样，算法整体 计算过程简单但效果不佳，会出现粒子退化现象 3.2实际验证 导致最后创建的地图精度不高；文献[15]对提议 为了验证本文改进算法的实际性，在室内环 分布进行改进，引入粒子群算法更新粒子集，并 境下利用旅行家2号移动机器人进行实际验证， 对所有权重较低粒子进行重采样，计算复杂度有 完成同时定位与地图构建。该机器人内部有里程 所提升；而改进的RBPF算法通过量子粒子群算 计，并随身携带URG-hokuyo激光传感器。在 法，只考虑粒子的位置量，且针对部分粒子进行 PC机上运行Liunx((Ubuntu12.04)的ROS系统。 重采样，整体计算复杂度介于RBPF算法和文献[15] 选取安徽工程大学电气工程学院实验室部分 算法之间，但由于改进的RBP℉算法能以较少粒 区域作为本次实验的室内环境。如图4所示，选
由图 2 和表 2 可知，改进的 RBPF 算法在进行 轨迹和路标估计时所用粒子数和运行时间比 RBPF 算法和文献[15]算法少。在轨迹估计方面，改进 的 RBPF 算法得到的轨迹与机器人实际轨迹误差 较小，而 RBPF 算法和文献[15]算法得到的轨迹波 动较大；在路标估计方面，利用改进的 RBPF 算法 得到的路标估计与实际路标较为接近，而 RBPF 算法和文献[15]算法得到的路标估计则在一定程 度上远离实际路标。因此，与 RBPF 算法和文献[15] 算法相比，改进的 RBPF 算法在机器人轨迹估计 和路标估计方面能够得到更加满意的效果。 表 2 3 种算法的对比数据 Table 2 Comparison data of three algorithms 算法 轨迹 RMSE 路标 RMSE 粒子数 运行时间/s RBPF 1.293 1.451 50 3.962 文献[15] 0.976 1.154 36 2.471 改进的 RBPF 0.737 1.047 25 1.363 下面利用维多利亚公园数据集对 RBPF 算 法、文献[15]算法和改进的 RBPF 算法的性能进一 步验证。由于悉尼维多利亚公园数据集并未提供 相关噪声参数的信息，故将噪声参数设置为：车 辆速度控制噪声为 1.0 m/s，驾驶角控制噪声为 2.0°； 路标观测的角度噪声为 2.5°，测距噪声为 1.6 m。 3 种算法分别采用 20 个粒子、15 个粒子和 10 个 粒子来描述车辆轨迹和环境地图。 RBPF 算法、文献[15]算法和改进的 RBPF 算 法的仿真结果如图 3 所示。其中，灰色粗线表示 GPS 路径 (即真实路径)，黑色细线表示估计路径，黑点 表示估计路标。 由图 3 可知，3 种算法在不同程度上估计出 GPS 路径，但 RBPF 算法采用 20 个粒子得到的轨迹在 部分区域出现明显的不匹配现象，偏差较大；文 献[15]算法采用 15 个粒子得到的轨迹相比 RBPF 算法不匹配现象减少；而改进的 RBPF 算法采用 10 个粒子得到的轨迹与 GPS 路径之间的误差较小， 吻合度更高。同时，RBPF 算法和文献[15]算法出现 粒子匮乏问题而导致估计的路标个数不完全，而改 进的 RBPF 算法能精确地估计所有设定的路标。 由上述仿真可知，RBPF 算法的提议分布缺少 观测信息且所有粒子都有参与重采样，算法整体 计算过程简单但效果不佳，会出现粒子退化现象 导致最后创建的地图精度不高；文献[15]对提议 分布进行改进，引入粒子群算法更新粒子集，并 对所有权重较低粒子进行重采样，计算复杂度有 所提升；而改进的 RBPF 算法通过量子粒子群算 法，只考虑粒子的位置量，且针对部分粒子进行 重采样，整体计算复杂度介于 RBPF 算法和文献[15] 算法之间，但由于改进的 RBPF 算法能以较少粒 子数获得更好的估计结果，使得算法整体运行时 间降低。整体而言，改进的 RBPF 算法具有更好 的有效性和优越性。 −200 −150 −100 −50 0 50 100 −50 0 50 100 150 200 −100 x/m y/m (a) RBPF 仿真结果 −200 −150 −100 −50 0 50 100 −50 0 50 100 150 200 −100 x/m y/m −200 −150 −100 −50 0 50 100 −50 0 50 100 150 200 −100 x/m y/m (b) 文献[15]仿真结果 (c) 改进的 RBPF 仿真结果 图 3 维多利亚公园数据集仿真结果 Fig. 3 Simulation results based on Vitoria Park data 3.2 实际验证 为了验证本文改进算法的实际性，在室内环 境下利用旅行家 2 号移动机器人进行实际验证， 完成同时定位与地图构建。该机器人内部有里程 计，并随身携带 URG-hokuyo 激光传感器。在 PC 机上运行 Liunx(Ubuntu 12.04) 的 ROS 系统。 选取安徽工程大学电气工程学院实验室部分 区域作为本次实验的室内环境。如图 4 所示，选 第 5 期 伍永健，等：量子粒子群优化下的 RBPF-SLAM 算法研究 ·833·
·834· 智能系统学报 第13卷 取的区域为8m×1.5m,机器人以0.2m/s的速度 由图5和表3可知，RBPF-SALM算法采用了39 移动，利用里程计信息和激光数据信息实时构建 个粒子，粒子退化严重，降低粒子多样性，创建的地 栅格地图。 图不够精确；文献[15]算法采用了24个粒子，创 建的地图有所改善，但效果不显著；改进的RBPF- SALM算法只使用了16个粒子获得了比RBPF-SALM 算法和文献[15]算法更精确的地图，同时轨迹和 路标估计的均方根误差、运行时间也大大缩减。 表33种算法创建地图数据 Table 3 Map data of three algorithms 算法 轨迹RMSE路标RMSE粒子数运行时间s RBPF 2.013 1.391 39 317 文献[15) 1.279 1.026 24 201 图4实验室环境 改进的RBPF 0.925 0.836 16 126 Fig.4 Laboratory environment (a)RBPF实验结果 (b)文献[15]实验结果 (©)改进的RBPF实验结果 图5Rviz实时构建地图 Fig.5 Rviz building a map in real time 4结束语 magnetic anomalies[J].Chinese Journal of scientific instru- ment,2015,36(1):181-186 为解决RBPF算法中粒子退化和多样性降低 [2]KUNDU A S.MAZUMDER O,DHAR A,et al.Occu- 问题，本文提出一种QPSO-RBPF-SLAM算法。将 pancy grid map generation using 360 scanning xtion pro 机器人运动模型和观测模型作为提议分布，在重 live for indoor mobile robot navigation[Cl//Proceedings of 采样过程中结合量子粒子群思想和遗传算法，利 2016 IEEE First International Conference on Control, 用QPSO算法更新粒子位姿，根据权值划分粒子 Measurement and Instrumentation.Kolkata,India,2016: 种类引入自适应交叉变异操作对所得粒子集进行 464-468. 优化、调整。同时，在机器人ROS平台上利用旅 [3]AKAI N,OZAKI K.A navigation method based on topo- 行家2号机器人进行实验，以较少的粒子数和较 logical magnetic and geometric maps for outdoor mobile 短时间在精确估计机器人位姿的同时能够创建 robots[C]//Proceedings of 2015 IEEE/SICE International 较高精度的栅格地图。下一步，在获得的高精 Symposium on System Integration.Nagoya,Japan,2015: 度栅格地图的基础上对移动机器人进行路径规划 352-357 研究。 [4]RAMAITHITIMA R,WHITZER M,BHATTACHARYA 参考文献： S,et al.Automated creation of topological maps in un- known environments using a swarm of resource-con- [1]张聪聪王新珩，董育宁.基于地磁场的室内定位和地图 strained robots[J].IEEE robotics and automation letters, 构建0.仪器仪表学报，2015,36(1)：181-186. 2016,1(2):746-753. ZHANG Congcong,WANG Xinheng,DONG Yuning. [5]戴雪梅，郎朗，陈孟元.强跟踪平方根容积卡尔曼滤波 Simultaneous localization and mapping based on indoor SLAM算法[J].电子测量与仪器学报，2015,29(10)：
取的区域为 8 m×1.5 m，机器人以 0.2 m/s 的速度 移动，利用里程计信息和激光数据信息实时构建 栅格地图。 图 4 实验室环境 Fig. 4 Laboratory environment 由图 5 和表 3 可知，RBPF-SALM 算法采用了 39 个粒子，粒子退化严重，降低粒子多样性，创建的地 图不够精确；文献[15]算法采用了 24 个粒子，创 建的地图有所改善，但效果不显著；改进的 RBPFSALM 算法只使用了 16 个粒子获得了比 RBPF-SALM 算法和文献[15]算法更精确的地图，同时轨迹和 路标估计的均方根误差、运行时间也大大缩减。 表 3 3 种算法创建地图数据 Table 3 Map data of three algorithms 算法 轨迹 RMSE 路标 RMSE 粒子数 运行时间/s RBPF 2.013 1.391 39 317 文献[15] 1.279 1.026 24 201 改进的 RBPF 0.925 0.836 16 126 (a) RBPF 实验结果 (b) 文献[15]实验结果 (c) 改进的 RBPF 实验结果 图 5 Rviz 实时构建地图 Fig. 5 Rviz building a map in real time 4 结束语 为解决 RBPF 算法中粒子退化和多样性降低 问题，本文提出一种 QPSO-RBPF-SLAM 算法。将 机器人运动模型和观测模型作为提议分布，在重 采样过程中结合量子粒子群思想和遗传算法，利 用 QPSO 算法更新粒子位姿，根据权值划分粒子 种类引入自适应交叉变异操作对所得粒子集进行 优化、调整。同时，在机器人 ROS 平台上利用旅 行家 2 号机器人进行实验，以较少的粒子数和较 短时间在精确估计机器人位姿的同时能够创建 较高精度的栅格地图。下一步，在获得的高精 度栅格地图的基础上对移动机器人进行路径规划 研究。 参考文献： 张聪聪, 王新珩, 董育宁. 基于地磁场的室内定位和地图 构建[J]. 仪器仪表学报, 2015, 36(1): 181–186. ZHANG Congcong, WANG Xinheng, DONG Yuning. Simultaneous localization and mapping based on indoor [1] magnetic anomalies[J]. Chinese Journal of scientific instrument, 2015, 36(1): 181–186. KUNDU A S, MAZUMDER O, DHAR A, et al. Occupancy grid map generation using 360° scanning xtion pro live for indoor mobile robot navigation[C]//Proceedings of 2016 IEEE First International Conference on Control, Measurement and Instrumentation. Kolkata, India, 2016: 464–468. [2] AKAI N, OZAKI K. A navigation method based on topological magnetic and geometric maps for outdoor mobile robots[C]//Proceedings of 2015 IEEE/SICE International Symposium on System Integration. Nagoya, Japan, 2015: 352–357. [3] RAMAITHITIMA R, WHITZER M, BHATTACHARYA S, et al. Automated creation of topological maps in unknown environments using a swarm of resource-constrained robots[J]. IEEE robotics and automation letters, 2016, 1(2): 746–753. [4] 戴雪梅, 郎朗, 陈孟元. 强跟踪平方根容积卡尔曼滤波 SLAM 算法[J]. 电子测量与仪器学报, 2015, 29(10): [5] ·834· 智 能 系 统 学 报 第 13 卷
第5期 伍永健，等：量子粒子群优化下的RBPF-SLAM算法研究 ·835· 1493-1499 bot.2016.38(2):169-177 DAI Xuemei,LANG Lang,CHEN Mengyuan.Strong [I4]宋宇，李庆玲，康轶非，等.平方根容积Rao-Blackwil- tracking square-root cubature Kalman filter based on lised粒子滤波SLAM算法[J].自动化学报，2014， SLAM algorithm[J].Journal of electronic measurement 40(2:357-367. and instrumentation,2015,29(10):1493-1499. SONG Yu,LI Qingling,KANG Yifei,et al.SLAM with [6]韩萍，桑威林，石庆研.一种新型非线性卡尔曼滤波方法 square-root cubature Rao-Blackwillised particle filter[J]. [).仪器仪表学报，2015,36(3)：632-638. Acta automatica sinica.2014.40(2):357-367. HAN Ping,SANG Weilin,SHI Qingyan.Novel nonlinear [15]林海波，柯晶晶，张毅.结合粒子群寻优与遗传重采样 Kalman filtering method[J].Chinese Journal of scientific 的RBPF算法U.计算机工程，2016,42(11)295-299. instrument,2015,36(3):632-638. LIN Haibo,KE Jingjing,ZHANG Yi.Rao-Blackwellized [7]周自牧，肖康.基于概率密度函数塑形法的EKF和 particle filter algorithm combined particle swarm optimiz- UKF优化U.控制工程，2016(S1:40-45. ation and genetic Re-sampling[J].Computer engineering, ZHOU Zimu,XIAO Kang.Optimization of EKF and UKF 2016,42(11):295-299. based on PDF shaping method[J].Control engineering of [I6]罗元，苏琴，张毅，等.基于优化RBPF的同时定位与地 China.2016(S1):40-45. 图构建[J].华中科技大学学报：自然科学版，2016 [8]王红旗，刘勇，罗宇锋.带定位盲区的矿井人员全局无线 44(5):30-34. 定位算法.控制工程，2015,22(3)：505-509 LUO Yuan,SU Qin,ZHANG Yi,et al.Simultaneous loc- WANG Hongqi,LIU Yong,LUO Yufeng.Personnel glob- alization and mapping implementation based on optim- al wireless positioning algorithm in mine with positioning ized RBPF[J].Journal of Huazhong university of science blind[J].Control engineering of China,2015,22(3): and technology:natural science edition,2016,44(5): 505-509. 30-34. [9]李天成，范红旗，孙树栋粒子滤波理论、方法及其在多 [17刀李仁府，独孤明哲，胡麟，等.基于QPSO算法移动机器 目标跟踪中的应用[J].自动化学报，2015,41(12)： 人轨迹规划与实验[].控制与决策，2014,29(12)： 1981-2002 2151-2157 LI Tiancheng,FAN Hongqi,SUN Shudong.Particle filter- LI Renfu,DOKGO Myong-chol,HU Lin,et al.Mobile ing:theory,approach,and application for multitarget track- robot trajectory planning based on QPSO algorithm and ing[J].Acta automatica sinica,2015,41(12):1981-2002. experiment[J].Control and decision,2014,29(12): [10]ABBASI A,JAVARI A,JALILI M,et al.Enhancing pre- 2151-2157 cision of Markov-based recommenders using location in- 作者简介： formation[C]//Proceeings of 2014 International Confer- 伍永健，男.1992年生，硕土研究 ence on Advances in Computing,Communications and 生，主要研究方向为移动机器人路径 Informatics.New Delhi,India,2014:188-193. 规划。 [11]闫德立，宋永端，宋宇，等.一种改进的高斯混合概率假 设密度SLAM算法[J.控制与决策，2014,29(11)： 1959-1965. YAN Deli,SONG Yongrui,SONG Yu,et al.An im- proved gaussian mixture PHD SLAM algorithm[J].Con- 陈跃东，男，1956年生，教授，主 trol and decision,,2014,29(11):1959-1965. 要研究方向为传感器信号处理、移动 [12]陈卫东，刘要龙，朱奇光，等.基于改进雁群PS0算法的 机器人定位及导航。 模糊自适应扩展卡尔曼滤波的SLAM算法).物理学 报，2013,62(17)：170506. CHEN Weidong,LIU Yaolong,ZHU Qiguang,et al. Fuzzy adaptive extended Kalman filter SLAM algorithm 陈孟元，男，1984年生，副教授， based on the improved wild geese PSO algorithm[J].Acta 主要研究方向为嵌入式系统开发、图 physica sinica,.2013,62(17):170506. 像处理、传感器信息融合及优化。 [13]陈炜楠，刘冠峰，李俊良，等.室内环境的元胞自动机 SLAM算法U.机器人，2016,38(2)：169-177. CHEN Weinan,LIU Guanfeng,LI Junliang,et al.An in- door SLAM algorithm based on cellular automata[J].Ro-
1493–1499. DAI Xuemei, LANG Lang, CHEN Mengyuan. Strong tracking square-root cubature Kalman filter based on SLAM algorithm[J]. Journal of electronic measurement and instrumentation, 2015, 29(10): 1493–1499. 韩萍, 桑威林, 石庆研. 一种新型非线性卡尔曼滤波方法 [J]. 仪器仪表学报, 2015, 36(3): 632–638. HAN Ping, SANG Weilin, SHI Qingyan. Novel nonlinear Kalman filtering method[J]. Chinese Journal of scientific instrument, 2015, 36(3): 632–638. [6] 周自牧, 肖康. 基于概率密度函数塑形法的 EKF 和 UKF 优化[J]. 控制工程, 2016(S1): 40–45. ZHOU Zimu, XIAO Kang. Optimization of EKF and UKF based on PDF shaping method[J]. Control engineering of China, 2016(S1): 40–45. [7] 王红旗, 刘勇, 罗宇锋. 带定位盲区的矿井人员全局无线 定位算法[J]. 控制工程, 2015, 22(3): 505–509. WANG Hongqi, LIU Yong, LUO Yufeng. Personnel global wireless positioning algorithm in mine with positioning blind[J]. Control engineering of China, 2015, 22(3): 505–509. [8] 李天成, 范红旗, 孙树栋. 粒子滤波理论、方法及其在多 目标跟踪中的应用[J]. 自动化学报, 2015, 41(12): 1981–2002. LI Tiancheng, FAN Hongqi, SUN Shudong. Particle filtering: theory, approach, and application for multitarget tracking[J]. Acta automatica sinica, 2015, 41(12): 1981–2002. [9] ABBASI A, JAVARI A, JALILI M, et al. Enhancing precision of Markov-based recommenders using location information[C]//Proceeings of 2014 International Conference on Advances in Computing, Communications and Informatics. New Delhi, India, 2014: 188–193. [10] 闫德立, 宋永端, 宋宇, 等. 一种改进的高斯混合概率假 设密度 SLAM 算法[J]. 控制与决策, 2014, 29(11): 1959–1965. YAN Deli, SONG Yongrui, SONG Yu, et al. An improved gaussian mixture PHD SLAM algorithm[J]. Control and decision, 2014, 29(11): 1959–1965. [11] 陈卫东, 刘要龙, 朱奇光, 等. 基于改进雁群 PSO 算法的 模糊自适应扩展卡尔曼滤波的 SLAM 算法[J]. 物理学 报, 2013, 62(17): 170506. CHEN Weidong, LIU Yaolong, ZHU Qiguang, et al. Fuzzy adaptive extended Kalman filter SLAM algorithm based on the improved wild geese PSO algorithm[J]. Acta physica sinica, 2013, 62(17): 170506. [12] 陈炜楠, 刘冠峰, 李俊良, 等. 室内环境的元胞自动机 SLAM 算法[J]. 机器人, 2016, 38(2): 169–177. CHEN Weinan, LIU Guanfeng, LI Junliang, et al. An indoor SLAM algorithm based on cellular automata[J]. Ro- [13] bot, 2016, 38(2): 169–177. 宋宇, 李庆玲, 康轶非, 等. 平方根容积 Rao-Blackwillised 粒子滤波 SLAM 算法[J]. 自动化学报, 2014, 40(2): 357–367. SONG Yu, LI Qingling, KANG Yifei, et al. SLAM with square-root cubature Rao-Blackwillised particle filter[J]. Acta automatica sinica, 2014, 40(2): 357–367. [14] 林海波, 柯晶晶, 张毅. 结合粒子群寻优与遗传重采样 的 RBPF 算法[J]. 计算机工程, 2016, 42(11): 295–299. LIN Haibo, KE Jingjing, ZHANG Yi. Rao-Blackwellized particle filter algorithm combined particle swarm optimization and genetic Re-sampling[J]. Computer engineering, 2016, 42(11): 295–299. [15] 罗元, 苏琴, 张毅, 等. 基于优化 RBPF 的同时定位与地 图构建[J]. 华中科技大学学报: 自然科学版, 2016, 44(5): 30–34. LUO Yuan, SU Qin, ZHANG Yi, et al. Simultaneous localization and mapping implementation based on optimized RBPF[J]. Journal of Huazhong university of science and technology: natural science edition, 2016, 44(5): 30–34. [16] 李仁府, 独孤明哲, 胡麟, 等. 基于 QPSO 算法移动机器 人轨迹规划与实验[J]. 控制与决策, 2014, 29(12): 2151–2157. LI Renfu, DOKGO Myong-chol, HU Lin, et al. Mobile robot trajectory planning based on QPSO algorithm and experiment[J]. Control and decision, 2014, 29(12): 2151–2157. [17] 作者简介： 伍永健，男，1992 年生，硕士研究 生，主要研究方向为移动机器人路径 规划。 陈跃东，男，1956 年生，教授，主 要研究方向为传感器信号处理、移动 机器人定位及导航。 陈孟元，男，1984 年生，副教授， 主要研究方向为嵌入式系统开发、图 像处理、传感器信息融合及优化。 第 5 期 伍永健，等：量子粒子群优化下的 RBPF-SLAM 算法研究 ·835·