（控制理论与控制工程专业论文）基于单目视觉的移动机器人同时定位和地图构建.pdf

学位论文作者签名： 麦j 昂k 签字日期：加馏年月乡日 学位论文版权使用授权书 本学位论文作者完全了解 重庭啻电太堂有关保留、使用学位论 文的规定，有权保留并向国家有关部门或机构送交论文的复印件和磁盘， 允许论文被查阅和借阅。本人授权 重迭邮电太堂可以将学位论文的 全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫描等 复制手段保存、汇编学位论文。 ( 保密的学位论文在解密后适用本授权书) 学位论文作者签名： 毒、k k 导师签名： 期：伽8 年月i ) 日期：谁厶夕 l l 摘要 摘要 导航是移动机器人研究领域的一个热门问题。 是在未知的环境中能够自主定位，而解决自主 地图构建。视觉传感器能获得直观和丰富的环 优势，因此，研究如何利用单目视觉传感器进 图构建，具有很重要的意义。 感器的同时定位和地图构建进行了深入研究。 定位和地图构建的问题描述、研究难点和主要 法和f a s t s l a m 方法进行了详细介绍；接着对 地图构建系统的原理进行了分析，设计了一个 目视觉获取的图像信息作为观测的s l a m 系统 框架，整个系统包括视觉跟踪模块、路标延迟初始化模块、地图库管理模 块以及s l a m 算法模块；然后对里程计和视觉传感器建立了数学模型并分 析了其不确定性；之后对整个系统的各个模块进行了详细的分析设计。视 觉跟踪模块采用s i f t 算法进行环境特征提取与匹配，以获得匹配的图像 点对以便于新特征的延迟初始化和已有路标的更新。延迟初始化模块采用 直接线性三角测量获得新特征的三维坐标，并采用u n s c e n t e d 变换处理不 确定性信息，当新特征的不确定性低于某个范围时就将其作为有效的路标 加入地图库。地图库管理模块用来快速地搜索和匹配路标，以及有效地检 测闭环。s l a m 模块采用f a s t s l a m 算法利用里程计的预测信息和摄像头 的观测信息进行机器人位姿和路标位置的更新，从而创建出更精确的地 图。三维重建的结果显示了三角测量方法的可靠性，s l a m 仿真结果显示 本文的算法比单独使用里程计能获得更精确的地图。 关键词：移动机器人，同时定位和地图构建，单目视觉，s i f t ，f 矗s t s l a m b a s e ds l a ms y s t e m ，af r a m e w o r ki sd e s i g n e dw h i c hc a nu s et h eo d o m e t r y i n f o r m a t i o na sc o n t r o li n p u ta n ds i n 9 1 ec a m e r ai m a g e sa so b s e r v a t i o n t h e w h o l es y s t e mc o n s i s to ff o u rm a i nm o d u l e s ：t h ev i s u a lt r a c k i n gm 0 1 d u l e ，t h e d e l a y e dl a n d m a r ki n i t i a l i z a t i o nm o d e l ，t h el a n d m a r kd a t a b a s em a n a g e m e n t m o d u l ea n dt h es l a ma l g o r i t h mm o d u l e t h e no d o m e t r ya n dc 砌e r aa r e m o d e l l e da n dt h e i ru n c e r t a i n t yi sd i s c u s s e d a f t e rt h a t ，a 1 1t h em o d u l e so ft h e s l a ms y s t e ma r ea n a l y z e da n dd e s i g n e d c a r e f u l l y t h ev i s u a lt r a c k i n g m o d u l eu s e ss i f ta l g o r i t h mt od e t e c t i n ga n dm a t c h i n gf e a t u r ep o i n t st og i v e a ni n p u tt ot h ed e l a y e dl a n d m a r ki n i t i a l i z a t i o nm o d e lt od on e wl a n d m a r k i n t i a l l i z a t i o no rt h es l a mm o d u l et ou p d a t et h el a n d m a r k sa l r e a d yi nt h em a p t h ed e l a y e dl a n d m a r ki n i t i a l i z a t i o nm o d u l eu s ed i r e c t1 i n e a rt r i a n g u l a t i o nt o c a l c u l a t ean e wf e a t u r e s3 dc o o r d i n a t e s ，t h e nt h eu n s c e n t e dt r a n s f 0 r mi s u s e dt 0d e a lw i t ht h eu n c e r t a i n t y ，w h e nan e wf e a t u r e su n c e r t a i n t yi sb e l o w s o m ev a l u e ，t h en e wf e a t u r ei st r e a t e da sa nv a l i dl a n d m a r ka n da u g m e n t e d i n t ot h e m a pd a t a b a s e m a pm a n a g e m e n tm o d u l em a k e s t h el a n d m a r k m a t c h i n gm o r eq u i c k l ya n dr o b u s t l y ， a n da l s oc a nd e c e c tl o o pc l o s u r e e f 代c t i v e l y a t1 a s tt h es l a mm o d u l eu s e sf a s t s l a ma l g o r i t h mt ou p d a t e t h er o b o t p o s ea n d1 a n d m a r k sp o s i t i o nu s i n go d o m e t r yi n f o r m a t i o na n dc a m e 豫 o b e r s c r v a t i o n t h er e s u l t so f3 dr e c o n s t r u c t i o ns h o wt h a tt h et r i a n g u l a t i o n l i m e t h o di sc o r r e c ta n ds l a ms i m u l a t i o na r ep r e s e n tt os h o wt h ee 伍c i e n c y0 f t h ea l g o r i t h m k e yw o r d s ： m o b i l e r o b o t ， s i m u l t a n e o u sl o c a l i z a t i o na n d m a p p i n g ， m o n o c u l a rv i s i o n ，s i f t ，f a s t s l a m n l 重庆邮电大学硕士论文 目录 目录 摘要i a b s t r a c t 1i 目录：iv 第一章绪论1 1 1 研究目的与意义1 1 2 国内外研究现状3 1 3 论文的研究内容和组织结构5 第二章移动机器人s l a m 概述7 2 1 s l 棚问题描述一7 2 2s l a m 研究的难点9 2 2 1 状态超多维问题：9 2 2 2 数据关联问题1 0 2 2 3 误差累积问题1 l 2 2 4 闭环问题1 2 2 3s l a m 的主要研究方法13 2 3 1 基于扩展卡尔曼滤波器的s l a m 1 3 2 3 2 基于粒子滤波器的s l a m 1 5 2 4f a s t s l a m 算法原理1 6 2 4 1f a s t s l a m 问题描述1 6 2 4 2f a s t s l a m 后验概率分解1 7 2 4 3f a s t s l a m 算法描述1 9 2 5 本章小结2 2 第三章基于单目视觉的s l a m 系统研究2 3 3 1 单目视觉s l a m 系统框架2 3 3 2 传感器模型的建立2 4 3 2 1 里程计模型2 5 3 2 2 视觉传感器的观测模型2 8 3 3 视觉跟踪模块设计2 9 3 3 1slf t 特征检测3 0 3 3 2sif t 特征点匹配3 5 重庆邮电大学硕士论文 目录 3 4 路标的延迟初始化3 5 3 4 1 延迟初始化的缓冲区设计3 6 3 4 2 直接线性三角测量3 7 3 4 3 路标的估计3 9 3 5 路标数据库的管理4 0 3 6 本章小结4 1 第四章基于f a s t s l a m 的同时定位与地图构建4 3 4 1 基于f a s t s l a m 的同时定位和地图构建4 3 4 1 1 采样新位姿和更新路标位置4 4 4 2 2 加权重采样4 5 4 2 实验结果。4 6 4 2 1 特征的匹配与三维重建实验4 6 4 2 2 同时定位和地图构建仿真一4 8 4 3 本章小节5 0 第五章全文总结和展望5 2 5 1 全文总结5 2 5 2 展望5 2 致谢5 4 参考文献5 5 附录：攻读硕士学位期间从事的科研工作5 8 v 重庆邮电大学硕士论文 第一章绪论 1 1 研究目的与意义 第一章绪论 自从19 5 9 年世界第一台真正意义上的机器人在美国诞生以来，机器 人技术在短短的几十年内取得了迅猛的发展。从第一台机械手到现在的大 规模自动化生产线，机器人技术在工业领域的广泛应用大大提升了生产效 率并反过来促进了机器人技术的进一步发展。随着现代科学技术的飞速发 展，机器人在功能和技术层次上有了很大的提高，移动机器人就是典型的 代表。 移动机器人技术是当今机器人研究领域的热点和主要发展方向。移动 机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于 一体的智能控制系统。相对于固定的工业机器人和操作手而言，智能移动 机器人具有移动能力，能自主的探索未知环境，不仅可以作为人类的朋友 与人共享生活空间，而且能够代替人类到复杂或危险的工作环境下执行任 务，因此得到了广泛的重视和快速的发展。移动机器人研究始于6 0 年代 末期，斯坦福研究院( s r i ) 的n i l s s o n 等人，在19 6 9 年至19 7 2 年中研制出 了名为s h a k e y l l 】的自主移动机器人，标志着移动机器人研究的正式开端。 此后，随着计算机硬件技术、传感器技术、自动化技术和人工智能技术的 发展，设计和制造真正具有自主能力的智能移动机器人逐渐成为机器人研 究人员的主要目标。 移动机器人为了能够有效地探索未知环境并顺利完成给定的任务，就 需要具有自主导航能力。按照l e o n a r d 和d u r r a n t w h y t e 的说法【2 1 ，自主 导航就是要回答“w h e r ea mi ? ，“w h e r e 锄ig o i n g ? 和“h o ws h o u l dig e t t h e r e ? ”这三个问题。第一个问题是移动机器人的定位问题，即确定机器 人在环境中所处的位置和方向。第二个问题与机器人的具体任务有关，即 任务规划。第三个问题是路径规划问题，就是要求从机器人当前位置与目 标位置之间寻找一条合理的路径，保证机器人能够安全地到达目标地点， 从而完成指定的任务。上面三个问题中，移动机器人的定位问题是解决其 它两个问题的先决条件，因此具有重要的意义。 移动机器人利用内部传感器( 如里程计) 进行定位时，由于内部传感 重庆邮电大学硕士论文 第一章绪论 器的不确定性( 如轮子打滑、地面不平、机械误差等) 会造成定位结果产 生偏差，并且随着时间的推移这种偏差会累计起来，导致定位失败。因此 移动机器人一般装备有外部传感器，利用外部传感器的观测结果来修正机 器人的位姿。 移动机器人在已知的环境中运动时，通常设有位置已知的物体作为路 标，机器人可以利用携带的外部传感器测量自身和路标的相对位置，从而 获取自身在环境中的绝对位置以实现准确的定位导航，这属于已知地图的 定位问题，目前已经有很多有效的解决方法。然而大多数情况下，移动机 器人需要在未知环境中运动，例如在进行星球探测时，通常无法容易的获 得探测机器人将要面对的环境；排爆机器人进入危险区域排除险情的时 候，也无法获得先验的环境信息。这属于未知地图的定位问题。 对于未知地图的移动机器人定位导航，机器人没有先验的环境地图来 进行定位，而一般机器人携带的传感器的探测范围有限，只能获取环境的 局部信息，因此只有通过机器人的移动来探索环境的全部信息，建立全局 环境地图并利用该地图进行全局定位。在建立环境地图的过程中，机器人 必须知道自己在各个观察点的位姿，并基于此位姿计算观察到的路标的位 置，从而建立地图。而机器人的内部传感器( 里程计) 的不确定性使得仅 仅依靠其内部传感器无法准确地知道机器人在各个观察点的位姿，必须依 赖外部传感器获得的信息修正机器人的位姿，从而利用准确的机器人位姿 来获得准确的路标位置，即精确的地图构建需要精确的定位。另一方面， 机器人利用外部传感器进行定位，必须知道观察到的路标的位置，并基于 此位置计算机器人的位姿。如果路标的位置不准确会导致计算出来的机器 人位姿不准确，即精确的定位又依赖于精确的地图信息。可以看出，定位 与地图构建相互依赖、相互决定，形成“鸡生蛋、蛋生鸡的问题 ( e g g c h i c k e n ) ，因此必须同时加以考虑，即同时定位与地图构建 ( 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 唇或 c o n c u r r e n tm a p p i n ga n d l o c a l i z a t i o n ，简称s l a m 或c m l ) 。 同时定位和地图构建是实现自主移动机器人的关键，是自主移动机器 人领域的基础性难题，因此深入研究其基本理论和解决方法以实现完全自 主的移动机器人具有重要的科研价值。而由于机器视觉技术的飞速发展， 视觉传感器在机器人中的应用也越来越普遍。视觉传感器具有价格低廉、 获取信息丰富等优点，采用视觉传感器不仅可以降低机器人的成本而且能 够提高机器人的感知能力，从而为实现廉价的自主移动机器人创造了有利 条件。因此，研究基于视觉传感器的移动机器人同时定位和地图构建，并 2 确定性，使得s l a m 归结为一个估计问题，而解决估计问题一般使用概 率方法。s m i t h 等人于19 9 0 年提出基于扩展卡尔曼滤波器( e x t e n d e d k a l m a nf i l t e r ，即e k f ) 的随机建图方法( s t o c h a s t i cm a p p i n g ) 【4 1 ，揭开 了s l a m 研究的序幕。扩展卡尔曼滤波器是卡尔曼滤波器在非线性系统 中的推广。它对非线性方程进行线性化，用泰勒展开式的一阶项来近似非 线性系统方程，从而运用卡尔曼滤波器估计增广的系统状态。十几年来， e k f 方法成为s l a m 问题的主要研究方法，并针对不同的环境提出了基 于e k f 的s l a m 算法【5 1 。e k f 方法的瓶颈在于其计算复杂性，难以满足 大规模环境的地图构建和实时性的要求。e k f 算法在滤波的每一步中， 都要对增广的系统协方差矩阵进行完全更新。随着观测到的环境路标的增 加，系统状态空间的维数也随着增加，计算量也随之增大。另外，e k f 算法是基于已知观测值和路标之间对应关系这个假设的，e k f 算法本身 并不能解决数据关联问题，这样，一部分观测值与路标之间的错误关联将 导致e k f 算法发散。 基于粒子滤波器的s l a m 算法的出现，提高了同时定位和地图构建 的速度，已成为s l a m 研究领域最重要，最有效的方法。粒子滤波器的 基本思想是用一组滤波器( 每个滤波器称为一个粒子) 来估计机器人的可 能位姿( 即处于该位姿的概率) ，每个滤波器对应一个机器人位姿，利用 观测值对每个滤波器进行加权传播，从而使最可能的位姿的概率越来越 重庆邮电大学硕士论文 第一章绪论 高。它很容易实现，而且不像e k f 那样必须假定噪声服从高斯分布。粒 子滤波器可以处理位姿跟踪、全局定位和机器人绑架问题( k i d n a p p i n g p r o b l e m ) ( 即在机器人的里程计没有记录的情况下将机器人从一个地方 搬到另一个地方) 。粒子滤波器在机器人定位领域较早取得的成功体现在 1 9 9 9 年d e l l a e r t 的论文中1 6 】。由于标准的粒子滤波器要取得满意的结果需 要大量的粒子，使得计算复杂度大大增加，因此研究人员引入了改进的粒 子滤波器来处理s l a m 问题。m u r p h y 等人【7 】采用r a o b l a c k w e l l i s e d 【8 】粒 子滤波器来有效地估计机器人的路径和地图。目前r a o b l a c k w e l l i s e d 粒 子滤波器已经成为应用最为广泛的粒子滤波器。m o n t e m e r l o 【9 1 等人基于 r a o b l a c k w e l l i s e d 粒子滤波器设计了一种称为f a s t s l a m 的s l a m 算法， 并在真实的机器人上实现了该算法，取得了很好的效果。 目前s l a m 中使用的传感器大部分是声纳和激光雷达，声纳传感器 成本低但精度不高，激光雷达精度高但成本较高，这两种都属于主动传感 器，只能得到简单的特定平面的二维信息。视觉传感器属于被动传感器， 视觉图像中包含了丰富的环境信息，结合一定的先验信息或目标识别算 法，可以得到意义更明确的地图。 视觉很早就应用到地图构建中了。早在2 0 世纪8 0 年代，斯坦福大学 就研制出了第一个应用在地图构建中的机器人“斯坦福推车 【l0 1 ，它使用 了一个单目摄像机运动了5 0 厘米，拍摄了9 幅图像，然后对这些图像进 行特征提取，再把这些特征映射到三维坐标中，得到环境地图。此后经过 2 0 几年的发展，机器视觉和地图构建都取得了长足的发展，产生了大量 的算法。 最近十年，由于视觉传感器的价格大幅下降，而且性能也越来越好， 越来越多的移动机器人都把视觉传感器作为其必备传感器，而且由于一些 良好的特征提取算法的出现，使得基于视觉的移动机器人同时定位和地图 构建的研究也变的热门起来。h a r r i s 的三维视觉系统d r o i d 【l l 】通过在连 续的图像中检测角点特征来进行三位重建，并利用卡尔曼滤波器来跟踪特 征点的位置。d r o i d 系统对于小规模的环境具有较好的精度，当环境规 模较大时会因为长期的飘移而导致无法创建地图。s i m 和d u d e k 【1 2 】提出通 过学习自然特征来进行位姿估计，能够在没有人工路标的环境下进行地图 构建。l o w e 等【1 3 】采用s i f t 【1 4 】特征提取算法提取环境中的s i f t 特征点作 为路标，利用立体视觉摄像机来确定特征点的位置，通过卡尔曼滤波器对 路标的位置进行估计，成功地构建了实验室环境的地图。r o b e r t 等人【”】 基于r a o b l a c k w e l l i s e d 粒子滤波器设计了一个实时的视觉s l a m 框架， 4 重庆邮电大学硕士论文第一章绪论 能够在没有里程计等内部传感器的情况下操作，既可以使用立体视觉作为 观测传感器也支持单目视觉。d a v i s o n 【1 6 】构建了一个基于单目摄像头的 s l a m 系统m o n o s l a m ，能在小规模环境下利用单个普通的摄像头实现 实时的定位和地图构建，但不能应用于较大规模的环境。e v o l u t i o n r o b o t i c s 公司的商业化视觉s l a m 软件v s l a m 【1 7 ，鹕1 采用s i f t 算法提取 环境特征，利用单目摄像头和里程计实现了小规模环境的同时定位和地图 构建，能够满意地处理动态的环境，如光照变化，人和物体的移动等情况， 而且能够很快地从动态扰动中恢复过来，很好地解决了机器人“绑架”问 题。 国内基于视觉的移动机器人同时定位和地图构建的研究比较少，目前 只有哈尔滨工业大学，浙江大学，上海交通大学等几所大学在这方面做了 一些工作。哈尔滨工业大学的厉茂海等人【”】采用改进的r a o b l a c k w e l l i z e d 粒子滤波器进行了移动机器人同时定位和地图构建研究，在仅有单目视觉 和里程计的基础上，建立了鲁棒的感知模型，通过s i f t 提取环境特征， 并采用u n s c e n t e d 卡尔曼滤波器更新特征，取得较好的效果。浙江大学的 吕强【2o 】利用s i f t 算法进行了单目视觉里程计的研究，取得了一些初步的 成果，虽然不涉及地图构建问题，但对于基于单目视觉的s l a m 研究中的 位姿估计问题具有重要的意义。上海交通大学的徐宁【2 l 】利用改进的s i f t 算法h a 盯i s s i f t 实现了实时的单目视觉定位和对象识别。 1 3 论文的研究内容和组织结构 本论文是在教育部资助项目“基于单目视觉的移动机器人导航系统 ( 编号：z 2 0 0 5 2 6 3 0 0 3 ，2 0 0 7 2 0 0 9 ) ，国家外专局资助项目“智能机器 人技术及其应用 ( 编号：2 0 0 7 5 0 0 0 0 2 8 ，渝外专 2 0 0 7 】4 0 号) 的资助下开 展的研究工作，旨在建立一个能够运行在小型服务机器人平台上的视觉 s l a m 系统，使得机器人能够在未知的环境中自主构建环境地图，同时利 用该地图完成自主导航。为了降低移动机器人的成本，使之更快地得到普 及，选用廉价的普通摄像头作为传感器是一个较好的选择。因为相对于声 纳的低精确度和激光的高成本而言，视觉传感器不仅具有成本的优势，而 且能获得比其他传感器更丰富的环境信息。移动机器人只有拥有了较为完 善的自主运动能力才能更好地为执行任务，从而为产业化铺平道路。因此， 研究基于单目视觉的移动机器人同时定位和地图构建具有重要的意义。 论文主要研究了基于单目视觉的移动机器人s l a m 的算法和实现技 5 维重建的初步试验，并给出了s l a m 仿真的初步结果。试验结果表明了 本文的方案的正确性。 本文共分五章，各章内容安排如下： 第一章为绪论，介绍了本文的研究目的与意义、国内外研究现状及本 文的研究内容和结构安排。 第二章对移动机器人s l a m 的理论进行了详细的阐述，对研究难点和 方法作了深入的分析，并对e k f 和f a s t s l a m 方法进行了详细的论述。 第三章讨论了基于单目视觉的移动机器人s l a m 框架设计，建立了运 动模型和观测模型，并对整个系统的各个模块进行了深入的研究，给出了 解决方案。 第四章为s l a m 算法的实现部分，主要研究了f a s t s l a m 在基于单目 视觉和里程计的移动机器人系统中的具体实现方法。并给出了三维重建的 结果和f a s t s a l m 算法的仿真结果。 第五章为全文总结，指出了今后的研究方向和改进措施。 6 第二章移动机器人s l 州概述 器人s l a m 概述 建问题密切相关，已知环境地图的定 问题已经被广泛研究，提出了许多有 器人位姿都事先未知的情形，问题就 题同时进行考虑，即同时定位和地图 一个未知的环境中，移动机器人从一 的位置并在每一个位置利用外部传感 图，并根据已经创建的地图来确定机 器人自身的位姿，如此循环，从而增量地创建地图的过程。其过程的示意 图如图2 1 所示。图中深色的三角形表示机器人的实际位姿，浅色的三角 形表示机器人的估计位姿，深色的星形表示路标的实际位置，浅色的星形 表示路标的估计位置。 x 图2 1s l a m 问题示意图 移动机器人在未知的环境中运动时，需要执行控制命令并且利用外部 传感器观察环境中的路标。在这个过程中，控制输入和观测数据都会受到 干扰，导致控制结果和观测结果不准确，即机器人里程计记录的位姿信息 7 ( z l ，z ，) 和所有运动控制输入信息u ，- ( u ，) ，估计当前( t 时刻) 的机器人的位姿五和路标位置现。即求后验概率分布函数 p ( 五，i z 。，【，) 。 若初始的先验概率分布函数p ( ，lz o ，u o ) 已知，根据贝叶斯估计理 论，通过预测和更新两步就可以以递归的方式估计后验概率分布函数 p ( 五，掰，iz ，扩) 。根据贝叶斯公式和马尔科夫性，有： 聃川z f 阶盟盟笔筹学攀( 2 4 ) 由c h a p m a n - k o l m o g o r o v 方程和马尔科夫性有： 尸( 置，砚iz 卜1 ，【厂卜1 ) = p ( 置，i 五- ，- l ，z h ，u ) 尸( 置- l ，一。lz ，u ) d ( 置- l ，竹一。) ( 2 5 ) = p ( 五，啊i 置- ，- ，u ) 尸( 葺中鸭一。iz h ，u ) d ( 置巾啊一。) p ( 墨，搠，iz ，【，) = ，7 尸( z fi 五，鸭) 肛置，l 置- ，啊- ，u ) p ( 置书码一。l z ，u 川) d ( _ - ，惕一。) 2 石 式中p ( 置，啊l 墨- l 啊- l u ) 根据式( 2 1 ) 和( 2 2 ) 确立；刀为归一化常量。 8 l a m 概述 2 2s l a m 研究的难点 视化地 的机器 在s l a m 中，主要难点包括状态超多维问题、数据关联问题、误差累 积问题和闭环问题等。下面分别予以介绍。 2 2 1 状态超多维问题 s l a m 需要估计机器人的位姿和路标的位置，即系统状态( 置，研，) ， 对于存在n 个路标的二维环境，z 包含三个变量( x ，y 坐标和方向) ，m 包含2 个变量( 每个路标包含2 个变量，即x ，y 坐标) ，于是共有2 + 3 维状态。 在大规模环境中可能存在很多的路标，特别是对于视觉传感器而言， 提取的环境特征可能有成千上万个，因此对( 置，聊，) 的估计实际上是一个 超多维问题。如果要估计后验概率分布户( 置，所，jz ，u 。) ，则系统状态( x ，川，) 的维数可能高达( 2 + 3 ) f ，如此高维空间的连续状态概率分布的估计显然 是一个困难的问题，因此超多维问题成为s l a m 研究的一个难点。 9 图2 3 数据关联的不确定性 在上图中机器人获得一个观测结果( 黑点表示) ，然而此时环境中存 在两个距离很相近的路标( 五角星表示) ，并且由于机器人的不确定性和 两个路标位置估计的不确定性，机器人很难判断该观测值究竟来自于哪一 个路标。如果机器人对观测值和路标的关联发生错误，则会导致错误的更 新，并且此后的预测也会发生错误，从而增大定位与建图的误差。 实现m 个观测值与拥有n 个路标的地图之间的数据关联的复杂度与 m 呈指数关系，假设每个观测值d ，( 江1 ，m ) 有刀，个可能的匹配，那么对 于m 个观测值，就需要在指数空间兀，中搜索正确的匹配。数据关联 的搜索空间与环境的复杂程度以及机器人的定位误差有关，环境的复杂程 度的增加会使m 增大，而误差的增大会使刀，增大。 在s l a m 中，由于系统状态的估计通常依赖于统计信息，因此基于统 计方法的数据关联被广泛采用。最常用的数据关联方法是最近邻法n n ( n e a r e s tn e i g h b o r ) 【2 2 23 1 ，即通过计算观测值与相应路标在统计意义下的 1 0 重庆邮电大学硕士论文第二章移动机器人s 乙w 概述 最短距离来建立二者之间的对应关系。最近邻数据关联本质上是一种假设 检验的方法，根据距离度量决定接受或者拒绝关联结果。该方法计算非常 简单，但对于距离很近的两个路标可能会出现关联错误。近年来关于数据 关联的研究增多，相继提出了基于多假设检验的关联方法【2 4 1 、基于联合兼 容性检测的关联方法【2 5 1 、基于几何关系的关联方法【2 6 1 、基于整数规划的关 联方法【27 】等多种更为鲁棒的数据关联方法。数据关联是s l a m 研究的一个 难点，也是今后该领域的一项重要内容。 2 2 3 误差累积问题 s l a m 中的误差主要来自三个方面：移动机器人运动误差( 里程计的 误差) 、外部传感器观测误差和错误的数据关联产生的误差。 当移动机器人在已知地图的环境中进行定位时，里程计的误差会随着 机器人的不断运动而增大，但是当机器人观测到环境中位置已知的路标 时，可以利用路标的位置对里程计的误差进行校正，每一次观测都能使机 器人的定位误差趋于观测误差，因此定位误差不会发散，而是基本保持在 一个较小的范围。然而当移动机器人在未知的环境中进行s l a m 时，随着 机器人的运动，里程计的误差不断累积，使得机器人的定位误差不断增大， 基于机器人的位姿构建的地图的误差也不断增大，即定位误差会传递给地 图构建；反过来，路标位置估计的误差又进一步增大机器人定位的误差， 上述的过程反复进行下去，可能产生发散的定位与地图构建结果，因此， 准确性成为s l a m 最重要的评价指标。 图2 4s l a m 中误差累积示意图 图2 5 机器人检测到闭环时的误差示意图 从上图可以看出，随着机器人的不断运动，误差逐渐增大，但是当机 器人因观测到先前构建地图的路标而形成闭环时，定位与地图构建的不确 定性瞬间减小了。可见闭环检测直接影响了定位与地图构建的效果，因此， 能否实现有效的闭环也成为评价s l a m 算法的优劣的一项重要指标。 在大规模的环境中，闭环检测成为一项十分困难的工作，因为在大规 模环境中，由于误差长时间累积，数据关联变得十分困难，错误的数据关 1 2 究方法。 下面分别对基于扩展卡尔曼滤波器和粒子滤波器的s l a m 方法的原理 进行介绍。 2 3 1 基于扩展卡尔曼滤波器的s l a m 卡尔曼滤波器( k f ) 是最小二乘意义下最优的状态估计方法，它是 线性系统状态估计的标准工具。而扩展卡尔曼滤波器( e k f ) 是k f 在非线 性系统中的推广，通过将非线性系统进行泰勒展开，利用展开的一阶项来 近似非线性方程，然后使用k f 滤波过程进行状态的递推估计【3 1 1 ，以测量 误差为依据进行估计和校正，不断逼近真实值。s l a m 问题从统计的角度 来讲是一个滤波问题，而且由于移动机器人运动模型和观测模型高度非线 性，因此通常使用e k f 进行处理。e k f 可以很好地除去系统中的高斯噪 声干扰，在系统噪声和观测噪声服从高斯白噪声分布的情况下，可以得到 统计意义上的最优估计。 基于e k f 的s l a m 算法，将路标状态和机器人状态作为增广的系统 状态空间同时进行估计，估计过程包括状态预测、观测更新和状态增广三 个步骤，对增广的系统状态和增广的系统协方差矩阵进行递归地更新。系 学硕士论文第二章移动机器人s l a m 概述 矩阵通常包含机器人位姿和地图之间的不确定信息，且当机器人 地图中的路标时，协方差矩阵协方差矩阵的任何子阵的行列式呈 的趋势。从理论上讲，当观测次数趋于无穷时，每个路标的协方 器人起始位姿的协方差有关。e k f 方法是s l a m 研究中最重要 一，因此下面对其进行详细的介绍。 如下的非线性系统： 工( f + 1 ) = ( x ( f ) ，z | ( f ) ) + w ，1 、 z o + 1 ) = 办( x o + 1 ) ) + m 分别为系统的运动方程和观测方程。w 和v 分别为系统噪声和 ，且都为零均值的高斯白噪声。其统计特性如下： 研w 】= o ，e 【w 嵋2 】= q ( ) ( 2 8 ) 三【v 】= o ，研v f v l = 犬( f ) 系统状态模型和观测模型均存在随机噪声，因此，系统的状态也 量，在给定观测数据的前提下，系统状态估计为： 曼( ，i f ) = 研z ( ，i 圳z ( 1 ) ，z ( f ) 】 ( 2 9 ) 差矩阵表示为： 尸( f i f ) = e ( x ( ，) 一量o l f ) ) ( z ( f ) 一量o i ，) ) 7l z ( 1 ) ，2 0 ) 】 ( 2 1 0 ) e k f 的状态估计过程分为两步一一预测和更新。 在第t 时刻时预测系统下一时刻的状态、协方差以及观测值： 量o + lir ) = 厂( 曼( r l f ) ，“( f ) ) 尸( f + 1f ) = v ，厂尸( fif ) v ，r + v 。q ( 珍v ，r ( 2 11 ) 三( ，+ 1if ) = 厅( 就f + 1if ) ) 其中v ，厂= 芸l ，越( 们表示系统模型对x ( f ) 的雅可比矩阵，v 。厂表示系统 模型对w 的雅可比矩阵。 第t + 1 时刻获得观测值为z 0 + 1 ) ，利用观测值进行如下更新： 黧端三黼篙揣臻，矾删 尸o + 1if + 1 ) = p ( f + 1f ) 一形o + 1 ) 鼽( f + 1i ，) 矽7 ( f + 1 ) 其中 双f + 1 ) = z o + 1 ) 一j i i ( 圣0 + 1if ) ) ( f + ll f ) = v ，厅- 尸( f + li ，) v ，7 办+ 尺( f + 1 ) ( 2 1 3 ) 形( ，+ 1 ) = v ，r 尸( f + 1i f ) - 1 0 + 1i f ) ( 2 13 ) 中妖，+ 1 ) 称为新息，o + 1if ) 表示新息的协方差，v 。 = 尝l 印p ) 1 4 z 来递推计算状态置取不同值时的置信度p ( 置fz ) ，由此获得状态的最 优估计。 粒子滤波器将所关心的状态向量表示为一组带有相关权值的随机样 本，并且基于这些样本和权值可以计算出状态估值。粒子滤波算法用有限 的样本或粒子集合描述后验概率分布，高概率的区域粒子密度高，低密度 的区域有少量的粒子或者没有粒子。当样本数趋于无穷大时，可以逼近任 何形式的概率分布。因此粒子滤波器能够比较精确地表达基于观测量和控 制量的后验概率分布。 粒子滤波器是针对离散时间、部分能观、能控m a r k o v 链的后验概率估 计方法，目前主要应用于室外大环境下的同时定位和地图构建。粒子滤波 器易于实现，不需要线性化非线性模型，但对于高维状态空间，样本数很 大，需要高的运算速度，从而对计算机硬件的要求高，因此粒子滤波常用 于全局定位问题。与卡尔曼滤波器相比，粒子滤波器在以下三个方面具有 优越性：( 1 ) 粒子滤波的计算量是d ( 1 0 9 k ) ，n 是样本数，k 是一次观测 的路标数。( 2 ) 粒子滤波可以处理后验概率为非高斯、多模型分布的情况， 可更充分地利用观测数据，处理否定信息。而在这种情况下，e k f 将导致 数据关联的失败。( 3 ) 应用粒子滤波处理数据关联问题具有较强的鲁棒性。 重庆邮电大学硕士论文 第二章移动机器人s l a m 概述 2 4f a s t s l a m 算法原理 f a s t s l a m 是一种粒子滤波器，它使用粒子滤波器来估计机器人的位 姿，用扩展卡尔曼滤波器来估计路标的位置。 2 4 1f a s t s l a m 问题描述 设，为从开始到现在的机器人位姿的集合，即s 弛，s ：，s ) ；o 是所 有路标的位置状态 儡，岛，吼) ；7 = 五，乞，五) 是从开始到现在的对路标 的观测集合；“。是从开始到现在时刻的控制输入，即里程计的信息； 玎7 = 强，他，以，) 是从开始到现在的数据关联结果，即把特定的观测与特定 的路标进行对应。则s l a m 的一般模型是计算概率密度函数 p ( j ，o 阿，“，) ，即估计路标的位置和当前时刻的机器人位姿。而 f a s t s l a m 算法则是估计路标的位置和所有时刻的机器人位姿，即计算概 率密度函数p ( s ，o 阿，甜，) ，该问题也称为f u l ls l a m 问题。计算 p ( s ，e ，“，刀) 的理由是，p ( s 。，0h ，材，刀) 可以分解成几个比较简单的因 式乘积，大大降低了计算复杂度。 下面推导f a s t s l a m 算法的滤波器模型。如果一个概率函数在某个时 刻的值能够递归地从前一时刻的值计算出来，那么这个概率函数就是一个 滤波器。大多数的s l a m 算法都是一种b a y e s 滤波器，f a s t s l a m 的滤波 器模型将在下面进行推导。根据b a y e s i a n 法则有： p ( s ，ez 。，“7 ，刀) = 7 7 p ( 刁ij ，e ，刀。，z 一，材) p ( s ，0l 刀，z 卜1 ，“。) ( 2 1 4 ) 其中刁为归一化常量，与计算后验概率中所需要的变量无关。这里我们注 意到这样一个事实，即t 时刻的观测z ，只取决于三个变量：观测时的机器 人位姿s 、数据关联的结果和被观测到的路标的实际位置r 。因此我 们有： p ( 刁l s 7 ，e ，玎，z 卜1 ，“) = p ( 刁i q ，或，惕) ( 2 1 5 ) 此外，p ( s ，e ，z 卜1 ，“) 能够被分解成如下形式： p ( s ，0i 刀，z 卜1 ，材) = p ( qis 。一，0 ，玎。，z 卜1 ，材) p ( s 卜”，ei 以，2 一，“。) ( 2 16 ) 上式右边的两个乘积项都可以通过丢弃对计算概率无用的项而被进一步 简化。事实上，由s 一。和“，的信息足够预测墨的信息，所以式( 2 16 ) 右边 乘积中第一项里其他的变量都可以被忽略。类似地，和坼并没有包含对 计算，0 的后验概率有用的信息。因此我们可以重写式( 2 16 ) 而得到： 1 6 图2 6f a s t s l a m 问题的动态贝叶斯网络模型 从上图可以看出，机器人的初始位姿为墨，在一系列的运动控制输入 ，“：，甜，的作用下，机器人运动到位姿只。在运动过程中，机器人观测到 其附近的路标：在f = 1 时刻机器入观测到路标鼠( 即，l l = 1 ) ，观测值记为毛； 在忙2 时刻机器人观测到路标幺( 即玎，= 2 ) ，记观测值为乙；依此类推。 f a s t s l a m 就是基于这个特性对后验概率p ( s ，o | z ，甜，刀) 进行了分 解： p ( s 7 ，e 材。，以) = p ( s 7i z ， “7 ，刀) 兀p ( 幺z ，以) 一= l p ( o z ，玎) = 兀p