关键词:
无迹卡尔曼滤波
强跟踪
机器人位姿
地图
摘要:
传统概率假设密度同时定位与建图(Probability Hypothesis Density-Simultaneous Localization and Mapping,PHD-SLAM)方法缺乏在线自适应调整能力,容易受到不确定噪声、初始系统参数选择以及线性化近似误差的影响,从而导致粒子退化问题,进而影响机器人位姿和地图特征点的估计精度。针对这一问题,本文提出了一种基于强跟踪和无迹卡尔曼滤波(Unscented Kalman filter,UKF),并融合最新观测数据来产生重要性密度的PHD-SLAM算法(Strong Tracking UKF PHD-SLAM,SUPHD-SLAM)。所提算法在重要性采样阶段将上一时刻的机器人位姿和地图特征点增广为联合向量,为了避免传统PHD-SLAM中扩展卡尔曼滤波(Extended Kalman filter,EKF)引入的线性化误差,利用UKF对粒子进行预测,并通过引入强跟踪滤波中的渐消因子修正UKF预测后不精确的位姿状态协方差,保持量测新息正交,从而抑制不确定噪声和不精确初始系统参数设置对状态估计的影响。随后通过UKF更新每个位姿粒子,引导粒子向高似然区域移动,以获得更准确的位姿的重要性密度,从而避免粒子退化。从重要性密度中采样新的位姿粒子,针对每个位姿粒子使用基于UKF的PHD滤波计算地图特征点,并用单簇(Single-Cluster,SC)策略更新每个位姿粒子的权重。最后,提取权重最大的位姿粒子及其对应的地图作为状态估计。仿真实验表明,SUPHD-SLAM相较于PHD-SLAM 1.0和PHD-SLAM 2.0,保证计算效率的同时,能够有效的提高机器人位姿和地图特征点的估计精度。