[发明专利]基于粒子群优化算法的SLAM闭环检测方法

权利要求书

1.一种基于粒子群优化算法的SLAM闭环检测方法，包括如下步骤：

(1)利用深度RGB-D相机获取当前帧图片，判断所获取的当前帧图片是否为关键帧图片，若是，则执行步骤(2)，否则，舍弃所获取的当前帧图片；

(2)计算当前关键帧图片的描述子：

(2a)从当前关键帧图片中提取500个尺度不变性FAST；

(2b)利用旋转不变性BRIEF描述子公式，计算当前关键帧图片中每一个尺度不变性FAST特征点的旋转不变性BRIEF描述子；

(2c)按照下式，计算当前关键帧图片的描述子：

其中，l表示当前关键帧图片的描述子，∑表示求和操作，u表示当前关键帧图片中第u个尺度不变性FAST特征点，u＝1,2,...,500，g(u)表示当前关键帧图片中第u个尺度不变性FAST特征点的旋转不变性BRIEF描述子；

(3)判断当前关键帧图片是否是第一帧关键帧图片，若是，则执行步骤(4)，否则，执行步骤(5)；

(4)创建帧图片描述子库：

在计算机内存中创建一个用于存放帧图片的空集合；

(5)扩充帧图片的描述子库：

将前一关键帧图片的描述子添加到帧图片的描述子库中；

(6)判断帧图片描述子库中关键帧图片数是否大于50，若是，则执行步骤(7)，否则，执行步骤(1)；

(7)获得最优帧图片：

(7a)按照下式，计算当前关键帧图片描述子和帧图片描述子库中任意一帧图片描述子的曼哈顿距离：

其中，d(l,r)表示当前关键帧图片描述子l和帧图片描述子库中第j帧图片描述子r间的曼哈顿距离，j的取值小于帧图片描述子库中的帧数，n表示当前关键帧图片描述子和帧图片描述子库中第j帧图片描述子r的维度，n＝1,2,...,256，|·|表示取绝对值操作，l_n表示当前关键帧图片描述子l第n维的值，r_n表示帧图片描述子库中第j帧图片描述子r第n维的值；

(7b)将粒子群中粒子的个数设置为10，每个粒子的位置表示帧图片描述子库中图片第j帧，并随机为每个粒子赋予不同的初始位置和初始速度；

(7c)利用下述适应度公式，计算初始化后当前关键帧描述子与帧图片描述子库中每个粒子的位置所对应描述子的适应度值：

其中，h表示当前关键帧图片描述子l和帧图片描述子库中粒子位置为图片第j帧的描述子r的适应度值；

(7d)将每个粒子的初始位置作为该粒子的个体历史最优位置，将所有粒子中适应度值最高的粒子的个体最优位置作为粒子群的全局最优位置；

(7e)将每个粒子的迭代次数设置为30；

(7f)利用下述粒子速度公式，计算每个粒子的速度：

V_v(k)＝ωV_v(k-1)+cσ₁(w_vb(k-1)-w_v(k-1))+cσ₂(w_g(k-1)-w_v(k-1))

其中，V_v(k)表示第v个粒子第k次迭代的速度，ω表示惯性权重，V_v(k-1)表示第v个粒子第k-1次迭代的速度，c表示学习因子，σ₁表示[0,1]区间上的随机数，w_vb(k-1)表示第v个粒子第k-1次迭代的个体历史最优位置，w_v(k-1)表示第v个粒子第k-1次迭代的位置，σ₂表示[0,1]区间上的随机数，w_g(k-1)表示所有粒子第k-1次迭代时所经历的最优位置；

(7g)利用下述粒子位置公式，计算每个粒子的位置：

w_v(k)＝w_v(k-1)+1*V_v(k)

其中，w_v(k)表示第v个粒子第k次迭代的位置；

(7h)利用与步骤(7c)相同的适应度公式，计算当前关键帧描述子与帧图片描述子库中当前代每个粒子的位置所对应的描述子的适应度值；

(7i)将每个粒子当前次迭代的适应度值与其前一次迭代的个体历史最优位置的适应度值比较，将适应度值大的位置作为每个粒子的当前个体历史最优位置，将所有粒子当前适应度值最高的粒子的个体最优位置作为粒子群的全局最优位置；

(7j)判断当前迭代次数是否等于30，若是，则执行步骤(7k)，否则，将当前迭代次数加1后执行步骤(7f)；

(7k)获得粒子群的全局最优位置处的最优帧图片；

(8)获得暴力匹配对：

利用暴力匹配方法，对当前关键帧图片与最优帧图片进行匹配，得到当前关键帧图片与最优帧图片的暴力匹配对；

(9)判断暴力匹配对是否等于25对，若是，则执行步骤(10)，否则，执行步骤(1)；

(10)当前关键帧图片与最优帧图片匹配成功，形成闭环，将匹配的最优帧图片输出。