《基于ROS的移动机器人室内导航算法研究及实现分析研究电气工程及其自动化专业.docx》由会员分享，可在线阅读，更多相关《基于ROS的移动机器人室内导航算法研究及实现分析研究电气工程及其自动化专业.docx（41页珍藏版）》请在课桌文档上搜索。
1、摘要移动机器人作为机器人的重要分支，在工业、军事、医疗、太空探索等众多领域扮演着越来越重要的角色。自主导航作为移动机器人的鲜明特征和基本功能，已成为近年来研究的热点。移动机器人自主导航主要包含三个方面的问题：(1)地图构建，机器人需要在已知的环境地图中设定移动目标并规划路径，而如何在未知的环境中创建地图是自主导航首先要解决的问题。(2)定位，移动机器人要在全局坐标系中找到自身精确的位置和方向信息，这是自主导航的前提。(3)路径规划，在完成地图构建与定位后，机器人需要规划一条可以安全无碰撞抵达目的地的最优路径。其中前两个问题可由同时定位与地图构建(SimultaneousLocalization
2、andMapping,SLAM)技术解决。SLAM可解释为机器人在未知环境中探索时，依靠内部和外部传感器确定自身位置和姿态的同时完成未知环境地图的绘制。针对长久以来相对隔离的机器人开发与编程环境，本文采用更加开放的ROS开源机器人操作系统作为软件平台，使用TUmeBot3Burger移动机器人搭建了基于激光传感器的室内自主导航系统。本文所做具体工作如下：首先，对同时定位与地图构建技术进行了论述，介绍其中所涉及的地图表达方式及定位算法。并在ROS中搭建移动机器人模型与SLAM系统框架，使用占据栅格地图和基于粒子滤波器的蒙特卡罗定位算法完成仿真。其次，对路径规划中全局路径规划和局部路径规划两个子问
3、题进行研究。采用A*算法完成全局路径规划，得到当前位置与目标点之间的无碰撞最短路径。当机器人沿该条最短路径前进时，使用DWA动态窗口法进行局部路径规划，实现实时避障。通过结合代价地图，最终在RoS中完成导航仿真。最后，在ROS中将上述SLAM与导航系统进行搭建，使用搭载了360。激光传感器的TUmeBot3移动机器人在实际室内环境中完成了地图构建、自主定位、路径规划和实时避障的实验，验证了本文设计的导航系统的可行性。关键词：移动机器人，自主导航，同时定位与地图构建，路径规划，ROSAbstractAsanimportantbranchofrobots,mobilerobotsplayaninc
4、reasinglyimportantroleinmanyfieldssuchasindustry,military,medicalandspaceexploration.Autonomousnavigation,asadistinctivefeatureandbasicfunctionofmobilerobots,hasbecomeahottopicinrecentyears.Autonomousnavigationofmobilerobotsmainlyincludesthreeproblems:(1)Mapping,therobotneedstosetthetargetandplanthe
5、pathinthemap.Howtocreateamapinanunknownenvironmentisthefirstproblemtosolvefortheautonomousnavigation.(2)Localization,themobilerobotmustfinditsownprecisepositionandorientationintheglobalcoordinatesystem.Thisisthepremiseofautonomousnavigation.(3)Pathplanning,aftersettingthetargetinthemap,therobotneeds
6、toplanashortestpaththatcanbesafelyandcollision-freetoreachthedestination.ThefirsttwooftheproblemscanbesolvedbySimultaneousLocalizationandMapping(SLAM)technology.SimultaneousLocalizationandMappingisthecomputationalproblemofconstructingorupdatingamapofanunknownenvironmentwhilesimultaneouslykeepingtrac
7、koftherobotslocationwithinit.Forarelativelylong-termisolatedrobotdevelopmentandprogrammingenvironment,thispaperusesROSopensourcerobotoperatingsystemasasoftwareplatform,andusesTurtleBot3Burgermobilerobottobuildanindoorautonomousnavigationsystembasedonlasersensors.Thispaperhascompletedtheworkasfollows
8、:Firstly,theSimultaneousLocalizationandMappingisdiscussed,alsorelatedmaprepresentationmethodandlocalizationalgorithmsareintroduced.ThispaperbuildsamobilerobotmodelandtheSLAMsystemframeworkinROS,thenusesoccupancygridmapandMonteCarlolocalizationcompletethesimulation.Secondly,twosub-problemsofglobalpat
9、hplanningandlocalpathplanninginpathplanningarestudied.TheA*algorithmisusedtocompletetheglobalpathplanningandobtainthecollision-freeshortestpathbetweenthecurrentpositionandthetargetpoint.Whentherobotmovesalongtheglobalpath,theDWAalgorithmisusedtoperformlocalpathplanningtoachievereal-timeobstacleavoid
10、ance.Bycombiningthecostmap,thenavigationsimulationisfinallycompletedinROS.Finally,aSLAMandthenavigationsystemarebuiltintheROS.ByusingTurtleBot3mobilerobotequippedwitha360olasersensor,completedthemapconstruction,autonomouspositioning,pathplanning,andreal-timeobstacleavoidanceexperimentsinanactualindo
11、orenvironment.Thefeasibilityandreliabilityofthenavigationsystemareconfirmed.Keywords:mobilerobots,autonomousnavigation,SLAM,pathplanning,ROS.1.1.2 .2 .4.6 .7 .8 .8 .9 .9 .10 .11 .11 .12 .13 .16 .17 .17 .19.21.21 .22 .25 .26 .26 .28 .28 .28,j*Z右1*.1.1 课题研究背景及意义1.2 国内外研究现状1.2.1 国外研究现状1.2.2 国内研究现状1.3 机
12、器人操作系统ROS简介131机器人操作系统的发展1.3.2机器人操作系统主要特征.1.4 本文主要研究内容及组织结构第二章同时定位与地图构建2.1J力./2;.彳）/*1.1.1 卡尔曼滤波器1.1.2 蒙特卡罗定位2.3 SLAM在ROS中实现2.3.1 SLAM整体框架2.3.2 移动机器人模型2.3.3 ROS中模拟SLAM2.4 本章小结第三章路径规划3.1 A*全局路径规划算法3.2 DWA局部路径规划算法3.3 路径规划在ROS实现3.3.1 代价土也图3.3.2 ROS中模拟导航3.4 本章小结第四章基于RoS的室内激光导航实现4.1 室内导航实验平台4.2 基于ROS的室内激光
13、导航系统设4.2.1 ROS基本框架4.2.2 激光导航系统框架4.3 导航实验及结果分析.431室内导航环境介绍4.3.2实验过程与结果.4.4 本章小结第五章总结与展望5.1 工作总结5.2 工作展望30303033a* 35 错误!未定义书签。致谢第一章绪论1.1 课题研究背景及意义随着科技的不断进步和发展，融合机械工程、电子工程及人工智能等众多学科的移动机器人受到了越来越多的关注。在工业领域中，装备有自动引导装置的AGV无人搬运车极大的提高了物料搬运效率；家用的扫地机器人使地面始终保持干净整洁；农业生产中的果蔬采摘机器人和农药喷洒机器人在提高果蔬产量的同时减少了人力成本；医院中的护理机
14、器人可以实时监控患者的状态。此外，在军事、地质勘探、空间及深海探索等众多领域中，移动机器人都扮演了不可替代的角色。不同国家和科研组织对移动机器人的定义各不相同，但总得来说，移动机器人是一种可在一定范围移动的自动化装置。与工业机器人和其他类型的机器人相比，可移动性无疑是移动机器人最具鲜明的特征。移动机器人在工作中往往要从一个地点平稳快速地移动到另一个地点，这就要求机器人具有最基本的自主导航能力。1994年，Leonard和DUrrandt阐述了机器人自主导航的三个关键问题：“我在哪”,“我要去哪以及“我怎样到达“。第一个问题本质上是移动机器人自主定位问题，要求移动机器人要在参考系中找到自己的位置
15、和方向，是自主导航的基础。第二个问题确定了机器人的导航目标，通常由操作人员指出。第三个是路径规划问题，将目标在已知地图上标定后，机器人将自动规划合理的路径，确保安全无碰撞地到达目的地。由上述三个问题可以看出，机器人自身定位及完整的地图信息是导航的必要条件。在上世纪移动机器人导航研究的初始阶段，定位与地图构建这两个问题通常被分开讨论。一种情况是在机器人的精确位置和姿态已知条件下，通过外部传感器获取周围环境信息进行地图构建。另一种情况则是先将已知地图录入机器人，机器人利用环境信息不断校正自身姿态最终实现精确定位。但是，大多数实际使用场景无法满足前两种情况，陌生的环境下的机器人事先并不知道自身方位，
16、也无法获直接获取完整的地图信息。与先有鸡还是先有蛋的问题相似，机器人定位与地图构建是两个互相依赖的进程。为此，同时定位与地图构建(SimUltaneOUSLocalizationandMapping,SLAM)的技术被很多学者认为是导航技术的关键。SLAM可以描述为机器人在未知环境移动的过程中，通过内外部传感器获得自身里程信息与外部环境信息，在创建增量式地图的同时不断更新自身的位置信息。在获取位置和地图信息后，导航研究的另一重要问题则为路径规划。路径规划包含两个子问题：全局路径规划和局部路径规划。全局规划指根据地图信息规划一条可以绕过障碍物的最优路径。局部规划指当移动机器人在行进过程中遇到地图
17、信息不包含的障碍物时,可以实时地调整其姿态及行进速度以避免碰撞。如今移动机器人技术飞速发展，对其导航稳定性、实时性、精确度等要求也越来越高。近年来国内外的大批学者投身于移动机器人的导航研究，并为自主定位、地图构建、路径规划和实时避障等问题的解决做出了巨大的贡献。但距离日益提升的机器人自主导航要求仍存在不小的差距，依旧有很多问题需要被解决。图1.1移动机器人自主导航框架1.2 国内外研究现状1.2.1 国外研究现状欧美和日本等地区首先在二战结束后开始了移动机器人的研究工作，并取得了很多投入使用的实际成果。1972年斯坦福研究所(StanfOrdResearchInStiulte)发布了第一个真正
18、意义上的移动机器人Shakey。Shakey配备了相机、测距仪与碰撞传感器等多种传感器，并可以通过无线电与电脑通信。融合了计算机视觉及人工智能的Shakey可以自行分析命令并执行导航作业，其采用的A*路径规划算法至今仍被广泛地使用。上世纪末移动机器人开始走出实验室，代表当时移动机器人最高水准的是由NASA发射的火星探测车。第一辆火星探测车索杰纳号只有11公斤重，最大活动范围只有5米，功能十分有限。在2003年和2004年，NASA分别发射了勇气号与机遇号两个“双胞胎”火星漫游车，在火星研究工作中取得空前的成就。高精度的定位和导航技术也在其5年的服役期中发挥了至关重要的作用。勇气号与机遇号采用了
19、多种定位技术，有适用于全局定位的无线电测控定位和高分辨率卫星影像定位，也有适用于局部定位的航迹推算和视觉侧程法，通过优化组合各种定位方法，保证了其定位信息的准确性。近年来，随着移动机器人的市场不断扩大，很多顶尖公司与研究机构开始涉足这一领域。日本本田公司在2000年推出了两足机器人ASlMO,ASlMo通过头部的两个摄像头捕获的视觉信息检测多个物体的移动，判断其距离和方向，完成对物体的跟踪与识别。在躯干的下部还配备了激光、红外和超声波传感器，通过与内部陀螺仪配合，ASIMO可以精确地识别周边环境并控运动速度。加上预先录入的地图信息，最终实现自主导航与避障。与精巧的ASlMo同样站在移动机器人顶
20、峰的还有波士顿动力在2005年推出的BigDog和2016年的Atlaso被称为“世界上最有野心的腿式机器人”的BigDog能够以10公里的时速前进，攀登35度的斜坡，并承载180千克的负载。2016年推出的Atlas也是一款双足机器人,与ASIMC）使用电机和减速器的驱动方式不同,Alias使用了更加复杂的液压驱动，使其拥有了更快的响应、更高的能量输出和更持久的续航。研究团队通过在周围环境粘贴二维码标识，机器人可以通过视觉扫描完成对目标点的确定以及整体的路径规划。图1.1 Shakey移动机器人图1.2勇气号火星探测车图1.3 ASIMO移动机器人图1.4 BigDog四足机器人图1.5 A
21、HaS移动机器人122国内研究现状相对于欧美等发达国家，我国在此领域研究起步较晚，与世界顶尖水平也存在一定的差距。但面对近些年国内不断增加的人力成本以及对移动机器人的消费需求，各大科研院校和高新企业加快了对移动机器人的研究，与国外的差距也逐渐缩小。目前国内高校中较为优秀的移动机器人研究团队有哈工大机器人所、上海交大机器人中心、北航智能机器人研究所、中科院沈阳自动化所等网。这些团队在近十年也取得了很多骄人的成就，例如中科大研制的可佳服务机器人、国防科学技术大学研制的HQ3无人车、浙江大学研制的仿生四足机器人等。这些研究填补了国内自主移动机器人的空白，为国内机器人产业的蓬勃发展打下了基础。商用及家
22、用移动机器人产业的发展也带动了一批国内机器人企业的壮大。新松、博实等机器人上市公司陆续推出了拥有自主知识产权的自动引导车。自主引导车(AUtomatedGuidedVehicle,AGV)在自动化运输领域承担的重要角色，其主要的导航方式包括轨道或磁条导航、激光雷达导航和视觉识别二维码导航。轨道或磁条的导航方式要事先在地面铺设磁条，通过读取磁条信息实现机器人点到点的移动，虽然定位精准但移动的灵活性很低。激光导航使用激光传感器获取周围环境信息，定位精确最高且线路灵活，但激光雷达其较高的成本及环境要求使其在现阶段难以大规模使用。视觉导航需要事先在地面张贴二维码，AGV通过视觉传感器检测识别二维码完成
23、定位。视觉导航凭借其较低的成本以及较高的灵活性，成为各大AGV厂商的首选。除了工业领域的AGV自主引导车，扫地机器人和无人机等家用移动机器人出现的频率也越来越高。扫地机器人同样也有三种主流的导航方式。一是惯性导航，机器人使用内置陀螺仪和加速度计推算位置信息，但在移动过程中其定位精度随误差累积迅速降低。虽然惯性导航精度低且无法自主规划清扫路径，但得益于较低的研发难度和硬件成本，反而成为低端扫地机器人最广泛使用的导航方式。二是激光雷达导航，依赖机器人顶部的激光雷达，扫地机器人可以创建所在房屋的二维地图并完成定位。小米扫地机器人为此导航方式的代表，其配备有LDS激光测距、超声雷达、三轴陀螺仪等十多种
24、传感器，在沿墙清扫、实时避障及断崖检测方面都有很好的表现。最后则是视觉导航，与AGV视觉识别二维码不同的是，用户无需在室内布置二维码识别标识，而是依靠机器人自身的摄像头与红外传感器完成室内三维地图的创建。视觉导航算法实现难度较高，且在较强光照或无光的室内环境中存在较大干扰，目前仅有少数高端扫地机器人配备视觉导航系统。不仅是上述所说的两种地面移动机器人使用了导航技术，自主导航也是空中移动机器人的重要功能。大疆DJI在2016年发布的精灵4PRO无人机上不仅搭载了实现户外全局定位与导航的GPS模块，还添加了双目视觉传感器和惯性测量元件构建的视觉里程计。精灵4搭载了三套双目视觉系统，分别位于机身前后
25、和底部。位于机身底部的双目立体视觉传感器可以探测下方物体获得高度信息，前后的视觉系统则构建场景的深度图并重建无人机周围30米范围内的三维地图。当GPS信号失效或在室内环境时，视觉里程计代替GPS为整架无人机提供精确的定位。通过建立机身周围环境的三维地图，实现避障、悬停、视觉追踪和智能返航等功能，大大提高了飞行过程中的安全和可靠性。图1.6自主引导车图L7家用扫地机器人图1.8精灵4无人机1.3 机器人操作系统ROS简介1.3.1 机器人操作系统的发展近几十年来大量的工业、商业机器人被开发研制，但不同厂商使用的机器人开发系统也各有不同，使得开发人员和现场工程师难以对多种机器人进行控制。甚至同一种
26、机器人因为硬件设施的更换，也会造成软件的不兼容。这种相对隔离的机器人编程环境大大制约了机器人开发人员的交流以及机器人的推广普及。为了解决这一问题，机器人软件平台孕育而生。软件平台中包含了机器人常用的传感、识别、实时定位与建图、导航和机械臂控制等功能，而硬件则面向软件平台设计，并使用标准化的接口实现与软件平台的搭接。通过使用软件平台，机器人的开发人员无需掌握过多的硬件知识，只需专注于算法和程序设计，极大地提升了机器人的开发效率。目前相对流行的软件平台包括美国的Re)S、欧洲的OROCoS、日本的OPenRTM和韩国的0PR0S,ROS则是其中使用最多的也是最具前景的软件平台。机器人操作系统(RO
27、botOPeratingSyStem,ROS)的雏形是2007年斯坦福人工智能研究所在STAIR(StanfordAIRobot)项目开发的Switchyard系统刈。从2008年开始，WillowGarage公司接替斯坦福人工智能研究所负责ROS的主要开发并在2010年推出了ROS1.0版本。在随后近十年的时间中，先后12个版本的机器人操作系统被陆续推出。目前其支持180多种机器人和80多种传感器，麻省理工、斯坦福大学、加州大学和东京大学等世界顶尖机器人科研院校也纷纷使用ROS进行机器人开发。本文使用在2016年推出的ROSKineticKame版本并配合Ubuntu16.04XenialX
28、erus搭建了移动机器人导航的整体框架。BCDEFe*o*GHlJKL图1.9ROS版本发展1.3.2 机器人操作系统主要特征根据官方网站的描述山|：“机器人操作系统提供了包括硬件抽象、低级设备控制、常用功能的实现、进程之间的消息传递以及功能包管理在内的服务。还提供了用于在多台计算机之间获取、构建、编写和运行代码的工具和库J与传统的操作系统如WindoWs、Linux和MaCoS不同的是，ROS是一种元操作系统，它要依附于其他操作系统，以使用其进程管理系统、文件系统和用户界面。RoS的目标是建立一个协作开发机器人软件的环境，将机器人研究和开发中的代码重用做到最大化。与其他机器人软件平台相比，R
29、OS最显著的特征有：1 .分布式进程：ROS中最小的执行单元是节点(NOde),每个节点可以单独运行，也可以多个节点相互通信并交换数据。2 .功能包管理：开发者将相同目的的节点封装到功能包内，易于编译与修改。本文使用的ROSKinetic版本在开源社区中已有一千多个功能包被发布，相同构架的算法与程序可使用和借鉴，避免大量的重复开发。3 .多语言编程：在使用ROS进行开发时，系统提供了客户端库并可调用API将其加载到使用的代码。这意味着开发人员可以选用自己较为熟悉的编程语言，如C+、Python等。相对于传统的相对单一的机器人编程方法，ROS很大程度上减轻了开发人员的负担。正是基于以上三点特征，
30、ROS将全球范围内机器人开发合作变为现实，并使研究过程中的代码重用变得越来越普遍。1.4本文主要研究内容及组织结构本文选用激光传感器搭建整个导航系统，并在研究移动机器人室内导航所需SLAM和路径规划算法后将其在ROS中实现，最终使用TUrUeBOt3Burger移动机器人在室内环境中进行验证。本文分为五个章节，各章节安排如下：第一章绪论主要分析了自主导航所包含内容，并对机器人国内外研究现状以及机器人操作系统进行最基本的介绍。第二章主要介绍了同时定位与地图构架所包含的算法，其中包括环境地图的表示方法、机器人在陌生环境中的定位方法以及在ROS中实现SLAM的整体流程。第三章研究了路径规划问题，包括
31、全局路径规划和局部路径规划两个子问题并在ROS中进行模拟。第四章在ROS中搭建了移动机器人导航系统，并以TUrtIeBOt3Burger移动机器人为平台在室内环境中对导航系统进行验证。第五章总结全文内容并对移动机器人导航未来作出展望。第二章同时定位与地图构建同时定位与地图构建(SLAM)指机器人在未知环境移动的过程中通过内外部传感器获得自身里程信息与外部环境信息，在创建增量式地图的同时不断更新自身的位置信息。本质上SLAM并非一个算法，而是由定位与地图构建这两个步骤集成的概念，而每个步骤都有相应的算法进行实现。本章内容包括常用的地图表示方法、定位方法及在RC)S中构建的SLAM框架。2.1 地
32、图表示方法与我们人类通过地图完成对周围环境的描述与识别相同，机器人对环境的描述也是通过构建地图完成的。机器人将传感器采集的数据转换为可读取的地图，使用算法的不同和传感器类型的差异也会造成地图描绘方式的不同。目前占据栅格地图、特征地图和拓扑地图H3这三种环境地图在机器人领域中较为常用。占据栅格地图是在ROS中最为常用的一种地图”41,也是本文所使用的二维地图。占据栅格地图将机器人周围环境分割为很多小的栅格，并在每个栅格中使用占有度等概率的方法表示障碍物存在的可能性。如图2.1所示，中心浅灰色表示机器人可以自由移动的区域，黑色表示障碍物和地图边界，而最外围的深灰色则是未被确定的区域。在RC)S中使
33、用0到100之间的数值定义该栅格的占有度，占有度越小就表示越接近自由区域，反之则表明存在障碍物越有可能存在，使用-1表示该区域未被探索。占据栅格地图构建较为方便且易于用户辨认，但在大范围环境中路径规划效率较低且无法解决视觉导航中的物体识别问题。图2.1ROS中的占据栅格地图特征地图是将传感器采集的环境特征信息抽象为一个点、一条直线或者一个圆弧，常用于视觉SLAM中I。相比第一种占据栅格地图，特征地图所包含细节较少，虽然路径规划效率较高但其定位精度难以保证。拓扑地图是一种将环境信息转化为大量节点和连线的结构图1。拓扑地图表示了各特征点之间的相对位置关系，而精准的距离和方位信息则被忽略。生活中常见
34、的地铁线路图和公交线路图就是一种拓扑地图，每个站点在地图中都是一个节点，节点之间连线则为路径信息。拓扑地图可以很好地解决第三章所描述的路径规划问题，但其结构图的表达方式使操作人员很难直接读取地图也很难识别某一点的精确位置，不便于目标点的确定及局部的路径规划。2.2 定位方法精确的定位是自主导航的前提，也是绪论中提出三个问题中第一个问题“我在哪里所要解决的。对于室外行驶的汽车和移动机器人来说，最简单定位的方法自然是使用GPSo但对于室内移动机器人来说，由于室内GPS信号较弱且移动量较小，难以使用GPS进行精确定位和导航。当机器人在室内移动时，航位推算法为最简单的位置估计方法。其本质为在知道初始方
35、位的前提下,通过计算移动的距离和转角，推算下一刻的位置。对于室内移动机器人，则使用安装在电机上的编码器和安装在本体中的惯性测量单元对机器人的位置进行推算。编码器测得车轮转动角度，通过与已知车轮直径数据和两侧车轮转动角度差的计算可以得到移动机器人的行驶距离及偏转角度。同样，惯性测量单元测量机器人的加速度与速度数据，对其积分也可以获得里程信息。但使用编码器和惯性测量单元进行位置推算的过程中每一步都存在微小误差，误差不断累积，将会造成长时间移动后的位置完全丢失。行驶过程中常见的打滑、失步等情况也使航位无法推算。为了解决航位推算法的低精度问题，可以在机器人中添加传感器，通过传感器获取周围环境信息并再次
36、校正位置。目前这种多测量元件综合的定位方法被广泛地应用于机器人导航之中，常用的算法包括卡尔曼滤波和基于粒子滤波的蒙特卡罗定位。2.2.1 卡尔曼滤波器卡尔曼滤波器已有50多年历史，但依旧是最常用的融合算法之一。卡尔曼滤波器是一种递归滤波器，可在有噪声的线性系统中跟随目标值状态网。基于贝叶斯公式的卡尔曼滤波器预先假设了一个移动模型，并使用这个模型从过去状态预测当前的状态。在导航过程中与之匹配的则是利用航位推算法计算移动的距离和角度，从而进行下一时刻方位的预测。但由于航位推测存在较大误差，还需要激光测距仪获得实际距离值对机器人移动模型进行修正。虽然激光传感器的数据精度高于航位推算法，但仍存在一定误
37、差。卡尔曼滤波就解决了如何将两者结合这一问题。假设航位推算法与激光传感器都存在高斯分布的误差，通过计算两个误差的方差值最后得出最优分配权重，并将整个过程重复迭代，从而提高定位的精准性。整个过程可由下列公式进行表示。使用线性微分方程描述机器人移动过程：X(k)=/*X(Z-1)+B*U(k)+IV(Zc)(2-1)激光传感器的距离测量值：Z（八）=H*X(k)+V(k)(2-2)其中X(k)为当前的机器人位姿状态，U（八）为对系统的控制量，Z(Zc)为激光传感器测量值，4、B、”都为系统参数，W（八）和心口分别为航位推测和激光测距的误差，他们的协方差分别为Q和R。根据上一状态进行预测可得目前的状
38、态参数：X(kk-I)=A*X(c-1%-1)+8*U(Zc)(2-3)其中X(kk-1)是预测得到的目前状态，X(c-lc-l)是上一状态的最优结果。此时系统的协方差为：P(kk-1)=4*X(k-Ilk-l)A+Q(2-4)此时使用激光传感器数据补偿航位推测法得出的估计值，得到目前状态的最优估计值:X(kk)=X(kk-1)+K(Z(Z)-H*X(kk-1)(2-5)P(kR-1)*昭H*P(kk-l)*H+Rv)在得到目前的最优估计值后，还要求出目前的结果的协方差以进行下一时刻的推算：P(kky)=PWk-1)-K*H*PWk-1)(2-7)将上述过程反复迭代便可对机器人的精确位姿进行计
39、算，但卡尔曼滤波要求机器人内部里程信息和外部传感器信息的测量误差都符合高斯分布且仅适用于线性系统。但现实中多为非线性的移动机器人系统难以满足上述要求，为了应对这一情况，扩展卡尔曼滤波(EKF)被广泛使用。此外还有很多卡尔曼滤波的变体，例如快速卡尔曼滤波(FKF)和无损卡尔曼滤波(UKF)也都在EKF的基础上提高了机器人的定位精度。2.2.2 蒙特卡罗定位蒙特卡罗定位是一种使用粒子滤波器的定位算法，而粒子滤波器则是目前较为流行的目标跟踪算法1。上述的卡尔曼滤波算法可以看作是一种分析方法，其通过计算系统状态方程进行定位，而粒子滤波器则是通过大量的尝试和错误的方式获得机器人的位置。无论使用哪种算法，
40、移动机器人的里程计信息和激光传感器数据始终存包含误差，粒子滤波器则使用粒子群来描述位置的不确定性。在此种方法中，每个粒子都是姿态(x,y,O)和权重的函数,通过反复更新粒子的分布和权重以推测机器人的精确位置。整个粒子滤波器包括了下图四个步骤。图2.2蒙特卡罗定位过程粒子初始化：在一开始不知道机器人位置和方向的情况下，将N个粒子在所有可能的粒子初始化更新权值重采样1姿态范围内随机分布，每个粒子的权重都为1/N。如果一开始知道机器人大致的初始位置和方向，可将粒子放在机器人附近以加快运算过程。预测状态：根据机器人运动的系统模型结合包含噪声的里程计信息移动各粒子，不断生成机器人可能运动到的位置。更新权
41、值：基于激光传感器测得的距离信息，计算每个粒子的概率，并通过概率值更新粒子权重。重采样：权重低的粒子将被去除，并以权重高的粒子为中心产生更多相同的高权重粒子，以保证总粒子数不变。除了第一步初始化外，第二部到第四部将在整个过程中重复执行。在粒子数量足够的强况下，蒙特卡罗定位的精度将高于上述的扩展卡尔曼滤波和无损卡尔曼滤波。但过大的粒子数目也会造成运算量过大、实时性较差的情况，需要在不同移动状态中不断调整粒子数以满足定位需求。2.3 SLAM在ROS中实现2.3.1 SLAM整体框架如前文所说，RoS是个开源的机器人操作系统，为了减少算法及代码的重复编写，不同内容的功能包被开发者上传到ROS社区中
42、。ROS中涉及SLAM的功能包包括gmapping.cartographer和rtabm叩等，本文使用目前最为流行的基于激光传感器的gmapping功能包在ROS中实现SLAMo在ROS中实现SLAM对移动机器人硬件有相应的要求。最基础的要求为移动机器人能够在地面上实现平移及旋转操控，其次要通过航位推测法推算机器人移动的距离和转角,最后为了实现多检测元件融合的精度定位，还需安装可以检测距离信息的传感器。下图为ROS中基于gmapping功能包实现SLAM的整体流程。图2.3基于ROS的SLAM流程图2.3.2 移动机器人模型为了在ROS中实现对SLAM的构建与仿真，首先需要搭建一个满足SLAM
43、硬件要求的移动机器人。本节创建了一个配备激光雷达的移动四轮移动机器人模型并发布其坐标变换、里程信息及传感器信息。图2.4移动机器人模型建立步骤图2.5移动机器人模型1 .URDF文件：标准化机器人描述格式(UnifiedRobotDescriptionFormat,URDF)是一种描绘机器人其内部结构、关节、连接的XML格式文件。其中关键代码包括定义机器人本体和四个轮子几何形状及相对位置的Vlinkname=baSiJHnk、linkname-whee,以及定义本体与轮子之间连接的jointname=tbasicto-whee,o在连接关节定义中，将basicjink设置为父节点，wheelj
44、ink设置为子节点，并设置其为revolute可旋转关节。2 .Xacro文件：使用宏简化URDF文件，并添加模拟HOkUyO激光传感器，在之后的模拟中激光发射源将被从gazebo_ros_demo包中调用。3 .tf坐标变换：由于传感器测量的仅是障碍物及其自身的距离值，并非实际移动机器人与障碍物之间的距离。为了实现机器人的精确定位，需要坐标变化实现雷达坐标系向机器人坐标系的位置转换，并在ROS中以坐标变换树的形式展现。本文创建的机器人坐标变换如下图所示。其中m叩为固定的地图坐标系，OdOm为里程计坐标系。在运动一开始map与Odom相互重合，但由于运动过程中的航位推算误差，两者之间将会产生里
45、程计误差，通过map-odom-basejink得到机器人在map中的坐标。baseink为机器人主体坐标系，而HokuyoJink为传感器坐标系，通过baseink-hokuyoink定义传感器与机器人本体的相对位置。下图为创建的四轮移动机器人tf坐标变换树。4 .发布里程信息：里程信息指移动过程中相对起始点的距离，在SLAM中则是baseJink到原点OdOm的距离。里程信息由机器人移动时的线速度及角速度计算得出。5 .发布激光传感器信息：在180。激光传感器hokuyojink坐标系中以/robot/IaSer/scan为主题发布激光数据。6 .创建基础控制器：基础控制器是唯一有效地控制
46、机器人的方法，基础控制器通过订阅CnKLVel主题，生成正确的线速度和角速度命令来控制机器人。2.3.3 ROS中模拟SLAM在配置好移动机器人模型及发布其里程计、传感器和tf坐标变换后，就可以使用激光传感器信息和里程计信息来完成定位和创建地图。首先将创建的移动机器人模型放置在模拟的三维地图中，并使用Gazebo仿真软件打开。GaZeb。适用于复杂室内和室外环境的机器人仿真，能生成实际传感器的反馈以及物体之间的物理交互。下图为Gazebo模拟三维环境中的四轮移动机器人模型:/ Fixed Frame Grid fi Map LiserScin Status: Ok图2.7Gazebo中模拟SL
47、AM过程通过发布hukoyu激光传感器信息，获取障碍物距离数据，并在RViZ软件中显示。图2.8中红色线条为激光传感器点云:0Disptays GlobalOptionsFixedFfdfnemapBackgroundColorH48;48:48FrameRate30DefaultU9bt /GlobalStatus:OkTopicSC411UnreliableSdecUbIeStyleFUtSquares丽岫M005lAlpha1DecayTime0PosRIonTrdnsZo.XYZColorTransformerIntensityQueueSUe10ChannelNameintensityUserainbowUKweftRainbowOMinColor0:0;0Size(m)Pointsizeinmeters.图2.8RViZ中激光点云图此时启动SLAM配置文件map.launch,调用gmapping功能包完成地图构建。同时发布基础控制器命令，使
