平方根容积卡尔曼滤波概率假设密度算法在移动机器人同时定位与地图构建中的应用  

The application of square-root cubature Kalman filter and probability hypothesis density in simultaneous localization and mapping for mobile robots

作　　者：闫德立[1,2] 宋永端[1] 宋宇[1] 康轶非[1] 

机构地区：[1]北京交通大学电子信息工程学院,北京100044 [2]石家庄铁道大学电气与电子工程学院,河北石家庄050043

出　　处：《控制理论与应用》2014年第8期1009-1017,共9页Control Theory & Applications

基　　金：国家自然科学基金资助项目(61134001;60905055;51274144);国家"973"计划资助项目(2012CB215202);国家"863"计划资助项目(SS2012AA052302);河北省自然科学基金资助项目(F2012210031);博士后科学基金资助项目(2013T60197);中央高校基本业务费资助项目(2014JBM014)

摘　　要：针对杂波环境或数据关联模糊环境下移动机器人同时定位与地图构建(SLAM)的问题,本文提出平方根容积卡尔曼滤波概率假设密度(SRCKF-PHD)SLAM算法,该算法的主要特点在于:1)采用容积规则方法计算非线性函数高斯权重积分以及机器人位姿粒子权重,达到改善位姿估计性能的目的;2)在高斯混合概率假设密度更新过程中,将平方根容积卡尔曼滤波应用于高斯项权重更新及观测似然计算中,保证了协方差矩阵的对称性和半正定性,提高了地图估计的精度和稳定性.通过仿真实验及CarPark数据集,将提出算法与RB-PHD-SLAM算法进行对比,结果表明该算法对机器人位姿估计精度及地图估计精度的提高是有效的.A simultaneous localization and mapping （SLAM） algorithm based on square-root cubature Kalman filter and probability hypothesis density （SRCKF-PHD） is proposed,which is applied to situations of high clutter or ambiguous data association.The main contributions are：1） to improve the performance of robot pose estimation,the cubature rule is utilized to calculate Gaussian weighted integral of the nonlinear function and robot pose particle＇s weight; 2） in the process of GM-PHD update,SRCKF is utilized for calculating measurement likelihood and Gaussian component＇s weight,which guarantees the symmetry and positive semi-definiteness of the covariance matrix and improves the numerical stability and accuracy.The proposed algorithm is compared with the RB-PHD-SLAM algorithm in simulation and Car Park data set.The results show that the proposed algorithm outperforms RB-PHD-SLAM algorithm.

关 键 词：移动机器人 同时定位与地图构建 平方根容积卡尔曼滤波 概率假设密度 

分 类 号：TP242[自动化与计算机技术—检测技术与自动化装置]