基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法

摘要

本发明公开了一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括：(1)利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并对机器人位姿状态进行采样；(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的特征地图信息；(3)利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。本发明同时从采样粒子的质量和数量两个方面对传统的FastSLAM算法进行改进：增强算法在估计过程中的数值稳定性和精度，提高采样粒子的质量；在粒子重采样过程中根据估计不确定度动态确定最少所需粒子个数，提高算法的计算效率。

主权项

一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：(1)将运动噪声和测量噪声同时增广至机器人位姿状态向量中，利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并根据该粒子提议分布对机器人位姿状态进行采样；(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的路标特征地图信息；(3)在粒子重采样阶段，利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。