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

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