（计算机应用技术专业论文）基于全景视觉的单目slam系统.pdf

基于全景视觉的单目s l a m 系统 摘要 智能移动机器人是一种在复杂的环境下工作的具有自规划、自组织、自适 应能力的机器人。机器人智能导航的研究是该领域的一个热点课题。智能导航 的目的就是在没有人干预的情况下使机器人有目的地移动并完成特定任务，进 行特定操作。本文旨在结合国家自然科学基金的需求，研究移动机器人同步定 位与制图( 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 l a m 系统存在的摄像机标定复杂度高以及代价 比较昂贵等缺点，本文提出了一种基于全景视觉的单目s l a m 系统。文章对基于 全景视觉的单目s l a m 算法进行了详细的阐述，给出了包括全景图像展开与校正、 特征提取与匹配、特征深度求取、机器人运动模型、观测模型的建立、状态更 新、地图建立、地图管理各方面的相关算法细节与具体实现，最后针对基于特 征s l a m 系统在实际应用中存在的困难提出了用基于图像索引的导航算法与其配 合使用。以广茂达a s - r 机器人为研究平台，实现各种算法和功能，其结果证明该 算法是有效可行的。 关键字：单目s l a m ，全景视觉，展开矫正，多尺度特征，深度求取，后验位 姿校正，统计直方图，相关度矩阵。 m o n o c u l a rs l a ms y s t e mb a s e do np a n o r a m i cv i s i o n a b s t r a c t i n t e l l i g e n tr o b o t sa r ea b l et ow o r ki nc o m p l e xe n v i r o n m e n t sw i t ht h ec a p a c i t i e s o fs e l f - o r g a n i z i n ga n ds e l f - p l a n n i n g n a v i g a t i o np r o b l e mi sah o ti s s u ec o n c e r n e d i nr e s e a r c h i n go ns u c hk i n do fr o b o t s i t sa i mi st om o v ep u r p o s e l ya n dd ot h ejo b w i t h o u ta i d s w i t hs u p p o r t so ft h ep r o j e c to fn a t i o n a ln a t u r a ls c i e n c ef o u n d a t i o n o fc h i n a ，t h i sp a p e ra i m st od e v e l o pan e wa l g o r i t h mf o rn a v i g a t i n gb a s e do nt h 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 ) a l g o r i t h mt om a k et h er o b o t s t o t a l l ya u t o n o m o u si nt h eu n k n o w nb u ts t r u c t u r e de n v i r o n m e n t s t h en a v i g a t i o n t e c h n i q u e s a r ef i r s t l yr e v i e w e di n t h i sp a p e ra n dt h e nt h es l a mp r o b l e mi s i n t r o d u c e d ，b a s e do nt h ea n a l y s i so ft h el o c a l i z a t i o np r o b l e ma n dt h em a pb u i l d i n g p r o b l e mw h i c ha r et w ok e yp o i n t si nt h en a v i g a t i o nt e c h n i q u e s ，i n c l u d i n gi t s s t r u c t u r e ，c h a r a c t e r i s t i c s ，a n dc a t e g o r i e sa n ds oo n a st h eb i c a m e r ab a s e ds l a m h a ss h o r t c o m i n g sa sh i g hc a l i b r a t i o nc o m p l e x i t ya n dh i g hc o s t ，t h i sa r t i c l ep r o p o s e s p a n o r a m i cv i s i o nb a s e dm o n o c u l a rs l a ms y s t e m t h i sa r t i c l ei n t r o d u c e st h e a r i t h m e t i ci nd e t a i li n c l u d i n gp a n o r a m i ci m a g eu n w r a p p i n ga n dc o r r e c t i n g ，f e a t u r e e x t r a c t i n ga n dm a t c h i n g ，f e a t u r ed e p t hc a l c u l a t i n g ，b u i l d i n go fr o b o t s m o t i o n m o d e la n do b s e r v a t i o nm o d e l ，s t a t eu p d a t i n g ，m a pc o n s t r u c t i n ga n dm a n a g i n g a t l a s ti no r d e rt os o l v et h ed i f f i c u l t i e so ft h i sa r i t h m e t i ci np r a c t i c a la p p l i c a t i o n s ，t h i s p a p e rp r o p o s e su s i n gn a v i g a t i o na r i t h m e t i cb a s e do ni m a g ei n d e x i n gt ow o r kw i t ht h i s a r i t h m e t i c t h ee x p e r i m e n tr e s u l t sb a s e do nt h e “g r a n d a r ”r o b o tv e r i f yt h e v a l i d i t yo ft h ea l g o r i t h m k e y w o r d s ：m o n o c u l a rs l a m 。p a n o r a m i cv i s i o n ，u n w r a pa n dr e c t i f i c a t i o n ， m u l t i s c a l ef e a t u r e s ，d e p t hc a l c u l a t i o n ，p o s t p o s i t i o nc a l i b r a t io n ， s t a t i s t i c a lh i s t o g r a m ，c o r r e l a t i o nm a t r i x 插图清单 图2 1 基于特征的s l a m 算法框架1 1 图2 2 预测矫正算法1 4 图2 3 线性卡尔曼滤波算法结构1 4 图2 4 扩展卡尔曼滤波算法方程与结构1 6 图3 1 摄像机坐标系2 1 图3 2 包含标定板的全景图像2 1 图3 3r 和臼对应关系2 3 图3 4 全景图像及其展开结果2 4 图3 5k 取不同值时的匹配成功率和正确率对比2 5 图3 6 特征匹配结果2 6 图3 7 特征提取方法对比2 6 图4 1 摄像机牮标系2 9 图4 2 深度预测与观察3 0 图4 3 深度收敛过程3 1 图4 4 运动模型构建3 2 图4 5 机器人后验位姿与罗盘数据的差异3 4 图4 6 单目s l a m 系统算法流程3 5 图5 1 不同特征组合实验效果对比3 8 图5 2 相关度矩阵3 9 图5 3 阶数选择4 0 图5 4 相关度矩阵分解4 0 图5 5 弱优势抑制前后对比4 l 图5 6 方向相关度4 2 图5 7 算法流程4 2 图5 8 实际位姿和预测位姿的对比4 3 图5 9 监测机器人的闭环路径运动4 4 图5 1o 闭环轨迹估计4 4 表格清单 表3 1 不同阶数产生的残差比较2 2 表3 2n 为6 时对应系数一2 2 表3 3s i f t 和多尺度h a r r is 特征处理时间比较2 6 表4 1 本文算法获得特征位置3 1 2 独创性声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的研究成果。据我所 知，除了文中特别加以标注和致谢的地方外，论文中不包含其他人已经发表或撰写过的研究成果， 也不包含为获得 金月曼王些太堂 或其他教育机构的学位或证书而使用过的材料。与我一同 工作的同志对本研究所做的任何贡献均已在论文中作了明确的说明并表示谢意。 学位论文作者签名：签字日期：年月日 学位论文版权使用授权书 本学位论文作者完全了解金月巴王些盔堂有关保留、使用学位论文的规定，有权保留并向国 家有关部门或机构送交论文的复印件和磁盘，允许论文被查阅和借阅。本人授权盒目墨王些态堂可 以将学位论文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫描等复制手 段保存、汇编学位论文。 ( 保密的学位论文在解密后适用本授权书) 学位论文作者签名 签字日期：年 学位论文作者毕业后去向： 工作单位： 通讯地址： 导师签名： 签字日期：年月日 电话： 邮编： 致谢 值此论文完成之际，谨向我的导师方帅副教授表示最衷心的感谢和崇高的 敬意! 在这两年多的学习和研究工作中，方老师给予我悉心的指导和亲切的关 怀，激励我在学习、工作和生活中不断进取，使我顺利完成了学业。从论文的 选题、撰写、修改到定稿，无不倾注着方老师的心血。方老师广博的学识、开 阔的学术思维、严谨的治学精神和踏实诚恳的工作作风，为我树立了榜样，时 刻激励着我，并深深地影响了我! 同时，非常诚挚地感谢王浩教授! 王老师敏锐的学术洞察力、科学务实的 工作作风和孜孜不倦的工作热情，对我的研究工作给予了有力地指导。 感谢合肥工业大学人工智能与数据挖掘研究室的姚宏亮、方宝富、李俊照、 于磊、杨静老师对我的关心和帮助；感谢胡大伟、李龙、鲍敦桥、李毅、胡俊 坤、何海燕、朱京红同学，大家一起研讨问题，集思广益，对我的论文有很大 启发；感谢王勇、李庆菲、孙洪伟、施国强、尚丽、方辉、占吉青、朱志超等 师弟师妹以及研究生2 0 0 6 级13 班同学的关心和支持。 还要感谢计算机与信息学院的曹航老师、徐静老师、王新生老师在日常事 务中提供的帮助。 最后，由衷地感谢我的父母、妹妹和所有家人。他们在学习、生活中对我 坚持不懈的鼓励和无微不至的关怀，是我不断前进的最大动力，在此深表谢意! 作者：高明 2 0 0 9 年4 月 第一章绪论 自二十世纪六十年代末期，斯坦福研究院( s r i ) 的n i l s s o n 等人研制出了第 一台名为s h a k e y i 】的自主移动机器人以来，移动机器人已不再仅仅局限于工业 制造领域，而是在军事、民用、科研等诸多领域都得到了广泛的应用；同时机 器人的工作环境也是具有了多样化，从纯粹的室内环境发展到了航空、地下、 水下、地面等诸多场所。这些机器人可以是便利于残疾人生活起居的智能轮椅， 或是传输物品的传递员，同时可以是战斗中的侦查兵，未知领域如金字塔和外 新球的探索先锋等。移动机器人应用领域的日益增广不断给移动机器人定位和 导航的研究提出新的课题，促使对移动机器人定位与导航的研究不断深入和发 展。 1 1 课题研究的目的与意义 移动机器人的自主导航( a u t o n o m o u sn a v i g a t i o n ) 是指机器人依靠自身携带 的传感器在特定环境中，按时间最优、路径最短或能耗最低等准则实现从起始 位置到目标位置的无碰撞运动。顾名思义，自主导航需要解决三个基本问题， 即：我在哪里? 我要去哪里? 如何去? 其中第一个是定位问题，第二个是定位 和目标识别问题，最后一个是路径规划问题。导航的环境可分为未知环境，已 知环境和部分已知环境，本文研究的是未知环境下的移动机器人自主导航。 在众多的基于传感器信息的机器人导航方法中，基于视觉的方法被广泛的 应用。但是目前人们研究的重点是基于双目乃至多目智能机器人导航系统，基 于双目乃至多目智能机器人导航系统的优点是机器人可以实时获取环境特征的 深度，但是由于多目摄像机标定比较困难，导致多目智能机器人导航系统不具 备很好的稳定性。所以本文研究的目的是实现基于单目视觉的智能机器人导航 系统，实现在较为复杂环境中的导航任务。 1 2 课题研究的内容 本课题在国家自然科学基金( 6 0 7 0 5 0 15 ) 的资助下，在未知环境下研究基于 全景展开图像的单目s l a m 系统算法的实现。研究的主要内容包括以下几方面： ( 1 ) 全景图像无畸变的展开 为了进行全景摄像机的标定和更好的利用全景图像的信息，本文首先将其 无畸变的展开，这样做的目的是便于提取全景图像中的直线、角点以及其它特 征。 ( 2 ) 特征深度的求取 本文的主要研究内容之一是通过帧间迭代的方法来获取特征的深度，特征 深度的获取是s l a m 系统的关键部分，但基于单目的s l a m 系统是无法利用单帧图 像来获取特征的深度的，这就需要通过帧间迭代的方法来获取其深度，这种方 法虽然有一定的延时，但不影响s l a m 系统的实现和运行。 ( 3 ) 机器人后验位姿的获取 由于机器人在运动过程中存在各种运动误差，所以从罗盘上获得的读数有 时并不能反应机器人的真实位置信息，所以对机器人进行后验位姿校正是非常 必要的。本文通过深度已知的特征作为观察对象，运用e k f 算法对机器人进行后 验位姿校正。 ( 4 ) 基于特征的s l a m 系统与图像索引定位方法的结合使用 为了提高基于特征的s l a m 系统的效率，本文提出将基于特征的s l a m 系统 与图像索引定位方法混合使用从而帮助基于特征的s l a m 系统更好的定位。 1 3 论文组织结构 本文共分为六章： 第一章为绪论，主要介绍了本论文所作研究的目的与意义以及研究的主要 内容和要处理的关键问题，并对论文的组织结构进行概括说明。 第二章主要对移动机器人导航技术进行概述，包括移动机器人的导航方式 以及机器人导航中的两个主要问题：机器人定位与制图，并追踪概述了相关技 术的国内外发展现状与发展趋势，同时对移动机器人同步定位与制图( s l a m ) 算 法，特别是基于特征的s l a m 算法进行了详细的剖析，包括算法的属性、分类， 构架等。 第三章介绍了全景图像的无畸变展开以及特征提取与匹配方法，该章节是 本文的重点之一，也是基于展开全景图像的单目s l a m 算法的重要组成部分。 第四章详述了基于展开全景图像的单目s l a m 系统的特征深度求取、机器人 后验位姿校正、特征初始化以及地图管理等算法，并给出了基于展开全景图像 的单目s l a m 算法的算法流程。 在第五章中，本文针对基于特征的单目s l a m 算法在导航中存在的问题提出 基于统计直方图的机器人索引导航算法与基于特征s l a m 算法的结合方法，二者 的结合是本文的创新点之一，结合的主要目的是利用索引导航提高基于特征 s l a m 算法运行效率以及检测机器人是否做闭环路径运动从而帮助基于特征的单 目s l a m 系统更好的定位。 第六章为全文的总结以及对今后工作的展望。 2 第二章移动机器人导航技术 2 1 机器人导航概述 智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障 碍物的环境中向目标自主的运动，从而完成一定作业功能的机器人系统。由于 其意义重大，所以已经成为移动机器人研究领域的热点课题。智能导航要解决 三方面的问题【2 1 ( 1 ) 选择适当的传感器系统对移动机器人所处的环境进行感知； ( 2 ) 通过一定的检测手段获取移动机器人在环境中的位置、方向以及其他 必须的信息； ( 3 ) 用一定的算法对所获取的信息进行处理并建立环境的模型，寻求一条 最优或近似最优的无碰撞路径，实现移动机器人安全移动的路径规划。 当移动机器人在未知或不确定环境下运行时，必须通过传感器来采集环境 信息，并通过一定算法把传感器收集到的数据进行分析、融合，用来建立外部 环境的模型，正确、全面的反映出外部环境的特征，为导航决策提供正确的依 据。 2 2 移动机器人导航方式 在机器人导航技术中首先要解决的问题是采用什么样的导航方式，即采用 什么样的传感器系统对外界环境进行感知，并指导机器人的后续动作。常见的 机器人导航方式有【2 】：电磁导航、光反射导航、环境地图模型匹配导航、路标 导航、视觉导航、基于非视觉传感器数据的导航以及g p s 导航等。 电磁导航也称地下埋线导航，这种导航方式是2 0 世纪5 0 年代美国开发的， 到2 0 世纪7 0 年代这种导航方式迅速发展并开始运用于柔性生产。其原理是在 路径上连续的埋设多条引导电缆，分别流过不同频率的电流，通过感应线圈对 电流的检测来获得感知信息。该技术简单实用，但是成本高、改造和维护困难。 光反射导航的原理是在路径上连续的铺设光反射条，是一种简单、价格便 宜的导航系统。同电磁导航一样，该技术也相当的成熟，目前国内制造行业使 用的移动机器人大多是基于这两种导航方式的，但是它们应用范围比较窄，不 适合在动态、变换工作环境下的移动机器人导航。 环境地图模型匹配导航是机器人通过自身的各种传感器探测周围环境，利 用感知到的局部环境信息进行局部的地图构造，并与其内部事先存储的完整地 图进行匹配。通过匹配，机器人可确定自身的位置，并根据预先规划的一条全 局路线，采用路径跟踪和相关避障技术来实现导航。 路标导航是指：路标在环境中的坐标、形状等特征己知的前提下，机器人 通过对路标的探测来确定自身的位置，同时将全局路线分解成为路标与路标间 的片段，逐段行进完成导航。这种导航方式又可分为人工路标导航和自然路标 3 导航。虽然人工路标导航比较容易实现，但它人为地改变了机器人工作的环境； 自然路标导航不改变工作环境，但机器人需要通过对工作环境中的自然特征进 行识别以完成导航。路标探测的稳定性和鲁棒性是采用该方法时需要研究的主 要问题。 视觉导航方式具有信号探测范围宽、获取信息完整等优点，是移动机器人 导航的一个主要的发展方向。在视觉导航系统中，目前国内外应用最多的是采 用在移动机器人上安装车载摄像机的基于局部视觉的导航方式，这种导航方式 所有的计算设备和传感器都装载在机器人车体上，利用装配的摄像机拍摄周围 环境的局部图像，然后通过图像处理技术，如特征识别、距离估计等，进行机 器人定位及规划下一步的动作。以后机器人所拍摄的图像经变换后与之相对照， 从而得知机器人当前位置。视觉导航中的图象处理计算量大，实时性差，这始 终是一个瓶颈问题。为了提高导航系统的实时性和导航精度，仍需要研究更加 合理的图象处理方法。 另外，还有基于非视觉传感器数据的机器人导航方式。一般机器人都安装 了一些非视觉传感器，如超声传感器、红外传感器、接触传感器等。利用这些 传感器等。以实现机器人导航。比如，当机器人在光线很暗的环境中的时候， 视觉导航方式将会失效。在这种情况下，可以利用其他传感器，比方说超声波 传感器来进行导航。因为超声波传感器的使用不受光线的影响，而且由于超声 波传感器构成系统具有结构简单、体积小、性价比高等众多优点，使之在机器 人特别是自主移动机器人领域中广泛的运用。 g p s 全球定位系统是以距离作为基本的观测量，通过对四颗g p s 卫星同时 进行伪距离测量，计算出用户( 接收机) 的位置。通过这种方式，机器人可以通 过安装卫星信号接收装置来实现自身定位。 除了以上几种常见的导航方式外，还有气味导航、声音导航等。但是在实 际应用中，往往采用几种不同导航方式的组合来达到最佳的导航效果。比如， 有研究人员利用超声波传感器和视觉传感器的组合进行导航【3 】：将超声数据与 图像数据结合，通过事先训练好的神经网络预侧障碍物的可能位置，从而使得 机器人能够在动态非结构化环境中实现自主导航。 2 3 移动机器人的定位问题 定位是确定移动机器人在二维( 三维) 工作环境中相对于全局坐标的位置 及其本身的姿态，它是移动机器人导航最基本的环节之一 4 , 5 j 。定位方法根据 机器人工作环境复杂性、配备传感器的种类和数量等不同而有多种不同方法。 移动机器人自主定位手段主要有两类：相对定位和绝对定位。相对定位手段主 要有以下几种： ( 1 ) 使用里程计。在这种方法中，通过使用编码器来测量车轮的转速，从 4 而可以计算出机器人的相对位置，实现相对定位。使用里程的优势在于：它是 完全自主的，并且通过结合其他方法很容易预测下一时刻机器人的位置，而劣 势在于：定位误差无限增大，必须周期性的对误差进行削减。 ( 2 ) 惯性定位。通过测量机器人相对于初始位置的距离和方向来确定机器 人的当前位置。该方法通常采用回转仪或加速计等仪器来测量车轮转速或加速 率，然后通过一次或二次积分推导出机器人的位置。由于其要对数据进行积 分处理，很容易导致延时，或者将一些小的常量误差放大；此外该方法设备造 价较高。 ( 3 ) 航位推算。这是一种经典的相对位姿估计方法上的转数表来推算出机 器人的行使距离和方向一般使用安装在车轮航位推算的优点是方法简单，价格 低廉；但其本质上容易受标定误差，车轮与地面的不良接触，颠簸等因素影响， 因此误差较大。 由上述可知，相对定位技术的优点在于具有良好的短期精度、低廉的价格 以及较高的采样速率，但是由于相对定位技术的基本思路都是基于测量值的积 累，因此无法避免时间漂移问题。随着路径的增长，任何小的误差经过积累都 会无限的增加。因此相对定位不适合长距离和长时间的准确定位，除非采用外 部手段对误差进行约束。 绝对定位手段主要有以下几种： ( 1 ) 基于活动信标( 信号灯) 。信号灯通过发射光或者声波而被机器人感知。 通过测量这些被感知( 3 个或更多) 的信号灯的作用范围，来决定机器人的绝对 位置。应该注意，这些信号灯在环境中的位置必须事先己知的。 ( 2 ) 基于人工路标识别。将路标放置在环境中的已知位置上。保证任意时 刻机器人能够发现若干路标。通过测量机器人与这些路标之间的相对距离来定 位。使用该方法的好处在于定位误差是有界的。但是，与信号灯不同的是： 人工路标一般不能看成点状，而是看作一些特征的集合，这样机器人每次感知 的只是路标的一部分，可能会使结果产生误差。 ( 3 ) 基于自然路标识别。这里的自然路标是指环境中那些固有的、明显的、 容易识别的物体。这种定位方法的可靠性和基于人工路标识别的方法基本一 样。但是要求我们必须对该环境事先了解，否则识别过程将会有难度在实际 的机器人导航系统中，常常采用相对定位与绝对定位相结合的定位方式来对机 器人进行定位。 2 4 移动机器人的制图问题 2 4 1 地图的类型 地图构建也是移动机器人自主导航中的一个很重要的内容。机器人利用对 环境的感知信息对现实世界进行建模，自动的构建一个地图。典型的地图表示 5 方法有尺度地图以及拓扑地图6 ，7 1 。尺度地图中又分为栅格尺度地图【8 ，9 】以及几 何尺度地图【1 0 l1 1 。栅格地图在表示环境信息时比较形象，但是受限于环境的规 模。几何地图可以简洁的表示环境，但是要求环境不能过于复杂；拓扑地图不 需要尺度信息，只依赖环境中特定的地点，但是地图构建过程比较复杂。 栅格地图最开始由e 1 f e s 和m o r a v e c 提出1 1 2 , 1 3 】。对于2 d 的栅格地图，环境 被分割为均匀的单元栅格，每个栅格赋一个0 - 1 的值来表示此处的状态：1 表 示被占据；而o 表示的是空闲。对于3 d 环境，每个单元格的值代表这个栅格的 高度信息，从而来表示整个环境地图。 几何尺度地图由一组环境路标特征组成，每个路标特征用一个几何原型来 近似。这种地图只局限于表示可以参数化的环境路标特征或可建模的对象如： 点、线、面。这种构建方法大都是基于对环境信息的相对观测，然后用这些观 测到的环境特征表示环境地图。由于以几何位置关系来表示环境地图，所以为 了保证地图的一致性，要求各观测信息的位置是相对精确的。对于结构化的环 境，用一些几何模型来表示环境空间是可行的。比如，可以用线段来拟合室内 的墙面、用点来拟合墙角等：对于室外环境，可以用点特征表示道路两侧的树 木位置，用曲面来拟合路面地形等。 在利用拓扑地图表示环境时，它没有一个明显的尺度概念，而是选用一些 特定的地点来描述环境空间信息。拓扑地图通常表示为一个图表，图中的节点 表示一个特定的地点，连接节点的弧线表示特定地点之间的路径信息。拓扑地 图对于结构化的环境是一个很有效的表示方法，但是在非结构化的环境中，地 点的识别变得非常的复杂，这种情况下仅以拓扑信息进行机器人定位是不充分 的。 另外，为了更好地表示环境模型，还可以用混合拓扑和尺度地图的表示方 法来表示环境【1 4 , 1 5 】。通过加入尺度信息来补偿拓扑信息使得地图表示方法具有 拓扑地图的高效性以及尺度地图的一致性和精确性。 2 4 2 机器人制图存在的问题 无论要建立哪种类型的环境地图，机器人必须要通过传感器来了解环境。 在这个了解过程中传感器固有的或些人为的因素会导致测量误差即测量噪 音。机器人制图过程中面临的一个主要挑战就来自于测量噪音。如果测量数据 在统计意义上独立，通过多次测量可以削减、甚至消除噪音的影响。然而在实 际制图过程中，这些数据却是相互联系的。可以设法将测量误差控制在一定范 围内，但是它们不断积累时，就可能会影响到下一时刻机器人对传感数据的解 释。整个制图过程中，机器人都要受到环境中这些系统的，互相联系的误差的 影响。如何调整这些误差是正确制图的关键。但是不管是从理论的还是实践的 角度来看，现存的算法都是十分复杂且需要进一步完善的。 第二个体现制图复杂性的方面是环境中要描述的实体的维度，即：要用多 6 少元素对其进行描述。比如，要较为详细的对一个二维的建筑平面进行制图， 一般需要上千的元素。而对于三维而言，则会高达上百万甚至更多。从统计角 度看，每一个元素都是机器人要进行估计的对象。这样显然增加了制图问题的 复杂程度。 第三方面难点在于制图本身是个协调、匹配的过程，也就是我们常说的数 据关联问题。即：如何判断在不同时刻得到的传感器数据中，哪些表示的是环 境中同一元素。只有找到这些相关数据，对其进行融合处理，才会得到对该物 体的准确认识，从而获得更为精确的制图。 第四，目前的算法主要集中在对静态的、结构化的、中小环境的研究上， 而如何在动态的、非结构的、大型环境中进行机器人制图，研究较浅。 最后一个问题在于，机器人制图是与其运动路径有关的。即机器人的定位 与制图是密切联系的。如果机器人对自身的位置有准确的认识，就可以得到较 为精确的制图；同样，如果机器人对环境( 地图) 了解很好，就很容易得到自身 的位置信息。但是，当定位信息和环境信息都不确定的情况下，两者就应该同 时考虑。这就是本研究中涉及的一个研究重点：移动机器人的同步定位与制图 ( s l a m ) 问题。 2 5 同步定位与制图问题 同步定位与制图( s l a m ) 是当前在移动机器人领域中自主导航的热门研究 方向【l6 。1 8 】。2 0 0 2 年在瑞典的k t h 举行的一个以s l a m 为主题的s u m m e rs c h o o l ， 移动机器人领域内最知名的专家到场为5 0 名国际学生讲学，从而将s l a m 的研 究引向了高潮。s l a m 算法是本研究的最主要问题，将在本章接下来的中对其进 行详细的分析。 2 5 1 同步定位与制图算法介绍 移动机器人的定位和地图创建是机器人领域的热点研究问题，也是导航中 重要环节。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已 经有了一些实用的解决方法。然而在很多环境中机器人不能利用全局定位系统 进行定位，而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这 时机器人需要在自身位置不确定的条件下，在完全未知环境中，创建地图同时 利用该地图进行自主定位和导航。这就是移动机器人的同步定位与地图创建 ( s l a m ) 问题，也称为c m l ( c o n c u r r e n tm a p p i n ga n dl o c a l i z a t i o n ) 。该研究领域的 代表人物有s m i t h s e l 饼c h e e s e m a n 旧】。由于s l a m 算法具有重要的理论与应用价 值，被很多学者认为是实现真正全自主移动机器人的关键。近几年来，其研究 取得了很大的进展，并己应用于各种不同的环境，如：室内环境 2 0 , 2 1 】、水下 2 2 , 2 3 】 以及室外环境【2 4 , 2 5 】。 7 2 5 2s l a m 算法性质和分类 s l a m 算法有很多重要的属性，影响着地图特征和机器人位置估计中的不确 定性。包括：状态估计的收敛性，估计过程的一致性，状态协方差矩阵更新的 计算复杂度【1 9 】。 ( 1 ) 收敛性：s l a m 算法具有三个重要的收敛性，这三个关键的收敛结果 是：1 ) 地图协方差矩阵的任意子矩阵的行列式随着每一次观测单调下降。2 ) 在极限情况下，随着观测数量的增加，特征估计变得完全相关。3 ) 地图的精度 与第一个特征被观测时机器人的位置精度有关。这些结果表明：随着观测数量 的增加，地图估计的不确定性将降低到有限的误差范围内，地图特征的关系将 是完全确定的。 ( 2 ) 一致性：为了维护s l a m 算法估计的一致性，对状态协方差矩阵进行更 新维护是必要的。既然对环境的观测是相对机器人的，所以机器人估计中的任 何误差和地图估计中的误差使绝对相关的。在没有其它外部的关于特征和机器 人位置信息情况下，为了使系统状态估计的误差在有限的范围内，保持状态估 计之间的一致性是很必要的。所以就必须维护机器人状态与环境特征之间的协 方差矩阵。 ( 3 ) 计算复杂度：s l a m 算法应用到大规模环境时的一个重要局限性是计 算环境特征之间，特征与机器人之间的相关信息时的计算负担。由于特征数量 很大，协方差矩阵的更新维护导致了s l a m 算法的计算复杂性。对于那些包含上 万个特征的环境，计算负担使得系统协方差的更新变的难以执行。所以需要一 个有效的方法来提高算法的计算效率。 根据所依据的理论基础的不同，s l a m 可以分为以下几种： ( 1 ) 基于扩展卡尔曼滤波( e k f ) 的c m l s l a m 这是最常用的一种s l a m 方法，适合解决非线性系统的估计问题。该方法用 平面坐标表示机器人和环境特征的位置，将机器人运动与环境特征的关系描述 为两个非线性模型：机器人运动模型和观测模型通过这两个模型，运用扩展卡 尔曼理论的思想来实现。主要包括预测与更新两个阶段 2 2 , 2 4 】 ( 2 ) 基于概率的c m l s l a m 尽管不如e k f 那样流行，但由于用概率表达机器人定位问题的不确定性非 常自然合理，基于概率的c m l 也吸引了很多人的目光。其中，较流行是最大相似 性( m a x i m u ml i k e l i h o o de s t i m a t i o n ，m l e ) 方法。一种非常有效的最大相似性 估计算法称为b a u m - w e l c h ( 或口一卢) 算法【2 6 1 ，基于这种方法的机器人制图与定 位问题可看作是机器人位置与环境特征位置的最大相似性估计问题。该算法包 括两步：e ste p ( e x p e c t a t i o n ) 和m - ste p ( m a x i m i z a t i o n ) 。 ( 3 ) 基于粒子滤波器( p a r t i c l ef i l t e r ) 的c m l s l a m 粒子滤波器定位也称为m o n t ec a r l o 定位2 7 , 2 8 】，其基本思想是用一组滤波器 来估计机器人的可能位置( 处于该位置的概率) ，每个滤波器对应_ 个位置，利 用观测对每个滤波器进行加权传播，从而使最有可能的位置的概率越来越高。 ( 4 ) 基于r a o b l a c k w e l l i z e d 粒子滤波器( r b p f ) 的c m l s l a m 采用r a o b l a e k w e l l i z e d 粒子滤波器解s l a m 问题的关键思想是：给定观测信 息z 1 ，和控制输入信息u 的条件下，估计机器人的运动轨迹和环境地图的联合后 验分布p ( 轰。，mh ，z l ，) ，该思想最早由m u r p h y f 8 9 】提出。根据贝叶斯公式有： p ( 缶：，mu 1 j ，z l 。) = p ( mi 眚：，z l 。) p ( 磊：，lu i ：t ) z l ：，) ( 2 一1 ) 这样，s l a m 问题就可以分为两步解决：首先仅估计机器人的运动轨迹，然后基 于估计的轨迹计算环境模型。( 2 1 ) 式中的p ( 岛，h ，z 1 ，) 部分类似与机器人定 位问题，当观察和控制输入信息到达时，采用粒子滤波器递增地进行估计； p ( mi 点 而：，) 部分类似与已知定位条件下的地图构建问题，可以非常方便地解决。 于是可以采用r a o b l a c k w e l l i z e d 粒子滤波器来实现s l a m 算法。再次滤波器中， 每个粒子代表一条机器人的可能路径，每个粒子与一个独立的完整地图相联系， 且基于粒子中的机器人轨迹的估计对相应的地图进行更新。 ( 5 ) 基于空间扩展信息滤波器的s l a m 基于空间扩展信息滤波器的s l a m 29 】由s e b a s t i a nt h r u n 等人提出，是对e k f 算法的改进。它不再用协方差矩阵表示空间信息的相关性，取而代之用空间信 息矩阵来表示空间信息间的内在固有的关系，并且使用网状数据结构仅维护邻 近的环境特征( 地图) 。 ( 6 ) 基于集合理论估计的s l a m 基于集合理论的方法【3 0 】不假设噪声服从某种分布，而只假设噪声是有界 的。具体方法是：定义一个可行状态集合( f e a s i b l es t a t es e t ) 和一个观测集合 ( m e a s u r e m e n ts e t ) ，前者表示机器人和环境特征的状态估计，后者表示符合条 件( 观测误差小于边界) 的状态集合。算法首先根据机器人本身的状态方程计 算可行状态集合，然后根据观测值计算观测集合，最后取两个集合的交集作为 某时刻的经估计校正后的机器人与环境特征的状态集合( 地图) 。 此外根据所制地图的不同，s l a m 又可以分为：基于栅格的s l a m 【3 、基于 特征s l a m 3 2 , 2 4 , 2 5 1 以及基于拓扑结构的s l a m 33 1 。 在基于栅格的s l a m 算法中，环境表示为一组具有特定分辨度的栅格的组 合。每个栅格中有特定概率值来表示其被占据的可能性。使用基于栅格s l a m 算 法的时候，假设各个栅格的状态变量是相互独立的，而没有考虑栅格之间的关 联性。另外，当机器人采用该种s l a m 算法运行较长一段路程后，很难再回到出 发点。这是因为该方法中仅在局部坐标中考虑了估计的不确定性，却没考虑局 部坐标与全局坐标中的相互联系。总的说来，基于栅格的s l a m 方法可以提供较 为丰富的环境描述，对于局部环境下的规划与导航有很大帮助，但是通过该方 法无法获得一致性较强的全局地图。 9 基于特征地图的s l a m 算法是将环境表示为一组组参数化的特征值，比如说 点，线，角等。这里的特征指的是环境中那些突显于背景的、易于传感器分辨 检测的且可以通过参数描述的东西。使用这种方法的时候，必须对环境中不同 类型的特征分别建立测量模型以便准确提取。基于特征的s l a m 算法是最为流行 的一种，尽管在环境地图描述中仅仅用到特征的位置值，但是还可以通过大小， 颜色等信息进行匹配等。这种s l a m 算法常常是基于卡尔曼滤波的。使用这种方 法时，状态向量不仅包括机器人的位姿信息而且还有特征的位置信息。随着新 特征的不断提取与确认，状态向量将不断的变大。因为用于描述环境的特征值 的测量都是相对机器人的，所以对环境特征测量的不确定性是与机器人位姿估 计的不确定性息息相关的。可以在理论上证明，随着时间的不断推移、测量的 不断进行，地图中的特征将是完全相关的，也就是说此时随意给定一个特征的 绝对坐标值，将会得到一个精准的地图。在环境特征容易识别的场合下，该算 法运用很好，但是对于特性不是太明显的非结构化环境中，算法的运用遇到难 题。 在基于拓扑结构的s l a m 算法中使用“图”来描述环境。具体说是通过节点 和弧线来描述环境。每个节点表示环境中突显的地方，边用来表示相邻节点间 的相对位置。对于纯粹的拓扑地图来说，无须知道每个节点的具体位置，它只 用来表示节点间的连通性。但是该方法也有很大的弊端，比如当环境稍微复杂 一些时，其对环境的识别能力会明显下降且无法识别相近的环境。尽管s l a m 算 法的理论基础已经被很好的研究，但是要将其更好的运用于实际，特别是大型 的非结构化环境，仍有大量理论和实际的问题需要解决包括：计算效率、地图 管理、局部地图与全局地图之间的协调融合、数据匹配以及传感器管理等【3 4 5 | 。 2 5 3 基于特征s l a m 算法构架简介 上文中讲到根据所得到地图类型的不同，可以将s l a m 算法化分为基于栅格 的s l a m ；基于特征的s l a m 以及基于拓扑结构的s l a m 三大类。其中，基于特征的 s l a m 是运用最广的一类，下文将对该类s l a m 算法进行详细阐述。基于特征的s l a m 算法架构由图2 1 表示。由图2 1 可以看出，整个算法实际上是由三部分构成即： 预测一实践一更新。首先要建立机器人的动力学模型以及运动过程中的观测模 型。某一时刻k ，在给定控制参数v ，口后，根据动力学模型可以预测机器人在 k + l 时刻的位姿状态；同时结合观测模型还可以预测在k + 1 时刻机器人对旧( 已 经核实的) 特征f 的观测值d i ( k + 1k ) ；在实践环节中，即在时刻k 到时刻k + 1 的 过程中，机器人基本上要完成三大工作：环境特征的提取和合理性检验，以及 相关数据的匹配。从而获得对f 特征的实际测量d i ( 尼+ 1k + 1 ) ，同时筛选出新的 特征巧+ ，。将对特征f 的估计测量和实际测量的差异值( 残差) d 、机器人位姿 估计以及旧的地图一起作为输入参数，通过一定滤波算法即可更新机器人位姿， 进而得到新特征f + 。的位置信息并将其融入到旧地图中尘成新地图。总而言之， 1 0 该方法要求机器人的位置和所有地图特征的位置存放在同一个状态向量中，并 且这个向量是增广的。当观测到环境中新的特征时，就要把它作为新的地图特 征添加到状态向量中。然后通过滤波就可以构建一个收敛的地图。在极限情况 下，随着对环境特征的重复观测，地图特征之间的相对关系变得完全确定，并 且地图的不确定性控制在一个小的范围内。这个误差范围与机器人初始位置的 不确定性有关。 图2 1 基于特征的s l a