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

说明书

技术领域

本发明涉及移动机器人自主定位与环境感知技术领域，特别涉及一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法。

背景技术

伴随着机器人技术的蓬勃发展，机器人对未知环境的自主认知能力已经成为机器人学中研究的一个热点。构建未知环境的地图，也就是移动机器人的定位与导航问题是机器人自主认知的其中一项重点研究内容和研究热点。其中，同时定位与地图构建(SLAM)则是移动机器人实现定位和导航的一种有效手段，就是机器人根据获取到的传感器相关数据，提取特征，经过特征匹配，最终自主构建机器人所处未知环境的地图并同时获取自身位置，对自身进行定位。进而，可以利用构建好的环境地图，继续进行其他的相关研究，如：路径规划，导航避障，完成任务等。

早期的未知环境的地图构建常用的方式是航迹推算法，一般都是假设获得到的自身传感器数据是精确的，并且假设机器人的位姿是可以完全通过传感器数据得到。根据这种方法，提出了多种构建环境地图的方式，如：基于卡尔曼滤波的构建特征地图的方法。实际上，通过航迹推算，即只通过自身传感器获得机器人的位姿是不准确的，它会存在一定的测量误差。误差的来源主要是轮子打滑、地面的摩擦等因素，而且这些误差会随着时间的增长而累积，最终导致机器人完全偏离实际的航线，所建的未知环境地图完全错误。

目前现有的常用于同时定位与地图构建的方法有：基于卡尔曼的滤波方法(Kalman Filter，缩写为KF)、基于扩展卡尔曼滤波算法(Extended Kalman Filter，缩写为EKF)、基于粒子滤波算法(Particle Filter，缩写为PF)、基于Markov原理的蒙特卡洛方法、基于最大似然估计的SLAM方法等。

卡尔曼滤波和扩展卡尔曼滤波对于系统的要求比较高，需要对运动模型和观测模型进行近似线性化处理，而且要求系统噪声为白噪声。在计算时，运用卡尔曼滤波算法对系统的状态递归的进行估计，需要反复的计算协防矩阵，计算量极大。扩展卡尔曼滤波在SLAM问题中应用很广泛，也是很多其他方法的理论基础。但是，该方法存在对系统的要求高以及计算量大的问题，算法复杂、用时长，而且近似线性化的过程会引入线性化误差。

粒子滤波算法的基本思想是用随机样本来描述概率分布。在测量的基础上，通过调节各个粒子的权值大小和样本位置来近似实际的概率分布，并以样本均值作为系统的估计值。但是粒子滤波存在计算量较大，还有经过多次迭代，粒子会存在退化的问题。

发明内容

本发明的目的在于提供一种实现未知环境下的移动机器人建图与定位功能的基于改进的粒子滤波的移动机器人同时定位与地图构建的方法。

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

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

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

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

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

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

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

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