（模式识别与智能系统专业论文）结构化环境下基于激光和单目视觉的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 d m 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 方法。主要工作如下： ( 1 ) 针对二维结构化环境，提出了基于激光角点特征的s l a m 方法。通过应 用改进的运动学模型，并采用角点和线段混合观测的方法，提高了基于激光角 点特征的s l a m 的准确性和鲁棒性。 ( 2 ) 针对三维结构化环境，提出了基于竖直线特征的单目视觉s l a m 方法。 为了解决单目视觉s l a m 中的特征深度初始化问题，使用逆深度的表示方法描述 竖直线特征的状态。 ( 3 ) 在上述两个工作的基础上，针对三维结构化环境，提出了基于激光和单 目视觉信息融合的主动s l a m 方法。通过激光和视觉信息的融合得到更加准确的 特征估计，进而提高了s l a m 的准确性。另外，通过采取基于最优控制的主动 s l a m 方法，减小机器人定位与建图的不确定性，提高了s l a m 的准确性和鲁棒 性。 关键词：同时定位与建图扩展卡尔曼滤波器多传感器融合结构化环境 a b s t r a c t a b s t r a c 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 dm a p p i n g ( s l 气m ) i nu n k n o w ne n v i r o n m e n tf o r m o b i l er o b o ti st h ec o r ei s s u eo fa u t o n o m o u se x p l o r a t i o n i th a sc a u s e dw i d ec o n c e r n s i n c eb o r na n dp l e n t yo fm e t h o d sa r ep r o p o s e df o r s o l u t i o n s n o w a d a y s ，m o s t r e s e a r c h e so ns l a mf o c u so ns t r u c t u r e de n v i r o n m e n t , w h i c hi so n eo fi m p o r t a n t e n v i r o n m e n tf o rs l a mr e s e a r c ha n d a p p l i c a t i o n s i n c ep o i n t sa n dl i n e sc a nb ee a s i l y e x t r a c t e db yl a s e ra n dv i s i o ns e n s o r s ，a sl a n d m a r k si ns l a m ，t h e s es e n s o r sa r e m a i n l ys e n s o r so fs l a mi ns t r u c t u r e de n v i r o n m e n t f o c u s i n go nu n k n o w ns t r u c t u r e de n v i r o n m e n t s ，t h i st h e s i si n v e s t i g a t e st h e s l 州m e t h o d sb a s e do nl a s e ra n dm o n o c u l a rv i s i o n , a n dp r o p o s e sa c t i v es l 气m m e t h o d sb a s e do nf u s i o no fl a s e ra n dv i s i o nd a t a o u rw o r ki n c l u d e st h ef o l l o w i n g a s p e c t s ： ( 1 ) t h es l a mm e t h o db a s e do nl a s e rc o m e rf e a t u r e si s p r o p o s e df o r t w o - d i m e n s i o n a ls t r u c t u r e de n v i r o n m e n t t h ea c c u r a c ya n dr o b u s t n e s sa r ei m p r o v e d d u et oe m p l o y i n gt h ei m p r o v e dm o t i o nm o d e la n dc o m b i n e dm e a s u r e m e n to fp o i n t s a n dl i n e s ( 2 ) t h em o n o c u l a rs l a mm e t h o db a s e do nv e r t i c a ll i n ef e a t u r e si sp r o p o s e df o r t h r e e - d i m e n s i o n a ls t r u c t u r e de n v i r o n m e n t 1 1 1 e i n v e r s e d e p t hr e p r e s e n t a t i o n o f v e r t i c a ll i n e si se m p l o y e dt os o l v et od e p t h i n i t i a l i z a t i o ni nm o n o c u l a rs l a m ( 3 ) b a s e do nt h ea b o v em e t h o d s ，a na c t i v es l a ma p p r o a c hb a s e do nf u s i o no f l a s e ra n dv i s i o nd a t aa r ep r o p o s e df o rt h r e e - d i m e n s i o n a ls t r u c t u r e de n v i r o n m e n t f u s i o no fl a s e ra n dv i s i o nd a t al e a d st om o r ea c c u r a t ee s t i m a t eo ff e a t u r e s ，w h i c hc a n i m p r o v et h ea c c u r a c yo fs l a m 、h a t sm o r e a l la c t i v es l a ma p p r o a c hb a s e do n o p t i m a lc o n t r o li sp r o p o s e dt or e d u c et h eu n c e r t a i n t yo fr o b o tl o c a l i z a t i o na n dm a p b u i l d i n ga n di m p r o v et h ea c c u r a c yo fs l a m 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 ，e x t e n d e dk a l m a nf i l t e r , m u l t i s e n s o rf u s i o n ，s t r u c t u r e de n v i r o n m e n t i i 南开大学学位论文版权使用授权书 本人完全了解南开大学关于收集、保存、使用学位论文的规定， 同意如下各项内容：按照学校要求提交学位论文的印刷本和电子版 本；学校有权保存学位论文的印刷本和电子版，并采用影印、缩印、 扫描、数字化或其它手段保存论文；学校有权提供目录检索以及提供 本学位论文全文或者部分的阅览服务；学校有权按有关规定向国家有 关部门或者机构送交论文的复印件和电子版；在不以赢利为目的的前 提下，学校可以适当复制论文的部分或全部内容用于学术活动。 学位论文作者签名：慈超 1 ，7 年月1 日 经指导教师同意，本学位论文属于保密，在年解密后适用 本授权书。 指导教师签名：学位论文作者签名： 解密时间：年月 日 各密级的最长保密年限及书写格式规定如下： 南开大学学位论文原创性声明 本人郑重声明：所呈交的学位论文，是本人在导师指导下，进行 研究工作所取得的成果。除文中已经注明引用的内容外，本学位论文 的研究成果不包含任何他人创作的、己公开发表或者没有公开发表的 作品的内容。对本论文所涉及的研究工作做出贡献的其他个人和集 体，均已在文中以明确方式标明。本学位论文原创性声明的法律责任 由本人承担。 学位论文作者签名： 趣 1 口口呼年6 只i e l 第一章绪论 第一章绪论 1 1引言 自第一台现代机器人1 9 5 9 年诞生以来，人类就对其产生了十足的兴趣。随 着计算机的不断发展，机器人技术水平得到不断提高，机器人从人类的想象走 进人们的真实生活中，使很多的原来人类梦想的事情变成了现实。 移动机器人是机器人领域具有举足轻重作用的一大分支，是把运动能力、 感知能力和推理能力集合于一体的移动智能体。过多年的研究与发展，移动机 器人经已经逐步走向实用化，在制造业、物流业、服务业等传统领域，已经有 了不少使用移动机器人提高生产效率或者替代人工作业的实例。而在一些对国 家经济、社会、国防等领域具有重大战略意义的项目中，移动机器人的需求也 日益明显。随着人类活动范围的不断扩展，移动机器人的应用进一步扩展到星 球探测、海洋开发、军事反恐、灾难救助、危险品处理等领域，而这些领域都 存在着很大程度的不确定性，有的甚至是完全未知的新环境。因此研究未知环 境中移动机器人的探索问题，对提高移动机器人的实用性至关重要，并有着广 泛的经济和社会价值。 移动机器人在实际的应用中必须回答三个基本问题“在哪里? ，“要 去哪里? 以及“如何到达那里? i i 】，即导航的三个基本问题，换句话说导航 是驱使机器人从一个地方运动到另一个地方的科学技术，它是移动机器人的重 要研究课题，长期以来一直是移动机器人领域的活跃领域。国际上对移动机器 人自主导航的研究起步于八十年代后期，主要研究在环境地图已知的条件下， 通过定位、路径规划、路径跟踪和避障等导航过程来完成指定的任务。然而， 随着移动机器人应用范围的不断扩展，未知环境下的导航逐渐成为移动机器人 领域的备受关注的热点问题。由于环境地图未知，如何通过传感器感知环境， 并确定自身的位置便成为移动机器人导航的难点。移动机器人同时定位与建图 ( 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 2 1s l a m 问题的提出 1 2 研究背景 最早的自主导航系统出现在自动化工厂里，移动机器人通过检测在地下的 磁体或者涂在地面上的引导标志进行路线跟踪，这是一种非常简单的导航方式， 仅需要简单的感知和处理能力。一种取代路线跟踪的导航方法是通过里程计和 惯性传感器为机器人提供在环境中的位置和方向信息，但是基于里程计和惯性 传感器的估计方法具有误差积累效应，最终会导致无界的定位误差。第一次真 正成功的定位系统是通过应用灯塔实现的，依靠布置在环境中的位置已知的灯 塔，移动机器人通过传感器观测这些灯塔确定自身的位姿。随后，利用自然环 境存在的自然对象作为陆标的定位方式在很多领域的应用非常成功。例如，1 9 9 7 年美 蚕n a s a 的探路者机器人( p a t h f i n d e rr o v e r ) 登陆火星，其导航就利用了火星 地形图，旅居者( p a t h f i n d e rl a n d e r ) 拍摄探路者的视频信息，并传回地球，探路者 的图形模型被叠加在视频图像上，从而确定探路者在火星上的位置 2 1 。这种方 式，环境信息事先以先验地图的方式提供给机器人，机器人将传感器获得环境 信息与先验地图对比，利用两者的差异修正定位误差。以上所述的导航方法都 是基于环境地图已知的条件下，通过各种传感器( 里程计、超声、激光、视觉等) 来感知环境和运动，以此来进行机器人的定位的。而定位所用到已知环境地图 要么是由人工设定或测量得出的，要么是在已知机器人的绝对全局位置的条件 下，通过传感器获取环境信息，通过估计方法得出的。 对于已知地图的机器人定位和已知机器人位置的建图已经有了多种有效的 解决方法 3 1 1 4 。然而在很多环境中机器人无法利用全局定位系统进行精确定位， 而且难以事先获取工作环境的地图，特别是对于人所无法进入的危险或极限环 境，获取先验地图是不可能的。在未知环境中，由于没有先验地图，机器人无 法使用地图信息进行定位；而由于无法确定机器人的位置，不能建立环境地图。 因此在未知环境中定位与建图相互依赖、相互决定、相互耦合，从而形成“鸡 生蛋、蛋生鸡 的问题【5 】。如何借助于二者之间的相互制约关系，同时在线完 成定位与建图就成为当前移动机器人导航领域的热点问题同时定位与建图 ( 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 。 2 第一章绪论 1 2 2 研究现状 最早的研究工作始于上世纪九十年代早期s m i t hs e l f 和c h e e s e m a n 的研究工 作 6 1 ，s l a m 是当今移动机器人研究领域的热点。目前，是否具备同时定位与建 图能力已被认为是机器入是否能够实现真正自主导航的关键前提1 7 】。 近些年来，各国学者们针对各种的环境，使用不同的传感器，提出多种地 图表示形式及滤波方法来解决机器人的同时定位与建图的过程，很多工作获得 了很好的同时定位与建图的结果，并应用到机器人的实际工作中。 1 2 2 1 传感器类型 传感器是机器人感知和认识环境的工具。在s l a m 中，机器人通过超声、 激光、视觉、里程计、g p s 等设备来感知自身的运动、感知环境，并将这些传 感器信息进行融合，在建立环境地图的同时确定自身的位置。其中，里程计或 陀螺仪通常被用来预测机器人的位姿，超声、激光和视觉传感器用于观测环境 特征。传感器的使用很大程度上取决于机器人工作的环境。通常，基于超声和 激光传感器的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 研究中的热点。为了建立三维环境地图，视觉和3 d 激光传感 器被广泛应用。 1 2 2 2 环境地图表示 根据对环境表示方法的不同，机器人建立的环境地图通常分为栅格地图、 几何特征地图、拓扑地图、混合地图等多种。栅格地图的表示方法将环境划分 为大小相等的独立栅格，机器人在运动过程中利用传感器观测信息确定栅格是 否被覆盖；几何特征地图将环境定义为点、边、角、面的集合，机器人在建图 过程中通过视觉、激光等传感器探测环境，从中提取点、直线、直角、曲线等 3 第一章绪论 抽象的几何特征，用以描述环境的基本组成单元，从而构建环境地图；拓扑地 图将环境表示为节点和边的组合，节点对应环境中的一个特征状态或特征区域 ( 如走廊的交点等) ，边对应环境中连接特征状态的路径。拓扑图通常不需要机 器人准确的位置信息，因此对机器人定位误差具有较强的鲁棒性；混合地图顾 名思义是将两种或多种地图表示方法进行组合，从而实现不同地图表示方法之 间的优势互补。混合地图通常包括栅格地图和几何特征地图的混合、栅格地图 和拓扑地图的混合以及拓扑地图和几何特征地图的混合等多种方式。混合地图 由于融合了不同的地图表示方法，因此通常同时具有局部地图信息丰富而全局 地图鲁棒的优点，所以更适合于大规模环境的地图表示，但混合地图的计算复 杂度较高。 1 2 2 3s l a b 方法 现有的s l a m 主要的研究方法包括基于扩展的卡尔曼滤波器( e k f ) 的方法、 基于粒子滤波器( p a r t i c l ef i l t e r , p f ) 的方法和期望最大化方法( e x p e c t a t i o n m a x i m i z a t i o n , e m ) 等。 应用卡尔曼滤波器( k ff i l t e r ) 进行s l a m 研究最早可以追溯至u s m i t hr 1 9 8 7 年的工作【8 】，由此奠定了s l a m 问题基于概率理论架构解决的基础，至今仍广泛 应用。对于离散线性系统，卡尔曼滤波器是最t j 、- - 乘意义下的最优滤波器1 9 。 而实际的s l a m 问题很少是线性情况，因此使用扩展的卡尔曼滤波器( e k d 。在 基于e k f 的s l a m 中，将机器人的位姿和环境特征的位置组成系统状态，并假设 它们的服从高斯分布，然后将系统运动模型和传感器观测模型进行线性化，使 用卡尔曼滤波过程进行递推计算，从而得到一致的估计结果。e k f 方法是目前使 用最为广泛的s l a m 方法，但是该算法的时间复杂度为o ( n 3 ) ，空间复杂度为 o ( n 2 ) ，其中n 为环境中的特征数目。基于e k f 的一些改进或变种方法如u k f t l 0 】、 e i f i l l 】、c e k f t l 2 】等也被广泛应用于s l a m q b 。 粒子滤波器( p f ) 方法，也称为m o n t ec a r l o 方法，其核心思想是用粒子集合表示 概率分布，而不是解析的概率分布表示( 例如高斯分布) ，每个一个粒子代表一 个概率分布的采样，如果采样足够多，那么能充分表示任何分布。p f 方法已被 广泛应用于机器人定位研究中，并且表现出十分出色的处理性能。但是p f 方法 逼近概率分布需要的粒子数随分布的维数指数级增长，因此许多成功的应用只 4 第一章绪论 限制在低维状态空间的情况。然而，最近的一些研究，例如， r b p f ( r a o b l a c k w e l l i z e dp a r t i c l ef i l t e r i n g ) s l a m 方法【1 3 】和f a s t s l a m l 7 1 的方法等 方法，应用机器人问题中的条件独立特性将粒子滤波器扩展到高维情况，成功 地运用粒子滤波器解决了s l a m 问题。另外，d p 。s l a m t l 4 】等方法也给出使用粒 子滤波器的s l a m 解决方案。 在基于e m 方法的s l a m 中，s l a m 被转化为计算极大似然地图的问题 1 5 1 。 e m 算法通过两个步骤实现同时定位与建图，分别是期望步骤( e x p e c t a t i o n ) 或者e 步骤和最大化步骤( m a x i m i z a t i o n ) 或者m 步骤。在e 步中根据当前极大似然地图计 算机器人位姿估计，在m 步中基于e 步得到的机器人位姿计算新的极大似然地 图，e 步和m 步交替进行，从而使得机器人位姿估计和极大似然地图不断更新。 e m 方法比扩展卡尔曼滤波器方法的优点在于能够处理s l a m 中的数据关联问 题，不需要陆标能够唯一识别，而且能够估计复杂的后验概率分布，不像e k f 方法要求高斯分布的假设。但是，e m 算法的运算过于复杂，难以满足实时计算 的要求。 1 2 2 4 应用环境 移动机器人工作的环境按环境结构化信息来划分，可以分为结构化环境和 非结构化环境。结构化环境是指相对简单的人造环境，以水平直线和竖直直线 为轮廓，是一种移动机器人应用的理想环境，例如室内环境和室外楼宇环境等。 目前，s l a m 的研究主要面向于二维结构化环境。在结构化环境中，移动机器人 能够通过激光、超声提取环境中陆标【1 6 1 或直线段 1 7 1 1 8 】【1 9 1 的二维位置信息，并对 它们进行估计，同时利用它们完成自身三维位姿的估计。随着s l a m 研究的不断 深入和技术水平的不断提高，三维结构化环境也开始成为s l a m 研究的重要对 象。在三维环境中，机器人通过视觉或三维激光传感器提取三维空间的特征点 或线段，并对它们的三维位置进行估计，同时完成自身位姿的估计 2 0 】【2 l 】【2 2 1 【2 3 1 。 非结构化环境是指自然环境或十分混乱的人造环境，例如山地、水下、星 球表面和灾后废墟等。非结构化环境相对于结构化环境具有不确定性、不稳定 性和不可预知性，为移动机器人同时定位与建图带了不少难题。例如在水下环 境很难提取特征点或线，且机器人以六自由度方式运动，同时暗流、水下生物 等有可能影响机器人的运动( 或称绑架，是指由突然外力改变机器人的位姿) 或 5 第一章绪论 改变环境，这些都会影响机器人的同时定位与建图工作。尽管如此，目前一些 学者已经开始对在非结构化环境进行同时定位与建图进行研究，并取得了不错 的成果1 7 1 1 2 4 】【2 5 1 。 1 2 3 现存主要问题 目前，s l a m 已成为未知环境下移动机器人导航的主要研究内容。尽管有 关s l a m 的研究已取得了一些研究成果，但是在s l a m 的研究和应用方面，尚 存在一些局限性和有待解决的问题。 ( 1 ) 单一传感器。现有研究通常只使用一种传感器进行s l a m 。多数s l a m 的研究工作只使用激光作为感知环境的唯一传感器建立二维环境地图。但是激 光传感器感知分辩能力低，在高度混乱或者拥挤的环境很难提取点特征和直线 特征，且很难建立环境的三维地图。而基于视觉的s l a m 方法能够较好的解决 数据关联问题，并能够构建三维地图，但是使用视觉传感器又引入了其他一些 问题，如视觉特征深度估计问题。而且视觉s l a m 通常建立的地图为特征点地 图，很难被人类理解，且很难用于避障和路径规划。 ( 2 ) 多传感器相对独立使用。在多传感器s l a m 的研究中，尽管同时使用激 光和视觉等传感器，但是由于激光和视觉提取的特征不同，通常只是在同一 s l a m 方法框架下独立的处理激光特征和视觉特征，没有将激光和视觉信息充 分融合。 ( 3 ) 运动方式非主动。现有研究方法中机器人通常以随机方式或按跟踪预先 指定路径的方式探索环境，进行环境定位与建图，忽略了机器人根据实时的环 境主动选取运动方式的问题。 另外，数据关联、实时性及动态环境等问题也是s l a m 研究中的难点，有 待解决。 综上所述，目前有关移动机器人同时定位与建图的研究尚存在很多方面的 不足，而这些不足阻碍了s l a m 研究成果迈向实用化的进程，因此也成为s l a m 研究所必须解决的问题。 6 第一章绪论 1 3 本文动因 目前，移动机器人已经开始应用到人们的生活中，例如反恐防爆机器人已经 应用到爆炸物的查找和拆除；安保和向导机器人也已经在一些图书馆和博物馆 执勤；家庭服务和娱乐机器人也开始走近人们的生活。以上所述的这些机器人 作业的环境通常是室内或室外楼宇环境。尽管这些结构化环境对人类“已知 ， 但是人很难将对环境的认知“传授”给这些机器人，因此对于机器人来说仍然 是未知环境。为了完成导航和更高级的任务，机器人必须首先对环境建图。因 此，在这类结构化环境中，同时定位与建图成为机器人自主作业的基础。 本文将针对结构化环境，围绕移动机器人如何使用激光和单目视觉传感器进 行同时定位与建图问题展开研究工作。本文将在激光和视觉单独使用的基础上， 提出激光和视觉信息融合的s l a m 方法，并采取主动策略，提高s l a m 的鲁棒 性和准确性。 1 4 本文主要内容及目标 本文的研究领域是移动机器入导航，本文关注的问题是结构化环境下移动 机器人使用多传感器进行同时定位与建图，本文的目标是提出结构化环境下基 于激光和视觉信息融合的主动s l a m 方法，提高s l a m 的鲁棒性和准确性。 本文将对基于激光和单目视觉的s l a m 方法进行深入研究，针对结构化环 境提出激光和视觉信息融合的主动s l a m 方法。具体讲，本文将在以下三个方 面开展研究工作： l 、基于激光角点特征的s l a m 。本文将提出在结构化环境下激光角点特征 的表示、提取、匹配的方法和把激光角点特征作为陆标进行s l a m 的方法。 2 、基于竖直线特征的单目视觉s l a m 。为了解决单目视觉无法通过一次观 测获得特征距离信息的问题，本文将使用竖直线特征的逆深度表示来描述特征 的状态，并提出基于竖直线特征的单目视觉s l a m 方法。 3 、基于激光和单目视觉信息融合的主动s l a m 。为了将激光和视觉信息进 行融合，本文将使用激光和视觉观测同一特征，将观测的信息进行融合来得到 更加准确的观测。另外，根据当前s l a m 系统的状态及其不确定性，采取主动 的策略减小系统的不确定性，提高同时定位与建图的准确性。 7 第一章绪论 1 5 本文组织结构 本文共分为六章，各章节内容和结构安排如下： 第一章是绪论，概括介绍本文的研究背景、研究内容和研究目标。 第二章为相关工作的概述，对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 方法，并给出实验。 第六章是总结与展望，总结本文的工作，并展望下一步的研究工作。 8 第二章相关工作综述 第二章相关工作概述 本文的研究得益于移动机器人同时定位与建图领域研究成果，本章详细介 绍与本文相关的研究工作和成果，包括s l a m 的数学定义、s l a m 的贝叶斯估 计、基于扩展卡尔曼滤波的s l a m 方法，作为后续章节工作的基础。 2 1s l a b 简介 机器人代替人在极限或者危险环境下作业已经显示出了巨大的吸引力，例如 在人类难以存在的星际环境、深海环境、以及高度危险的煤矿环境、高压电站、 高度放射或者污染的环境等。 s l a m 作为一种实现移动机器人真正自治的关键技术，对危险或者极限环境 下工作的机器人具有重要的意义，它可以使得机器人的探索不再或者很少需要 人的干预，真正面向未知环境作业。在s l a m 研究的二十年里，取得了可喜的成 绩，其中s l a m 的应用领域也在不断扩大。s l a m 的主要优势是排除了对人工基 础设施或者环境的先验拓扑知识的需要1 1 6 1 。 2 4 】成功将s l a m 技术应用于煤矿环 境的建图，如图2 1 ( a ) ，【2 5 首次将s l a m 技术应用于水下机器人，如图2 1 ( b ) ， 2 6 将s l a m 技术应用于可穿戴视觉或者机器人，使得可穿戴视觉中的重定位问 题不再需要先验地图或者人为标记，如图2 1 ( c ) 。【2 7 将s l a m 技术应用于无人 飞机( u n m a n n e d a e r i a lv e h i c l e s ，u a v s ) 的自主导航中，使得u a v 可以不需要g p s ， 陆标或者地图的先验信息，如图2 1 ( d ) 。 2 1 1问题描述 同时定位与建图是指在未知环境中，机器人使用带有噪声的传感器获得自 身的运动和环境信息，通过滤波算法估计自身的未知状态和环境特征的状态， 以此来完成同时定位与建图的过程。 9 第二章相关工作综述 ( a ) 煤矿环境应用r b l 水下应用 0 ) 可穿戴视觉室内环境应用 ( d ) 无人机空中应用 图21 s l a m 技术的各种应用 212 s l a m 的数学定义 在s l a m 中，系统的状态空间是包含机器人相对于环境的位姿状态和描述 环境模型的陆标状态，这里用五表示t 时刻的系统状态。由于状态变量不能直接 观测，机器人需要来自内外部传感嚣的信息进行状态估计。为方便起见，假设 机器人收集的信息构成如下两个集合： z 。= z l ，毛， ( 2 1 ) 2 “，叱，虬) ( 22 ) 其中表示f 时刻的传感器观测，虬表示【卜1 ，f ) 时间段施加的控制输入。s l a m 的目的就是在给定传感器观测一集合和控制输入一的集合的情况下估计t 时刻 状态变量的后验概率分布： 第二章相关工作综述 p ( 五f z t “) ( 2 3 ) 对于移动机器人，s l a m 的状态变量包括机器人位姿和环境陆标位置状态两 部分，如果用表示机器人t 时刻的位姿，所，表示t 时刻的环境地图，那么，上 述后验概率分布可以重新表达为： p ( ，m t z tu ) ( 2 4 ) 如果假设世界模型是静态的，那么环境地图不随时间变化，地图的时间索引 可以省去，( 2 4 ) 式变为： ，( 墨，聊jz t “)( 2 5 ) 2 1 3 s l a m 问题的贝叶斯估计 贝叶斯滤波器是概率推理的原型，事实上是每一个成功s l a m 算法应用的 基本原则。贝叶斯规则使得根据观测d 推断状态石的后验概率通过下式成为可 能。 p ( x id ) = r l p ( d i 工) p ( 力 ( 2 6 ) 其中p ( d i 功表示在假设x 下观测到d 的概率，p ( d f 工) 是产生式模型，它描述在 不同的x 下获得传感器观测的过程。p ( x ) 称为先验概率，7 是规一化参数，确保 p ( x i d ) 是合理的概率分布。p ( x l d ) 称为后验概率，它根据证据d 更新先验概率。 对( 2 5 ) 式应用贝叶斯规则，可得： p ( t ，n lz tu 。) = r i p ( z , i 墨，m ，z t - i , u 。) p ( & ，mz t - i “) ( 2 7 ) 根据马尔可夫假设，观测互只与t 时刻的机器人位姿s ，以及地图所有关，从 而p ( z rt ，m ，z 卜1 ，“) 可以简化为p ( 乙i ，m ) ，因此后验概率分布( 2 5 ) 变为： p ( q ，m i z t9 u ) = r i p ( z , i 墨，所) p ( 墨，所i z t - i ) ( 2 8 ) 对( 2 8 ) 式p ( s ，圳z t - iu 。) 应用全概率公式以及条件概率定义公式，可得： p ( 最，竹j2 t - i “) = i p ( 岛，咒js , - l , z 卜1 ，“) p ( 墨一llz 卜1 ，“。) 啦一1 = p ( 墨i 聊，- l ，7 ，弘) p ( mls , 书z t - l , u t ) p ( 一。z t - i , u t ) 幽一l ( 2 9 ) 根据马尔可夫假设，岛只是一。和控制输入坼的函数，从而可得： 第二章相关工作综述 p ( s t ，m z t - l u t ) - - p ( s t 叩u , ) p ( ms , 州z t - i ，“t ) p ( s 川 z t - l , u t ) d s t 一。 = r e ( s t s t 中u t ) p ( s , _ 1 , mz t - l , u ) d s , 一。 2 1 0 综合( 2 7 ) ，( 2 8 ) ，( 2 10 ) 式，最终s l a m 的后验分布表达为： p ( s ，m l z t , u t ) = 刁p ( 乙l j ，m ) ，p ( s tl 一。，“，) p ( t l ，z l z t - i ，l l t ) 幽一。 ( 2 1 1 ) 上式p ( s ts t - ! u ，) 称为机器人运动模型，它表示机器人t - 1 时刻执行控制输入 “，后位姿为j ，的概率。p ( z ，i _ ，z ) 称为观测模型，它物理上对传感器建模，在地 图，z 和机器人位姿已知的假设下，观测模型表示五被观测到的概率。 2 2 扩展卡尔曼滤波方法 2 2 1扩展的卡尔曼滤波器 卡尔曼滤波器( i ( f ) 方法假设系统状态服从高斯分布，因此可以使用高斯分布 的均值和协方差矩阵来描述系统状态，并通过预测和更新过程对系统状态进行 估计。k f 方法是最d , - 乘意义下最优的状态估计方法，常用于线性系统的状态 估计。通过将非线性系统泰勒展开进行线性化，扩展的卡尔曼滤波器e k f 将k f 方法推广到非线性系统。由于移动机器人运动模型是非线性系统，因此通常使 用e k f 进行处理。 由于e k f 假设系统状态服从高斯分布，因此使用系统状态和其协方差矩 阵p 描述。下面，首先给出系统模型。 假设非线性系统的状态模型为： x ( k + 1 ) = ( x ( 七) ，甜( 七) ，v ( 露) )( 2 1 2 ) 其中，x ( k ) 表示k 时刻的系统状态，u ( k ) 为系统的控制输入，v ( k ) 为零均值高 斯白噪声，其协方差为q ( k ) ，且有层( v ( 七) ) = 0 ，e ( v ( 七) v ( 后) r ) = q ( k ) 。 假设系统的观测模型为： z ( k + 1 ) = h ( x ( k + 1 ) ) + w ( k + o ( 2 1 3 ) 其中，z ( k + 1 ) 表示k + l 时刻的系统的观测，w ( g + 1 ) 为零均值高斯白噪声，其协 1 2 第二章相关工作综述 方差为尺( 尼+ 1 ) ，且有e ( “七+ 1 ) ) = 0 ，e ( w ( 尼+ 1 ) 以后+ 1 ) 7 ) = r ( 七+ 1 ) 。 e k f 估计包括预测和更新两个过程。 ( 1 ) 预测过程。由k 时刻的系统状态预测k + l 时刻的系统状态j ( 露+ 1i 七) 、 协方差矩阵户( 尼+ 1i k ) 及系统状态的观测值2 ( 尼+ 1 ) x ( 尼+ 1 悻) = 厂( x ( 后肛) ，“( 后) ，o ) ( 2 1 4 ) 户( 后+ 1 肛) = v f 户( 五陋) v r x f + v f g ( 七) v z s ( 2 1 5 ) 2 ( 后+ 1j 后) = j l ( j + 1j 尼) ) ( 2 1 6 ) 其中 厂= 飘乩。) 为系统模型厂对肌i 啪雅可比矩阵一厂表示 系统模型对“( 后) 的雅可比矩阵。 ( 2 ) 更新过程。k + 1 时刻，获得系统状态的观测值z ( 尼+ 1 ) ，利用z ( 后+ 1 ) 对 系统状态进行更新。 j ( 七+ 1 i 尼+ 1 ) = j ( 七+ 1 i 七) + k ( 后+ 1 ) y ( 尼+ 1 ) f 2 1 7 ) p ( k + 1i 后+ 1 ) = ( ，一k ( k + i ) v z h ) p ( k + l i 七) ( 2 1 8 ) r ( k + 0 = z ( 七+ 1 ) z ( k + 1k ) ( 2 1 9 ) s r ( k + 1 ) = v x h 户( 后+ 1 i 尼) v 互 + 月( 七+ 1 )( 2 2 0 ) k ( 后+ 1 ) = p ( 七+ 1 i 后) v 工h s r ( k + 1 ) 。1 ( 2 2 1 ) 其中，v x h 为观测模型对系统状态j ( 后+ 1 k ) 的雅可比矩阵，厂( 后+ 1 ) 被 称为新息，s r ( 露+ 1 ) 为其协方差矩阵，k ( 足+ 1 ) 被称为卡尔曼增益。 2 2 2 基于扩展卡尔曼滤波器的s l a m 方法 基于e i 让的s l a m 中，将机器人的位姿置和环境特征( 或陆标) 的位置以定 义为系统状态，并假设系统状态服从均值为j 、协方差为户的高斯分布。即 r 1 l 广 1 ， x = 研柳2 l - t 以j2 l - t kj ( 2 2 2 ) 1 3 第二章相关工作综述 户= 研( x 一膏) ( x j ) r 】= 0 厶 pp 砷r砷啊 pp m ，”k 咱 尸 * r a n & p 一k 口 ( 2 2 3 ) 其中，t 表示第f 个特征的状态，已，表示机器人位姿与第f 个特征位置的相关 性，厶。，= 己i 码表示第i 个特征位置与第个特征位置的相关性。 假设机器人的运动学模型毫( 尼+ 1 ) = 厂( 毫( 后) ，“( 后) ，v ( 尼) ) 的噪声，( 七) 服从零 均值、协方差为q ( k ) 的高斯分布。由于假设静态环境，因此环境特征的位置不 变，所以疋( 尼+ 1 ) = 毫( 七) 。传感器的观测模型2 ( 尼+ 1 ) = ( j ( 后+ 1 ) ) + w ( 七+ 1 ) 的 噪声以后) 服从零均值、协方差为r ( k ) 的高斯分布。机器人使用传感器得到实际 的观测值z ( k + 1 ) ，通过e k f 的更新过程对机器人和环境特征的状态进行更新。 基于e k f 的s l a m 就是不断地迭代进行预测更新过程来同时确定机器人的 位姿和环境特征的位置的。 2 2 3 基于e k _ f 的s l a m 方法的评价标准 由于e k f - s l a m 假设系统状态服从高斯分布，因此可以使用机器人位姿协 方差矩阵的椭球体积和特征( 或陆标) 位置的椭球体积作为s l a m 不确定性的量化 评价标准【2 8 】，即： c ( kk ) = c ( 已( 后l 七) ) + 1 ，z c ( & 竹( 尼i 尼) ) i = 1 = 万兀厕韧n 兀0 万丽( 2 2 4 ) i i = l j = 万厕+ n 刀厢 i = l 其中，五( p ) 表示p 的第f 个特征根，c ( 0 ( 七怍) ) = 万d e t ( 名) 表示机器人位置的 不确定性，c ( & 。( 七陋) ) 表示第f 个特征的不确定性。c ( 尼降) 越小，表示整个系 统的不确定性越小，认为s l a m 结果越准确。另外，s l a m 系统状态的估计值 与真实值的差值，即误差，是否在由协方差矩阵确定的9 5 假设检验区间内， 是判定s l a m 过程是否一致的一种标准。 1 4 第三章基于激光角点特征的s l a m 第三章基于激光角点特征的s l a m 激光传感器被广泛使用在移动机器人的定位、建图和导航中。基于激光传 感器的s l a m 研究大部分针对室内环境，提取角点或线段等环境的结构化信息作 为陆标。本章针对结构化室内环境，提出一种点和线段混合的角点特征表示， 在原有基于点特征观测的基础上，提出角点和线段混合的观测更新方法，增加 角点特征被观测更新的次数，提高s l a m 的准确性。另外，应用一种改进的运动 学模型【z 引，提高了s l a m 的准确性和鲁棒性。 3 1 特征表示和提取方法 3 1 1角点特征的表示 所谓角点特征，这里是指在二维结构化环境中的直角点，如墙角等特征。 其状态包括角点的二维坐标和由角点的边与坐标系的夹角表示的方向。角点特 征在世界坐标系形下的状态可以由一个直角坐标系f 的状态表示，坐标系f 的 原点为角点特征在世界坐标系形下二维坐标，坐标系f 的x 轴与世界坐标系形 的x 轴的夹角表示角点的方向。角点特征状态为 x 7 = i y ；彤1 1 ( 3 1 ) 其中砖表示b 坐标系在彳坐标系下的状态，( 弗，谬) 为角点坐标系，的原点， 群为角点坐标系f 与世界坐标系形的夹角，取值范围为( 一万，万】。而机器人在 世界坐标系矽下的状态也可以由一个直角坐标系j r 的状态表示 碟= i j ，置硝i ( 3 2 ) 则角点坐标系f 在机器人坐标系尺下的状态为 群= 群。夥= ( 噼) o 碟( 3 3 ) 其中0 为复合运算， 为逆运算，其定义如下 1 5 第三章基于激光角点特征的s l