移动机器人FASTSLAM算法研究.doc

本科毕业设计题目：移动机器人FastSLAM算法研究学 院:专 业:学 号:学生姓名:指导教师:日 期:摘 要移动机器人同时定位与地图创建是实现未知环境下机器人自主导航的关键性技术，具有广泛的应用前景，也是目前机器人研究的热门课题之一。基于卡尔曼滤波器的SLAM算法有计算的复杂性以及对数据融合误差非常敏感的缺点，使其不能再实际环境中得到广泛应用。为解决传统SLAM算法的缺陷，介绍了一种基于Rao-Blackwellized粒子滤波器的FastSLAM方法，该方法将SLAM问题分解为对机器人姿态和路标在地图中的位置的递归算法。每一粒粒子都有对应的地图，再将地图估计分解成N个独立的特征估计，路径估计采用粒子滤波器，地图估计采用扩展卡尔曼滤波器。FastSLAM有机地将粒子滤波器与卡尔曼滤波器集成在一起，鲁棒性地解决数据关联和多目标跟踪问题，其时间消耗与路标的数量成对数关系，计算量小，用时短。基于Rao-Blackwellized粒子滤波器的FastSLAM算法是一种高效的机器人同步定位和绘制地图的算法，其具有高效性和准确性，该方法使用提高了机器人地图创建的实时性，增强了避障能力。关键词： 移动机器人； FastSLAM算法； 路径估计； 地图估计 Abstract Mobile robot localization and mapping the key technologies of the robot autonomous navigation in unknown environment, has broad application prospects, is currently a hot research topic of the robot. The Kalman filter-based SLAM algorithm to calculate the complexity and the error is very sensitive to the shortcomings of the data fusion, so that it can not be widely applied in the actual environment.FastSLAM based on Rao-Blackwellized particle filter SLAM problem is decomposed into the position of the robot pose and landmarks on the map recursive algorithm to solve the defects of the traditional SLAM algorithm. A particle has a corresponding map, and then map the estimated decomposed into N independent characteristics estimated path estimation using particle filter, map estimated using extended Kalman filter.The FastSLAM organic particle filter and Kalman filter integrated, robust solution to data association and multi-target tracking, the time consumption and the number of landmarks logarithmic small amount of calculation, with short.FastSLAM algorithms based on Rao-Blackwellized particle filter is an efficient robot simultaneous localization and mapping algorithm, its efficiency and accuracy, the method to improve the robot map created real-time, and enhance the ability to obstacle avoidance.Key words: Mobile robot; The FastSLAM algorithm; The path estimation;The map estimation目 录1 绪论11.1 移动机器人定位和地图创建问题21.1.1 移动机器人国内外发展状况21.1.2 移动机器人的地图构建问题31.1.3 机器人的定位方法52 基于粒子滤波器的SLAM算法72.1 SLAM的通用框架和理论模型72.2 粒子滤波器定位的基本原理82.3 扩展卡尔曼滤波器算法92.3 粒子重采样102.4 移动机器人SLAM问题描述102.4.1 SLAM计算复杂度102.4.2 SLAM的联合估计112.4.3 SLAM的后验估计表示112.4.4 SLAM公式推导132.4.5 有效的数据关联142.4.6 FastSLAM的粒子表示形式152.4.7 FastSLAM的计算时间复杂度163 模型建立173.1 运动模型173.2 观测模型184 FastSLAM算法步骤194.1 FastSLAM算法步骤194.2 新位姿采样204.3 环境特征估计的更新215算法流程图和代码225.1 FastSLAM算法伪代码225.2 FastSLAM算法流程226 仿真环境建立和仿真结果246.1 仿真环境介绍246.2 仿真结果247 总结和展望29参考文献30致谢311 绪论移动机器人的同时定位与地图创建(SLAM)问题有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。在火灾、地震或者极地考察现场存在一些潜在危险，可能会对进入人员生命构成危险，就需要移动机器人来代替人进入这些环境，进行探测地形和检查是否有伤员的工作。民用方面可以用于自主行驶车辆或者车辆辅助驾驶系统，还有服务机器人，智能机器人，智能玩具等。军用方面可以用于对敌方控制区侦查和对危险易爆炸物的搜索和拆除。科学方面可以用于外星探索和采矿等。SLAM问题研究是实现机器人完全自主以及服务人类的关键,因此,对移动机器人SLAM的研究具有重要的理论意义和应用价值,由于自主移动机器人具有广泛的应用、巨大的经济利益和深远的科学价值，各国家和科研工作者对其相关技术发展都投入了大量的研究。在20世纪70年代末期，法国的系统分析与架构实验室（LAAS）研制了HILARE机器人，这是欧洲的第一个移动机器人研究项目，HILARE安装了视觉传感器、激光测距仪和超声波传感器等设备，用二维多边形环境模型和全局坐标系统实现导航和路径规划功能。多年来国内外有大量的科技工作致力于这方面的研究工作，对许多问题的认识和求解都取得了长足的进步，但是仍然有很多问题需要解决。由于移动机器人系统运动模型难以精确建模，再加上其他传感器感知误差存在高度不确定性和外界干扰因素等，从而造成了机器人系统的高度复杂性和不确定性。由于在很多情况下，环境的信息是部分、甚至是完全未知的，此时必须将定位和地图构建结合起来，使得移动机器人利用对自身的定位来构建地图，同时利用构建地图来定位，在这一问题中，移动机器人定位与地图构建是密切相关的，地图创建过程依赖于精确的机器人位姿信息，任何一方都无法单独实现，必须同时加以考虑，近年来，SLAM算法越来越受到人们的重视，针对移动机器人的SLAM问题，主要有基于粒子滤波器的SLAM方法和基于扩展卡尔曼滤波以及EKF的各种变种SLAM方法。SLAM(Simultaneous Localization And Mapping）问题是指把移动机器人放在未知环境中，机器人增量式地创建未知环境的连续地图，同时确定它在地图中的位置，由于SLAM问题的解法使机器人实现了真正的自主导航，所以在过去几十年中逐渐成为了移动机器人领域的研究热点。SLAM一方面为了实现精确定位，机器人需要利用位置相对确定的环境特征不断校正自身位置，一方面为了确定环境特征的位置，机器人需要了解自身所在的精确位姿。机器人的定位与地图构建有利于进一步协助运动规划和躲避障碍物，体现机器人对环境探测效率的同时又体现出对环境地图信息描述的精准程度。SLAM也称CML(Concurrent Mapping and Localization),最初于上世界80年代早期由Smith Self和Cheeseman给出了基础概率方法的解决方案， SLAM 问题的解决方法大致分为两类:基于概率估计的方法和非概率估计的方法。基于概率模型的方法占主导地位。1.1 移动机器人定位和地图创建问题1.1.1 移动机器人国内外发展状况1950年和1951年，William Grey Walter研制了两个可完成自动避障等智能行为的移动机器人。在20世纪50年代初期，Marvin Minsky等人研制了具有跟踪功能的移动机器人。1956年，在由美国国防部高级研究计划局（DARPA）主办的Dartmouth会议上，正式提出了“人工智能”概念，认为移动机器人应能够执行和学习有意义的任务，一定程度上能够模仿动物或人的能力。1969年，Nilsson等人在斯坦福机器人研究所（SRI）研制了第一个智能移动机器人Shakey，它装备了摄像机、测距仪和接触传感器等设备，并通过无线连接与DECPDP 10计算机相连，其任务是自动避障和运动目标跟踪。在20世纪70年代末期，Hans Moravec在SRI研制了CART移动机器人，它能够利用摄像机进行避障，并能利用连续的多幅图像构建二维环境模型，并可以实现路径规划。在20世纪70年代末期，法国的系统分析与架构实验室（LAAS）研制了HILARE机器人，这是欧洲的第一个移动机器人研究项目，HILARE安装了视觉传感器、激光测距仪和超声波传感器等设备，用二维多边形环境模型和全局坐标系统实现导航和路径规划功能。目前，移动机器人的研究范围已经扩展到水下、空中、地面、甚至太空和行星表面等多种环境，如美国马里兰系统工程实验室（Marine Systems Engineering Laboratory）研制的EAVE III型自主式水下移动机器人，美国海军研制的新型战场准备自主水下航行器（BPAUV）、美国无人车辆协会（the Joint of Unmanned Vehicle）组织开展的自主式航空飞行器的研究，世界著名移动机器人制造商IROBOT公司的金字塔探索者Pyramid Rover、月球车，以及卡耐基-梅隆大学（CMU）研制的CMU-Rover，日本Tsukuba大学的Yamabiko和MELDOG，法国系统分析与结构实验室（LAAS）的Hilare2，德国Karlsruhe大学的KAMRO等。美国航空航天局（NASA）研制的第一代火星探测机器人Sojourner于1997年成功登陆火星，开启了人类利用移动机器人技术进行空间探测和开发的序幕。为了在火星上进行长距离探险，NASA又开始了新一代样机的研制，命名为Rocky7（如图1.2所示），并在Lavic湖的岩溶流上和干枯的湖床上进行了成功的实验。最值得一提的是，2004年1月美国NASA火星探测移动机器人Spirit（如图1.3所示）和Opportunity先后在火星表面顺利登陆。Spirit和Opportunity的成功着陆标志着人类发射的星际探测器自动化程度提升到了前所未有的水平，同时也标志着智能移动机器人的研究工作也已经提高到了一个前所未有的高度，正在为人类的进步发展贡献力量。我国也针对这一趋势制定了以月球为近期目标的空间探测计划。研究和发展我国的月球探测移动机器人技术，不但对于我国在激烈的空间技术和资源竞争中取得有力地位具有关键意义，同时也对包括移动机器人导航控制在内的相关技术的发展有巨大的促进作用。我国对移动机器人的研究虽然起步较晚，但是越来越受到重视，国家863智能机器人组把智能移动机器人的研究作为今后发展的重点。以中科院自动化研究所、清华大学、国防科技大学、东北大学、上海交通大学、哈尔滨工业大学为代表的我国机器人研究基地，已经取得了令人瞩目的成果。清华大学研制的THMR系列移动机器人系统；自动化所研制的集多种传感器、视觉、语音识别与会话功能于一体的智能移动机器人CASIA-I；此外，沈阳自动化研究所机器人实验室研制的激光导引AGV、防爆机器人、“海极号”和“探索者号”水下机器人，香港城市大学智能设计、自动化以及制造研究中心的自动导航车和服务机器人，哈尔滨工业大学机器人研究所研制的能在未知、复杂、多变、恶劣的环境下运行的全方位轮式移动机器人平台等都说明了我国移动机器人技术取得的突出成就。但从总体水平上看，我国移动机器人技术比起发达国家还有一定的差距。1.1.2 移动机器人的地图构建问题移动机器人通过自身所带的传感器实现对外部环境的感知。由传感器获得的数据经处理后要以某种抽象的形式来对环境空间进行表达，并作为地图信息存储在移动机器人中。移动机器人利用对环境的感知信息实现环境的建模，自动的构建一个地图，利用地图给出的信息在外部环境中运动，并持续感知周围环境，进一步完善和更新地图。在移动机器人导航并自主定位过程中，按照一定的空间表达规则将传感器信息转换成地图的过程是移动机器人导航并自主定位的基础，而空间表达的方法又决定了地图的表达形式。因此，采用何种空间表达方法对移动机器人自主定位和地图的持续更新具有很大影响。目前地图创建方法很多大致可以归为四类:栅格表示法、几何信息表示法、拓扑图表示法和混合表示法。1. 基于栅格的地图表示法 栅格地图表示法最先是由Moravec和Elfes于1985年提出的。它将机器人工作环境划分为一系列栅格，其中每个栅格都被分配一个概率值。此概率值表示该栅格被障碍物占据的可能性大小。栅格地图的优点在于易于重建和维护，对任何一个栅格的感知信息可以直接与环境中的某个区域相对应，特别适合于处理超声测量数据。但是环境空间的分辨率和栅格大小相关，当需要增加分辨率时就必须缩小栅格的大小，进而增加了运算的时间和空间复杂度。另外，在传统的栅格地图表示法中，每个栅格之间被认为是相互独立的，因此，利用栅格表示的地图和实际环境地图具有很大的不一致性9。为改进传统栅格地图表示法的这种缺点，Thrun提出了一种采用期望最大化方法进行环境地图表示的方法，同时还考虑了各个相邻栅格之间的依赖关系。在Bert等人的研究中，对环境采用全局栅格和局部栅格结合的方法进行建模，同时利用霍夫变换对栅格地图中的线段进行抽象描述，并采用卡尔曼滤波器进行位置估计。Stepan等人提出了一种基于单目摄像头和激光测距仪的栅格地图构造方法，对栅格地图的质量利用机器人的历史路径信息进行优劣评估。Noykov等人提出了一种利用声纳传感器数据进行栅格地图构造的方法，利用统计方法对声纳数据概率模型进行建模并结合模糊理论进行声纳数据融合。Grisetti等人采用Rao-Blackwellized粒子滤波器对栅格地图进行估计，采取自适应方法获取粒子数，同时，在粒子采样函数的选取时考虑了最近多次传感器的测量数据。2. 基于特征的地图表示法基于特征的地图表示法是指利用可以识别的环境路标进行环境地图的表示，其中每个路标特征都用几何原型进行表示。此种地图表示方法局限于可以进行参数化的路标环境，或者是可建模的对象环境，比如点、线、面等。对特征地图的重建大多都是基于外部传感器对环境的检测数据，然后利用这些检测数据进行环境路标特征的提取。对结构化的室内环境，可以利用一些简单的集合模型进行环境空间的表示，对于室外环境，可以采用曲面进行环境空间的表示。Drumhellor利用线段进行地图构造，并利用激光测距仪的测量数据进行机器人自身定位。Arras等人用激光测距仪数据提取水平直线特征，同时利用视觉系统进行垂直直线特征的提取，最后综合这些直线进行环境地图的构造。Roumeliotis等人针对激光测距仪对环境的检测数据提出了一种分层的路标检测方法，在底层采用两个卡尔曼滤波器分别进行环境拐角和线段等基本特征的检查，在上层再对底层检测到的特征进行较为复杂的路标特征合成，比如墙，门，地面等。Luis Pedraza等人提出利用B-样条的方法进行非结构化环境中的路标描述，然后运用扩展卡尔曼滤波器进行机器人定位。Luis Pedraza等人另外还提出采用B-样条曲线进行环境边界表示的方法，即BS-SLAM算法，BS-SLAM算法的意义在于它是第一个运用参数化方法进行环境曲线特征描述的算法。3. 基于拓扑的地图表示法利用拓扑地图进行机器人运行环境描述最先是由Brooks提出的。拓扑地图表示法中的地图是根据实际环境结构，由位置节点和节点之间的连线组成。其中节点表示环境中的特定地点，节点之间的连线表示的是不同地点之间的路径信息。拓扑地图可以组织为层次结构，例如在底层，一个位置可能就是一个房间，但是在更上一层则可能为一个建筑物。为了利用拓扑地图方法进行环境地图的表示，必须有效的识别环境中各个特定的地点。对结构化的环境而言，各个特定的地点可以容易进行识别，但是在非结构化的环境中，节点的识别会非常复杂。利用拓扑地图进行环境表示的关键在于拓扑地图中各个顶点的选取，David等人提出了一种基于声纳数据和视觉信息相结合的拓扑地图顶点选择方法，相对于单独采用声纳数据进行顶点检测，此方法可以得到更加精确的拓扑地图描述。Choset等人提出利用通用Voronoi图（Generalized Voronoi graph，GVG）进行环境地图表示的方法，在GVG地图表示中添加了机器人传感器的一些度量信息，通过对各个GVG图的匹配进行机器人自身定位。Ranganathan等人提出利用GVG图和视觉传感器检测到的路标进行地图表示的方法。另外，为更好地进行环境地图模型的描述，研究者还提出了拓扑地图和尺度地图混合进行环境表示的方法，通过在拓扑地图中加入尺度信息来补偿拓扑地图中的信息丢失。GVG地图表示法已经被用于美国国家航空和宇宙航天局空间站中的机器人三维定位问题研究中，这样的地图表示方法具有拓扑地图的高效性和尺度地图的一致性以及精确性等特点。4. 基于视图的地图表示法在基于视图的地图表示方法中，对环境并不是利用拓扑形式或者路标特征进行表示，而是采用机器人的一系列历史路径估计信息进行表示。基于视图的地图表示方法首先是由Lu和Milios提出的，在他们的研究中，环境地图是由多次激光扫描数据和各个扫描数据之间的关系组成。Fleischer等人提出了基于视觉的视图地图表示方法，Malauchlan提出了一种基于批处理和迭代处理的三维环境视图表示方法。Eustice等人针对机器人在海底环境运行的特殊性，提出了一种新的地图表示方法VAN（Visually Augmented Navigation），在VAN的机器人定位与地图重建算法中，SLAM的状态向量由机器人在不同时刻的位姿组成。Eustice等人还对基于视图的地图表示方法中的信息矩阵，稀疏扩展信息滤波器（Sparse Extended Information Filter，SEIF）以及稀疏联合树滤波器（Thin Junction Tree，TJT）中的信息矩阵进行了比较，从比较结果可以看出，基于视图的地图表示方法中的信息矩阵相对于SEIF和TJT中的信息矩阵有两个优点，一是信息矩阵的稀疏性要更好，二是它对应信息矩阵的稀疏性是自然的，并不需要像SEIF那样经过强制的信息丢失处理。基于视图的地图表示法适合于那些并不容易进行特征抽象的复杂地图环境。1.1.3 机器人的定位方法在自主移动机器人中，无论是局部实时避障还是全局规划，都要精确地获得机器人及障碍物的当前状态，因此对于移动机器人来说，精确地位置估计是实现自主导航的必要内容，对移动机器人定位的研究具有非常重要的意义。目前，移动机器人多采用各种机载传感器进行定位。1 里程计定位在移动机器人的两个驱动电机组上均装有里程计，它们是使用最为广泛的导航工具之一，能在短距离内提供精确地定位，但它的误差是随着时间增加而累计增长的，尤其是方向误差。当用来长距离和长时间定位时，由于误差的不断累加，会导致移动机器人在行驶过程中发生很大的偏离，从而影响定位效果9。尽管有这个缺点，由于其在短距离内依然能够获得较好的效果。因此绝大多数的研究者还是认为里程计是机器人导航系统中的一个重要组成部分。2 路标定位基于人工路标、自然路标或者“灯塔”，一般来说，路标都有固定的已知位置，机器人自身的传感器能对路标进行识别，参照路标进行相对定位。为了使问题简化，经常假设机器人的当前位置和方向角已经划定，机器人只需要在有限的区域内寻找路标。缺点是需要人工干预，在现场设置路标，这是人为对环境的再结构化，移动机器人的自主性不高。路标对信息的反馈存在一定的误差，这也影响了机器人对于自身位置的确定。3 视觉定位视觉定位技术主要是利用摄像机获取工作空间的图像，通过图像处理、模式识别等手段对获取的图片进行分析，提取有用的环境信息，为机器人的定位、路径规划、避障以及创建地图提供数据。其优点是具有很高的空间分辨率。探测范围广、精度高、能够获得场景的大部分信息。缺点是摄像头在使用过程中容易受到光线和环境的影响。且获得的图像提取和识别选用困难，耗费的计算了很大，实时性也不易满足。4 GPS定位GPS全球定位技术是一种应用范围较广的绝对定位技术。它利用环绕在地球上空的24颗卫星准确定位出使用者的准确位置。若是已知地面接收器与三个卫星之间的距离，从理论上来讲，就可以计算出接收器的经纬度和高度，单纯利用GPS定位精度还比较低，所以在移动机器人定位和导航中一般还同时融合里程计和其他传感器的数据，其缺点是不适合室内环境、水下环境以及对精度要求较高的移动机器人定位。5 地图匹配定位 地图构建和匹配算法是基于地图匹配定位的关键，首先要获得机器人工作环境的全局地图，在进行地图匹配定位时，利用把机器人机载传感器获取其局部地图，将局部地图和先存放在寄存器中的全局地图做比较，如果是匹配的，那么机器人就能确定在这个环境中的全局位置和方向。地图匹配的缺点是它对于机载传感器获得的局部地图要求比较高，地图上要有足够多的与全局特征匹配的特征点，而这些特征点在一般相对简单的环境里还是比较少的。6 惯性定位惯性定位是一种不依赖于任何外部信息的自主式定位。惯性传感器是移动机器人常用的一类内部传感器，一般利用陀螺仪计算移动机器人的姿态，利用加速仪计算出移动机器人的位置，将测量结果积分合成得到机器人的位姿。这种方式的优点是不需要外在的参照物，可以自主定位。缺点是惯性传感器数据会随着时间产生偏差，每次积分后原先的位置和方向上的微小误差都会随之放大，且有误差累计效应。2 基于粒子滤波器的SLAM算法2.1 SLAM的通用框架和理论模型 图2-1 SLAM通用框架SLAM问题是把定位和地图构建结合在一起的，下图为SLAM的理论模型： 图2-2 SLAM的理论模型2.2 粒子滤波器定位的基本原理粒子滤波（PF）定位方法用粒子集描述概率分布，而不用概率分布函数本身表示。卡尔曼滤波器（KF）和扩展卡尔曼滤波器（EKF）利用参数化模型(一个高维的高斯函数)来表示概率密度分布；粒子滤波器采用一系列有限的采样粒子代表概率密度分布，高概率区域含有高概率的粒子，反之亦然。粒子滤波以样本集合的方法逼近概率分布，如果粒子数趋于无穷大，那么这种非参数化的粒子分布能够逼近任何复杂的概率分布。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布。它采用一组带有权值的随机样本来描述概率分布。在概率高的区域，粒子的密度就大，相反，在概率低的区域，粒子的密度就小。粒子滤波定位方法优点很突出：能够处理多峰分布；能够很大地降低内存耗费；能够更精确地定位，且便于实现；不受限于高斯分布，在很多定位问题中，如位姿跟踪、全局定位甚至绑架问题，都取得较好的应用。粒子滤波方法作为一种基于贝叶斯估计的非线性滤波算法，在处理非高斯非线性时变系统的参数估计和状态滤波问题方面具有独特的优势。EKF只适用于高斯噪声模型，对于非线性的模型表现非常差。而粒子滤波则适用于任意噪声模型。利用EKF只能在滤波误差和预测误差很小的情况下通过对系统的非线性进行近似以获得令人满意的估计精度，否则滤波初期估计协方差下降太快会导致滤波不稳定甚至发散。此外EKF还要求噪声必须服从高斯分布，大大限制它的应用范围。而PF在测量的基础上，通过调节个粒子的权值大小和样本的位置，来近似实际的概率分布，并以样本的均值作为系统的估计值。因此，PF不受线性化误差和高斯噪声假定的限制，适用于任何状态转换和观测模型，有效克服了EKF的缺点。PF也有缺点：相对于EKF，粒子滤波的计算了较大，特别是在采用非常多的粒子数目时。随着计算机处理能力的不断增强，以及各种改进的实时粒子滤波算法不断涌现，PF算法计算了大等障碍正在逐渐消失；在采用PF算法时，在粒子滤波中，普遍存在的问题可能是存在的退化现象。因为PF抽取N个具有权值的粒子后，系统如接收到新的观测数据，则实时更新每个粒子的权值。重要性权值的分布会变得越来越倾斜，这时有可能出现粒子匮乏现象。即经过几次迭代后，少数样本会占去总体样本的绝大部分权重，除了一个粒子以外，其余粒子均只有微小的权值，可忽略不计，这样使得样本集不能体现所要表达的后验概率密度，同样使得大量计算浪费在几乎不起任何作用的粒子的更新上。2.3 扩展卡尔曼滤波器算法卡尔曼滤波器是由Kalman在20世纪60年代提出的一种递推滤波器算法，它不需要保留使用过的测量数据，当系统检测到新的测量数据后，可以利用递推公式获得新的系统状态估计。扩展卡尔曼滤波器是在卡尔曼滤波器的基础上针对非线性系统输出估计提出的一种改进方法。在扩展卡尔曼滤波器中，对非线性系统首先采用一级泰勒展开进行线性化近似，然后利用最小均方误差准则对非线性系统状态方程和测量方程的输出进行估计。在基于扩展卡尔曼滤波器的机器人同步定位与地图重建（Extended Kalman Filter SLAM，EKF-SLAM）算法中，系统噪声均被默认为符合高斯分布，因此利用系统状态的均值和协方差进行系统状态输出的描述。在EKF-SLAM中，系统状态中包括机器人的位姿和已检测到所有路标的位置，因此，协方差矩阵的大小随着路标数量成平方比而增大。在EKF-SLAM算法中，根据机器人的运动状态方程，并结合机器人当前位姿和运动控制输入利用扩展卡尔曼滤波器对机器人下一时刻的位姿进行预测估计。当机器人传感器检测到外部路标时，根据传感器测量方程，结合机器人当前位姿、路标位置以及传感器的测量信息采用扩展卡尔曼滤波器进行系统状态的更新。传统的EKF-SLAM算法存在如下两个典型问题，一是计算复杂度，由于在EKF-SLAM中维护着系统状态的协方差，并且在每次接受到传感器测量数据时需要进行系统状态协方差的更新，因此EKF-SLAM算法的计算复杂度为（其中为已检测到的路标数目）。二是系统状态估计的一致性，由于EKF-SLAM算法中利用扩展卡尔曼滤波器对机器人运动方程和传感器测量方程的输出进行估计，然而在扩展卡尔曼滤波器中，对系统输出采用的是一级泰勒公式展开近似，因此当系统的非线性非常严重时，采用扩展卡尔曼滤波器对系统输出估计的精确性会严重降低，甚至导致输出估计的发散。针对EKF-SLAM算法计算复杂度大的问题，Newman等人提出了一种基于局部子地图序列的SLAM算法，它的核心是重建一系列相互独立而且具有固定大小的局部地图，然后再利用扩展卡尔曼滤波器将各个局部地图合并为一个完整的全局地图，从而保证SLAM过程中的耗时不会随着检测到路标数目的增大而无限增大。Lina等人从机器人位姿估计一致性最大化角度对局部地图的大小取值进行了分析。Chong等人同样提出利用子地图进行环境地图描述的方法，他的做法是对子地图之间的相对位置进行单独估计，当机器人回到先前的子地图区域时，采用搜索算法对机器人进行重新定位。子地图分割法可以有效降低计算的复杂度，但它带来的最大问题是数据关联的确认上有一定的困难。2.3 粒子重采样通过增加采样点数可以有效地克服退化现象，采样点数越小退化现象越严重，然而采样点数大时又影响计算的实时性。可见通过增加采样点数来解决退化问题是不现实的。目前消除退化问题的两个关键技术分别是：选取适当的重要密度函数和进行再采样。粒子滤波的收敛精度和收敛速度直接关系到它能否实现应用及应用的范围，是另一个值得重点研究的关键内容。为了避免粒子匮乏，Gorden等人提出了重采样方案，其主要思想史除去那些权值小的粒子，并复制权值大的粒子，重采样通过过滤样本集中权值较低的样本。保留高权重样本，使权重集中在权重大的样本上，来达到解决退化问题，一般根据权重对后验概率密度进行离散近似处理。重采样原理：时刻的先验概率可由带有权值的粒子近似表示。经过系统观测并重新计算权值，那些权值大的粒子可以分裂出若干新的粒子，而权值很小的粒子将被丢弃，如此即可得到一组新的粒子。这些新的粒子加入随机量后即可预测在t时刻的状态，即系统状态转移过程。最后再次进入系统观测过程，并预测下一时刻的状态。重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化也是应该考虑的问题之一。2.4 移动机器人SLAM问题描述2.4.1 SLAM计算复杂度SLAM解法在实际应用中需要满足实时性要求，即传感器的数据采集周期应大于SLAM算法处理一帧传感器数据的时间。全状态SLAM算法如EKF-SLAM的计算复杂度与状态向量的维数成指数关系，在路标数目较大时全状态SLAM算法不能满足实时性要求，因而不适合大范围环境中的SLAM问题，Guivant等人提出了CEKF-SLAM（Compressed EKF）算法，采用近似的EKF全方差计算方法，将大范围环境分成若干局部子区域，在大部分时间仅用EKF更新状态向量中的局部估计，在机器人从一个局部区域转向另一个局部区域时对状态向量的均值和方差进行全局更新，从而大大提高了计算效率。此外，Leonard等将整个地图分解为一系列子地图，然后对每个子地图进行滤波估计，降低了状态向量方差更新过程中的计算复杂度。扩展信息滤波器（Extended Information Filter，EIF）是EKF的对称形式，它保持了EKF数学严谨、估计准确的特点，并且，其信息矩阵几乎稀疏，特别适合SLAM问题，Eustice等从稀疏信息矩阵的角度提出了SEIF-SLAM算法，估计精度也得到了保证。Montemerlo等人采用Rao-Blackweuise粒子滤波器解决SLAM问题，并提出了FastSLAM算、法它的计算量与某一时刻观测到的地图特征数目成线性关系，尤其适用于大范围环境中的SLAM问题。2.4.2 SLAM的联合估计机器人运动误差如何干扰传感器误差的先后不确定会导致定位和构建地图之间先后的关系的不确定。机器人运动，位姿估计被运动误差干扰。环境特征在全中的位置也基于粒子滤波器的同时定位与地圈构件的方法研究被观测误差和位姿估计误差所干扰。然而，与观测噪声不同的是，位姿误差系统性地作用于全局地图。这个作用也就是，路径的误差与地图的误差是相互关联的。因此，没有正确路径估计的条件下无法估计地图的误差。尽管，路径误差和地图误差的关系使SLAM问题在变得更加复杂和困难，下面就要讲怎样把这个关系分解，把SLAM问题转换成一些小的问题的乘积，然后可以用更有效的方法解决这些问题。2.4.3 SLAM的后验估计表示在机器人的控制过程中会有噪声干扰，可以把每个控制量或者观测量以及一些噪声模型当成一个概率约束，每个控制量概率地约束两个连续的机器人位姿，观测量约束机器人和目标的相对位置，新的观测量会更新地图特征和机器人位姿以及以前所观测的地图特征，开始，这些约束可能很不确定。然而，随着地图的特征被反复的观测，约束越来越确定。在无限次的观测和控制后，所有的地图特征的位置就完全相关。可以通过估计算法估计最可能的机器人位姿和地图。但是这些方法需要处理的是一组变化无常的观测量和控制量，不适合在线操作，这些算法一般不能估计地图的不同部分的确定性。最流行的在线解决SLAM问题的方法是：已知传感器读数，对所有可能的地图和路径估计后验概率分布。SLAM算法的一些基本参数：地图含有N个特征，记为： （2.1）在t时刻，加入的控制量： （2.2）机器人轨迹： （2.3）在平面环境中包含三个参数机器人的坐标和方向角， t时刻以前所有路标的观测量： （2.4）本文假设每个观测量提供的信息是一个环境特征相对于目前位置的确切位置。变量代表了某个被观察到的环境特征。实际上，并不会观察到环境特征的唯一性，因为很多环境特征看起来是一样的。对应观测量的唯一的环境特征写做，相关性变量： （2.5）表示所有数据关联的集合，确定的过程叫数据关联，表示在时刻t，第个路标是否被观测到。路径信息和地图信息两个量都依赖于观测量，必须把构建地图和定位过程一起考虑。机器人依靠传感器得到的观测量、自身的控制量以及相关性变量来得到路径信息和地图信息。解决SLAM问题的核心是在于应该把定位和地图构建视为整体。实际上，这是因为路径信息和地图信息两个量都依赖于观测量。所以要想找到解决方法，必须把构建地图和定位过程一起考虑。在实际应用中，这些点的环境特征可以对应传感器提取特征的位置。SLAM问题的实质就是根据已记录的从时刻1到t的观测量、控制信息以及初始状态信息来决定所有路标位置集合和机器人位姿。若在t时刻所测得的路标数已知，那么SLAM问题将变得简单得多，概率分布描述成在时刻t的路标位置和小车状态的联合后验密度函数，即SLAM被定义为一个关于路径和路标的后验估计： (2.6)如果数据关联的集合也已知，那么SLAM后验