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

Abstract

本发明涉及一种基于分布式边缘无味粒子滤波的同步定位与地图构建方法，首先建立坐标系并初始化环境地图；然后分别给匹配成功的各路标点建立子滤波器；接着在机器人运动模型的基础上，分别在各子滤波器中产生粒子群，获得每个粒子的状态向量及其方差；引入噪声，利用无味变换计算扩展后的粒子状态向量，并更新扩展后的粒子优化粒子群；然后计算粒子权值并归一化，统计每个子滤波器的聚合数据并将数据传送给主滤波器；接着计算全局估计和方差；其次判断每个子滤波器的有效抽样尺度和采样阈值，对粒子退化严重的子滤波器进行重采样；然后输出机器人状态向量和其方差，并存入地图。最后使用卡尔曼滤波算法更新路标点状态，直到机器人不再运行为止。

Description

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

一、技术领域

基于分布式边缘无味粒子滤波的同步定位与地图构建方法能够使机器人在未知环境中实现自主定位与地图创建，属于机器人自主导航领域。

二、背景技术

同步定位与地图构建（SimultaneousLocalizationandMapping,SLAM）其基本思想是：让机器人在未知环境中从未知位置开始移动,通过自身所带传感器扫描到的路标点的信息进行自身位置估计，同时构建增量式地图。自从移动机器人诞生以来,对定位问题的研究就和地图创建问题密切相关，因而同步定位与地图构建有着重要的理论与应用价值，被很多学者认为是实现机器人真正自主移动的关键，现已被广泛用于众多领域。

粒子滤波（ParticleFilter）被认为是目前最适于解决非线性非高斯模型的方法。近年来发展起来的基于分布式粒子滤波的同步定位与地图构建方法将SLAM中的状态向量分解为机器人状态（位姿）估计和路标点状态估计两部分，依据传感器在当前时刻测得的并已经在地图中存在的每个路标点（即匹配的路标点）分别建立子滤波器，从而实现对机器人位姿和路标的估计，能够有效提高系统的容错能力。因此，分布式粒子滤波相比其他方法更适于解决SLAM问题。然而，采用粒子滤波对SLAM进行状态估计无可避免的会受到粒子滤波固有问题的制约，因而存在以下主要的缺陷：

(1)粒子退化问题：随着迭代次数的增加，重要性权重的方差随时间递增，粒子权重逐渐集中到少数粒子上，有较大权值的粒子被多次选择，使得粒子集多样性变差，不足以用来近似表达后验概率分布，因而难以保证估计精度，导致滤波容易发散。

(2)重要性函数选择问题：一般粒子滤波算法选择先验概率密度作为重要密度函数，没有考虑当前测量值和模型噪声，从重要性概率密度中取样得到的样本与从真实后验概率密度采样得到的样本有很大偏差，从而影响估计精度。

三、发明内容

本发明针对现有分布式粒子滤波SLAM方法的不足，提出一种分布式边缘无味粒子滤波SLAM方法，以抑制粒子退化现象，提高系统的估计精度。

本发明方法的整体流程见图1。该方法的整体流程可分为三个部分：数据关联部分，状态估计部分及地图更新部分。其中数据关联部分包括在传感器测量范围内通过传感器测得环境路标点状态信息（即路标点与机器人的相对距离和角度），并与构建的环境地图中已存储的路标点相匹配（即该路标点已经在地图中存在）；状态估计部分包括对所有匹配的路标点分别建立子滤波器并对机器人位姿进行估计，然后对各个子滤波器的估计结果进行汇总并将其传送给主滤波器，从而获得精确的机器人位姿估计；地图更新部分包括将传感器测得的周围环境信息加以处理，结合机器人位姿估计值完成从传感器观测到全局地图的映射，并对机器人和路标点位置信息进行实时更新。该方法首先进行初始化并建立坐标系。根据首次传感器扫描结果建立并存储初始环境地图。然后分别给传感器测得的能匹配路标点（首次扫描不考虑匹配）建立粒子滤波器滤波器，作为子滤波器并行分布处理各个传感器数据。接着在机器人运动模型的基础上，在各子滤波器中产生粒子群，并获得每个粒子的状态向量及其方差。引入噪声，扩展粒子的状态向量和方差的维数。经过无味变换，计算粒子状态的扩展向量，并对变换后的粒子群进行时间更新和测量更新。然后计算粒子权值并归一化，统计每个子滤波器的聚合数据并将数据传送给主滤波器。接着计算全局估计和方差，从而得到较为准确的机器人状态。其次分别计算各子滤波器的有效抽样尺度，根据每个子滤波器的有效粒子数和门限值间关系，对粒子退化严重的子滤波器进行重采样，并定期对主滤波器进行全局重采样。然后输出机器人状态向量和其方差，并存入地图。最后使用卡尔曼滤波算法对路标点状态和方差进行更新更新，时间后置为下一时刻，判断机器人是否继续运动，如继续则传感器重新扫描环境信息，获得新的路标点状态信息，若新测得的路标点能与地图中信息相匹配则建立子滤波器程序循环，否则将没匹配上的路标点状态直接存储到地图中，如机器人不继续运行则程序终止。本方法能够有效抑制粒子的退化现象，并具有很好的滤波精度。本发明在分布式SLAM方法的基础上选取易于采样的高斯分布函数近似机器人系统状态的真实分布，融入了当前观测值，使得重要性函数尽可能接近机器人系统状态的真实后验概率分布，从而提高算法的滤波精度；优化分布式粒子滤波中粒子的权值，减小权系数的方差，增加有效抽样尺度的估计值，从而解决粒子退化问题；在各个子滤波器自适应的根据样本情况进行重采样的同时定期进行全局重采样，从而保证粒子集的一致性。该方法具体包括如下步骤：

1)初始化。

建立坐标系，以机器人初始时刻的位置为坐标原点，以正东和正北方向为x和y轴的正方向；

全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿（即机器人的位置和角度）一起作为全局地图进行存储；

2)地图匹配并分布式产生粒子群。

获取机器人k-1时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配（初始时刻不需要考虑匹配），对于k-1时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图，与地图中已存储的路标点信息一起用于下一时刻的路标点匹配；对于k-1时刻测得的能够匹配上的路标点分别对其建立子滤波器，所述的子滤波器均为粒子滤波器，用于并行分布式对机器人下一时刻的状态和路标点信息进行估计，从而提高算法的容错能力和估计精度；

然后，在机器人运动模型的基础上，加入均值为0的高斯白噪声，从而在每个子滤波器中产生位置随机粒子数相等的粒子群，粒子群中每一个粒子的状态代表机器人可能的状态，粒子群中每个粒子群的位置均在机器人测量位置周围，用以精确估计机器人实际位置；

由于当前时刻机器人的状态向量等于上一时刻机器人状态向量和机器人在两时刻间机器人状态的变化量之和，因此第j个子滤波器中第i个粒子在k-1时刻的状态向量为

$X_{k-1}^{i,j}=X_{k-2}^{i,j}+{\mathrm{\ΔX}}^{i,j}$

其中，包含三个分量分别为第j个子滤波器中第i个粒子在k-1时刻的位置和角度，为第j个子滤波器中第i个粒子在k-2时刻的状态向量，ΔX^i,j为在k-1与k-2机器人状态向量的变化量；

3)引入噪声扩展粒子维数。

引入噪声扩展粒子维数，对步骤2中的引入噪声来扩展该状态向量的维数，以获得扩展后的各个子滤波器中粒子的状态向量和方差；

扩展后的k-1时刻的第j个子滤波器中第i个粒子的状态向量为：

$X_{k-1}^{i,j,a}=E\left[X_{k-1}^{i,j}\right]=\left[\begin{array}{ccc}{X}_{k-1}^{i,j}& 0& 0\end{array}\right];$

其方差为：

$P_{k-1}^{i,j,a}=E\left[(X_{k-1}^{i,j}-{\stackrel{\‾}{X}}_{k-1}^j){(X_{k-1}^{i,j}-{\stackrel{\‾}{X}}_{k-1}^j)}^T\right]=\left[\begin{array}{ccc}{p}_{k-1}^{i,j}& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right]$

其中a为扩展后状态向量和其方差矩阵的维数，为k-1时刻第j个子滤波器中第i个粒子的状态向量，为k-1时刻第j个子滤波器中第i个粒子的状态向量的方差，过程噪声和测量噪声均为不相关的零均值高斯白噪声，其方差分别为Q和R；

4)通过无味卡尔曼滤波算法更新优化粒子群。

首先对步骤3中得到的扩展后的粒子状态向量进行无味变换，得到变换后的粒子的状态向量然后对其进行时间更新和测量更新，最后根据更新结果对粒子群进行更新优化；

时间更新具体如下：

首先把k-1时刻的每个粒子对机器人状态估计的变换后的粒子状态向量分别代入机器人运动模型f(·)和路标点观测模型h(·)，得到每个粒子对k时刻机器人状态的一步预测向量和观测向量的一步预测向量

然后计算每个粒子的一阶统计特性的权系数和二阶统计特性的权系数

最后根据以上数据计算每个子滤波器中所有粒子的状态向量的一步预测加权和和观测向量的一步预测加权和

测量更新具体如下：

首先计算每个子滤波器中粒子对机器人状态的一步预测误差和观测的一步预测误差

然后计算每个子滤波器的滤波增益K^j；

最终得到k时刻的每个子滤波器对机器人状态的估计和其方差的估计

更新优化粒子群具体如下：

根据得到的k时刻的状态和方差分别在每个子滤波器中构造一个均值为方差为的高斯分布的函数，并根据该函数重新生成每个子滤波器的粒子群，得到新的每个子滤波器中粒子的状态向量计算粒子权值并归一化；

5)根据步骤4中生成的粒子群，计算每个子滤波器中粒子的权重值，其中第j个子滤波器中第i个粒子在k时刻的粒子权值的计算公式如下：

${\stackrel{\text{~}}{\mathrm{\ω}}}_k^{i,j}=\frac{P\left(Z_k^{i,j}\right|X_k^{i,j})\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\text{\ω}}_{k-1}^{i,j}P\left(X_k^{i,j}\right|X_{k-1}^{i,j})}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{i,j}q\left(X_k^{i,j}\right|Z_k^{i,j},X_{k-1}^{i,j})}$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，为当前时刻与第j个子滤波器中第i个粒子相对应的传感器测得的观测信息,为第j个子滤波器中第i个粒子在k-1时刻的权值,N为子滤波器中粒子数，为关于和的后验估计，为关于和的后验估计，为关于和的后验密度函数；

然后归一化权值，具体如下：

计算k时刻每个子滤波器中粒子的权值和，其中第j个子滤波器中粒子在k时刻的权值和的计算公式如下：

${\mathrm{\ω}}_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\ω}}}_k^{i,j}$

计算k时刻每个子滤波器中每个粒子的归一化权值，其中第j个子滤波器中第i个粒子在k时刻的归一化权值的计算公式如下：

${\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}=\frac{{\widetilde{\mathrm{\ω}}}_k^{i,j}}{{\mathrm{\ω}}_k^j}$

6)根据步骤5中得到的中每个粒子的归一化权值计算每个子滤波器在k时刻的聚合数据，其中，表示第j个子滤波器在k时刻的聚合数据；

$S_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$X_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{X}_k^{i,j}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$G_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}\right)}^2$

$P_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}{X}_k^{i,j}{\left(X_k^{i,j}\right)}^T$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，N为子滤波器中粒子数，为未归一化时子滤波器中粒子权重的和，为未归一化时的本地估计，为机器人的状态估计误差，为粒子权重的平方和，用于重采样时计算有效粒子数；

7)把所有子滤波器中所有粒子的状态向量及其权值传送给主滤波器，通过主滤波器对子滤波器中所有数据进行统计计算，最终输出k时刻机器人的全局状态估计和方差从而得到较为准确的机器人状态；

8)重采样，分别计算各子滤波器的有效抽样尺度，当所有子滤波器的有效抽样尺度均大于采样阈值时,直接进行第9步，否则，对于有效抽样尺度均小于采样阈值的子滤波器重复步骤2至8；

9)把所有子滤波器得到的最后一次k时刻机器人的全局状态估计和方差作为最终结果输出，并计算机器人的全局状态估计的均方根误差，然后把所有子滤波器得到的最后一次k时刻机器人的全局估计存入全局地图；

10)路标点更新，使用卡尔曼滤波算法对k-1时刻的路标点的状态向量及其方差进行时间更新和量测更新，获得k时刻路标点状态量的全局估计和方差，并将其存入全局地图，与全局地图中已存储的路标点一起用于下一时刻的地图匹配的；

11)判断机器人是否继续运动，如果继续则跳转至步骤2，否则程序终止。

四、有益效果

本发明方法保留了分布式粒子滤波SLAM方法的分布式思想，将SLAM问题分解为对移动机器人的状态估计和基于估计路径的环境特征的位置估计，从而降低状态量的维数，能够在降低算法的计算复杂度的同时提高系统的容错能力。在此基础上，本发明方法选取的重要性函数易于采样，并引入了当前观测信息，能够有效近似概率密度的真实分布，从而提高SLAM的估计精度；通过采用边缘化算法优化粒子的权值，降低粒子重要性权值方差，有效抑制粒子退化现象，从而提高滤波精度。因此，本发明方法能够有效解决一般分布式粒子滤波SLAM方法的粒子退化问题，同时具有良好的滤波精度。

五、附图说明

图1：基于本发明方法的整体流程图；

图2：车辆机器人参数具体位置示意图；

图3：车辆机器人坐标系-建立机器人运动模型；

图4：车辆基于本发明方法和DPF方法进行同步定位与地图构建并与GPS数据相比较的效果图；

图5：用本发明方法和DPF方法进行同步定位与地图构建的均方根误差比较图，其中：

图5(a)为在两种方法下车辆x方向的均方根误差、图5(b)为在两种方法下车辆y方向的均方根误差。

六、具体实施方式

1、系统模型

建立坐标系，机器人运动模型和路标点的状态模型建立在机器人坐标系中，路标点的观测模型建立在传感器坐标系中，车辆机器人坐标系如图3所示。从概率的角度考虑，SLAM问题中移动机器人在k时刻的运动状态可由k-1时刻的机器人运动状态决定。假设环境中路标点是静止不动的，可建立如下机器人运动模型：

其中，和分别为机器人在前后两时刻间的在x方向和y方向位置变化量和机器人角度变化量，α为前后两时刻间机器人角度的变化量，为根据角度变化量和前一时刻的机器人角度求出的现在时刻的机器人角度值，v_c为机器人移动速度，为机器人在k时刻的状态，

以及路标点的状态模型，此处假设路标点静止不动:

$X_{k+1|k}^I=X_{k|k}^I=\left[\begin{array}{c}{x}_{k|k}^I\\ y_{k|k}^I\end{array}\right]$

将机器人状态量与观测到的路标点的信息相联系，可得到如下路标点的观测模型：

其中，Z＝(r_iβ_i)^T为每一个路标点的观测状态，其中r_i为第i个路标点的传感器测得的机器人与路标点间距离，β_i为第i个路标点路标点到机器人与水平方向的夹角，X_I＝(x_iy_i)^T为每个路标点的坐标，(X_LY_L)^T为机器人当前坐标，a为角度系数，η是引入的观测噪声。

2、本发明基于分布式边缘无味粒子滤波的同步定位与地图构建方法，参照图1和图2，本发明方法的具体实施步骤包括如下：

步骤1：初始化。

建立坐标系，以机器人初始时刻的位置为坐标原点，以正东和正北方向为x和y轴的正方向；

全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿（即机器人的位置和角度）一起作为全局地图进行存储；

步骤2：地图匹配并分布式产生粒子群。

获取机器人k-1时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配，对于k-1时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图；对于k-1时刻测得的能够匹配上的路标点分别对其建立子滤波器，所述的子滤波器均为粒子滤波器，用于并行分布式对机器人下一时刻的状态和路标点信息进行估计；

然后，在机器人运动模型的基础上，加入均值为0的高斯白噪声，从而在每个子滤波器中产生位置随机粒子数相等的粒子群，粒子群中每一个粒子的状态代表机器人可能的状态；

其中第j个子滤波器中第i个粒子在k-1时刻的状态向量为

$X_{k-1}^{i,j}=X_{k-2}^{i,j}+{\mathrm{\ΔX}}^{i,j}$

其中，包含三个分量分别为第j个子滤波器中第i个粒子在k-1时刻的位置和角度，为第j个子滤波器中第i个粒子在k-2时刻的状态向量，ΔX^i,j为在k-1与k-2机器人状态向量的变化量；

步骤3：引入噪声扩展粒子维数。

引入噪声扩展粒子维数，对步骤2中的引入噪声来扩展该状态向量的维数，以获得扩展后的各个子滤波器中粒子的状态向量和方差；

扩展后的k-1时刻的第j个子滤波器中第i个粒子的状态向量为：

$X_{k-1}^{i,j,a}=E\left[X_{k-1}^{i,j}\right]=\left[\begin{array}{ccc}{X}_{k-1}^{i,j}& 0& 0\end{array}\right]$

方差为：

$P_{k-1}^{i,j,a}=E\left[(X_{k-1}^{i,j}-{\stackrel{\‾}{X}}_{k-1}^j){(X_{k-1}^{i,j}-{\stackrel{\‾}{X}}_{k-1}^j)}^T\right]=\left[\begin{array}{ccc}{p}_{k-1}^{i,j}& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right]$

其中，x为状态向量的维数，a为方差矩阵的维数，为k-1时刻第j个子滤波器中第i个粒子的状态向量，为k-1时刻第j个子滤波器中第i个粒子的状态向量的方差，过程噪声和测量噪声均为不相关的零均值高斯白噪声，其方差分别为Q和R；

步骤4：通过无味卡尔曼滤波算法更新优化粒子群。

通过无味卡尔曼滤波算法更新优化粒子群，具体为，首先对步骤3中得到的扩展后的粒子状态向量进行无味变换，得到变换后的粒子的状态向量然后对其进行时间更新和测量更新，最后根据更新结果对粒子群进行更新优化；

时间更新：

首先把k-1时刻的每个粒子对机器人状态估计的变换后的粒子状态向量分别代入机器人运动模型f(·)和路标点观测模型h(·)，得到每个粒子对k时刻机器人状态的一步预测向量和观测向量的一步预测向量

然后计算每个粒子的一阶统计特性的权系数和二阶统计特性的权系数

最后根据以上数据计算每个子滤波器中所有粒子的状态向量的一步预测加权和和观测向量的一步预测加权和

其具体过程为:

$X_{k|k-1}^{i,j}=f\left({\tilde{X}}_{k-1}^{i,j,a}\right)$

${\stackrel{\‾}{X}}_{k|k-1}^j=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{W}_{i,j}^m{X}_{k|k-1}^{i,j}$

$Z_{k|k-1}^{i,j}=h\left({\tilde{X}}_{k-1}^{i,j,a}\right)$

${\stackrel{\‾}{Z}}_{k|k-1}^j=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{W}_{i,j}^m{Z}_{k|k-1}^{i,j}$

测量更新：

首先计算每个子滤波器中粒子对机器人状态的一步预测误差和观测的一步预测误差

然后计算每个子滤波器的滤波增益K^j；

最终得到k时刻的每个子滤波器对机器人状态和其方差的估计；

其具体过程为:

$P_{k|k-1}^j=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{W}_{i,j}^c[X_{k|k-1}^{i,j}-{\stackrel{\‾}{X}}_{k|k-1}^j]{[X_{k|k-1}^{i.j}-{\stackrel{\‾}{X}}_{k|k-1}^j]}^T$

$P_{z_{k|k-1}{z}_{k|k-1}}^j=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{W}_{i,j}^c[Z_{k|k-1}^{i,j}-{\stackrel{\‾}{Z}}_{k|k-1}^j]{[Z_{k|k-1}^{i,j}-{\stackrel{\‾}{Z}}_{k|k-1}^j]}^T$

$P_{x_{k|k-1}{z}_{k|k-1}}^j=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{W}_{i,j}^c[X_{k|k-1}^{i,j}-{\stackrel{\‾}{X}}_{k|k-1}^j]{[Z_{k|k-1}^{i,j}-{\stackrel{\‾}{Z}}_{k|k-1}^j]}^T$

$K^j=P_{x_{k|k-1}{z}_{k|k-1}}^j{\left(P_{z_{k|k-1}{z}_{k|k-1}}^j\right)}^{-1}$

${\stackrel{\‾}{X}}_k^j={\stackrel{\‾}{X}}_{k|k-1}^j+K^j(Z_k-{\stackrel{\‾}{Z}}_{k|k-1}^j)$

${\stackrel{\‾}{P}}_k^j=P_{k|k-1}^j-K^j{P}_{z_{k|k-1}{z}_{k|k-1}}^j{\left(K^j\right)}^T$

其中，Z_k为k时刻路标点的观测向量；

最后，更新优化粒子群，

根据得到的k时刻的状态和方差分别在每个子滤波器中构造一个均值为方差为的高斯分布的函数，并根据该函数重新生成每个子滤波器的粒子群，得到新的每个子滤波器中粒子的状态向量

步骤5：计算粒子权值并归一化。

根据步骤4中生成的粒子群，计算每个子滤波器中粒子的权重值，其中第j个子滤波器中第i个粒子在k时刻的粒子权值的计算公式如下：

${\stackrel{\text{~}}{\mathrm{\ω}}}_k^{i,j}=\frac{P\left(Z_k^{i,j}\right|X_k^{i,j})\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\text{\ω}}_{k-1}^{i,j}P\left(X_k^{i,j}\right|X_{k-1}^{i,j})}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{i,j}q\left(X_k^{i,j}\right|Z_k^{i,j},X_{k-1}^{i,j})}$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，为当前时刻与第j个子滤波器中第i个粒子相对应的传感器测得的观测信息,为第j个子滤波器中第i个粒子在k-1时刻的权值,N为子滤波器中粒子数，为关于和的后验估计，为关于和的后验估计，为关于和的后验密度函数；

然后归一化权值，具体如下：

计算每个子滤波器中粒子的权值和其中第j个子滤波器中粒子在k时刻的的权值和为的计算公式如下：

${\mathrm{\ω}}_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\ω}}}_k^{i,j}$

计算每个子滤波器中每个粒子的归一化权值其中第j个子滤波器中第i个粒子在k时刻的归一化权值的计算公式如下：

${\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}=\frac{{\widetilde{\mathrm{\ω}}}_k^{i,j}}{{\mathrm{\ω}}_k^j}$

步骤6：计算聚合数据

根据步骤5中得到的中每个粒子的归一化权值计算每个子滤波器在k时刻的聚合数据，其中，表示第j个子滤波器在k时刻的聚合数据；

$S_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$X_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{X}_k^{i,j}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$G_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}\right)}^2$

$P_k^j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}{X}_k^{i,j}{\left(X_k^{i,j}\right)}^T$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，N为子滤波器中粒子数，为未归一化时子滤波器中粒子权重的和，为未归一化时的本地估计，为机器人的状态估计误差，为粒子权重的平方和用于重采样时计算有效粒子数；

步骤7：建立主滤波器，并对子滤波器数据处理。

把子滤波器中所有粒子的状态向量及其权值传送给主滤波器，通过主滤波器对子滤波器中所有数据进行统计计算，最终输出k时刻机器人的全局状态估计和方差

所有子滤波器中粒子权重和为：

$C_k=\underset{j=1}{\overset{J}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}=\underset{j=1}{\overset{J}{\mathrm{\Σ}}}{S}_k^j$

全局估计为：

${\hat{X}}_k=\underset{j=1}{\overset{J}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}{X}_k^{i,j}=\underset{j=1}{\overset{J}{\mathrm{\Σ}}}\frac{X_k^j}{C_k}$

全局方差为：

${\hat{P}}_k=\underset{j=1}{\overset{J}{\mathrm{\Σ}}}\frac{P_k^j}{C_k}-{\hat{X}}_k{\hat{X}}_k^T$

其中J为子滤波器个数。

步骤8：重采样。

重采样，分别计算各子滤波器的有效抽样尺度，当所有子滤波器的有效抽样尺度均大于采样阈值时,直接进行第9步，否则，对于有效抽样尺度均小于采样阈值的子滤波器重复步骤2至8；

每个子滤波器的有效粒子数定义如下：

${\hat{N}}_{\mathrm{eff}}^j=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}\right)}^2}$

其中，为第j个子滤波器的有效粒子数。

分别计算各子滤波器的有效粒子数，如果门限值，则对该子滤波器进行重采样。为了进一步保证粒子集的一致性还可以定期进行全局重采样，即对主滤波器进行重采样。

步骤9：输出最终结果。

把所有子滤波器得到的最后一次k时刻机器人的全局状态估计和方差作为最终结果输出，并计算机器人的全局状态估计的均方根误差，然后把所有子滤波器得到的最后一次k时刻机器人的全局估计存入全局地图；

通过计算算法的均方根误差函数，以验证算法的精度和性能。

位置均方根误差函数（RMSE）定义如下：

$\mathrm{RMSE}=\sqrt{\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{(X_{k-1}^{i,j}-{\hat{X}}_k)}^2}$

其中表示k时刻第j次采样的估计偏差。分别计算机器人在x和y方向上的位置均方根误差。

步骤10：路标点更新。

路标点更新，使用卡尔曼滤波算法对k-1时刻的路标点的状态向量及其方差进行时间更新和量测更新，获得k时刻路标点状态量的全局估计和方差，并将其存入全局地图，与全局地图中已存储的路标点一起用于下一时刻的地图匹配的；

假设环境信息不变，路标点下一时刻的位置信息仍然等于上一时刻，每个路标点状态模型的时间更新方程均为：

$X_{k+1|k}^I=X_{k|k}^I$

P_k+1|k＝P_k|k+Q

其中，为路标点在k时刻的状态向量，为路标点状态在k时刻的一步预测向量，P_k|k为误差矩阵，P_k+1|k为一步预测误差。

量测方程为：

$K_{k+1}=P_{k+1|k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R)}^{-1}$

$X_{k+1|k+1}^I=X_{k+1|k}^I+K_{k+1}(Z_{k+1}-H_{k+1}{X}_{k+1|k}^I)$

$P_{k+1|k+1}=P_{k+1|k}-K_{k+1}{H}_{k+1}{K}_{k+1}^T$

其中，H_k+1为k+1时刻的测量矩阵，Z_k+1为k+1时刻的路标点观测向量。

为了进一步提高滤波精度可以引入过程噪声和量测噪声，Q和R分别为过程噪声和量测噪声的方差。

步骤11：判断机器人是否继续运动，如果继续则跳转至步骤2，否则程序终止。

3本发明方法的效果可以通过以下仿真实验进一步说明

1)仿真条件及参数

本方法所用传感器数据是由悉尼大学的研究人员在维多利亚公园利用激光测距传感器测得的，我们把其输入到计算机中，作为机器人传感器测得的路标点状态信息。车辆机器人参数为L=2.83，b=0.5，a=3.78，机器人初始位姿 $X_0={\left[\begin{array}{ccc}{x}_0& y_0& {\mathrm{\φ}}_0\end{array}\right]}^T={\left[\begin{array}{ccc}0& 0& -\frac{112\×\mathrm{\π}}{180}\end{array}\right]}^T$ 过程噪声和量测噪声均为均值为0的高斯白噪声，每个子滤波器的粒子数均为N=285，仿真持续时间T=375s。

2)仿真内容及结果分析

在相同粒子数的条件下分别用本发明方法（DUMPF）和基于分布式粒子滤波（DPF）的同步定位与地图构建（SLAM）方法对传感器测量得的自然路标点的信息进行滤波估计，得到车辆和路标点位置估计，并和同步机器人自带GPS测得的数据相比较；同时计算本发明方法和分布式粒子滤波SLAM方法的估计均方根误差，仿真实验结果分别见图4和图5。

由图4可知，随着机器人运动时间的增加，分布式粒子滤波SLAM方法的估计精度逐渐变差。随着运动时间的增加，分布式粒子滤波SLAM方法估计得到的行驶轨迹已经明显偏离GPS数据，而本发明方法仍能很好地利用传感器信息对机器人位置进行估计。图中有些地方由于GPS覆盖盲区而不能获得GPS测量数据。

如图5所示，随着仿真时间的增加，分布式粒子滤波SLAM方法在x和y方向上的位置均方根误差逐渐增大且明显大于本发明方法，与分布式粒子滤波SLAM方法相比本发明方法的均方根误差平均值小，且不会随时间变化而大幅增加。

仿真结果表明：分布式边缘无味粒子滤波SLAM方法具有很好的滤波估计精度和性能，能够有效克服一般分布式粒子滤波SLAM方法粒子退化和估计精度低的缺点。

Claims (1)

1.基于分布式边缘无味粒子滤波的同步定位与地图构建方法，基于四轮机器人，其特征在于依次包括下述步骤：

1)初始化，

建立坐标系，以机器人初始时刻的位置为坐标原点；

全局地图初始化，机器人在初始位置，利用传感器扫描测得的路标点位置信息建立环境地图，并把它与机器人初始时刻的位姿一起作为全局地图进行存储；

2)获取机器人k-1时刻测得的路标点状态信息，并与已在全局地图中存储的路标点信息相匹配，对于k-1时刻测得的未匹配上的路标点状态信息，直接将其添加至全局地图，与地图中已存储的路标点信息一起用于下一时刻的路标点匹配；对于k-1时刻测得的能够匹配上的路标点分别对其建立子滤波器，所述的子滤波器均为粒子滤波器，用于并行分布式对机器人下一时刻的状态和路标点信息进行估计；

然后，在机器人运动模型的基础上，加入均值为0的高斯白噪声，从而在每个子滤波器中产生位置随机粒子数相等的粒子群，粒子群中每一个粒子的状态代表机器人可能的状态；

其中，第j个子滤波器中第i个粒子在k-1时刻的状态向量为

其中，分别为第j个子滤波器中第i个粒子在k-1时刻的位置和角度，为第j个子滤波器中第i个粒子在k-2时刻的状态向量，ΔX^i,j为在k-1与k-2机器人状态向量的变化量；

3)引入噪声扩展粒子维数，对步骤2)中的引入噪声来扩展该状态向量的维数，以获得扩展后的各个子滤波器中粒子的状态向量和方差；

扩展后的k-1时刻的第j个子滤波器中第i个粒子的状态向量为：

$X_{k-1}^{i,j,a}=\left[\begin{array}{ccc}{X}_{k-1}^{i,j}& 0& 0\end{array}\right]$

其方差为： $P_{k-1}^{i,j,a}=\left[\begin{array}{ccc}{P}_{k-1}^{i,j}& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right]$

其中，a为扩展后状态向量和其方差矩阵的维数，为k-1时刻第j个子滤波器中第i个粒子的状态向量，为k-1时刻第j个子滤波器中第i个粒子的状态向量的方差，过程噪声和测量噪声均为不相关的零均值高斯白噪声，其方差分别为Q和R；

4)通过无味卡尔曼滤波算法更新优化粒子群，具体为，对步骤3中得到的扩展后的粒子状态向量进行无味变换，得到变换后的粒子的状态向量然后对其进行时间更新和测量更新，最后根据更新结果对粒子群进行更新优化；

时间更新具体如下：

首先把k-1时刻的每个粒子对机器人状态估计的变换后的粒子状态向量分别代入机器人运动模型f(·)和路标点观测模型h(·)，得到每个粒子对k时刻机器人状态的一步预测向量和观测向量的一步预测向量

然后计算每个粒子的一阶统计特性的权系数和二阶统计特性的权系数

最后根据以上数据计算每个子滤波器中所有粒子的状态向量的一步预测加权和和观测向量的一步预测加权和

测量更新具体如下：

首先计算每个子滤波器中粒子对机器人状态的一步预测误差和观测的一步预测误差 $X_{k|k-1}^{i,j}-{\stackrel{\‾}{Z}}_{k|k-1}^j;$

然后计算每个子滤波器的滤波增益K^j；

最终得到k时刻的每个子滤波器对机器人状态的估计和其方差的估计

更新优化粒子群具体如下：

根据得到的k时刻的和方差分别在每个子滤波器中构造一个均值为方差为的高斯分布的函数，并根据该函数重新生成每个子滤波器的粒子群，得到新的每个子滤波器中粒子的状态向量

5)计算每个子滤波器中的粒子权值并归一化；

根据步骤4)中重新生成的粒子群，计算每个子滤波器中粒子的权重，其中第j个子滤波器中第i个粒子在k时刻的粒子权重的计算公式如下：

${\widetilde{\mathrm{\ω}}}_k^{i,j}=\frac{P\left(Z_k^{i,j}\right|X_k^{i,j})\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_{k-1}^{i,j}P\left(X_k^{i,j}\right|X_{k-1}^{i,j})}{\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_{k-1}^{i,j}q\left(X_k^{i,j}\right|Z_k^{i,j},X_{k-1}^{i,j})}$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，为当前时刻与第j个子滤波器中第i个粒子相对应的传感器测得的观测信息,为第j个子滤波器中第i个粒子在k-1时刻的权值,N为子滤波器中粒子数，为关于和的后验估计，为关于和的后验估计，为关于和的后验密度函数；

然后归一化权值，具体如下：

计算每个子滤波器中粒子的权值和其中第j个子滤波器中粒子在k时刻的的权值和为的计算公式如下：

${\mathrm{\ω}}_k^j=\underset{i=1}{\overset{N}{\Σ}}{\widetilde{\mathrm{\ω}}}_k^{i,j}$

计算每个子滤波器中每个粒子的归一化权值其中第j个子滤波器中第i个粒子在k时刻的归一化权值的计算公式如下：

${\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}=\frac{{\widetilde{\mathrm{\ω}}}_k^{i,j}}{{\mathrm{\ω}}_k^j}$

6)根据步骤5)中得到的每个粒子的归一化权值计算每个子滤波器在k时刻的聚合数据，其中，表示第j个子滤波器在k时刻的聚合数据；

$S_k^j=\underset{i=1}{\overset{N}{\Σ}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$X_k^j=\underset{i=1}{\overset{N}{\Σ}}{X}_k^{i,j}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}$

$G_k^j=\underset{i=1}{\overset{N}{\Σ}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}\right)}^2$

$P_k^j=\underset{i=1}{\overset{N}{\Σ}}{\stackrel{\‾}{\mathrm{\ω}}}_k^{i,j}{X}_k^{i,j}{\left(X_k^{i,j}\right)}^T$

其中，为优化粒子群后得到的第j个子滤波器中第i个粒子在k时刻的状态向量，N为子滤波器中粒子数，为未归一化时子滤波器中粒子权重的和，为未归一化时的本地估计，为机器人的状态估计误差，为粒子权重的平方和，用于重采样时计算有效粒子数；

7)把所有子滤波器中所有粒子的权值和状态向量传送给主滤波器，通过主滤波器对子滤波器中所有数据进行统计计算，最终输出k时刻机器人的全局状态估计和方差

8)重采样，分别计算k时刻各子滤波器的有效抽样尺度，当所有子滤波器的有效抽样尺度均大于采样阈值时,直接进行第9步，否则，对于有效抽样尺度均小于采样阈值的子滤波器重复步骤2)至步骤8)；

9)把所有子滤波器得到的最后一次k时刻机器人的全局状态估计和方差作为最终结果输出，并计算机器人的全局状态估计的均方根误差，然后把所有子滤波器得到的最后一次k时刻机器人的全局估计存入全局地图；

10)路标点更新，使用卡尔曼滤波算法对k-1时刻的路标点的状态向量及其方差进行时间更新和量测更新，获得k时刻路标点状态量的全局估计和方差，并将其存入全局地图，与全局地图中已存储的路标点一起用于下一时刻的地图匹配的；

11)判断机器人是否继续运动，如果继续则跳转至步骤2)，否则程序终止。