[发明专利]基于模糊-自适应的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表示目标的真实位姿坐标，表示目标的估计位姿坐标，则估计的误差协方差矩阵为：