CN106529678A - 一种基于最大最小蚂蚁***优化的slam数据关联方法 - Google Patents

Abstract

本发明公开了一种基于最大最小蚂蚁***优化的SLAM数据关联方法，首先采用检验门限过滤来缩小数据关联的解空间范围，提高算法的搜索效率；然后将数据关联问题转化为最优路径求解问题，并利用解释树模型表示关联路径节点；最后将最大最小蚂蚁***与联合最大似然准则相结合，将数据关联假设的联合最大似然值作为目标函数进行路径搜索。本发明缩小了数据关联的关联解空间，提高了算法的搜索效率；有效地弥补了蚁群算法收敛速度慢、搜索停滞及易陷入局部最优的缺陷。

Description

一种基于最大最小蚂蚁***优化的SLAM数据关联方法

技术领域

本发明涉及机器人自主导航领域，尤其涉及一种基于最大最小蚂蚁***优化的SLAM数据关联方法。

背景技术

同时定位与地图构建(Simultaneous Localization and Mapping,SLAM)的问题可以描述为在未知先验信息环境中，移动机器人利用所安装的传感器构建增量式地图，同时根据地图与位置估计进行自身定位，是机器人实现完全自主导航的关键技术问题，也是当前移动机器人领域研究的重点和热点。数据关联作为SLAM问题的重要组成部分，建立了不同时刻、不同地段所获得的传感器量测与特征地图之间的对应关系。然而，在实际SLAM过程中，数据关联算法的关联正确率和计算复杂度直接影响着SLAM的精确性和实时性。

数据关联通常是由统计估计的方法来确定。目前，在SLAM问题中常用的数据关联算法有单匹配最近邻(Individual Compatibility Nearest Neighbor,ICNN)算法、联合相容分支定界(Joint Compatibility Branch and Bound,JCBB)算法以及多假设跟踪(Multiple Hypothesis Tracking,MHT)算法等。其中，ICNN算法计算存储量小，简单易行，但由于仅考虑单一最近环境特征间的关联匹配，因此只适合小规模SLAM环境，而对于特征分布密度或环境规模较大的情况，其抗干扰能力较差；JCBB算法虽然考虑了各量测特征匹配对的相关性，关联正确率高，但由于需要不断利用联合相容检验准则来检验所有观测值和地图特征之间的相容性，以及分支定界算法来搜索解空间，导致计算复杂度非常高，实际应用受到限制；MHT算法通过综合考虑相邻几帧的观测来进行关联决策，适用于初始化及观测噪声较大的场合，但由于假设数目会随着时间成指数增长，故不适合实时的应用。此外，在《Dementia&Geriatric Cognitive Disorders》中2004年发表的文献《Towards LazyData Association in SLAM》中，提出了一种惰性数据关联(Lazy Data Association,LDA)算法，能够回溯修正过去错误关联，但需要计算维数较大矩阵的逆，难以在实际中应用；在《计算机应用》中2009年发表的文献《一种基于蚁群算法的SLAM数据关联方法》中，提出了一种将联合最大似然准则与蚁群优化算法相结合的数据关联方法，显著地提高了最大似然算法的关联正确率，但由于蚁群算法收敛速度慢、搜索停滞及易陷入局部最优的缺陷，使得其关联正确率和运行时间需要进一步的改善。

发明内容

本发明所要解决的技术问题是针对背景技术中所涉及到的缺陷，提供一种基于最大最小蚂蚁***优化的数据关联方法，使得数据关联在正确率和实时性上都有所提高，有效地解决移动机器人的SLAM数据关联问题。

本发明为解决上述技术问题采用以下技术方案：

一种基于最大最小蚂蚁***优化的SLAM数据关联方法，包括以下步骤：

步骤1)，在移动机器人的工作环境中布置若干静态的路标特征点，并设定移动机器人的运动初始位置和速度；

步骤2)，利用移动机器人自带的声呐传感器采集环境路标特征信息，令已存的环境路标特征集合为x＝(x₁,x₂,…,x_n)，新观测到的环境路标特征集合为z＝(z₁,z₂,…,z_m)，建立观测到的环境路标特征集合z与已存的环境路标特征集合x之间的数据关联假设H_m：

H_m＝{h₁,…,h_i,…,h_m}

其中，n为已存路标特征点的个数，m为新观测到的路标特征点的个数；h_i为新观测到的路标特征z_i的关联取值，若新观测到的路标特征z_i在已存的环境路标特征集合x中存在与其一致的路标特征x_j，取值为j，此时(z_i,x_j)为h_i对应的关联匹配对；否则取值为0；i为大于等于1小于等于m的自然数，j为大于等于1小于等于n的自然数；

步骤3)，采用下式中的检验门限条件对所有的关联匹配对进行检验，并滤除不满足检验条件的关联匹配对：

式中，为新观测到的路标特征z_i与已存的环境路标特征x_j在t时刻的马氏距离，γ为置信度为1-α时的卡方分布值，其中α的取值预设；

然后用解释树模型表示满足检验条件的关联匹配对，其中每个节点视为蚂蚁游走的路径节点；

步骤4)，令蚂蚁的数目为M，迭代最大次数为N，将每只蚂蚁的禁忌表都设置为空集，将蚂蚁最短路径集合设置为空集，将每只蚂蚁选择的次数设置为0，将迭代次数Ne设置为0，将信息素初始值τ_ij(0)设置为预设的信息素初始最大值τ_max；

步骤5)，对于M只蚂蚁中的每一只蚂蚁：

步骤5.1)，获得该蚂蚁在当前节点下可以游走的所有节点的集合A；

步骤5.2)，在集合A中剔除掉该蚂蚁在禁忌表中的节点，得到集合B；

步骤5.3)，针对集合B中的每一个节点，按照下列公式计算该蚂蚁从当前节点游走到其的概率

式中，i、j、r均为大于等于1小于等于m的自然数，k为大于等于1小于等于M的自然数，B_k为该蚂蚁的禁忌表，β₁为信息启发因子、β₂为期望启发因子，启发式信息η_ij和η_ir值分别为1/N_ij和1/N_ir，N_ij为当前节点与该节点的路径长度，N_ir为当前节点与集合B中除该节点以外其他任一节点的路径长度；τ_ij为当前节点到该节点之间路径上的信息素，τ_ir为当前节点与集合B中除该节点以外其他任一节点之间路径上的信息素；

步骤5.4)，在集合B中选择游走到其概率最高的节点作为该蚂蚁的下一个节点，将此节点加入到该蚂蚁的禁忌表后向其进行游走；

步骤5.5)，根据下式计算该蚂蚁的信息素最小值τ_min：

式中，p_best为步骤5.4)中该蚂蚁从当前节点游走到下一个节点的概率，num为该蚂蚁选择的次数；

步骤5.6)，对该蚂蚁选择的次数num进行加1；

步骤6)，反复执行步骤5)，直至所有蚂蚁都到达最底层叶节点；

步骤7)，计算每只蚂蚁游走的路径总长度，并选出路径总长度最短的蚂蚁，将该蚂蚁的路径和路径总长度作为一个元素添加至蚂蚁最短路径集合中，并将该蚂蚁经过的路径的信息素按照下式进行更新：

τ_ij＝ρ·τ_ij+1/f(s^best)

式中，ρ为信息素挥发因子，f(s^best)表示最短路径总长度的值，当τ_ij的值小于τ_min时将其设置为τ_min，当τ_ij的值大于τ_max时将其设置为τ_max；

步骤8)，对迭代次数Ne进行加1，将每只蚂蚁的禁忌表都设置为空集，将每只蚂蚁选择的次数设置为0，将信息素初始值设置为预设的信息素初始最大值τ_max；

步骤9)，反复执行步骤5)至步骤8)，直到当Ne大于最大迭代次数N；

步骤10)，在蚂蚁最短路径集合中筛选出路径总长度最短的蚂蚁，将其路径作为最佳路径，并将该路径中的节点作为最优数据关联假设进行输出。

作为本发明一种基于最大最小蚂蚁***优化的SLAM数据关联方法进一步的优化方案，步骤3)中确定新观测到的路标特征z_i与已存的环境路标特征x_j之间的马氏距离的具体过程包括：

步骤3.1)，建立移动机器人在t时刻的观测模型：

z_t,i＝h_t,j(x_t|t-1)+ω_t,i

式中，h_t,j()为移动机器人在t时刻的第i个观测量z_t,i与***状态x_t|t-1之间的非线性观测函数，ω_t,i是均值为零、协方差为R_t,i的随机测量噪声；

步骤3.2)，将上式中的非线性观测模型在当前估计值处对其线性化处理：

式中，雅克比矩阵

步骤3.3)，分别计算t时刻的第i个观测量z_t,i和***状态x_t|t-1的估计值间的距离新息向量ν_t,ij及其协方差S_t,ij如下：

式中，P_t|t-1为估计值的方差矩阵；

步骤3.4)，按下式计算马氏距离为：

作为本发明一种基于最大最小蚂蚁***优化的SLAM数据关联方法进一步的优化方案，步骤7)中将蚂蚁的经过的路径的信息素更新后采用下式对该信息素进行平滑处理：

τ_ij＝τ_ij+δ·(τ_max-τ_ij)

式中，0≤δ≤1。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1.采用检验门限过滤极小可能的关联假设，缩小了数据关联的关联解空间，提高了算法的搜索效率；

2.利用解释树模型将关联匹配对表示成路径节点，以联合最大似然值作为优化目标，从而将联合最大似然数据关联问题转变为求解最优路径问题；

3.将联合最大似然准则和最大最小蚂蚁***相结合，改善了最大似然算法数据关联正确率低以及定位精度差的问题，并有效地弥补了蚁群算法收敛速度慢、搜索停滞及易陷入局部最优的缺陷，在一定程度上本方法提高了数据关联正确率并缩短了运行时间。

附图说明

图1(a)、图1(b)分别为检验门限、不确定数据关联的示意图；

图2为本发明解释树模型结构示意图；

图3为本发明数据关联流程图；

图4(a)、图4(b)、图4(c)、图4(d)分别为仿真环境地图、最大似然算法数据关联结果、基于蚁群算法的联合最大似然数据关联结果、本发明数据关联结果；

图5(a)、图5(b)、图5(c)、图5(d)分别为数据关联正确率随时间步变化图、X轴定位误差随时间步变化图、Y轴定位误差随时间步变化图、方向角定位误差随时间步变化图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本发明主要由三部分组成：第一部分，采用检验门限过滤来缩小数据关联的解空间范围，提高算法的搜索效率；第二部分，将数据关联问题转化为最优路径求解问题，并利用解释树模型表示关联路径节点；第三部分，将最大最小蚂蚁***与联合最大似然准则相结合，将数据关联假设的联合最大似然值作为目标函数进行路径搜索。

本实施方式中，为描述方便，用MMAS-JML表示最大最小蚂蚁***优化的联合最大似然数据关联方法，用ML表示最大似然数据关联方法，用ACO-JML表示蚁群算法优化的联合最大似然数据关联方法。

本发明一种基于最大最小蚂蚁***优化的SLAM数据关联方法，如图3所示具体实施方式包括以下步骤：

步骤1)，在MATLAB软件平台下搭建如图4(a)所示的仿真环境地图。地图为10m×10m的正方形区域，并在路径内外侧均匀分布168个静态路标特征点。其中，曲线表示运动轨迹，圆圈表示运动路径节点，点表示环境特征，十字表示特征估计，三角形表示搭载有声呐传感器的移动机器人***。声呐传感器测量距离为3m，测量角度范围为[-90°,90°]，角度测量误差为0.125°，距离测量误差为0.01m。机器人***在X和Y方向上的测量噪声均为0.1m，方向角的测量噪声为0.3°。机器人从原点位置开始出发，以0.25m/s的速度沿曲线逆时针运动，并采用扩展卡尔曼滤波SLAM方法进行闭环实验。

步骤2)，利用移动机器人自带的声呐传感器采集环境路标特征信息，令已存的环境路标特征集合为x＝(x₁,x₂,…,x_n)，新观测到的环境路标特征集合为z＝(z₁,z₂,…,z_m)，建立观测到的环境路标特征集合z与已存的环境路标特征集合x之间的数据关联假设H_m：

H_m＝{h₁,…,h_i,…,h_m}

其中，n为已存路标特征点的个数，m为新观测到的路标特征点的个数；

h_i为新观测到的路标特征z_i的关联取值，若新观测到的路标特征z_i在已存的环境路标特征集合x中存在与其一致的路标特征x_j，取值为j，此时(z_i,x_j)为h_i对应的关联匹配对；否则取值为0；其中，i为大于等于1小于等于m的自然数，j为大于等于1小于等于n的自然数；

数据关联算法需要从关联假设H_m中选出一种最优关联假设匹配对，这不仅要把观测值与地图中已有特征正确地关联上，还需要能够识别新特征和排除虚假观测。然而，随着观测数目的增加，数据关联算法的计算复杂度会呈指数增长。考虑到移动机器人在实际运动过程中每个时间步的运动范围是有限的，故不需要将每个观测量与已存特征进行数据关联。为了排除极小可能的关联假设，减小关联解空间，数据关联算法需要对每个特征设定检验门限。

步骤3)，建立移动机器人在t时刻的观测模型为：

z_t,i＝h_t,j(x_t|t-1)+ω_t,i

式中，h_t,j()为移动机器人在t时刻的第i个观测量z_t,i与***状态x_t|t-1之间的非线性观测函数，ω_t,i是均值为零、协方差为R_t,i的随机测量噪声。

将上式中的非线性观测模型在当前估计值处对其线性化处理：

式中，雅克比矩阵

分别计算t时刻的第i个观测量z_t,i和***状态x_t|t-1的估计值间的距离新息向量ν_t,ij及其协方差S_t,ij如下：

式中，P_t|t-1为估计值的方差矩阵；

按下式计算马氏距离为：

采用下式中的检验门限条件对所有的关联匹配对进行检验，并滤除不满足检验条件的关联匹配对：

式中，为新观测到的路标特征z_i与已存的环境路标特征x_j在t时刻的马氏距离，γ为置信度为1-α时的卡方分布值，其中α的取值预设；

如图1(a)所示，检验门限可看作在观测空间中以为中心的超椭圆，其中和分别表示新观测到的路标特征z_i与已存的环境路标特征x_j的估计值，只有位于该椭圆中的观测才有可能被接受为有效观测。当存在多个新观测到的路标特征同时位于一个已存的环境路标特征的检验门限中，如图1(b)所示，则会产生不确定性数据关联，因此就需要依据数据关联准则来从多个关联假设中选择出最符合实际情况的一个。

最大似然准则(Maximum Likelihood,ML)，通常是用来考虑***所获取到的每个新观测到的路标特征与已存的环境路标特征集合中单个特征相关联的可能性。假定真实观测量均匀分布，且观测噪声服从高斯分布，利用最大似然准则计算t时刻第i个新观测到的路标特征z_i与第j个已存的环境路标特征x_j之间的关联概率值f_ij：

式中h_l表示关联概率最大的第i个新观测到的路标特征z_i与第j个已存的环境路标特征x_j之间的关联关系，n表示新息向量的维数。

对关联概率值f_ij取对数，并整理得：

式中，N_ij表示关联概率值相对应的规范距离。

利用下式将求取最大关联概率的问题等价转化为求取最小规范距离的问题：

虽然ML方法简单易行，但由于仅考虑单个观测和特征间的独立关联，使得在环境不确定度增大时，关联正确率会迅速下降。为增强***的稳定性，联合最大似然准则(JointMaximum Likelihood,JML)考虑了数据关联的整体匹配最大可能性，利用该准则求取每个关联匹配对的关联概率值的总乘积，并将其作为最大的关联假设：

利用下式将求取整体最大关联概率的问题等价转化为求取最小规范距离的问题：

为了直观地描述数据关联问题的解空间，在满足唯一性约束条件下构建了如图2所示的解释树模型，在本实施方式中，图2只绘制了部分的解释树节点以方便描述解释树模型，而在实际算法中解释树节点数目按具体的关联匹配对数目。树的根节点为空节点Φ，每一层对应一个新观测到的路标特征z_i，若z_i与x_j关联匹配，则节点表示一个相应的关联匹配对(z_i,x_j)；若z_i不与任何特征关联，则为空节点。从根节点到低层叶节点的一条路径即表示新观测到的环境路标特征集合z与已存的环境路标特征集合x间可能的关联假设，如图中的路径：Φ→(z₁,x₂)→Φ→(z₃,x₄)。用该解释树模型表示满足检验条件的关联匹配对，其中每个节点视为蚂蚁游走的路径节点，这样就可以利用最大最小蚂蚁***(Max-Min AntSystem,MMAS)来搜索出最佳路径，并作为最优数据关联假设。

步骤4)，令蚂蚁的数目为M，迭代最大次数为N，将每只蚂蚁的禁忌表都设置为空集，将蚂蚁最短路径集合设置为空集，将每只蚂蚁选择的次数设置为0，将迭代次数Ne设置为0，将信息素初始值τ_ij(0)设置为预设的信息素初始最大值τ_max；其它参数设置如表1所示。

步骤5)，对于M只蚂蚁中的每一只蚂蚁：

步骤5.1)，获得该蚂蚁在当前节点下可以游走的所有节点的集合A；

步骤5.2)，在集合A中剔除掉该蚂蚁在禁忌表中的节点，得到集合B；

步骤5.3)，针对集合B中的每一个节点，按照下列公式计算该蚂蚁从当前节点游走到其的概率

式中，i、j、r均为大于等于1小于等于m的自然数，k为大于等于1小于等于M的自然数，B_k为该蚂蚁的禁忌表，β₁为信息启发因子、β₂为期望启发因子，启发式信息η_ij和η_ir值分别为1/N_ij和1/N_ir，N_ij为当前节点与该节点的路径长度，N_ir为当前节点与集合B中除该节点以外其他任一节点的路径长度；τ_ij为当前节点到该节点之间路径上的信息素，τ_ir为当前节点与集合B中除该节点以外其他任一节点之间路径上的信息素；

步骤5.4)，在集合B中选择游走到其概率最高的节点作为该蚂蚁的下一个节点，将此节点加入到该蚂蚁的禁忌表后向其进行游走；

步骤5.5)，根据下式计算该蚂蚁的信息素最小值τ_min：

式中，p_best为步骤5.4)中该蚂蚁从当前节点游走到下一个节点的概率，num为该蚂蚁选择的次数；

步骤5.6)，对该蚂蚁选择的次数num进行加1；

步骤6)，反复执行步骤5)，直至所有蚂蚁都到达最底层叶节点；

步骤7)，计算每只蚂蚁游走的路径总长度，并选出路径总长度最短的蚂蚁，将该蚂蚁的路径和路径总长度作为一个元素添加至蚂蚁最短路径集合中，并将该蚂蚁经过的路径的信息素按照下式进行更新：

τ_ij＝ρ·τ_ij+1/f(s^best)

式中，ρ为信息素挥发因子，f(s^best)表示最短路径总长度的值，当τ_ij的值小于τ_min时将其设置为τ_min，当τ_ij的值大于τ_max时将其设置为τ_max；

步骤8)，对迭代次数Ne进行加1，将每只蚂蚁的禁忌表都设置为空集，将每只蚂蚁选择的次数设置为0，将信息素初始值设置为预设的信息素初始最大值τ_max；

步骤9)，反复执行步骤5)至步骤8)，直到当Ne大于最大迭代次数N；

步骤10)，在蚂蚁最短路径集合中筛选出路径总长度最短的蚂蚁，将其路径作为最佳路径，并将该路径中的节点作为最优数据关联假设进行输出。

为了验证本文所提数据关联算法的有效性，将MMAS-JML与ML、ACO-JML进行了对比试验，并分别得到各算法在环境地图中的实际仿真结果，如图4(b)、图4(c)、图4(d)所示。相应的关联正确率及定位误差随时间步的变化结果如图5和表2所示。

表1MMAS-JML算法中的仿真参数

符号	描述	参数值
			α	检验门限	0.05
ρ	信息素挥发因子	0.1
			β1	信息启发因子	1.0
β2	期望启发因子	5.0
			M	蚂蚁的数目	50
N	迭代最大次数	100
			δ	信息素平滑参数	0
τmax	初始最大信息素	1.0

表2不同数据关联算法性能对比

关联算法	运行时间/s	平均关联正确率/％	总运行时间步数
				ML	81.8304	0.8064	168
ACO-JML	117.5198	0.9089	168
				MMAS-JML	110.8654	0.9117	168

根据图4、图5以及表2的仿真结果，进行如下分析：

(1)从全程路标估计及关联正确率结果来看：图4(b)中真实路标红点与路标估计蓝色十字的距离相比图4(c)和图4(d)较远，偏差较大；由于ML算法仅考虑单个量测与特征相关联的可能性，所以平均关联正确率远低于ACO-JML和MMAS-JML；相比ACO-JML，MMAS-JML克服了ACO算法搜索过程中易陷入局部最优解的不足，因而在关联正确率上有所提升。

(2)从机器人定位误差结果来看：在运行初始阶段，ML算法的定位精度与ACO-JML和MMAS-JML接近，但随着时间步数的增加，其累积误差明显增大，导致定位精度下降，而ACO-JML和MMAS-JML能够较好地抑制机器人累积误差的增长，定位效果较好。

(3)从运行时间来看：由于考虑了数据关联的整体匹配最大可能性，以及避免了搜索过程中停滞和收敛速度慢的缺陷，MMAS-JML算法虽然运行时间较ML算法长，但相比ACO-JML却有所降低。

综合考虑数据关联正确率和实时性两个方面，MMAS-JML算法具有更大的优势。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于最大最小蚂蚁***优化的SLAM数据关联方法，其特征在于，包括以下步骤：

步骤1)，在移动机器人的工作环境中布置若干静态的路标特征点，并设定移动机器人的运动初始位置和速度；

步骤2)，利用移动机器人自带的声呐传感器采集环境路标特征信息，令已存的环境路标特征集合为x＝(x₁,x₂,…,x_n)，新观测到的环境路标特征集合为z＝(z₁,z₂,…,z_m)，建立观测到的环境路标特征集合z与已存的环境路标特征集合x之间的数据关联假设H_m：

H_m＝{h₁,…,h_i,…,h_m}

其中，n为已存路标特征点的个数，m为新观测到的路标特征点的个数；h_i为新观测到的路标特征z_i的关联取值，若新观测到的路标特征z_i在已存的环境路标特征集合x中存在与其一致的路标特征x_j，取值为j，此时(z_i,x_j)为h_i对应的关联匹配对；否则取值为0；i为大于等于1小于等于m的自然数，j为大于等于1小于等于n的自然数；

步骤3)，采用下式中的检验门限条件对所有的关联匹配对进行检验，并滤除不满足检验条件的关联匹配对：

$D_{t,ij}^2<\mathrm{\&gamma;}$

式中，为新观测到的路标特征z_i与已存的环境路标特征x_j在t时刻的马氏距离，γ为置信度为1-α时的卡方分布值，其中α的取值预设；

然后用解释树模型表示满足检验条件的关联匹配对，其中每个节点视为蚂蚁游走的路径节点；

步骤4)，令蚂蚁的数目为M，迭代最大次数为N，将每只蚂蚁的禁忌表都设置为空集，将蚂蚁最短路径集合设置为空集，将每只蚂蚁选择的次数设置为0，将迭代次数Ne设置为0，将信息素初始值τ_ij(0)设置为预设的信息素初始最大值τ_max；

步骤5)，对于M只蚂蚁中的每一只蚂蚁：

步骤5.1)，获得该蚂蚁在当前节点下可以游走的所有节点的集合A；

步骤5.2)，在集合A中剔除掉该蚂蚁在禁忌表中的节点，得到集合B；

步骤5.3)，针对集合B中的每一个节点，按照下列公式计算该蚂蚁从当前节点游走到其的概率

$p_{ij}^k=\left\{\begin{array}{c}\frac{{\mathrm{\&tau;}}_{ij}^{{\mathrm{\&beta;}}_1}\&CenterDot;{\mathrm{\&eta;}}_{ij}^{{\mathrm{\&beta;}}_2}}{\underset{r\&NotElement;B_k}{\&Sigma;}{\mathrm{\&tau;}}_{ir}^{{\mathrm{\&beta;}}_1}\&CenterDot;{\mathrm{\&eta;}}_{ir}^{{\mathrm{\&beta;}}_2}},j\&NotElement;B_k\\ 0,otherwise\end{array}\right.$

式中，i、j、r均为大于等于1小于等于m的自然数，k为大于等于1小于等于M的自然数，B_k为该蚂蚁的禁忌表，β₁为信息启发因子、β₂为期望启发因子，启发式信息η_ij和η_ir值分别为1/N_ij和1/N_ir，N_ij为当前节点与该节点的路径长度，N_ir为当前节点与集合B中除该节点以外其他任一节点的路径长度；τ_ij为当前节点到该节点之间路径上的信息素，τ_ir为当前节点与集合B中除该节点以外其他任一节点之间路径上的信息素；

步骤5.4)，在集合B中选择游走到其概率最高的节点作为该蚂蚁的下一个节点，将此节点加入到该蚂蚁的禁忌表后向其进行游走；

步骤5.5)，根据下式计算该蚂蚁的信息素最小值τ_min：

${\mathrm{\&tau;}}_{min}=\frac{{\mathrm{\&tau;}}_{max}\&CenterDot;(1-\sqrt[num]{p_{best}})}{\sqrt[num]{p_{best}}\&CenterDot;(num/2-1)}$

式中，p_best为步骤5.4)中该蚂蚁从当前节点游走到下一个节点的概率，num为该蚂蚁选择的次数；

步骤5.6)，对该蚂蚁选择的次数num进行加1；

步骤6)，反复执行步骤5)，直至所有蚂蚁都到达最底层叶节点；

步骤7)，计算每只蚂蚁游走的路径总长度，并选出路径总长度最短的蚂蚁，将该蚂蚁的路径和路径总长度作为一个元素添加至蚂蚁最短路径集合中，并将该蚂蚁经过的路径的信息素按照下式进行更新：

τ_ij＝ρ·τ_ij+1/f(s^best)

式中，ρ为信息素挥发因子，f(s^best)表示最短路径总长度的值，当τ_ij的值小于τ_min时将其设置为τ_min，当τ_ij的值大于τ_max时将其设置为τ_max；

步骤8)，对迭代次数Ne进行加1，将每只蚂蚁的禁忌表都设置为空集，将每只蚂蚁选择的次数设置为0，将信息素初始值设置为预设的信息素初始最大值τ_max；

步骤9)，反复执行步骤5)至步骤8)，直到当Ne大于最大迭代次数N；

步骤10)，在蚂蚁最短路径集合中筛选出路径总长度最短的蚂蚁，将其路径作为最佳路径，并将该路径中的节点作为最优数据关联假设进行输出。

2.根据权利要求1所述的基于最大最小蚂蚁***优化的SLAM数据关联方法，其特征在于，步骤3)中确定新观测到的路标特征z_i与已存的环境路标特征x_j之间的马氏距离的具体过程包括：

步骤3.1)，建立移动机器人在t时刻的观测模型：

z_t,i＝h_t,j(x_t|t-1)+ω_t,i

式中，h_t,j()为移动机器人在t时刻的第i个观测量z_t,i与***状态x_t|t-1之间的非线性观测函数，ω_t,i是均值为零、协方差为R_t,i的随机测量噪声；

步骤3.2)，将上式中的非线性观测模型在当前估计值处对其线性化处理：

$h_{t,j}\left(x_{t|t-1}\right)\&cong;h_{t,j}\left({\hat{x}}_{t|t-1}\right)+H_{t,j}(x_t-{\hat{x}}_{t|t-1})$

式中，雅克比矩阵

步骤3.3)，分别计算t时刻的第i个观测量z_t,i和***状态x_t|t-1的估计值间的距离新息向量ν_t,ij及其协方差S_t,ij如下：

$v_{t,ij}=z_{t,i}-h_{t,j}\left({\hat{x}}_{t|t-1}\right)$

$S_{t,ij}=H_{t,j}{P}_{t|t-1}{H}_{t,j}^T+R_{t,i}$

式中，P_t|t-1为估计值的方差矩阵；

步骤3.4)，按下式计算马氏距离为：

$D_{t,ij}^2=v_{t,ij}^T{S}_{t,ij}^{-1}{v}_{t,ij}.$

3.根据权利要求1所述的基于最大最小蚂蚁***优化的SLAM数据关联方法，其特征在于，步骤7)中将蚂蚁的经过的路径的信息素更新后采用下式对该信息素进行平滑处理：

τ_ij＝τ_ij+δ·(τ_max-τ_ij)

式中，0≤δ≤1。