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

权利要求书

1.一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征是，包括以下步骤：

S1：初始化机器人初始时刻位姿，默认起始位置为零位；

S2：根据移动机器人t-1时刻的位姿信息，即t-1时刻的后验概率密度函数，通过已有的先验知识对当前时刻的状态进行预测，得到t时刻先验概率密度函数，根据先验概率密度函数生成采样粒子集p；采集N个粒子，对粒子的权值进行初始化处理，均为1/N；

S3：选取重要性概率密度函数，即运动模型和观测值相结合的建议分布，生成新的采样粒子集q，并计算粒子权重，更新粒子权值，并进行权值的归一化处理；

S4：根据外部传感器获取的观测信息，根据采样N个粒子以及得到的粒子权值，计算当前时刻t随机样本粒子的加权和来表示后验概率密度函数估计，得到当前时刻t移动机器人的位姿和环境的地图信息，作为输出，用于同时定位与地图构建时更新地图信息；

S5：判断是否有新的观测值输入，如果有新的观测值输入，判断是否有足够的有效粒子，设置有效粒子动态阈值T，将有效粒子数N_eff与T进行比较，如果N_eff小于T，则结合遗传算法进行重采样，然后返回S3，如果N_eff大于等于T，则直接返回S3；如果没有新的观测值输入，则结束循环，得到环境地图。

2.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：

S2所述的获取t时刻的先验概率密度函数的方法为：

根据t-1时刻的后验概率密度函数p(x_t-1|y_1:t-1)，通过已有的先验知识对t时刻的状态进行预测，得到t时刻的先验概率密度函数p(x_t|y_1:t-1)，服从一阶马尔科夫模型：

p(x_t|y_1:t-1)＝∫p(x_t|x_t-1)p(x_t-1|y_1:t-1)d_xt-1

其中，p(x_t|x_t-1)为状态变量的转移概率密度函数，x为机器人的位姿估计，y为外部传感器获取到的环境观测值。

3.根据权利要求1所述的一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法，其特征在于：

S2所述的由t时刻先验概率密度函数生成采样粒子集p，采集N个粒子，为：

{x0:ti,i=1~N}.]]>