[发明专利]一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法

摘要

本发明公开了一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，初始化机器人初始时刻位姿；根据t‑1时刻的位姿信息，得到t时刻先验概率密度函数，生成采样粒子集p；对粒子的权值进行初始化处理；选取重要性概率密度函数，生成新采样粒子集q，计算粒子权重，更新粒子权值，进行权值的归一化处理；计算当前时刻t随机样本粒子的加权和来表示后验概率密度，得到移动位姿和环境地图信息；判断是否有新观测值输入，如果有则返回，如果没有则结束循环，在返回之前，判断是否需要进行重采样。根据系统状态的不同，设定动态的阈值进行判断，并结合了遗传算法。本发明减小了粒子退化问题对SLAM的影响，减小了SLAM问题的计算量。

主权项

一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征是，包括以下步骤：S1：初始化机器人初始时刻位姿，默认起始位置为零位；S2：根据移动机器人t‑1时刻的位姿信息，即t‑1时刻的后验概率密度函数，通过已有的先验知识对当前时刻的状态进行预测，得到t时刻先验概率密度函数，根据先验概率密度函数生成采样粒子集p；采集N个粒子，对粒子的权值进行初始化处理，均为1/N；S3：选取重要性概率密度函数，即运动模型和观测值相结合的建议分布，生成新的采样粒子集q，并计算粒子权重，更新粒子权值，并进行权值的归一化处理；S4：根据外部传感器获取的观测信息，根据采样N个粒子以及得到的粒子权值，计算当前时刻t随机样本粒子的加权和来表示后验概率密度函数估计，得到当前时刻t移动机器人的位姿和环境的地图信息，作为输出，用于同时定位与地图构建时更新地图信息；S5：判断是否有新的观测值输入，如果有新的观测值输入，判断是否有足够的有效粒子，设置有效粒子动态阈值T，将有效粒子数Neff与T进行比较，如果Neff小于T，则结合遗传算法进行重采样，然后返回S3，如果Neff大于等于T，则直接返回S3；如果没有新的观测值输入，则结束循环，得到环境地图。