[发明专利]一种基于聚类和膜计算的改进FastSLAM方法

说明书

本发明公开了一种基于聚类和膜计算的改进FastSLAM方法，其包括：初始化，采样，聚类，膜计算优化，计算权重，计算位姿并更新地图和重采样的一系列过程，实现了机器人快速自定位和环境地图构建的功能。该方法通过使用聚类方法对粒子集进行预处理和使用膜计算优化方法的高度并行性，加快了粒子搜索速度，扩大了搜索范围，减缓了粒子退化情况，保证了粒子的多样性，促使粒子更快的分布在真实的位姿附近，有效提高了机器人定位和建图的精度和速度。

技术领域

本发明涉及一种基于聚类和膜计算的机器人同时定位和建图的FastSLAM方法的改进方法。

背景技术

同时定位与建图(SLAM)是指移动机器人在未知环境下通过自身携带的传感器进行定位，并同时对周围环境进行建图，是进行自主导航、避障等任务的关键技术。目前，解决SLAM问题最常用的是基于概率的方法，其中表现最为突出的是快速同时定位与建图(FastSLAM)方法，FastSLAM因其计算复杂度低，同时具有鲁棒的数据关联能力打破了高斯环境的限制，得到广泛使用。然而，FastSLAM频繁的重采样会导致机器人估计姿态的粒子集失去多样性，随着时间的不断增长，定位精度会不断发生退化。

近年来，研究者不断提出改进方法试图解决这种问题。Unscented FastSLAM方法利用无迹变换进行粒子滤波、特征初始化和特征估计。通过无迹粒子滤波(UPF)代替FastSLAM中的扩展卡尔曼滤波方法(EKF)，使滤波精度得到了很大的提升。但是，该方法计算量大，耗时太长，导致效率不高。Chang等人提出的PSOFastSLAM方法则是利用粒子群来优化FastSLAM，但由于粒子群方法会出现易陷入局部最优和粒子退化问题，因此对FastSLAM方法的精度并没有很大的改善，且粒子集仅受一个最优粒子吸引，优化后粒子集可能聚集在一个很小的范围，缩小了搜索范围，所以导致机器人在状态突变的情况下没有足够的稳定性。Cugliari等人利用无迹变换提高了粒子滤波和特征估计器的精度，并提出了一种自适应选择性重采样技术。这些技术已经显示出比传统的FastSLAM更好的性能。然而，这些方法对缓解粒子退化问题效果并不佳。

发明内容

针对上述问题，本发明的目的是为了提供一种基于聚类和膜计算的改进FastSLAM方法。该方法利用聚类方法对粒子的有效预分类，使用膜计算的并行性和分布式的特点，在相同数量的粒子数目条件下，可以获得更快的全局最优值的搜索速度，在相同运行时间内可以分布更多的粒子数量，保证方法局部搜索精度的同时，扩大搜索范围，满足了全局搜索的多样性，促使预测粒子更快的分布在真实的位置附近，减缓粒子退化，有效提高了机器人定位的精度和速度。

为了达到上述目的，本发明所采用的技术方案为：

一种基于聚类和膜计算的改进FastSLAM方法，其特征在于，所述方法包括下述步骤：

(1)初始化，获取机器人当前时刻t的位姿x_t，环境地图特征map_t，从t时刻到t+1时刻，机器人运动量u_t，观测量z_t，观测量噪声协方差矩阵H_t；

(2)采样，在x_t周围随机设置L个粒子，每个粒子代表一个机器人可能的位姿，再根据提议概率分布其中(k)表示第k个位姿，对机器人在t+1时刻可能的位姿进行采样，越接近提议分布的粒子权重越大，得到Q个粒子，组成粒子集φ_t，设每个粒子权重为w^(k)，k∈[1,2,…,Q]：

(3)聚类，根据每个粒子的权重，将粒子集φ_t中所有粒子聚类为M个族群；

(3a)聚类初始化，粒子集φ_t中共有Q个粒子，随机选取M个粒子，作为M个族群的初始族群中心，记为(m)表示第m个族群，n表示迭代次数，设迭代次数初始值为n＝0，最大迭代次数设为N_k-means；