CN102136088A - 一种基于模糊逻辑的同步定位与地图创建数据关联方法 - Google Patents

Abstract

本发明提出一种基于模糊逻辑的同步定位与地图创建数据关联方法，涉及自主机器人技术领域。该方法在机器人获得传感器对环境特征的观测值时，首先获取特征观测和特征估计误差椭圆，得到两个误差椭圆圆心距离的归一化新息以及两个误差椭圆的重叠比例；然后将归一化新息和重叠比例以及关联变量进行模糊化，建立模糊推理机制，将模糊化的归一化新息和重叠比例作为输入，模糊化的关联变量作为输出，并构造模糊规则；最后对模糊化的输出进行逆模糊化处理，得到单一数值的关联变量，执行地图特征管理，剔除虚假特征和虚假观测。本发明方法具有更好的抗干扰能力和鲁棒性，加快了观测值和特征的关联，且容易实现，计算量小，适合于特征密集的环境。

Description

一种基于模糊逻辑的同步定位与地图创建数据关联方法

技术领域

本发明涉及自主机器人技术领域，特别涉及一种基于模糊逻辑的同步定位与地图创建(Simultaneous Localization and Mapping，简称SLAM)数据关联方法。

背景技术

移动机器人的定位问题是确定机器人在各时刻相对于工作环境的相对位置和方位，定位是保证机器人自主导航的关键。对于已知环境中的机器人自主定位与已知机器人位置的地图创建已经有了一些实用的解决方法，然而在很多环境中机器人不能利用全局定位系统进行定位，而且事先获取机器人工作环境的地图很困难，甚至是不可能的。机器人环境建图问题通常都与机器人定位问题相关，可以分为机器人定位已知的环境建图问题和机器人定位未知的环境建图问题。机器人定位已知的建图问题相对比较简单，研究者进行了广泛的研究并提出了多种有效的解决方法。但是当机器人无法从外部获得定位信息时例如通过GPS、射频信标等，问题变得更加复杂，出现了许多新的特征。此时要求机器人从一个未知的位置出发，在不断运动的过程中利用自身携带的传感器对环境的感知递增地建立环境的导航地图，同时利用已建立的地图同步刷新自己的定位，从而全面地回答“我在哪里？”这一问题，这就是移动机器人同步定位与地图创建(SLAM)问题。

在SLAM问题研究的初期，研究者主要是将目标跟踪中的数据关联技术移植于SLAM中。直到最近几年，随着机器人应用环境越来越复杂，数据关联问题的研究逐渐受到人们的关注并提出了一些用于SLAM的数据关联算法。数据关联是建立传感器测量与以前其他测量数据之间的关系，以确定它们是否有一个公共源的处理过程，几乎所有的状态估计算法都会遇到数据关联的问题。数据关联处理必须建立每个测量与大量的可能数据集合的关系，每个数据集合表示一个说明该观测源的假设，它们可能是下列几种可能之一：(1)对前面已检测到的每一目标都有一个集合，当前一个测量与其中之一有同一源；(2)新目标集合，表示该测量是真实的，并且以前没有该目标的测量；(3)虚警集合，表示该测量是不真实的，它们可能是噪声、干扰或杂波剩余产生的，在某些条件下可以将它们消除掉。由于机器人位姿的不确定、特征密度的变化、环境中动态目标的干扰及观测中虚假成分的存在使得数据关联是一个非常困难、复杂的过程。与一般简单的定位不同，SLAM算法对数据关联非常敏感，错误的数据关联不仅会影响机器人的定位，还会改变建立的地图，直接导致SLAM算法不一致及发散。上述关于移动机器人同步定位与地图创建问题的说明具体可参考：文献1：季秀才，“机器人同步定位与地图创建中数据关联问题研究”，长沙，国防科学技术大学，2008；文献2：郭剑辉，“移动机器人同时定位与地图创建方法研究”，南京，南京理工大学，2008。

在早期SLAM研究中，最近邻数据关联方法被广泛应用，它通过对单个量测和特征间的马氏距离进行度量，完成独立关联。随后，JoséNeira等人针对SLAM问题，在分析最近邻关联算法局限性的基础上，提出了联合相容性检验(Joint Compatibility Test)关联算法。Sebastian Thrun等人将Monte Carlo数据关联方法用于FastSLAM算法中。Bailey Tim等人提出了基于图论的数据关联算法，用于对连续两帧传感器的路标观测进行数据关联。Dirk等人针对SLAM问题提出了惰性数据关联方法(Lazy Data Association)，对历史的错误数据关联进行修正，从而获得更准确的状态估计。这些方法各有特点，也存在一些不足，在复杂环境中算法所需的运算量较大，难以满足实时性需求，因此近年来研究者致力于适用性广、准确率高、计算量小的数据关联方法的探索。

在2005年2月16日公开的公开号为CN1580814的中国专利《基于小波变换的多帧最近邻快速数据关联方法》中公开了一种基于小波变换的多帧最近邻快速数据关联的方法。该发明中通过小波变换和多速率运动模型的应用，把最近邻方法推广到多帧情况。首先把模式空间内的量测预测通过小波逆变换方法映射到量测空间，形成量测空间内序列量测预测，然后在序列帧内使用最近邻方法进行数据关联，关联后的量测数据用小波变换方法压缩映射到模式空间形成多速率运动模型的量测数据，然后使用多速率运动模型进行状态更新，更新后的结果通过小波逆变换方法映射到量测空间，形成量测空间内的状态更新，从而完成一个递归循环。曾文静、张铁栋、徐玉如、姜大鹏在2009年1月公开的《一种基于蚁群算法的SLAM数据关联方法》中针对SLAM的数据关联问题，提出了基于蚁群算法的数据关联方法。将SLAM的数据关联问题演化为组合优化问题，通过利用蚁群算法解决组合优化问题的优势，结合联合最大似然法(Joint Maximum Likelihood，简称JML)关联理论，将蚁群算法应用于选择量测和特征的关联集合。该方法在保证关联效率的前提下有效地降低了运算时间，是一种解决SLAM数据关联的可行算法。周武、赵春霞在2009年3月公开的《SLAM问题的一种优化数据关联算法》中为了优化联合相容分支定界算法(JCBB)在准确度和计算复杂度方面的性能，对它进行了三处改进：一是采用互斥准则和最优准则来提高关联的准确度；二是根据机器人的位姿和传感器的测量范围将数据关联限定在局部可能区域中；三是自适应地进行分批数据关联，仿真实验结果表明，优化JCBB算法(OJCBB)在保证准确度的同时大大降低了计算复杂度。张海强、窦丽华、方浩、陈杰在2010年7月公开的《SLAM问题中的自适应序贯数据关联》中提出利用观测数据和移动机器人状态等信息，通过虚拟关联来自适应地调整阈值的方法，并采用序贯关联方法防止路标丢失和增广滞后。该方法可以很好地适应环境和噪声的变化，有效地防止虚假路标并降低观测丢弃率。

但是上述数据关联方法都并未充分考虑噪声及干扰带来的信息不确定性和模糊性，并且在真实观测与估计值距离较大时采用最近邻法会出现可能丢弃正确关联假设的情况，算法的抗干扰能力和鲁棒性不是很好。由于传感器的观测信息和路标的状态估计都存在误差，因此在进行数据关联时关联效果不是很好。

发明内容

本发明针对当前数据关联方法未充分考虑噪声及干扰带来的信息不确定性和模糊性这一问题，利用特征观测值和估计值的误差椭圆，通过模糊推理，提供了一种适用于特征密集较大环境的基于模糊逻辑的同步定位与地图创建数据关联方法。

一种基于模糊逻辑的同步定位与地图创建数据关联方法，当机器人在k时刻获得传感器对环境特征的观测值时，需要通过以下步骤对观测量值和地图中的特征进行关联：

步骤1：根据特征观测和特征估计的误差协方差矩阵，分别求取所对应的2.45-sigma误差椭圆，得到两个误差椭圆：特征观测误差椭圆和特征估计误差椭圆；

步骤2：对两个误差椭圆圆心的距离进行归一化处理得到归一化新息；

步骤3：求取两个误差椭圆的重叠区域分别在两个误差椭圆中所占的面积比例，得到两个误差椭圆的重叠比例；

步骤4：确定输入输出模糊集合的隶属度函数，对输入变量和输出变量进行模糊化；所述的输入变量为步骤2得到的归一化新息，以及步骤3得到的两个误差椭圆的重叠比例，所述的输出变量为表示特征观测和特征估计的匹配程度的关联变量Degree；

步骤5、建立模糊推理机制：将步骤4中模糊化的输入变量作为模糊推理机制的输入，将模糊化的关联变量Degree作为模糊推理机制的输出；

步骤6：构造模糊推理机制的模糊规则，将模糊化后的输入变量应用到模糊规则的前项，进行聚合规则的输出；

步骤7：对模糊化的输出进行逆模糊化处理，得到单一数值的关联变量；

步骤8：根据得到的关联变量，执行地图特征管理，剔除虚假特征和虚假观测。

本发明方法的积极效果在于，充分考虑了噪声及干扰带来的信息不确定性和模糊性，具有处理多个候选关联假设的能力，而且在真实观测与估计值距离较大时避免了最近邻法可能丢弃正确关联假设的情况，使本发明方法具有更好的抗干扰能力和鲁棒性，加快了数据关联方法的收敛，加快了观测值和特征的关联；此外，本发明方法容易实现，计算量小，适合于特征密集的环境。

附图说明

图1为本发明数据关联方法的步骤流程图；

图2中，(a)为两个误差椭圆相离的示意图；(b)为两个误差椭圆相交且圆心不在重叠区域的示意图；(c)为两个误差椭圆相交且圆心位于重叠区域的示意图；(d)为两个误差椭圆相交且圆心重合的示意图；

图3为本发明数据关联方法步骤三中采用八边形近似估计重叠区域面积的示意图；

图4中，(a)为归一化新息的隶属度函数的示意图；(b)为两个误差椭圆重叠比例的隶属度函数的示意图；(c)为关联变量的隶属度函数的示意图；

图5为本发明数据关联方法步骤五所建立的模糊推理机制示意图；

图6为本发明数据关联方法步骤六中立方体表示的模型推理机制的示意图；

图7为本发明数据关联方法的仿真环境示意图；

图8中，(a)为使用最近邻数据关联方法时的机器人位置误差曲线图；(b)为使用最近邻数据关联方法时的路标位置估计的误差曲线图；

图9中，(a)为使用本发明数据关联方法时的机器人位置误差曲线图；(b)为使用本发明数据关联方法时的路标位置估计的误差曲线图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的一种基于模糊逻辑的同步定位与地图创建数据关联方法，如图1所示，主要包含以下步骤。

步骤一：根据特征观测和估计的误差协方差矩阵，分别求取所对应的2.45-sigma误差椭圆，得到特征观测误差椭圆和特征估计误差椭圆。

机器人在k时刻的位姿为X_V(k)＝[x_V(k)，y_V(k)，ψ_V(k)]，其中x_V(k)、y_V(k)和ψ_V(k)分别表示k时刻机器人的横坐标、纵所标和方向角。机器人运动方程为：

$X_V\left(k\right)=[x_V\left(k\right),y_V\left(k\right),{\mathrm{\ψ}}_V\left(k\right)]=\left[\begin{array}{c}{x}_V(k-1)+\mathrm{dt}{U}_{k-1}\cos ({\mathrm{\ψ}}_v(k-1)+\mathrm{\α}(k-1))\\ y_V(k-1)+{\mathrm{dtU}}_{k-1}\sin ({\mathrm{\ψ}}_v(k-1)+\mathrm{\α}(k-1))\\ {\mathrm{\ψ}}_v(k-1)+\mathrm{dt}{U}_{k-1}\sin \mathrm{\α}(k-1)/L\end{array}\right]+\left[\begin{array}{c}{w}_x\\ w_y\\ w_{\mathrm{\ψ}}\end{array}\right]---\left(1\right)$

其中，dt为时刻k与时刻k-1之间的间隔时间；U_k和α(k)分别为k时刻的速度和方向控制输入量，x_V(k-1)，y_V(k-1)和ψ_V(k-1)分别表示k-1时刻机器人的横坐标、纵所标和方向角，U_k-1和α(k-1)分别为k-1时刻的的速度和方向控制输入量；w_x、w_y和w_ψ分别为横坐标、纵所标和方向角的系统噪声；L为机器人轴距。

机器人观测方程为：

$z\left(k\right)=\left[\begin{array}{c}r\left(k\right)\\ \mathrm{\θ}\left(k\right)\end{array}\right]=\left[\begin{array}{c}\sqrt{{(x_V\left(k\right)-x_i\left(k\right))}^2+{(y_V\left(k\right)-y_i\left(k\right))}^2}\\ \arctan \left[\frac{y_i\left(k\right)-y_V\left(k\right)}{x_i\left(k\right)-x_V\left(k\right)}\right]-{\mathrm{\ψ}}_V\left(k\right)\end{array}\right]+\left[\begin{array}{c}{w}_r\\ w_{\mathrm{\θ}}\end{array}\right]---\left(2\right)$

其中，(x_V(k)，y_V(k))和(x_i(k)，y_i(k))分别为k时刻机器人的坐标和第i个路标的坐标；w_r和w_θ为观测噪声，r(k)和θ(k)分别为k时刻机器人观测到的距离和角度。为测试本发明方法在非平稳、非高斯噪声下的实施情况，设置机器人初始状态为X_V(0)＝[0，0，0]^T，轴距为4m，运行线速度为3m/s，最大舵角为30°；速度误差为0.3m/s，角度误差为3°/s；传感器测距误差为0.1m/s，测角误差为1°/s。

当机器人在k时刻获得传感器对环境特征的观测值时，根据特征观测和估计的误差协方差矩阵，获取每个特征估计和观测的误差椭圆。本发明中观测值就是位置矢量X，对于位置矢量X＝[x，y]^T，其估计值为

估计误差协方差矩阵为：

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

ρσ_xσ_y表示x和y的二阶混合中心矩，ρ是不为零的实数，σ_x表示x的标准差，σ_y表示y的标准差。则估计值

以概率P落入以X为圆心的椭圆区域可表示为：

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

其中，D²为椭圆区域，

当式(4)取等号时，得到下式：

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

然后通过下面坐标变换：

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

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

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

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

其中，u、v为新坐标系下的坐标分量，参数θ∈[0，2π)为椭圆长轴方向角，σ_u表示u的标准差，σ_v表示v的标准差。

最后进一步得到误差椭圆的三个参数：半长轴a、半短轴b和长轴方向；

a＝max(Dσ_u，Dσ_v)    (8)

b＝min(Dσ_u，Dσ_v)    (9)

$\mathrm{\&theta;}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&rho;\&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{\&rho;\&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(10\right)$

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

若ρ＜0，则

为了保证误差椭圆的有效性，可以对长轴设定一个阈值A，当计算出的误差椭圆长轴长度超过阈值T_A时，认为该观测数据或特征估计为无效数据。所述的阈值T_A一般选为机器人的最大有效观测距离，通常在30m到50m之间。

步骤二、对特征观测和估计值对应的椭圆圆心的距离即新息进行归一化处理。

所述的新息就是观测值和估计值的差，也叫残差。在满足数据有效的前提下，根据两误差椭圆圆心的分布情况，将它们的相对位置分为四种情况，如图2所示，归一化新息residual为：

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

图2的(a)中两个椭圆相离，两椭圆相切视为这类相对位置的极限情况，此时A₁、A₂分别为两圆心连线与椭圆的交点，residual≥1。图2的(b)中两个误差椭圆相交，圆心不在重叠区域中，此时A₁、A₂仍为两圆心连线与椭圆的交点，1/2≤residual＜1。图2的(c)中两椭圆相交，且圆心位于重叠区域中，此时A₁、A₂为两圆心连线延长线与椭圆的交点，0≤residual≤1/2。图2的(d)中两椭圆的圆心重合，定义此时的归一化新息为0。其他特殊情况如一个椭圆包含于另一椭圆等也可按上述分类进行处理。

步骤三、求取重叠区域分别在两个误差椭圆中所占的面积比例。本发明实施例中利用八边形逼近的方法近似估计两个误差椭圆及重叠区域的面积。如图3所示，两误差椭圆的重叠区域使用两个八边形围成的多边形近似，再将这个多边形分解成4个三角形。每个三角形的面积通过海伦公式计算：

$S=\sqrt{p(p-a)(p-b)(p-c)}---\left(12\right)$

其中，半周长p为：p＝(a+b+c)/2，a，b，c分别为三角形的三条边长。这样就得到两椭圆重叠区域的面积area为：

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

其中，n为重叠区域所分解的三角形的总个数，S_i为第i个三角形的面积。

最后得到特征观测误差椭圆的重叠比例per_obs和特征估计误差椭圆的重叠比例per_est：

per_obs＝area/ellipse_obs    (14)

per_est＝area/ellipse_est    (15)

其中ellipse_obs和ellipse_est分别为采用八边形逼近的方法得到的特征观测误差椭圆和特征估计误差椭圆的八边形近似面积。

步骤四、确定输入输出模糊集合的隶属度函数，将输入输出变量进行模糊化。将归一化新息进行模糊化，将其分为Exact、Close、Medium、Far和Very far，Exact、Close、Medium、Far和Very far五个模糊集合，其隶属度函数如图4的(a)所示，Exact表示非常接近，归一化新息范围在[0，0.15]，Close表示很接近，归一化新息范围在[0.1，0.45]，Medium表示中等接近，归一化新息范围在[0.3，0.7]，Far表示远距离，归一化新息范围在[0.55，0.9]，Veryfar表示非常远距离，归一化新息大于0.85。将两误差椭圆重叠比例分为五个模糊集合：None、Little、Medium、Most和All，他们的隶属度函数如图4的(b)所示，None表示无重叠，重叠比例在[0，0.1]，Little表示少部分重叠，重叠比例在[0.05，0.4]，Medium表示部分重叠，重叠比例在[0.3，0.7]，Most表示大部分重叠，重叠比例在[0.6，0.95]，ALL表示全部重叠，重叠比例在[0.9，1]。模糊推理的结果为特征观测和特征估计的关联变量，代表特征观测和特征估计的匹配程度。关联变量对应的模糊集合为Very Low、Low、Medium、High和Very High，其隶属度函数如图4(c)所示，Very Low表示关联度很低，关联变量范围在[0，0.25]，Low表示关联度低，关联变量范围在[0，0.5]，Medium表示关联度中等，关联变量范围在[025，0.75]，High表示关联度高，关联变量范围在[05，1]，Very High表示关联度非常高，关联变量范围在[075，1]。将清晰变量进行模糊化主要是将清晰变量投影到模糊域，求出其对应于模糊集合的隶属度。

步骤五、建立模糊推理机制。建立如图5所示的模糊推理机制，将归一化新息以及观测误差椭圆和估计误差椭圆的重叠比例进行模糊化后作为输入变量，将数据关联结果作为模糊推理的执行结果。数据关联结果也就是输出变量，是一个表示特征观测和特征估计的匹配程度的关联变量Degree。

步骤六、构造模糊规则，将模糊化后的输入应用到模糊规则的前项，进行聚合规则的输出。对于步骤五所述的三个输入量和一个输出量的模糊推理机制，可以利用5×5×5的立方体方便的表示模糊规则。针对SLAM具体应用环境，通过实验或者向专家咨询可以抽取并构造模糊规则。所述的模糊推理机制通过输入输出变量的关系可构造出125条(5×5×5)独立的模糊规则，如图6所示为采用立方体模型表示的示意图。构建模糊规则的形式为：IF A AND B AND C THEN D，A表示模糊化的归一化新息，B表示模糊化的观测误差椭圆的重叠比例，C表示模糊化的估计误差椭圆的重叠比例，D表示模糊化的关联变量。例如，底层元素点(5，5)的规则为：

IF residual is Exact AND per_obs is All AND per_est is All THEN Degree is Very High。

在此不考虑不同规则的重要性程度，设置各条规则权重相同且为1。将模糊化后的输入应用到模糊规则的前项，进行聚合规则的输出。

步骤七、对模糊化的输出变量进行逆模糊化处理，得到单一数值。所述的模糊化的输出变量就是指模糊化的关联变量。

步骤八、对观测量和地图中的特征进行关联之后，要对地图中的特征进行管理，剔除虚假特征和虚假观测：将地图中的特征分为两个集合：确定特征集合和待定特征集合。观测值和路标间的关系包含三种可能：a.观测源于某一路标；b.观测由一新的路标产生；c.观测不由任何真实路标产生，即虚假观测。当传感器获得一个新的观测量时，使用模糊数据关联方法判断新观测量与地图中特征关联情况。为关联变量设定一个阈值T_B，如果步骤七得到的关联变量达未达到阈值T_B认为该观测和特征是无关联的；如果达到阈值T_B，则将新观测值和该特征进行关联，并利用观测值进行状态估计的更新。所述的阈值T_B，设定为0.8～0.9之间。优先对观测值和确定集合中的特征进行关联，如果观测值能与该集合中的特征进行关联，就利用观测值更新特征的状态估计；否则，就考察它与待定特征集合中特征的关联情况。如果观测值与待定特征集合中的特征关联成功，那么该特征的可关联数增加1，当某一待定特征的关联数超过某一数值T_C时，认为这个特征是稳定的，将其移动到确定特征集合中。所述的数值T_C的选取视环境路标的密度而定，通常选取5～10之间的整数。若观测量与多个特征产生关联，则认为该观测量是一个虚假观测，将其剔除。如果观测量不能和地图中已有特征关联，将其作为新的特征放入待定特征集合。待定特征集合中的特征，从其进入集合开始，经过一定长度的时间(通常选为机器人20次观测)后如果仍不能转化成确定特征，就被视为是虚假特征，将其从地图中剔除。

本发明实施例基于数学分析软件MATLAB做仿真实验，选取最近邻数据关联与本发明提出的数据关联方法进行比较，仿真环境如图7所示。其中“+”代表路标，机器人从坐标(0，0)开始，沿曲线逆时针行驶一周，实验中利用扩展卡尔曼滤波器(EKF)进行状态估计。图8中(a)和(b)分别为使用最近邻数据关联方法时机器人位置和第一个路标估计的误差曲线。图9中(a)和(b)分别为使用本发明的数据关联方法时机器人位置和第10个路标估计的误差曲线，其中虚线代表95％置信边界，实线代表误差值。由图8中的(a)可以看出机器人位置两个方向上的估计误差较大，并且接近置信区间的边界，表明当环境中特征间距比较小并且传感器同一时间可能观测到多个特征时，使用最近邻数据关联方法会导致较大的估计误差，进而影响SLAM算法收敛速度。图8的(b)中，路标估计的x方向误差落在置信区间之外，说明对于该路标的数据关联很有可能是误关联。图9的(a)和(b)中，机器人位置和路标的估计误差都明显减小，并且误差曲线较为平稳，表明本发明的数据关联方法具有较高的关联正确率，从而提高SLAM环境构图成功率和机器人定位精度，同时加快了算法收敛速度。

通过分析本发明实施例的仿真结果可知，在路标比较密集的环境中，由于最近邻数据关联方法将单个观测和特征进行独立关联，忽略了噪声干扰带来的信息不确定性和模糊性，尤其是当传感器在同一时间观测到多个环境特征的时候，容易发生误关联，而错误的数据关联又会影响机器人定位和路表估计的精度。本发明的数据关联方法由于使用隶属度对观测信息和估计值的关联进行了模糊化，有效表达了实际系统中的不确定性和不精确性信息，通过模糊推理过程能够消除这种不确定性和模糊性，因此有效降低了机器人位置和路标的估计误差。

Claims (10)

1.一种基于模糊逻辑的同步定位与地图创建的数据关联方法，其特征在于，该数据关联方法针对特征密集环境，当机器人在k时刻获得传感器对环境特征的观测值时，需要通过以下步骤对观测量值和地图中的特征进行关联；

步骤1：根据特征观测和特征估计的误差协方差矩阵，分别确定所对应的2.45-sigma误差椭圆，得到两个误差椭圆：特征观测误差椭圆和特征估计误差椭圆；

步骤2：对两个误差椭圆圆心的距离进行归一化处理得到归一化新息；

步骤3：确定两个误差椭圆的重叠区域分别在两个误差椭圆中所占的面积比例，得到两个误差椭圆的重叠比例；

步骤4：确定输入输出模糊集合的隶属度函数，对输入变量和输出变量进行模糊化；所述的输入变量为步骤2得到的归一化新息，以及步骤3得到的两个误差椭圆的重叠比例，所述的输出变量为表示特征观测和特征估计的匹配程度的关联变量Degree；

步骤5、建立模糊推理机制：将步骤4中模糊化的输入变量作为模糊推理机制的输入，将模糊化的关联变量Degree作为模糊推理机制的输出；

步骤6：构造模糊推理机制的模糊规则，将模糊化后的输入变量应用到模糊规则的前项，进行聚合规则的输出；

步骤7：对模糊化的输出进行逆模糊化处理，得到单一数值的关联变量；

步骤8：根据得到的关联变量，进行地图特征管理，剔除虚假特征和虚假观测。

2.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤1中所述的2.45-sigma误差椭圆，具体是通过下面过程建立的：

首先，对于位置矢量X＝[x，y]^T，其估计值为

则估计的误差协方差矩阵C为：

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

其中，ρσ_xσ_y表示x和y的二阶混合中心矩，ρ是不为零的实数，σ_x表示x的标准差，σ_y表示y的标准差。

估计值

以概率P落入以X为圆心的椭圆区域表示为：

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

其中，D²为椭圆区域；

$D=\sqrt{-21n(1-P)}---\left(2\right)$

将式(1)取等号，得到：

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

然后通过下面坐标变换：

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

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

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

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

其中，u、v为新坐标系下的坐标分量，参数θ∈[0，2π)为长轴方向角，σ_u表示u的标准差，σ_v表示v的标准差；

最后进一步得到误差椭圆的三个参数：半长轴a、半短轴b和长轴方向；

a＝max(Dσ_u，Dσ_v)    (6)

b＝min(Dσ_u，Dσ_v)    (7)

$\mathrm{\&theta;}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{tg}}^{-1}\left(\frac{2{\mathrm{\&rho;\&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{\&rho;\&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(8\right)$

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

若ρ＜0，则

判断得到的误差椭圆的半长轴a的长度是否超过设定的阈值T_A，所述的阈值T_A选为机器人的最大有效观测距离，如果超过，则该观测数据或特征估计为无效数据。

3.根据权利要求2所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，所述的阈值T_A的取值范围为30m～50m。

4.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤2中所述的归一化处理，是指将绝对距离转化为相对于两矢量间不确定性的相对距离，归一化新息residual为：

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

其中，O₁和O₂分别为步骤1得到的特征观测误差椭圆和特征估计误差椭圆的中心，A₁为两个椭圆圆心连线与特征观测误差椭圆的交点，A₂为两个椭圆圆心连线与特征估计误差椭圆的交点。

5.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤3中所述的重叠区域，是通过八边形逼近的方法进行近似得到，将两个误差椭圆的重叠区域近似为了一个多边形，将这个多边形分解为n个三角形，利用海伦公式

求得每个三角形的面积，半周长p为：p＝(a+b+c)/2，a、b、c分别为三角形的三条边长，最后两椭圆重叠区域的面积area就是三角形面积之和：S_i为第i个三角形的面积；最后得到特征观测误差椭圆的重叠比例per_obs和特征估计误差椭圆的重叠比例per_est：

per_obs＝area/ellipse_obs    (10)

per_est＝area/ellipse_est    (11)

其中ellipse_obs和ellipse_est分别为采用八边形逼近的方法得到的特征观测误差椭圆和特征估计误差椭圆的面积。

6.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤4中所述的输入输出模糊集合的隶属度函数，分别是：将两个误差椭圆的圆心间的归一化新息分为五个模糊集合：Exact、Close、Medium1、Far和Very far，其中，Exact表示非常接近，Close表示很接近，Medium1表示中等接近，Far表示远距离，Very far表示非常远距离；将两个误差椭圆的重叠区域的比例分为五个模糊集合：None、Little、Medium2、Most和All，其中，None表示无重叠，Little表示少部分重叠，Medium2表示部分重叠，Most表示大部分重叠，All表示全部重叠；将关联变量Degree分为五个模糊集合：Very Low、Low、Medium3、High和Very High，其中，Very Low表示关联度很低，Low表示关联度低，Medium3表示关联度中等，High表示关联度高，Very High表示关联度非常高。

7.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤6所述的模糊规则的行式为：IF A AND B AND C THEN D，其中A表示模糊化的归一化新息，B表示模糊化的观测误差椭圆的重叠比例，C表示模糊化的估计误差椭圆的重叠比例，D表示模糊化的关联变量。

8.根据权利要求1所述一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，步骤8所述的地图特征管理，具体是：将地图中的特征分为两个集合：确定特征集合和待定特征集合，其次将观测值和路标间的关系分为三种情况：a.观测源于某一路标；b.观测由一新的路标产生；c.观测不由任何真实路标产生，为虚假观测；判断步骤7得到的关联变量是否达到设定的阈值T_B，如果未达到阈值T_B认为该观测和特征是无关联的；如果达到阈值T_B，则将该观测值和该特征进行关联，并利用该观测值进行状态估计的更新，在将观测值和特征进行关联时，优先对该观测值和确定特征集合中的特征进行关联，如果成功进行关联，就利用观测值更新特征的状态估计；如果该观测值不能与该集合中的特征进行关联，就考察它与待定特征集合中的特征的关联情况，如果该观测值与待定特征集合中的特征关联成功，那么利用观测值更新特征的状态估计，同时该特征的可关联数增加1，当某一待定特征的关联数超过某一数值T_C时，则将该特征移动到确定特征集合中，若观测量与多个特征产生关联，则将该观测值剔除，所述的数值T_C根据环境路标的密度设定；如果该观测值不能和地图中已有特征关联，将其作为新的特征放入待定特征集合中，待定特征集合中的特征，从其进入待定特征集合开始，经过一定长度的时间后如果仍不能转化成确定特征，将其从地图中剔除。

9.根据权利要求8所述的一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，所述的阈值T_B，设定为0.8～0.9之间，所述的数值T_C选取5～10之间的整数。

10.根据权利要求8所述的一种基于模糊逻辑的同步定位与地图创建数据关联方法，其特征在于，所述的一定长度的时间，为机器人20次观测的时间。

Images