（通信与信息系统专业论文）基于视觉的机器人同时定位与地图构建.pdf

摘要 定位与地图构建是自主机器人研究必须解决的关键问题。但是单纯使用基于 航迹推演方法存在累积误差，使得地图构建的致性遭到破坏。为此，希望利用自 然环境路标的多次观测具有高度一致性的特点，通过对定位和路标地图的同时估 计( s l a m 获褥更高的定位及路标位置估计精度。s a m 在随机过程框架内，通 过概率推演的方法实现机器人状态与环境路标状态的同时估计。视觉信息较其他 传感方式具有信息量大、成本低、能耗小及直观性好等特点，特别适合诸如外星球 导航的s l a m 任务。同时，近年来视觉s l a m 逐渐兴起，逐步开始从室内应用扩展 到窒岁 应雳，扶二维扩展到三维，越发接近实际应用效果。本论文将对如何翻爱视 觉信息实现机器人s l a m 问题展开研究。主要研究内容包括： 首先阐述s l a m 问题的贝叶斯滤波理论基础。为避免全状态估计，将通 过r a o - b l a c k w e l l i s e d 分解实现机器人状态与路标状态的解耦，并给出该分解的证 明过程；阐述粒子滤波器的相关基础瑾论，并给出最基本的粒子滤波算法；给出 r a o - b l a c k w e l l i s e d 粒子滤波器实现s l a m 的种有效算法：f a s t s l a m 算法的具体 实现过程。 然后磅究s i f t 特征点的提取过程，得照静近似的s i f t 特惩提取方法；针 对s i f t 全局搜索效率低下的问题，发展一种基于k d - t r e e 的近似最临近搜索方法， 用以提高匹配速度；提出一种有效的s i f t 特征管理方法，用以有效分割予地图及 特征维护。 其次季| l 震立体逛配得到的s i f t 特妊作力鸯然环境路标，发展一种基于双雷视 觉的机器人s l a m 方法。在提出双目视觉s l a m 的算法的框架后，基于前后两幅 立体图像匹配信息，提出一种鲁棒的视觉里程计方法；以该里程计输出作为s l a m 初始运动控制参数，在f a s t s l a m2 0 算法框架内，逐步完成粒子预测提取，路标更 薪及权值计算等一系列关键步骤的实现。 再次针对单阏视觉s l a m 问题遇到的特征点增多及运动不确定性增加等特 点，发展一种基于高斯混合模型( g m m ) 及密度估计的粒子预测提取方法，用以提 高傣计效率与精度：利用g m m 对环境路标避行建模，发展静只需要维护一个高 新混合型路标地图的、基于r a o - b l a c k w e l l i s e d 粒子滤波器的s l a m 算法。对该算 法中所需的粒子预测提取，后验状态密度估计，权值计算，路标更新等步骤给予详 细讨论，并给出具体实现方法。 摘要 最后是全文的总结和展望。 关键词：机器人，视觉s l a m ，粒子滤波器，r a o - b l a c k w e l l i s e d 分解，视觉里糕计， s i f t ，特铤地嚣管理，赢斯混合模型，密度佶计 a b s t r a c t l o c a l i z a t i o na n dm a p p i n ga r et h et w ok e yp r o b l e m sn e e d e dt ob es o l v e df o r a u t o n o m o u sr o b o t b u tt h el o c a l i z a t i o na p p r o a c ho n l yb a s e do nd e a dr e c k i n gh a s a c c u m u l a t i o ne r r o r ，a n dm a k et h ec o n s i s t e n tc h a r a c t e r i s t i co fm a p p i n gb e e nd e - s t r o y e d b e c a u s et h em u l t i p l eo b s e r v a t i o n sh a v eh i g hd e g r e eo fc o r r e l a t i o n ，a n dw e w i s ht ou s et h i si n f o r m a t i o nt oc o n s t r a i nt h ee r r o ra c c u m u l a t e d ，t h u so b t a i nm o r e a c c u r a t el o c a l i z a t i o na n dl a n d m a r k s p o s i t i o n i nt h es t o c h a s t i cp r o c e s sf r a m e w o r k ， s l a mu t i l i z e sp r o b a b i l i s t i ci n f e r e n c et e c h n o l o g yt or e a l i z es i m u l t a n e o u sl o c a l i z a t i o n a n dm a p p i n ge s t i m a t i o np r o c e s s m e a n w h i l e ，v i s i o ni n f o r m a t i o nh a st h ea d v a n t a g e s o fm u c hq u a n t i t y , l o wc o s t ，m i n i m a le n e r g yc o n s u m p t i o na n di n t u i t i v ee f f e c t s ，w h i c h p a r t i c u l a r l ys u i t a b l ef o rt h es l a mt a s ko fo u t - s p a c ep l a n e te x p l o r a t i o n i nr e c e n t y e a r s v i s i o ns l a mh a sb e e nr a i s e dg r a d u a l l y , i t sa p p l i c a t i o n sh a sb e e ne x p a n d e d f r o mi n d o o re n v i r o n m e n tt oo u t d o o re n v i r o n m e n t ，3 d o fe s t i m a t i o nt o6 d o fe s t i m a t i o n 。a n dm o r ea n dm o r ec l o s et op r a c t i c a la p p l i c a t i o n t h i sd i s s e r t a t i o nw i l l d i s c u s st h ep r o b l e mo fh o wt ou s ev i s i o ni n f o r m a t i o nt or e a l i z er o b o ts l a m t h em a i nr e s e a r c hc o n t e n t si n c l u d e ： f i r s t l y , t h et h e ! o r e t i c a ls o l u t i o nf o rs l a mp r o b l e mb a s e do nb a y e sf i l t e r i s p r e s e n t e d 。i no r d e rt oa v o i dt h ef u l l - s t a t ee s t i m a t i o n ，r a o - b l a c k w e l l i s e df a c t o r i z a - t i o ni su s e dt od e c o u p l et h et h ee s t i m a t i o np r o c e s so fr o b o ta n dl a n d m a r ks t a t e s p a c e ，m e a n w h i l eas u c c i n c tp r o o fo ft h i sf a c t o r i z a t i o ni sp u to u t ；t h eb a s i ct h e o r y o fp a r t i c l ef i l t e ri si n t r o d u c e d ，a n dt h eb a s i cp a r t i c l ef i l t e ra l g o r i t h mi sp r e s e n t e d ； t h ed e t a i l e dr e a l i z a t i o np r o c e s so ff a s t s l a ma l g o r i t h mi sp r e s e n t e d ，w h i c hi sa n e f f i c i e n ta p p r o a c ht os o l v es l a mp r o b l e mb a s e do nr a o b l a c k w e l l i s e dp a r t i c l ef i l t e r s e c o n d l y , t h ee x t r a c t i o np r o c e s so fs i f tf e a t u r ei sd i s c u s s e d ，a n da na p p r o x - i m a t es i f te x t r a c t i o nm e t h o dw i l lb ed e v e l o p e d 。a g a i n s tt h el o we f f i c i e n c yc h a r - a c t e r i s t i co fs i f t sg l o b a lm a t c h i n g ，a na p p r o x i m a t en e a r e s tn e i g h b o r ss e a r c h i n g a l g o r i t h mb a s e d0 nk d - t r e ew i l lb ed e v e l o p e d i no r d e rt oe x t r a c ts u b - m a pe f - f e c t i v e l ya n dm a i n t a i nt h ef e a t u r em a pe f f i c i e n t l y , a ns i f tf e a t u r em a n a g e m e n t m e t h o dw i l lb ea d v a n c e d 。 t h i r d l y , t h r o u g hu s i n gt h e s t e r e om a t c h e ds i f tf e a t u r e sa sn a t u r a ll a n d m a r k s ， o n es l a ma l g o r i t h mb a s e do n l yo nb i n o c u l a rv i s i o ni n f o r m a t i o nw i l lb ed e v e l o p e d a f t e rp u t t i n go u tt h ef r a m e w o r ko fb i n o c u l a rv i s i o ns l a m w i t ht h ep r e v i o u sa n d a b s t r a c t p r e s e n ts t e r e om a t c h e di m a g ei n f o r m a t i o n ，o n er o b u s tv i s u a lo d o m e t r ya l g o r i t h m w i l lb ed e v e l o p e d ，w h i c hw i l lb eu s e da sc o n t r o li n p u t so fs l a mp r o c e s s t h e n i nf r a m e w o r ko ff a s t s l a m2 0 ，t h es p e c i f i cr e a l i z a t i o n so fd r a w i n gs a m p l e sf r o m p r o p o s a ld e n s i t y u p d a t i n gt h el a n d m a r kp o s i t i o n ，p a r t i c l e sw e i g h tc o m p u t i n g a r e d i s c u s s e dd e t a i l e d l y f o u r t h l y ，a g a i n s tt h ee n c o u n t e r e df e a t u r en u m b e ra n di n i t i a lm o t i o nu n c e r - t a i n t yi n c r e a s e dp r o b l e m sf o rm o n o c u l a rv i s i o ns l a m ，i no r d e rt oi n c r e a s et h el o - c a l i z a t i o na c c u r a c ya n dc o m p u t a t i o n a le f f i c i e n c y ，an e wp r o p o s a ld e n s i t yw i t hg a u s - s i a nm i x t u r em o d e l ( g m m ) a n dd e n s i t ye s t i m a t i o np r o c e s sf o rr 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 rw i l lb ed e v e l o p e d t h ep o s t e r i o rd e n s i t yo fl a n d m a r kp o s i t i o nw i l l b em o d e l e da sg m m ，t h u san e ws l a ma l g o r i t h mb a s e do nr 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 rw i l lb ed e v e l o p e d ，i nw h i c ho n l yo n eg m m l a n d m a r km a pn e e dt ob e m a i n t a i n e d t h e na l lt h er e a l i z a t i o np r o c e s ss u c ha sd r a w i n gs a m p l e sf r o mp r o p o s a l d e n s i t y , p o s t e r i o rd e n s i t ye s t i m a t i o n ，p a r t i c l e sw e i g h tc o m p u t a t i o na n dl a n d m a r k u p d a t ea r ed i s c u s s e dd e t a i l e d l y l a s t l yt h ec o n c l u s i o na n dp e r s p e c t i v ea r eg i v e na tt h ee n do ft h ed i s s e r t a t i o n k e y w o r d s ：r o b o t ，v i s i o nb a s e ds 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 ，v i s u a l o d o m e t r y , s i f t ，f e a t u r em a n a g e m e n t ，b i n o c u l a rv i s i o ns l a m ，m o n o c u l a rv i s i o n s l a m ，g a u s s i a nm i x t u r em o d e l ，d e n s i t ye s t i m a t i o n 8 s t s l m ) 8 t ，( m ) 巩 e t 磊 名 名t n t 佗# 几 u t u t p ( 茁) p ( zi 可) b e l ( x ) p ( s t8 t 一1 ，u t ) 8 t ，0 p ( 2 ；tt ，t t t 、)t ，t ， 6 ( ) g ( ) e x 】 叫5 m ) r t 主要符号对照表 t 时刻摄像机状态，包括摄像机位置及姿态角 从1 时刻到t 时刻的摄像机状态轨迹 t 时刻粒子滤波器中第m 个粒予的状态 粒子滤波器中第m 个粒子所表征的摄像机轨迹 第k 个关联的自然环境路标位置， 钆= ( 茁七，c k ) ，z 为均值，c 为协方差 t 时刻的路标地图，即到t 截止，累积获得的环境路标集合 t 时刻的观测 t 时刻观测的第k 个目标( 元索) 从1 时刻到t 时刻观测总和 石t 与e t 的关联结果 石# 与e t 的关联结果 从1 时刻到t 时刻数据关联结果总和 t 时刻的控制输入，即t 时刻初始运动估计输出( r o ，t o ) 1 时刻到t 时刻控制输入总和 变量茁的概率密度分布 变量刃关于变量可的条件概率密度 变量z 的后验概率密度分布 运动模型 观测模型 d i r a c 脉冲函数 建议分布( 粒子预测分布) 变量z 的数学期望 t 时刻粒子滤波器第m 个粒子的权值 旋转 平移 主要符号对照表 m ，v q t 0 t f e 嘲 k ( 忍，t 。) a f ( x ；p ，) 厂( s t 一1 ，地) h ( s t ，臼) p g i 。x ) 、 s l a m s f m d o g l o g k f e k f s p k f s p p f g s f g m m 摄像机投影矩阵 分布服从于 运动控制噪声的协方差矩阵 观测噪声的协方差矩阵 基础矩阵 本质矩阵 向量t 的反对称矩阵 摄像机内参 双目摄像机外参 变量z 服从均值为肛，协方差为的高斯分布 系统函数 观测函数 变量z 的概率密度分布为高斯混合模型 同时定位与地图构建( 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 t r u c t u r ef r o mm o t i o n d i f f e r e n c eo fg a u s s i a n l a p l a c eo fg a u s s i a n 卡尔曼滤波器( k a l m a nf i l t e r ) 扩展卡尔曼滤波器( e x t e n d e dk a l m a nf i l t e r ) s i g m a - p o i n t 卡尔曼滤波器( s i g m ap o i n tk a l m a nf i l t e r ) s i g m a - p o i n t 粒子滤波器( s i g m ap o i n tp a r t i c l ef i l t e r ) 高斯混合滤波器( g a u s s i a ns u mf i l t e r ) 高斯混合模型( g a u s s i a nm i x t u r em o d e l ) 关于学位论文使用授权的说明 本人完全了解浙江大学有关保留、使用学位论文的规定，即：学校有权保留送 交论文的复印件，允许论文被查阅和借阅；学校可以公布论文的全部或部分内容， 可以采用影印、缩印或其他复制手段保存论文。 ( 保密的论文在解密后应遵守此规定) 作者签名： 日期： 导师签名： 日期： 第一章绪论 本章就机器人同时定位与地图构建( s l a m ) 问题展开综述性质的讨论。首先 阐述研究机器人s l a m 问题背景及研究意义；然后就当前国内外相关技术的发展 现状及趋势展开讨论；其次对视觉s l a m 实现相关技术展开研究；最后提出本文研 究内容及论文章节安排。 1 1 引言 定位与地图构建是自主机器人导航中必须解决的两大关键问题 1 】，它的估 计精度将对后续路径规划与运动控制产生巨大影响。一旦定位或地图构建出现问 题，会使得机器人自主运动遭遇潜在威胁，甚至产生灾难性后果，如与障碍物碰撞， 或跃入沟壑发生翻车等事故。机器人定位是指”w h e r ea mi ? ”的问题，指机器人 运动过程中，如何通过传感器自主感知、确定自身的位置与姿态角。机器人地图构 建是指w h a td o e st h ew o r l dl o o k sl i k e ? 问题，指如何感知环境，以完成识别目标 及检测障碍物等任务。虽在地球上可利用全球定位系统( g p s ：g l o b a lp o s i t i o n i n g s y s t e m ) 获得绝对位置信息，但在诸如矿井、海底、外星球、室内及室外植被与 建筑物覆盖密集的区域，g p s 将不能或不能实时提供定位信息。而基于惯性测 量系统( i m u ：i n e r t i a lm e a s u r e m e n tu n i t ) 、轮盘编码器、电子罗盘( d c ：d i g i t a l c a m p a s s ) 等只能提供相对位移或姿态角变化信息，往往存在累积误差，特别是存 在打滑或颠簸严重的情况下，该种基于航迹推演( d e a dr e c k i n g ) 方法使得定位误 差不断积累，从而给后续地图构建与路径规划带来致命影响。对地面机器人而言， 当单独一种传感器不能有效提供定位信息，组合定位成为一种选择，即利用g p s 、i m u 、d c 等传感器，通过卡尔曼( k f ：k a l m a nf i l t e r ) 等工具完成组合滤波， 以获得传感器观测的一致性来减少误差 2 。但该减少累积误差的方法严重依赖 于各种传感器的互补性，同时受到各种因素的制约，如在室内机器人定位问题，首 先g p s 不可用，另外，当前价格因素使得小型及成本控制型机器人在室内应用得 到限制。另外，在月球等外星球环境下，目前还没有可用的卫星定位系统，而松软 的月球表面使得打滑现象十分严重，单纯依靠惯导定位存在严重累积误差 4 】。美 国国家航空航天局( n a s a ：n a t i o n a la e r o n a u t i c sa n ds p a c ea d m i n i s t r a t i o n ) 开发 的”勇气号”和”机遇号”火星车，都采用了视觉里程计技术。视觉里程计指的是利用 视觉信息，估计帧与帧之间摄像机的平移与旋转，经过特征提取，特征跟踪匹配，运 动估计，运动修正等步骤完成计算。火星车上面利用该技术是为了有效修正因打 第一章绪论 滑而引起的误差漂移，但受到计算能力的影响，往往偶尔使用一下视觉里程计修正 误差。但当下一代火星车 3 】将速度提高到2m s 左右，单纯使用视觉里程计也会 带来误差积累问题，使得定位出现不可靠问题。而地图构建依赖定位，即多帧环境 传感信息依赖定位进行配准或叠加。当以上绝对或相对定位方式不能有效约束累 积误差时，先定位后地图构建的方法将变得不可行，必须考虑新的定位与地图构建 方法。基于此，希望通过利用自然环境信息，即多次观测具有高度相关性这一特点 约束定位误差。但自然环境信息的确定往往由定位信息和观测值共同确定。即定 位需要地图，而地图构建也需要定位信息。 l e a n a r df 1 1 等人首先提出同时定位与地图构建( 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 ；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 t i o n ) 方法，为机器人 定位与地图构建开辟新的研究方向。它通过建立一全状态滤波器，同时估计机器 人定位信息与环境特征信息，以获得最小的一致性误差。s l a m 的基本目的为利 用多次观测特征具有高度相关性这一特点，有效约束定位的累积误差，使得观测同 时与机器人状态( 包括位置与方位) 和构建的环境特征地图吻合。因此，s l a m 首 先遇到的问题便是如何提取自然环境特征并加以利用。显然，环境特征提取与传 感器密切相关。目前，主要传感器包括声纳、激光雷达和视觉传感器( 单目或多目 摄像机) 。声纳的特点为价格低廉，主动式传感，不受天气、光照等外部因素影响。 缺点为回波信息弱，角度分辨率差，测量范围有限( 不超过1 5m ) 。在上世纪9 0 年 代，人们利用声纳提取室内近线段特征，完成s l a m 做了大量研究工作 5 【6 7 】。 随着s l a m 研究向广度与深度发展，s o n a r 的方法逐渐被淘汰，近年来机器人领 域主流杂志与会议鲜有这方面的研究。在自主机器人领域，激光雷达是应用最为 广泛的传感器，它的特点为精度高、速度快和测量距离远。缺点为价格贵，且大多 为单线激光雷达。精度高的特点使得它特别适合中远距离的栅格地图生成，但对 s l a m 而言，特征提取不具有普遍适应性，如在室内环境，存在直线及拐点等明显 特征，而对于室外环境，则规则几何特征难于提取，只能根据具体的环境加以甄选。 但无可否认，激光雷达目前仍然是自主机器人进行障碍物检测的主要工具。视觉传 感器指利用一个或多个摄像机完成外部世界的感知。视觉传感器最接近人类感知 效果，具有信息量大，特征丰富的特点。缺点主要为特征的深度信息恢复存在一定 困难，计算量大，同时易受光照，气候等因素的影响。但目前随着计算能力不断增 强，其限制逐步被解决。对视觉s l a m 而言，最主要的特点是图像特征易于提取， 且数量大。同时基于图像间的特征关联较其他传感器数据更容易，大量的图像跟 踪及识别方面的研究成果为其提供可靠的手段。同时视觉传感器具有能耗低、重 量轻等特点，对环境限制也比较少，特别适合在诸如月球、火星等环境工作，事实 一2 一 浙江大学博士学位论文 证明无论美国、欧洲还是日本的外太空探测均采用视觉传感器作为机器人的主要 导航手段。同时室内微小机器人及人形机器人也逐渐采用视觉s l a m 技术完成定 位与地图生成。因此，以视觉为特点的s l a m 更具有一般意义，只要存在一定纹理 的环境，均可用来做。同时其价格低廉的特性，使得该种方法在机器人领域具有广 泛的应用前景。基于视觉s l a m 在近几年才逐渐兴起，主要归功于稳定图像特征 提取技术以及计算机计算能力的提高。 1 2 机器人s l a m 的历史、发展现状及趋势 为达到减少直至消除累积误差的目的，s l a m 要求估计出一致的机器人轨迹 和环境路标状态。在证明了s l a m 问题的一致性和收敛性后，计算效率问题成为 s l a m 问题最终解决的主要障碍。目前，s l a m 的解决方案主要基于贝叶斯滤波框 架，方法主要包括基于全状态的扩展卡尔曼滤波和基于r a o - b l a c k w e l l i s e d 粒子滤波 器。而对于全状态扩展卡尔曼滤波，其o ( n 2 ) 的计算复杂度限制了其在较大规模 s l a m 中的应用。r a o - b l a c k w e l l i s e d 粒子滤波器方案将s l a m 的全状态问题解耦， 极大降低了计算复杂度，为大规模s l a m 的解决提供了一种较为可行的方法。环 境路标提取与关联作为s l a m 稳定实现的重要，为实现能够适应各种环境要求， 信息提取方式必然多样化。同时基于三维的、室外环境的s l a m 也将成为今后发 展的重要方向。 1 2 1s l a m 问题的历史 s l a m 问题首次在1 9 8 6 年的i c r a ( i n t e r n a t i o n a lc o n f e r e n c eo fr o b o t i c sa n d a u t o m a t i o n ) 会议上提出，那时基于概率推演的方法在机器人和人工智能领域刚 刚兴起。p e t e rc h e e s m a n ，j i mc r o w l e y ，h u g hd u r r a n t w h y t e 提出利用贝叶斯估 计理论完成机器人状态和地图构建同时估计方法的可能性，经过与会者的长时间 的讨论，得出结论，即：一致性( c o n s i s t e n t ) 概率型地图是机器人领域的一个基本 问题，它的理论框架及计算问题值得去研究。地图的一致性是指若去除定位的不 确定性，实际观测与计算得到的观测应保持一致。在接下来的几年里，一些重要的 论文相继发表。s i m i t h ，c h e e s m a n i 0 和d u r r a n t w h y t e 【1 1 率先为建立了环境 路标几何不确定性统计表示方法。该论文的贡献在于揭示了路标的位置信息与其 后该路标的多次观测具有高度相关性，奠定了s l a m 以后研究实现的总体方法。 a y a c h e 和f a u g e r a sf 1 2 也开展了视觉导航的前期性工作，c r o w l e y 1 3 1 5 hc h a t i l a ，l a u m o n d 1 4 基于声纳的室内s l a m 工作，框架为卡尔曼滤波器( k f ：k a l m a n f i l t e r ) 。随后，l e o n a r d ，d u r r a n t w h y t e 1 5 发表一篇该研究领域非常重要的论 一3 一 第一章绪论 文，文章指出要获得一致全面的机器人定位与环境地图构建，必需估计机器人状态 与地图每个路标的联合状态，即完成全状态估计。在完成地图构建一致性理论探 索后，研究者逐渐开始关注地图构建收敛性( c o n v e r g e n c e ) 问题，即s l a m 生成地 图误差是否最终会收敛到一极小值的问题。起初认为地图误差不会收敛，而是随 时间缓慢累积。1 9 9 5 年d u r r a n t 。w h y t e 1 6 首次提出解决s l a m 问题的一种系统 框架，并证明s l a m 最终会收敛。文章同时指出：一旦将机器人状态与路标位置放 入一全状态估计器中，最终状态是收敛的；路标间多次观测的关联性越大，s l a m 将获得更好的收敛性能。由于全状态估计使得被估状态维数随路标数平方增长， 使得计算效率( c o m p u t a t i o n a le f f i c i e n c y ) 问题必须面对。这些构成s l a m 问题的 3 c 原则，即c o n s i s t e n c e ，c o n v e r g e n c e ，c o m p u t a t i o n a le f f i c i e n c y 。此后，在基于 k f 框架内的s l a m 问题得到研究，其应用场合逐渐从室内、室外、海底、航空等 环境发展，并逐渐将问题向数据关联( d a t aa s s o c i a t i o n ) 和验证s l a m 的自闭环 ( l o o pc l o s i n g ) 方向发展。t h r u n 6 2 1 提出一种脱离k f 框架的s l a m 算法，提出 利用基于e m ( e x p e c t a t i o nm a x i m i z a t i o n ) 概率型算法，为s l a m 开辟了新途径。 2 0 0 2 的i c r aw o r k s h o p 吸引了近1 5 0 名相关研究领域专家。同年在瑞典皇家工 学院s u m m e rs c h o o l 吸引了几乎该领域全部重要的学者齐聚一堂，讨论s l a m 的 现状与未来。 1 2 2 当前s l a m 的解决方案 由于运动、观测等过程均具有不确定性，往往将s l a m 建模为概率推演过程， 则在随机过程的框架内解决此估计问题，即利用统计估计理论对机器人状态和环 境路标进行估计。因此，首先面临的便是被估状态的后验概率表示方式，最常用的 是高斯模型。对于近似线性过程和近似高斯噪声，e k f 和u k f 等均是有效滤波工 具。如基于e k f 滤波的全方差s l a m 方案 1 7 】，采用全状态系统模型，分别通过预 测更新，观测更新等步骤完成滤波。由于采用全状态滤波，使得状态空间随路标数 目增加而不断增大，由于e k f 算法复杂度为o ( n 3 ) ，n 为状态空间长度。可以想 象，e k f 全状态滤波方案不适合高维状态估计。因此，需采用各种方法限制状态 空间大小，如考虑到状态观测方程的稀疏性，观测更新的时间复杂度可降为o ( n 2 ) ，同理，考虑到系统方程的稀疏性，预测更新的复杂度可降为o ( n ) ，但总体o ( n 2 ) 的计算复杂度依旧限制了该方案在大规模环境中的应用。因此，为保证s l a m 算 法的实时性，有必要寻找计算效率更高的方法【1 8 】 1 9 】。改进的基本思想为采用优 化或近似的e k f 全协方差计算。最直接的方法为减少状态空间中的路标数目，减 少状态维数，从而降低计算复杂度 2 0 】。d u r r a n t w h y t e 【2 1 】给出一种有目标选择 一4 一 浙江大学博士学位论文 路标的方法，使得不过多损失s l a m 一致性和收敛性前提下，使得e k f 可以应用 于大规模s l a m 。 另一种方法为在最耗时间的协方差矩阵更新上进行改善，如采用近似替代技 术，或通过一定方法最小化或剔除路标之间的相关性，出现了一系列近似优化算 法。其中比较有效为c o v a r i a n c ei n t e r s e c t ( c i ) 方法【2 2 ，它通过一定变换方法使 得更新只考虑协方差矩阵的对角元素，从而极大提高计算效率。g u i v a n t 1 8 】等提 出s l a m 滤波的简化方案，使得对于观测只需更新部分地图特征，使得计算复杂 度降低为o ( n ) 。 另外一种方法为地图分割法，如相对地图法【2 4 】和子地图法。子地图法通过把 整个地图分解为一系列子地图( s u b m a p ) ，从而对每个子地图进行滤波估，降低了 协方差更新过程中的计算复杂度。l e o n a r df 2 5 提出了d s m ( d e c o u p l e ds t o c h a s t i c m a p p i n g ) 算法，发展了子地图全方差更新方法，并确定各子地图之间的相关性。 c l s f ( c o n s t r a i n e dl o c a ls u b m a pf i l t e r ) 2 6 】则构建独立的局部子地图，然后应用 约束规则，定期将该子地图融入全局地图中。c h o n g 等人 2 7 】也提出一个子地图策 略，既随着机器人的移动，生成多个独立的局部子地图。对子地图之间的相对位置 进行单独估计，当机器人位置估计有可能回到先前子地图区域时，则应用搜索算法 对机器人重新定位。子地图分割法虽能够有效降低计算复杂度，但如何进行子地 图分割没有发展出比较一致的方法，同时子地图分割带来最大的问题是给数据关 联带来麻烦2 8 1 。 e k f s l a m 另外一个致命缺点便是数据关联脆弱问题，即一旦数据关联出现 错误，会将误差带入估计结果中，而且不可恢复，即通常说的e k f 缺乏自恢复能 力。该问题使得e k f s l a m 往往不能自闭环( s e l f - l o o pc l o s i n g ) ，即当机器人经过 大范围移动后，重新回到已经访问过的位置时，往往不能够回到原点。因此，有效 的特征检测与匹配是实现e k f s l a m 重要的前提保障。由于线性的运动方程与 观测方程，及高斯的运动噪声与观测噪声，才能从理论上保证s l a m 问题具有一 致性与收敛性。而e k f 仅将运动模型或观测模型线性化来近似，导致后验概率有 时不能真实反映实际情形。因此，在e k f s l a m 中，非线性问题的线性化可能导 致协方差矩阵不可逆，地图构建不一致等问题。补救办法为采用更好的近似方法， 如采用u k f 等更好线性化技术完成协方差更新。但未能从根本上解决问题。 与e k f 利用高斯模型表征机器人后验概率不同，还可用一组离散的粒子表征 后验概率。粒子滤波器( p f ：p a r t i c l ef i l t e r ) 作为蒙特卡罗方法( m o n t ec a r l o ) 的 一种应用，它通过使用一组离散加权采样数据模拟所估计状态的后验概率，并通过 预测状态、更新权值和重采样等步骤完成滤波。它最主要特点是不拘泥于系统的 一5 一 第一章绪论 线性假设和传感器高斯噪声假设，可以有效处理非线性、非高斯的系统。t h r u n 等 人【3 0 】首先将粒子滤波器引入到机器人定位中，通过改进粒子预测分布( 也称建议 分布：p r o p o s a ld e n s i t y ) ，在室内环境获得良好的效果。但单纯的粒子滤波器不适 合像s l a m 这样高维的状态估计。为此，m u r p h y 等人【3 l 】 3 2 首先提出并实现将 全状态滤波器解耦，提出了l h o - b l a c k w e l l i s e d 粒子滤波器的实现方法，为使用粒 子滤波器解决s l a m 问题提供了理论基础。而m o n t m e r l o 等人3 4 1 在2 0 0 2 年首次 将r a o - b l a c k w e l l i s e d 粒子滤波器应用到机器人s l a m 中，并取名为f 、嬲t s l a m 算 法。f a s t s l a m 基于递归的蒙特卡罗采样( 粒子滤波) ，首次实现了利用粒子簇表 征非线性、非高斯系统的状态分布。r a o - b l a c k w e l l i t i o n 分解【3 1 】最重要的贡献在 于证明：当机器人状态轨迹已知，则各路标之间的位置相互独立( 互不相关) 。因 此f a s t s l a m 的方法为将一粒子滤波器表征做机器人轨迹滤波。同时每个粒子维 护一路标地图( 使得轨迹己知，路标之间相互独立) ，则每个路标通过e k f 估计。 该算法特点为计算复杂度可降为o ( ml o gn ) ，m 为粒子数，n 为特征数目。该算 法已经被证明对于s l a m 问题非常有效 3 5 。但是f a s t s l a m 还是需要稳定的环 境特征提取及数据关联作为保障【3 6 】。针对室内环境基于激光雷达的s l a m 问题， h a h n e l 3 7 1 用激光扫描模型( 1 a s e rs c a nm o d e l ) 代替路标，以激光点集构成栅格地 图，并利用匹配误差对粒子进行重采样，发展了f a s t s l a m 算法，使其接近真正意 义上的致密地图重建。g r i s e t t i 3 8 】更是提出一种自适应的粒子预测分布方法和 s e l e c t i v e 重采样方法，用以减少粒子数目。另一个基于r a o - b l a c k w e l l i s e d 方法的 栅格型地图s l a m 方法为d p s l a m ，该方法由e l i a z a r 3 9 】【4 0 提出，该方法的重 要贡献在于抛弃了f a s t s l a m 中每个粒子维护一个地图( 以极高存储空间为代价) 的思路，而是仅维护一个栅格地图，用该地图维护粒子( 即与常规方法相反) ；同时 大幅度减少因重采样而大量拷贝重复地图元素带来的计算量无谓消耗问题。s h u y u nc h u n g 4 1 1 提出一种相对地图与绝对地图融合的方法。c h a n k ik i m 4 2 】利用 u k f 代替t s l a m 中的e k f 估计手段。j o s e - l u i sb l a n c o 4 4 1 提出一种混合地 图，即拓扑地图与栅格地图混合生成的方式，以使得f a s t s l a m 能够有效完成大规 模环境的s l a m 工作。b a i l e y 【4 5 】对f a s t s l a m 地图构建一致性从理论角度进行 了讨论，得出结果为无论粒子数多少与环境路标的数量大小，f a s t s l a m 随时间逐 步发散，但总能产生次优的估计结果。但粒子数的提高确实有助于获得更好的地 图，发散的原因在于粒子重采样过程使得使得高维的轨迹信息得到破坏。实验已经 证明f a s t s l a m 的结果次于全状态e k f s l a m ，但是其计算效能，及扩展空间比 e k f 更大，总体上比e k f s l a m 在大规模环境应用上具有无可比拟的优越性。 在国内，近年来才逐渐开展研究，而且主要以跟踪国外技术及以激光雷达提取 一6 一 浙江大学博士学位论文 室内线特征为主 4 6 】【4 7 】【4 9 【5 0 【5 1 】 5 2 】 5 4 】【5 3 ，