[发明专利]基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法

说明书

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

技术领域

本发明属于移动机器人自主导航领域，具体涉及一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法。

背景技术

移动机器人作为智能机器人的重要分支，在诸如制造工业、国防军事、航天航空、卫生医疗、家庭服务等领域得到了广泛的应用。为了实现移动机器人在复杂未知环境下，不经过人工干预而顺利完成特定作业任务，移动机器人必须具备自主导航的能力。作为自主导航的关键前提，移动机器人需要通过携带的内部、外部传感器对自身姿态进行估计，并且同时对未知环境进行地图描述。移动机器人同时定位与地图创建(SimultaneousLocalization and Mapping，SLAM)方法可以充分利用定位与地图创建两者之间的相关性，在没有先验环境地图信息及GPS等辅助定位设备的情形下，实现未知环境的增量式地图创建以及自身姿态的在线估计。

Montemerlo等人基于Rao-Blackwellized粒子滤波器思想提出了FastSLAM算法，有效地克服了基于高斯滤波器的SLAM算法计算复杂度高、数据关联鲁棒性差等缺点，实现了任意非线性非高斯随机系统的状态估计。在FastSLAM算法中，粒子提议分布的计算是影响算法性能的关键因素之一。

传统的FastSLAM算法利用一阶泰勒级数展开对机器人的非线性运动模型和测量模型进行线性化近似，忽略了二阶以上统计项的信息，因而在处理非线性程度较高的模型时容易造成较大的累积误差，最终导致算法的估计一致性较差。

Kim等人和Song等人分别将无味卡尔曼滤波和容积卡尔曼滤波器应用于FastSLAM的粒子提议分布的计算中，通过一系列特定选取的Sigma点来直接近似状态的后验概率密度函数，从而避免了对非线性函数的线性化操作。但是当状态向量的维度较高时，无味卡尔曼滤波器的中心Sigma点的权值可能为负数，因而不能保证后验协方差矩阵的正定性；容积卡尔曼滤波器用来限定容积点的球体半径与状态向量的维度成正比，因而出现所谓的非局部采样问题。

另一方面，粒子重要性重采样过程也是影响FastSLAM算法性能的重要因素之一。通常而言，采用较多的粒子数目能够更加精确地逼近机器人位姿状态的后验概率分布，而较少的粒子数目能够大幅度减少算法对计算资源的需求。因而，在FastSLAM的粒子重要性重采样过程中，在保证算法估计精度的同时尽量提高算法的计算效率，应考虑如何选择尽可能少的符合实际状态后验概率分布的粒子。

发明内容

本发明提供了一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，旨在同时从采样粒子的质量和数量两个方面对传统的FastSLAM算法进行改进，增强算法在估计过程中的数值稳定性和精度，提高采样粒子的质量，同时在粒子重采样过程中根据估计不确定度动态确定最少所需粒子个数，提高算法的计算效率。

一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：

(1)将运动噪声和测量噪声同时增广至机器人位姿状态向量中，利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并根据该粒子提议分布对机器人位姿状态进行采样；

(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的路标特征地图信息；