量子粒子群优化下的RBPF-SLAM算法研究
|
摘要
为了解决传统Rao-Blackwellized粒子滤波(RBPF)存在提议分布精度不高以及重采样过程出现的粒子退化和多样性丢失问题,提出一种量子粒子群(QPSO)优化下的Rao-Blackwellized粒子滤波同时定位与地图构建(RBPF-SLAM)算法。将机器人运动模型和观测模型融合作为混合提议分布,提高提议分布的精度;在重采样过程中引入量子粒子群优化算法更新粒子位姿,根据权值划分粒子种类,引入自适应交叉变异操作,对所得粒子集进行优化、调整,有效地防止粒子退化以及保持粒子的多样性。利用本文算法不仅用MATLAB进行仿真实验,而且结合了旅行家2号移动机器人在机器人操作系统(ROS)上进行实际验证。结果表明,本文算法能以较少粒子数精确估计出机器人的位姿和高精度的地图,误差和运行时间也大大降低了。
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.
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.
引文
[1]张聪聪,王新珩,董育宁.基于地磁场的室内定位和地图构建[J].仪器仪表学报,2015,36(1):181-186.ZHANG Congcong,WANG Xinheng,DONG Yuning Simultaneous localization and mapping based on indoor magnetic anomalies[J].Chinese Journal of scientific instrument,2015,36(1):181-186.
[2]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 of2016 IEEE First International Conference on Control,Measurement and Instrumentation.Kolkata,India,2016:464-468.
[3]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.
[4]RAMAITHITIMA R,WHITZER M,BHATTACHARYAS,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.
[5]戴雪梅,郎朗,陈孟元.强跟踪平方根容积卡尔曼滤波SLAM算法[J].电子测量与仪器学报,2015,29(10):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.
[6]韩萍,桑威林,石庆研.一种新型非线性卡尔曼滤波方法[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.
[7]周自牧,肖康.基于概率密度函数塑形法的EKF和UKF优化[J].控制工程,2016(S1):40-45.ZHOU Zimu,XIAO Kang.Optimization of EKF and UKFbased on PDF shaping method[J].Control engineering of China,2016(S1):40-45.
[8]王红旗,刘勇,罗宇锋.带定位盲区的矿井人员全局无线定位算法[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.
[9]李天成,范红旗,孙树栋.粒子滤波理论、方法及其在多目标跟踪中的应用[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.
[10]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.
[11]闫德立,宋永端,宋宇,等.一种改进的高斯混合概率假设密度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.
[12]陈卫东,刘要龙,朱奇光,等.基于改进雁群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.
[13]陈炜楠,刘冠峰,李俊良,等.室内环境的元胞自动机SLAM算法[J].机器人,2016,38(2):169-177.CHEN Weinan,LIU Guanfeng,LI Junliang,et al.An indoor SLAM algorithm based on cellular automata[J].Robot,2016,38(2):169-177.
[14]宋宇,李庆玲,康轶非,等.平方根容积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.
[15]林海波,柯晶晶,张毅.结合粒子群寻优与遗传重采样的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.
[16]罗元,苏琴,张毅,等.基于优化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.
[17]李仁府,独孤明哲,胡麟,等.基于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.
[2]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 of2016 IEEE First International Conference on Control,Measurement and Instrumentation.Kolkata,India,2016:464-468.
[3]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.
[4]RAMAITHITIMA R,WHITZER M,BHATTACHARYAS,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.
[5]戴雪梅,郎朗,陈孟元.强跟踪平方根容积卡尔曼滤波SLAM算法[J].电子测量与仪器学报,2015,29(10):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.
[6]韩萍,桑威林,石庆研.一种新型非线性卡尔曼滤波方法[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.
[7]周自牧,肖康.基于概率密度函数塑形法的EKF和UKF优化[J].控制工程,2016(S1):40-45.ZHOU Zimu,XIAO Kang.Optimization of EKF and UKFbased on PDF shaping method[J].Control engineering of China,2016(S1):40-45.
[8]王红旗,刘勇,罗宇锋.带定位盲区的矿井人员全局无线定位算法[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.
[9]李天成,范红旗,孙树栋.粒子滤波理论、方法及其在多目标跟踪中的应用[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.
[10]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.
[11]闫德立,宋永端,宋宇,等.一种改进的高斯混合概率假设密度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.
[12]陈卫东,刘要龙,朱奇光,等.基于改进雁群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.
[13]陈炜楠,刘冠峰,李俊良,等.室内环境的元胞自动机SLAM算法[J].机器人,2016,38(2):169-177.CHEN Weinan,LIU Guanfeng,LI Junliang,et al.An indoor SLAM algorithm based on cellular automata[J].Robot,2016,38(2):169-177.
[14]宋宇,李庆玲,康轶非,等.平方根容积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.
[15]林海波,柯晶晶,张毅.结合粒子群寻优与遗传重采样的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.
[16]罗元,苏琴,张毅,等.基于优化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.
[17]李仁府,独孤明哲,胡麟,等.基于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.