基于分布式边缘无味粒子滤波的同步定位与地图构建方法

基于分布式边缘无味粒子滤波的同步定位与地图构建方法
【专利摘要】本发明涉及一种基于分布式边缘无味粒子滤波的同步定位与地图构建方法，首先建立坐标系并初始化环境地图；然后分别给匹配成功的各路标点建立子滤波器；接着在机器人运动模型的基础上，分别在各子滤波器中产生粒子群，获得每个粒子的状态向量及其方差；引入噪声，利用无味变换计算扩展后的粒子状态向量，并更新扩展后的粒子优化粒子群；然后计算粒子权值并归一化，统计每个子滤波器的聚合数据并将数据传送给主滤波器；接着计算全局估计和方差；其次判断每个子滤波器的有效抽样尺度和采样阈值，对粒子退化严重的子滤波器进行重采样；然后输出机器人状态向量和其方差，并存入地图。最后使用卡尔曼滤波算法更新路标点状态，直到机器人不再运行为止。
【专利说明】基于分布式边缘无味粒子滤波的同步定位与地图构建方法
一、【技术领域】
[0001]基于分布式边缘无味粒子滤波的同步定位与地图构建方法能够使机器人在未知环境中实现自主定位与地图创建，属于机器人自主导航领域。
二、【背景技术】
[0002]同步定位与地图构建(Simultaneous Localization and Mapping, SLAM)其基本思想是:让机器人在未知环境中从未知位置开始移动，通过自身所带传感器扫描到的路标点的信息进行自身位置估计，同时构建增量式地图。自从移动机器人诞生以来，对定位问题的研究就和地图创建问题密切相关，因而同步定位与地图构建有着重要的理论与应用价值，被很多学者认为是实现机器人真正自主移动的关键，现已被广泛用于众多领域。
[0003]粒子滤波(Particle Filter)被认为是目前最适于解决非线性非高斯模型的方法。近年来发展起来的基于分布式粒子滤波的同步定位与地图构建方法将SLAM中的状态向量分解为机器人状态(位姿)估计和路标点状态估计两部分，依据传感器在当前时刻测得的并已经在地图中存在的每个路标点(即匹配的路标点)分别建立子滤波器，从而实现对机器人位姿和路标的估计，能够有效提高系统的容错能力。因此，分布式粒子滤波相比其他方法更适于解决SLAM问题。然而，采用粒子滤波对SLAM进行状态估计无可避免的会受到粒子滤波固有问题的制约，因而存在以下主要的缺陷:
[0004](I)粒子退化问题:随着迭代次数的增加，重要性权重的方差随时间递增，粒子权重逐渐集中到少数粒子上，有较大权值的粒子被多次选择，使得粒子集多样性变差，不足以用来近似表达后验概率分布，因而难以保证估计精度，导致滤波容易发散。
[0005](2)重要性函数选择问题:一般粒子滤波算法选择先验概率密度作为重要密度函数，没有考虑当前测量值和模型噪声，从重要性概率密度中取样得到的样本与从真实后验概率密度采样得到的样本有很大偏差，从而影响估计精度。
三、
【发明内容】

[0006]本发明针对现有分布式粒子滤波SLAM方法的不足，提出一种分布式边缘无味粒子滤波SLAM方法，以抑制粒子退化现象，提高系统的估计精度。
[0007]本发明方法的整体流程见图1。该方法的整体流程可分为三个部分:数据关联部分，状态估计部分及地图更新部分。其中数据关联部分包括在传感器测量范围内通过传感器测得环境路标点状态信息(即路标点与机器人的相对距离和角度)，并与构建的环境地图中已存储的路标点相匹配(即该路标点已经在地图中存在)；状态估计部分包括对所有匹配的路标点分别建立子滤波器并对机器人位姿进行估计，然后对各个子滤波器的估计结果进行汇总并将其传送给主滤波器，从而获得精确的机器人位姿估计；地图更新部分包括将传感器测得的周围环境信息加以处理，结合机器人位姿估计值完成从传感器观测到全局地图的映射，并对机器人和路标点位置信息进行实时更新。该方法首先进行初始化并建立坐标系。根据首次传感器扫描结果建立并存储初始环境地图。然后分别给传感器测得的能匹配路标点(首次扫描不考虑匹配)建立粒子滤波器滤波器，作为子滤波器并行分布处理各个传感器数据。接着在机器人运动模型的基础上，在各子滤波器中产生粒子群，并获得每个粒子的状态向量及其方差。引入噪声，扩展粒子的状态向量和方差的维数。经过无味变换，计算粒子状态的扩展向量，并对变换后的粒子群进行时间更新和测量更新。然后计算粒子权值并归一化，统计每个子滤波器的聚合数据并将数据传送给主滤波器。接着计算全局估计和方差，从而得到较为准确的机器人状态。其次分别计算各子滤波器的有效抽样尺度，根据每个子滤波器的有效粒子数和门限值间关系，对粒子退化严重的子滤波器进行重采样，并定期对主滤波器进行全局重采样。然后输出机器人状态向量和其方差，并存入地图。最后使用卡尔曼滤波算法对路标点状态和方差进行更新更新，时间后置为下一时刻，判断机器人是否继续运动，如继续则传感器重新扫描环境信息，获得新的路标点状态信息，若新测得的路标点能与地图中信息相匹配则建立子滤波器程序循环，否则将没匹配上的路标点状态直接存储到地图中，如机器人不继续运行则程序终止。本方法能够有效抑制粒子的退化现象，并具有很好的滤波精度。本发明在分布式SLAM方法的基础上选取易于采样的高斯分布函数近似机器人系统状态的真实分布，融入了当前观测值，使得重要性函数尽可能接近机器人系统状态的真实后验概率分布，从而提高算法的滤波精度；优化分布式粒子滤波中粒子的权值，减小权系数的方差，增加有效抽样尺度的估计值，从而解决粒子退化问题；在各个子滤波器自适应的根据样本情况进行重采样的同时定期进行全局重采样，从而保证粒子集的一致性。该方法具体包括如下步骤:
[0008]I)初始化。
[0009]建立坐标系，以机器人初始时刻的位置为坐标原点，以正东和正北方向为X和y轴的正方向；
[0010]全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿(即机器人的位置和角度)一起作为全局地图进行存储；
[0011]2)地图匹配并分布式产生粒子群。
[0012]获取机器人k-Ι时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配(初始时刻不需要考虑匹配)，对于k-Ι时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图，与地图中已存储的路标点信息一起用于下一时刻的路标点匹配；对于k-Ι时刻测得的能够匹配上的路标点分别对其建立子滤波器，所述的子滤波器均为粒子滤波器，用于并行分布式对机器人下一时刻的状态和路标点信息进行估计，从而提高算法的容错能力和估计精度；
[0013]然后，在机器人运动模型的基础上，加入均值为O的高斯白噪声，从而在每个子滤波器中产生位置随机粒子数相等的粒子群，粒子群中每一个粒子的状态代表机器人可能的状态，粒子群中每个粒子群的位置均在机器人测量位置周围，用以精确估计机器人实际位置；
[0014]由于当前时刻机器人的状态向量等于上一时刻机器人状态向量和机器人在两时刻间机器人状态的变化量之和，因此第j个子滤波器中第i个粒子在k-Ι时刻的状态向量
为处丨:[0015]X1kIl ^xilI1 +AXij
[0016]其中，Xg包含?、也、φ i三个分量分别为第j个子滤波器中第i个粒子在k-Ι时刻的位置和角度，X'iU为第j个子滤波器中第i个粒子在k-2时刻的状态向量，AX1-J
为在k-Ι与k-2机器人状态向量的变化量；
[0017]3)引入噪声扩展粒子维数。
[0018]引入噪声扩展粒子维数，对步骤2中的$'引入噪声来扩展该状态向量的维数，以
获得扩展后的各个子滤波器中粒子的状态向量和方差；
[0019]扩展后的k-Ι时刻的第j个子滤波器中第i个粒子的状态向量为:
[0020]
【权利要求】
1.基于分布式边缘无味粒子滤波的同步定位与地图构建方法，基于四轮机器人，其特征在于依次包括下述步骤: 1)初始化， 建立坐标系，以机器人初始时刻的位置为坐标原点； 全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿一起作为全局地图进行存储； 2)获取机器人k-Ι时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配，对于k-Ι时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图，与地图中已存储的路标点信息一起用于下一时刻的路标点匹配；对于k-Ι时刻测得的能够匹配上的路标点分别对其建立子滤波器，所述的子滤波器均为粒子滤波器，用于并行分布式对机器人下一时刻的状态和路标点信息进行估计； 然后，在机器人运动模型的基础上，加入均值为O的高斯白噪声，从而在每个子滤波器中产生位置随机粒子数相等的粒子群，粒子群中每一个粒子的状态代表机器人可能的状态； 其中，第j个子滤波器中第i个粒子在k-Ι时刻的状态向量为
【文档编号】G01C21/00GK103644903SQ201310424318
【公开日】2014年3月19日 申请日期:2013年9月17日 优先权日:2013年9月17日
【发明者】裴福俊, 李昊洋, 武玫 申请人:北京工业大学