毕业设计（论文）机器人地图创建方法研究.doc

《毕业设计（论文）机器人地图创建方法研究.doc》由会员分享，可在线阅读，更多相关《毕业设计（论文）机器人地图创建方法研究.doc（28页珍藏版）》请在三一办公上搜索。

1、本科学生毕业论文论文题目：机器人地图创建方法研究学 院：电子工程学院年 级：2007级专 业：自动化姓 名：学 号：指导教师：2010年 5 月 16 日摘要移动机器人同时定位和地图创建是实现移动机器人完全自主导航的关键。自主移动机器人在未知环境下作业时，首先要解决的基本问题就是其自身的定位问题，而定位问题与环境地图的创建又是相辅相成的。本文从相关理论和关键技术等方面，系统地总结了同步自定位和地图创建的研究现状，从环境特征提取、定位与地图创建、数据相关等多个方面对移动机器人即时定位与地图创建问题进行了综述。着重介绍了基于EKF模型的SLAM算法、基于尺度不变特征变换算法和基于概率论的方法，分析

2、了目前存在的难题，并指出了未来研究的发展方向。关键词移动机器人；特征提取；地图创建；数据相关；EKFAbstract The mobile robot simultaneous localization and mapping is critical to realizing the fully autonomous navigation.When autonomous mobile robots operate in an uncertain environment.one of the most fundamental tasks is to localize itself .The lo

3、cation is associated with map building. Based on related theories and technologies , This paper summarizes the achievements in simultaneous localization and mapping systematically,and the paper summarizes the current research on SLAM in some aspects ,such as feature extraction,methods and data assoc

4、iation.introduces SLAM algorithms based on EKF model,scale constant feature transformation and probabilistic approach in detail ,analyzes the current open issues and indicates the prospective research direction in the future development .Key words Mobile robot；Feature extraction；Filter；Data associat

5、ion；EKF目录摘要IAbstractII第一章 绪论11.1 机器人同时定位与地图创建(SLAM)概述11.2地图的表示方法21.3特征提取31.4机器人地图创建的研究及发展趋势4第二章 地图创建的一些关键性问题52.1不确定性信息处理方法52.2数据关联52.3累计误差62.4算法复杂度72.5机器人地图创建的导航问题72.6探索规划8第三章 移动机器人地图创建方法93.1基于卡尔曼滤波(KF)的SLAM算法93.1.1算法基本原理93.1.2扩展卡尔曼滤波(EKF)103.1.3仿真实验113.2基于尺度不变特征变换(SIFT)算法123.2.1移动机器人模型123.2.2估计与更新方

6、程143.2.3 SIFT算法153.2.4实验结果153.3基于概率的SLAM算法163.3.1 SLAM问题的概率描述163.3.2基于概率论的SLAM173.3.3稀疏扩展信息滤波183.3.4 FastSLAM算法19结论20参考文献21致谢23第一章 绪论1.1 机器人同时定位与地图创建(SLAM)概述在完全未知的环境中由机器人自主地依靠其自身携带的传感器提供的信息建立环境模型正成为自主移动机器人研究中的一个热点问题。为了能够有效地探索未知区域并完成给定的任务，机器人需要自主创建地图的能力。下面对这个问题作些详细的解释：完全未知环境，即机器人对环境一无所知，不存在任何先验信息，包括环

7、境大小、形状、障碍物位置等等，且环境中不存在诸如路标、灯塔等人为设定的参照物。在这种未知环境中，机器人创建地图行为的完成必须依赖于其传感器所获得的信息，如里程仪、声纳、激光测距仪、视觉等等。由于传感器自身的限制，感知信息存在不同程度的不确定性。直接使用感知信息进行地图创建很难得到准确的环境模型，因此通常需要对感知信息再处理，通过多感知信息的融合获得较为准确的环境信息。机器人根据在运动过程中获取的传感器数据，利用相关地图模型和集成算法，生成关于环境的地图表示。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法，然而在很多环境中机器人不能利用全局定位系统进行定位，而

8、且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航，这就是移动机器人的同时定位与地图创建(SLAM)问题。定位问题是移动机器人的基本问题，是指移动机器人通过对内部状态的检测或对外部环境的感知估算自身位置和姿态的过程。移动机器人地图创建问题描述如下：移动机器人从初始位置，经过一系列的位置并且在每一个位置获得传感器对环境的感知信息，移动机器人的目标是处理这些传感器数据,确定机器人的位姿，并且同时创建环境地图。要想开发一个有效的SLAM 系统，关键是选择好环境的描述方法。目前，应用卫星全球定位系统等装

9、置进行室外机器人的定位已经取得成功，但由于环境、任务等因素移动机器人大多采用机载传感器定位。它主要由位姿跟踪、地图获取组成。位姿跟踪指机器人在走行中对其位置和姿态的估计过程。SLAM方法在过去的十几年中逐渐成为机器人导航问题的研究热点，吸引了大量的研究人员，并取得了很多实用性的成果，是否具备并发建图与定位的能力被许多人认为是机器人是否能够实现自主的关键的前提条件。SmithR 、SelfM 和CheesemanP 于1986 年提出基于EKF(ExtendedKalmanFilter) StochasticMapping方法，揭开了SLAM研究的序幕。在以后的十几年中,研究范围不断扩大，从有人

10、工路标到完全自主、从户内到户外，出现许多SLAM方法。由于自主机器人固有的特点（缺乏自身位置和环境的先验信息；靠外部和内部传感器获得知识；环境、传感器信息及机器人运动本身具有不确定性），各种方法归纳起来都是一个“估计校正”的过程，必须解决像如何估计机器人及环境特征状态，如何校正和更新地图、地图如何表示、不确定性如何处理等问题。1.2地图的表示方法机器人地图可以分为两大类：几何地图和拓扑地图。几何地图又可分为栅格地图和特征地图。基于栅格的地图表示方法1 即将整个环境分为若干相同大小的栅格，对于每个栅格指出其中是否存在障碍物。这种方法已经在许多机器人系统中得到应用，是使用较为成功的一种方法。栅格地

11、图的缺点是：当栅格数量增大时（在大规模环境或对环境划分比较详细时），对于地图的维护所占用的内存和CPU 时间迅速增长，使计算机的实时处理变得很困难。栅格地图是一个近似的解决方案，但其对特定感知系统的假设参数不敏感，具有较强的鲁棒性。基于几何特征的地图表示方法2,3是指机器人收集对环境的感知信息，从中提取更为抽象的几何特征，例如线段或曲线，使用这些几何信息描述环境。这种表示方法更为紧凑，且便于位置估计和目标识别。几何特征的提取需要对感知信息作额外的处理，且需要一定数量的感知数据才能得到结果。特征地图由一系列包含位置信息的特征组成。拓扑图也是一种紧凑的地图表示方法4 ，特别在环境大而简单时。这种方

12、法将环境表示为一张拓扑图(graph)，图中的节点对应于环境中的一个特征状态、地点（由感知决定），如果节点间存在直接连接的路径则相当于图中连接节点的弧。这种表示方法可以实现快速的路径规划。由于拓扑图通常不需要机器人准确的位置信息，对于机器人的位置误差也就有了更好的鲁棒性。但当环境中存在两个很相似的地方时，拓扑图的方法将很难确定这是否为同一节点（特别是机器人从不同的路径到达这些节点时）。拓扑地图由环境中特征位置或区域组成的节点及其连接关系成，根据连接关系信息，移动机器人可从一个节点区域运动到另一节点域。拓扑地图抽象度高，适合于大环境的结构化描述。地图创建最简单的方法为增进式方案。首先利用里程计数

13、据估计机器人的位置，然后根据机器人获取的新信息建立局部地图并对全局地图更新。1.3特征提取移动机器人配备多种内部和外部传感器，内部传感器主要有里程计、陀螺仪等。外部传感器有视觉、激光、声纳、红外等。移动机器人融合内部传感器信息和外部环境特征信息进行精定位和地图创建，外部传感器信息经过处理形成外部环境的几何特征，以长度、宽度、位置等参量表示。由于单目视觉系统不能提供环境特征的深度信息，无法表征环境特征的位置，因此移动机器人即时定位与地图创建中，主要通过声纳或激光传感器在2D平面上的扫描信息（距离、角度、强度等）获得外部环境的轮廓特征及其位置信息。声纳传感器具有成本低、波束覆盖范围宽的特点，但角度

14、分辨率低，不精确，且容易产生虚假和多重反射回波信号，增加特征匹配的难度。而激光传感器以其快速、扫描精度高、角分辨率高、方向性强等特点，得到了越来越多的应用。环境特征的选择和描述不追求细节，注重特征的鲁棒性和稳定性，主要采用室内环境标识的最小环境模型，提取能够反映大范围环境的结构化特征，如直线和线段、角、点、垂直线等，分别与墙、墙角、凸角、门等特征对应。特征提取的方法很多，对于直线和点特征主要采取哈夫变换方法，如加权哈夫变换法、直方图法5等。特征的选择非常广泛，多面体、平面体、直线、角点、目标边缘等均可作为地图特征6,7。然而单个点特征对路标的唯一性识别和定义非常困难，可记录目标表面分布的特征点

15、集合组成空间结构用于目标定位。Ayache等采用三目立体摄像机获取环境中的直线段信息，最终获取更高层的特征信息。Leonard等利用RCD模型从多次扫描的声纳数据序列中提取距离信息恒定的区域所对应的固定特征。Ip Y.Lip等采用EAFC算法从原始声纳测量数据中提取线段特征。Grossmann、Tardos等用Hough变换和地图联接技术实现感知特征的提取，以减少镜面反射引起的错误特征检测。Chong将离散声纳测量数据不断累计合并至局部平面，提取平面、角点和边缘等地图特征，消除声纳镜面反射现象引起的虚假目标。多传感器融合是提高特征检测能力的重要手段。Castellanos等对激光和摄像机数据在

16、特征级进行融合，利用冗余数据提高特征表示可靠性，罗荣华8等也提出基于信息融合的地图创建方案。1.4机器人地图创建的研究及发展趋势准确定位同时建造环境地图的能力是自主移动机器人的重要特征之一。为使自主型移动机器人在真实环境部署和生存，真正实现完全自主，地图创建的研究还需要在下面几个方面取得突破：(1)实时性：人类处理地图信息和理解环境可以在非常短暂的时间完成，而现在计算机定位和建造地图的方法无论基EKF、基于概率还是其它方法都远远做不到实时处理。实时性要求两方面的工作：一方面开发专用的建图芯片或板卡，通过与主CPU并行工作提高处理速度；另一方面需要改进现有算法的效率或研究新的更好的SLAM方法，

17、降低算法的时间和空间需求，提高运算速度。(2)鲁棒性：SLAM处理的主要对象就是系统中存在的各种不确定性。不确定性的存在也使SLAM算法很难保证稳定性。如基于EKF算法非常依赖数据关联的准确性，如果关联不准确，很容易使算法发散。基于尺度不变特征变换的SLAM算法很大地改进了算法的稳定性，但其对相关性的处理使它很难作为一种通用的解决方案。(3)准确性：建立于各种不确定性上的准确定位确实是个很难的问题。为此，需要尽量减少系统中的不确定因素。传感器方面，由于声纳的噪声数据很难处理，近年很多研究倾向于激光雷达和视觉传感器。未来建立在激光雷达和视觉上的SLAM研究将越来越深入和完善。此外，SLAM算法包

18、括地图表示的方法等，也必须改进以减小位置误差。概括来讲，自主移动机器人的SLAM 研究的主要发展趋势可以归纳为以下几点：(1)机器人作业环境由已知的结构化环境向未知的非结构环境拓展。(2)环境感知方面采用能提供更大信息量的多传感器系统（如激光雷达和视觉系统等）和多机器人系统（也称为分布式可移动感知系统）。(3)更加注重概率、模糊数学等不确定性处理工具在地图描述和数据关联算法中的应用。(4)在实验研究中强调实机途径，开发在真实环境下具有高鲁棒性和高适应能力的实用技术。(5)特种应用和服务业的需求日益迫切，可以预见面对真实世界的非结构化和动态特点，为了提高移动机器人的自主能力，高适应性、高鲁棒性、

19、高效率的SLAM 理论、方法与技术将是今后的研究热点。第二章 地图创建的一些关键性问题该领域所涉及的关键性问题可以归结为六点，分别为不确定信息处理方法、数据关联、累计误差、算法复杂度、机器人导航问题、探索规划。这些问题并不是孤立考虑的，彼此间存在紧密的关联和渗透。下面分别对上述问题根据国内外研究的现状加以阐述。2.1不确定性信息处理方法事实上，机器人地图创建的困难就在于系统中无处不在的不确定性。机器人本身机械性能或未知外力（绑架）造成的不确定性将导致运动估计出现误差，观测的不确定性（包括传感器的不确定性和数据关联的不确定性）将导致校正失效，更不必说动态环境中的环境的不确定性。如何处理不确定性，

20、尽量减少各种客观存在的不确定性引起的误差是SLAM的关键，也是各种算法的精髓。作为SLAM 的信息来源，机器人本体的动力学、运动学模型及其携带的各种传感器的准确性是决定地图精度的一个关键。实际上这些渠道获得的信息都带有不同程度的不确定性。感知信息的不确定必然导致地图和定位双方的不确定。而同样的，当依据地图和感知决策后续移动时也就带有不确定性，造成不确定性的逐步传递。因此必须解决以下问题：在地图和位置的表示中，如何描述运动和感知信息的不确定性？在迭代过程中，如何处理旧信息与新信息的关系，连续更新地图与位置？如何依据不确定的信息进行决策？对于上述问题，目前使用的主要是模糊逻辑9 和概率10 的方法

21、，如Bayes 估计，Kalman 滤波和Markov 推理等。以概率描述信息的不确定性其优点在于适应不确定模型，对于性能差的传感器也能表示其感知信息，在实际应用中鲁棒性较好，在很多实现中得到了应用，缺点是计算量较大。模糊逻辑提供了一个处理不确定性（特别是声纳感知的不确定性）较为鲁棒且有效的工具。同时，在对多种不确定性信息进行建模和融合时可以有更多的操作符供选择。2.2数据关联为了获得全局的环境地图和实现定位，还需要将不同时间、不同地点的感知信息进行匹配和联合，存在局部数据之间的关联问题，也存在局部数据与全局数据的关联与匹 配问题。在SLAM 中使用的数据关联算法也多种多样，如近邻算法11和联

22、合相容性算法。由于根据机器人的里程仪提供的位置信息一般存在累计误差，感知数据通常要融合其他外部传感器的数据，如声纳、激光测距仪或视觉传感器等等。这些传感器提供的丰富和多样的感知数据既为数据关联提供了资源，也对数据描述的一致性和数据关联算法的适用性提出了挑战。上述研究可一定程度上提高特征选择的鲁棒性，然而特征检测仍具有不确定性。数据关联是指不同时刻对环境的观测是否来源于同一特征。对于EKF模型来说，数据关联的不准确将导致算法无法收敛。在大多数模型中，这种关联通常体现在特征之间的相似度和它们之间的相对距离。常见的数据关联多采用最邻近算法，方法认为传感器观测值和特征之间的每个匹配均是独立的，没有考虑

23、相邻特征标志之问的相互关系，而事实上测量预测误差是相关的。当机器人误差增加或者陷入混乱状态时，该方法将容易导致错误匹配。Moutarlier等采用启发式方法从机器人估计位置出发搜寻机器人旋转和位移变化，实现感知特征与已存储特征的最佳匹配。Neria等提出基于JCCB的约束搜索算法，把环境中的相关特征联合形成有意义的环境标志，如门、墙、角点等，然后进行最佳关联处理，成功剔除错误匹配信息。Ba1ev等将数据关联视为图的匹配问题，通过求取最大公共子图来实现特征标志的匹配。数据关联是对两个特征标志进行匹配，确定它们是否对应环境中的同一物体。 SLAM中的数据关联主要需要完成三个任务：新特征标的检测、特

24、征标志的匹配以及地图之间的匹配。虽然在目标跟踪、传感融合等领域，数据关联已经得到较好的解决，但是这些方法的计算量大，不能满足SLAM的实时性要求。实现个标志与拥有个标志的地图之间的数据关联的复杂度与之间呈指数关系，假设每个观测到的标志有 个可能的匹配，那么对于个标志需要在指数空间中搜索正确的匹配。数据关联的搜索空间与环境的复杂程度以及机器人的定位误差有关，环境的复杂程度的增加会使增大，而误差的增大会使增大。2.3累计误差SLAM 中的误差主要来自三个方面：观测误差、里程计的误差和错误的数据关联带来的误差。当机器人在已知地图的环境中进行定位时，机器人可以通过观测位置已知的特征标志对里程计的误差进

25、行补偿，每一次观测使机器人的位置误差趋向于观测误差与特征标志的位置误差之和。然而在SLAM中，由于机器人的位置和环境中的特征标志的位置都是未知的，观测信息不能有效纠正里程计的误差，机器人的位置误差随着机器人的运动距离而增大，而机器人的位置误差的增大将导致错误的数据关联，从而增大特征标志的位置误差；反过来，特征标志的误差又将增大机器人的位置误差。因此，机器人的位置误差与特征标志的位置误差密切相关，它们之间的相互影响使机器人和特征标志的位置估计产生累计误差，难以保证地图的一致性。2.4算法复杂度真正完全自主的机器人要求建图与定位必须能够在线计算，因而对计算复杂度的要求是非常苛刻的。现在的算法最多能

26、够运行几天或几个星期，可存储和计算的环境特征达到104 级，但仍然与真实世界有很大差距。单从运算速度角度来说，现有计算机与人相比虽然有差距，但这种差距呈日渐减小趋势。但进行定位判断等处理时，计算机却远远不能与人相比。原因之一就是人脑的并行计算及分布式存储。将神经网络技术或其它并行算法应用于SLAM，极有可能较大幅度提高SLAM算法在真实环境部署的可能性。基于卡尔曼滤波器的SLAM 的计算量主要来源于系统协方差矩阵的更新，即维持机器人与特征标志之间以及各特征标志之间的相关关系。这些相关关系对于SLAM算法的收敛性十分重要。研究表明，忽略这些相关关系不能保证地图的一致性，使创建的地图失去意义。为了

27、降低SLAM的时间复杂度，需要在不影响地图一致性的条件下限制这些相关关系。2.5机器人地图创建的导航问题 在机器人剖建地图的过程中必然涉及到机器人的导航问题，包括自定位、运动规划和避障。避障可以通过设计其行为来实现与地图创建关系。比较紧密的主要是定位和规划。存在全局地图的情况下，自身的定位比较容易解决，当能够获得精确的自身位置时，创建地图只是一个积累信息的过程。但在完全未知的环境里，机器人没有任何的参照物，其能依靠自己并不十分准确的传感器来获取外界信息，由于计算实时性的限制，视觉传感器也不能提供更多的信息，如同一个盲人在建筑物里摸索的情况，这种情况下，定位是比较困难的。有地图的定位和有定位的地

28、图创建都是容易解决的，但无地图的定位和未解决定位的地图刨建如同“无鸡生蛋、无蛋生鸡”。实际上目前机器人地图刨建的一个主要难点也正是难以解决机器人自身精确定位的问题。已有的研究中对这个问题的解决方法可分为两类，一类利用自身携带的多种内部传感器（包括里程仪、罗盘、加速度计、陀螺仪等等），通过多种传感信息的融合减小定位的误差，使用的融合算法多为基于卡尔曼滤波的方法。这类方法由于没有参考外部信息，在长时间的漫游后误差的积累仍然会比较大。另一类方法在依靠内部传感器估计自身运动的同时，使用外部传感器（如激光测距仪、视觉等等）感知环境，对获得的信息进行分析抽取环境特征并保存，在下一步通过对环境特征的比较对自

29、身位置进行校正。但是这种方法依赖于能够取得环境特征，而环境特征12，13有时是很难提取出的，如：（1）环境特征不够明显时，如在茫茫沙漠中，人也无法完成哪怕大约的定位。（2）传感器信息比较少，难以从一次感知信息中获得环境特征。例如使用声纳或红外传感器。在地图创建中，为了提高效率，希望在尽短的时间内获得尽可能多的未知环境信息。那么就需要解决这样一些问题：j如何确定未知区域，在最开始时所有方向都是未知区域，但一段时间后，已经建立了部分地图，这时就要寻找并标注未知区域。k选择在当前情况下最值得探索的区域，比较探索该区域所能获得的信息量和探索该区域需要付出的代价（能量、时间等等）。l规划前往该区域的路径

30、，寻找一条能保证安全的最短路径。在这方面进行的研究目前还比较少太多数地图创建的研究还主要着眼于解决创建地图的主要问题。2.6探索规划研究探索规划的主要目的是提高地图创建的效率，使机器人在较短的时间内感知范围覆盖尽可能大的区域，在这方面的研究成果较少，一般遵循的策略是：让机器人总是沿着最短路径到最近的未探索区域14。这种方法原理上效率较高，但考虑到信息的不确定性，精心设计的路径未必可行，而且复杂的算法也会影响计算的实时性。因此，需要设计更高效的控制策略，实现精确性和实时性的折衷，既能利用重复定位获取的关联数据来提高地图和定位精度，又能保证全局的收敛性和较高的探索效率。针对大规模环境，为了提高作业

31、速度，多机器人的SLAM 问题的研究也得到关注。但总的来讲现有的研究还是对单机器人方法的一种简单扩展。在多机器人的探索策略方面存在两种思路：一种是完全分布私的，机器人之间共享信息，但独立作出探索决策；另一种策略是分散探索，集中决策和创建地图。第三章 移动机器人地图创建方法3.1基于卡尔曼滤波(KF)的SLAM算法扩展Kalman滤波是移动机器人即时定位与地图创建中最重要的理论方法，最早的应用是Smith 等人提出基于扩展卡尔曼滤波器的随机地图创建( stochastic mapping)方法15。方法用一包含机器人位姿向量和环境特征向量的高维向量表示空间环境，机器人控制信号或里程计信息输入到系

32、统状态方程，完成对位姿和环境特征的预测，环境特征则用于更新位姿和特征地图。预测与更新之间，要进行特征匹配或数据关联处理，只有匹配的特征才用来更新位姿和特征地图。随机地图将机器人的位姿向量和环境特征向量组织在一个高维状态向量之中，用扩展Kalman滤波作最小均方差估计。机器人的控制信号或里程计的信息输入到系统状态方程之中，完成对位姿和地图特征的预测。上述成果奠定该类算法的理论研究框架，但是算法实施中存在两大问题：（1)计算量大。方法需要计算系统协方差矩阵，即维持机器人与特征标志之间以及各特征标志之间的相互关系。(2)不确定性高。由于机器人系统和传感器自身的局限性、外界环境的干扰造成机器人定位和特

33、征识别的不确定性。针对这些问题，研究人员提出了多种改进方案以保证地图创建的精确度和可靠性，并降低空间和计算复杂度。目前，基于扩展卡尔曼滤波的SLAM算法已成为该领域最广泛最重要的研究方向之一。3.1.1算法基本原理建立非线性离散时间系统的系统模型如下： (3-1) (3-2)其中为系统输入控制向量，和为0为均值高斯白噪声序列。即 (3-3) (3-4)其中，为系统过程噪声的协方差矩阵，为系统观测噪声的协方差矩阵。EKF将非线性函数f()和h()围绕当前系统状态估计值扩展成一阶泰勒公式，实现非线性系统的线性化，并运用KF算法递推计算系统的状态估计。其实现步骤如下：1.根据前一时刻状态估计k-1和

34、协方差矩阵预测当前时刻的状态估计和协方差矩阵： (3-5) (3-6) 其中，为系统状态方程的jacobian矩阵在处的值。2.由预测协方差矩阵预算Kalman增益矩阵 (3-7) (3-8)其中为系统观测协方差矩阵，为系统观测方程的jacobian矩阵在处的值。根据状态的预测估计和实际观测值修正系统的状态估计并计算系统的协方差矩阵： (3-9) (3-10)3.1.2扩展卡尔曼滤波(EKF)卡尔曼滤波器假设系统是线性系统，但实际的机器人运动模型与观测模型是非线性的。因此，通常采用扩展卡尔曼滤波器，基于EKF的建图与定位可以归纳为一个循环迭代的估计校正过程，EKF算法在处理不确定信息方面有独到

35、之处，因此EKF成为最广泛的SLAM方法。根据以上的公式和假设，可以设计卡尔曼滤波器公式为： (3-11) (3-12)其中和为互不相关且均值为零的正态白噪声序列，方差分别为和，而K时刻的状态向量和量测向量可表示为如下形式： (3-13) (3-14)量测值是从状态预测向量中计算得到的目标位置，它是指目标点在机器人坐标系中的具体位置，可以表示为： (3-15)所以系统的状态方程和量测方程为： (3-16) (3-17)其中， (3-18)3.1.3仿真实验实验在Matlab 环境下进行，移动机器人的尺寸为：左右驱动轮的半径，宽度，初始位姿为，激光观测的最大有效距离设为30m，随机选择了29个特

36、征点和14个航点，其所构建的仿真环境模型如图2-1所示，通过上文提出的同时定位与地图创建框架进行仿真实验，结果如图2-2所示。由于系统融合了内部和外表传感器的信息，预测与量测进行了数据关联匹配，并通过EKF 进行递归最优估计，极大的改善了只用航位推算方法实现定位所带来的误差积累，从而使得移动机器人的运行轨迹与航线之间具有很好的拟合性，表明了EKF 算法具有很强的鲁棒性；同时由于实时的捕获了当前地图信息，并对地图不断的更新，使得在未知环境下移动机器人的自主导航成为可能，表明框架具有很大的实际应用。验证了框架的可行性。本实验采用（EKF）对地图或机器人位姿进行更新，匹配成功则对机器人位姿进行更新，

37、实现机器人的实时定位；匹配失败则认为测量到得特征为新特征，将其加入到环境中，作为下一时刻的参照物，实现了地图的实时更新。图3-1 基于环境特征的地图图3-2 同时定位与地图创建3.2基于尺度不变特征变换(SIFT)算法3.2.1移动机器人模型在二维环境中，移动机器人的位姿使用，表示，其中，表示移动机器人相对世界坐标位置，表示机器人的朝向，在位姿跟踪问题中，移动机器人的初始位姿是已知的。地图的创建过程是指移动机器人在自身位姿已知的情况下的环境地图创建过程。在SLAM中，机器人利用自身携带的传感器识别未知环境的特征标志，然后根据机器人与特征标志之间的相对位置和里程计的读数估计机器人和特征标志的世界

38、坐标。这种在线的定位与地图创建需要保持机器人与特征标志之间的详细信息。在室内特定环境下，基于环境特征SLAM方法的基本思想是将移动机器人的位姿和环境特征坐标表达在一个状态向量中，在机器人的移动过程中通过对环境特征的观测做最准确的估计。假设移动机器人在世界坐标系（用，表示）中为一个点，机器人的起始位置为世界坐标系原点，前进的方向（机器人的朝向）为机器人坐标系中的横坐标，即轴，逆时针旋转90度为纵坐标轴。如图3-3所示 图3-3 机器人的移动过程因此世界坐标系中的方程为： (3-19) (3-20) (3-21)其中和表示时刻机器人在世界坐标系中的位置；表示时刻机器人的朝向；是时刻移动机器人的位移

39、；k表示时刻机器人的偏航角。由于目标是静止物体，所以在世界坐标系中的位置可以表示为：， (3-22)和是第个坐标在世界坐标系中的位置。机器人获得的目标位置是指第个目标在机器人坐标系中的位置，因此还需要将其转换成世界坐标，可表示为： (3-23) (3-24)3.2.2估计与更新方程采用EKF实现系统状态的估计与更新，当机器人在运动过程中发现新的特征时，需要根据新特征的观测矢量和机器人的当前状态计算新特征标志的初始状态，并更新状态矢量和协方差矩阵。其中， 在SLAM中，系统的状态包括机器人和环境特征在机器人坐标系中的位置的估计，而协方差矩阵表示估计的误差，扩展卡尔曼滤波方法对信息的处理一般分为预

40、测和更新，并且此方法对信息的估计是无偏估计。卡尔曼滤波预测方程为： (3-25) (3-26) (3-27)更新方程为： (3-28) (3-39) (3-30)其中，是状态噪声协方差，是测量噪声协方差，是误差协方差，是增益矩阵。预测方程用来预测当前状态，误差协方差矩阵为下一时刻的先验估计。新方程将先验估计和测量值结合，得到一个较精确的后验估计。3.2.3 SIFT算法LoweDG在2004年总结了现有的基于不变量技术的特征检测方法，并正式提出了一种基于尺度空间的，对图像缩放，旋转甚至仿射变换保持不便性的图像局部特征描述算子SIFT算子，即尺度不变特征变换。SIFT算法首先在尺度空间进行特征检

41、测，并确定关键点的位置和关键点所处的尺度，然后使用关键点领域梯度的主方向做为该点的方向特征，以实现算子对尺度和方向的无关性，梯度和方向的计算方程为： (3-31) (3-32)上式为处梯度的模值和方向公式，其中L所用的尺度为每个关键点各自所在的尺度。在实际计算时，可在以关键点为中心的邻域窗口内采样，并用直方图统计邻域像素的梯度方向。直方图的峰值代表了该关键点处邻域梯度的主方向，即作为该关键点的方向。3.2.4实验结果 在试验室内对移动机器人进行试验，让机器人沿着长为三米的正方形行走，机器人行走的路线和地图创建的结果如3-4所示。机器人实际路线和估计路线基本一致，而与里程计定位有一定差距，是由于

42、里程计本身存在的缺陷，比如机器人行走过程轮子打滑，就会产生实际移动距离和机器人内部里程表数据不一致，因此，随着时间的推移，里程计的累积误差逐渐增大，从而导致自我定位与实际相差较大。 图3-4 基于尺度不变特征变换的SLAM实验结果3.3基于概率的SLAM算法由于SLAM 问题涉及未知且不确定的环境描述和传感器噪声，一般使用概率方法来描述该问题。扩展Kalman 滤波方法是多种SLAM算法的基础，用于同步创建地图和估计机器人的位置。但是其计算量大和对数据关联问题处理困难的缺点也十分明显，尽管不如EKF那样流行，但由于用概率表达机器人定位问题的不确定性非常自然合理，基于概率的SLAM也吸引了很多人

43、的目光。同样是基于概率，也有多种地图表示及建图方法。3.3.1 SLAM问题的概率描述首先介绍一下相关的符号表示16 ：离散时间k= 1 ,2 , ，是机器人在时刻的实际位姿（位姿包括机器人位置和朝向），表示控制变量，它使得机器人位姿从改变到，表示第个路标的实际位置；表示时刻在位姿处对第个路标的测量值；表示时刻得到的总测量值。另外，我们用大写字母来表示以上变量的集合：状态集合，控制输入集合 ，所有路标集合(即地图) ，其中l 是整个地图上路标的个数;观测集合。基于上面的符号表示，我们可用概率表示SLAM 中两个重要模型：运动模型和测量模型。运动模型： 是在已知时刻机器人控制输入 和时刻位姿的条

44、件下求取 时刻位姿 的条件概率。运动模型可由机器人的运动学和动力学特性求得。测量模型： 是在已知时刻机器人位姿和路标位置集合的条件下求取k 时刻测量值的条件概率。测量模型在使用栅格地图的情况下可通过反复测量传感器数据而求得。整个SLAM 问题可以用概率来表示，其含义是在已知所有控制输入和观测值的条件下求取机器人位姿和地图的联合条件概率。基于机器人运动是Markov 过程和周围环境是静态的这两个假设，应用Bayes 公式，得到下式： (3-33)这是一个回归算式，其中是时刻的条件概率，是时刻的条件概率，是归一化常数。若运动模型和测量模型已知，就可以通过上式迭代求解SLAM 问题，其中最基本的方法

45、就是前面所介绍的扩展Kalman 滤波。3.3.2基于概率论的SLAM在SLAM问题中，整个状态矢量包含机器人位姿和地图 。移动机器人的工作环境可看成一个平面，机器人位姿包括3 个变量：直角坐标和,姿态朝向 即。每个路标可以由其直角坐标来表示：，i =1 ,2 , , l ，路标可以是周围环境中的人工路标或其他物体。假设在一张地图中有 个路标，相应的状态矢量可以表示成下列维矢量 。在扩展Kalman 滤波算法中，位姿和地图的联合估计可由其均值和协方差矩阵来确定。其中，均值矢量是维的，协方差矩阵是 维的。假设环境是静态的，则地图分量M 满足线性要求，但是机器人位姿通常是前一时刻和控制输入 的非线性函数。这在计算上增加了难度,而扩展Kalman 滤波将非线性函数使用泰勒级数展开作线性化近似。对于大多数移动机器人来说，这种线性化处理不会引起较大的偏差。线性化的结果就是状态方程可以由一个线性函数再加上高斯噪声来表示：