移动机器人FASTSLAM算法研究

本科毕业设计题目：移动机器人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致谢31331 绪论移动机器人的同时定位与地图创建(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后验概率简化为： (2.7)SLAM在t时刻的后验概率可以用时刻后验概率函数来递归计算，这个递归的更新算法，称为SLAM的贝叶斯滤波器。下面的递归公式，也就是贝耶斯滤波器，可以用于已知在的后验概率情况下，计算时刻的后验概率： ( 2.8) 是代表概率分布的归一化因子。一般式中的积分是无法计算的，可以假设后验概率分布为一个特定形式来计算。机器人在环境移动时，采集了它自身运动的相关信息，这个信息可以用机器人轮子上带的里程计、内部的导航单元或通过观测机器人执行的控制命令得到。FastSLAM是RBPF(Rao-Blackwellised Particle Fitering)滤波器的一个实例 ，Murphy等人对动态贝叶斯网络（Dynamic Bayesian Networks）进行分析得出结论：若机器人的路径估计已知，则陆标估计（,）之间相互独立。由贝叶斯公式及特征估计之间的独立性假设，对后验概率分布分解： (2.9)其中右边第一部分用粒子滤波器估计，第二部分用扩展卡尔曼滤波器估计，每个粒子都有自己单独的地图，在每个粒子中，如果环境特征的个数是已知的，那么每个环境特征的位置都可以用一次EKF独立的进行估计。假设需要k个粒子实现SLAM，FastSLAM总共有kn个卡尔曼滤波器，粒子滤波器的每个粒子代表机器人的一条可能运动路径，利用观测信息计算每个粒子的权重： (2.10) FastSLAM 的EKF与传统的EKF相似，它近似线性高斯函数测量模型，线性高斯测量模型得到的分布 是高斯分布的，即使动作模型不是线性的，这也是对机器人姿态使用采样来近似分布的结果。2.4.4 SLAM公式推导SLAM分解可以从SLAM的路径后验概率中直接推导。用条件概率的定义，SLAM后验概率可以写成： (2.11)因此，要得到分解的后验概率，对所有的非负值t需要以下方程： (2.12)这个论断的证据可以从归纳中得到。要得到这个结果，需要两个中间结果第一个量是观测到的环境特征的概率。这个量可以用贝耶斯定理写成： (2.13)目前的观测量仅仅与机器人的状态和被观测到的环境特征有关。在（2.13）最右边的项，在没有新的观测量的情况下，当前的位姿、当前的控制量、当前的数据关联对没有影响。因此，所有这些变量可以省去得到： (2.14)需要的第二个中间结果是，没有观测到的任意环境特征的概率这很简单，如果没有观测到环境特征，环境特征后验概率不变。因此，环境特征的后验概率在时刻等于时刻。首先，在时刻做归纳假设： (2.15)归纳的开始情况,没有加入观测量。因此，分解因式是对的。在,再一次用贝耶斯定理来扩展（2.12）的左边可以得到： (2.16)等于单独的环境特征后验概率的乘积。 (2.17)2.4.5 有效的数据关联在实际的SLAM应用中，很少能观测到数据关联。然而，如果环境特征位置的不确定性相对于环境特征间的平均距离低，确定正确的约束关联的简单提示很见效。数据关联的最普通的方法是用最大似然定理来给每个观测量指派。换句话说，每个观测量是被指派给最可能的产生它的环境特征。如果最大的概率仍比固定的限值低，就被考虑加入一个新的环境特征。在EKF的情况下，观测量的概率可以写成观测量和期望观测量的差值的函数。这个差值被称为修正值： (2.18)数据关联的提示经常用负的对数似然方程表示，如下： (2.19)方程的第二部分就是Mahalanobis距离：通过观测量和环境特征估计的协方差米制规格化的距离。由于这个原因，米制的数据关联通常被称为最邻近的数据关联。最大似然数据关联主要用在数据关联比不正确的数据关联显著合适的时候。然而，如果环境特征的位置很不确定，多个数据关联的概率很高。如果选取的数据关联是错误地。对于结果地图的精确性会有影响。如果机器人的传感器噪声严重的话，很容易产生这种数据关联的不确定性。解决这个问题的一个方法是只加入那些得到明确数据关联的观测量。然而，如果SLAM环境是有噪声的，大多数的观测量得不到处理。而且，没有加入观测量会导致高估环境特征的协方差，使以后的数据关联变得更加不确定。2.4.6 FastSLAM的粒子表示形式在FastSLAM中，路标估计利用独立的EKF实现，一个路标对应一个EKF滤波器，地图是一组N个2维的高斯模型，而不是一个单独的联合2*N维高斯模型。共有M个粒子滤波器用来估计路径，每个粒子含有N个独立的EKF滤波器，用来估计N个路标，第i个粒子表示： （2.20）其中包含一个路径和基于该路径上N个路标高斯估计，第N个路标的高斯表征的均值和方差分别是和，如图2-3所示： 图2-3 FastSLAM的粒子表示2.4.7 FastSLAM的计算时间复杂度FastSLAM有机地将粒子滤波器与卡尔曼滤波器集成在一起，鲁棒性地解决数据关联和多目标跟踪问题，FastSLAM算法将状态估计分解成了采样部分和解析部分。由于这种分解，其计算复杂度仅为O，其中N,M分别为FastSLAM算法所用的粒子数和未知环境中的特征个数,每个粒子中的高斯组用稳定二叉树表示，高斯参数和在树的叶子上，访问每个高斯模型所需要的时间与K成对数增长关系。在复制粒子时，所有高斯模型树中只有一个路径必须被修改，为了完成这个树，对于所有与之分开的枝干，所有的相关点从生成的粒子复制过来。所有与之分开的枝干就指向了与生成树同样的子树，生成一个这样的不完全的书只用了K对数时间，此外，由于需要用来达到叶子的步骤数与路径的长度相等，访问高斯模型也需要K对时间。在每个更新步骤需要创建M个新粒子，所以更新总共需要0时间。FastSLAM算法是基于粒子滤波的，它对于机器人位姿和数据融合采样，在每个粒子的基础上计算路标的位置，用一些平衡的二叉树来表示路标，使得更新路标只需要路标数的对数次，每个粒子对位姿的不确定性是来自数据融合中，能更清楚的在噪声环境中区分出来路标，甚至能在很简单的数据融合条件下进行更新路标。 图2-4 从上一时刻复制和更新树的过程图3 模型建立3.1 运动模型在SLAM中有两个系统模型：观测模型和运动模型。运动模型描述了t时刻机器人位姿的概率分布。机器人运动模型如图3.1所示： 图3.1 机器人运动模型机器人的运动模型见式： （3.1）其中：、分别为时刻第i个粒子所表征的机器人质心坐标和方向角；为时刻到t时刻的时间间隔；、分别是机器人的速度和角速度；、分别为相应的噪声项。3.2 观测模型 观测模型即描述了当机器人位姿为时，观测到的信息为的概率。其中：为时刻里程计给出的控制信息。和分别表示时刻机器人的速度和角速度。机器人利用自身配备的距离/方向传感器探测陆标，得到陆标的距离和方向角。其观测模型见式： （3.2） 其中：分别是机器人的位置；是机器人的方向角；、分别是对应于测量距离和方向的噪声。机器人观测模型如图3.2所示： 图3.2 机器人观测模型 4 FastSLAM算法步骤4.1 FastSLAM算法步骤基于粒子滤波器和EKF的FastSLAM算法如下：1 根据初始坐标，随机生成个粒子、粒子权值组成的粒子集，见式 、： （4.1） （4.2） （4.3）2 预测机器人位姿 ，根据时刻粒子集中的每个粒子及运动模型，计算时刻粒子。3 计算权重，重要性权值计算方法见式 （4.4）4 根据观测模型计算粒子对应的特征点坐标，并将其与环境陆标集中的某一陆标进行关联。5 对粒子权值进行更新，见式: （4.5）式中，函数G为高斯概率计算公式，可根据对粒子集权值进行更新。6 归一化见式： （4.6）7 计算有效粒子数，设定一个阈值，若有效粒子数小于这个阈值，根据粒子权值对粒子进行重采样 ，去除权值小的粒子，复制权值大的粒子。8 计算权重。9 环境地图更新，利用对进行更新。10 通过公式计算粒子k时刻的位姿，若还要计算下一时刻的位姿，则转到步骤；否则结束定位。4.2 新位姿采样粒子集是递增更新的，是根据时刻的粒子和路标观测量，控制量得来的。机器人的新位置是由上一个位置和控制输入推算得出的，然后结合新的观测值来更新路标的位置。FastSLAM对上一时刻的路径和最近的控制量进行采样,t时刻中的每个粒子生成由可能性运动模型取样中获得的一个对机器人姿态估计的可能性估计，见式： （4.7） 然后将这种估计连同路径加入到一个临时粒子组中，假设中的粒子组根据来分配，则新的粒子也根据分配。通过这种方法生产M个粒子后，通过对临时粒子组取样获得新组。每个粒子通过权重因数更新获得。4.3 环境特征估计的更新环境特征估计是以机器人的位姿为条件的，个EKF加到集合的每个粒子上。第个环境特征的后验概率很容易得到。它的计算是要看是否，也就是，是否环境特征在时刻被观察到。对于被观察到的环境特征，根据通常用贝耶斯定理的分解后验概率的程序。 （4.8） （4.9）为第m个粒子的观测量概率函数的倒数。 若，则简单地使用高斯公式不变： （4.10） t时刻的均值和协方差矩阵更新： （4.11） （4.12）