[发明专利]基于改进重采样方法和粒子选取的FastSLAM算法

说明书

技术领域

本发明涉及机器人同时定位与地图创建领域，具体来说是一种对传统FastSLAM算法的改进算法，尤其是对FastSLAM算法中的重采样标准判定的修正和对粒子重采样方法的修正。

背景技术

移动机器人在自身位置和姿态都不确定的条件下，利用自身的内部设备和外部测量传感装置在完全未知的环境中创建地图，同时利用所创建地图进行自主定位被通称为同时定位与地图创建问题。自从Smith等人提出基于扩展卡尔曼滤波器的同时定位于地图创建算法之后，扩展卡尔曼算法便成为研究机器人同时定位与地图创建问题的主要方法。但是随着对机器人同时定位与地图创建问题的深入研究，人们发现扩展卡尔曼滤波器算法存在很明显的缺陷：一是其计算复杂度和环境中已检测路标数目(N)成平方比(O(N²))，并且在同一时刻即使只检测到一个路标，整个状态协方差矩阵都必须进行更新；二是扩展卡尔曼滤波器算法对数据关联问题处理不够健壮，它不能对数据关联的错误及时进行自恢复。

针对扩展卡尔曼滤波器所存在的问题，Montemelo等人提出了FastSLAM算法。在FastSLAM算法中机器人同时定位与地图创建问题被分解为机器人位姿估计和路标位置估计两个过程。对机器人位姿的估计使用粒子滤波器，对路标位置估计使用扩展卡尔曼滤波器算法进行；机器人位姿估计中每个粒子代表机器人一条可能的轨迹，同时使用粒子权重来表示此轨迹的好坏，每个粒子中的路标位置估计是相互独立的。和扩展卡尔曼滤波器算法的计算复杂度不同，FastSLAM算法的计算复杂度为O(MlogN)，其中M表示所使用的粒子数目，N表示已检测到的路标数目。对于数据关联而言，在FastSLAM算法中，每个粒子的数据关联是相互独立的，当其中部分出现问题时，在重采样阶段这部分粒子会被衰退掉而不影响机器人同时定位与地图创建的整体估计，从而可以提高数据关联的健壮性。尽管FastSLAM很好的解决了扩展卡尔曼滤波器算法所出现的复杂度和数据关联问题，但是随后Bailey等人发现使用FastSLAM算法对于机器人位姿估计一致性很差，而且粒子的多样性随着机器人的运动呈指数趋势下降，Bailey还发现通过增加粒子数目只能稍微提高机器人位姿估计的一致性，但是随之带来的问题是机器人同时定位与地图创建过程的实时性遭受到严重破坏。

发明内容

本发明提供了一种基于改进重采样方法和粒子选取的FastSLAM算法，主要是在FastSLAM的框架下，对机器人位姿估计的粒子滤波器的重采样判定标准进行了改进，并且对粒子重采样时新粒子的生成方法进行了改进。

一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步骤：

1)当机器人有输入控制数据，依据机器人运动预测模型对每个粒子中的机器人位姿进行预测，当机器人外部传感器有路标的检测数据时，依据外部传感器的测量模型对每个粒子中的机器人位姿和对应路标位置进行更新；

2)对机器人的位姿使用粒子滤波器来进行估计，在每次使用机器人运动预测模型或者传感器的测量模型对机器人位姿进行更新后都需要进行粒子重采样判断，以确定是否需要进行粒子重采样。使用当前粒子有效数，粒子权重协方差，以及粒子测量残余一致性综合进行判断。当有效粒子数小于总粒子数的75％，粒子权重协方差大于粒子权重均值，并且粒子测量残余一致性在95％的一致性区域之外时，进行粒子重采样；

3)在步骤2中的粒子重采样条件满足时，使用指数等级方法和交叉算子进行新粒子的生成。首先对根据粒子权重对粒子排序，再使用指数方法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后依据父粒子和交叉算子进行新粒子的生成；

4)依据生成的新粒子计算机器人的定位和地图创建。

所述的机器人运动预测模型、机器人外部传感器的测量模型均采用现有标准FastSLAM算法中常用的模型。

所述的粒子权重分布协方差的分布w_cov按照式(1)进行计算。