CN105509755B - 一种基于高斯分布的移动机器人同步定位与地图构建方法 - Google Patents

Abstract

本发明请求保护一种基于高斯分布重采样Rao‑Blackwellized粒子滤波的移动机器人同时定位与地图构建方法，该方法包括步骤：S1，由机器人位姿和里程计控制信息估计机器人初始位姿；S2，根据地图执行扫描匹配方法；S3，在轨迹的提议分布中进行粒子采样；S4，计算每个粒子的权重并更新各粒子权值；S5，在高斯分布的基础上进行粒子重采样，具体是通过排序粒子权重大小，分散高权重粒子获得重采样后的新粒子；S6，根据机器人位姿和观测信息计算地图并进行地图更新。本发明可以获得可靠的栅格地图精度。

Description

一种基于高斯分布的移动机器人同步定位与地图构建方法

技术领域

本发明属于移动机器人导航领域，特别是一种高斯分布的同时定位与地图构建方法

背景技术

移动机器人的同时定位与地图构建(Simultaneous Localization and Mapping,SLAM)是移动机器人在其自身位置不确定的条件下，在完全未知环境中根据自身的位姿估计和传感器观测数据创建地图。SLAM最早由Smith,Self和Cheeseman提出，它解决了从移动机器人得到一系列观测值从而构建未知关键地图的问题。

SLAM问题的研究中，对于非线性、非高斯系统的状态估计，目前研究热点是基于序列蒙特卡罗方法(Sequential Monte Carlo,SMC)的粒子滤波算法。该算法对系统噪声没有限制，它能通过预测和更新系统概论密度函数的采样集来近似非线性系统的递归贝叶斯估计。Murphy等人将Rao-Blackwellized粒子滤波(Rao-Blackwellized Particle Filter,RBPF)算法作为一种新的方式来处理SLAM问题。随后，Montemerlo等人于2003年提出了一种基于Rao-Blackwellized粒子滤波器的FastSLAM算法(RBPF-SLAM)。相比适合高斯线性系统的EKF-SLAM算法，RBPF-SLAM算法根据SLAM问题的条件独立特性，将高维的移动机器人轨迹和环境地图联合后验概率密度估计解耦为低维状态估计，从而解决了状态空间SLAM算法的维数灾难问题，提高了SLAM的求解效率。

然而随着粒子权值的方差随时间递增，粒子退化现象是不可避免的。为了减少粒子退化现象，引入重采样方法，即舍弃权值较小的粒子，复制权值较大的粒子，产生多个等权值的粒子。这样虽然改善了粒子退化，但同时造成粒子匮乏。为此，自适应的重采样的方法成为研究重点，Moral等人对自适应重采样策略及收敛性进行了详细分析，一些自适应改进重采样的SLAM算法被提出，虽然这些算法能够抑制样本匮乏现象，提高粒子滤波算法的估计准确性，但是该算法的代价仍然是增加了计算时间，虽然可以以较少的粒子数目来实现算法，但也仅仅能达到有限的精度与时间上的平衡。因此高斯分布重采样移动机器人同时定位与地图构建方法的研究RBPF-SLAM中保持粒子多样性，避免粒子匮乏、同时保证可靠精确的估计有非常重要的意义。

发明内容

针对现有技术的不足，提出了一种可获得可靠的栅格地图精度、保持粒子多样性的基于高斯分布的移动机器人同步定位与地图构建方法。本发明的技术方案如下：一种基于高斯分布的移动机器人同步定位与地图构建方法，其包括以下步骤：

S1，输入t-1时刻的粒子集，其中移动机器人在每个点的运行轨迹为粒子，移动机器人t-1时刻的观测值和t-1时刻施加的控制量，由机器人位姿和里程计控制信息估计机器人初始位姿；

S2，根据移动机器人的初始位姿和传感器观测数据创建地图，并根据地图执行扫描匹配方法得出移动机器人的轨迹；

S3，根据步骤S2扫描匹配方法得出移动机器人的轨迹，求出移动机器人轨迹的提议分布，将提议分布近似为高斯分布，在高斯分布中进行粒子采样；

S4，计算每个粒子的权重并更新各粒子权值；

S5，在高斯分布的基础上进行粒子重采样，具体是通过排序粒子权重大小，分散高权重粒子获得重采样后的新粒子；

S6，根据机器人位姿和观测信息计算地图并进行地图更新。

进一步的，步骤S2扫描方法通过来判断扫描匹配是否成功。扫描匹配方法的步骤为：

首先由后验分布递归贝叶斯滤波器结合传感器数据来估计机器人的位姿，形成初始样本集为：

其中为各位姿采样集合，x₁,x₂,...,x_t代表每一次机器人可能的位姿。

样本集的后验概率密度为

其中，为能使得该后验概率密度函数最大的值。

进一步的，步骤S3求取轨迹提议分布的计算方法为：

首先根据观测模型的似然函数为：

将提议分布近似为高斯分布：

其高斯分布参数和分布为：

其中，L⁽ⁱ⁾是观测模型的似然函数，x是该似然函数的自变量，是观测模型的概率表达，z_t是观测值，是t-1时刻的地图，为归一化因子，是里程计读数，{x_j}取里程计的最近一次读数。

进一步的，步骤S4中每个粒子的权重的计算方法为：

对每个粒子的权重进一步简化：

其中表示t时刻第i个粒子的权重大小，是运动模型的概率表达。

进一步的，步骤S5中，高斯分布中进行粒子重采样的计算方法：

其中α是调整分布程度的参数，i是进入采样区粒子的序列数，j是当前新得到粒子的序列数，Σ_k表示新粒子离散程度的方差，设置方差来分散高权重粒子。

本发明的优点及有益效果如下：

本发明提供了一种在高斯分布的基础上分散高权重粒子获得重采样后的新粒子，保证了粒子多样性，从而避免粒子匮乏而造成栅格地图构建不精确。在粒子数减少的条件下保持可靠的估计，有效地减轻了计算压力。

附图说明

图1是本发明提供优选实施例基于高斯分布的移动机器人同步定位与地图构建方法流程图。

具体实施方式

以下结合附图，对本发明作进一步说明：

如图1所示，本发明提供了一种基于高斯分布的移动机器人同步定位与地图构建，其特征在于，包括以下步骤：

S1，输入量为t-1时刻的粒子集，t-1时刻的观测值和t-1时刻施加的控制量，由机器人位姿和里程计控制信息u_t-1估计机器人初始位姿

S2，根据地图执行扫描匹配方法，通过来判断扫描匹配是否成功；扫描匹配方法的步骤为：

首先由后验分布递归贝叶斯滤波器结合传感器数据来估计机器人的位姿，形成初始样本集为：

其中为各位姿采样集合，x₁,x₂,...,x_t代表每一次机器人可能的位姿。

样本集的后验概率密度为

其中，为能使得该后验概率密度函数最大的值。

S3，在轨迹的提议分布中进行粒子采样；在本实施方式中，具体计算方法为：

观测模型的似然函数为：

将提议分布近似为高斯分布：

其高斯分布参数和分布为：

其中，L⁽ⁱ⁾是观测模型的似然函数，x是该似然函数的自变量，是观测模型的概率表达，z_t是观测值，是t-1时刻的地图，为归一化因子，是里程计读数，{x_j}取里程计的最近一次读数。

S4，计算完步骤S3中涉及的提议分布之后，在本实施方式中，每个粒子的权重的计算方法为；

可对上式进一步简化：

其中表示t时刻第i个粒子的权重大小，是运动模型的概率表达。

S5，对S4每个粒子权重大小计算之后，在本实施方式中，在高斯分布的基础上进行粒子重采样，具体是通过排序粒子权重大小，分散高权重粒子获得重采样后的新粒子，在高斯分布中进行粒子重采样的计算方法：

其中α是调整分布程度的参数，i是进入采样区粒子的序列数，j是当前新得到粒子的序列数，Σ_k表示新粒子离散程度的方差，设置方差来分散高权重粒子。

S6，根据机器人位姿和观测信息计算地图并进行地图更新。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (4)

1.一种基于高斯分布的移动机器人同步定位与地图构建方法，其特征在于，包括以下步骤：

S1，输入t-1时刻的粒子集，其中移动机器人在每个点的运行轨迹为粒子，移动机器人t-1时刻的观测值和t-1时刻施加的控制量，由机器人位姿和里程计控制信息估计机器人初始位姿；

S2，根据移动机器人的初始位姿和传感器观测数据创建地图，并根据地图执行扫描匹配方法得出预估的移动机器人的轨迹；

S3，根据步骤S2扫描匹配方法得出移动机器人的轨迹，求出移动机器人轨迹的提议分布，将提议分布近似为高斯分布，在高斯分布中进行粒子采样；

S4，计算每个粒子的权重并更新各粒子权值；

S5，在高斯分布的基础上进行粒子重采样，具体是通过排序粒子权重大小，分散高权重粒子获得重采样后的新粒子；步骤S5中，高斯分布中进行粒子重采样的计算方法：

其中α是调整分布程度的参数，i是进入采样区粒子的序列数，j是当前新得到粒子的序列数，Σ_k表示新粒子离散程度的方差，设置方差来分散高权重粒子；

S6，根据机器人位姿和观测信息计算地图并进行地图更新。

2.根据权利要求1所述的一种基于高斯分布的移动机器人同步定位与地图构建方法，其特征在于，步骤S2扫描方法通过来判断扫描匹配是否成功，扫描匹配方法的步骤为：z_t是观测值，是t-1时刻的地图；

首先由后验分布递归贝叶斯滤波器结合传感器数据来估计机器人的位姿，形成初始样本集为：

其中为各位姿采样集合，x₁,x₂,...,x_t代表每一次机器人可能的位姿；

样本集的后验概率密度为

其中，为能使得该后验概率密度函数最大的值。

3.根据权利要求1或2所述的一种基于高斯分布的移动机器人同步定位与地图构建方法，其特征在于，步骤S3求取轨迹提议分布的计算方法为：

首先根据观测模型的似然函数为：

将提议分布近似为高斯分布：

其高斯分布参数和分布为：

其中，L⁽ⁱ⁾是观测模型的似然函数，x是该似然函数的自变量，是观测模型的概率表达，z_t是观测值，是t-1时刻的地图，为归一化因子，是里程计读数，{x_j}取里程计的最近一次读数。

4.根据权利要求3所述的一种基于高斯分布的移动机器人同步定位与地图构建方法，其特征在于，步骤S4中每个粒子的权重的计算方法为：

对每个粒子的权重进一步简化：

其中表示t时刻第i个粒子的权重大小，是运动模型的概率表达。