基于强跟踪UKF的自适应PHD-SLAM算法
Adaptive PHD-SLAM Based on Strong Tracking and UKF
-
摘要: 传统概率假设密度同时定位与建图(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,保证计算效率的同时,能够有效的提高机器人位姿和地图特征点的估计精度。Abstract: The traditional probability hypothesis density-simultaneous localization and mapping (PHD-SLAM) method lacked online adaptive adjustment capabilities and was easily affected by uncertain noise, the choice of initial system parameters, and linearization approximation errors, which led to particle degradation problems and subsequently affected the estimation accuracy of the pose and map features of the robot. This study addressed this issue by proposing a PHD-SLAM algorithm based on strong tracking and an unscented Kalman filter (UKF), which integrated the latest observation data to generate importance density (strong tracking UKF PHD-SLAM, SUPHD-SLAM). In the proposed algorithm, during the importance sampling stage, the pose and map features of the robot at the previous moment were augmented into a joint vector. To avoid the linearization errors introduced by the extended Kalman filter (EKF) in the traditional PHD-SLAM, the UKF was used to predict the particles, and the fading factor in strong tracking filtering was introduced to correct the inaccurate pose state covariance after the UKF prediction, maintaining the orthogonality of the measurement innovation and thereby suppressing the influence of uncertain noise and inaccurate initial system parameter settings on state estimation. Subsequently, each pose particle was updated through the UKF, guiding the particles towards the high likelihood region to obtain a more accurate pose importance density, thus avoiding particle degradation. New pose particles were sampled from the importance density, and the PHD filter based on UKF was used to calculate the map features for each pose particle. The single cluster (SC) strategy was used to update the weights of each pose particle. Finally, the pose particle with the highest weight and its corresponding map were extracted as the state estimation. Simulation experiments showed that, compared to PHD-SLAM 1.0 and PHD-SLAM 2.0, SUPHD-SLAM effectively improved the estimation accuracy of the pose and map features of the robot while ensuring computational efficiency.