石祥滨,耿 凯,刘翠微
1(辽宁大学 信息学院,沈阳 110036)2(沈阳航空航天大学 计算机学院,沈阳 110136)
同时定位与地图构建(simultaneous localization and mapping,SLAM)指机器人处于未知环境下,在运动过程中建立环境的地图,同时估计自身的位置,是实现机器人路径规划和导航等自主任务执行的关键技术,闭环检测是SLAM中关键环节和基础问题,对于消除机器人位姿累积误差,构建全局一致地图和提高整体系统的鲁棒性至关重要[1,2].
最简单实现闭环检测的方法是对所有关键帧做遍历匹配,根据正确的特征匹配点数目决定哪幅图像存在关联,但随着关键帧的增加,计算复杂度过高,对于大规模SLAM系统不实用,另一种方法是随机抽取关键帧进行检测,虽然能在一些系统中使用且保持常数时间的复杂度,但其选择过于盲目,抽取到正确的闭环帧数量较低,总的检测效率并不高.
直接根据特征匹配比较两幅图像的相似性,对光照变化不稳定且特征提取比较费时,主流方法采用视觉词袋模型(bag of visual word,BoVW)解决这一问题,其是在文本检索领域的词袋(bag of word,BoW)[3]模型上发展而来,通过特征聚类将一幅图像表示为差异性的单词,利用单词差别判定不同图像相似性,能有效节省时间.基于此,Cummins 等[4]通过Chow-Liu 树探讨了词袋中不同单词之间的关联性,提出了基于Chow-Liu 树的闭环观测概率模型,Angeli 等[5]通过建立增量式的词典结构进行闭环检测,Labbe′ 等[6,7]采用离散贝叶斯估计不同场景可能出现闭环的概率,不对所有场景进行闭环检测,只对可能发生闭环概率较大的场景进行检测,提高了闭环检测的实时性.文献[4]和文献[5]的研究能在一定程度上解决闭环检测问题,但其将当前图像与所有历史场景进行对比,耗费时间与计算资源过大,效率较低.文献[6,7]使用部分概率大的历史场景判断闭环,提高了实时性,但对于过长距离相似场景会出现误匹配,影响全局地图的构建.
词袋模型广泛应用于各种SLAM系统中,但词袋单词特征的提取不关注单词的空间位置与排列顺序,只在乎单词有无,仅使用词袋进行闭环检测,容易产生感知偏差,造成闭环错误判定.针对以上问题,提出了一种基于显著特征互信息熵的闭环检测算法.
本文通过控制闭环帧数量以及提取显著特征来提高闭环检测的实时性与准确性,结构如图1所示,主要工作如下:
图1 结构流程图Fig.1 Structural flow chart
1)关键帧提取.根据改进的ORB[8]特征采用PNP算法计算出前后帧相对位姿变化,然后计算不同帧的信息熵差异,将满足两者条件的作为关键帧,能够减少数据量,提高实时性.
2)闭环候选帧筛选.提取关键帧的单词特征,根据词袋模型逆序索引将与当前帧相似的关键帧作为闭环候选帧.
3)显著图生成.利用图像帧颜色与深度信息提取显著特征生成显著图.
4)互信息熵计算.将显著图分块,分别计算不同显著图不同块区域的互信息熵,判断图像的相似度,用于几何判定是否产生闭环.当产生闭环时,利用图优化求解器g2o[9]迭代优化全局位姿,建立一致的3D点云地图.
闭环检测用于检测机器人是否经过之前相同的位置,通过查找历史相同场景图像建立整体地图的闭环关系,优化地图与位姿.闭环检测可以显著消除累计误差,但检测错误同样会破坏地图结构,且损害程度远大于未检测出闭环时的影响.闭环需要谨慎选择,以下为本文的闭环选取策略.
目前大多SLAM系统根据位姿的相对变化选取关键帧,选择简单有效,但鲁棒性不强,一些系统在此基础上采用时空一致性进行了改进,以一定时间与一定位移选择关键帧,仍然存在冗余,本文考虑相对位姿变化和帧间信息熵差异,将具有一定位移与帧间信息差别的图像作为关键帧.
运动中的相机位姿可以用一个4×4的矩阵T来表示如式(1):
(1)
其中R为3×3的旋转矩阵,t为1×3的平移向量.
在SLAM前端的局部地图中会产生一个参考关键帧Tref,根据式(2)得到当前帧Tcur与参考关键帧Tref的相对位姿变化Tdel.
(2)
然后通过式(3)-式(4)计算当前帧相对位移矩阵Tdel的旋转余弦距离Norm(R)和平移欧式距离Norm(t):
Norm(R)=cos-1(0.5×(R(0,0)+R(1,1)+R(2,2)-1))
(3)
(4)
利用Norm(R) ,Norm(t)值判断当前图像是否满足阈值条件,对于满足条件的图像根据式(5)计算信息熵E,设当前帧信息熵为Ei,通过式(6)剔除冗余的关键帧.
(5)
(6)
式(5)中pi为某个灰度在该图像中出现的概率,式(6)表示当前帧与前一帧的信息熵差异是否大于当前帧与前m帧差异的均值,ε为均值系数.
文献[10]中通过直接遍历所有帧检测闭环,文献[11]随机抽取一些帧判断是否出现闭环,闭环帧的选择盲目,检测实时性较差,效率较低,相比这两种方法,先在关键帧中选择具有可能出现闭环的图像作为闭环候选帧,然后对闭环候选帧进行检测,可以避免遍历所有历史数据,减少比较次数,节省时间.
本文采用词袋模型中逆序索引获取闭环候选帧.词袋模型将所有特征通过聚类形成树形结构,使类内具有较高的相似度,类间相似度较低.树形结构中根节点为所有特征,第一层为所有特征根据k-means算法聚为K类的结果,然后对于第一层每个节点继续聚为K类,以此类推,直至最后一层的叶子节点,每个叶子节点中包含一个逆序索引结构,存储了当前节点的图像索引号,以及该单词对应的权值.
查找相似图像时,首先提取当前图像特征M,通过查找树形结构将每个特征m∈M与根节点逐层比较,然后将其表示为叶子单词,利用单词的逆序索引查找出包含当前单词节点的图像,将得到的所有图像作为闭环候选帧.
由3.2节得到闭环候选帧后,对产生的候选帧提取显著特征,显著特征是人类视觉机制对图像感兴趣的特征,通过显著特征可以将有限的计算资源用于处理图像中更重要的信息.文献[12-14]中直接利用底层特征根据全局或区域对比度计算图像显著性.文献[15-17]将图像分割为超像素,然后在其基础上,根据颜色、纹理、多尺度特征提取显著区域.文献[18]结合了深度信息,经过超像素分割后利用神经网络训练提取显著特征.文献[12-17]均利用图像的颜色、纹理、多尺度等传统特征,在纹理少或者颜色没有明显对比度时会经常失效,文献[18]引入深度信息以弥补不足,但采用神经网络训练比较耗时,需要GPU加速.本文在加入深度信息后同时考虑实时性,利用局部-全局深度距离对比度计算深度显著性,然后融合颜色显著性得到最终显著图.
首先,将彩色信息转换到Lab颜色空间如式(7)所示,Lab空间包括L、a、b三个通道,L表示亮度空间,a和b表示颜色空间,相比于RGB颜色空间,Lab空间具有更广的色域,能更精确的处理颜色信息.
(7)
在得到Lab空间图像后计算三通道均值Iμ={Lμ,aμ,bμ}如式(8)所示:
(8)
利用当前像素与均值像素欧式距离根据式(9)获取颜色显著值:
(9)
然后,利用式(10)计算深度显著值,式中α和β为权重系数,深度显著特征提取包括三种机制:
1)距离近的物体比远的物体更能引起人的注意,认为显著性强, 即当前像素的深度值dcur.
2)当前深度与邻域对比度差异大,认为显著性强,如式(11)所示,其中R为以当前像素位置为中心,一定距离为半径的圆形区域,di表示当前像素深度值,dj表示圆形区域内其他像素深度值.
3)当前深度与全局深度对比度差异大,认为显著性强,如式(12)所示,考虑实时性,不对图像所有像素进行对比,只利用最大、最小以及平均深度作为全局深度进行对比.
(10)
(11)
dall=|di-dmax|+|di-dmin|+|di-dmean|
(12)
最后,结合颜色与深度显著值并赋予权重,由式(13)得最终显著图,图2为根据彩色图像与深度图像生成的显著图.
(13)
闭环候选帧采用聚类方法得到的单词对空间位置不敏感,当一幅图像聚类为人、车和树三个单词时,词袋模型只知道图像中存在这些单词,不知道它们确切的空间分布,此时两幅相似图像会产生误匹配,影响后续闭环检测效果.
针对这一问题,本文在显著图基础上进行分块处理,将图像均匀分为四部分I={I1,I2,I3,I4},对于每部分分别求互信息熵,得到表示两幅图像相似度的四维向量,根据向量距离判断相似性,进而检测闭环.
图2 处理后生成的显著图Fig.2 Saliency map generated after processing
设闭环候选帧为Fcan,当前帧为Fcur,首先计算两帧中对应块的信息熵E1,E2,E3,E4,由式(14)计算两幅图像每块对应的联合信息熵UnE1(Img1,Img2),UnE2(Img1,Img2),UnE3(Img1,Img2),UnE4(Img1,Img2):
(14)
其中X,Y是随机变量的取值集合.
然后根据式(15)计算两幅图像对应的互信息熵Mut1(Img1,Img2),Mut2(Img1,Img2),Mut3(Img1,Img2),Mut4(Img1,Img2),得到表示两张图像相似度的四维向量Mut(Img1,Img2)={Mut1,Mut2,Mut3,Mut4}.
Mut(Img1,Img2)=E(Img1)+E(Img2)-UnE(Img1,Img2)
(15)
最后通过式(16)判断两张图像相似度,将符合要求的M值进行验证.
(16)
图3是当前关键帧与词袋模型得到的部分闭环候选帧,其中(a)是当前关键帧,(b)、(c)、(d)是获得的闭环候选帧,表1中是根据闭环候选帧显著图计算得到的对应熵值数据.
图3 词袋模型产生的部分闭环候选帧Fig.3 Partial closed-loop candidate frames generated by word bag model
表1 闭环候选帧与当前关键帧相关熵值Table 1 Closed-loop correlation entropy between candidate frames and current key frames
本文利用颜色与深度信息提取显著特征,并通过互信息熵对词袋模型产生的闭环候选帧二次筛选,简化参与闭环优化数据,过程如算法1所示,首先将闭环候选帧KF={f1,f2,…,fn}转换为Lab颜色空间,计算三通道均值,以此提取颜色显著性,然后对于深度图像,分别获取当前深度dcur、当前深度与周围深度对比度darea以及当前深度的全局对比度dall,通过三者提取深度显著性,最后融合彩色显著性与深度显著性得到最终的显著图,通过计算显著图的互信息熵进行闭环验证.
算法 1.Mutual information entropy extraction of significant features
输入:n个闭环候选帧图像KF={f1,f2,…,fn}及其对应深度图像KF_D={fd1,fd2,…,fdn}当前帧F_cur
输出:s个闭环验证帧Ver_F={v1,v2,…,vs}
1.KF_Lab←F_cur;
2.for i=1 to n in KF_Lab do
6.end for
7.Iμ←KF_Lab
11.for i=1 to n in Salient do
12. Divide_four_part(Salienti,F_cur);
13. for j=1 to 4 inSalientido
14. Cal_Entropy(Salienti);
15. Cal_Joint_Entropy(Salienti,F_cur);
16. Cal_Mutual_Entropy(Salienti,F_cur);
17. end for
18. Mutual_Entropy←{part1,part2,part3,part4}
19. if(L2_Norm(Mutual_Entropy)>T)
20. Ver_F.push(Salienti);
21. end if
22.end for
本文采用的视觉传感器为微软的深度相机Kinect,实验计算机配置:CPU为i5处理器,主频2.6GHz,内存4G,无GPU加速,系统为Ubuntu14.04.分别使用标准RGB-D SLAM数据集[19]和实际场景进行实验.
为了检验本文算法在SLAM系统上位姿估计的精确度,从慕尼黑大学公共数据集中选取了五组数据集进行实验,其中fre1_360是RGBD相机旋转360度得到的室内环境数据集,fre1_floor是追踪平面地板得到的室内数据集,两者均不存在闭环,用于检验无闭环条件下SLAM系统的精确度,fre1_desk与fre2_desk是经过多次扫描室内四张桌子所得数据集,其中包含局部闭环,fre1_room轨迹包含了整个室内环境,存在全局闭环.fre1_desk、fre2_desk和fre1_room用于检验具有闭环检测算法的SLAM系统精确度.以上五组数据集慕尼黑大学给出了RGBD相机的真实轨迹,是由高精度设备采集得到,可以通过均方根误差(Root Mean Squared Error,RMSE)表示真实轨迹与估计位姿之间的差别,评判闭环检测的效果.
表2 均方根误差与时间对比Table 2 Comparison of root mean square error and time
表2是本文算法与文献[20](RGBD-SLAM-V1)和文献[11](RGBD-SLAM-V2)算法在五组数据集下的均方根误差(RMSE)和每帧处理时间(TIME)对比.文献[20]是慕尼黑大学完成的RGBD-SLAM系统,该系统不包括闭环检测模块,文献[11]是同一作者在文献[20]基础上进行的改进工作,包括利用GPU加速技术进行加速,加入了基于词袋模型闭环检测模块.由表2知,本文算法在精度和时间上都明显优于文献[20]算法,精度上,文献[20]采用了匹配性能优于本文ORB的SIFT[21]特征描述子,但由于本文具有闭环检测模块,能消除累积误差,所以最终精确度优于文献[20],验证了闭环检测在SLAM系统中的重要性.时间上,文献[20]采用效果更好的SIFT描述子,特征提取非常耗时,实时性较差.相比于文献[11],本文算法整体上精度和时间略优,精度上,文献[11]采用了词袋模型进行闭环检测,本文先利用词袋模型提取闭环候选帧,然后计算闭环候选帧的显著特征互信息熵进行闭环检测,过滤掉差异性较大的候选帧,所以精度上优于文献[11]算法.时间上,文献[11]利用GPU对SIFT特征进行并行加速,相比于文献[20]速度有明显的提升,但仍略差于本文算法,原因是本文采用的ORB特征比SIFT特征提取速度快两个数量级,显著提高了系统实时性.
图4是以上四组数据集在本文算法下的的绝对轨迹误差图,能直观的感受到真实轨迹与估计位姿的误差值.其中,实线表示真实轨迹,重虚线表示估计轨迹,轻虚线表示两者差别.
图4 真实轨迹与估计位姿对比Fig.4 Comparison of real trajectory and estimated posture
为体现本文算法的实用性,在真实场景下进行了建图,如图5所示,实验场景为室内办公室,包括书柜、桌子、沙发等实物.图6为重建后的三维点云图,其中,图6(a)和图6(b)是采用传统闭环检测生成的点云地图,两幅图像均能重建出室内环境的整体结构与内容,但图6(a)上方的沙发与图6(b)上方的书柜出现了重叠模糊现象,而且地图的整体结构不够规整,主要原因是多次检测出不准确的闭环,导致优化后的位姿不够精准,地图产生偏差.图6(c),图6(d)是采用本文算法构建的地图,图中的沙发、书柜无重影与模糊现象,原因是本文算法通过提高闭环检测的准确性,减少了错误闭环约束信息,提高了位姿精度,产生较好的地图构建效果.
图5 办公室真实场景Fig.5 Real office scene
图6 办公室重建点云图Fig.6 Office reconstruction point cloud
本文提出了一种基于显著性特征互信息熵的闭环检测算法,旨在增强同一场景图像的正确匹配率,提高闭环检测的正确率.首先,设计了融合位姿与信息熵变化的关键帧选择模型.然后,根据图像颜色与深度信息给出了新的显著特征提取方法.最后,基于显著图提出了显著特征互信息熵的闭环判定架构.实验数据表明,在无GPU加速情况下,本文算法能保证SLAM系统的准确性与实时性,下一步考虑将生成的三维地图应用到室内机器人的导航避障需求中,将研究如何在三维地图下进行路径规划,实现机器人的自主导航.