[发明专利]一种基于最大最小蚂蚁系统优化的SLAM数据关联方法

摘要

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

主权项

1.一种基于最大最小蚂蚁系统优化的SLAM数据关联方法，其特征在于，包括以下步骤：步骤1)，在移动机器人的工作环境中布置若干静态的路标特征点，并设定移动机器人的运动初始位置和速度；步骤2)，利用移动机器人自带的声呐传感器采集环境路标特征信息，令已存的环境路标特征集合为x＝(x1,x2,…,xn)，新观测到的环境路标特征集合为z＝(z1,z2,…,zm)，建立观测到的环境路标特征集合z与已存的环境路标特征集合x之间的数据关联假设Hm：Hm＝{h1,…,hi,…,hm}其中，n为已存路标特征点的个数，m为新观测到的路标特征点的个数；hi为新观测到的路标特征zi的关联取值，若新观测到的路标特征zi在已存的环境路标特征集合x中存在与其一致的路标特征xj，取值为j，此时(zi,xj)为hi对应的关联匹配对；否则取值为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的自然数，Bk为该蚂蚁的禁忌表，β1为信息启发因子、β2为期望启发因子，启发式信息ηij和ηir值分别为1/Nij和1/Nir，Nij为当前节点与已存的环境路标特征集合x中第j个节点的路径长度，Nir为当前节点与集合B中除该节点以外其他任一节点的路径长度；τij为当前节点到已存的环境路标特征集合x中第j个节点之间路径上的信息素，τir为当前节点与集合B中除该节点以外其他任一节点之间路径上的信息素；步骤5.4)，在集合B中选择游走到其概率最高的节点作为该蚂蚁的下一个节点，将此节点加入到该蚂蚁的禁忌表后向其进行游走；步骤5.5)，根据下式计算该蚂蚁的信息素最小值τmin：式中，pbest为步骤5.4)中该蚂蚁从当前节点游走到下一个节点的概率，num为该蚂蚁选择的次数；步骤5.6)，对该蚂蚁选择的次数num进行加1；步骤6)，反复执行步骤5)，直至所有蚂蚁都到达最底层叶节点；步骤7)，计算每只蚂蚁游走的路径总长度，并选出路径总长度最短的蚂蚁，将该蚂蚁的路径和路径总长度作为一个元素添加至蚂蚁最短路径集合中，并将该蚂蚁经过的路径的信息素按照下式进行更新：τij＝ρ·τij+1/f(sbest)式中，ρ为信息素挥发因子，f(sbest)表示最短路径总长度的值，当τij的值小于τmin时将其设置为τmin，当τij的值大于τmax时将其设置为τmax；步骤8)，对迭代次数Ne进行加1，将每只蚂蚁的禁忌表都设置为空集，将每只蚂蚁选择的次数设置为0，将信息素初始值设置为预设的信息素初始最大值τmax；步骤9)，反复执行步骤5)至步骤8)，直到当Ne大于最大迭代次数N；步骤10)，在蚂蚁最短路径集合中筛选出路径总长度最短的蚂蚁，将其路径作为最佳路径，并将该路径中的节点作为最优数据关联假设进行输出。