第39卷 第8期 计算机工程 2013年8月 V_0l_39 NO.8 Computer Engineering August 2013 ・人工智能及识别技术・ 文章绩号I 1000—_3428(2013)08—0231—_04 文l畎标lR码I A 中圈分类号I TP24 基于扩展卡尔曼滤波的实时视觉SLAM算法 梁超,王亮,刘红云 (北京工业大学电子信息与控制工程学院,北京100124) 摘要:单目视觉同步定位与地图创建(SLAM)算法的计算复杂度较高,难以满足实时处理的要求。为解决该问题,提出一种SLAM 的优化算法。使用FAST特征点提取环境特征,对于每一个特征点构造BRIEF描述子,以提高算法执行效率,通过引入1-point 随机抽样一致算法对算法的框架进行改进,降低算法的计算复杂度,实现视觉SLAM算法的实时处理。实验结果表明,在相机速 度为30 s的情况下,该算法能满足实时性要求。 关健词:同步定位与地图创建;扩展卡尔曼滤波;FAST角点;BRIEF描述子;随机抽样一致;单目视觉 Real—timeⅥsion SLAM Algorithm Based 0n Extend Kalman Filtering LIANG Chao,WANG Liang,LIU Hong・yun (College ofElectronic Information and Control Engineering,Beijing University ofTechnology,Beijing 100124,China) [Abstract]Monocular Simultaneous Localization and Mapping(SLAM)is computational complexity,and it is hard to reach real—time process.In order to solve this problem,this paper proposes an SLAM optimization algorithm.It uses the Feature from Accelerated Segment Test(FAST)comer to extract the environment featrures,and makes the Binary Robust Independent Elementary Feature(BRIEF)descriptor for eveyr feature points,and improves the execution efifciency of the algorithm.1-point Random Sample Consensus(RANSAC)algorithm is introduced to improve the algorithm framework,to reduce the computational complexity of algorithm and to achieve real-time processing of visual SLAM.Experimental results show that when the camera speed is 30 frames per second,the proposed algorithm can meet the real-time requirements. [Key words]Simultaneous Localization and Mapping(SLAM);Extended Kalman Filtering(EKF);FAST comer;BRIEF descriptor; Random Sample Consensus(RANSAC);monocular vision DOI:1O.3969 ̄.issn.1000—3428.2013.08.050 1概述 SLAM算法由于对于硬件的要求更小,在提出之后很快成 为了研究的热点。在单目SLAM算法中,假定周围的环境 同步定位与地图创建(Simultaneous Localization and 物体都是静止的,相机在每次运动观测的过程中都会计算 Mapping,SLAM)算法主要解决的问题是使机器人可以在未 得到一个从相机光心到特征点的空间向量,通过计算2个 知的环境中自主的探测周围的环境,以实现机器人的自主 向量之间的角度,可得出特征点的视差值,从而得到深度 导航I ~1。 信息。文献[6】基于扩展卡尔曼滤波的同步定位与地图创建 目前,大多数同步定位与地图创建算法都使用距离传 算法,以及文献[7】的并行跟踪与定位(Parallel Tracking and 感器,如激光、雷达、声纳等,去测量周围的环境信息, Mapping,PTAM)算法,是目前处于领先的2种单目同步定 确定机器人的位置以及环境信息。但是这些传感器存在重 位与地图创建算法。 量大、耗能高等缺点,不适合负载能力有限的机器人_jJ,相 PTAM算法使用定位与地图创建相互分离的算法,利 比于距离传感器,视觉传感器则具有价格低廉,及重量轻 用图像中稀疏稳定的特征点,实时计算相机的运动轨迹, 等特性,并且不易受到外界环境的影响,所以,受到了广 同时,异步提取周围环境大量的特征点,构建出稠密的特 泛的关注。最先提出的视觉SLAM算法是基于立体视觉的 征地图。这种算法具有很高的计算精度,而且可以构建出 SLAM算法,其中,最具代表性的是文献[4]所描述的算法, 稠密的环境地图,但是这种算法只限于室内环境和运动区 文献[5]的系统与之相似。而相比于立体视觉,单目视觉 域较小的情况。文献[6]的SLAM算法虽然对于环境具有更 基金项目:国家自然科学基金资助项目(60975065) 作者倚介:梁超(1987--),男,硕士研究生,主研方向:机器视觉,图像处理;王亮,讲师、博士;刘红云,副教授、硕士 收藕日期:20 12-04—09 .|}回日期:20 12—05-2 1 E-mail:cliang827@gmail.tom 232 计算机工程 2013年8月15日 好的适应性,但是具有很高的计算复杂度,所以,很难达 到计算的实时性。 造成这样复杂度高的原因主要有2点:(1)视觉SLAM 算法都是以图像特征点为基础的,而目前高精度的图像特 征点提取和匹配计算量大,难以达到实时性的处理,这是 视觉SLAM算法所面对的主要问题。(2)扩展卡尔曼算法本 身具有很高的计算复杂度,当扩展卡尔曼矩阵维数增大时, 计算的复杂度显著提高。本文针对这一问题,对文献[6]的 单目视觉SLAM算法进行改进,引入基于BRIEF 算子的 图像特征点提取与匹配方法,使用1-point RANSAC 的思 想对于原算法框架进行优化。 2基于扩展卡尔曼滤波的SLAM算法 2.1相机状态系统的描述 . 在单目视觉SLAM算法中,相机的状态矩阵包括了在 此时刻的所有信息,在相机的状态矩阵中,环境特征以三 维坐标的形式表示。本文使用 表示世界坐标系,C表示 相机坐标系,那么整个状态系统可以表示为相机的运动状 态和周围环境的稀疏特征点。使用文献[101的恒定速度的相 机模型,那么在世界坐标系下系统可以表示为: xV Pxx % % X= 1 ,P= 2 尸y1 : : : 其中,Y 表示特征点i的三维空间坐标,以及俯仰角所组 成的向量(Yf=I Xi Zi oi l );P表示系统误差 协方差矩阵。状态向量 中储存的是机器人的位置以及周 边特征点的位置信息,而系统估计量的不确定度存放在相 关矩阵P中。相机的状态向量 由在世界坐标系下的空 间位置矢量, 、方位角矢量q 、速度矢量 以及角速度 矢量O)w组成,总共13个元素,表示为: X =Itw q v O)w] (2) 其中,r =I Y Z I为相机的空间矢量坐标; q =[q。q q q3] 为相机的空间角度的四元数表示;相 机的线速度和角速度分别为 =l Vx I 以及f_Ow= [ COy ]T。 2.2相机的运动模型 由于相机在空间的运动是自由的不受约束的,很难直 接建立精确的相机运动模型,因此本文假定相机在运动的 过程中是角加速度、线加速度恒定的,于是假定在每一步 的过程中,未知的加速度a 和角加速度Of 都是零均值和 高斯分步的, 是相机运动的间隔,则相机的运动可以由 向量表示为: = ㈣ 假定协方差噪声矩阵为对角矩阵,无关联的噪声都为 线性且为角位移分量。可以得到状态更新矩阵为: , + +V )at fv= g q xq( ̄o + )At (4) + ) 日1ww + 其中,g(( + )at)为通过角度旋转矢量(f_O + )× 定义的旋转四元数。 在相机的运动过程中,新的状态估计量矩阵 ( ,“) 必然伴随着状态不确定矩阵 的增长而增长,矩阵 可 以通过雅克比矩阵计算,计算公式如下: 弋 T : (5) d on 其中, 是关于噪声rl的协方差矩阵。 2.3相机的测量模型 测量模型包含2个部分:系统已经观测到的环境特征 和相机的位置。测量的过程发生在图像特征点提取的阶段, 首先将特征点从世界坐标系下转化到相机的坐标系下,特 征向量 表示为: =[ Y z ] 一, ) (6) 图像中的特征点矩阵hi(“,v)的确切位置可以通过相机 的针孔模型得到,计算公式如下: 红 1r ]j Mo一弘u ,l一 (7) v vo一)kv÷ ,l一 其中,厂为相机的焦距和“0、V0是特征点的图像坐标; 、 为像素尺寸。 利用摄像机的畸变模型,可以得到最终的图像特征点 位置,下面所示的方程为本文系统使用的畸变模型: u-/10 (8) (9) 、,lT二凡1, 其中: r: 而 (1O) 2.4扩展卡尔曼滤波的迭代过程 扩展卡尔曼滤波的迭代过程包括预测和矫正2个部分。 在预测阶段,系统通过现有的参数预测下一阶段的状态; 在矫正阶段,之前预测的状态通过测量值被矫正。如果矩 阵z 表示特征点观测值,矩阵hi表示特征点的预测值,则 具体的计算公式如下: 第39卷第8期 梁超,王亮,刘红云:基于扩展卡尔曼滤波的实时视觉SLAM算法 233 Xnew=Xold+w(zf一 ) ew= 1d+WSWT (11) (12) 汉明距离计算特征点的匹配。由于汉明距离的计算过程是 序列的逐位异或,因此计算速度非常快,而且汉明变换的 方法对于光照问题也有较好的鲁棒性。 3.1特征点的提取 为了快速地提取特征点,首先在使用FAST(Feature from Accelerated Segment Test) 刮特征点提取环境特征之后,对于 矩阵’.’表示为卡尔曼增益: Pxx hi P.L__% T 1=:S-1 Pv# M a矿.¨ 一1 M a T Ox 每一个特征点构造BRIEF特征描述子,这样可以最大限度 地节省时间。在实际的提取过程中,使用对于图像进行分 LljJ 其中,S为特征点的不确定度矩阵,表示特征点的误差, 每一个特征点的不确定度表示为: Si: + + dxv dxv dxv dxv b Pxx + Pxx +R )Xy, v, y 其中.R表示噪声钜阵。 3特征点的提取与匹配 目前,大部分的视觉SLAM算法都是基于特征点的, 也就是说主要的环境特征是以路标的形式表示的,而环境 特征则是通过图像特征点的方式提取得到的,所以,图像 特征点提取的好坏直接关系到整个SLAM算法的结果。根 据SLAM算法中的要求,图像特征点应该满足易于提取和 匹配,并且便于数据的融合。 最流行的特征点是尺度不变特征转换(Scale—invariant Feature Transform,SIFT)t1 1]特征算子。但是SIFT特征计算 的复杂度非常的高,所以,相应的提取和匹配的时间也就 很长,不能用于实时I陛的系统中。在文献[12]的系统中是通过 提取Shi—Tomasi[121特征点作为系统的环境特征。Shi—Tomasi 特征通过角点周围的图像块来描述特征点的信息,提取的 特征点具有部分的旋转不变性,但是这种特征点对于光照 十分的敏感,所以,也不是十分适合视觉SLAM算法。 通过对于视觉SLAM算法的分析,发现在实际的应用过 程中,算法对于图像的尺度不变性和旋转不变性要求不高, 因为SLAM过程中的图像的尺寸是固定的,而且机器人运动 过程中出现图像旋转的情况也较少,所以希望找到一种特征 描述子,只对图像的关照变化有很强的鲁榉眭,这样可以很好 地满足系统的稳定『生要求,同时,忽略对于特征点尺度和旋转 不变性的考虑,还可以减少计算的复杂度,提高计算的时间。 通过比对分析,最终使用BRIEF(Binary Robust Inde- pendent Elementary Feature)算子。BRIEF特征点使用二进制 序列来描述特征点,在提取了特征点之后,BRIEF算子取 一个以特征点为中心的临域,对于领域中的每一个点与中 心点进行灰度值比较,如果中心点的灰度值小于此点,则 这一点的值被置为1,否则为0,为了提高特征算子的鲁棒 性,在比较前先对图像块进行高斯滤波,这样对于每一个 特征点就会形成一个二进制序列。在匹配的过程中,使用 块提取的方法:首先随机选取一个图像坐标,以这个坐标 为中心取一个20 ̄30的矩形区域,并判断此区域中有没有 被提取的特征点,如果没有,则进行FAST特征点的提取, 并在提取的特征点中选取一个精度最高的点作为此区域的 特征点,否则重新选取图像坐标,直到达到图像特征点的 个数,或达到最大迭代次数。在实验中这种方法可以降低 计算复杂度,并且避免特征点在一些区域分布过密的情况。 3.2特征点的匹配 在匹配的过程中,对于2幅图像提取到的特征点计算 汉明距离。当2个特征点之间的汉明距离小于设定的阈值 时,认为2个特征点为同一个点。在匹配完毕后,还要做 的是对于误匹配特征点的剔除,因为在一些纹理相似的区 域内,特征点的误匹配是很难避免的,特征点的误匹配会 导致整个系统的崩溃,所以必须要对误匹配的点进行剔除。 通过实验发现,一般误匹配的特征点在2幅图像中的位置 距离都比较远,而相机捕获的视频是连续的,所以,特征 点在下一幅图像中的位置应该不会距离上一时刻位置太 远,考虑到的相机运动的速度,认为特征点在下一时刻应 该出现在上一时刻为中心的一个20x20的矩形区域内,满 足这个要求的认为是正确的匹配,否则视为误匹配而删除。 4扩展卡尔曼算法的优化 本文前面所介绍的扩展卡尔曼算法的计算复杂度是非 常高的,尤其是在卡尔曼算法的更新部分,它的复杂度为 O(N2),其中,Ⅳ表示扩展卡尔曼矩阵的维数,且 表示特 征点的个数。在实验中也发现这部分算法占用了大量的时 间,是导致算法不能实时性的一个重要原因之一,所以, 下面尝试对于此部分算法进行改进,减小计算的复杂度。 1-point随机抽样一致(Random Sample Consensus, RANSAC)算法是目前效果较好的方法之一,此算法的思想 是通过引入RANSAC方法降低扩展卡尔曼算法中矫正过程 中的运算复杂度,主要方法是降低矫正过程中的矫正次数, 即降低计算过程中矩阵的维数,提高运算的速度。1-point RANSAC算法分为2步执行: (1)合理内点的计算 1-point RANSAC算法的输入为相互的特征点 匹配点z,具体实现为:在得到匹配点后,首先根据文 献[141提到的方法计算这些匹配点测量值概率分布函数的 值rl(hA&I 一】), ),只有计算结果大于等于99%的点被 238 计算机工程 2013年8月15日 [2]Belhumeur P,Hespanda J,Kiregeman D.Eigenfaces vs.Fisher- faces:Recognition Using Class Speciifc Linear Projection[J]. IEEE Trans.on Pattern Analysis and Machine Intelligence, 糌 磊 1997,19(7):71 1—720. [3]Chen Lifen,Liao Hongyuan,Kou Mingtat,et a1.A New LDA— based Face Recognition System Which can Solve the Smal l Sample Size Problem[J].Pattern Recognition,2000,33(10): 1713—1726. 投影轴数 图3 3种算法在Yale库上的识别率比较 表2 3种算法在Yale库上的最大识别率比较 [4]Roweis S L Saul L K.Nonlinear Dimensionality Reduction by Locally Linear Embedding[J].Science,2000,290(5500): 2323—2326. [5]He Xiaofei,Yan Shuichen,Hu Yuxiao,et a1.Face Recognition Using Laplacianfaces[J].IEEE Trans.on Pattern Analysis and Machine Intelligence,2005,27(3):328—340. 由图3可得,本文提出的LPP-RA算法在Yale库的识 别性能高于LPP、PCA。由表2可得,本文提出的LPP—RA [6】Yu Weiwei,Teng Xiaolong,Liu Chongqing.Face Recognition Using Discriminant Locality Preserving Projections[J].1inage and Vision Computing,2006,24(3):239—248. [7】Yang Liping,Gong Weiguo,Gu Xiaohua,et a1.Null Space 算法在Yale库的最大识别率高于LPP、PCA。 7结束语 本文提出一种基于排斥图和吸引图的局部保持投影算法。 将排斥图和吸引图应用到局部保持投影算法中,推导排斥和 吸引Laplaceans矩阵,使不同类但是近邻的样本相互排斥, Discriminant Locality Preserving Projections for Face Recognition[J].Neurocomputing,2008,71(1 8):3644—3649. [8]杨利平,龚卫国,辜小花,等.完备鉴别保局投影人脸识别 算法[J].软件学报,2010,21(6):1277—1286. [9]Kokiopoulou E,Saad Enhanced Graph—based Dimen— 使同类但不是近邻的样本相互吸引,定义样本相似性度量相 对应的最近邻分类器,该近邻分类器最大限度地去除样本提 取特征时存在的噪声和特征值变异。实验结果验证了该算法 的有效性。在本文提出的相似度分类器中,其分类效果相对 不明显,如何提高相似度分类器分类效果需要进一步研究。 参考文献 [1]_rurk M,Pentland A.Eigenfaces for Recognition[J].Journal of Cognitive Neuroscience,1991,3(1):71-86. sionality Reduction with Repulsion Laplaceans[J].Pattern Recognition,2010,40(1):2392—2402. [10】Qiu Xipeng,Wu Lide.Face Recognition by Stepwise Non— parametric Margin Maximum Criterion[c]//Proc.of the 1 0th IEEE International Conference on Computer Vision.Beijing, China:[s.n.】,2005. 编辑刘冰 (上接第234页) [7】Klein Q Murray D Parallel Tracking and Mapping for [1 1]Lowe D G Object Recognition from Local Scale・invariant Features[C]fProc.of the 7th International Conference on Computer Vision.Washin【gton D.C.,USA:IEEE Computer Society,1999. Small AR Workspaces[C]//Proc.of International Symposium on Mixed and Augmented Reality.Nara,Japan:[s.n.],2007. [8]Calonder M,Lepetit Strecha C,et a1.BRIEF:Binary Robust Independent Elementary Features[EB/OL].(2010—1 1-21). [12】Shi J,Tomasi C.Good Features to Track[C]//Proc.of IEEE Computer Society Conference on Computer Vision and Pattem http://soc.fudan.edu.cn/vip/pr0jects/gradproj/wiki/BRIEF—Bin ary_Robust_lndependent ElementaryFeatures. Recognition.1S.I.]:IEEE Press,1994. [9]Civera J,Grasa O G Davison A J,et a1.1-point RANSAC for EKF—based Structure from Motion[C]//Proc.of IEEE/RSJ International Conference on Intelligent Robots and Systems. 【13]Rosten E,Drummond T.Machine Learning for High—speed Comer Detection[C]//Proc.of European Conference on Computer Vision.Graz,Austria:[s.n.],2006. [14】Davison A J,MoRon N D,Reid I D,et a1.MonoSLAM:Real— time Single Camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2007,29(6):1052-1067. St.Louis,USA:【s.n.】,2009. [10】Davisou A J.Real—time Simultaneous Localization and Map— ping with a Single Camera[C]//Proc.of the 9th International Conference on Computer Vision.Nice,France:[s.n.],2003. 编辑刘冰