基于模糊-自适应的slam数据关联方法
【技术领域】
[0001] 基于模糊-自适应的SLAM数据关联方法将模糊逻辑规则运用到特征观测值与估 计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阔值，从而改善数据关联 效果，进而在同步定位与地图构建中提高关联正确率及定位精度，属于机器人自主导航领 域。
【背景技术】
[0002] 同步定位与地图构建（SimultaneousLocalizationandMapping,SLAM)其基本 思想是：让机器人在未知环境中从未知位置开始移动，通过自身所带传感器扫描到的路标 点的信息进行自身位置估计，同时构建增量式地图。在SLAM过程中，根据观测信息进行实 时的地图估计与更新，其中数据关联是一个关键问题。SLAM中的数据关联是指建立在不同 时间、不同地点的传感器观测之间、传感器观测与地图特征之间或者各特征之间的对应关 系，W确定它们是否源于同一物理实体的过程，同时也包括将未与地图中特征相匹配的传 感器观测信息确定为新特征的过程。
[0003] 目前，数据关联方法主要包括基于独立匹配的最近邻算法（ICNN)、基于联合概率 的数据关联（JPDA)、多维分配法（MDA)W及多假设跟踪算法（MHT)等。其中最近邻数据关 联方法被广泛应用，它通过对单个观测值和特征间的马氏距离进行度量，完成独立关联，假 设关联对之间的马氏距离服从卡方分布，并根据预设的置信度计算得到关联阔值和增广阔 值。
[0004] 上述方法各有特点，也存在一些不足，主要体现在：
[0005] (1)错误的数据关联将导致SLAM过程的发散，因此通常设置较低的关联阔值来保 证数据关联的正确性。但是较低的关联阔值将导致较高的观测丢弃率。因此当路标分布、 系统噪声等因素发生变化时，观测丢弃率会升高，从而导致SLAM估计性能降低。
[0006] (2)在不确定性较大的复杂环境中，关联的正确率有待提高。在大规模关联问题 中，算法所需的运算量较大，难W满足实时性需求。
[0007] (3)数据关联算法需要防止引入虚假的新路标。上述方法不能有效地防止虚假路 标，尤其是在环境中路标较稀疏的情况下，将一个路标识别为两个路标，导致在两者之间分 配该路标的观测数据，从而使得观测对移动机器人的位姿修正作用减弱。
【发明内容】
[000引本发明针对现有SLAM数据关联方法的不足，提出一种基于误差楠圆和关联阔值 的模糊-自适应数据关联方法，将模糊逻辑规则运用到特征观测值与估计值的数据关联 中，并能够根据环境和噪声的变化自适应地确定阔值，从而改善数据关联效果。仿真实验和 真实实验证明了该方法的有效性。
[0009] 模糊逻辑基于W下思想：
[0010] 1、计算待匹配观测一-估计对的误差楠圆，圆屯、分别为当前时刻的估计值和特征 观测值。计算两个圆屯、之间的几何距离，并进行归一化处理，结合观测一-估计对协方差的 大小使其能反映出观测值和估计值间的相关性。
[0011] 2、如图1所示，图中特征观测与估计的两个误差楠圆互有重叠，并且每个特征的 两个误差楠圆间都有一个最大的重叠区域，在该个区域中其两个楠圆重叠部分所占的比例 则反映出估计一一观测之间的相关联程度。
[0012] 该种基于模糊逻辑思想的基本结构如图2所示，将归一化新息W及观测误差楠圆 和估计误差楠圆的重叠比例进行模糊化后作为输入变量，将数据关联结果作为模糊推理的 执行结果。通过建立适当的模糊推理规则，进行模糊推理，最终将输入映射到输出，实现数 据关联。建立=输入单输出的模糊系统，由模糊逻辑思想，可W看到围绕其中的两个关键要 素；1)误差楠圆；2)模糊化。
[0013] 误差楠圆即描述待定点位置各方向上误差分布规律的楠圆，它表示估计值W-定 概率落入W真实值为圆屯、的楠圆区域。误差楠圆由半长轴、半短轴和长轴方向3个参数确 定，由误差协方差矩阵可W求出相应的误差楠圆参数。
[0014] W某一目标起始位置为坐标原点建立坐标轴，设其真实位置矢量为X= [XyyjT， 其估计矢量为义=[疋，义r，其中X。，y。表示目标的真实位姿坐标，屯，j)。表示目标的估计位 姿坐标，则估计的误差协方差矩阵为：
[0015] (1)
[0016] 其中Oy，Oy分别为矢量坐标的方差，P是协方差相关系数，由于C是正定矩阵， 所WIPI《1。估计矢量义W概率P落入W真实位置矢量X为中屯、的楠圆区域，其楠圆区 域表达式和概率积分公式如下，其中D为楠圆区域。
[0019] 为推导楠圆区域D与概率P的关系，进行变量替换，令〇'=[(/7^=c 对式（3)的积分区域进行替换，则该式可W写成：
[0020]
(4)
[0021] 再将C和n转换到极坐标下，令C=rsin0，n=rcos0可得；
[0022]
巧）
[0023] 式中0为长轴方向角，可得楠圆区域D与概率P具有如下关系
[0024]
(6)
[0025] 通常使用半长轴a、半短轴b和长轴方向0 =个参数确定楠圆，该=个参数通过协 方差c和概率P计算。利用式（1)可将楠圆(义-义fc-i(义-义)=公2表示为W下形式：
[0029] 利用。济。乃直接得到：
[0030] 当 0 0y时，a=D〇x，b=D〇y，白=0;
[003"1]当 0x< 0y时，a=D〇y，b=D〇x，6 =子；
[0032] 若P声0,进行坐标变换：
[0037] 此时下式成立；
[0042] 最终获得误差楠圆的参数：
[0043] 半长轴；a=max值 0U，D0V)(巧）
[0044] 半短轴；b=min值0U,D0V) (16)
[0045] 长轴方向角
。7) 几巧
[0046] 当。x=。肺若P> 0,则食=与若P< 0,则0 = -J。
[0047] 表1列出了几种误差楠圆W及所对应估计值落入其楠圆区域的概率。
[0048] 表1误差楠圆及其概率
[0049] Table1Commonerrorellipsesanditsprobability
[0050]
[0051] 若选择较小的概率值，则楠圆区域面积较小，有较多的估计点被排除在区域之外； 若选择较大的概率值，则楠圆区域面积较大，落入误差楠圆的估计点也比较多。为了获得较 高的数据关联正确率，同时考虑到算法计算量问题，要选择适当的误差楠圆进行计算。对于 特征密度较大的环境，选择较小的误差楠圆可W降低计算量，同时避免一些错误数据关联； 对于特征分布稀疏的环境，选择较大的误差楠圆能保证关联正确率，同时减少丢弃正确关 联假设的可能。另外环境中噪声干扰和传感器观测的不确定性也会对误差楠圆的选取产生 一定的影响。
[0化2] 对于每一时刻传感器获得的观测信息中的某一观测值和环境地图中某一待匹配 特征状态估计值，利用上述方法分别计算它们对应的误差楠圆。为了保证误差楠圆具有实 际意义，要避免长轴出现"无穷大"的情况，因此设定一个阔值，当计算得到的楠圆半长轴值 超过阔值时，认为该观测值为无效数据，无需对其进行关联匹配；当半长轴在阔值范围内， 才继续对其进行关联处理。
[0化3]①归一化新息及其模糊化
[0化4]对于每一个特征的一对楠圆间有一个最大的重叠区域，该个区域在楠圆中所占比 例可W反映观测一一估计对之间的相关性，如图3所示。图3(a)为中两个误差楠圆相离 (可将两楠圆相切视为该类相对位置的特殊情况），分别为两楠圆中屯、连线与楠圆的交点， 此时resi化al> 1。图3(b)中两个误差楠圆相交，且两楠圆中屯、不在重叠区域中，A仍为 两楠圆中屯、连线与楠圆的交点，此时1/2《resi化al《1。图3(c)中两个楠圆相交，且圆 屯、落入重叠区域内为两楠圆圆中屯、连线的延长线与楠圆的交点，此时〇《resi化al《1/2。 图3(d)中两楠圆的圆屯、重合，将此时的归一化新息定义为0。
[0055]定义归一化新息如下，其中〇1，〇2为两楠圆的中屯、。
[0056]
(18)
[0化7]将上述清晰的归一化新息按照语言模糊集进行模糊化，用于描述归一化新息的模 糊语言变量为VeiTclose、Close、Medium、Far和VeiTfar,模糊化后的变量也将被映射到 五个模糊集合中，其相应