未知环境下基于SLAM的移动机器人导航算法研究.doc

1、武汉理工大学硕士学位论文摘 要 智能移动机器人是一种在复杂的环境下工作的具有自规划、自组织、自适应能力的机器人。导航算法的研究是智能机器人研究领域的一个热点话题。智能导航的目的就是在没有人干预下使机器人有目的地移动并完成特定任务，进行特定操作。本文旨在结合国家自然科学基金和湖北省青年杰出人才基金项目的需求，研究移动机器人同时定位与制图（Simultaneous Localization and Mapping，SLAM）问题，并在此基础上，研究机器人导航算法，使得机器人在完全未知结构化环境中实现“完全自主”。 文章首先回顾了导航技术的发展，对导航技术中的核心问题：定位与制图进行了详细分析，指出

2、了机器人定位与制图研究中存在的问题，包括制图复杂度、数据匹配难度以及定位制图关联度等；在此基础上引出了本研究的重点：移动机器人同时定位与制图（SLAM）算法，并对其算法构架、属性、以及分类等相关内容进行了介绍。针对基于点特征的同时定位与制图算法中存在的计算复杂度与信息丰富度之间的矛盾，本文提出了一种基于线特征的同时定位与制图算法。文章对基于线特征的SLAM算法进行了详细的阐述，给出了包括机器人运动模型、观测模型的建立、数据匹配、状态更新、地图建立、地图管理等方面的相关公式；并通过仿真实验证实了本算法的正确性与可靠性。针对导航算法中对路径规划的要求，本研究中着重考虑了机器人的局部规划即：避障；并

3、对避障算法VFH/VFH进行了剖析。根据VFH/VFH算法与SLAM算法的特点，将二者相结合，形成了一种新的机器人导航算法。该导航算法较基于VFH/VFH的导航算法而言，使用范围更广，导航效果更佳。关键词：导航，同时定位与制图，VFH/VFHAbstract Intelligent robots are a kind of robots that are able to work in complex environments with the capacities of self-organizing and self-planning. Navigation problem is a ho

4、t issue concerned in researching on such kind of robots. Its aim is to move purposely and do the job without aids. With supports of the project of National Natural Science Foundation of China and the project of Excellent Youth Fund in Hubei province, this paper aims to develop a new algorithm for na

5、vigating based on the Simultaneous Localization and Mapping (SLAM) algorithm to make the robots totally autonomous in the unknown but structured environments. The navigation techniques are firstly reviewed in this paper and then the Simultaneous Localization and Mapping (SLAM) problem is introduced,

6、 based on the analysis of the localization problem and the map building problem which are two key points in the navigation techniques, including its structure, characteristics, categories and so on. As there is a conflict between the requirements of computational complexity and information-richness

7、within the point-feature based SLAM algorithm, a line-feature based SLAM algorithm is well presented in this paper. All operations required for building and maintaining this map，such as model-setting, data association, and state-updating are described and formulated. This approach has been programme

8、d and successfully tested in the simulation work To meet the need of path planning in navigation, here we mainly concern the local planning, that is, obstacle avoiding. We introduce the VFH/VFH+, an algorithm for obstacle avoidance, in detail in the paper. What is more, in our work, we combine VFH a

9、nd SLAM together to develop a new way for navigation, which has more applications and behaves better than the one based merely on VFH does.Key words: navigation, SLAM, VFH/VFH目 录第1章 绪 论11.1课题研究的目的与意义11.2课题研究的内容21.3论文组织结构2第2章 移动机器人导航技术42.1机器人导航概述42.2移动机器人导航方式42.3移动机器人的定位问题62.4移动机器人的制图问题72.4.1 地图的类型72

10、.4.2机器人制图存在的问题82.4.3 同时定位与制图问题92.5 本章小结10第3章 同时定位与制图（SLAM）算法113.1 同时定位与制图算法介绍113.1.1 SLAM算法性质113.1.2 SLAM算法分类123.2 基于特征SLAM算法构架简介143.3传感器的选择及特征提取153.3.1传感器选择153.3.2特征提取173.4噪音模型建立173.5滤波技术简介173.6卡尔曼滤波简介183.6.1线性卡尔曼滤波简介193.6.2 扩展卡尔曼滤波（EKF）简介213.7 本章小结22第4章 一种基于线特征SLAM算法研究244.1过程模型建立244.2 观测模型的建立264.3

11、 算法的实施264.3.1 机器人运动初始化264.3.2 数据匹配274.3.3 状态更新304.3.4 新信息处理314.3.5 地图管理324.4 SLAM算法仿真334.4.1 仿真环境的创建334.4.2 SLAM算法示例354.5本章小结37第5章 机器人避障算法385.1 常见的避障算法介绍385.2 VFH避障算法介绍395.2.1 （Certainty Value）值的确定395.2.2 VFH算法阐述415.2.3 VFH+ 算法阐述435.3 VFH+算法仿真475.3.1 仿真环境的创建475.3.2 仿真结果说明495.4 本章小结51第6章 基于SLAM的导航算法5

12、26.1基于SLAM的导航算法介绍526.2基于SLAM的导航算法特点536.3导航新算法的仿真546.4本章小结55第7章 全文总结及展望567.1全文总结567.2本文主要贡献577.3未来研究方向57参考文献58致 谢62作者攻读硕士期间参与项目及发表的论文63- 64 - - 第1章 绪 论 机器人的诞生和机器人学的建立和发展是20世纪自动控制最具说服力的成就，是20世界人类科学技术进步的重大成果，而作为机器人中重要分支之一的移动机器人更是给人们带来了无限的惊喜。移动机器人的研究始于20世纪60年代末期。斯坦福研究所（SRI）的Nils NVILSSEN和Charles Rosen等人

13、，在1966年至1972年中研制出了取名Shakey的自主移动机器人，其目的是研究、应用人工智能技术以及在复杂环境下机器人系统的自主推理、规划和控制；20世纪70年代末，随着计算机的应用和传感器技术的发展，移动机器人研究又出现了新的高潮；20世纪90年代以来，以研制高水平的环境信息传感器和信息处理技术，高适应性的移动机器人控制技术、真实环境下的规划技术为标志，开展了移动机器人更高层次的研究1。目前，移动机器人正向着具有自组织、自学习、自适应的智能化方向发展，导航能力的高低是移动机器人智能化水平的重要体现。随着移动机器人应用领域的日益增加，对移动机器人导航研究不断提出新的课题，使移动机器人的导航

14、研究不断深入和发展234。1.1课题研究的目的与意义智能移动机器人是一种在复杂的环境下工作的具有自规划、自组织、自适应能力的机器人。智能导航研究的目标就是在没有人干预下使机器人有目的地移动并完成特定任务，进行特定操作。机器人通过装配的信息获取设备来获得外部环境信息，判定自身状态，实现自我定位，规划并执行下一步动作。 在移动机器人导航控制理论和方法研究中，确定性环境的导航控制方法已经取得了大量的研究和应用成果。对未知环境中的导航控制也开展了一些研究，并提出了若干方法，但是尚未形成统一和完善的体系结构，还有许多关键理论和技术问题有待解决与完善。这些问题包括环境的建模、定位、导航控制器的学习与优化、

15、故障诊断以及路径规划等。未知环境中的移动机器人只具有较少的先验知识，其导航控制方法涉及环境认知、优化策略、知识表示与获取等多项关键问题。对移动机器人在未知环境中导航理论和方法的研究，将推动认知科学、模式识别、非线性控制等前沿学科的研究，带动航天、海洋、军事、建筑、交通、工业和服务业等领域移动机器人导航控制系统的研究开发，为无人勘探车、无人排险车和无人运输车等用于航天、军事、深海作业和核工业领域的移动机器人系统的应用奠定理论和技术基础。具体来看比如：在民用方面，可以将其用于物流自动化仓库中的AGV、AHV小车，以提高物流自动化程度；在军事方面，可以有助于实现机器人智能侦察与作战等。.1.2课题研

16、究的内容本课题在国家自然科学基金（60475031）和湖北省青年杰出人才基金项目（2005ABB021）的资助下，研究在未知环境中，移动机器人自主的建立环境地图并根据该地图实现定位、避障与导航的算法。主要针对静态的、结构化的未知环境进行研究。研究的主要内容包括以下几方面：（1） 避障策略的拟订；机器人在运动之前都应进行障碍物检测，合理的避障算法对于成功的导航而言显得尤为重要。本研究中将对避障算法VFH（Vector Field Histogram）进行分析与应用。（2） 定位方法的设计；本文主要研究的是移动机器人在自身位置信息与环境信息均不确定的情况下，通过传感器了解环境来自我定位，同时根据自

17、身位置信息绘制环境地图，并根据该地图实现导航目的。因此，本论文将采用同时定位与制图（SLAM）算法对机器人进行定位。论文将会对本研究中提出的基于线特征SLAM算法进行详细的剖析。（3） 导航新算法的提出； 在研究避障算法VFH以及定位算法SLAM的基础上，找出两者结合的可能性以及优越性，构造新的导航算法。1.3论文组织结构由于机器人自主导航的最重要内容就是机器人的定位问题，所以本文的研究工作也就围绕机器人的自主定位进行。在综述国内外研究的基础上，深入研究了基于线特征的机器人同时定位与制图算法；详细介绍了避障算法VFH/VFH，并将两者结合构造了一种新的导航算法。论文主要分为七章：第一章为绪论，

18、主要介绍了本论文所研究的目的与意义以及研究的主要内容及要处理的关键问题；并对论文的组织结构进行概括说明。第二章中主要对移动机器人导航技术进行了概述，包括移动机器人的导航方式以及机器人导航中的两个主要问题：机器人定位与制图；并追踪概述了相关技术的国内外发展现状与发展趋势。第三章对移动机器人同时定位与制图（SLAM）算法，特别是基于特征的SLAM算法进行了详细的剖析，包括算法的属性、分类，构架等。第四章介绍了本研究中提出的一种基于线特征的SLAM新算法，详细分析了实现算法所需要的各个步骤；并通过仿真实验证实了该新算法的正确性与可靠性。第五章对主要研究了机器人避障算法VFH以及VFH，包括：算法的构

19、架以及实现算法的各个步骤等；并对算法进行了仿真实验。在第六章中，首先介绍了同时定位与制图算法与避障算法VFH/VFH+结合、产生新导航算法的可能性以及新算法的优越性；然后在对导航新算法进行说明的同时，通过仿真实验证实了其正确性与鲁棒性。第七章为全文的总结以及对今后工作的展望。文章结构框架如图1-1所示。其中星号标示处为本研究的创新点。 图1-1 全文结构框架第2章 移动机器人导航技术2.1机器人导航概述 智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中向目标自主的运动，从而完成一定作业功能的机器人系统。而目前智能的导航技术已经成为移动机器人研究领域的热点课题。智能导

20、航要解决三方面的问题5：（1）选择适当的传感器系统对移动机器人所处的环境进行认识； （2）通过一定的检测手段获取移动机器人在空间中的位置、方向以及所处的环境的信息； （3）用一定的算法对所获取的信息进行处理并建立环境的模型，寻求一条最优或近似最优的无碰撞路径，实现移动机器人安全移动的路径规划。 当移动机器人在未知或不确定环境下运行时，必须通过传感器来采集环境信息，并通过一定算法把传感器收集到的数据进行分析、融合，用来建立外部环境的模型，正确、全面的反映出外部环境的特征，为导航决策提供正确的依据。2.2移动机器人导航方式 在机器人导航技术中首先要解决的问题是采用什么样的导航方式，即采用什么样的传

21、感器系统对外界环境进行感知，并指导机器人的后续动作。常见的机器人导航方式有5：电磁导航、光反射导航、环境地图模型匹配导航、路标导航、视觉导航、基于传感器数据的导航以及卫星导航等。 电磁导航也称地下埋线导航，这种导航方式是20世纪50年代美国开发的，到20世纪70年代这种导航方式迅速发展并开始运用于柔性生产。其原理是在路径上连续的埋设多条引导电缆，分别流过不同频率的电流，通过感应线圈对电流的检测来获得感知信息。该技术简单实用，但是成本高、改造和维护困难。光反射导航的原理是在路径上连续的铺设光反射条，是一种简单、价格便宜的导航系统，同电磁导航一样，该技术也相当的成熟，目前国内制造行业使用的移动机器

22、人大多是基于这两种导航方式的，但是它们应用范围比较窄，不适合在动态、变换工作环境下的移动机器人导航。环境地图模型匹配导航是机器人通过自身的各种传感器探测周围环境，利用感知到的局部环境信息进行局部的地图构造，并与其内部事先存储的完整地图进行匹配。通过匹配，机器人可确定自身的位置，并根据预先规划的一条全局路线，采用路径跟踪和相关避障技术来实现导航。路标导航是指：路标在环境中的坐标、形状等特征已知的前提下，机器人通过对路标的探测来确定自身的位置，同时将全局路线分解成为路标与路标间的片段，逐段行进完成导航。这种导航方式又可分为人工路标导航和自然路标导航。虽然人工路标导航比较容易实现，但它人为地改变了机

23、器人工作的环境；自然路标导航不改变工作环境，但机器人需要通过对工作环境中的自然特征进行识别以完成导航。路标探测的稳定性和鲁棒性是采用该方法时需要研究的主要问题。 视觉导航方式具有信号探测范围宽、获取信息完整等优点，是未来移动机器人导航的一个主要的发展方向。在视觉导航系统中，目前国内外应用最多的是采用在移动机器人上安装车载摄像机的基于局部视觉的导航方式，这种导航方式所有的计算设备和传感器都装载在机器人车体上，利用装配的摄像机拍摄周围环境的局部图像，然后通过图像处理技术，如特征识别、距离估计等，进行机器人定位及规划下一步的动作。比如，有研究人员利用Fourier变换处理机器人全方位图像，并将关键位

24、置的图像经Fourier变换所得的数据存储起来作为机器人定位的参考点。以后机器人所拍摄的图像经变换后与之相对照，从而得知机器人当前位置。这些图象识别、路径规划等高层的决策都由车载计算机完成，因而延迟问题比较明显。视觉导航中的图象处理计算量大，实时性差，这始终是一个瓶颈问题。为了提高导航系统的实时性和导航精度，仍需要研究更加合理的图象处理方法。另外，还有基于传感器数据的机器人导航方式。一般机器人都安装了一些非视觉传感器，如超声传感器、红外传感器、接触传感器等。利用这些传感器亦可以实现机器人导航。比如，当机器人在光线很暗的环境中的时候，视觉导航方式将会失效。在这种情况下，可以利用其他传感器，比方说

25、超声波传感器来进行导航。因为超声波传感器的使用不受光线的影响，而且由于超声波传感器构成系统具有结构简单、体积小、性价比高等众多优点，使之在机器人特别是自主移动机器人领域中广泛的运用。 卫星导航是通过全球定位系统（GPS）来实现的。GPS全球定位系统是以距离作为基本的观测量，通过对四颗GPS卫星同时进行伪距离测量，计算出用户（接收机）的位置。通过这种方式，机器人可以通过安装卫星信号接收装置来实现自身定位。除了以上几种常见的导航方式外，还有气味导航、声音导航等。但是在实际应用中，往往采用几种不同导航方式的组合来达到最佳的导航效果。比如，有研究人员利用超声波传感器和视觉传感器的组合进行导航6。将超声

26、数据与图像数据结合，通过事先训练好的神经网络预测障碍物的可能位置，从而使得机器人能够在动态非结构化环境中实现自主导航。2.3移动机器人的定位问题定位是确定移动机器人在二维（三维）工作环境中相对于全局坐标的位置及其本身的姿态，它是移动机器人导航最基本的环节之一78。 定位方法根据机器人工作环境复杂性、配备传感器的种类和数量等不同而有多种不同方法。移动机器人自主定位手段主要有两类：相对定位和绝对定位。相对定位手段主要有以下几种：（1） 使用里程计。在这种方法中，通过使用编码器来测量车轮的转速，从而可以计算出机器人的相对位置，实现相对定位。使用里程的优势在于：它是完全自主的，并且通过结合其他方法很容

27、易预测下一时刻机器人的位置；而劣势在于：定位误差无限增大，必须周期性的对误差进行削减。（2） 惯性定位。通过测量机器人相对于初始位置的距离和方向来确定机器人的当前位置。该方法通常采用回转仪或加速计等仪器来测量车轮转速或加速率，然后通过一次或二次积分推导出机器人的位置。由于其要对数据进行积分处理，很容易导致延时，或者将一些小的常量误差放大；此外该方法设备造价较高。（3） 航位推算。这是一种经典的相对位姿估计方法。一般使用安装在车轮上的转数表来推算出机器人的行使距离和方向。航位推算的优点是方法简单，价格低廉；但其本质上容易受标定误差，车轮与地面的不良接触，颠簸等因素影响，因此误差较大。由上述可知，

28、相对定位技术的优点在于具有良好的短期精度、低廉的价格以及较高的采样速率，但是由于相对定位技术的基本思路都是基于测量值的积累，因此无法避免时间漂移问题。随着路径的增长，任何小的误差经过积累都会无限的增加。因此相对定位不适合长距离和长时间的准确定位，除非采用外部手段对误差进行约束。绝对定位手段主要有以下几种：（1） 基于活动信标（信号灯）。信号灯通过发射光或者声波而被机器人感知。通过测量这些被感知（3个或更多）的信号灯的作用范围，来决定机器人的绝对位置。应该注意，这些信号灯在环境中的位置必须事先已知的。（2） 基于人工路标识别。将路标放置在环境中的已知位置上。保证任意时刻机器人能够发现若干路标。通

29、过测量机器人与这些路标之间的相对距离来定位。使用该方法的好处在于定位误差是有界的。但是，与信号灯不同的是：人工路标一般不能看成点状，而是看作一些特征的集合，这样机器人每次感知的只是路标的一部分，可能会使结果产生误差。（4） 基于自然路标识别。这里的自然路标是指环境中那些固有的，明显、容易识别的物体。这种定位方法的可靠性和基于人工路标识别的方法基本一样。但是要求我们必须对该环境事先了解，否则识别过程将会有难度。在实际的机器人导航系统中，常常采用相对定位与绝对定位相结合的定位方式来对机器人进行定位。2.4移动机器人的制图问题2.4.1 地图的类型地图构建也是移动机器人自主导航中的一个很重要的内容。

30、机器人利用对环境的感知信息对现实世界进行建模，自动的构建一个地图。典型的地图表示方法有尺度地图以及拓扑地图910。尺度地图中又分为栅格尺度地图1112以及几何尺度地图1314。栅格地图在表示环境信息时比较形象，但是受限于环境的规模。几何地图可以简洁的表示环境，但是要求环境不能过于复杂；拓扑地图不需要尺度信息，只依赖环境中特定的地点，但是地图构建过程比较复杂。栅格地图最开始由Elfes和Moravec提出1516。对于2D的栅格地图，环境被分割为均匀的单元栅格，每个栅格赋一个0-1的值来表示此处的状态：1表示被占据；而0表示的是空闲。对于3D环境，每个单元格的值代表这个栅格的高度信息，从而来表示

31、整个环境地图。几何尺度地图由一组环境路标特征组成，每个路标特征用一个几何原型来近似。这种地图只局限于表示可以参数化的环境路标特征或可建模的对象如：点、线、面。这种构建方法大都是基于对环境信息的相对观测，然后用这些观测到的环境特征表示环境地图。由于以几何位置关系来表示环境地图，所以为了保证地图的一致性，要求各观测信息的位置是相对精确的。对于结构化的环境，用一些几何模型来表示环境空间是可行的。比如，可以用线段来拟合室内的墙面、用点来拟合墙角等；对于室外环境，可以用点特征表示道路两侧的树木位置，用曲面来拟合路面地形等。在利用拓扑地图表示环境时，它没有一个明显的尺度概念，而是选用一些特定的地点来描述环

32、境空间信息。拓扑地图通常表示为一个图表，图中的节点表示一个特定的地点，连接节点的弧线表示特定地点之间的路径信息。拓扑地图对于结构化的环境是一个很有效的表示方法，但是在非结构化的环境中，地点的识别变得非常的复杂，这种情况下仅以拓扑信息进行机器人定位是不充分的17。另外，为了更好地表示环境模型，还可以用混合拓扑和尺度地图的表示方法来表示环境1819。通过加入尺度信息来补偿拓扑信息使得地图表示方法具有拓扑地图的高效性以及尺度地图的一致性和精确性。2.4.2机器人制图存在的问题无论要建立哪种类型的环境地图，机器人必须要通过传感器来了解环境。在这个了解过程中传感器固有的或一些人为的因素会导致测量误差即测

33、量噪音。机器人制图过程中面临的一个主要挑战就来自于测量噪音。如果测量数据在统计意义上独立，通过多次测量可以削减、甚至消除噪音的影响。然而在实际制图过程中，这些数据却是相互联系的。可以设法将测量误差控制在一定范围内，但是它们不断积累时，就可能会影响到下一时刻机器人对传感数据的解释。整个制图过程中，机器人都要受到环境中这些系统的，互相联系的误差的影响。如何调整这些误差是正确制图的关键。但是不管是从理论的还是实践的角度来看，现存的算法都是十分复杂且需要进一步完善的。第二个体现制图复杂性的方面是环境中要描述的实体的维度，即：要用多少元素对其进行描述。比如，要较为详细的对一个二维的建筑平面进行制图，一般

34、需要上千的元素。而对于三维而言，则会高达上百万甚至更多。从统计角度看，每一个元素都是机器人要进行估计的对象。这样显然增加了制图问题的复杂程度。 第三方面难点在于制图本身是个协调、匹配的过程，也就是我们常说的数据关联问题。即：如何判断在不同时刻得到的传感器数据中，哪些表示的是环境中同一元素。只有找到这些相关数据，对其进行融合处理，才会得到对该物体的准确认识，从而获得更为精确的制图。 第四，目前的算法主要集中在对静态的、结构化的、中小环境的研究上，而如何在动态的、非结构的、大型环境中进行机器人制图，研究较浅。最后一个问题在于，机器人制图是与其运动路径有关的。即机器人的定位与制图是密切联系的。如果机

35、器人对自身的位置有准确的认识，就可以得到较为精确的制图；同样，如果机器人对环境（地图）了解很好，就很容易得到自身的位置信息。但是，当定位信息和环境信息都不确定的情况下，两者就应该同时考虑。这就是本研究中涉及的一个研究重点：移动机器人的同时定位与制图（SLAM）问题。2.4.3 同时定位与制图问题同时定位与制图（SLAM）是当前在移动机器人领域中自主导航的热门研究方向202122。2002年在瑞典的KTH举行的一个以SLAM为主题的Summer School，移动机器人领域内最知名的专家到场为50名国际学生讲学，从而将SLAM的研究引向了高潮。SLAM算法是本研究的最主要问题，将在第三章中对其进

36、行详细的分析。2.5 本章小结在本章中，首先概括介绍了机器人导航问题。对包括：电磁导航、光反射导航、环境地图模型匹配导航、路标导航、视觉导航、基于传感器数据的导航以及卫星导航等在内的多种机器人导航方式进行了介绍；对机器人定位与制图进行了说明，包括：定位方式、制图类型等，并在分析机器人制图研究存在的问题的基础上引出了本研究的重点之一：同时定位与制图（SLAM）算法。第3章 同时定位与制图（SLAM）算法3.1 同时定位与制图算法介绍移动机器人的定位和地图创建是机器人领域的热点研究问题，也是导航中重要环节。 对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。 然而

37、在很多环境中机器人不能利用全局定位系统进行定位,而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中，创建地图同时利用该地图进行自主定位和导航。 这就是移动机器人的同时定位与地图创建(SLAM) 问题，也称为CML (Concurrent Mapping and Localization) 。该研究领域的代表人物有Smith，Self 和Cheeseman 23 。由于SLAM算法具有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。近几年来，其研究取得了很大的进展，并已应用于各种不同的环境，如： 室内环境24 2

38、5 、水下26 27 以及室外环境28 29。3.1.1 SLAM算法性质SLAM算法有很多重要的属性，影响着地图特征和机器人位置估计中的不确定性。包括：状态估计的收敛性，估计过程的一致性，状态协方差矩阵更新的计算复杂度23。收敛性：SLAM算法具有三个重要的收敛性，这三个关键的收敛结果是：A.地图协方差矩阵的任意子矩阵地行列式随着每一次观测单调下降；B.在极限情况下，随着观测数量的增加，特征估计变得完全相关。C.地图的精度与第一个特征被观测时机器人的位置精度有关。这些结果表明：随着观测数量的增加，地图估计的不确定性将降低到有限的误差范围内，地图特征的关系将是完全确定的。一致性：为了维护SLA

39、M算法估计的一致性，对状态协方差矩阵进行更新维护是必要的。既然对环境的观测是相对机器人的，所以机器人估计中的任何误差和地图估计中的误差使绝对相关的。在没有其它外部的关于特征和机器人位置信息情况下，为了使系统状态估计的误差在有限的范围内，保持状态估计之间的一致性是很必要的。所以就必须维护机器人状态与环境特征之间的协方差矩阵。计算复杂度：SLAM算法应用到大规模环境时的一个重要局限性是计算环境特征之间，特征与机器人之间的相关信息时的计算负担。由于特征数量很大，协方差矩阵的更新维护导致了SLAM算法的计算复杂性。对于那些包含上万个特征的环境，计算负担使得系统协方差的更新变的难以执行。所以需要一个有效

40、的方法来提高算法的计算效率。3.1.2 SLAM算法分类根据所依据的理论基础的不同，SLAM可以分为以下几种：（1）基于扩展卡尔曼滤波（EKF）的CML/ SLAM 这是最常用的一种SLAM方法，适合解决非线性系统的估计问题。该方法用平面坐标表示机器人和环境特征的位置，将机器人运动与环境特征的关系描述为两个非线性模型：机器人运动模型和观测模型.通过这两个模型，运用扩展卡尔曼理论的思想来实现。主要包括预测与更新两个阶段2628。 （2）基于概率的CML/ SLAM 尽管不如EKF 那样流行，但由于用概率表达机器人定位问题的不确定性非常自然合理，基于概率的CML 也吸引了很多人的目光。其中，较流行

41、是最大相似性(Maximum Likelihood Estimation ，MLE)方法。一种非常有效的最大相似性估计算法称为Baum-Welch (或-) 算法30，基于这种方法的机器人制图与定位问题可看作是机器人位置与环境特征位置的最大相似性估计问题。该算法包括两步：E-Step (expectation) 和M-Step (maximization)。 （3）基于粒子滤波器( particle filter) 的CML/SLAM 粒子滤波器定位也称为Monte Carlo 定位3132，其基本思想是用一组滤波器来估计机器人的可能位置(处于该位置的概率)，每个滤波器对应一个位置，利用观测对

42、每个滤波器进行加权传播，从而使最有可能的位置的概率越来越高。 （4）基于空间扩展信息滤波器的SLAM33 该方法由Sebastian Thrun 等人提出，是对EKF 算法的改进。它不再用协方差矩阵表示空间信息的相关性，取而代之用空间信息矩阵来表示空间信息间的内在固有的关系，并且使用网状数据结构仅维护邻近的环境特征(地图)。 （5）基于集合理论估计的SLAM34 基于集合理论的方法不假设噪声服从某种分布，而只假设噪声是有界的。 具体方法是：定义一个可行状态集合(feasible state set) 和一个观测集合(measurement set) ，前者表示机器人和环境特征的状态估计，后者表

43、示符合条件(观测误差小于边界)的状态集合。算法首先根据机器人本身的状态方程计算可行状态集合，然后根据观测值计算观测集合，最后取两个集合的交集作为某时刻的经估计校正后的机器人与环境特征的状态集合(地图)。此外根据所制地图的不同，SLAM又可以分为：基于栅格的SLAM35；基于特征的SLAM282936以及基于拓扑结构的SLAM37。在基于栅格的SLAM算法中，环境表示为一组具有特定分辨度的栅格的组合。每个栅格中有特定概率值来表示其被占据的可能性。使用基于栅格SLAM算法的时候，假设各个栅格的状态变量是相互独立的，而没有考虑栅格之间的关联性。另外，当机器人采用该种SLAM算法运行较长一段路程后，很

44、难再回到出发点。这是因为该方法中仅在局部坐标中考虑了估计的不确定性，却没考虑局部坐标与全局坐标中的相互联系。总的说来，基于栅格的SLAM方法可以提供较为丰富的环境描述，对于局部环境下的规划与导航有很大帮助，但是通过该方法无法获得一致性较强的全局地图。 基于特征地图的SLAM算法是将环境表示为一组组参数化的特征值，比如说点，线，角等。这里的特征指的是环境中那些突显于背景的、易于传感器分辨检测的且可以通过参数描述的东西。使用这种方法的时候，必须对环境中不同类型的特征分别建立测量模型以便准确提取。基于特征的SLAM算法是最为流行的一种，尽管在环境地图描述中仅仅用到特征的位置值，但是还可以通过大小，颜

45、色等信息进行匹配等。这种SLAM算法常常是基于卡尔曼滤波的。使用这种方法时，状态向量不仅包括机器人的位姿信息而且还有特征的位置信息。随着新特征的不断提取与确认，状态向量将不断的变大。因为用于描述环境的特征值的测量都是相对机器人的，所以对环境特征测量的不确定性是与机器人位姿估计的不确定性息息相关的。可以在理论上证明，随着时间的不断推移、测量的不断进行，地图中的特征将是完全相关的，也就是说此时随意给定一个特征的绝对坐标值，将会得到一个精准的地图。在环境特征容易识别的场合下，该算法运用很好，但是对于特性不是太明显的非结构化环境中，算法的运用遇到难题。 在基于拓扑结构的SLAM算法中使用“图”来描述环

46、境。具体说是通过节点和弧线来描述环境。每个节点表示环境中突显的地方，边用来表示相邻节点间的相对位置。对于纯粹的拓扑地图来说，无须知道每个节点的具体位置，它只用来表示节点间的连通性。但是该方法也有很大的弊端，比如当环境稍微复杂一些时，其对环境的识别能力会明显下降且无法识别相近的环境。尽管SLAM算法的理论基础已经被很好的研究，但是要将其更好的运用于实际，特别是大型的非结构化环境，仍有大量理论和实际的问题需要解决包括：计算效率、地图管理、局部地图与全局地图之间的协调融合、数据匹配以及传感器管理等3839。3.2 基于特征SLAM算法构架简介上文中讲到根据所得到地图类型的不同，可以将SLAM算法化分

47、为基于栅格的SLAM；基于特征的SLAM以及基于拓扑结构的SLAM三大类。其中，基于特征的SLAM是运用最广的一类，下文将对该类SLAM算法进行详细阐述。基于特征的SLAM算法架构由图3-1表示。由图3-1可以看出，整个算法实际上是由三部分构成即：预测实践更新。首先要建立机器人的动力学模型以及运动过程中的观测模型。某一时刻，在给定控制参数后，根据动力学模型可以预测机器人在时刻的位姿状态；同时结合观测模型还可以预测在时刻机器人对旧（已经核实的）特征的观测值；在实践环节中，即在时刻到时刻的过程中，机器人基本上要完成三大工作：环境特征的提取和合理性检验，以及相关数据的匹配。从而获得对特征的实际测量，同时筛选出新的特征。将对特征的估计测量和实际测量的差异值（残差）、机器人位姿估计以及旧的地图一起作为输入参数，通过一定滤波算法即可更新机器人位姿，进而得到新特征的位置信息并将其融入到旧地图中生成新地图。总而言之，该方法要求机器人的位置和所有地图特征的位置存放在同一个状态向量中，并且这个向量是增广的。当观测到环境中新的特征时，就要把它作为新的地