基于激光雷达的智能车定位

自二十世纪七十年代初期美国斯坦福大学(SRI)的第一台被叫做Shakey的移动机器人研究成功，成为了我们人类科学技术在二十世纪以来的重要成果。移动机器人的问世是现代自动化技术与人工智能技术发展与融合的典型代表，也昭示着制造技术发达的水平。在过去50年中，在计算机技术飞速发展的带动下，人工智能技术取得了重大的突破性进展。越来越多的领域开始尝试使用人工智能技术提高生产效率，其中模式识别技术取代传统低效率识别方式、智能控制技术代替人工操作、传感器技术和数据融合让数据利用越来越精确。这一系列技术推动着智能机器人技术的飞速发展。在传统的工业系统当中，原有工业机器人对生产生活效率的提高起着微乎其微的作用。如今的智能机器人更加自动化并且工作环境不再局限于室内等构造简单的环境，而能胜任各种复杂多变的工作环境，如海陆空等。应用的领域也越来越广泛和无法估量，目前已经运用到了工农业自动化、军事、医疗等。智能移动机器人作为机器人学和人工智能领域结合的重点研究对象，以其突出的研究价值及在各领域潜在的广阔前景受到了全世界的高校和图1-1自主机器人在各个领域的应用智能车，即能实现不需要驾驶员，仅仅通过计算机和无线传感器控制就能实现自己行动的车辆，是移动智能机器人在交通领域的重要应用。智能车使用多种车载传感器，例如视觉相机、GPS导航、激光雷达测距仪、里程计等，获取车辆周围的环境情况，并通过传感器的数据创建出表示环境的地图，包括路径、障碍物的位置以及自身的位置，再这些的基础上完成定位、导航和避障等等行为。无人驾驶车辆顾名思义它具有智能控制的方式，它不再需要驾驶员这个可变因素较高的成分，而用了相对来说稳定的电脑来控制车辆行驶。我们给智能车安装了导航、定位、避障等等功能，使其能够有效稳定的完成任务，大大的提高了交通的效率和安全系数。这些技术都有同样的目的——保障智能车的安全可靠。而要实现智能车自主行驶最基本的技术便是定位，同时它也是研究中的最重点的问题。本课题中最关键的两个问题是对智能车的建图和定位，他们是相互依存的。智能车想要定位，那么需要有一个有效的环境地图，同时地图的创建又需要智能车当前的定位为基础。因为它们之前存在着这样密不可分的关系，所以我们称这个过程为同步定位与地图创建(simultaneouslocalizationandmapbuilding,SLAM),它是指智能车在当前未知的环境中一边建图一边在所创建的地图上定位自身的过程。这就是智能车能在未知环境中有效的移动而不是傻傻的到处乱跑的关键所在，在这个基础上智能车还可以更进一步的研究制造，完成更加复杂的任本次毕业设计的题目是基于激光雷达的智能车定位，我个人认为本次设计的选题意义在于对ROS平台的理解和应用，因为如今智能车以及智能机器人等智能的。况且能将所学到的应用到实际当中更是最好的。其次是从激光雷达传感器中获取数据的学习，因为之前对此是一无所知的状态，所以要想学习并理解激光雷达的数据获取还是要下很多功夫的，数据的获取对于智能车定位来说是必不可少的一个环节，因为这也是对其地图搭建和实现定位重要基础。最后就是对基于粒子滤波的SLAM算法进行一定学习和理解了，此次设计的定位算法所用到的方法就是粒子滤波进行定位。智能车的定位对于我们日常生活中的工作和学习是至关重要的，在生活中智能车可以代替人类完成很多人不能完成的工作和任务，尤其是在一些特殊环境下的工作。对智能车的研究意义重大，它可以解决人类的生活困难，在学习过程中也能加深我们对智能车的了解以便于更好的控制他为我们工作当前时代，各个世界强国都对智能技术趋之若鹜，特别体现在对智能车技术的需求，在这种背景下智能车技术成为了评价一个国家科技综合国力的标准之一，美国、日本、欧盟等都走在了世界前列。上世纪70年代末世界上首辆智能车在日本研发并制作成功。该车上采集信号的传感器由2台TV摄像机组成并装有专用的信号处理器，并在当时已经可以达到30公里每小时的速度。美国也在智能车领域进行了深入的研究并为此投入了巨额的费用，同时也是最早开始智能车研究的国家之一。从1980年开始，美国的卡内基梅隆大学、麻省理工大学、斯坦福大学就得到国家的支持开始了智能车方面的科学研究。很快他们就在智能车方面取得了重大的研究成果，研制出了如图1-2所示的NavLba11智能车。该出配备有目前世界上主流的传感器，通过视觉相机、里程计、激光雷达、GPS导航等完成了自主移动，并为后来智能车的发展做出了良好的铺垫，具有很重大的意义。图1-2卡内基梅隆大学的NavLab11智能车在欧洲，智能车的研究发展以欧盟为代表取得了卓越的成绩，各国之间也逐渐形成了统一的合作关系并建立组织。他们开发了自己的智能车框架项目，并且在欧盟各个国家的科学家组建的团队通力合作研究下，这些项目有了很卓著的成果。其中最具有代表性的实际应用是1997年12月Frog公司的ParkShuttle在荷兰阿姆斯特丹Schiphol机场的应用，如图1-3所示，虽然那只是早期的智能车技术，但在当时已经是很了不起的成就。它能在出发点和终点按一定的路线来1999年2月，在荷兰的鹿特丹，相同的智能车技术用于ParkShuttle在Rivium商业区的应用上。2002年，CyberCab以荷兰园艺博览会为平台完成了试验，任务是短距离输送当时的游客。如图1-4所示，这时技术已经比早期成熟许多，虽然同样是在规定的路线上完成对游客的运输工作，智能车系统能够完成2004年6月，法国借鉴欧盟的智能车项目，开发出了自己的智能车并进行了大规模的演示。2005年6月，该项目有了突破性的进展随之而来是法国智能车项目越来越多的展示，这些各种各样的成果展示都向世界展示了他们在智能车研究上取得的成就和未来发展的速度将越来越快。图1-3Frog公司CyberCar在机场的应用图1-4雅马哈公司的智能车辆CyberCab我们国家对智能车项目的研究起步比世界上其他大国晚，期初的规模也比不上国际水平，但我们投入了大量的精力现在也取得了不错的进展和成果。如上海交大、国防科大、清华、浙大等高校及其研究单位在这方面工作上做出了巨大的付出，同时也取得了杰出的成果。在1991年到1995年期间，我国的第一款智能车面世，取名为ALVLAB1。对程中自主进行道路的侦查和选择，能有效躲避障碍物并能够在野外泥泞等复杂环境中保持良好运行状态。在直路状态下最高时速21.6km/h,当在转弯以及躲避障碍物时也能保持12km/h的时速。总体上达到了国际智能车的性能水平。在大好形势下，我国在第九个“五年计划”期间成功研制出了我国的第二代智能车异，在第一代智能车基础上增加了支持临场感遥控驾驶及战场侦查等功能。其在复杂道路环境和无人驾驶状态下最高时速达到了74km/h,平均正常行驶速度为30.6km/h。在特殊路段如越野环境中，白天行驶时速最高达到24km/h,夜晚行驶时速达到了15km/h。在前两代智能车的基础上，由中科院与清华大学计算机系合作提供技术的支撑，在国家重点实验室中研制成功了我国新一代智能车THMR-V(TsingHuaMobileRobotV)。在原有基础上改良了智能车的道路适应系统，让其可以同时兼顾高速公路和一般道路的行驶。智能车THMR-V车体由道奇7座厢式车改装而来，设计其在高速公路上的时速为80km/h,在一般道路上的时速为20km/h。目前此智能车已经能够完成在非结构化道路环境下进行道路的侦查和选择和躲吉林大学也在中国智能车发展的道路上紧紧跟随上述各个单位的脚步，在组建自己的科研小组后成功的研制出了自己的智能车JUTIV-II。该智能车于上世纪九十年代初研制成功，具备了当时智能车普遍的功能，在国内是较早跟上当时技术水平的单位之一。还有2013年我国首次送上月球的智能车“玉兔号”(如图1-5所示),其代表这我国在智能车方面的研究已经跻身世界前列，是非常具有意义的。该车采用太阳能为动力源，具备了在外太空爬坡、避障和导航等功能。上海交通大学主持了欧盟CyberC3项目，并参加了欧盟CyberCars-2项目，已经研制出多辆智能车，如图1-6所示，该智能车性能稳定、优异，已经在上海市向市民进行了演示，并且投入实际工作和生活中使用。图1-5玉兔号月球车图1-6上海交通大学Cyberc3智能车辆本课题将把激光雷达集成到ROS平台上开展工作，以提高智能车的地图定位本课题利用ROS搭建智能车软件平台，利用TurtleBot和激光雷达搭建智能法设计能力、C++语言应用开发能力和综合应用能力。第一章：主要介绍本课题的研究背景和意义，以及智能车在国内外的发展概况第二章：介绍了智能车定位过程中创建的各种地图类型及其优缺点，详细介绍了本次课题采用的地图——基于激光雷达传感器的线段特征采集以及局部地第四章：介绍了实验智能车平台，实验的过程中重要步骤和代码的分析。第五章：对本课题的总结和展望。实现智能车在未知环境中的定位与其实现自主创建地图是互相依存的。地图是智能车所在环境的信息表现方式，它依据智能车所配传感器收集的信息而建立。根据创建使用的地图不同，有三种不同的方法创建地图来供智能车使用：拓扑方法、栅格法、特征法。这三个方向对应着不同的环境建图。本文主要讨论的是室内环境下的智能车即时定位与同步建图(SLAM),所以选择采用创建基于特征的地图。本章重点讲述各种方法创建的地图类型和它们各自的优缺点，也介绍了智能车传感器的模型并详细介绍了本课题所使用的地图创建过程——特征地图的创建。对于智能车来说，创建地图的方法是它能否快速有效的完成SLAM过程的关键。在如今智能车研究发展的结果表明一张切实有效的地图，都应满足以下三个(2)地图的更新迭代不困难，新的信息随时可以吸收；(3)智能车可以使用该地图完成我们对它布置的任务。前文介绍了创建地图有三种方法，而每种方法创建出的地图都有自己的特点和适用范围，应该根据实际情况选择合适的地图表示方法。2.1.1拓扑地图基于拓扑图表示法是将智能车所在的外部环境表示为一张拓扑图，周围环境中的每一个路标点(可以自行做标记)对应于地图中的一个节点，若果地图中的节点两两之间有一段弧则说明在真实环境中的2个路标点之间存在可以通行的路径。如下图2-1所示。拓扑地图通过非常抽象的拓扑图表示环境，不追求环境中具体的情况、甚至不同的节点之间它们实际的位置关系也是不精确的，所以创建时很方便。因为拓扑图具有这样的特点，所以它很适合用于创建大范围、室外、环境情况复杂的地图，如果需要对环境中的情况有需要的话可以将这些信息储存为对应节点的描述文件。基于拓扑地图的智能车SLAM有着难以创建和更新的缺点，维护也比较麻烦。特别是环境中又有两个地方很相似，而传感器的误差是不可避免的，通常这样的话我们就无法分析它们是否为相同的节点，创建的地图可能就出现非常大的误差，导致智能车无法完成任务。图2-1拓扑地图栅格法对地图的表示已经越来越多的应用在了智能车自主地图的创建，并在栅格地图上进行自主定位。该方法将智能车所在的环境信息收集起来，建立一个坐标系，并将用一个一个的栅格来储存收集的各种信息，每一个栅格有自己的坐标，也有一个数值用来表示该方格代表的环境中存在障碍物的概率。栅格法创建地图的优点在于创建的过程不复杂，数据的结构是坐标系也很容易计算，使得地图在需要维护和更新时更加的方便。如图2-2。图2-2占有栅格地图来创建整个地图，而全局地图的更新是通过更新局部栅格来实现的，再通过将局部地图与全局地图匹配，可以确定智能车的位置。栅格地图非常适合于智能车的路径规划是该方法最突出的优点，因为可行或不可行的区域颜色对比非常明显。颜色越深的地方代表障碍物存在的概率越大，反之概率就越小。不过它的创建难点在于确定每一个栅格的大小，这是显而易见的。如果要求地图的精度高，那么我们需要选择尺度较小的栅格，那么创建的地图数据结构很很大，导致需求的储存空间和计算的时间复杂度很高；如果选择大尺寸的栅格，那么地图的误差就会难以估计。因此栅格法在大规模地图中应用具将传感器置于需要检测的环境中，智能车通过传感器得到环境的具体数据并对这些数据进行分析，从而建立抽象的几何模型，如点、线、面等模型来表示环境。这就是特征地图。如图2-3所示为提取线段创建的特征地图。图2-3基于线段的特征地图特征地图具有表示紧密细致并且让定位目标位置和识别其信息变得更加方便的优点。比如在办公室等室内的封闭环境中，多数都是由显著的几何特性线段组成，这时就可以利用环境中的几何特征进行建模，可以简单明了的将环境具体的表现出来。由这种方法创建出来的特征地图能有效的对目标进行精确定位且能直观体现目标的位置信息。特征地图有效解决了在不同坐标系中难以转换和不同传感器之间的数据难以有效交叉相容的难题，更有利于在真实的室内环境中有效实施，很适用于智能车导航中的自定位，很多智能车系统都采用了这种方法。因为室内环境有很多的几何特征，便于采集，并且地图表达简单，存储空间较小。所以本课题采用特征地图来进行智能车的定位。基于特征地图的智能车SLAM需要从传感器收集的数据中采集出所需要的线段特征，再进行下一步的工里程计(Odometry)具有价格不高、采样速率高、短距离内的定位误差极小的特点，成为智能车很常用的一种定位工具。它的工作原理是通过光电编码器获得车轮在实验过程中旋转过的度数，(如图2-4所示)再结合车轮的周长，推算出智能车行走的路程距离来得到智能车距离出发点的位置情况。不过缺点在于误差较大，通常会结合其他的传感器完成数据的采集。本文采用常见的增量式正交光电编码器。在一个周期T内采样可以推算智能车左轮和右轮的位移分别是：假设智能车的2个车轮之间的宽度为D,那么在一个周期T内智能车位置和上式左侧就是里程计的输出量。2.2.2激光雷达测距仪模型激光雷达测距仪是移动智能车研究中常用的测距传感器，属于外部传感器。激光雷达测距仪采用的是激光束，它不受光照影响，采集到的环境信息误差相比里程计得到的信息精确度很高，并且具有质量轻、体积小、距离远、实时性高、抗干扰能力强等优点使其可以作为自主智能车主要的的环境信息获取传感器。工作原理和声呐类似如图2-5所示。由发射口发出一束激光信号，在信号传播的过程中如果遇到障碍物它就会反射回来，激光雷达传感器有自己的接收器，可以接受被反射回来的激光信号。通过发射和接收之间的时间间隔来计算出障碍物的距离，主要用于收集创建地图的数据。本文采用的激光传感器是日本北阳公司生产的2D激光扫描仪Hokuyo指标名称参数指标名称参数电源电压DC12V±10%光源半导体雷射入=870nm测距范围扫描角度测距精度0.1-10m:±30mm;角度分解度约0.25(360/1,440分割)扫描时间噪音接口USB2.0(全速)环境温湿度湿度：<85%RH(无凝露)激光扫描测距产品，测量距离0.1to30m,测量范围为270°,角度分辨率为0.25°(360°/1,440steps),精度为0.1toZ,=(p₁+o,,α₁+o₄)'i=1,2,3…,N2.3线段特征提取和局部地图匹配预期的特征或者特征模板进行对比并配对出相同的特征。上述过程中，通常传感器检测到的几何特征参数数目会大于被估计的数目。因为传感器的测量都存在不可避免的误差，所以不会存在相同的情况，而要考虑的就是如何优化我们的检测到的结果。例如我们用最小平方值来估计，可以得到一个有效的估计参数，使得我们得到的估计值与由传感器采集数据计算而来的测量值之间差异最小。2.3.2线段特征检测特征方法创建地图的具体流程如图2-6所示。通过激光雷达和里程计得到的原始数据提取出特征线段，然后进行滤波、聚类、拟合等步骤得到局部地图，再通过选取的角度对局部地图进行扫描，然后一一匹配来减少误差。最终的环境地据匹配聚类滤波拟合据匹配聚类滤波拟合数图2-7激光数据处理流程图激光雷达测距仪得到的数据并不是直接的特征地图，而是一系列的点。我们需要去除误差较大的点后才可以进行线段特征的提取。这一过程称为滤波，能提高地图的准确性。由于室内环境下线段特征容易采集，所以本课题采用线段特征的提取方法构建室内地图。这里我们使用的是聚类法提取特征，该方法主要是给各个采集点赋于不同的权值为数据处理手段，因此具有更高的精确性。得到了环境中的线段特征，我们需要通过聚类法将它们拟合成直线，然后才可以创建环境的特征地图。因为传感器的原因和数据计算过程中难免存在大量难以估计的误差，那么为了提高地图的精确度，对得到的线段特征使用一定的算法进行拟合得到直线特征，减少误差。实验得到的观测数据与之前的预测值的匹配问题，也称作数据关联。比如说通过扫描环境模型中特征值对应的路标点，这样做可以确认我们的测量值是否和预测值相同，或者说相同的测量值和预测值是否来自于同一个路标点。这样做可以防止错误的测量值被我们用于地图创建过程中的聚类，提高地图的可靠性。那么我们就需要更加复杂的数据关联技术进行关联匹配。Besl和McKay在1992年提出了ICP(IterativeClosestPoint)迭代最近点算法。这是一种从不同的角度对模型中距离最近的2个采集点进行不同的扫描匹配，找到对全局最合适的匹配方案，如图2-7所示。这个匹配问题可以描述为：对智能车从一个采集点到下一个采集点运动前后对同一路标点的两组激光测量数据，求出一个运动系数以使这两组数据指向的都是这个相同的路标点。然后继续重复上述的匹配过程。图2-8在椭圆模型中进行ICP匹配(1)计算距离最近点(2)计算配准(3)更新点集(4)迭代终止本章讲述在智能车定位过程所涉及到的几种地图类型及根据它们各自的特点，本课题采用的了其中的特征地图。介绍了在室内环境下采用基于特征方法表示智能车所处环境，并建立特征地图。特征地图使用里程计和激光雷达测距仪采集的数据，提取线段特征描述环境，简单明了便于储存。在创建地图的过程中由于激光雷达传感器的功能不稳定，地图有时会出现错误，这时需要等待地图的更新以改正。2007年WillowGarage公司和美国斯坦福的联合项目诞生了ROS操作系统。ROS从诞生开始就是面向全世界的智能机器人爱好者和研究人员，开源并且不收费，因此被广泛的使用在全世界各种各样的机器人上，是一种可靠优秀的分布式框架操作系统。它可以提供类似Windows这样的操作系统所具备的大多数功能，并且自带有许多简单有效的工具软件。最重要的一点是她支持多种编程语言编写的程序，能在机器人领域轻松加愉快的实现代码复用，因此ROS还支持代码库的上述所有功能都能由ROS的基础工具实现。本次课题就会用到R供的用于机器人导航的完整包navigation,其中包含了本次实验所使用到的重AMCL模块实现的。我们还会用到ROS系统自带的一个可视化3D软件RVIZ,它提供给我们地图的可视化界面。ROS的主要特点有点对点设计、不依赖编程语言、精简与集成、便于测试、开源并且规模大。这一级主要包括几个重要概念：节点，节点管理器，参数服务器，消息，主题，服务(1)节点：节点执行计算的进程，ROS上一个机器人控制系统有许多节点组成。(2)消息：每个不同节点间相互联络是通过传送消息进行通讯的，每一个消息(3)主题：消息发布订阅传输系统来传送，节点之前的通信是依靠主题来互通消息的。主题是用于识别消息内容的名称，发布消息必须伴随着一条主题。(4)服务：与主题类似也是一种消息传输机制。虽然基于主题的发布或者订阅模型是很灵活的通信模式，但是它对于同步传输模式并不适合。在ROS中服务被互行为表现为类似一个远程程序的调用。subscribeROS节点管理器通过RPC(RemoteProcedureCallProtocol,远程过程调用)提供了主题和服务的注册信息以及对其他计算图表的查找。节点之间是可以直接相连的，通过节点管理器提供的表信息，如同IP地址一样。节点就能找到其他节点并确定一种协议进行通信。比如控制节点订阅和发布消息的模型如下：XML/RPC8○3)connect("scan*.TCP)4)TCPserver:foo:23456)dataXML/RPC:foo:1234TCPdata:foo:2345messages句二二XML/RPC:foo:1234ROSRPC:foo:3456XML/RPC0图3-4ROS控制器控制服务ROS文件系统级类似于Windows系统，可以通过硬盘分区，通过图形界面来查看ROS系统中的文件，比如源代码文件等等。图3-5ROS系统文件种类ROS计算图中包含的内容类似于Windows中各种各样的文件，它们统一包含在ROS文件系统结构中，主要有以下2个重要的组织形式：包和堆。(1)包图3-6包图3-7堆功能包功能包清单消息服务代码其他功能包集图3-8ROS文件结构是开源的并且希望最大速度让它发展，所以采用软件仓库的管理模式来处理。这样做使得ROS爱好者和研究人员可以在通过其他人的创意来丰富自己的机器人，同时也可以共享自己的想法，达到共同推进ROS发展的目的。因为此类自上到下结构的设计，使得一个能够实现个体自主工作和根据环境的不同进行优化。这种结构上分布的优点，让独立地发展和实施工作成为可能，以指数级增加。社区级文件系统级社区级程序分布式管理程序如何组织构建描述程序如何运行3.2粒子滤波理论分析粒子滤波的核心是对智能车的路径位姿及地图进行分解的一种混合滤波算法。根据该思想，将联合后验概率分解：p(S,m/Z,,U,)=p(S,/Z,,U,)p(m/Z,S,)是t-1时刻前的控制量，实际反映为里程计的反馈。具体的算法流程如后面小节介绍。粒子滤波器是一种基本统计工具，其核心是序列重要采样(SequentialImportanceSampling,SIS)通常也称之为蒙特卡洛滤波器。目前已经成功应用在目标跟踪、智能车定位、地图创建等各个相关领域。通过采样样本表示概率分布，每一个样本称作一个粒子，每一个粒子赋予不同的权值。它提供了一种方便有效的计算后验概率的方法。粒子滤波器包括四部分：1)生成粒子集(采样步骤);4)重采样。根据先验概率随机选取N个粒子，作为初始状态，每个粒子对应自己的权值为W₀=1/N,根据规则和目标的先验概率得到初始状态集为X。=P(X₀)。积，即P(x₁,x₂)=P(x₂|x₁)P(x₁)。如果P(x₂,x₁)可以解析的表示，那么只需要对对于每一个粒子，根据特定的粒子历史计算预期分布，并且根据该分布采集X”~π(X₄|XO,Zx,U)并将该样本加入到粒子的历史中。3.2.3粒子权重的计算每个粒子的权值可以由如下的递推式得到：xw;P(z,ls,,z_,)其中，Z'¹代表t-1时刻及之前对环境特征的观测值，Z'表示t时刻的观测值，S'表示t时刻的智能车的位置。如果t时刻前的估计值不在测量值计算出来目标区间，那么计算会变得非常复杂，而且不一定能有结果。这里有一个优化的方案：将t时刻的观测值直接引入到对下一代的粒子集合的产生中。得到：p(s,m_,,s,,z,,u,,)的粒子样本集合，那么可以推出新的权值的计算方式为：3.2.4重采样当得到所有的样本粒子后，我们并不是要使用所有的样本粒子。我们会用权值高的粒子来代替那些权值较低的粒子，得到更加精确的定位信息。这个过程就称为重采样。这样做又存在一个关键的问题，当我们多次迭代之后，样本粒子中只剩下那些重复的权值大的粒子，我们的样本就会退化，即那些权值大的粒子就3.2.5状态参数的估计根据前文提到的各种模型得到粒子样本，再根据粒子样本估计得到的结果，更新对应的地图参数，通过激光数据得到的当前估计的位姿来对下一步的粒子滤波进行采样。具体高斯估计的参数如下：其中的为归一化参数：设智能车与设定的特征路标点的距离为r,行驶路线与路标的夹角为a,那其中的q为：q=(0m,y-S₂y)²+(0m,x-s,)²SLAM算法的过程，主要使用特征地图进行定位，对粒子的重要性权值计算和重本次实验平台是由实验室老师和研究生师兄自主搭建的一个智能小车，如图4-1、4-2、4-3所示。4-1轮式智能车4-2轮式智能车4-3轮式智能车该机器人采用前文介绍的激光扫描仪HokuyoUTM-30LX,基于TurtleBot智能机器人平台。TurtleBot是一个较为简单的基础机器人开发平台，自带有软硬件，使用ROS作为操作系统，能实现机器人的自主建图、导航、定位等功能。本次实验采用JoyStick手柄进行操控智能车，实验的环境为西南科技大学东7A座8楼的走廊。(1)打开ROS(2)在turtlesim控制节点 $rosrunturtlesimturtlesimnode启动小海龟仿真平面(3)发布控制命令在新的terminal中输入以下指令： $rostopicpub-1/turtlel/cmd_velgeometry_msgs/Twist-可以得到小海龟绘制8字形状命令，效果如下：4.2.2TurtleBot实现建图在使用完成TurtleBot对ROS进行初步认识之后，现在我们就应该把本课题的主体软件程序应用到此平台上来。在第二章中，我们介绍了激光雷达传感器采集环境。现在只要将数据添加到TurtleBot上，我们就可以用运用这些数据进行创建地图，加载地图的工作了。具体的各个命令顺序如下：I$roslaunchturtlebot_bringupminimal.launch $roslaunchturtlebot_navigationgmapping_demo.launch $roslaunchturtlebot_rviz_launchersview_navigation.launch $roslaunchturtlebot_teleopkeyboard_teleop.launch V如果还没实现存储地图工作，不能关上gmapping.launch运行保存的地图。4.3.1创造一个简单的ROS包ROS的软件以包的方式存在。包可以用自己写的软件封装，也可使用ROS提供的工具，例如roscreate-pkg来创建package:生成manifest.xml文件，如图4-4所示。<!--Theexporttagcontainsother,unspecified,tags--><!--Youcanspecifythatthispackageisameta<!--Youcanspecifythatthispackageisameta<!--<metapackage/>-->(2)将该路径添加到ros系统中(即更新ros查找路径):通过将改路径添加到ros的环境变量中来实现。export.ROS_PACKAGE_PATH=YOUR_BEGINNER_TUTORIALS_PATH:$ROS_PACKAGEPATHcatmanifest.xml,得到结果如图4-5。<!--Theexporttagcontainsother,unspecified,tags---><!--Youcanspecify<!--Youcanspecifythatthispackageisa<!--<metapackage/>-->图4-5申明显示结果rosdep命令：根据ROSpackages来安装系统变量msg:msg文件是储存了ROS消息的内容，可以生成各种适合于ROS的源代分开。srv文件存储在srv文件路径。(2)创建一个msg:_a.在已经创建好的beginner_tutorials_num”>msg/num”>msg/Num.msgpackage上创建msg:然后，用“geditCMakeLists.txt”打开package的CMakeList.txt,取b.查看msg文件：图风《《《《《□《《图风《《《《《□《《建立完成packge,相关程序及进行操作其文档应该在~/catkin_ws/src文件下，等价的打开命令通常是如图4-6所示的。$catkin_init_workspace$cmake…/Src-DCMAKE_INSTALL_PREFIX=../install-DCATKIN_DEVEL_PREFIX=../devel图4-6存放目录等价命令4.3.3用ruiz包实时显示机器人建立的地图Rosrunrvizrviz创建地图过程中截图如图4-7所示。GlobalStatus:okGeldTFLocalMapStatus:OkTop北PotionToleranceAngleToleranceKeepLengthLengthofeacharrow.3030fps图4-7创建地图的过程截图在上图中的黑点即为移动智能车的定位所在。(1)通过RVIZ对智能车进行直线定位，通过手柄控制智能车在走廊上进行直线行驶一定的距离，记录智能车在地图上与路路标的距离，使用卷尺测量智能车与路标的实际距离。(2)通过RVIZ对机器人进行非直线定位，通过手柄控制智能车在走廊上进行非直线行驶一定的距离，记录智能车在地图上与路路标的距离，使用卷尺根据数据可以对定位精度进行测试。测试数据截图如下所示：orlentation:y:0.0covartance:[6.1,6.0,0z:0.cstanp:Z:0.0orlentatlon:Y8O.thear:X:0.00.0,0.0,Y:OZ:0.0X:X:0.0,0.0,0.0,1.7976931348623157G0.0,1.7976931348623157e+308,0.0,0.0,0.0,0.0,turtlebotOturtlebotturtlebotOturtlebot:~Pose:x:0.0y:0.6tisttx:0.0x:0.Cy:0.0covarlance:[6.1,0.0.0.0.0.0.0.0,对数据进行整理可以得到：理论值(cm)实际值误差误差范围(cm)起始坐标终止坐标距离82定位精度在进行多次测量中，大部分误差范围均维持在10cm以内，仅少数误差范围超过10cm。验证了本文讨论的基于激光雷达的智能车定位算法是有效4.4重要源代码及分析$roslaunchturtlebot_navgmapping_demo.launch文件是初始化算法的文件，其中：入加载算法所需要的参数，并初始化；gmapping.launch.xml文件包含所有需要用到的参数，其中：<nodepkg="gmapping"type="slam_gmapping"name="slam_gmapping"output="screen">加载名为gmapping的包并初始化，生成名为slam_gmapping的可执行文件，参算法的主函数main.cpp:#include<ros/ros.h>#include"slam_gmapping.h"main(intargc,char**argv){ros::init(argc,argv,"slam_gmapping");gn.startLiveSlam();ros::spin();其中ros::init(argc,argv,"slam_gmapping");初始化gmapping节点；SlamGMappinggn;定义了一个类的对象gn;gn.startLiveSlam();调用gn里的一个功能函数来创建地图并定位。slam_gmapping.h中封装了一个很重要的类SlamGMapping:(部分重要的代码)classSlamGMapping{public:SlamGMapping();SlamGMapping(unsignedlongintseed,unsignedlongintmax_duration_buffer);~SlamGMapping();voidvoidvoidvoidsensor_msgssensor_msgs::LaserScan::ConstPtr&scan);laserCallback(const上述代码定义了激光数据的回调，以及地图的保存和更新。在SlamGMapping这个类中的构造函数SlamGMapping:SlamGMapping::SlamGMappingSlamGMapping::SlamGMapping():map_to_odom(tf::Transform(tf::createQuaternionFromRPY(0,0,0),tf::Point(0,laser_count_(0),private_nh_("~"),scan_filter_sub_(NULL),scan_filter_(NULL),}它的主要功能就是实现各种参数的初始化SlamGMapping