基于萤火虫优化粒子滤波的机器人定位与建图方法及装置

摘要

本发明涉及一种基于萤火虫优化粒子滤波的机器人定位与建图方法及装置，其中方法包括：步骤S1：获得采样初始时刻粒子；步骤S2：通过机器人运动模型和观测模型得到t时刻的目标值，并通过重要性采样得到t时刻的粒子集合；步骤S3：利用改进后的萤火虫算法优化采样粒子的位置，分别得到所有粒子t+1时刻优化后的位置；步骤S4：计算优化后的粒子权重并进行归一化，筛选有效粒子，并基于有效粒子进行重采样，直至粒子总数达到设定数目；步骤S5：根据步骤S4得到的粒子群进行机器人位姿与环境估计。与现有技术相比，本发明解决了固定步长因子导致的收敛速度慢、在极值点附近易“振荡”等问题，提高了粒子全局寻优和局部寻优的精度与效率。