基于KLD重采样的抗差自适应UFastSLAM算法

A robust adaptive UFastSLAM with KLD-resampling

针对声呐SLAM系统中噪声统计特性不准确或状态突变而导致无迹快速SLAM(UFastSLAM)算法性能下降及因采用固定粒子数而使得算法实时性不高的问题,提出一种基于改进粒子建议分布估计和自适应KLD重采样的UFastSLAM(RAUFastSLAM)算法。在载体位姿估计阶段,融入抗差自适应因子,利用抗差自适应无迹粒子滤波算法(RAUPF)对载体位姿进行估计;在特征状态估计阶段,利用抗差自适应无迹滤波算法(RAUKF)对环境特征的位置进行更新;在粒子重采样阶段,采用基于KL散度的自适应粒子重采样方法在线实时调整所需的粒子数,在保证精度的同时提高算法的实时性。实验结果表明,在量测噪声统计特性不准确且量测信息中有异常干扰时,与UFast SLAM算法相比,所提方法的载体位置估计、航向角估计和特征位置估计的精度分别提高了18.79%、16.67%和18.81%,对异常干扰具有更好的鲁棒性。

Aiming at the problems that the performance of UFastSLAM algorithm is degraded due to inaccurate noise statistical characteristics or sudden state changes in sonar-based SLAM systems,and the real-time performance of the algorithm is not high due to the fixed number of particles,a UFastSLAM algorithm based on improved particle proposal distribution estimation and adaptive KLD resampling(RAUFastSLAM)is proposed.In the stage of carrier pose estimation,the robust adaptive factor is incorporated,and the robust adaptive unscented particle filter(RAUPF)algorithm is used to estimate the carrier pose.In the feature state estimation stage,the robust adaptive unscented filtering(RAUKF)algorithm is utilized to update the position of the environment feature.In the particle resampling stage,the adaptive particle resampling method based on KL divergence is used to adjust the required number of particles online in real time,which can improve the real-time performance of the algorithm while ensuring the accuracy.Experimental results show that the accuracy of vehicle position estimation,heading angle estimation and feature position estimation of the proposed method is improved by 18.79%,16.67%and 18.81%than that of the UFastSLAM algorithm,respectively,which has better robustness to abnormal interference.