一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法与流程

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

背景技术：

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

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

目前现有的常用于同时定位与地图构建的方法有：基于卡尔曼的滤波方法(kalmanfilter，缩写为kf)、基于扩展卡尔曼滤波算法(extendedkalmanfilter，缩写为ekf)、基于粒子滤波算法(particlefilter，缩写为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(xt-1|y1：t-1)，通过已有的先验知识对t时刻的状态进行预测，得到t时刻的先验概率密度函数p(xt|y1：t-1)，服从一阶马尔科夫模型：

p(xt|y1：t-1)＝∫p(xt|xt-1)p(xt-1|y1：t-1)dxt-1

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

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

s3具体为：

引入一个已知的、容易采样的重要性概率密度函数q(x0：t|y1：t)，选择接近系统状态后验概率，而且能使重要性权重方差最小，选择的重要性概率密度函数为：

根据重要性概率密度函数获得新的采样粒子集q，从粒子集q中随机抽样得到n个独立同分布的粒子：

此时f(xt)的期望表示为：

其中，f(xt)为系统的状态方程函数；w(xt)为重要性权重：

得到权重w(xt)，即表示为第i个粒子在t时刻的权值，并进行权值的归一化处理：

s4所述的获得环境地图的机器人位姿、更新地图的方法具体为：

根据分布获得的随机样本粒子的加权和来表示后验概率密度函数估计，得到t时刻移动机器人的位姿估计：

其中：表示为第i个粒子在t时刻的状态，粒子滤波器中的每个粒子都代表对机器人路径的估计，当粒子的后验概率密度函数，即t时刻机器人的位姿更新的时候，结合外部传感器获取到的观测信息，同时更新地图，即对环境信息进行特征提取和匹配，机器人的当前位置根据机器人起始位置和当前时刻的观测共同确定。

s5中有效例子为：

s5中有效粒子动态阈值t的范围设定在

与传统的机器人基于扩展卡尔曼滤波、粒子滤波的同时定位与地图构建不同，本发明采用改进的粒子滤波算法，针对不同的环境，设定不同的动态阈值，判断系统是否需要进行重采样，重采样过程结合了遗传算法，可以更好地保留好的粒子，减小了粒子退化问题对slam的影响，这样一来减小了slam问题的计算量，效果更好，实现起来更加方便。

粒子滤波是基于蒙特卡洛方法，将复杂的积分问题转化为求均值的问题，简化求解过程；粒子滤波对于基于控制量和观测量的后验概率分布可以准确的表达。改进的粒子滤波根据地图的复杂度，设定动态阈值，减小计算量；再加上机器人操作系统ros开发全面，代码开源，接口统一等优势，因此基于改进的粒子滤波算法的同时定位与地图构建方法可以作为移动机器人slam领域中一个非常有效而且高效的手段。

本发明涉及的方法解决了移动机器人在未知环境中的同时定位与地图构建问题，利用改进的粒子滤波算法，在机器人操作系统ros上实现地图的构建和机器人位姿的估计，为移动机器人接下来的导航问题奠定了基础，也可以直接应用到诸如工业机器人、服务型机器人或者探测机器人的领域中。

附图说明

图1是本发明方法流程图；

具体实施方式

基于改进的粒子滤波的移动机器人同时定位与地图构建方法，通过利用改进的粒子滤波算法，在选择重要性概率密度函数时，选用运动模型和观测值相结合的建议分布。在粒子滤波的重采样阶段，将用于判断有效粒子数的阈值设定为动态阈值，根据环境的复杂度选择不同的临界值；选择引入遗传算法，将该算法中的选择，交叉，变异，遗传也应用在粒子的选择中，即保证了粒子数充足，又防止粒子单一性。

下面结合附图对本发明进行详细说明。

首先初始化机器人初始时刻的位姿，一般定义相对位置，即设定初始时刻的位姿为(0，0，0)。

机器人的系统一般可以表示为它的状态方程和观测方程：

状态方程：xt＝f(xt-1，vt-1)

观测方程：yt＝h(xt，nt)

其中：x为系统状态，y为观测到的数据，f和h分别表示状态转移函数和测量函数，v和n分别表示过程噪声和测量噪声。

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

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

由当前时刻的先验概率生成采样粒子集p：

对粒子的权值进行初始化处理均为则粒子的分布p的一个估计可以表示为如下形式：

其中：δ(x-xt)为单位冲激函数(狄拉克函数)，即δ(x-xt)＝0，x≠xt，且∫δ(x)dx＝1

根据蒙特卡洛方法将复杂的积分过程近似为求和过程进行求解，当前状态的期望可以表示为：

选取重要性概率密度函数：

引入一个已知的、容易采样的重要性概率密度函数q(x0：t|y1：t)，可以分解为：

q(x0：t|y0：t)＝q(x0：t-1|y0：t-1)q(xt|x0：t-1，y0：t)

重要性概率密度函数结合了外部传感器获得的观测值y1：t和运动模型，尽可能的选择接近系统状态后验概率，选择重要性概率密度函数的一个标准是使得粒子权值的方差最小，提高抽样的效率。一般选择的重要性概率密度函数为：

根据重要性概率密度函数获得新的采样粒子集q，从粒子集q中随机抽样得到n个独立同分布的粒子：

引入重要性采样函数后，期望e[f(xt)]的可以表示为：

根据上述相应的关系，得到粒子的权重w(xt)：

更新当前的粒子的权值w(xt)，得到粒子权重的递归形式，即w(xt)和w(xt-1)的关系为：

粒子的权值归一化处理得：

根据分布获得的随机样本粒子的加权和来表示后验概率密度函数估计，得到t时刻移动机器人的位姿估计：

其中：表示为第i个粒子在t时刻的状态，粒子滤波器中的每个粒子都代表对机器人路径的估计。

当粒子的后验概率密度函数，即t时刻机器人的位姿更新的时候，作为系统的输出结合外部传感器获取到的观测信息，同时更新地图，即对环境信息进行特征提取和匹配，得到所处未知环境的地图。机器人的当前位置可以根据机器人起始位置和当前时刻的观测共同确定。

同时判断是否有新的观测值的输入，如果有，则重新进行上述步骤的循环，不断的更新地图，直至建图结束，不再有新的观测值输入。

判断如果有新的观测值的输入，循环重新计算之前，需要判断系统是否需要重采样。由于经过多次迭代后，受到环境和噪声的影响，粒子的权值方差会随着循环不断增大，权值退化严重，有效粒子数减少。利用有效粒子来衡量粒子权值的退化程度。有效粒子越小，权重方差越大，粒子权值退化严重。计算t时刻系统有效粒子为：

根据系统的复杂程度设定动态阈值t，与有效粒子进行比较。如果系统状态复杂，粒子数多，可设定动态阈值为如果系统状态简单，粒子数少，可设定动态阈值为动态阈值t的大致范围可以设定在如果有效粒子数小于阈值t，则结合遗传算法进行重采样后，返回执行选取重要性概率密度函数；反之，则跳过重采样，直接执行选取重要性概率密度函数。

若有效粒子小于设定的阈值，就需要进行重采样，以丰富粒子的多样性和有效性。

重采样的方法是：

保持粒子数目不变，在粒子归一化的基础上，通过服从u[0，1)的均匀分布中产生的随机数来确定粒子是否被复制。将权值大的粒子依照权值所占比例进行复制。在实现重采样时，可以结合遗传算法。对于权重大粒子，利用遗传算法中的选择、交叉、遗传、变异等方式，留下优良的粒子，并继续进行迭代。既保证了有效粒子个数的充足，还能保证粒子的多样性。即：对粒子集进行重采样，重采样后的粒子集为将重采样后的粒子集的权值赋值为

重采样后：

其中：为t时刻的粒子，为t时刻重采样后的粒子

ni是粒子产生粒子时被复制的次数