（通信与信息系统专业论文）基于粒子滤波器的同时定位与地图构建.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 ) i b l 题的研究是智 能机器入研究领域的热点话题。机器人同时定位与精确地图创建能力是自主移动机器人在 未知环境运动的先决条件。最近十几年来实现s l a m 的算法主要是用e k f 方法。然而，基 于e k f 的s l a m 方法在实际的大环境中运用时，有两个缺陷：二次方复杂度和易无效的数 据关联。本文采用另一种方法实现s l a m 并且解决这两个问题。 文章首先回顾了s l a m 技术的发展，指出已有s l a m 技术存在的问题，在此基础上引 出了本文研究的重点：基于粒子滤波器实现s u 蝴算法，并对其算法构架、属性、以及性 能等福关内容进行了介绍。通过仿真实验证实了本算法的正确性与可靠性。 这个算法的思想是把后验概率分解成两部分，是路径的后验概率，另一个是以路径 概率为条件的环境特征的后验概率。分解的后验概率可以用粒子滤波器有效地逼近。加入 新观测量需要的时间长短变为环境特征数目的对数。除了对机器人的路径采样，粒子滤波 器也对数据关联的可能性进行采样。通过对数据关联的可能性采样使得粒子滤波器能够在 已知和未知的数据关联下都能实现s l a m 算法。用仿真实验结果比较粒子滤波器与e k f 方 法实现s l a m 的性能，结果表明前者可以在相当大的环境甚至是数据关联很不确定的环境 下得到更加精确的地图。此外，讨论了在线性高斯的情况下，粒子滤波器实现s l a m 的算 法的收敛性。 关键词：s l a m ；粒子滤波器；移动机器人；卡尔曼滤波器；数据关联 中嗣海洋大学碰士学位论文 s i m u l t a n e o u sl o c a ii z a t i o n a n dm a p p i n gb a s e do np a r t i 0 i ef ii t e r a b s t r a c t i n t e l l i g e n tr o b o t s a r eal ( i n do fr o b o t st h a ta a b l et ow o r ki n c o m p l e x e n v i r o n m e n t sw i t ht h ec a p a c i t i e so fs e l f - o r g a n i z i n ga n ds e | f - p l a n n i n g s l a mp r o b l e mi sah o t i s s u ec o n c e r n e di nr e s e a r c h i n go hs u c hk i n do f r o b o t 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 ) i sa ne s s e n t i a lc a p a b i l i t yf o rm o b i l er o b o t su n k n o w ne n v i r o n m e n t s t h ee x t e n d e d k a l r a a nf i l t e r k f ) h a ss e r v e da st h ed e - f a c t oa p p r o a c ht os l a mf o rt h el a s ts e v e n t e e n y e a r s h o w e v e r , e k f - b u s e ds l a ma l g o r i t h m ss u f f e rf r o mt w ow e l l k n o w ns h o r t c o m i n g st h a t c o m p l i c a t e t h e i ra p p l i c a t i o nt ol a r g e , r e a l - w o r l de n v i r o n m e n t s ：q u a d r a t i cc o m p l e x i t ya n d s e n s i t i v i t yt of a i l u r e si nd a t aa s s o c i a t i o n t h i sp a p e rw i l lp r e s e n ta na l t e r n a t i v ea p p r o a c ht 0 s l a mt h a ts p e c i f i c a l l ya d d r e s s e st h e s et w oa r e j l q s l a mt e c h n i q u e sa r e f i r s t l y r e v i e w e di nt h i s p a p e r a n dt h e nt h ee m p h a s i so f t h i sp a p e ri si n t r o d u c e d ：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 gb a s e do np a r t i c l ef i l t e r , b a s e d o nt h e a n a l y s i s o ft h e p r e v i o u s s l a m l i m i t a t i o n , i n c l u d i n g i t s a r i t h m e t i c k a u e t u r e ，c h a r a c t e r i s t i c s ，p e r f o r m a n c ea n ds oo n t h i sa p p r o a c hh a sb e e np r o g r a m m e da n d s u c c e s s f u l l yt e s t e di nt h es i m u l a t i o nw o r k , t h i sa p p r o a c h , f a c t o r st h ef u l ls l a mp o s t e r i o re x a c t l yi n t oap r o d u c to far o b o tp a t h p o s t e r i o r , a n dl a n d m a r kp o s t e r i o r s c o n d i t i o n e do nt h er o b o tp a t he s t i m a t e t h i sf a c t o r e d p o s t e r i o rc a n b ea p p r o x i m a t e de f f i c i e n t l yu s i n ga p a r t i c l ef i l t e r t h et i m er e q u i r e dt oi n c o r p o r a t e a no b s e r v a t i o ni n t op a r t i c l ef i l t e r - b a s e ds l a ms c a l e sl o g a r i t h m i c a l l yw i t ht h en u m b e ro f l a n d m a r k si nt h em a p ， i na d d i t i o nt os a m p l i n go v e rr o b o tp a t h s ，p a r t i c l ef i l t e r - b a s e ds l a mc a l ls a m p l eo v e rp o t e n t i a l d a t aa s s o c i a t i o n s s a m p l i n go v e rd a t aa s s o c i a t i o n se n a b l e sp a r t i c l ef i l t e r - b a s e ds l a mt ob e u s e di ne n v i r o n m e n t sw i t hh i g h l ya m b i g u o u sl a n d m a r ki d e n t i t i e s t h i sd i s s e r t a t i o nw i l ld e s c r i b e t h ep a r t i c l ef i l t e r - b a s e ds l a ma l g o r i t h mg i v e nb o t hk n o w na n du n k n o w nd a t aa s s o c i a t i o n t h ep e r f o r m a n c eo fp a r t i c l ef i l t e r - b a s e ds l a mw i l lb ec o m p a r e da g a i n s tt h ee k f - b a s e d s l a mo ns i m u l a t e d r e s u l t sw i l ls h o wt h a ts l a mp a r t i c l ef i l t e r - b a s e ds l a mc a l lp r o d u c e a c c u r a t em a p si ne x t r e m e l yl a r g ee n v i r o n m e n t s ，a n di ne n v i r o n m e n t sw i t hs u b s t a n t i a ld a t a n 独创声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的研究成果。 掘我所知，除了文中特别加以标注和致谢的地方外，论文中不包含其他人已经发表或撰写 过的研究成果，也不包含未获得 l 逵! 垫退直基丝益垩挂型重盟 鲍：奎拦巫窒2 或其他教育机构的学位或证书使用过的材料。与我一同工作的同志对本研 究所做的任何贡献均已在论文中作了明确的说明并表示谢意。 学位论文作者签名：剃签字同期。哼年6 月f 同 学位论文版权使用授权书 本学位论文作者完全了解学校有关保留、使用学位论文的规定，有权保留并向国家有 关部门或机构送交论文的复印件和磁盘，允许论文被查阅和错阅。本人授权学校可以将学 位论文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫描等复制手 段保存、汇编学位论文。( 保密的学位论文在解密后适用本授权书) 学位论文作者鹕。鞠蚴 签字日期& 0 7 年6 月f 日 学位论文作者毕业后去向： 工作单位： 通讯地址： 导师签字：彳彳1 ，摩 签字日期一7 年易月f 日 电话： 邮编： 基于粒7 滤波嚣的同时定位与地图构件的方法研究 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 ，已在机器人领 域的各类文献资料中得到极度关注。s l a m 是用于解决移动机器人在未知环境下移动的 问题。机器入对自身运动的状态和环境中的目标的位置进行相对观察。这些观测都有误 差干扰。s l a m 的目标是重新建一个精确的全局地图和得到机器人所行走的路径。s l a m 是移动机器人能否真正自主的先决条件吲。 如果已知环境的真实地图，估计机器人路径的就直接是定位问题【l “。同样的，如果 机器人真实的路径可知，地图构建就是相对容易的工作了d 7 j 引。如果机器人的路径和地 图都未知，就必须同时考虑定位和建图因此，把这个问题称为同时定位与地图创建。 1 1s l a m 的历史及当前研究状况 机器人的诞生和机器人学的建立和发展是自动控制最具说服力的成就。是2 0 世界人 类科学技术迸步的重大成果，而作为机器人中重要分支之一的移动机器人更是给人们带 来了无限的惊喜。移动机器人的研究始于2 0 世纪6 0 年代末期。斯坦福研究所( s r i ) 的n i l s n v i l s s e n 和c h a r l e sr o s e n 等人，在1 9 6 6 年至1 9 7 2 年中研制出了取名s h a k e y 的自主移 动机器人，其目的是研究、应用人工智能技术以及在复杂环境下机器人系统的自主推理、 规划和控制：2 0 世纪7 0 年代末，随着计算机的应用和传感器技术的发展，移动机器人研 究又出现了新的高潮；2 0 世纪9 0 年代以来，以研制高水平的环境信息传感器和信息处理 技术，商适应性的移动机器人控制技术、真实环境下的规划技术为标志，开展了移动机 器人更高层次的研究【l 】。目前，移动机器人正向着具有自组织、自学习、自适应的智能化 方向发展，导航能力的高低是移动机器人智能化水平的重要体现。随着移动机器入应用 领域的日益增加，对移动机器人导航研究不断提出新的课题，使移动机器人的导航研究 不断深入和发展1 2 3 1 1 4 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 d l o c a l i z a t i o n ) 。该研究领域的代表人物有s m i t hs e l f 和c h e e s e m a n 【2 3 1 。由于s l a m 算法具 中田海洋大学硕士学位论文 有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。 1 2s 圳的应用 移动机器人在未知精确全局地图的环境中运动，必须具备s l a m 能力。而且，移动 机器人已经显示出在远程探测方面具备潜质：可以进入遥远、危险的地方探测【2 ”，以及 因代价昂贵而人类无法到达的地方【5 明( 见图1 1 ) 如果要在海下、地下、外星等极端的环 境下自主操作，移动机器人必须具备构建地图并且根据地图可靠的导航的能力即使在 良性的环境下例如室内，一般也很难得到精确的先验地图。能够构建未知环境地图，就 可以用最小的的基础构造的配置机器人这对于动态环境非常重要 ( a ) 海下 c o ) 地下( c ) 外星 图1 1s l a m 的目标环境图 用s l a m 算法创建的地图的意义，比较典型的是做为运动规划和勘探的基础。在2 0 0 2 年七月，9 个矿工在一个荒矿钻眼之后陷入地下三天半，后来经过调查，此次事故的正 是地图不精确造成的【2 0 】。此后，移动机器人和s l a m 作为取得荒矿精确的地图的技术来 研究。如图1 1 b 所示的机器人，能用s l a m 技术建造内部矿区的三维地图 5 9 1 1 3s l 棚算法的框架 上文中讲到根据所得到地图类型的不同，可以将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 算法架构由图1 2 表示。 由图3 1 可以看出，整个算法实际上是由三部分构成即：预测数据采集和处理一 一更新。首先要建立机器人的动力学模型以及运动过程中的观测模型。某一时刻七。在给 定控制参数后，根据动力学模型可以预测机器人在k + l 时刻的位姿状态；同时结合观测模 型还可以预测在k + l 时刻机器人对旧( 已经核实的) 特征只的观测值；在实践环节中，即在 2 基于粒子滤波器的同时定位与地图构件的方法研究 时刻k 到时刻k + l 的过程中，机器人基本上要完成三大工作：环境特征的提取和合理性 检验，以及相关数据的匹配。从而获得对只特征的实际测量，同时筛选出新的特征将 对特征的估计测量和实际测量的差异值( 参量) 、机器人位姿估计以及旧的地图一起作为输 入参数，通过一定滤波算法即可更新机器人位姿，进而得到新特征的位黄信息并将其融 入到旧地图中生成新地图。总而言之，该方法要求机器人的位置和所有地图特征的位置 存放在同一个状态向量中，并且这个向量是增广的。当观测到环境中新的特征时，就要 把它作为新的地图特征添加到状态向量中。然后通过滤波就可以构建一个收敛的地图 在极限情况下，随着对环境特征的重复观测，地图特征之间的相对关系变得完全确定， 并且地图的不确定性控制在一个小的范围内。这个误差范围与机器人初始位置的不确定 性有关。 图1 2s l a m 算法结构 1 4 本课题的主要工作及论文的组织结构 本文主要研究基于粒子滤波器的自主移动机器人s l a m 问题。对常用的s l a m 方法进 行了综述，比较了各种方法的优缺点。介绍了基于e k f 的s l a m 方法，分析该算法的的缺 陷和不足基础上提出基于粒子滤波器的s l a m 方法。并对其算法构架、属性、以及性能等 相关内容进行了分析。用定量的实验，在各种仿真的数据环境下比较r a o - b l a e k w e l l i z e d 粒 子滤波器和e k f 实现s l a m 算法。 本文各章内容安排如下： 中田海洋大学硬士学位论文 第一章首先对s l a m 进行了综述回顾了s l a m 的发展历史和当前研究现状，介绍它 的应用意义，并且给出算法框架 第二章，首先详细描述了s l a m 问题，包括它的定义、联合概率和后验概率、s l a m 的结 构和特性阐述了s l a m 需要处理的几个方面，包括地图描述方法和数据关联问题。最后 具体介绍了e k f ( 扩展卡尔曼滤波器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 的算 法，分析该算法的缺陷和不足 第三章，介绍了粒子滤波器的基本概念和算法。分解s l a m 的后验概率。进而详细讲述基 于粒子滤波器实现s l a m 的算法，并把该算法中关键的问题具体展开，包括数据关联中的 相互排斥和排除特征在此基础上，又介绍了对数的粒子滤波器实现s l a m 的算法最后 进行仿真实验。实现了基于粒子滤波器的s l a m 算法并且与基于e k f 的s l a m 算法进行比 较 第四章，本文所作的主要工作，对粒子滤波器的s l a m 算法改进，得到仿真结果。把当前 观测量加入到粒子滤波器的假设分布中，当运动噪声比传感器噪声高时，可以得到更精确 的结果。其中做了关于在线性高颠环境下收敛性的讨论。 第五章，总结和展望。 4 基于粒子滤波晷的同时定位与地图构件的方法研究 2s l a m 问题的描述 这一章是关于s l a m 的总的概括，还有大部分文献中常用的s l a m 方法。主要的内容是基 于e k f 的算法。 2 1s l 棚问题的定义 假设移动机器人在未知的静止的环境运动。机器人执行控制命令并且观察环境特征。 控制量和观测量都受干扰。s l a m 是从一系列的有噪声控制量和观测量中恢复地图和机器人 的路径。 如果机器人的路径是确定知道的( 例如利用g p s ) ，那么就直接是构建地图的问题。环 境中的目标的位置用独立的滤波器估计的。而当机器人的路径是未知。机器人的路径跟地 图的误差相关因此机器人的状态和地图必须同时估计。 机器人的位姿误差和地图的误差间的关联可以从图2 1 a 看到。机器人沿着预先规划的路径 移动，观察附近的用圆圈标的环境特征。荫影椭圆形代表机器入位姿随着时间的不确定。 由于控制量有误差干扰，机器人的位姿随着机器人的移动变得更加不确定。环境特征位置 的估计是没有阴影的椭圆。很明显，在机器人的位姿变得更加不确定的时候，新观察到的 环境特征的位置的不确定性也在增加。 在图2 。1 b ，机器人完成了一圈的运动，并且进行新的观察原来观测过的环境特征。因 第一个环境特征的位置是确定，那么机器人的位姿估计的不确定性会显著减小新的机器 人位姿的信息增加了原来机器人位姿的确定性。反过来原来观测过的环境特征位置的不确 定性也减少。这是因为s l 枷问题的相关特性：地图的误差是与机器人的路径误差相关联的 任何提供机器人位姿信息的观测量，都必定的提供所有的原来观测过环境特征的信息。 中国海洋大学硕士学位论文 ( a ) 在闭环之前：环境特征位置不确定性随着机器人位姿的不确定性增加而增 加阴影椭圆是随着时间变化机器人的位姿估计透明椭圆是环境特征估计 ( b ) 在闭环之后：重访已知的环境特征，机器人的位姿不确定性和所观 测环境特征位置的不确定性都减小了 2 2s l a m 的联合估计 图2 1 机器人运动误差与地图误差的关联 定位和构建地图之间先后的关系的不确定，是因为对于机器人运动误差如何干扰传感 器误差的先后不确定。机器人运动，位姿估计被运动误差干扰。环境特征在全中的位置也 6 基于粒子滤波嚣的同时定位与地圈构件的方法研究 被观测误差和位姿估计误差所干扰。然而，与观测噪声不同的是，位姿误差系统性地作用 于全局地图这个作用也就是，路径的误差与地图的误差是相互关联的因此，没有正确 路径估计的条件下无法估计地图的误差。 ( a ) 没有修正机器人路径的地图( b ) 用s l a m 创建的地图 图2 2 机器人的路径误差和地图误差的关联 图2 2 是移动机器人在常见的办公环境中运动，用一组激光传感器采集的扫描结果。 机器人用轮子上的编码器估计运动情况。图2 2 a ，根据编码器数据得到路径，用激光传感 器数据创建的地图。显然，随着机器人位姿估计的误差累积，全局地图逐渐变得不确定。 图2 2 b 是s l a m 算法处理机器人路径后，激光传感器数据所创建的地图。尽管，路径误差和 地图误差的关系使s l a m 问题在变得更加复杂和困难，下面就要讲怎样把这个关系分解，把 s l a m 问题转换成一些小的问题的乘积。然后用更有效的方法解决这些小问题。 2 3s l 删的后验概率估计 根据标准的s l a m 方程，机器人执行控制命令以及采集观察量，都有噪声干扰。可以把 每个控制量或者观察量，还有适当的噪声模型，看作一个概率约束。例如，每个控制量概 率地约束两个连续的机器人位姿。另一方面，观测量约束机器人和目标的相对位置。随着 约束网络的扩张，新的观测量不仅更新的地图特征和机器人的位姿，也更新以前所观测的 地图特征。图1 3 是观测量和控制量的约束的一个例子。 开始，这些约束可能很不确定。然而，随着地图的特征被反复的观测，约束越来越确 定。在无限次的观测和控制后，所有的地图特征的位置就完全相关。s l a m 原来的目标是已 知一组观测量和控制量的情况下，估计真实的地图和机器人的位姿。 7 中固海洋大学硕士学位论文 图2 3 观测量和控制量形成一个概率网络。这个网络对机器人的位姿和机器人所在环境 中环境特征的相对位置约束这些约束在图中用黑线表示 其中一个解决方法是通过估计算法估计最可能的机器人位姿和地图。然而，这些方法 需要处理的是一组变化无常的观测量和控制量，不适合在线操作吼“。而且，这些算法一 般不能估计地图的不同部分的确定性。机器人开发未知环境，这是必须要考虑的问题。 最流行的在线解决s l a m 问题的方法是：已知传感器读数，对所有可能的地图和路径估 计后验概率分布这个分布称为s l a m 后验概率分布，可以写成 p ，ol 一，)( 2 1 ) 其中暑是目前机器人位姿， 是地图。后验概率分布是以一组传感器读数，、控制量 和数据关联为条件的条件概率分布。数据关联是地图观测量一和地图 中的特征 的关联。看起来后验估计方法比最大似然估计方法可行性小。然而，通过有效的对全局发 展的状态假设，后验概率地估计是很有效的。 机器人在一个平坦的环境中操作，位姿包括机器人的五月z 面坐标和朝向。所有在论 文中的实验结果都是在平面环境中实现的，当然这个算法同样适用于三维的环境。完整的 机器人的路径，包括机器人每个时刻的位姿，用s 表示 = 辆s 2 ，墨， ( 2 2 ) 进一步假设机器人的环境可以用n 个不动的点状的环境特征的集合来表示。点状的环 境特征通常用于代表从传感器中提取的特征的位置，例如激光传感器体的几何特征或者摄 像设备的独特的视觉特征。 个环境特征位置的坐标会写成 q ，氏) 为了简化符号，全 8 基于粒子滤波嚣的同时定位与地图构件的方法研究 局地图就写为0 在实际的应用中，这些点的环境特征可以对应传感器( 例如摄像头，声纳和激光传感 器) 提取的特征的位置。本文后面是假设点的环境特征当然也可以用其他的描述方法， 更高的级的几何特征例如线段，也可以描述s l a m 地图“” 后验估计不仅估计晟可能的地图和机器人的路径，也对每个估计量的确定性有个量化 的估计。后验估计是必要的。跟只考虑地图最可能状态的思想相比，后验估计有几个优 点。首先，考虑了可能的分布，使在有噪声的环境下算法更加有效。其次，可以利用不确 定性大小求有这个算法不同部分的相关的信息。地图的某一个部分可能是很不确定的，而 其他部分又是确定的。 下面的递归公式，也就是贝耶斯滤波器，可以用于已知在t 吖的后验概率情况下，计 算时刻的后验概率。 p ( s t ，o i ，“t ，l | ) = ，7 p ( 刁i s , 0 ，珥) p “i s , - i ，l ) p ( 墨- l ， i 一。，t - i9 n t - 1 ) 趣一 ( 2 - 3 ) 一般情况下。( 2 3 ) 中的积分是无法计算的然而，这个公式可以通过假设后验概率分 布为一个特定形式来计算。很多估计算法包括卡尔曼滤波器和粒子滤波器，都是对贝耶斯 滤波器的逼近。 机器人在环境移动时，采集了它自身运动的相关的信息。这个信息可以用机器人轮子 上带的里程计、内部的导航单元或者仅仅是通过观测机器人执行的控制命令得到。不管如 何得到，任何的机器人运动的测量值都被称为控制量。t 时刻的控制量写为配所有的机器 人执行的控制量的集合写为。 | ，= h i ，* o0 9 )( 2 - 4 ) 机器人运动过程中，观测附近的环境特征。在平面s l a m 最常见的方程中，机器人观测 附近障碍物的距离和方位。耐刻的观测量写为互。所有观测量的集合写为， ，= 编，乞9o - o h 2 t ( 2 - 5 ) 在s l a m 文献中一般假设传感器的观测量可以分解成单个的环境特征的信息，因此每个 环境特征的观测量可以用其他的观测量独立的表现。所有成功地s l a m 应用中，环境特征是 通过未处理传感器数据一个一个提取的，这个假设是可行的。因此，本文假设每个观测量 提供的信息是一个环境特征皖相对于目前位姿置的确切位置。变量，代表了某个被观察到 的环境特征。实际上，并不会观察到环境特征的唯一性，因为很多环境特征看起来是一样 9 中田海洋大学确士学位论文 的。对应观测量的唯一的环境特征写傲珥例如， = 3 代表了在f 钢寸刻，机器人观测 到的环境特征的代码3 。环境特征唯一性一般称为数据关联所有数据关联的集合会写成 ， e 啊，鸣，一) ( 2 - 6 ) 为了再进一步简化，假设机器人每一步收到一个观测量五和执行一个控制t u , 可以 处理每一个时间步的多观测量，但是这个导致了更繁重的符号标注。 用上述定义的符号，s l a m 的主要的目标是：已知噪声的观测量= 和控制量矿，得到 最可能的机器入位姿岛的估计和地图o 通过以下的后验概率表达，后面称为s l a m 后验概 率 p 瓴，o 阿，) ( 2 - 7 ) 如果数据关联n 的集合也已知，那么s l a - i 后验概率简化为： p ( 每，e l 一，矗，)( 2 s ) 2 4s l 圳的马尔可夫链特性 s l a i l 问题可以描述为马尔可夫概率链。当前机器人位姿是是前一步_ l 和控制量坼 的概率函数。这个函数描述如何控制驱动机器人的运动，称为运动模型。此外，运动模型 描述了控制噪声如何把不确定性加入位姿估计的。运动模型如下 p ( i s , i ，吩) ( 2 9 ) 机器人采集的传感器观测量也受概率函数的控制，称为观测模型。观测量刁是被观测 到的环境特征气和机器人的位姿的函数。观测模型描述了机器人传感器误差模型。观 测模型如下 p ( 刁i 丑，0 ，哆) ( 2 - 1 0 ) 1 0 基于粒子滤波器的同时定位与地图构件的方法研究 图2 4 机器人模型和观潞模型 通过运动模型和观测模型，s l a m 在埘刻的后验概率可以用f o 时刻后验概率函数来递 归计算。这个递归的更新算法，称为s l a m 的贝耶斯滤波器，是多数在线s l a m 算法的基础 贝耶斯滤波器的推导 贝耶斯滤波器可以用如下的s l 脒后验概率推出。首先，后验概率用贝耶斯定理写为。 p ( 焉，o iz j ，斗) = 叩p ( 五i 丑o ，z h ，珥) p ( 墨，o l z r 1 , u ，) ( 2 1 1 ) 贝耶斯定理的分母是标准化常t , 7 。然后利用毛是独立的机器人位姿墨、地图0 和数据关 联啊的函数，如前面观测模型所描述。因此后验概率可以写成 叮p ( 五is , o , 珥) p ( ，oi2 - 1 ，蕾，1 1 ) 2 - 1 2 ) 用完全概率理论把最右边的项在 o 时刻的机器人的位姿鼻。为条件。 叩p ( 乙i o ，啊) f p ( ，oi 一l ，z t - iq ，p ( 丑一1l2 - 1 ，“，) 如一i ( 2 1 3 ) 积分内部最左边的顼可以用条件概率的定义来代替。 印p ( 4i s , ，o ，埤) j p ( ，o l 一l ，z t - lu t ，) p ( l - i ，一一，) p ( i z t - ! “ i t l ) 幽一l ( 2 1 4 ) 如运动模型中所述，仅仅是墨一l 和虬的函数，积分内的第一项可以简化。 t i p ( z , i ，o ，啊) i p ( i 墨一j ，坼) p ( o i 岛】，z 卜1 ，) p ( q ii z t - i “，厅) 幽一i( 2 - 1 5 ) 中国海洋大学磺士学位论文 积分内部最左边的两个项可以被合并。 r l p ( z ，i o ，吩) j ，( i i ，m ) p ( s 1 ， i z t - ! i ，) 幽1 ( 2 - 1 6 ) 当前的位姿鼻和数据关联珥没有提供新的关于斗l 或者 的信息，可以把他们从积分 最左边的项中除去。结果是已知一时刻s l 棚的后验概率，运动模型p 瓴l 墨1 ，虬) 和观测模 型p ( 弓i 岛， ，啊) ，用递归的公式计算s l a m 的在t 时刻的后验概率。 p ( 暑，o l z ，”。，拧) = r l p ( z ，i 丑 ，啊) i ，( 疋i i ，坼) ，( 一l ， l z , t - i ) 甜t - i ) ，l ，- 1 ) 凼，1 ( 2 1 7 ) 2 5s l 删的结构和离散性 任意时刻，观测量和控制量都是状态变量的子集数据和状态变量之间的相关性是离 散的，这个离散性使计算s l a m 后验概率更有效例如，两个环境特征间距离很大，通常相 关性很小附近一些分布很远的环境特征对相关性相似。一些近似的e k f 算法利用这一特 性把完整的地图分成子地图的集合。因此，大的e k f 可以被分解成一些联系不紧密的小e k f 这样得到近似的有效e k f 算法，它需要加入观测量的时间是线性的嘲甚至是常数的隰“”。 然而空间上分解s l a m 问题的新e k f 算法，面对的跟原始e k f 算法同样的难题：数据关联。 本文利用状态变量关系的离散性，用另一个方法解决s l a m 。不仅有效地计算s l m 后验概率， 还做到了多数据关联的假设。结果是s l a m 算法可以用于对数据关联显著不确定的大环境。 2 6 扩展卡尔曼滤波器( e k f ) ，j ，- | | j 。f 。： “。6 粼1 “g 相应的协方差矩阵。黑色的矩阵元素表 示关联性强 图2 , 5 应用e k f 的仿真数据集合 基于粒子滤波嚣的同时定位与地图构件的方法研究 s l a l 问题的主要方法由s m i t h 和c h e e s e m a n 在1 9 8 6 年论文中提出m 1 。首先被m o u t a r l i e r 和c h a t i l a 发展成为一个实用体系饥”这个方法就是用e k f 来估计机器人位姿和地图的后 验概率。e k f 用一个囊括了所有的地图特征和机器人的位姿的高维高斯分布来逼近s l a m 的 后验概率。这个多变量的高斯的协方差矩阵的反对角线的元素代表所有对的状态变量间的 关联。这样e k f 就能够体现所有s l a m 问题的相关误差。e k f 的一个仿真举例见图2 5 。图2 6 是相应的协方差矩阵。 矩阵元素越黑表示状态变量间的相关性越高。然而有两个问题使得e k f 在真实的大环 境下的应用变得复杂，这两个问题是：二次方复杂度和数据关联无效的灵敏度。解决这两 个问题后，e k f 方法就成了s l a m 的常用方法。 2 ，6 i - - 次方复杂度 e k f 来实现s l a m 最大的缺点是计算复杂。e k f 算法所需要的计算复杂度和存储容量是环 境特征数目的二次方倍。完全依靠e k f 的s l a m 算法处理的环境特征数目不能超过几百个。 但是，一般的大环境下可能会有凡百万的环境特征。 二次方复杂度是d q e k f 中所用的高斯描述造成的s l a m 后验概率的不确定性用协方差 矩阵表示，这个矩阵有( 2 r + 3 ) ( 2 + 3 ) 个元素，其中n 是地图环境特征的数目因此，需 要存储这些协方差矩阵的内存变成2 而且，因为所有两两状态变量的相关性是一直存 在的，加入e k f 中的任意传感器观铡量都必定会的影响其他的所有状态变量。加入一个观 测量，e k f 算法必须重新处理矩阵的每个元素，这需要二次方的时间。在实际应用中，s l a m 很少完全依靠e k f 。为了使计算e k f 的更新可行而使用各种方法。这些方法会在下面进一步 讨论。 2 6 2 - , 茧- 假设数据关联 基于e k f 的s l a m 方法的第二个问题是数据关联，即在观测量和环境特征之间创建地图。 最常见的是在数据关联已知的情况下用公式表达s l a m 问题。假设每个观测量对应一个标示 啊，珥指示产生读数的环境特征。例如( 2 - 1 ) s l n d 后验概率，是以关联为条件的。实际 上，观测量和环境特征之间的关联是隐藏的变量，要估计机器人的位姿和环境特征的位置 必须确定数据关联。 在e k f 中标准的数据关联方法是用一个最大的似然定律指派每个观测量对应相应的环 境特征；也就是每个观测量被指派给最可能产生这个观测量的环境特征。如果观测量的概 中嗣海洋大学硕士学位论文 率太小，需要加入一个新的环境特征到滤波器中e k f 没有表达数据关联不确定性的机制， 就一直存在错误数据关联的观测量带来的不良影响如果加入很多不正确的读数，滤波器 会发散。对于错误数据关联的灵敏度是典型的无效e k f 模型“” e k f 数据关联的精确性可以通过同时考虑多观测量的关联来改进，这也增加了计算量。 然而，这还不能解决e k f 潜在的数据关联问题：每步选择单个数据关联假设。给定观测量 的正确关联不总是先考虑的那个最有可能的选择。事实上，对于一个观测量真正的数据关 联可能开始看起来可能性不大而进一步观测量需要提供足够信息来确定关联正确。凡是 每一步维持唯一数据关联的e k f 算法，难免选择错误的数据关联如果一直不修正错误的 数据关联，重复的错误最终导致滤波器发散。 2 6 3e k f 实现s l a m 的算法 大致上，在2 1 5 的递归更新公式的积分中不能封闭的计算。通过限制s l 删后验概率的 形式、运动模型形式、和观测模型形式可以逼近s l a m 算法大部分目前的s l a m 算法来自于 最初的s m i t h 和c h e e s m a n 的学术论文中提出的用e k f 来估计s l a m 的后验概率删。 e k f 通过参数化高斯分布和一个协方差矩阵描述s l a m 的后验概率。均值“描述了最可能的 机器人的状态和环境特征，协方差矩阵把各对状态变量之间的关联编码。 p ( 日，0f ，栉) = ( 薯；鸬，。) 玉= 墨，b ， 鸬= 以，氏， 毛粥毛知 锯岛翻岛 ； 岛岛 知墨知 ( 2 - i s ) 对机器人来说，在平面内运动，均值向量“是孙”维，其中n 是环境特征的数量。描 述机器人的位姿需要三维，确定每个环境特征的位置需要两维。同样的，协方差矩阵是 ( 2 ，+ 3 ) ( 2 + 3 ) 维。因此，需要描述后验概率的参量的数目是环境特征数目n 的二次方。 1 4 基于粒子滤波嚣的同时定位与地图构件的方法研究 ( a ) 先验假设( 实线) 和新的观测值 ( 虚线) ( b ) 在加入新的观测值后的估计( 租 线) 图2 6 一维卡尔曼滤波器 图2 6 是关于e k f 在一维估计一个单独的环境特征的位置的简单例子图2 6 a 是目前的 估计的环境特征的位置( 实线) 和新的有噪声的对环境特征的观溺( 虚线) 卡尔曼滤波 器描述了把高斯假设加入到线性系统中的最佳程序。这样，新的后验概率加入预先的观测 量就得到了2 6 b 中的粗线。 基本的卡尔曼滤波器算法是对于有高斯噪声的线性系统最佳估计锄。e k f 仅仅是基本 的卡尔曼滤波器算法对于非线性系统的扩展：用非线性模型取代运动和测量模型，非线性 模型是在系统最可能的状态的线性化。大致上，如果真实的模型是接近线性的，并且如果 滤波器离散的时间步很小的话，这个逼近是适合的。 运动模型用线性化噪声斜方差只写为非线性函数厅“p ) 同样，观测模型用线性化噪声 协方差足写成非线性函数g ( 薯，嘿) 。e k f 更新方程可以写成如下 盯= | l l ( 鸬1 ，) - = 。+ 丘 g ：= v 耳g ( 薯，) l 。厅 z f = q 爵+ 足 墨= 旨z , - 1 p t = 城+ k | 心t 一2 。1 。= ( j k g i ) z j ( 2 1 9 ) 中田海洋大学瑚士学位论文 卡尔曼滤波器完全的推导见“。对于引入卡尔曼滤波器和e k f 的用法见。如果s l a m 问题是线性的和高斯，那么卡尔曼滤波器是既收敛又是最佳的0 1 ，注意到这一点是很重要 的。实际的s l a m 问题很少是线性的，然而e k f 还是可以得到较好的结果。由于这个原因， e k f 通常被比喻成在线s l a m 算法的金本位。 在s l a m 问题的应用上e k f 有两个本质的缺点：二次方的复杂度和单假设的数据关联。 加入控制和观测量进入到滤波器中的计算量受( 2 一1 9 ) 最后的方程约束在平面的情况下， k 和g f 的维数是璺忤僳以观测量维数( 通常是力因此，在计算z 。时内部的乘积需要 ， 的二次方的计算量。因此环境特征的数目受限，e k f 处理的环境特征的数目只能是几百个 第二个问题是数据关联。e k f 对每一个观测量保持了唯一的数据关联的假设。代表性的 选择了可能性最大的。如果当前环境特征观测量的概率太低，考虑新的环境特征的可能性。 如果这个数据关联是错误的，加入这个观测量到e k f 的影响就可能一直无法消除这是著 名的e k f 失败模型“”下面的部分会描述另一种方法解决s l a m ，用有效的测量和有效的数 据关联解决。 2 7s l a m 地图描述方法 2 7 1 子地图法 卡尔曼滤波器是最佳的解决线性高斯s l a l l 问题的方法，但它对大环境计算是不可行 的。因为协方差矩阵，描述了每两个状态变量的相关性，加大了e k f 计算的复杂度。加入 一个独立的环境特征的观测量必然会对其他的状态变量产生影响。 观测一个独立的环境特征对远处的环境特征的位置影响不大。因此很多研究者发展了基于 e k f 的s l a m 算法，把全局地图分解成几个小得的部分。推迟( p o s t p o n e m e n t ) 溆棚和压缩 e k f ( c e k f ) 都是在机器人位于单个的子她图时，把环境特信息延迟加入到全局地图中。 因为这些方法产生跟e k f 相同的结果，仍然是最佳的然而，这两个算法要求的计算量通过 一个常量因数降低了，因为全局地图更新执行的频率降低了。 把全局地图划分成子地图可以使地图元素间的相关性的描述更稀疏。增加的稀疏性可 以被用来计算更加有效的传感器更新。网络分组特征地图“1 、a t l a s 。1 、局部地图算法“， 还有分离随机地图创建框架m 都考虑子地图的稀疏网络之间的相关性。当机器人离开一个 子地图，重