（模式识别与智能系统专业论文）基于扩展卡尔曼滤波器的单目视觉slam研究.pdf

摘要 摘要 移动机器人同时定位与建图( s i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ，s l a m ) 是当前机器人领域的一个热点问题。基于视觉的s l a m 逐渐受到s i a m 领域研究 人员的关注。针对单目视觉s l a m 中的问题，本文主要完成了以下的几点工作： 第一、本文研究单目视觉s l a m 的基本原理，建立一个单目视觉s l a m 框架。 本文采用基于扩展卡尔曼滤波器的s l a m 算法，基于粒子滤波器的延迟陆标初始 化方法，实现了一个能够实时采集和处理数据的单目视觉s l a m 仿真系统。本文 的其他工作依托在该系统上开展。 第二、本文从视觉陆标入手，把最新的计算机视觉中的s l 心特征提取方法应 用到单目视觉s l a m 中。s u r f 具有尺度不变性、旋转不变性，相对于s i f t 而言， 具有更好的可剪裁性，消耗的计算资源更少。但是，s u r f 特征最早提出的时候 用于物体识别，没有考虑到s l a m 的特点。本文根据单目视觉s l a m 的特点，采 用s u r f 特征作为视觉陆标。 第三、本文提出新的适用于单目视觉s l a m 的陆标检测策略。在同样的参数 下，同一种特征陆标检测方法提取到的特征数目是不一样的。因此可以根据先验 的环境信息提出不同的陆标特征检测策略。本文提出了一种基于图像栅格分割的 陆标检测方法，把获取的图像分割为若干个子区域，然后从中心的子区域开始， 成螺旋状往外扩散搜索。这样搜索方法的优点是从图像的中心开始初始化陆标， 解决了陆标检测数量和实时性的矛盾，同时也保证了可以较长时间地观测陆标。 第四、本文提出新的适用于单目视觉s l a m 的陆标管理方法。目前，一般把 陆标被观测的总次数和陆标的可见性作为删除陆标的原则。但是这样的判断规则 只是考虑到陆标在全局地图上的稳定性，没有考虑到在局部区域内的稳定性。有 部分陆标只有在固定的观察角度下才能够被观测到，如果机器人离开了该位置， 此类陆标就不能够被观测到。另外，这样的判断规则不能够处理陆标被遮挡的情 况，经常会发生重复初始化同一个陆标的情况。本文引入永久陆标这种新的陆标 管理机制，保证这些在特定位置上具有良好稳定性的陆标不会被错误地排除掉。 关键词：同时定位与建图扩展卡尔曼滤波器单目视觉视觉陆标s u r f a b s t r a c t 一一_ _ _ - _ _ _ 一 a b s t r a c t m o b i l er o b o ts i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) i s o n eo ft h em o s t p o p u l a rp r o b l e m si nr o b o tr e s e a r c hs o c i e t y v i s i o nb a s e ds l a m b e c o m e sm o r ea n d m o r ei m p o r t a n t i nt h i st h e s i s ，w ef o c u so n t h em o n o c u l a rv i s i o ns l a mp r o b l e m a n d d e t a i l sa r ed e s c r i b e da sf o l l o w i n g s ： ( 1 ) r e s e a r c ho nt h eb a s i cp r i n c i p l eo fm o n o c u l a rv i s i o ns l a m a n dd e v e l o pa m o n o c u l a rv i s i o ns l a mf r a m e w o r k e x t e n d e dk a l m a nf i l t e r ( e k f ) i su s e dt oe s t i - m a t et h es t a t e so ft h ec a m e r aa n dt h el a n d m a r k s a n dp a r t i c l ef i l t e r , ad e l a yl a n d m a r k i n i t i a l i z a t i o nm e t h o d i su s e dt oe s t i m a t et h ei n i t i a lp o s i t i o no f t h el a n d m a r k o u rf r a m e w o r ki sa b l et oc o l l e c ta n dp r o c e s si m a g ed a t ai nr e a l - t i m e ( 2 ) i no i l rm o n o c u l a rv i s i o ns l a mr e s e a r c h ，an e w f e a t u r ee x t r a c t o rs u r f , w h i c h p r o v i d e sb o t hr o b u s tm a t c h i n ga n dh i g hs p e e de x t r a c t i o n ，i su s e dt od e t e c tp o i n tl a n d 。 m a r k sw i t ht h eu s eo fi m a g eo f i n d o o re n v i r o n m e n t a sa v e c t o r - b a s e df e a t u r ed e t e c t o r , s u c ha ss i f t , s u r fi si n v a r i a n tt or o t a t i o na n ds c a l et r a n s f o r m a t i o n ，a n dc a nc u to f f s o m ep a r t so fi t sc h a r a c t e r i s t i ct oa c h i e v eb e t t e rp e r f o r m a n c e h o w e v e r , u s e dt oo b 。 j e c tr e c o g n i t i o ni n i t i a l l y , s u r fd o e sn o tt a k ea d v a n t a g eo fs l a m i n f o r m a t i o ns u c ha s t h ep r e d i c t i n gp o s i t i o no fl a n d m a r kf e a t u r e si nt h ei m a g e w eu s es u r f t od e s c r i b e v i s i o nl a n d m a r k s s u r fp r o v i d e saf a s ta n dr o b u s tf e a t u r ee x t r a c t i n ga n dm a t c h i n g p e r f o r m a n c ea f t e rc o n s i d e r i n gl a n d m a r kp o s i t i o np r e d i c t i o ni ns l a m ( 3 ) i no r d e rt of i n das u i t a b l er e g i o nt oe x t r a c tl a n d m a r k s ，w ep r o p o s eas u b r e g i o n b a s e dl a n d m a r kf e a t u r ed e t e c t i n gs t r a t e g yw h i c hd e t e c t sl a n d m a r kf e a t u r e si nas p i r a l s e q u e n c ea st h ef o l l o ws t e p s ：1 ) d i v i d et h ei m a g ei n t os e v e r a ls u b r e g i o n s 2 ) s e l e c t t h ec e n t r a lr e g i o na st h ec u r r e n tr e g i o n 3 ) d e t e c tl a n d m a r kf e a t u r e si nc u r r e n tr e g m n a n da d dt h e mt ot h eq u e u es t o r i n gi n i t i a l i z i n gl a n d m a r k su n t i l t h eq u e u ei sf u l l 4 ) i fn o f e a t u r ei sd e t e c t e di nc u r r e n tr e g i o no rt h eq u e u ei sn o tf u l l ，s e tc u r r e n tr e g i o nt ot h en e x t o n ei nas p i r a ls e q u e n c e t h i sl a n d m a r kf e a t u r ed e t e c t i n gs t r a t e g ym a k e st h ed u r a t i o n o fo b s e r v i n gj u s ti n i t i a l i z e dl a n d m a r ka sl o n ga sp o s s i b l e m e a n w h i l et h i ss t r a t e g yi s c o m p a t i b l ew i t hd i f f e r e n tf e a t u r ed i s t r i b u t i o ns i t u a t i o n sa n d t r a d e so f ft h en u m b e ro f a b s t r a c t f e a t u r ea n dt h er e a l t i m ep e r f o r m a n c eo ft h es y s t e m ( 4 ) t h ed e l e t i o nc r i t e r i o ni su s u a l l yd e f i n e db yt h er a t i oo ft h en u m b e ro fd e t e c t i o n a n d m a t c h i n ga t t e m p t s h o w e v e r , t h e s em a n a g e m e n t c r i t e r i o n so n l yc o n s i d e rt h er o b u s t o fal a n d m a r ki ng l o b a lm a pb u tn o tt h er o b u s ti nal o c a lr e g i o n a sap r o p e r t yo f v i s u a lf e a t u r el a n d m a r k , m o s to ft h e s el a n d m a r k sc a no n l yb eo b s e r v e di nc e r t a i nv i e w p o i n t s o n c et h ec a m e r al e a v e st h e s ep o i n t s ，t h e s el a n d m a r k sa r eu n o b s e r v a b l es i n c e f e a t u r ed e s c r i p t o rc a nn o tb em a t c h e di nl a r g et r a n s f o r m a t i o n f u r t h e r m o r e ，l a n d m a r k s a r eo f t e no c c l u d e di ns o m ev i e wp o i n t s i fo n l yu s i n gr a t i od e l e t i o nc r i t e r i o n ，p a r to f o c c l u d e dl a n d m a r k sw i l lb ed e l e t e db u tw i l lb er e i i l i t i a l i z e do n c em e ya r eo b s e r v e d a g 咖w ei n t r o d u c eap e r m a n e n tl a n d m a r ks c h e m et oa v o i dt h ed r a w b a c ko fr a t i o c r i t e r i o n k e yw o r d s ：s i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) ，e x t e n d e d k a l m a n f i l t e r ( e k f ) ，m o n o c u l a rv i s i o n ，v i s i o nl a n d m a r k ，s p e e d e du pr o b u s tf e a t u r e ( s u r f ) ， i n 南开大学学位论文版权使用授权书 本人完全了解南开大学关于收集、保存、使用学位论文的规定， 同意如下各项内容：按照学校要求提交学位论文的印刷本和电子版 本；学校有权保存学位论文的印刷本和电子版，并采用影印、缩印、 扫描、数字化或其它手段保存论文；学校有权提供目录检索以及提供 本学位论文全文或者部分的阅览服务；学校有权按有关规定向国家有 关部门或者机构送交论文的复印件和电子版；在不以赢利为目的的前 提下，学校可以适当复制论文的部分或全部内容用于学术活动。 学位论文作者签名： 年月日 经指导教师同意，本学位论文属于保密，在年解密后适用 本授权书。 指导教师签名：学位论文作者签名： 解密时间：年月 日 各密级的最长保密年限及书写格式规定如下： 南开大学学位论文原创性声明 本人郑重声明：所呈交的学位论文，是本人在导师指导下，进行 研究工作所取得的成果。除文中已经注明引用的内容外，本学位论文 的研究成果不包含任何他人创作的、已公开发表或者没有公开发表的 作品的内容。对本论文所涉及的研究工作做出贡献的其他个人和集 体，均已在文中以明确方式标明。本学位论文原创性声明的法律责任 由本人承担。 学位论文作者签名： 年月日 第一章引言 第一章引言 自第一台现代机器人1 9 5 9 年诞生以来，人们对机器人的研究一直在持续地进 行。随着科学技术的不断发展，机器人从人类的想象走进人们的真实生活中，机 器人也使很多的原来人类梦想的事情变成了现实。机器人也从最初的工业领域逐 渐地扩展到星际探索、军事作战和侦察、危险品处理、医疗护理、家庭娱乐等多 个领域。 移动机器人是机器人领域内具有重要意义的一个分支，是把运动能力、感知 能力和推理能力集合于一体的移动智能体。论文 1 】提出，移动机器人在实际的应 用中必须同时回答：“w h e r ea l ni ? ”( “我在哪里? ) ，“w h e r ea l nig o i n g ? ( “要 去哪里? ) 和“h o ws h o u l dig e tt h e r e ? ( “如何到达那里? ) 这三个问题。这三 个基本问题反映到技术上就是移动机器人的定位问题和建图问题。移动机器人的 同时定位与建图( s i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ，s l a m ) 技术解决移动 机器人在部分甚至完全未知环境中的定位，并同时建立环境地图的问题，它比单 独的定位或者建图问题更加困难。移动机器人需要根据带有噪声的传感器观测周 围的环境，建立工作环境地图，同时根据部分已建地图估计自身在环境中的位姿， 定位与建图融为一体，同步进行计算处理，而不是独立的两个阶段。在移动机器 人导航领域，s l a m 已经被机器人研究社区公认为能够使得机器人真正地实现自 主导航或探索任务的关键技术。 1 1 同时定位与建图问题的提出 机器人在未知环境中进行导航或探索，需要同时具备对自身定位的能力和建 立环境地图的能力。在早期的移动机器人研究中，移动机器人的定位技术和环境 建立地图的技术的研究是两个独立的研究课题。移动机器人定位是指确定移动机 器人在工作环境中相对于全局坐标的位置及其自身的姿态。在早期的关于移动机 器人自主导航的研究中，大部分都是针对结构化的环境进行的，此时环境地图作 为先验的知识，以先验地图( p r i o dm a p ) 的形式给出。移动机器人通过传感器获 得环境信息，并且把采集到的环境信息与先验地图信息进行对比，利用两种信息 之间的差异来修正定位误差。因此，移动机器人的路径规划、定位、跟踪和避障 第一章引言 等任务均依赖于先验地图信息。创建环境地图技术则是要求移动机器人在对自身 准确定位的前提下，通过各种传感器感知环境信息，建立环境地图。 但是，随着人类在航天探测、海洋开发、军事侦察、危险品处理等领域的研 究越来越深入，移动机器人的应用领域也在不断地扩展，开始从结构化环境逐步 转向非结构化环境，乃至完全未知环境。人们发现，在未知环境中，移动机器人 是不可能预先知道环境的地图的，移动机器人对自身的定位和对环境地图的建立 需要同时进行。由于事先没有环境地图，因此难以使用地图信息进行定位；另一 方面，要建立环境地图，机器人必须能够准确定位，即定位与地图创建相互依赖、 相互决定，从而形成“鸡生蛋、蛋生鸡 的问题【2 1 。因此，如何借助于定位与建图 二者之间的相互制约，使二者同时在线完成就成为当前移动机器人导航中的热点 问题。 1 2 同时定位与建图的研究现状 在本节中，首先介绍s l a m 的数学定义，然后对常见的s l a m 的滤波器算法、 环境地图的表示方法和数据关联方法进行介绍。 1 2 1s l a m 的数学定义 s l a m 归纳起来是一个“感知一估计一校正 的过程，主要包括下面的三个 迭代步骤： ( 1 ) 移动机器人根据当前位姿估计与地图信息对下一步的位姿与地图信息 进行预测； ( 2 ) 在下一步中通过传感器观测环境中的陆标； ( 3 ) 利用观测信息校正上一步的机器人定位和建图结果。 在s l a m 中，系统的状态空间是包含机器人相对于环境的位姿状态和描述环 境模型的特征陆标的状态。这里，用& 表示第t 时刻的系统状态。由于系统状态不 能够直接观测，移动机器人需要来自内部和外部的传感器信息对系统状态进行估 计。假设机器人收集到的信息构成如下两个集合： z 。= z 1 ，z 2 ，g t ) ( 1 1 ) u = 1 1 1 1 ，u 2 ，u t ) 2二 ( 1 2 ) 第一章引言 其中，z t 表示第时刻的传感器观测值，u t 表示在哆一1 ，t ) 时间段施加的控制输入。 s l a m 的目的就是在给定传感器观测集合z t 和控制输入集合。的情况下，估 计t 时刻状态变量的后验概率分布： p ( x t l z 。，u 。) ( 1 3 ) 对于移动机器人，s l a m 的状态向量包括机器人的位姿和环境陆标位置状态 两部分，如果用s 。表示机器a t 时刻的位姿，m t 表示t 时刻的环境地图，即x t = s t ，m t ) 。那么，上述后验概率分布可以重新表达为： p ( s t ，m t z 。，u 。) ( 1 4 ) 如果假设世界模型是静态的，那么环境地图不随时间变化，地图的时间索 引亡可以省去，上式变为： p ( s t ，m l z 。，u ) ( 1 5 ) 贝叶斯滤波器是概率推理的原型。实际上，每一个成功的s l a m 算法都运用 了贝叶斯规则。贝叶斯规则使得根据观测d 推断状态z 的后验概率可以通过下面的 公式来表示： p ( x l d ) = 豫p ( d l x ) p ( x ) ( 1 6 ) 其中，p ( d l x ) 表示在假设z 下观测到d 的概率，p ( d l x ) 描述在不同的z 下获得传感器 观测的过程。p ( z ) 称为先验概率，叩是归一化参数，确i 屎p ( d l x ) 是合理的概率分 布。p ( z l d ) 称为后验概率，它根据证据d 更新先验概率。 对公式1 5 应用贝叶斯规则，得到： v ( s t ，m l z 。，u 。) = 卯( s f ，i i n ，z 扣1 ，0 ) p ( s ，m l z 扣1 ，u ) ( 1 7 ) 根据马尔可夫假设，观测z 。只是与t 时刻的机器人位姿s 。以及地图n l 有关系， 与t - 1 时刻的观测z t 一1 和控制输a u 。没有关系。因此，p ( s ，i m ，z 扣1 ，a t ) 化简为p ( s t ，i m ) 。 因此，s l a m 的后验概率分布变为： v ( s t ，m l z ，u t ) = n p ( s t ，l m ) p ( s t ，m l z 卜1 ，u 。)( 1 8 ) x 寸p ( s t ，m l z 扣1 ，u 。) 应用全概率公式，以及条件概率定义公式，得到： p ( s t ，m l z ，u 。) = ，p ( s t ，m i s t 一1 ，z 扣1 ，u 。) p ( s “i z 扣1 ，u 。) d s 一1 = f p ( s t l m ，s ，z 扣1 ，u t ) p ( m s 纠，z 扛1 ，u ) p ( s t 一1 1 - 扣1 ，u 。) d s ( 1 9 ) 3 第一章引言 根据马尔可夫假设，s t 只是t 一1 时刻的机器人位姿s 扣1 和控制输k u 。的函数， 从而可得： p ( s t ，m l z ，u 2 ) ：= f p ( ，s t f l s t 一- 1 ，, u t 、) p ( ，m l s t ，- 1 , z 。t 一- 1 l ，, u 。t 、) p ( s t _ p ( s t s t 1u t ) p ( s t1m l z u ) d s t1 1 i z 一1 u ) d s t 一1 ( 1 1 。)2 j f 一，一，一1 ，一 最终s l a m 后验分布表达式为： p ( s t ，m l z 。，u ) = 刀p ( z t l s t ，1 1 1 ) p ( s t i s t _ 1 ，毗) p ( s “，m l z 卜1 ，u 。) 施“ ( 1 1 1 ) ， 其中，p ( s 。i 吼一1 ，u 。) 称为机器人的运动模型，表示机器人在t 一1 时刻执行控制输 入u 。之后，机器人位姿为s 。的概率。p ( z 。i s 。，m ) 称为观测模型，它对物理上的传感 器建立模型，表示在已知地图i n 和机器人位姿s 。的假设下，观测到z 。的概率。 1 2 2 典型的s l a m 算法 s l a m 滤波器算法的研究起源于上世纪九十年代s m i t hs e l f 和c h e e s e m a n 的工 作 3 1 。s m i t h 等人在【4 】给出了s l a m 的基于估计理论的解决方案，他们首次提出了 基于扩展卡尔曼滤波器( e x t e n d e dk a l m a nf i l t e r , e k f ) 的随机建图方法。近几年 来，扩展卡尔曼滤波器方法成为当下最为流行的s l a m 工具。对于离散线性系统， 卡尔曼滤波器是最小二乘( 1 e a s ts q u a r e ) 意义下的最优滤波器【5 1 。 扩展卡尔曼滤波器是卡尔曼滤波器在非线性系统的推广，基于卡尔曼滤波 的s l a m 算法有严格的限制条件：首先，系统模型必须是线性的；其次，运动模型 和观测模型概率分布符合单峰正态分布；此外，地图必须是由可以区分的陆标表 示。基于卡尔曼滤波器的方法具有简单、收敛性可严格证明的优点，但是系统模 型的线性假设在很多情况不成立。基于扩展卡尔曼滤波器的s l a m 算法虽然能够 应用于非线性的运动和观测模型，但是单峰正态分布的假设仍然是必要前提。视 觉的s l a m 中，有很多是基于扩展卡尔曼滤波器方法的【6 ，7 1 。由于e k f 的时间复杂 度是o ( n 3 ) ，目前这类方法的s l a m 算法应用环境规模都比较小。 d e l l a e r t 等提出基于粒子滤波器( p a r t i c l ef i l t e r , p f ) 的s l a m 算法【8 1 。粒子 滤波器是s l a m 另一种常用的实现方式，粒子滤波器是一种序列蒙特卡洛滤波 方法，用一些粒子一采样状态逼近s l a m 的后验概率。在机器人应用领域，粒 子滤波器能够表示任何可用马尔可夫链表示的概率机器人模型，而且具有容易 实现的优点。粒子滤波的主要缺点是存在退化问题。在不断的研究和发展过程 中，粒子滤波算法得到许多改进。这些改进主要围绕提高粒子的多样性和改进 4 第一章引言 重要性分布函数的选择。粒子滤波的一些重要的变种有r b p f ( r a o - b l a c k w e l l i s e d p a r t i c l ef i l t e r ) 1 9 1 、f a s t s l a m 方法f 1 0 1 、d p s l a m 方法【l l 】等。r b p f 的主要思想 是使用粒子滤波器对机器人的位置建模，同时每个粒子都拥有独立的环境地 图表示，很好地解决了粒子滤波器中高维空间的概率分布需要大量的粒子的问 题。f a s t s l a m 算法应用了改进的粒子滤波器估计机器人路径的后验概率，每个粒 子拥有佗个e k f 滤波器用来估计扎个陆标位置。d p s l a m 是一种纯粒子滤波器基 于激光传感器的s l a m 算法，用粒子滤波器维护机器人位姿和地图中的陆标位置 的联合后验概率分布。 1 2 3 地图表示方法 在机器人领域中，环境地图的表示一般分为尺度地图( m e t r i cm a p ) 、拓扑地 图( t o p o l o g ym a p ) 和混合地图三种类型1 1 2 , 1 3 。s l a m 研究中常用尺度地图来表示 环境。 尺度地图，又称米制地图 1 3 】，是由m o r a v e c 于1 9 8 8 年提出的【1 4 1 。栅格地图和 特征地图是最常用的尺度地图形式。栅格地图将环境分割为若干个固定大小的栅 格区域( g r i d ) ，如果栅格区域中有障碍物，那么设定栅格区域的占有值为1 ，否则 为o 。栅格区域的占有值也可以用概率的方式来表示1 1 5 1 6 1 。栅格地图能够满足高 层次的导航任务需求，如路径规划、避碰等。但是，栅格地图对数据的管理较困 难，栅格的分辨率和计算代价有密切的关系。对于大规模环境来说，栅格地图的 存储代价和计算代价都比较大。 基于特征的地图用参数描述的特征的全局位置表示环境，参数特征可以是 点【1 7 】、直线0 8 】或线段 1 9 】等。因为环境由少数参数描述的特征构成，所以环境的表 示非常有效。但是由于环境的表示稀疏，基于特征的地图一般不利于路径规划。 此外，这种方式的地图仅仅适合存在几种观察对象能够被基本几何特征模型表示 的环境。 m a t a r i c 等在1 9 9 1 年提出移动机器人基于拓扑地图的导航方法f 2 0 1 。拓扑地图通 常使用图的结构描述地图信息，使用点来表示环境特定位置，使用边来表示不同 位置之间的路径。这种方式只维护不同特征位置的连接关系，并不维护不同特征 之间的尺度信息，如两个特征位置之间的方向信息和距离信息。拓扑地图对环境 规模的适应性强、表示紧凑、占用的存储空间小，是一种快速的导航和建图算法。 拓扑地图的主要不足是特征位置之间可靠的导航需要依赖于特征位置的识别，尤 5 第一章引言 其是当环境中的特征很相似时，特征位置的识别困难。此外，拓扑地图还存在基 于关键点( k e yp o i n t ) 的导航限制，移动机器人必须进行主动定位才能完成导航 任务。 混合地图综合了拓扑地图和尺度地图的优点。其中，拓扑地图将全局地图划 分为若干个相互连接的区域，避免维护全局参考坐标，而尺度地图具有适合的不 确定性表示，保证局部地图的一致性。混合地图本质上使用拓扑地图的架构，在 特征位置定义或者路径定义中使用尺度地图表示尺度信息。近年来，混合地图的 研究受到广泛的关注。多分辨率、分层地图表示法的出现，更进一步地扩展拓扑 地图的表示方法。 1 2 4 数据关联方法 数据关联( d a t aa s s o c i a t i o n ) 是指在s l a m 算法中将传感器观察信息与某个陆 标或者需要估计的地图区域联系起来的问题。其任务就是判断观察到的陆标是一 个在地图中已经存在的陆标还是一个新发现的陆标。常见的数据关联方法包括： ( 1 ) 最大似然数据关联，也称为最近邻数据关联，计算地图中每一陆标与当 前考虑的单个观测关联的可能性，是最基本最简单的数据关联方法 2 1 】。 ( 2 ) 单一相容性( i n d i v i d u a lc o m p a t i b i l i t y ) 检验方法，通过检验新息( i n n o v a t i o n ) 的卡方分布是否小于某一确定的值来决定观测与特征的关联是否成立，在s l a m 算 法中常被采用1 2 2 1 。 可靠的数据管理对s l a m 是必要的。对于扩展卡尔曼滤波器的s l a m 算法而 言，正确的数据关联是其有效工作的必要前提。有些s l a m 算法对错误的数据关 联具有更强的鲁棒性，如f a s t s l a m 【1 0 】。但是，过多的错误关联会导致建图或者机 器人位姿估计的错误。目前大多数s l a m 算法，不能够纠正错误数据关联造成的 影响。 不关联会导致同一个陆标在地图中出现多次，这一般不影响地图的构建，但 是在使用地图进行定位或导航的时候会出现问题。错误的数据关联会导致地图错 误，因为它将不同位置的陆标认为是同一个陆标。 将传感器观察和地图上的陆标关联起来困难的原因还包括传感器的不精确、 噪声的影响等。如果传感器本身越不精确或者观察的噪声越大，那么错误的数据 关联就越有可能发生。 此外，动态运动物体、稀疏特征数据也涉及到数据关联问题。通常s l a m 算 6 第一章引言 法假设环境是静止的，从而避免由于动态物体引发的数据管理问题。但是在实际 的环境中存在各种各样的动态因素，这种情况下数据关联问题如果不能够很好的 解决，建图质量会下降，滤波器可能会发散。 1 3 本文动因 传感器是机器人用于采集环境信息的功能单元。当前，s l a m 主要使用超声、 激光和视觉传感器等传感设备来观测环境。不同的传感器性能各异，应用的场合 也有较大的差异。一般而言，超声传感器能够直接测量距离信息，并且价格便宜， 但是对环境观测的精度低、范围短，检测速度低。由于超声对反射以及声波入射 角偏差敏感，在观测时会遇到重复检测以及镜面反射的问题，不适用于建图精度 要求高的场合。激光传感器能够提供精确的二维距离和方向信息的观测，是目前 二维环境中s l a m 实现的主流传感器。但是激光传感器体积大，能耗高，价格昂 贵，不适用于体型较小的机器人。同时，激光传感器的感知分辨率低，很难将观 测的特征与己知特征进行数据关联。此外，激光传感器在高度混乱或者拥挤的环 境中难以提取角或直线特征，具有感知漂移的问题f 2 3 1 。 人类所获得的信息有8 0 是来自于视觉【矧。以摄像机为代表的视觉传感器和 人类的视觉系统有十分相似的功能。作为人工视觉系统的一员，视觉传感器能够 获得丰富的信息，如物体的颜色、深度、方向、几何信息等等，是移动机器人了解 周围环境的一个重要传感器类型。摄像机作为一个实时传感器，让机器人领域开 辟了一个新的研究方向一视觉s l a m 。 视觉传感器重量轻，能够适合各种类型的机器人，尤其是小型机器人。视觉 感知到的信息丰富，在动态复杂环境下具有无可比拟的优势。因此，视觉传感器 逐渐在s l a m 领域获得广泛的应用。基于视觉的s l a m 采用的滤波器算法和其他 传感器，如激光传感器和超声传感器没有本质的不同，扩展卡尔曼滤波器和粒子 滤波器仍然是两大类主流的方法。视觉s l a m 和基于激光或超声的s l a m 的主要 区别是在环境特征陆标的提取、匹配、环境地图的表示和管理等方面。视觉传感 器应用到s i ，a m 中一方面可以简化s l a m 领域中的一些问题，如数据关联问题，另 外一方面又会引入一些独特的问题，例如单目视觉中的特征初始化问题。此外，视 觉领域的相关技术，特别是从运动中恢复结构技术( s t r u c t u r ef r o mm o t i o n ，s f m ) 可以应用到视觉s l a m 中。 与多目视觉或者立体视觉相比，单目视觉在方向测量中不确定性较大，而且 7 第一章引言 酗l1 可穿戴视觉s l a m l 2 8 捌，左图：视觉传感器佩戴在颈部的位置，右图：可穿戴视 觉s l a m 使用的视觉传感器 深度( 距离) 恢复上比较困难。但是，单目视觉具有计算负荷低、灵活、价格低等 特点，因此，单目视觉s l a m 被越来越多的研究人员所关注。很多s l a m 算法基于 单目视觉完成 2 5 , 2 62 ”。 单目视觉s l a m 可以应用到可穿戴( w e a r a b l e ) 视觉中1 2 8 2 9 i 。在图11 中可以 看出，视觉传感器佩戴在颈部的位置，通过佩戴者的活动使得视觉传感嚣获取 周围环境的信息。收集到的视觉图像信息通过佩戴者身上携带的笔记本电脑进 彳亍 s l a m 运算，在探索周围环境的同时建立起局部的地图。 受到可穿戴视觉的启发，有的单目视觉s l a m 研究使用视觉传感器作为唯一 的采集环境信息器。这种视觉s l a m 的一个优点是可以不需要移动机器人的运动 学模型，可以直接从传感器数据估计佩戴者或者机器人的运动( e g o - m o t i o a ) 。 综上所述，单目视觉s l a m 在多个应用场合中发挥着重要的作用，是一个值 得深入研究的课题。超声、激光和视觉这几种不同的传感器有着各自的优缺点， 适用于不同场合下的s l a m 研究。本文采用视觉传感器作为感知环境的手段，重 点研究针对室内环境的单目视觉s l a m 。 1 4 本文主要工作和目标 针对单目视觉s l a m 研究中的各个环节中的问题，本文重点研究室内环境下 基于扩展卡尔曼滤波嚣的单目视觉s l a m 。具体包括以下几个方面的工作： ( 1 ) 为了研究单目视觉s l a m 基本原理，本文首先需要建立一个单目视 觉s l a m 框架。本文采用基于扩展卡尔曼滤波器的s l a m 算法，基于粒子滤波器 8 第一章引言 的延迟陆标初始化方法。根据实验的需要，本文还需要实现了一个能够实时进行 数据采集和处理的单目视觉s l a m 仿真系统。本文的其他工作都是依托在该系统 上开展。 ( 2 ) 本文从视觉陆标入手，把最新的计算机视觉中的特征提取方法s u r f 应 用到单目视觉s l a m 中。s u r f 特征是对s i f t 特征的近似替代。s u r f 和s i f t 类似， 也是通过特征向量的方法来描述特征。s u r f 具有尺度不变性、旋转不变性，相对 于s i f t 而言，具有更好的可剪裁性，消耗的计算资源更少。标准模式下的s u r f 使 用6 4 f f 塞向量描述特征，可以根据应用场景添加或消去特征的旋转不变性，或者 把特征向量增加到1 2 8 维以提高匹配的准确度。但是s u r f 特征最早提出的时候用 于物体识别，没有考虑到s i ，a m 的特点，例如基于向量距离的匹配方法没有考虑 到陆标特征的位置是可以预测的。因此，本文需要根据单目视觉s l a m 的特点， 把s u r f 特征提取方法和s l a m 结合起来，使得s u r f 特征可以用作视觉陆标。 ( 3 ) 本文提出新的适用于单目视觉s l a m 的陆标检测策略。在同样的参数下， 同一种特征陆标检测方法提取到的特征数目是不一样的。因此可以根据先验的环 境信息提出不同的陆标特征检测策略。对于陆标特征稀疏的环境，对整个图像进 行检测；对于陆标特征密集的环境，只是取部分图像区域进行检测。采用不同的 策略可以适应不同环境的情况，能够保证实时性。本文提出了一种基于图像栅格 分割的陆标检测方法，把获取的图像分割为若干个子区域，然后从中心的子区域 开始，成螺旋状往外扩散搜索。如果在某个子区域成功地检测到陆标特征并且达 到需要初始化陆标特征的数量，则停止检测。这样搜索方法的优点是从图像的中 心开始初始化陆标，解决了陆标检测数量和实时性的矛盾，保证了可以较长时间 地观测陆标。 ( 4 ) 本文提出新的适用于单目视觉s l a m 的陆标管理方法。当前，一般根据 陆标被观测的总次数和陆标的可见性，即陆标被成功匹配的数目和尝试匹配的数 目的比值，作为删除陆标的原则。但是这样的判断规则只是考虑到在全局地图上 陆标的稳定性，没有考虑到在局部区域内陆标的稳定性。有部分陆标只有在固定 的观察角度下才能够被观测到，如果机器人离开了该位置，此类陆标就不能够被 观测到。另外，这样的判断规则不能够很好地处理遮挡的情况。当陆标被遮挡时， 会被误认为是一个不好的陆标，会被错误地排除掉。当该陆标再次被观测到的时 候，会发生重复初始化同一个陆标的情况。本文引入永久陆标这种新的陆标管理 机制，保证这些在特定位置上具有良好的稳定性的陆标不会被错误地排除掉。 9 第一章引言 1 5 本文组织结构 本文的主要内容和组织结构如下： 第一章介绍了s l a m 的发展历史和研究现状，以及本文选择单目视觉s l a m 作 为研究重点的原因。最后总结了本文的主要工作和目标。 第二章论述如何通过计算机视觉的方法提取图像中的关键点作为视觉陆标， 并且探讨如何利用这些视觉陆标为视觉s l a m 提供信息。 第三章介绍首先介绍扩展卡尔曼滤波器的基本原理，然后介绍如何实现基于 扩展卡尔曼滤波器的单目视觉s l a m 。 第四章介绍对陆标特征地图的管理方法，包括使用什么样的规则来增加陆 标、删除陆标等等。 第五章介绍相关的实验工作，首先介绍实验系统以及实验过程的设计，然后 对获得的实验结果进行分析。 最后在第六章对本文的工作做出总结。 1 0 第二章视觉陆标 第二章视党陆标 陆标是移动机器人同时定位与建图的基础。在下文中介绍如何通过计算机视 觉的方法提取图像中的特征作为视觉陆标，并且探讨如何利用这些视觉陆标为视 觉s l a m 提供信息。 2 1 引言 陆标是移动机器人获取定位信息的一种主要手段。第一个真正成功的定位系 统是通过采用灯塔( b e a c o n ) 陆标来实现的f l 】。灯塔陆标定位方法的原理是：在环 境中的已知位置中设置灯塔，移动机器人通过激光、超声等传感器观测灯塔，通 过灯塔的位置信息来修正机器人的位置信息。但是基于灯塔的定位方式需要事先 在环境中设置陆标，不适合同时定位与建图的应用。 另外一种常见的获取陆标的方式是利用环境中的自然存在物作为陆标。而视 觉陆标就是一种典型的自然陆标。采用视觉传感器获取图像信息，从图形中提取 出稳定的特征作为陆标。相对于其他陆标形式，视觉特征有着多个方面的优点【3 0 1 ： 首先，视觉陆标可以提供三维的信息； 其次，视觉陆标可以对机器人的运动提供准确的估计； 第三，视觉陆标可以被稳定的提取和匹配，可以较好地解决s l a m 中的数据 关联问题。 为了满足移动机器人的定位任务的需要，视觉特征应该具有如下六种特性【3 1 】： ( 1 ) 可重复性( r e p e a t a b i l i t y ) ：给定从不同的角度获取的同一物体的两幅图 像，在两幅图像中可以同时被检测出来的物体上的特征应该尽能的多。可重复性 意味着无论拍摄的条件如何变化，都能够从图像中检测出被识别物体的特征。 ( 2 ) 可区分性( d i s t i n c t i v e n e s si n f o r m a t i v e n e s s ) ：检测到的特征应该具有差异 性，以便不同特征之间的区分和匹配。 ( 3 ) 局部性( 1 0 c a l i t y ) ：特征应该尽量保持局部的特性，这是为了降低特征被 遮挡或一个特征对应多个相似物体的风险；此外，局部性保证了对不同观测条件 下获得的两幅图像之间的几何或者光线变形( p h o t o m e t r i cd e f o r m a t i o n ) 进行简单 的近似。 第二章视觉陆标 ( 4 ) 大量性( q u a n t i t y ) ：被检测出的特征数量应该尽可能的大，以保证即便 是在小的物体上也可以提取出足够的特征。 ( 5 ) 精确性( a c c u r a c y ) ：被检测出的特征在图像位置以及尺度等方面应该精 确定位。 ( 6 ) 高效性( e f f i c i e n c y ) ：特征检测应该尽可能的快速，高效性是实时性应用 的前提。 其中，可重复性是最为重要的特性之一，可以通过不变性或者鲁棒性两