CN104966123A - 基于模糊-自适应的slam数据关联方法 - Google Patents

Abstract

基于模糊-自适应的SLAM数据关联方法，本方法针对现有SLAM数据关联方法的不足，提出一种基于误差椭圆和关联阈值的模糊-自适应数据关联方法，将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果；为了有效地防止虚假路标，本发明根据观测数据来跟踪环境中的最小路标间距，将欧式距离最近的两个观测路标中的一个作为虚拟的新路标加入到系统状态中，并将另一观测量与虚拟路标通过模糊规则进行数据关联。仿真实验和真实实验证明了该方法能够解决数据关联问题，并能很好的适应环境和噪声的变化，从而有效地防止虚假路标并降低观测丢弃率。

Description

基于模糊-自适应的SLAM数据关联方法

技术领域

基于模糊-自适应的SLAM数据关联方法将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果，进而在同步定位与地图构建中提高关联正确率及定位精度，属于机器人自主导航领域。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)其基本思想是：让机器人在未知环境中从未知位置开始移动,通过自身所带传感器扫描到的路标点的信息进行自身位置估计，同时构建增量式地图。在SLAM过程中，根据观测信息进行实时的地图估计与更新，其中数据关联是一个关键问题。SLAM中的数据关联是指建立在不同时间、不同地点的传感器观测之间、传感器观测与地图特征之间或者各特征之间的对应关系，以确定它们是否源于同一物理实体的过程，同时也包括将未与地图中特征相匹配的传感器观测信息确定为新特征的过程。

目前，数据关联方法主要包括基于独立匹配的最近邻算法(ICNN)、基于联合概率的数据关联(JPDA)、多维分配法(MDA)以及多假设跟踪算法(MHT)等。其中最近邻数据关联方法被广泛应用，它通过对单个观测值和特征间的马氏距离进行度量，完成独立关联，假设关联对之间的马氏距离服从卡方分布，并根据预设的置信度计算得到关联阈值和增广阈值。

上述方法各有特点，也存在一些不足，主要体现在：

(1)错误的数据关联将导致SLAM过程的发散，因此通常设置较低的关联阈值来保证数据关联的正确性。但是较低的关联阈值将导致较高的观测丢弃率。因此当路标分布、系统噪声等因素发生变化时，观测丢弃率会升高，从而导致SLAM估计性能降低。

(2)在不确定性较大的复杂环境中，关联的正确率有待提高。在大规模关联问题中，算法所需的运算量较大，难以满足实时性需求。

(3)数据关联算法需要防止引入虚假的新路标。上述方法不能有效地防止虚假路标，尤其是在环境中路标较稀疏的情况下，将一个路标识别为两个路标，导致在两者之间分配该路标的观测数据，从而使得观测对移动机器人的位姿修正作用减弱。

发明内容

本发明针对现有SLAM数据关联方法的不足，提出一种基于误差椭圆和关联阈值的模糊-自适应数据关联方法，将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果。仿真实验和真实实验证明了该方法的有效性。

模糊逻辑基于以下思想：

1、计算待匹配观测——估计对的误差椭圆，圆心分别为当前时刻的估计值和特征观测值。计算两个圆心之间的几何距离，并进行归一化处理，结合观测——估计对协方差的大小使其能反映出观测值和估计值间的相关性。

2、如图1所示，图中特征观测与估计的两个误差椭圆互有重叠，并且每个特征的两个误差椭圆间都有一个最大的重叠区域，在这个区域中其两个椭圆重叠部分所占的比例则反映出估计——观测之间的相关联程度。

这种基于模糊逻辑思想的基本结构如图2所示，将归一化新息以及观测误差椭圆和估计误差椭圆的重叠比例进行模糊化后作为输入变量，将数据关联结果作为模糊推理的执行结果。通过建立适当的模糊推理规则，进行模糊推理，最终将输入映射到输出，实现数据关联。建立三输入单输出的模糊系统，由模糊逻辑思想，可以看到围绕其中的两个关键要素：1)误差椭圆；2)模糊化。

误差椭圆即描述待定点位置各方向上误差分布规律的椭圆，它表示估计值以一定概率落入以真实值为圆心的椭圆区域。误差椭圆由半长轴、半短轴和长轴方向3个参数确定，由误差协方差矩阵可以求出相应的误差椭圆参数。

以某一目标起始位置为坐标原点建立坐标轴，设其真实位置矢量为X＝[x_e,y_e]^T，其估计矢量为其中x_e,y_e表示目标的真实位姿坐标，表示目标的估计位姿坐标，则估计的误差协方差矩阵为：

$C={\left[\begin{array}{cc}{\mathrm{\σ}}_x^2& {\mathrm{\ρ\σ}}_x{\mathrm{\σ}}_x\\ {\mathrm{\ρ\σ}}_x{\mathrm{\σ}}_x& {\mathrm{\σ}}_y^2\end{array}\right]}_{\left(1\right)}$

其中σ_x，σ_y分别为矢量坐标的方差，ρ是协方差相关系数，由于C是正定矩阵，所以|ρ|≤1。估计矢量以概率P落入以真实位置矢量X为中心的椭圆区域，其椭圆区域表达式和概率积分公式如下，其中D为椭圆区域。

${(\hat{X}-X)}^T{C}^{-1}(\hat{X}-X)\≤D^2---\left(2\right)$

$P=\underset{{(\hat{X}-X)}^T{C}^{-1}(\hat{X}-X)\≤D^2}{\∫\∫}\frac{1}{2\mathrm{\π}\sqrt{\left|C\right|}}\exp \{-\frac{1}{2}{(\hat{X}-X)}^T{C}^{-1}(\hat{X}-X)\}dxdy---\left(3\right)$

为推导椭圆区域D与概率P的关系,进行变量替换,令对式(3)的积分区域进行替换，则该式可以写成：

$P=\underset{{\mathrm{\ζ}}^2+{\mathrm{\η}}^2\≤D^2}{\∫\∫}\frac{1}{2\mathrm{\π}}\exp \{-\frac{1}{2}({\mathrm{\ζ}}^2+{\mathrm{\η}}^2)\}d\mathrm{\ζ}d\mathrm{\η}---\left(4\right)$

再将ζ和η转换到极坐标下，令ζ＝r sinθ，η＝r cosθ可得：

$p=\frac{1}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\left\{\underset{0}{\overset{D}{\∫}}\exp (-\frac{r^2}{2})rdr\right\}d\mathrm{\θ}=1-\exp (-D^2/2)---\left(5\right)$

式中θ为长轴方向角，可得椭圆区域D与概率P具有如下关系

$D=\sqrt{-2ln(1-P)}---\left(6\right)$

通常使用半长轴a、半短轴b和长轴方向θ三个参数确定椭圆，这三个参数通过协方差C和概率P计算。利用式(1)可将椭圆表示为以下形式：

$\frac{1}{1-{\mathrm{\ρ}}^2}\[\frac{{(x-\hat{x})}^2}{{\mathrm{\σ}}_x^2}+\frac{{(y-\hat{y})}^2}{{\mathrm{\σ}}_y^2}-\frac{2\mathrm{\ρ}(x-\hat{x})(y-\hat{y})}{{\mathrm{\σ}}_x^2{\mathrm{\σ}}_y^2}\]=D^2---\left(7\right)$

若ρ＝0，上式变为:

$\frac{{(x-\hat{x})}^2}{{\mathrm{\σ}}_x^2}+\frac{{(y-\hat{y})}^2}{{\mathrm{\σ}}_y^2}=D^2---\left(8\right)$

利用σ_x和σ_y可直接得到：

当σ_x≥σ_y时，a＝Dσ_x，b＝Dσ_y，θ＝0；

当σ_x＜σ_y时，a＝Dσ_y，b＝Dσ_x，

若ρ≠0，进行坐标变换：

$u=(x-\hat{x})cos\mathrm{\θ}+(y-\hat{y})\sin \mathrm{\θ}---\left(9\right)$

$v=-(x-\hat{x})sin\mathrm{\θ}+(y-\hat{y})cos\mathrm{\θ}---\left(10\right)$

椭圆在新坐标系的表达式为：

$\frac{u^2}{{\mathrm{\σ}}_u^2}+\frac{v^2}{{\mathrm{\σ}}_v^2}=D^2---\left(11\right)$

此时下式成立：

E|dudv|＝ρσ_xσ_y(cos²θ-sin²θ)+(σ_x ²+σ_y ²)cosθsinθ＝0  (12)

并且

${\mathrm{\σ}}_u^2={\mathrm{\σ}}_x^2{\cos }^2\mathrm{\θ}+{{\mathrm{\σ}}_y}^2{\sin }^2\mathrm{\θ}+2{\mathrm{\ρ\σ}}_x{\mathrm{\σ}}_{y}sin\mathrm{\θ}cos\mathrm{\θ}---\left(13\right)$

${\mathrm{\σ}}_v^2={\mathrm{\σ}}_x^2{\sin }^2\mathrm{\θ}+{{\mathrm{\σ}}_y}^2{\cos }^2\mathrm{\θ}+2{\mathrm{\ρ\σ}}_x{\mathrm{\σ}}_{y}sin\mathrm{\θ}cos\mathrm{\θ}---\left(14\right)$

最终获得误差椭圆的参数：

半长轴：a＝max(Dσ_u,Dσ_v)  (15)

半短轴：b＝min(Dσ_u,Dσ_v)  (16)

长轴方向角： $\mathrm{\&theta;}=\{\begin{array}{cc}\frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&sigma;}}_x{\mathrm{\&sigma;}}_y}{{\mathrm{\&sigma;}}_x^2-{\mathrm{\&sigma;}}_y^2}\right)& {\mathrm{\&sigma;}}_x>{\mathrm{\&sigma;}}_y\\ \frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&sigma;}}_x{\mathrm{\&sigma;}}_y}{{\mathrm{\&sigma;}}_x^2-{\mathrm{\&sigma;}}_y^2}\right)+\frac{\mathrm{\&pi;}}{2}& {\mathrm{\&sigma;}}_x<{\mathrm{\&sigma;}}_y\end{array}---\left(17\right)$

当σ_x＝σ_y时，若ρ＞0，则若ρ＜0，则

表1列出了几种误差椭圆以及所对应估计值落入其椭圆区域的概率。

表1误差椭圆及其概率

Table 1 Common error ellipses and its probability

若选择较小的概率值，则椭圆区域面积较小，有较多的估计点被排除在区域之外；若选择较大的概率值，则椭圆区域面积较大，落入误差椭圆的估计点也比较多。为了获得较高的数据关联正确率，同时考虑到算法计算量问题，要选择适当的误差椭圆进行计算。对于特征密度较大的环境，选择较小的误差椭圆可以降低计算量，同时避免一些错误数据关联；对于特征分布稀疏的环境，选择较大的误差椭圆能保证关联正确率，同时减少丢弃正确关联假设的可能。另外环境中噪声干扰和传感器观测的不确定性也会对误差椭圆的选取产生一定的影响。

对于每一时刻传感器获得的观测信息中的某一观测值和环境地图中某一待匹配特征状态估计值，利用上述方法分别计算它们对应的误差椭圆。为了保证误差椭圆具有实际意义，要避免长轴出现“无穷大”的情况，因此设定一个阈值，当计算得到的椭圆半长轴值超过阈值时，认为该观测值为无效数据，无需对其进行关联匹配；当半长轴在阈值范围内，才继续对其进行关联处理。

①归一化新息及其模糊化

对于每一个特征的一对椭圆间有一个最大的重叠区域，这个区域在椭圆中所占比例可以反映观测——估计对之间的相关性，如图3所示。图3(a)为中两个误差椭圆相离(可将两椭圆相切视为这类相对位置的特殊情况)，分别为两椭圆中心连线与椭圆的交点，此时residual≥1。图3(b)中两个误差椭圆相交，且两椭圆中心不在重叠区域中，A仍为两椭圆中心连线与椭圆的交点，此时1/2≤residual≤1。图3(c)中两个椭圆相交，且圆心落入重叠区域内为两椭圆圆中心连线的延长线与椭圆的交点，此时0≤residual≤1/2。图3(d)中两椭圆的圆心重合，将此时的归一化新息定义为0。

定义归一化新息如下，其中O₁，O₂为两椭圆的中心。

$residual=\frac{\left|O_1{O}_2\right|}{\left|O_1{A}_1\right|+\left|O_2{A}_2\right|}---\left(18\right)$

将上述清晰的归一化新息按照语言模糊集进行模糊化，用于描述归一化新息的模糊语言变量为Very close、Close、Medium、Far和Very far，模糊化后的变量也将被映射到五个模糊集合中，其相应的隶属函数分布如图4所示。

②两误差椭圆重合比例及其模糊化

对于图3表示的几种误差椭圆位置关系中，特征状态估计值和观测值对应的误差椭圆的重叠区域能反映出它们之间的关联关系。直观上来看，重叠区域的面积越大，估计值和观测值的关联程度越大，但估计值和观测值的不确定性受到环境噪声、观测噪声、系统噪声等多种因素的影响，因此其误差椭圆的大小也是不断变化的。为了对误差椭圆重叠区域能反映出的关联程度进行定量考察，将重叠区域面积与两椭圆面积之比作为输入变量。

对于两椭圆相交形成的重叠区域，难以直接计算其面积。本发明使用八边形近似的方法，首先在椭圆内根据半长轴和半短轴与椭圆交点建立对称八边形，就可以将两个椭圆相交的重叠区域近似为两八边形的重叠区域，然后再将重叠区域分解为若干个三角形，三角形的面积是易于计算的，这样就可以得到两椭圆重叠面积的近似值。以图5为例，将两误差椭圆的重叠区域近似为两个八边形重叠区域，这是一个多边形区域，将其再划分为四个三角形，每个三角形的面积可通过海伦公式计算：

$S_i=\sqrt{p(p-a)(p-b)(p-c)}---\left(19\right)$

式中：p＝(a+b+c)/2，a、b、c分别为三角形的三条边长。最终两椭圆重叠区域面积area近似为三角形面积之和：

$area=\underset{i=1}{\overset{n}{\&Sigma;}}{S}_i---\left(20\right)$

再分别计算重叠区域在每个椭圆中所占比例：

per_obs＝area/ellipse_obs  (21)

per_est＝area/ellipse_est  (22)

其中per_obs和per_est分别为特征观测值和特征估计值对应的误差椭圆的重叠区域在各自椭圆中所占比例，ellipse_obs和ellipse_est分别为两误差椭圆的面积。

定义描述椭圆重叠区域比例的模糊语言变量为Seldom、Little、Medium、Most和Major，这些模糊集合对应的隶属函数如图6所示，将清晰的重叠比例量映射到上述模糊集合中进行模糊化处理。

将待匹配的特征观测值和特征估计值的关联程度作为模糊推理系统的输出值，记作Degree，其取值在[0,1]之间。定义描述关联程度的模糊语言变量为Very Low、Low、Medium、High和Very High，这些模糊集合对应的隶属函数如图7所示，将清晰地输出变量映射到模糊集合中进行模糊化。

数据关联算法除了要有正确的关联率之外，还需要防止引入虚假的新路标。其由两种途径产生：a、将已有环境路标的观测错误判断为新路标；b、将虚假的环境路标观测判断为新的路标。其中第一种途径的影响更为严重，在特征环境中，将一个路标识别为两个路标将导致在两者之间分配该路标的观测数据错误。

假设传感器返回M个观测量，记为z＝{z₁,...,z_m}。对于第m个观测量z_m，计算z_m与地图中每一个路标之间的马氏距离D_e，记其中最小的为D_m。为了有效地防止虚假路标，在基于固定关联阈值G₁的基础上，引入增广阈值G₂。若D_m＜G₁，则观测值与环境中的路标可作为关联对；若G₂＜D_m，则其观测值作为一个新路标的观测；若G₁≤D_m≤G₂，则丢弃该观测。用自适应的方法来调节增广阈值G₂。

其方法如下：

1)计算当前传感器观测中距离最近的两个观测：z_m,z_n；

2)定义当前机器人位姿估计为X^v和P^v，将z_m作为虚拟的新路标加入到系统状态中，即

X＝[X^v g(z_m)]  (23)

$P=\left[\begin{array}{cc}{P}^v& G_v\&CenterDot;P^v\\ P^v\&CenterDot;{G_v}^T& G_v\&CenterDot;P^v\&CenterDot;{G_v}^T+G_z\&CenterDot;R\&CenterDot;{G_z}^T\end{array}\right]---\left(24\right)$

其中，g(z_m)为逆观测模型，G_v和G_z分别为g关于X^v和z_m的雅克比矩阵，R为观测噪声；

3)将z_n作为虚拟路标的观测量，并计算记录z_n与虚拟路标之间的马氏距离D_e；

4)如果观测只有一个观测量，则D_e取为所有已得到的{D_e}中的最小值；

5)G₂＝max(D_e,G_2low)，其中G_2low为设定的下限值。

在最近邻关联算法中，错误的数据关联将导致SLAM过程的发散，因此通常设置较低的关联阈值来保证数据关联的正确性。但是较低的关联阈值将导致较高的观测丢弃率，为了使数据关联算法能够根据系统状态和环境中路标的分布情况来自适应的调节关联阈值，提出与自适应增广阈值类似的关联阈值设定方法。

其方法思想如下：

1)首先将关联阈值G₁初始化为预先设定的关联阈值下限D_low，然后按下面设置自适应关联阈值G₁；

2)当首次观测到多个路标时，在上节中得到的马氏距离D_e后，取G₁＝D_e/4；

3)在后续观测中，G₁＝min{G₁,D_e/4}，即G₁取为所有{D_e}中最小值的1/4；

4)当G₁≤D_low时，为防止过低的关联成功率，取G₁＝D_low。

通过以上内容的介绍，基于模糊-自适应的SLAM数据关联方法其特征在于将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果，具体方法步骤如下：

Step1：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j。

Step2：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

Step3：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

Step4：将z_n作为虚拟路标的观测量；

Step5：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

Step6：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

Step7：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

Step8：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

Step9：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

Step10：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

Step11：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

Step12：根据前一时刻的自适应的增广阈值G₂及关联阈值G₁来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测。当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测；

有益效果

本发明方法提出了模糊-自适应数据关联算法。为了解决在不确定性较大的复杂环境中关联正确率低，并有效的防止虚假路标，采用模糊-自适应的算法来提高关联正确率及定位精度。将模糊逻辑规则运用到特征观测值与估计值的数据关联中，将改善数据关联效果；为了有效地防止虚假路标，引入增广阈值，该方法根据观测数据来跟踪环境中的最小路标间距，将欧式距离最近的两个观测路标中的一个作为虚拟的新路标加入到系统状态中，并将另一观测量与虚拟路标通过模糊规则进行数据关联，并通过反馈回来的马氏距离，自适应的调节关联阈值。

附图说明

图1：特征观测和估计的误差椭圆重叠区域；

图2：基于模糊逻辑思想的原理结构图；

图3：椭圆误差相对位置，其中：图3(a)为两个误差椭圆相离。图3(b)中两个误差椭圆相交。图3(c)中两个椭圆相交。图3(d)中两椭圆的圆心重合。

图4：归一化新息的隶属度分布；

图5：误差椭圆重叠区域近似；

图6：误差椭圆重叠比率的隶属度分布；

图7：输出结果的隶属度分布；

图8：仿真环境示意图；

图9：数据关联正确率随控制误差变化情况；

图10：数据关联正确率随观测误差变化情况；

图11：基于模糊逻辑的数据关联算法；

图12：基于模糊-自适应的数据关联算法；

图13：移动车辆模型；

图14：室外实验基于最近邻数据关联算法仿真试验地图；

图15：室外实验基于马氏距离改进的最近邻数据关联算法仿真试验地图；

图16：室外实验基于模糊-自适应数据关联算法仿真试验地图；

图17：室外实验基于三种关联算法的系统每一步运行时间，其中图17(a)基于最近邻关联算法系统每一步运行时间。图17(b)基于改进的马氏距离最近邻关联算法系统每一步运行时间。图17(c)基于模糊-自适应关联算法系统每一步运行时间。

图18：室外实验基于三种关联算法的系统每一步关联正确率，其中图18(a)基于最近邻关联算法系统每一步关联正确率。图18(b)基于改进的马氏距离最近邻关联算法系统每一步关联正确率。图18(c)基于模糊-自适应关联算法系统每一步关联正确率。

图19：室内实验环境模型；

图20：室内实验基于最近邻数据关联算法仿真试验地图；

图21：室内实验基于马氏距离改进的最近邻数据关联算法仿真试验地图；

图22：室内实验基于模糊-自适应数据关联算法仿真试验地图；

图23：室内实验基于三种关联算法的系统每一步运行时间，其中图23(a)基于最近邻关联算法系统每一步运行时间。图23(b)基于改进的马氏距离最近邻关联算法系统每一步运行时间。图23(c)基于模糊-自适应关联算法系统每一步运行时间。

图24：室内实验基于三种关联算法的系统每一步关联正确率，其中图24(a)基于最近邻关联算法系统每一步关联正确率。图24(b)基于改进的马氏距离最近邻关联算法系统每一步关联正确率。图24(c)基于模糊-自适应关联算法系统每一步关联正确率。

具体实施方式

通过仿真实验对本发明提出的基于模糊-自适应的数据关联算法进行了验证，并在EKF-SLAM框架下，分别使用模糊算法、自适应算法、最近邻算法与模糊-自适应数据关联算法的实验结果统计数据对比情况。实验所用的环境是Juan Nieto和Tim Bailey等人设计的仿真实验平台，如图8所示。系统围绕一个闭环的轨迹运动，图中的点代表可观测的路标，在移动机器人移动过程中，用激光传感器持续测量了120个路标。具体步骤如下：

步骤一：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j。

步骤二：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

步骤三：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

步骤四：将z_n作为虚拟路标的观测量；

步骤五：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

步骤六：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

步骤七：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

步骤八：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

步骤九：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

步骤十：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

步骤十一：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

步骤十二：根据前一时刻的自适应的增广阈值G₂及关联阈值G1来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测。当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测；

表2为分别使用模糊算法、自适应算法、最近邻算法与模糊-自适应数据关联算法的实验结果统计数据对比情况，其中包括平均关联正确率、观测丢弃率、机器人在X方向和Y方向上的定位误差以及仿真平均时间。

表2几种算法实验结果统计数据对比

Table 2 The experiment result of several statistical data contrast

从以上数据可知，对于相对比较复杂的环境，基于模糊-自适应的数据关联算法在关联正确率及定位估计两个方面要优于其他三种算法。

1)在系统运行时间方面，NN算法比较占优势，由于只对单个观测值和单个特征进行关联检验，关联假设树的规模较小，且只需按照随机顺序单独进行判断；相对于模糊算法与模糊-自适应关联算法，由于均运用到模糊逻辑思想，具备多种关联假设相容性检验的能力，其关联树规模要比NN算法更复杂，所以其运行时间相对较长。

2)在关联正确率方面，模糊算法与模糊-自适应关联算法融合了多种特征信息进行模糊逻辑推理，并考虑了噪声及干扰带来的信息不确定性，相比与最近邻及自适应算法通过单一的观测值与估计值的马氏距离方法，其关联正确率更高。

3)在观测丢弃率方面，模糊-自适应关联算法与自适应算法都引入了自适应阈值，使其能够根据系统状态和环境中路标的分布来自适应的调节阈值，从而降低了观测丢弃率。相比较，最近邻算法与模糊算法采用的是固定阈值，当环境特征较复杂时，为了保证数据关联正确性而设置较低阈值，从而造成较高的观测丢弃率。

下面通过改变控制误差及观测误差来观察几种算法的抗干扰能力及鲁棒性。

1)保持机器人的观测误差不变，以初始控制误差为基础，成倍的增加控制误差方差。并比较最近邻数据关联、模糊-自适应关联算法、模糊算法、自适应算法在关联正确率上的变化情况，并分析控制误差对这几种关联算法的数据关联正确率的影响及产生原因，如图9所示。

由图9可以看出，当控制误差比较小时，基于模糊逻辑的两种算法的关联正确率都比较高；但随着控制误差的不断增加，基于马氏距离的最近邻算法的正确率迅速下降，相比较，同样基于马氏距离的自适应算法下降较平缓，但总体关联正确度不高；同时，当控制误差增大到一定程度时，其正确率才开始下降。其主要原因是由于控制误差的增大，机器人运动更新的不确定性及模糊性产生的影响越来越大，基于马氏距离的关联算法忽略了这些信息，而造成关联失败；基于模糊逻辑的两种算法的使用充分利用这些信息，从而提高关联正确率。

从总体上看，模糊-自适应关联算法相比其他三种算法受控制误差影响最小，其关联正确率最高。

2)保持系统控制误差不变，以初始观测误差为基础，将观测误差方差成倍增加，并比较最近邻数据关联算法、模糊-自适应关联算法、模糊关联算法、自适应关联算法在关联正确率上的变化情况，考察这四种算法的关联正确率，并分析观测误差对这几种关联算法的数据关联正确率的影响及产生原因如图10所示。

由图9与图10比较可知，增大观测误差对数据关联正确率的影响相比增大控制误差更为明显，而增大观测误差对四种算法的影响同控制误差一致。由以上结果表明，基于模糊-自适应的数据关联算法有较好的抗干扰能力及鲁棒性。

综上所述，在环境特征相对复杂的情况下，基于模糊-自适应的数据关联算法可以很好的进行数据关联，且有很好的抗干扰能力及鲁棒性。

对于基于模糊逻辑的关联算法和模糊-自适应数据关联算法之间的差异，图11与图12给出了这两种算法采用扩展卡尔曼滤波算法的SLAM在地图上的运行结果，以此来分析当加入自适应阈值时所带来的影响。

由图11可知，在整个地图的中心区域，可以明显的看到，当机器人重新返回到起点(即原点处)时，即在地图环境中横坐标在-30至0，纵坐标在-20至30处，由于自身位姿估计误差较大，基于模糊逻辑的数据关联算法将原点附近的已有路标错误的判定为新路标，使得SLAM结果与实际环境中不一致，从而导致地图构建错误，进而影响机器人在这段距离上的定位精度。

图12给出了采用了模糊-自适应的数据关联算法后的EKF-SLAM的结果，在模糊的基础上加入自适应阈值后，可见该算法在开始阶段和闭环时均有效的防止了产生虚假路标，并得到与实际环境一致的路标分布，其运动轨迹更加接近环境地图中的真实路径，定位精度更高。

仿真结果表明：由于基于马氏距离的数据关联算法将单个观测和特征进行独立关联，忽略了噪声干扰所带来的信息不确定性，当环境特征复杂时，传感器在同一时刻观测到多个环境特征，这时就容易发生错误的关联，从而影响机器人的定位和路标估计的精度。本算法通过模糊逻辑有效表达了实际系统中的不确定性信息，同时，该算法又引入自适应阈值，防止了虚假路标的出现，降低了机器人位姿和环境特征状态的估计误差。

上面提出的改进的数据关联算法都是在模拟环境下进行仿真实验，来验证其算法的正确性及有效性。但由于模拟环境实验数据缺乏说服性，下面我们使用维多利亚公园和德国学校实验室两组数据来进行算法验证，其中维多利亚公园数据采用的是激光传感器来获取周围环境外部信息；德国实验室数据采用的是视觉传感器来获取周围环境信息，并分别在室外和室内两种情况下进行实验，从而验证改进的数据关联算法应用到SLAM中的正确性及有效性，其仿真结果更加具有说服力。

在室外环境下采用的是维多利亚公园的数据，在小车上放置激光传感器来获取公园环境信息，其中规定传感器所获取的外部信息为树干的直径，这是因为树干的直径不受传感器探测角度的影响，其观测数据更加稳定可靠。

建立小车的运动模型及传感器的观测模型，如图13所示。

图中x_L与y_L是车辆中心在全局坐标系下的位置坐标，L为车辆前轴与后轴之间的距离，a为激光器中心到车辆后轴中心之间的距离，b为激光器中心到车辆中轴之间的距离。移动机器人的运动模型表示为：

$\begin{array}{c}X(k+1)=\left[\begin{array}{c}x(k+1)\\ y(k+1)\\ \mathrm{\&phi;}(k+1)\end{array}\right]=f(x,u)+\mathrm{\&gamma;}\\ =\left[\begin{array}{c}{x}_L\left(t\right)+\mathrm{\&Delta;}T\left(v_c\cos \right(\mathrm{\&phi;})-\frac{v_c}{L}\tan \left(\mathrm{\&phi;}\right)\left(a\sin \right(\mathrm{\&phi;})+b\cos \left(\mathrm{\&phi;}\right)\left)\right)\\ y_L\left(t\right)+\mathrm{\&Delta;}T\left(v_c\sin \right(\mathrm{\&phi;})-\frac{v_c}{L}\tan \left(\mathrm{\&phi;}\right)\left(a\sin \right(\mathrm{\&phi;})-b\cos \left(\mathrm{\&phi;}\right)\left)\right)\\ {\mathrm{\&phi;}}_L\left(t\right)+\mathrm{\&Delta;}T\frac{v_c}{L}\tan \left(\mathrm{\&alpha;}\right)\end{array}\right]+\mathrm{\&gamma;}\end{array}---\left(25\right)$

在全局坐标系下机器人的观测模型可表示为：

$\begin{array}{c}Z\left(k\right)=h\left(X\right(k))+\mathrm{\&epsiv;}\left(k\right)\\ =\left[\begin{array}{c}{x}_r+x_p^r\cos \mathrm{\&phi;}-y_p^r\sin \mathrm{\&phi;}\\ y_r+x_p^r\sin \mathrm{\&phi;}-y_p^r\cos \mathrm{\&phi;}\end{array}\right]+\mathrm{\&epsiv;}\left(k\right)\end{array}---\left(26\right)$

分别对最近邻关联算法、改进的马氏距离最近邻关联算法、模糊-自适应关联算法进行实验验证，如图14至图16，移动机器人(小车)在350m*350m的公园范围内移动。

图14到图16为SLAM系统中分别应用三种数据关联算法的仿真实验地图，下面分别从时间复杂度以及数据关联正确率两个方面分析验证改进的数据关联算法的有效性。图17为分别应用三种关联算法的每一步运行时间。图18为分别应用三种关联算法的数据关联正确率。

由以上仿真实验可知，改进的最近邻算法在运行时间没有大幅度增加的情况下，其数据关联正确率相比于最近邻算法更加准确；模糊-自适应关联算法，由于运用到模糊逻辑思想，具备多种关联假设相容性检验的能力，其关联树规模要比最近邻算法更复杂，所以其运行时间相对较长，而由于模糊-自适应算法考虑了噪声及干扰带来的信息不确定性，其关联正确率更高。

在室内环境下采用的是德国某实验室内部环境的数据。与维多利亚公园数据不同的是，其在小车上放置视觉传感器(摄像头)来获取室内环境信息，其中在室内环境的地面上，平均且对称地放置大小规格相同的圆形贴纸，视觉传感器所获取的环境信息就是地面上的圆形贴纸，并作为环境中的路标点。其室内实验环境模型如图19所示。

分别对最近邻关联算法、改进的马氏距离最近邻关联算法、模糊-自适应关联算法进行室内环境实验验证，如图20至图22所示，移动机器人(小车)在60m*60m的范围内移动。

图20至图22为SLAM系统中分别应用三种数据关联算法在室内环境中的仿真实验地图，下面分别从时间复杂度以及数据关联正确率两个方面分析验证改进的数据关联算法的有效性。图23为分别应用三种关联算法的每一步运行时间。图24为分别应用三种关联算法的数据关联正确率。

通过使用维多利亚公园和德国学校实验室两组真实数据，分别在室外和室内两种环境下进行实验，根据室内与室外两种不同的环境的仿真结果可知，基于模糊-自适应关联方法更适合在周围环境较复杂的情况下，其关联正确率更优于传统的最近邻算法，从而验证了基于模糊-自适应关联方法的有效性。

Claims (2)

1.基于模糊-自适应的SLAM数据关联方法，其特征在于：将归一化新息以及观测误差椭圆和估计误差椭圆的重叠比例进行模糊化后作为输入变量，将数据关联结果作为模糊推理的执行结果；通过建立适当的模糊推理规则，进行模糊推理，最终将输入映射到输出，实现数据关联；建立三输入单输出的模糊系统，由模糊逻辑思想，可以看到围绕其中的两个关键要素：1)误差椭圆；2)模糊化；

误差椭圆即描述待定点位置各方向上误差分布规律的椭圆，它表示估计值以一定概率落入以真实值为圆心的椭圆区域；误差椭圆由半长轴、半短轴和长轴方向3个参数确定，由误差协方差矩阵可以求出相应的误差椭圆参数；

以某一目标起始位置为坐标原点建立坐标轴，设其真实位置矢量为X＝[x_e,y_e]^T，其估计矢量为其中x_e,y_e表示目标的真实位姿坐标，表示目标的估计位姿坐标，则估计的误差协方差矩阵为：

$C=\left[\begin{array}{cc}{\mathrm{\&sigma;}}_x^2& {\mathrm{\&rho;\&sigma;}}_x{\mathrm{\&sigma;}}_x\\ {\mathrm{\&rho;\&sigma;}}_x{\mathrm{\&sigma;}}_x& {\mathrm{\&sigma;}}_y^2\end{array}\right]---\left(1\right)$

其中σ_x，σ_y分别为矢量坐标的方差，ρ是协方差相关系数，由于C是正定矩阵，所以|ρ|≤1；估计矢量以概率P落入以真实位置矢量X为中心的椭圆区域，其椭圆区域表达式和概率积分公式如下，其中D为椭圆区域；

${(\hat{X}-X)}^T{C}^{-1}(\hat{X}-X)\&le;D^2---\left(2\right)$

$P=\underset{{\left(\hat{X}-X\right)}^T{C}^{-1}\left(\hat{X}-X\right)\&le;D^2}{\&Integral;\&Integral;}\frac{1}{2\mathrm{\&pi;}\sqrt{\left|C\right|}}\exp \left\{-\frac{1}{2}{\left(\hat{X}-X\right)}^T{C}^{-1}\left(\hat{X}-X\right)\right\}dxdy---\left(3\right)$

为推导椭圆区域D与概率P的关系,进行变量替换,令对式(3)的积分区域进行替换，则该式可以写成：

$P=\underset{{\mathrm{\&zeta;}}^2+{\mathrm{\&eta;}}^2\&le;D^2}{\&Integral;\&Integral;}\frac{1}{2\mathrm{\&pi;}}\exp \left\{-\frac{1}{2}\left({\mathrm{\&zeta;}}^2+{\mathrm{\&eta;}}^2\right)\right\}d\mathrm{\&zeta;}d\mathrm{\&eta;}---\left(4\right)$

再将ζ和η转换到极坐标下，令ζ＝r sinθ，η＝r cosθ可得：

$p=\frac{1}{2\mathrm{\&pi;}}\underset{0}{\overset{2\mathrm{\&pi;}}{\&Integral;}}\left\{\underset{0}{\overset{D}{\&Integral;}}\exp \left(-\frac{r^2}{2}\right)rdr\right\}d\mathrm{\&theta;}=1-\exp (-D^2/2)---\left(5\right)$

式中θ为长轴方向角，可得椭圆区域D与概率P具有如下关系

$D=\sqrt{-2ln(1-P)}---\left(6\right)$

通常使用半长轴a、半短轴b和长轴方向θ三个参数确定椭圆，这三个参数通过协方差C和概率P计算；利用式(1)可将椭圆表示为以下形式：

$\frac{1}{1-{\mathrm{\&rho;}}^2}\&lsqb;\frac{{\left(x-\hat{x}\right)}^2}{{\mathrm{\&sigma;}}_x^2}+\frac{{\left(y-\hat{y}\right)}^2}{{\mathrm{\&sigma;}}_y^2}-\frac{2\mathrm{\&rho;}\left(x-\hat{x}\right)\left(y-\hat{y}\right)}{{\mathrm{\&sigma;}}_x^2{\mathrm{\&sigma;}}_y^2}\&rsqb;=D^2---\left(7\right)$

若ρ＝0，上式变为:

$\frac{{(x-\hat{x})}^2}{{\mathrm{\&sigma;}}_x^2}+\frac{{(y-\hat{y})}^2}{{\mathrm{\&sigma;}}_y^2}=D^2---\left(8\right)$

利用σ_x和σ_y可直接得到：

当σ_x≥σ_y时，a＝Dσ_x，b＝Dσ_y，θ＝0；

当σ_x＜σ_y时，a＝Dσ_y，b＝Dσ_x，

若ρ≠0，进行坐标变换：

$u=(x-\hat{x})cos\mathrm{\&theta;}+(y-\hat{y})sin\mathrm{\&theta;}---\left(9\right)$

$v=-(x-\hat{x})sin\mathrm{\&theta;}+(y-\hat{y})cos\mathrm{\&theta;}---\left(10\right)$

椭圆在新坐标系的表达式为：

$\frac{u^2}{{\mathrm{\&sigma;}}_u^2}+\frac{v^2}{{\mathrm{\&sigma;}}_{\mathrm{\&nu;}}^2}=D^2---\left(11\right)$

此时下式成立：

E|dudv|＝ρσ_xσ_y(cos²θ^-sin²θ)+(σ_x ²+σ_y ²)cosθsinθ＝0    (12)

并且

${\mathrm{\&sigma;}}_u^2={\mathrm{\&sigma;}}_x^2{\cos }^2\mathrm{\&theta;}+{\mathrm{\&sigma;}}_y^2{\sin }^2\mathrm{\&theta;}+2{\mathrm{\&rho;\&sigma;}}_x{\mathrm{\&sigma;}}_{y}sin\mathrm{\&theta;}cos\mathrm{\&theta;}---\left(13\right)$

${\mathrm{\&sigma;}}_v^2={\mathrm{\&sigma;}}_x^2{\sin }^2\mathrm{\&theta;}+{\mathrm{\&sigma;}}_y^2{\cos }^2\mathrm{\&theta;}-2{\mathrm{\&rho;\&sigma;}}_x{\mathrm{\&sigma;}}_{y}sin\mathrm{\&theta;}cos\mathrm{\&theta;}---\left(14\right)$

最终获得误差椭圆的参数：

半长轴：a＝max(Dσ_u,Dσ_v)               (15)

半短轴：b＝min(Dσ_u,Dσ_v)             (16)

长轴方向角： $\mathrm{\&theta;}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&sigma;}}_x{\mathrm{\&sigma;}}_y}{{\mathrm{\&sigma;}}_x^2-{\mathrm{\&sigma;}}_y^2}\right)& {\mathrm{\&sigma;}}_x>{\mathrm{\&sigma;}}_y\\ \frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&sigma;}}_x{\mathrm{\&sigma;}}_y}{{\mathrm{\&sigma;}}_x^2-{\mathrm{\&sigma;}}_y^2}\right)+\frac{\mathrm{\&pi;}}{2}& {\mathrm{\&sigma;}}_x<{\mathrm{\&sigma;}}_y\end{array}\right.---\left(17\right)$

当σ_x＝σ_y时，若ρ＞0，则若ρ＜0，则

表1列出了几种误差椭圆以及所对应估计值落入其椭圆区域的概率；

表1误差椭圆及其概率

若选择较小的概率值，则椭圆区域面积较小，有较多的估计点被排除在区域之外；若选择较大的概率值，则椭圆区域面积较大，落入误差椭圆的估计点也比较多；为了获得较高的数据关联正确率，同时考虑到算法计算量问题，要选择适当的误差椭圆进行计算；对于特征密度较大的环境，选择较小的误差椭圆可以降低计算量，同时避免一些错误数据关联；对于特征分布稀疏的环境，选择较大的误差椭圆能保证关联正确率，同时减少丢弃正确关联假设的可能；另外环境中噪声干扰和传感器观测的不确定性也会对误差椭圆的选取产生一定的影响；

对于每一时刻传感器获得的观测信息中的某一观测值和环境地图中某一待匹配特征状态估计值，利用上述方法分别计算它们对应的误差椭圆；为了保证误差椭圆具有实际意义，要避免长轴出现“无穷大”的情况，因此设定一个阈值，当计算得到的椭圆半长轴值超过阈值时，认为该观测值为无效数据，无需对其进行关联匹配；当半长轴在阈值范围内，才继续对其进行关联处理；

①归一化新息及其模糊化

对于每一个特征的一对椭圆间有一个最大的重叠区域，这个区域在椭圆中所占比例可以反映观测——估计对之间的相关性；两个误差椭圆相离(可将两椭圆相切视为这类相对位置的特殊情况)，分别为两椭圆中心连线与椭圆的交点，此时residual≥1；两个误差椭圆相交，且两椭圆中心不在重叠区域中，A仍为两椭圆中心连线与椭圆的交点，此时1/2≤residual≤1；两个椭圆相交，且圆心落入重叠区域内为两椭圆圆中心连线的延长线与椭圆的交点，此时0≤residual≤1/2；两椭圆的圆心重合，将此时的归一化新息定义为0；

定义归一化新息如下，其中O₁，O₂为两椭圆的中心；

$residual=\frac{\left|O_1{O}_2\right|}{\left|O_1{A}_1\right|+\left|O_2{A}_2\right|}---\left(18\right)$

将上述清晰的归一化新息按照语言模糊集进行模糊化，用于描述归一化新息的模糊语言变量为Very close、Close、Medium、Far和Very far，模糊化后的变量也将被映射到五个模糊集合中；

②两误差椭圆重合比例及其模糊化

对于几种误差椭圆位置关系中，特征状态估计值和观测值对应的误差椭圆的重叠区域能反映出它们之间的关联关系；直观上来看，重叠区域的面积越大，估计值和观测值的关联程度越大，但估计值和观测值的不确定性受到环境噪声、观测噪声、系统噪声等多种因素的影响，因此其误差椭圆的大小也是不断变化的；为了对误差椭圆重叠区域能反映出的关联程度进行定量考察，将重叠区域面积与两椭圆面积之比作为输入变量；

对于两椭圆相交形成的重叠区域，难以直接计算其面积；本发明使用八边形近似的方法，首先在椭圆内根据半长轴和半短轴与椭圆交点建立对称八边形，就可以将两个椭圆相交的重叠区域近似为两八边形的重叠区域，然后再将重叠区域分解为若干个三角形，三角形的面积是易于计算的，这样就可以得到两椭圆重叠面积的近似值；将两误差椭圆的重叠区域近似为两个八边形重叠区域，这是一个多边形区域，将其再划分为四个三角形，每个三角形的面积可通过海伦公式计算：

$S_i=\sqrt{p(p-a)(p-b)(p-c)}---\left(19\right)$

式中：p＝(a+b+c)/2，a、b、c分别为三角形的三条边长；最终两椭圆重叠区域面积area近似为三角形面积之和：

$area=\underset{i=1}{\overset{n}{\&Sigma;}}{S}_i---\left(20\right)$

再分别计算重叠区域在每个椭圆中所占比例：

per_obs＝area/ellipse_obs              (21)

per_est＝area/ellipse_est               (22)

其中per_obs和per_est分别为特征观测值和特征估计值对应的误差椭圆的重叠区域在各自椭圆中所占比例，ellipse_obs和ellipse_est分别为两误差椭圆的面积；

定义描述椭圆重叠区域比例的模糊语言变量为Seldom、Little、Medium、Most和Major，这些模糊集合对应的隶属函数，将清晰的重叠比例量映射到上述模糊集合中进行模糊化处理；

将待匹配的特征观测值和特征估计值的关联程度作为模糊推理系统的输出值，记作Degree，其取值在[0,1]之间；定义描述关联程度的模糊语言变量为VeryLow、Low、Medium、High和Very High，这些模糊集合对应的隶属函数，将清晰地输出变量映射到模糊集合中进行模糊化；

数据关联算法除了要有正确的关联率之外，还需要防止引入虚假的新路标；其由两种途径产生：a、将已有环境路标的观测错误判断为新路标；b、将虚假的环境路标观测判断为新的路标；其中第一种途径的影响更为严重，在特征环境中，将一个路标识别为两个路标将导致在两者之间分配该路标的观测数据错误；

定义传感器返回M个观测量，记为z＝{z₁,...,z_m}；对于第m个观测量z_m，计算z_m与地图中每一个路标之间的马氏距离D_e，记其中最小的为D_m。为了有效地防止虚假路标，在基于固定关联阈值G₁的基础上，引入增广阈值G₂；若D_m＜G₁，则观测值与环境中的路标可作为关联对；若G₂＜D_m，则其观测值作为一个新路标的观测；若G₁≤D_m≤G₂，则丢弃该观测；用自适应的方法来调节增广阈值G₂；

其方法如下：

1)计算当前传感器观测中距离最近的两个观测：z_m,z_n；

2)定义当前机器人位姿估计为X^v和P^v，将z_m作为虚拟的新路标加入到系统状态中，即

X＝[X^v g(z_m)]                    (23)

$P=\left[\begin{array}{cc}{P}^v& G_v\&CenterDot;P^v\\ P^v\&CenterDot;G_v^T& G_v\&CenterDot;P^v\&CenterDot;G_v^T+G_z\&CenterDot;R\&CenterDot;G_z^T\end{array}\right]---\left(24\right)$

其中，g(z_m)为逆观测模型，G_v和G_z分别为g关于X^v和z_m的雅克比矩阵，R为观测噪声；

3)将z_n作为虚拟路标的观测量，并计算记录z_n与虚拟路标之间的马氏距离D_e；

4)如果观测只有一个观测量，则D_e取为所有已得到的{D_e}中的最小值；

5)G₂＝max(D_e,G_2low)，其中G_2low为设定的下限值；

在最近邻关联算法中，错误的数据关联将导致SLAM过程的发散，因此通常设置较低的关联阈值来保证数据关联的正确性；但是较低的关联阈值将导致较高的观测丢弃率，为了使数据关联算法能够根据系统状态和环境中路标的分布情况来自适应的调节关联阈值，提出与自适应增广阈值类似的关联阈值设定方法；

其方法如下：

1)首先将关联阈值G₁初始化为预先设定的关联阈值下限D_low，然后按下面设置自适应关联阈值G₁；

2)当首次观测到多个路标时，在上节中得到的马氏距离D_e后，取G₁＝D_e/4；

3)在后续观测中，G₁＝min{G₁,D_e/4}，即G₁取为所有{D_e}中最小值的1/4；

4)当G₁≤D_low时，为防止过低的关联成功率，取G₁＝D_low；

通过以上内容的介绍，基于模糊-自适应的SLAM数据关联方法其特征在于将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果，具体方法步骤如下：

Step1：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j；

Step2：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

Step3：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

Step4：将z_n作为虚拟路标的观测量；

Step5：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

Step6：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

Step7：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

Step8：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

Step9：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

Step10：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

Step11：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

Step12：根据前一时刻的自适应的增广阈值G₂及关联阈值G₁来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测；当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测。

2.根据权利要求1所述的基于模糊-自适应的SLAM数据关联方法，其特征在于：系统围绕一个闭环的轨迹运动，在移动机器人移动过程中，用激光传感器持续测量了120个路标；具体步骤如下：

步骤一：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j；

步骤二：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

步骤三：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

步骤四：将z_n作为虚拟路标的观测量；

步骤五：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

步骤六：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

步骤七：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

步骤八：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

步骤九：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

步骤十：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

步骤十一：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

步骤十二：根据前一时刻的自适应的增广阈值G₂及关联阈值G1来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测；当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测。