（模式识别与智能系统专业论文）移动机器人同时定位与地图创建自适应算法研究.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 t f ) 的自适应 u k f - s l a m 算法。论文的主要工作及贡献包括： 1 对移动机器人导航系统进行了建模。所建立的模型主要包括环境地图模 型、机器人位置和运动模型、传感器观测模型，噪声模型等。这些基本模型是 s l a m 问题的研究的平台基础。 2 对传统s l a m 方法进行了分析。分析表明，扩展卡尔曼滤波算法( e k f ) 是 传统的非线性系统线性化方法，可以用于解决同时定位与地图创建问题；粒子滤 波算法( p f ) 具有状态估计精确度高的优点，不过需要随机产生大量粒子，需要 在导航的实时性上加以改进；无迹卡尔曼滤波算法( u k f ) 利用确定性采样点来 近似状态向量的概率分布，可以避免粒子退化问题。 3 针对上述几种s l a m 算法存在鲁棒性和自适应性差的缺点，提出了基于强跟 踪滤波器( s t f ) 的自适应u k f - s l a m 算法。针对s t f 具有极强的模型失配的鲁棒性， 且具有概念清晰、计算简单的优点，本文通过融合u k f 和强跟踪滤波器来优化s l a m 算法，获得u k f - s l a m 算法，该算法中的每个s i g m a 点均进行s t f 更新，通过s t f 在线 调节因子来相应调节滤波增益，达到提高自适应s l a m 算法的自适应调整能力和鲁 棒性的目的。 本文最后一章对全文进行总结，并且对同时定位与地图创建的研究前景进行 了展望。 关键词：同时定位与地图创建系统建模扩展卡尔曼滤波无迹卡尔曼滤波 粒子滤波强跟踪滤波器自适应算法 a b s t r a c t ab s t r a c t m o b i l er o b o tn a v i g a t i o ni sa ni m p o r t a n tp r o b l e mi nt h er o b o tr e s e a r c hf i e l d l o c a l i z a t i o ni st h eb a s i cf u n c t i o nt h a tr o b o ts h o u l dp o s s e s s ，w h i l ea tt h es a m et i m e ， l o c a l i z a t i o na n dm a p p i n gh a sac l o s er e l a t i o n s h i p i no r d e rt op r o v i d eam o b i l er o b o t w i t ht r u l ya u t o n o m o u sc a p a b i l i t i e s ，t h er o b o tl o c a l i z a t i o na n dm a p p i n g ( s l a m ) m u s t b es e e na so n ep r o b l e mt ob es o l v e d t h es u c c e s ss o l u t i o no fs l a mi so n eo f r e m a r k a b l ea c h i e v e m e n ti nt h er o b o tf i e l da n dh a sb e e nw i d e l yu s e di no u t d o o r , u n d e r w a t e ra n dl a n de n v i r o n m e n t s f a c e dw i t hr e a le n v i r o n m e n tw i t hc o m p l e x i t ya n d d y n a m i cc h a r a c t e r i s t i c s ，i no r d e rt oi m p r o v et h ea u t o n o m o u sc a p a c i t yo fm o b i l er o b o t ， t h es l a ms o l u t i o nw i t ha d a p t a b i l i t y , r o b u s t n e s sa n dh i g he f f i c i e n c yi st h er e s e a r c h f o c u so ft h er o b o tf i e l d t h ep a p e rf o c u s e so nt h er e s e a r c ho na d a p t i v ea l g o r i t h mo fm o b i l er 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 t od e a lw i t ht h ed e f e c t so ft r a d i t i o n a ls l a m a l g o r i t h m s ，t h ep a p e rp r o p o s e dt h ei m p r o v e ds o l u t i o nw h i c hm a k e st h es l a m a l g o r i t h m sm o r ea d a p t i v ea n dr o b u s t t h ea u t o n o m o u sn a v i g a t i o nc a p a c i t yc a nb e i m p r o v e dt h e r e f o r e t h em a i nc o n t r i b u t i o n so ft h i s t h e s i si n c l u d et h ef o l l o w i n g a s p e c t s ： f i r s t ，t h ep a p e re s t a b l i s h e dt h em o d e lo fm o b i l er o b o tn a v i g a t i o ns y s t e m m a i n m o d e l st h a tw i l lb eu s e di nt h er e s e a r c hi n c l u d e ：e n v i r o n m e n tm a p sm o d e l ；r o b o t l o c a t i o na n dm o t i o nm o d e l ；s e n s o ro b s e r v a t i o nm o d e l s ；n o i s em o d e l ，e ta 1 t h e a c c u r a t em o d e l i n gw i l le s t a b l i s ha p p r o p r i a t ep l a t f o r mb a s e m e n tf o rt h es l a m r e s e a r c h s e c o n d ，t h ep a p e rr e s e a r c h e ss o m et r a d i t i o n a ls l a mm e t h o d s f r o mt h er e s e a r c h , w ek n o wt h a te x t e n d e dk a l m a nf i l t e r ( e k e ) i st h et r a d i t i o n a lm e t h o do fl i n e a r i z a t i o n o fn o n l i n e a rs y s t e ma n di tc a ns o l v et h em o b i l er o b o ts 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 gp r o b l e m t h eb e t t e ra s p e c to fp a r t i c l ef i l t e r ( p f ) i si t sh i g ha c c u r a c yi ns t a t e e s t i m a t i o n ，b u tw h i l ei t sp r o b l e mi st h a tt h ew h o l ea l g o r i t h mw i l lg e n e r a t el a r g e a m o u n t so fp a r t i c l e s s oi no r d e rt oa c h i e v er e a l t i m en a v i g a t i o n ，t h ep a r t i c l ef i l t e r n e e d st ob ei m p r o v e d u n s c e n t e dk a l m a nf i l t e r ( u k f ) d e s c r i b et h ep r o b a b i l i t y d i s t r i b u t i o no fs t a t ev e c t o rb yc e r t a i n t ys a m p l i n gm e t h o d sa n di tc a na v o i dp a r t i c l e d e g r a d a t i o n t h e n ，t od e a lw i t ht h ep r o b l e mo fa b o v ea l g o r i t h m ss u c ha sb a dr o b u s t n e s sa n d i i i a d a p t a b i l i t y ，t h ep a p e rp r o p o s e d a na d a p t i v es l a ma l g o r i t h mb a s e do ns t r o n g t r a c k i n gu k f t h ea d v a n t a g eo fs t f i st h a ti t ss t r o n gr o b u s t n e s so fm o d e lm i s m a t c h ； a n da l s oi th a sc l e a rc o n c e p ta n dl o wc o m p u t a t i o n a lc o m p l e x i t y t h ei m p r o v e ds l a m a l g o r i t h mc o m b i n e st h es t r e n g t h so fs t r o n gt r a c k i n gf i l t e r ( s t f ) a n du k f i nt h e p a p e r ，e a c hs a m p l i n gp o i n to fu k f i su p d a t e db ys t f , a n dt h ef i l t e rg a i nw a sa d j u s t e d o n l i n e t h e r e f o r e ，a d a p t a b i l i t ya n dr o b u s t n e s sw a si m p r o v e d a tl a s t ，t h ep a p e rs u m m a r i z e dt h ew h o l er e s e a r c hc o m m i t m e n t a n da l s og i v et h e p r o s p e c to ft h em o b i l er o b o ts 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 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 ，s y s t e mm o d e l i n g ，e x t e n d e d k a l m a nf i l t e r ；u n s c e n t e dk a l m a nf i l t e r , p a r t i c l ef i l t e r , s t r o n gt r a c k i n gf i l t e r ，a d a p t i v e a l g o r i t h m i v 中国科学技术大学学位论文原创性声明 本人声明所呈交的学位论文，是本人在导师指导下进行研究工作所取得的成 果。除已特别加以标注和致谢的地方外，论文中不包含任何他人已经发表或撰写 过的研究成果。与我一同工作的同志对本研究所做的贡献均已在论文中作了明确 的说明。 作者签名：数羹丞 签字日期： 中国科学技术大学学位论文授权使用声明 作为申请学位的条件之一，学位论文著作权拥有者授权中国科学技术大学拥 有学位论文的部分使用权，即：学校有权按有关规定向国家有关部门或机构送交 论文的复印件和电子版，允许论文被查阅和借阅，可以将学位论文编入有关数据 库进行检索，可以采用影印、缩印或扫描等复制手段保存、汇编学位论文。本人 提交的电子文档的内容和纸质论文的内容相一致。 保密的学位论文在解密后也遵守此规定。 囱公开口保密( 年) 作者签名：邀童盛导师签名： 签字日期：坐颦二进 签字日期： 第一章绪论 1 1 引言 第一章绪论帚一早瑁比 机器人的诞生和机器人学的建立和发展是2 0 世纪人类科学技术进步的重大 成果。随着计算机应用、电子技术、人工智能和机械理论等领域的巨大成就，机 器人自诞生以来，几十年来取得了飞速发展。随着机器人技术的发展，具有高度 智能化特点和具有现实使用价值的机器人，深入到我们的生产生活中。这其中， 有一类机器人可以通过传感器感知周围环境和自身状态，实现在有障碍物的现实 环境中面向目标的自主运动，从而完成特定的作业功能，称为自主式移动机器人 $ , 1 0 , 4 8 1 。自主移动机器人的应用性随着研究的深入不断加强，不仅仅在工业制造 领域，而且在军事、民用、太空、水上、陆地各种环境也有广泛应用。 自主移动机器人导航技术主要解决以下几个方面的问题n 4 9 】：通过各种传感 器采集数据，从而获得环境中机器入的位置、方向、环境地图等信息；对采集的 数据进行处理，建立机器人的位置和环境地图模型等：利用建立的模型，寻找一 条良好的无障道路，引导自身安全行驶或跟踪己知路径达到目标位置。 1 2自主移动机器人综述 1 2 1 自主移动机器人研究意义 移动机器人的研究始于2 0 世纪6 0 年代末【4 1 ，最早的自主移动机器人s h a k e y ， 是由斯坦福研究所( s 对) 的n v i s s e n 和c h a r l e sr o s e n 等人研制出来的，如图1 1 所示。 其目的是研究人工智能技术以及在复杂环境下机器人系统的自主推理、规划和控 制。在二十世纪七十至八十年代，机器人的研究方向偏向于真正高自主的移动机 器人研制，美国和日本很多大学的学者都开始研究移动机器人仿真平台。2 0 世纪 9 0 年代以来，以研制高水平的环境信息传感器技术、信息处理技术、高适应性的 移动机器人控制技术和真实环境下的规划技术标志，开展了移动机器人更高层次 的研究。近年来，国外出现了许多新型机器人，如图1 2 所示的探月机器人和图 1 3 9 i 示的h o n d a 公司的智能机器人。 传统的工业机器人主要应用于制造和工业领域。随着社会生产生活的要求， 具有自主感知决策和执行功能的移动机器人日益受到重视。这种情况下，具有自 第一章绪论 我感知能力”的智能移动机器人应运而生。移动机器人最大的特点就是能够自由 移动，这种区别于传统工业机器人的能力使其适应各种情况胜任更多的工作， 能够替代很多传统机器人的工作。例如应用在太空探索、战场搜寻、深海探测等 条件比较艰苦危险、情况复杂的环境。智能移动机器人技术的发展对于一个国家 科技发展、现代工业和军事航空事业的发展具有非常重要的作用，因此日益受到 世界科技领域的关注。 纛盏国 图1 1 机器人s h a k c y 图1 2 探月机器人图1 , 3 智能机器人 122 机器人导航的研究内容 自主式移动机器人的导航是指机器八按照预先给定的任务命令，根据传感器 获得的信息进行地图创建和路径规划。并且在移动的过程中，不断感知周围的局 部环境信息自主地作出各种决策，从而不断的调整自身的位置和方向，引导自身 无碰、快速并且安全地到达目标位置”。移动机器人导航技术的主要研究内容有 三个方面”1 ：导航方式、定位方法以及路径规划方法。 移动机器人的常见的导航方式主要有：陆标导航”、环境地图模型匹配导 航”和视觉导航”。陆标导航是指机器人通过对陆标的检测来确定自身位姿。 在得知陆标在环境中的坐标点、形状等特征的前提下，将整个路线分解成为路标 之间的片断，从而币断地对陆标探测来完成导航；环境地图模型匹配导航是机器 人通过自身和携带的传感器，探测周围环境信息，利用感知数据进行局部地图创 建，将地图创建结果与其预先存储的完整地图进行模型匹配。如果模型匹配度高， 机器人即可确定自身的位姿，这样就可以利用全局规划的路线，采用路径跟踪和 避障技术，实现自主导航；视觉导航方式是目前最常用的导航方式，具有信息探 测范围广泛、数据信息完整可靠等优点。目前国内外应用最多的是采用在机器人 上安装车载摄像机的基于局部视觉的导航方式，其主要功能是要完成完成障碍物 和陆标的观测和识别。 移动机器人的定位技术9 1 可分为相对定位技术和绝对定位技术。相对定位技 第一章绪论 术主要有测距法【1 0 1 和惯性导航法【l 们。惯性导航法主要利用陀螺仪和加速度计实 现定位，根据测量数据的一次积分和二次积分可分别求出角度和位置参量。测距 法以位移方程为基础，其优点是具有良好的短期精度和较高的采样速率。相对定 位技术的缺点在于仅适于短距离和短时间的定位，因此在实际应用中，通常与绝 对定位技术相结合运用，集中两者的优点，以获得更加精确的位置估计p 】。绝对 定位技术中使用较为广泛的有路标定位一】、视觉定位5 5 1 和全球定位系统1 。 人工路标定位是在复杂的环境中，设置一些坐标和形状已知的路标，机器人通过 对路标的探测来确定在环境中的位姿。视觉定位是一种新的移动机器人定位方法， 通常视场是视觉定位系统性能衡量标准之一，因此通常情况下，视觉定位会采用 全景传感器来实现。全球定位系统用于解决机器人定位时存在近距离定位精度低 的缺点，目前在各种领域应用非常广泛。 路径规划是机器人导航的重要内容之一，也是能否准确到达目标位置的关键 步骤，它主要是根据某一性能指标搜索一条从起始状态到目标状态的最优和近似 最优的无碰路径 1 0 , 1 3 l 。目前主要分为两种类型：全局路径规划d 6 1 和局部路径规划 哪! 。两者的不同之处在于，前者是基于环境先验信息，后者是基于传感器信息。 各种未知的情况需要通过传感器获得。全局路径规划的主要方法有：可视图法 s 6 1 、自由空间法【5 7 】、拓扑法 5 s l 和栅格法p 9 1 等。局部路径规划的主要方法有：人 工势场法、遗传算法( g a ) 6 1 1 。目前除了这两种，还有基于行为的路径规划方 法。其中以m i t 的包容式体系结构 6 2 1 最有代表性。 1 2 3 移动机器人导航技术发展趋势 目前移动机器人的发展主要具有如下几个特点：应用环境的扩展，从传统的 简单环境到现在的室外大环境、太空、水下等等；应用领域的扩大，从传统工业 到航天、军事、民用等各种场合；同样，与过去单一的机器人应用相比，现代技 术的发展需要要求实现多机器人协作，以提高生产效率。这些发展特点对自主式 移动机器人导航提出了新的要求，因此现在的智能导航也呈现出如下新的发展趋 势：( 1 ) 机器人导航系统的分布式智能结构的研究。分布式智能结构把决策级的 操作分解到机器人本身的计算机和远端的主计算机，这样不仅可以提高系统的实 时性和鲁棒性，同时也可以减轻机器人的体积和自重【4 】。( 2 ) 如前所述，视觉导航 基于其具有的优点，在以后的导航应用中将会应用更为广泛。目前，视觉导航技 术已有很多研究成果，但是在实时性方面仍然有待改进，随着计算机技术和模式 识别技术的发展，相信视觉导航能够得到更好的发展d 1 。( 3 ) 虚拟现实( v r ) 技 术汇集了计算学、信号学、电子学等多方面的技术。将v r 技术应用到智能机器 人导航系统，可以为传感器融合技术提供良好的技术平台p 1 。( 4 ) 基于网络的 第一章绪论 机器人遥控和基于网络的多机器人协作也是智能导航的一个发展方向。目前网络 的应用非常普及，多机器人系统的导航也是一个研究热点。利用人机交互技术、 空间通信手段以及远程监控技术可以实现网络机器人遥控和多机器人协作1 4 。 1 3 移动机器人同时定位与地图创建综述 移动机器人实现自主导航的前提是自主定位，即能够准确地确定自身在环境 中相对于全局坐标系的位姿。传统的导航方式如陆标导航和环境地图是基于里程 计估计的，在复杂环境下会存在定位误差。地图创建也是导航中一个重点问题。 定位问题和地图创建问题密切关联，己知环境地图的定位问题和已知定位的地图 创建问题己经被广泛研究。但是当环境较为复杂时，地图和机器人的位置都事先 未知，要求机器人在一个完全未知的环境中从一个未知的位置出发，在递增地建 立环境的导航地图的同时，利用已建立的地图来同步刷新自身的位置。在上述问 题中，机器人位置和地图两者的估算是高度相关的，这样一种相辅相生、不断迭 代的过程，曾有人比喻成“鸡和蛋”的问题。被学术界称为同时定位和地图创建 ( 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 ) p , s o ，简称s l a m 问题。随着移动机器 人应用环境的扩展以及非结构化的特点，势必导致s l a m 问题成为移动机器人实 现自主导航能力的重要课题，日益受到关注。 1 3 1 s l a m 问题涉及的理论和技术 s l a m 问题包含四个基本方面：一是环境信息的提取问题，机器人在环境移 动的过程中，如何利用自身携带或者外界的传感器获取数据，这涉及到传感器选 择以及信息提取的问题；二是环境描述问题，即环境地图表示方式；三是如何表 达传感器信息，并且利用这些信息进行定位和地图创建，这其中涉及到的一个关 键技术是不确定信息的处理方式；四是s l a m 解决方法，随着应用环境的日益复 杂以及对于导航精确度要求不断提高，发展具有高适应性和强鲁棒性的s l a m 方 法是研究的重点。 1 3 1 1 环境特征提取 移动机器人的传感器主要分为自身的内部传感器以及外部传感器两种1 7 娜】。 内部传感器主要有里程计、陀螺仪等，用于检测机器人内部系统参数；外部传感 器有视觉、激光、声纳、红外等，获取的信息主要以长度、宽度、位置等参量表 示环境的几何特征。移动机器人通常需要融合内部传感器信息和环境特征信息进 行定位和导航。在机器人同时定位与地图创建中，目前主要采用的是声纳或激光 4 第一章绪论 传感器，可以通过距离、角度、强度等扫描信息得到外部环境的轮廓特征及其位 置信息。声纳传感器1 6 7 1 的优点主要在于低成本、较宽的波束覆盖范围。但是其 缺点也非常明显，例如分辨率低，容易产生虚假和多重反射回波信号，从而导致 特征匹配精度不高。激光传感器近几年得到广泛应用，快速、扫描精度高、角分 辨率高、方向性强等优点使得其在各种环境中比声纳传感器具有更多的优势。目 前视觉传感器也经常用于机器人导航，结合视觉信息与几何特征信息可以更好的 用于定位与地图创建。 在机器人定位与地图创建中，特征的鲁棒性和稳定性相对于特征的细节更为 重要。室内环境中，特征提取主要能够反映大范围环境的结构化特征，如线段、 角、点、垂直线等，分别与墙、墙角、凸角、门等特征对应。这些几何特征的提 取主要采用采取哈夫变换法、直方图法、三角测量融合法等7 1 。提取的不同特 征的组合可以形成不同的特征类，联系在一起之后，单一特征就可以和一个特征 类相匹配。 近年来，基于多传感器信息融合 2 4 1 的特征提取方法越来越受到人们的重视， 来自不同传感器可以充分理解环境特征，有利于机器人作出正确决策。多传感器 融合常用的方法有心4 】：加权平均法、贝叶斯估计、卡尔曼滤波、模糊逻辑、人 工神经网络等。加权平均是一种底层数据融合方法；贝叶斯估计是根据已有事实 推断未来的概率推理方法；卡尔曼滤波用模型的统计特性递推决定统计意义下的 最优融合数据估计。人工神经网络通过一定的学习算法融合传感器数据，获得网 络参数。 1 3 1 2 地图的表示方式 构建并维护一个准确的环境地图是进行精确定位的重要前提，也是自主导航 中的一个重要内容。机器人利用对环境的感知信息来对现实世界进行建模，自动 地构建环境地图。典型的地图表示方式大致可分为3 类：栅格地图4 1 、特征地图列 和拓扑地图【1 6 1 。 栅格地图如图1 4 所示。首先由e l f e s 和m o r a v e s 4 1 提出，在机器人的位姿估计、 路径规划、避障控制中均得到了广泛应用1 6 3 , 6 4 1 。栅格地图将工作空间分为若干栅 格，每一个栅格代表环境的一部分，并包含一个表示该单元格被占据可能性的概 率值。栅格地图是一种表示静态环境的方法，易于创建和维护。由于其对特定感 知系统的参数的敏感性，使其具有较强的鲁棒性。但是栅格地图的缺点在于低精 度和低实时性。栅格地图是一个近似的解决方案，在大规模的室外环境下、环境 较为复杂或者环境划分比较详细的情况下，信息量也显著增大，对于栅格地图的 维护所占用的内存t n c p u 时间迅速增长，使得实时处理变得很困难。 特征地图是一种紧凑的地图表示方法，通过传感器感知的环境数据，从中提 第一章绪论 取抽象的参量化特征如线段、圆、等来表征环境，如图l5 n 示。几何特征的提 取需要对感知信息作额外的处理，且需要一定数量的感知数据才能得到结果。有 关学者最早利用激光测距仪提取直线特征旧1 。对于大多数环境特别是室内环境 利用几何特征描述的地图都能精确的反映环境信息，而且几何特征地图存储信息 量小，方便应用于位姿估计和目标识别。特征地图法具有直观、精度高的特点，运 用参量法描述几何特征尤其适合于不同坐标系之问的转换虬及对传感器信息融 合技术。 豳。鲞 淘潮 a mk 圈14 栅格地图示例 拓扑地图也是一种紧凑的地图描述方法，适用于大而简单的环境。最早由 k u i p e r s 和b y u n 提出”并得到成功应用。如图16 所示。拓扑地图把室内环境表 示为带结点和相关连接线的拓扑结构罔，其中结点表示环境中的重要位置点 ( 拐角、门、电梯、楼梯等) ，边表示结点问的连接关系，如走廊等。拓扑地 图占用存储空间较少，这主要由环境的复杂度决定。拓扑地图的优点在于可以实 现快速的路径规划，甚至在不知道移动机器人准确的位姿信息的情况下，为人机 交互下达指令提供了一种更为自然的接口。但是由于环境表示未结点和相关连接 线，难以创建和维护，当传感器误差较大时例如声纳传感器，环境中若存在两 个很相似的地方将很难确定这是否为同一节点。 1313 不确定信息处理 机器人导航需要准确的定位和地图，但是机器人本身本体的运动模型、观测 模型及传感器的准确性和系统噪声是决定地图精度的一个关键。而在现实中，这 些渠道由于其模型的局限性，获得的信息都带有不同程度的不确定性。感知信息 的不确定必然导致地图和定位的不确定，不确定性会逐步传递，因为依据地图和 感知信息决定了后来的决策。这样会导致机器人避障和导航也就带有不确定性。 不确定性传递过程如图1 7 所示。 第一章绪论 对于不确定信息问题的解决目前主要使用的是模糊逻辑和概率”的方 法，如贝叶斯估计，卡尔曼滤波、马尔可夫推理、模糊逻辑等1 。以概率描述信 息的不确定性( 如贝叶斯估计、马尔可夫推理) 其优点在于：非常适合不确定模 型。即使传感器精确度低也能表示其感知信息，在实际应用中鲁棒性和自适应性 较好，因此得到广泛应用。缺点是计算量较大；模糊逻辑的优点在于；对多种不 确定性信息进行建模和融合时可以有更多的操作符供选择。因此为处理不确定信 息提供了较为鲁棒并且有效的工具，尤其是处理声纳传感器信息。 圈l5 特征地图示例 圈i 6 拓扑地图示倒 13 2s i a m 的宴现方法 从概率统计的观点来看，s l a m 是一个滤波问题。求解s l a m 问题，就是 第一章绪论 根据系统的初始状态以及从0 到t 时刻的控制信息与观测信息估计系统的当前状 态【3 】。目前s l a m 方法大致可以分为两类：一类是基于概率模型的方法s 2 6 工7 1 ， 例如基于卡尔曼滤波的方法如扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波 ( r a o b l a c k w e l l i s e d ，f a s t l s l a m ) 等。卡尔曼滤波器运用的前提是假设系统是 线性系统，在此基础上发展起来的e x t e n d e dk a l m a nf i l t e r ( e k f ) ，通过一阶泰勒 展开来近似表示非线性模型。扩展卡尔曼滤波方法是多种算法的基础，已经广泛 运用于机器人定位与地图创建，但是其缺点也非常明显。首先，实际中机器人的 运动模型与观测模型是非线性，因此e k f 用线性系统近似估计非线性系统通常 精度较低；其次e k f 计算量大、而且对数据关联问题处理困难。另一类为非概 率模型方法【7 1 记1 ，主有数据融合、基于模糊逻辑、s m s l a m 扫描匹配等。 图1 7 地图创建中不确定信息处理 1 3 2 1基于扩展卡尔曼滤波的方法 扩展卡尔曼滤波是移动机器人同时定位与地图创建中最基础最重要的理论 方法【3 】，最早由s m i t h 等人提出并应用于s l a m 问题。e k f 解决s l a m 问题的基本 原理是将机器人的位姿向量和环境特征向量合并在一个高维状态向量之中，用扩 展k a l m a n 滤波作最小均方差估计。基本的解决思路如下。系统状态变量为： x ( 尼) = 【毛。( 七) ，m 1 】t ( 1 1 ) 其中， ( 尼) = i x 。( 七) ，( 尼) ，x a k ) 7 表示机器人在k 时刻的位姿； m m ，m ：，川】t ；= 【五，咒】t ；f - 1 , 2 ，3 - f 表示路标集合。 解决s l a m 问题就是求取状态向量x ( k ) 的后验概率密度，即 p ( x v ( k ) ，mi 甜( 七) ，z ( 尼) ) 。 其中，u ( k ) 是控制变量，使得机器人位姿由x v ( k - 1 ) 移动到墨( 尼) ；z ( 尼) 表示 k 时刻在位姿托( 尼) 处对路标的观测值。 e k f s l a m 是应用卡尔曼滤波的原理求取状态变量x ( k ) 的均值z 和协方差 8 第一章绪论 矩阵只，从而计算p ( x ，( 尼) ，ml “( 尼) ，z ( 七) ) 。e k f s l a m 分为预测和更新两个步骤。 首先将机器入的控制信号输入到系统状态方程之中，实现对位姿和地图特征的预 测；然后观测和提取环境地图特征，用来更新位姿和特征地图。e k f 算法用线 性系统近似非线性系统，在处理不确定信息方面有优势，因此广泛应用于机器人 导航领域。自诞生以来，e k f 成为应用最广泛的s l a m 方法，并相继在室内、 结构化道路、水下、小范围室外等简单环境中取得成功。 由于同时定位与地图创建应用环境的拓展，目前，有关e k f 的研究主要面 向非结构化、大范围室外未知环境导航。但由于e k f 在地图中包含了机器人和 环境特征之间的协方差阵，为消除累积误差，每次估计和校正都要处理雅可比矩 阵，由此造成的计算复杂度高，使得其不适用于严重大规模环境。因此，如何降 低算法复杂度是该方法的研究重点。 1 3 2 2 基于粒子滤波的方法 粒子滤波( p a r t i c l ef i l t e r ) t 1 9 2 0 1 是一种应用样本集表示概率密度分布的蒙特卡 洛方法。当样本数量一o o 时可以逼近任何形式的概率分布，因此，粒子滤波 能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于任何状态空 间模型。因此可以用于解决s l a m 问题。r a o b l a e k w e l l i s e d 算法犯0 1 是最主要应用 于s l a m 中的粒子滤波算法，m u r p h y 等最先将该算法用于s l a m 问题9 1 。在此基 础上，美国卡耐基梅隆大学的m o n t e m e r l o 等提出了应用更为广泛的f a s t s l a m 算 法【3 们，并第一次成功应用于真实的机器人。该算法的原理是将s l a m 问题分解成 机器人定位问题和基于位姿估计的环境特征估计问题，综合运用粒子滤波和 e k f 。用粒子滤波算法估计整个路径的位姿，用e k f 估计环境特征的位置，每一 个e k f 对应一个环境特征。该方法融合了e k f 算法和粒子的优点，不仅降低了计 算的复杂度，又具有良好的鲁棒性，而且在处理数据关联问题上具有很好的效果。 粒子滤波解决s l a m 问题的优点在于精度高，但是此算法仍然存在一定的局 限性，最主要的表现在需要大量的样本才能够很好的近似系统的后验概率密度。 因此当机器人导航的环境较为复杂时，如果能够良好估计系统的状态，需要样本 的数量也大量增加，由此造成算法复杂度增加。此外，p f 算法的重采样步骤， 这个阶段会经常导致样本有效性和多样性的损失，造成样本贫化现象。综上，未 来p f 的研究重点主要集中在两个方面：一是能够克服例子贫化现象，保证粒子 的多样性和有效性；二是如何利用其它的采样策略以减少样本数量，从而复杂环 境下p f 算法的复杂度。如无迹卡尔曼滤波算法。 1 3 3 s l a m 方法存在的问题及解决方案 9 第一章绪论 1 3 3 1 超多维问题 s l a m 需要计算机器人的位姿x v ( k ) = 【x 。( 七) ，( 七) ，( 尼) 】7 和环境中的特征标 志的位置m = 所。，m 2 ，】；。在二维空间中机器人的位姿包括3 个变量，特征标 记包括2 个变量。这样的话，如果地图中有n 个标志，那么就对应有2 n + 3 个变量， 每个变量是状态估计中的一维，s l a m 的状态是一个2 n + 3 维的矢量。而对实际环 境进行描述可能需要上千甚至上万个特征标志，因此s l a m 是一个超多维问题。 1 3 3 2 数据关联 数据关联是指不同时刻对于环境的观测是否来源于同一特征，在s l a m 问题 中，数据关联的两个任务是：特征匹配和新环境特征的检测。如图1 8 所示。 如前所述，e k f 对于数据关联问题非常敏感，比较严重的情况是数据关联不准确 会导致滤波器发散。数据关联体现在特征之间的相似度和它们之间的相对距离。 常见的数据关联多采用最邻近算法，方法认为传感器观测值和特征之间的每 个匹配均是独立的，没有考虑相邻特征标志之间的相互关系，在路标距离较近 时容易失效。m o u t a r l i e r 等采用启发式方式3 1 从机器人估计位置除非搜寻机器人 旋转和位移变化，实现感知特征与己存储特征的最佳匹配；还有约束搜索算法等 等都用于解决数据关联问题。 f i g 1 8 c o m b i n e dc o n s t r a i n td a t aa s s o c i a t i o n ( c c d a ) p e r f o r m sb a t c h - v a l i d a t i o ng a t i n gb y c o n s t r u c t i n g a n ds e a r c h i n gac o r r e s p o n d e n c eg r a p h t h eg r a p hn o d e sr e p r e s e n t a s s o c i a t i o n st h a ta r ep o s s i b l ew h e nc o n s i d e r e di n d i v i d u a l l y t h ee d g e si n d i c a t e c o m p a t i b l ea s s o c i a t i o n s ，a n dac l i q u ei sas e to fm u t u a l l yc o m p a t i b l ea s s o c i a t i o n s ( e g ， t h ec l i q u e2 ，6 ，1 0i m p l i e st h a ta s s o c i a t i o n sa l - - - b 2 ，a 2 - - + b 3 ，a 4 - - - b lm a yc o e x i s 0 。 1 3 3 3 累积误差 1 0 第一章绪论 s l a m 中的误差主要来自三个方面：控制信息误差、观测误差和错误的数据 关联带来的误差。在己知环境地图的情况下进行机器人定位，误差可以通过已知 的环境特征对控制信号的误差进行补偿。由于s l a m 问题是关于位置环境下的定 位与地图创建问题，所以环境中的特征标志的位置都是未知的，因此不可以通过 观测环境特征信息纠正控制信号误差，这将导致机器人的位置误差随着机器人的 运动距离而增大。错误的机器人位置误差导致错误的数据关联，从而增大环境特 征位置误差；由此机器人位置误差和环境特征位置误差相互影响、互相关联。由 此而导致的机器人和地图特征的位置估计产生累计误差，破坏地图的一致性。 1 3 3 4 降低复杂度 s l a m 问题的计算复杂性，主要源于地图更新过程中需计算机器人与环境特 征对应的协方差矩阵。因为同时定位与地图创建是机器人位置特征与环境地图特 征之间的相对观测，所以机器人位姿的估计误差与地图的估计误差强相关，在估 计过程中必须保存两者之间的相关性( 即整个协方差阵) 来得到估计的一致性。由 此产生计算复杂度高，导致不能满足大规模复杂地图创建和导航实时性的需要。 因此降低计算复杂度的研究主要集中于使用较少的环境特征、改善地图表示方 式、局部子地图法、去相关法等方面。 使用较少的环境特征。从定位的角度来说，良好的滤波器能够准确快速的提 供移动机器人状态估计。s l a m 问题中，定位的收敛速率以及全局地图精度决定 于高质量的路标。过多的路标会导致计算的复杂度，因此如何在不影响定位收敛 速度和地图创建精度的前提下，选取那些对收敛速率有决定性作用的特征，显得 尤为重要。当然，很多情况下，部分路标信息的丢失将导致次优估计，但是由此 带来的计算复杂度的降低还是值得的。l e o n a r d 等人1 为每个特征赋予一个可信 度，如果特征从几个位置中一致可靠检测出，则可信度增加，否则就将错误或低可 信度的特征清除。d a v i s i o n 等嘶1 利用主动感知方法减少特征数目。d i s s a n y a k e 等口” 通过选择高可靠性路标减少算法的计算量，此外在机器人离开区域时从地图中 删除部分环境特征，再次限制地图尺寸。 改善地图表示方式。如前所述，机器人导航研究领域里，地图表示方法主要 有：栅格地图法、特征地图法和拓扑地图法。基于各种地图表示方法的特点，在 s l a m 中为了保证实时性，减少存储空间，一般采用特征地图法和拓扑地图法。但 是由于这两种地图表示方法的局限性，例如在大规模复杂环境中单纯采用特征地 图法，难以保证地图的全局一致性；而单纯采用拓扑地图法不能够保证定位的精 确度。集中两种地图表示法的优点，近年来，s l a m 问题经常采用将特征地图法 与拓扑地图法相结合的地图表示方式。这样既可以保证定位的精确度，同时又可 以保证地图的全局一致性。 第一章绪论 局部子地图法。局部子地图法从空间的角度将s l a m 分解为一些较小的子问 题。子地图法中主要需要考虑以下几个问题：如何划分子地图，如何表示子地图 间的相互关系，如何将子地图的信息传递给全局地图以及能否保证全局地图的一 致性。最简单局部子地图方法是不考虑各子地图之间的相互关系，将全局地图划 分为包括固定特征标志数的独立子地图，在各子地图中分别实现s l a m 。但是， 由于丢失了表示不同子地图之间相关关系的有用信息，这种方法不能保证地图的 全局一致性。对此，l e o n a r d 等人提出了d s m 方法m 】，d s m 中各子地图分别保存 自己的机器人位置估计，当机器人从一个子地图a 进入另一个子地图b 时，采用 基于e k f 的方法来将子地图a 中的信