基于ROS的移动机器人室内导航算法研究及实现（源码+万字报告+部署讲解等)

目 录
第一章 绪 论 1
1.1课题研究背景及意义 1
1.2国内外研究现状 2
1.2.1国外研究现状 2
1.2.2国内研究现状 4
1.3机器人操作系统ROS简介 5
1.3.1机器人操作系统的发展 5
1.3.2机器人操作系统主要特征 6
1.4本文主要研究内容及组织结构 7
第二章 同时定位与地图构建 8
2.1地图表示方法 8
2.2定位方法 9
2.2.1卡尔曼滤波器 9
2.2.2蒙特卡罗定位 10
2.3 SLAM在ROS中实现 11
2.3.1 SLAM整体框架 11
2.3.2移动机器人模型 12
2.3.3 ROS中模拟SLAM 13
2.4本章小结 16
第三章 路径规划 17
3.1 A*全局路径规划算法 17
3.2 DWA局部路径规划算法 19
3.3路径规划在ROS实现 21
3.3.1代价地图 21
3.3.2 ROS中模拟导航 22
3.4本章小结 25
第四章 基于ROS的室内激光导航实现 26
4.1室内导航实验平台 26
4.2基于ROS的室内激光导航系统设计 28
4.2.1 ROS基本框架 28
4.2.2激光导航系统框架 28
4.3导航实验及结果分析 30
4.3.1室内导航环境介绍 30
4.3.2实验过程与结果 30
4.4本章小结 33
第五章 总结与展望 34
5.1工作总结 34
5.2工作展望 34
参考文献 35
致谢 37

第一章 绪 论

1.1课题研究背景及意义

随着科技的不断进步和发展，融合机械工程、电子工程及人工智能等众多学科的移动机器人受到了越来越多的关注。在工业领域中，装备有自动引导装置的AGV无人搬运车极大的提高了物料搬运效率；家用的扫地机器人使地面始终保持干净整洁；农业生产中的果蔬采摘机器人和农药喷洒机器人在提高果蔬产量的同时减少了人力成本；医院中的护理机器人可以实时监控患者的状态。此外，在军事、地质勘探、空间及深海探索等众多领域中，移动机器人都扮演了不可替代的角色[1] 。
不同国家和科研组织对移动机器人的定义各不相同，但总得来说，移动机器人是一种可在一定范围移动的自动化装置。与工业机器人和其他类型的机器人相比，可移动性无疑是移动机器人最具鲜明的特征。移动机器人在工作中往往要从一个地点平稳快速地移动到另一个地点，这就要求机器人具有最基本的自主导航能力。
1994年，Leonard和Durrandt阐述了机器人自主导航的三个关键问题：“我在哪”,“我要去哪”以及“我怎样到达”[2] 。第一个问题本质上是移动机器人自主定位问题，要求移动机器人要在参考系中找到自己的位置和方向，是自主导航的基础。第二个问题确定了机器人的导航目标，通常由操作人员指出。第三个是路径规划问题，将目标在已知地图上标定后，机器人将自动规划合理的路径，确保安全无碰撞地到达目的地。
由上述三个问题可以看出，机器人自身定位及完整的地图信息是导航的必要条件。在上世纪移动机器人导航研究的初始阶段，定位与地图构建这两个问题通常被分开讨论。一种情况是在机器人的精确位置和姿态已知条件下，通过外部传感器获取周围环境信息进行地图构建。另一种情况则是先将已知地图录入机器人，机器人利用环境信息不断校正自身姿态最终实现精确定位。但是，大多数实际使用场景无法满足前两种情况，陌生的环境下的机器人事先并不知道自身方位，也无法获直接获取完整的地图信息。与先有鸡还是先有蛋的问题相似，机器人定位与地图构建是两个互相依赖的进程。为此，同时定位与地图构建(Simultaneous Localization and Mapping, SLAM)的技术被很多学者认为是导航技术的关键[3] 。SLAM可以描述为机器人在未知环境移动的过程中，通过内外部传感器获得自身里程信息与外部环境信息，在创建增量式地图的同时不断更新自身的位置信息[4] 。
在获取位置和地图信息后，导航研究的另一重要问题则为路径规划。路径规划包含两个子问题：全局路径规划和局部路径规划。全局规划指根据地图信息规划一条可以绕过障碍物的最优路径。局部规划指当移动机器人在行进过程中遇到地图信息不包含的障碍物时，可以实时地调整其姿态及行进速度以避免碰撞。
如今移动机器人技术飞速发展，对其导航稳定性、实时性、精确度等要求也越来越高。近年来国内外的大批学者投身于移动机器人的导航研究，并为自主定位、地图构建、路径规划和实时避障等问题的解决做出了巨大的贡献。但距离日益提升的机器人自主导航要求仍存在不小的差距，依旧有很多问题需要被解决。
图1.1 移动机器人自主导航框架

1.2国内外研究现状

1.2.1国外研究现状

欧美和日本等地区首先在二战结束后开始了移动机器人的研究工作，并取得了很多投入使用的实际成果。1972年斯坦福研究所(Stanford Research Institute)发布了第一个真正意义上的移动机器人Shakey[5] 。Shakey配备了相机、测距仪与碰撞传感器等多种传感器，并可以通过无线电与电脑通信。融合了计算机视觉及人工智能的Shakey可以自行分析命令并执行导航作业，其采用的A*路径规划算法至今仍被广泛地使用。
上世纪末移动机器人开始走出实验室，代表当时移动机器人最高水准的是由NASA发射的火星探测车。第一辆火星探测车索杰纳号只有11公斤重，最大活动范围只有5米，功能十分有限。在2003年和2004年，NASA分别发射了勇气号与机遇号两个“双胞胎”火星漫游车，在火星研究工作中取得空前的成就。高精度的定位和导航技术也在其5年的服役期中发挥了至关重要的作用[6] 。勇气号与机遇号采用了多种定位技术，有适用于全局定位的无线电测控定位和高分辨率卫星影像定位，也有适用于局部定位的航迹推算和视觉侧程法，通过优化组合各种定位方法，保证了其定位信息的准确性。
近年来，随着移动机器人的市场不断扩大，很多顶尖公司与研究机构开始涉足这一领域。日本本田公司在2000年推出了两足机器人ASIMO，ASIMO通过头部的两个摄像头捕获的视觉信息检测多个物体的移动，判断其距离和方向，完成对物体的跟踪与识别。在躯干的下部还配备了激光、红外和超声波传感器，通过与内部陀螺仪配合，ASIMO可以精确地识别周边环境并控运动速度。加上预先录入的地图信息，最终实现自主导航与避障。

与精巧的ASIMO同样站在移动机器人顶峰的还有波士顿动力在2005年推出的BigDog和2016年的Atlas。被称为“世界上最有野心的腿式机器人”的BigDog能够以10公里的时速前进，攀登35度的斜坡，并承载180千克的负载[7] 。2016年推出的Atlas也是一款双足机器人，与ASIMO使用电机和减速器的驱动方式不同，Atlas使用了更加复杂的液压驱动，使其拥有了更快的响应、更高的能量输出和更持久的续航。研究团队通过在周围环境粘贴二维码标识，机器人可以通过视觉扫描完成对目标点的确定以及整体的路径规划。
图 1.1 Shakey移动机器人 图 1.2勇气号火星探测车
图1.3 ASIMO移动机器人 图1.4 BigDog四足机器人 图1.5 Atlas移动机器人
1.2.2国内研究现状

相对于欧美等发达国家，我国在此领域研究起步较晚，与世界顶尖水平也存在一定的差距。但面对近些年国内不断增加的人力成本以及对移动机器人的消费需求，各大科研院校和高新企业加快了对移动机器人的研究，与国外的差距也逐渐缩小。
目前国内高校中较为优秀的移动机器人研究团队有哈工大机器人所、上海交大机器人中心、北航智能机器人研究所、中科院沈阳自动化所等[8] 。这些团队在近十年也取得了很多骄人的成就，例如中科大研制的可佳服务机器人、国防科学技术大学研制的HQ3无人车、浙江大学研制的仿生四足机器人等。这些研究填补了国内自主移动机器人的空白，为国内机器人产业的蓬勃发展打下了基础[9] 。
商用及家用移动机器人产业的发展也带动了一批国内机器人企业的壮大。新松、博实等机器人上市公司陆续推出了拥有自主知识产权的自动引导车。自主引导车(Automated Guided Vehicle , AGV)在自动化运输领域承担的重要角色，其主要的导航方式包括轨道或磁条导航、激光雷达导航和视觉识别二维码导航。轨道或磁条的导航方式要事先在地面铺设磁条，通过读取磁条信息实现机器人点到点的移动，虽然定位精准但移动的灵活性很低。激光导航使用激光传感器获取周围环境信息，定位精确最高且线路灵活，但激光雷达其较高的成本及环境要求使其在现阶段难以大规模使用。视觉导航需要事先在地面张贴二维码，AGV通过视觉传感器检测识别二维码完成定位。视觉导航凭借其较低的成本以及较高的灵活性，成为各大AGV厂商的首选。
除了工业领域的AGV自主引导车，扫地机器人和无人机等家用移动机器人出现的频率也越来越高。扫地机器人同样也有三种主流的导航方式。一是惯性导航，机器人使用内置陀螺仪和加速度计推算位置信息，但在移动过程中其定位精度随误差累积迅速降低。虽然惯性导航精度低且无法自主规划清扫路径，但得益于较低的研发难度和硬件成本，反而成为低端扫地机器人最广泛使用的导航方式。二是激光雷达导航，依赖机器人顶部的激光雷达，扫地机器人可以创建所在房屋的二维地图并完成定位。小米扫地机器人为此导航方式的代表，其配备有LDS激光测距、超声雷达、三轴陀螺仪等十多种传感器，在沿墙清扫、实时避障及断崖检测方面都有很好的表现。最后则是视觉导航，与AGV视觉识别二维码不同的是，用户无需在室内布置二维码识别标识，而是依靠机器人自身的摄像头与红外传感器完成室内三维地图的创建。视觉导航算法实现难度较高，且在较强光照或无光的室内环境中存在较大干扰，目前仅有少数高端扫地机器人配备视觉导航系统。
不仅是上述所说的两种地面移动机器人使用了导航技术，自主导航也是空中移动机器人的重要功能。大疆DJI在2016年发布的精灵4 PRO无人机上不仅搭载了实现户外全局定位与导航的GPS模块，还添加了双目视觉传感器和惯性测量元件构建的视觉里程计。精灵4搭载了三套双目视觉系统，分别位于机身前后和底部。位于机身底部的双目立体视觉传感器可以探测下方物体获得高度信息，前后的视觉系统则构建场景的深度图并重建无人机周围30米范围内的三维地图。当GPS信号失效或在室内环境时，视觉里程计代替GPS为整架无人机提供精确的定位。通过建立机身周围环境的三维地图，实现避障、悬停、视觉追踪和智能返航等功能，大大提高了飞行过程中的安全和可靠性。
图1.6 自主引导车         图1.7 家用扫地机器人        图1.8 精灵4 无人机

1.3机器人操作系统ROS简介

1.3.1机器人操作系统的发展

近几十年来大量的工业、商业机器人被开发研制，但不同厂商使用的机器人开发系统也各有不同，使得开发人员和现场工程师难以对多种机器人进行控制。甚至同一种机器人因为硬件设施的更换，也会造成软件的不兼容。这种相对隔离的机器人编程环境大大制约了机器人开发人员的交流以及机器人的推广普及。
为了解决这一问题，机器人软件平台孕育而生。软件平台中包含了机器人常用的传感、识别、实时定位与建图、导航和机械臂控制等功能，而硬件则面向软件平台设计，并使用标准化的接口实现与软件平台的搭接。通过使用软件平台，机器人的开发人员无需掌握过多的硬件知识，只需专注于算法和程序设计，极大地提升了机器人的开发效率。目前相对流行的软件平台包括美国的ROS、欧洲的OROCOS、日本的OpenRTM和韩国的OPROS，ROS则是其中使用最多的也是最具前景的软件平台。
机器人操作系统(Robot Operating System,ROS)的雏形是2007年斯坦福人工智能研究所在STAIR(Stanford AI Robot)项目开发的Switchyard系统[10] 。从2008年开始，WillowGarage公司接替斯坦福人工智能研究所负责ROS的主要开发并在2010年推出了ROS 1.0版本。在随后近十年的时间中，先后12个版本的机器人操作系统被陆续推出。目前其支持180多种机器人和80多种传感器，麻省理工、斯坦福大学、加州大学和东京大学等世界顶尖机器人科研院校也纷纷使用ROS进行机器人开发。
本文使用在2016年推出的ROS Kinetic Kame版本并配合Ubuntu 16.04 Xenial Xerus 搭建了移动机器人导航的整体框架。
图1.9 ROS版本发展

1.3.2机器人操作系统主要特征

根据官方网站的描述[11] ：“机器人操作系统提供了包括硬件抽象、低级设备控制、常用功能的实现、进程之间的消息传递以及功能包管理在内的服务。还提供了用于在多台计算机之间获取、构建、编写和运行代码的工具和库。”与传统的操作系统如Windows、Linux和MacOS不同的是，ROS是一种元操作系统，它要依附于其他操作系统，以使用其进程管理系统、文件系统和用户界面。
ROS的目标是建立一个协作开发机器人软件的环境，将机器人研究和开发中的代码重用做到最大化。与其他机器人软件平台相比，ROS最显著的特征有[12] ：

1. 分布式进程：ROS中最小的执行单元是节点(Node)，每个节点可以单独运行，也可以多个节点相互通信并交换数据。
2. 功能包管理：开发者将相同目的的节点封装到功能包内，易于编译与修改。本文使用的ROS Kinetic版本在开源社区中已有一千多个功能包被发布，相同构架的算法与程序可使用和借鉴，避免大量的重复开发。
3. 多语言编程：在使用ROS进行开发时，系统提供了客户端库并可调用API将其加载到使用的代码。这意味着开发人员可以选用自己较为熟悉的编程语言，如C++、Python等。相对于传统的相对单一的机器人编程方法，ROS很大程度上减轻了开发人员的负担。
   正是基于以上三点特征，ROS将全球范围内机器人开发合作变为现实，并使研究过程中的代码重用变得越来越普遍。

1.4本文主要研究内容及组织结构

本文选用激光传感器搭建整个导航系统，并在研究移动机器人室内导航所需SLAM和路径规划算法后将其在ROS中实现，最终使用TurtleBot3 Burger移动机器人在室内环境中进行验证。本文分为五个章节，各章节安排如下：
第一章绪论主要分析了自主导航所包含内容，并对机器人国内外研究现状以及机器人操作系统进行最基本的介绍。
第二章主要介绍了同时定位与地图构架所包含的算法，其中包括环境地图的表示方法、机器人在陌生环境中的定位方法以及在ROS中实现SLAM的整体流程。
第三章研究了路径规划问题，包括全局路径规划和局部路径规划两个子问题并在ROS中进行模拟。
第四章在ROS中搭建了移动机器人导航系统，并以TurtleBot3 Burger移动机器人为平台在室内环境中对导航系统进行验证。
第五章总结全文内容并对移动机器人导航未来作出展望。

第二章 同时定位与地图构建

同时定位与地图构建（SLAM）指机器人在未知环境移动的过程中通过内外部传感器获得自身里程信息与外部环境信息，在创建增量式地图的同时不断更新自身的位置信息。本质上SLAM并非一个算法，而是由定位与地图构建这两个步骤集成的概念，而每个步骤都有相应的算法进行实现。本章内容包括常用的地图表示方法、定位方法及在ROS中构建的SLAM框架。

2.1地图表示方法

与我们人类通过地图完成对周围环境的描述与识别相同，机器人对环境的描述也是通过构建地图完成的。机器人将传感器采集的数据转换为可读取的地图，使用算法的不同和传感器类型的差异也会造成地图描绘方式的不同。目前占据栅格地图、特征地图和拓扑地图[13] 这三种环境地图在机器人领域中较为常用。
占据栅格地图是在ROS中最为常用的一种地图[14] ，也是本文所使用的二维地图。占据栅格地图将机器人周围环境分割为很多小的栅格，并在每个栅格中使用占有度等概率的方法表示障碍物存在的可能性。如图2.1所示，中心浅灰色表示机器人可以自由移动的区域，黑色表示障碍物和地图边界，而最外围的深灰色则是未被确定的区域。在ROS中使用0到100之间的数值定义该栅格的占有度，占有度越小就表示越接近自由区域，反之则表明存在障碍物越有可能存在，使用-1表示该区域未被探索。占据栅格地图构建较为方便且易于用户辨认，但在大范围环境中路径规划效率较低且无法解决视觉导航中的物体识别问题。
图2.1 ROS中的占据栅格地图
特征地图是将传感器采集的环境特征信息抽象为一个点、一条直线或者一个圆弧，常用于视觉SLAM中[15] 。相比第一种占据栅格地图，特征地图所包含细节较少，虽然路径规划效率较高但其定位精度难以保证。
拓扑地图是一种将环境信息转化为大量节点和连线的结构图[16] 。拓扑地图表示了各特征点之间的相对位置关系，而精准的距离和方位信息则被忽略。生活中常见的地铁线路图和公交线路图就是一种拓扑地图，每个站点在地图中都是一个节点，节点之间连线则为路径信息。拓扑地图可以很好地解决第三章所描述的路径规划问题，但其结构图的表达方式使操作人员很难直接读取地图也很难识别某一点的精确位置，不便于目标点的确定及局部的路径规划。

2.2定位方法

精确的定位是自主导航的前提，也是绪论中提出三个问题中第一个问题“我在哪里”所要解决的。对于室外行驶的汽车和移动机器人来说，最简单定位的方法自然是使用GPS。但对于室内移动机器人来说，由于室内GPS信号较弱且移动量较小，难以使用GPS进行精确定位和导航。

当机器人在室内移动时，航位推算法为最简单的位置估计方法。其本质为在知道初始方位的前提下，通过计算移动的距离和转角，推算下一刻的位置[17] 。对于室内移动机器人，则使用安装在电机上的编码器和安装在本体中的惯性测量单元对机器人的位置进行推算。编码器测得车轮转动角度，通过与已知车轮直径数据和两侧车轮转动角度差的计算可以得到移动机器人的行驶距离及偏转角度。同样，惯性测量单元测量机器人的加速度与速度数据，对其积分也可以获得里程信息。但使用编码器和惯性测量单元进行位置推算的过程中每一步都存在微小误差，误差不断累积，将会造成长时间移动后的位置完全丢失。行驶过程中常见的打滑、失步等情况也使航位无法推算。
为了解决航位推算法的低精度问题，可以在机器人中添加传感器，通过传感器获取周围环境信息并再次校正位置。目前这种多测量元件综合的定位方法被广泛地应用于机器人导航之中，常用的算法包括卡尔曼滤波和基于粒子滤波的蒙特卡罗定位。

2.2.1卡尔曼滤波器

卡尔曼滤波器已有50多年历史，但依旧是最常用的融合算法之一。卡尔曼滤波器是一种递归滤波器，可在有噪声的线性系统中跟随目标值状态[18] 。基于贝叶斯公式的卡尔曼滤波器预先假设了一个移动模型，并使用这个模型从过去状态预测当前的状态。在导航过程中与之匹配的则是利用航位推算法计算移动的距离和角度，从而进行下一时刻方位的预测。但由于航位推测存在较大误差，还需要激光测距仪获得实际距离值对机器人移动模型进行修正。虽然激光传感器的数据精度高于航位推算法，但仍存在一定误差。卡尔曼滤波就解决了如何将两者结合这一问题。假设航位推算法与激光传感器都存在高斯分布的误差，通过计算两个误差的方差值最后得出最优分配权重，并将整个过程重复迭代，从而提高定位的精准性。整个过程可由下列公式进行表示。
使用线性微分方程描述机器人移动过程：

激光传感器的距离测量值：

其中为当前的机器人位姿状态，为对系统的控制量，为激光传感器测量值，、、都为系统参数，和分别为航位推测和激光测距的误差，他们的协方差分别为和。
根据上一状态进行预测可得目前的状态参数：

其中是预测得到的目前状态，是上一状态的最优结果。此时系统的协方差为：

此时使用激光传感器数据补偿航位推测法得出的估计值，得到目前状态的最优估计值：


在得到目前的最优估计值后，还要求出目前的结果的协方差以进行下一时刻的推算：

将上述过程反复迭代便可对机器人的精确位姿进行计算，但卡尔曼滤波要求机器人内部里程信息和外部传感器信息的测量误差都符合高斯分布且仅适用于线性系统。但现实中多为非线性的移动机器人系统难以满足上述要求，为了应对这一情况，扩展卡尔曼滤波(EKF)被广泛使用。此外还有很多卡尔曼滤波的变体，例如快速卡尔曼滤波(FKF)和无损卡尔曼滤波(UKF)也都在EKF的基础上提高了机器人的定位精度。

2.2.2蒙特卡罗定位

蒙特卡罗定位是一种使用粒子滤波器的定位算法，而粒子滤波器则是目前较为流行的目标跟踪算法[19] 。上述的卡尔曼滤波算法可以看作是一种分析方法，其通过计算系统状态方程进行定位，而粒子滤波器则是通过大量的尝试和错误的方式获得机器人的位置。无论使用哪种算法，移动机器人的里程计信息和激光传感器数据始终存包含误差，粒子滤波器则使用粒子群来描述位置的不确定性。在此种方法中，每个粒子都是姿态(x, y, θ)和权重的函数,通过反复更新粒子的分布和权重以推测机器人的精确位置。整个粒子滤波器包括了下图四个步骤。
图2.2 蒙特卡罗定位过程
粒子初始化：在一开始不知道机器人位置和方向的情况下，将N个粒子在所有可能的姿态范围内随机分布，每个粒子的权重都为1/N。如果一开始知道机器人大致的初始位置和方向，可将粒子放在机器人附近以加快运算过程。
预测状态：根据机器人运动的系统模型结合包含噪声的里程计信息移动各粒子，不断生成机器人可能运动到的位置。
更新权值：基于激光传感器测得的距离信息，计算每个粒子的概率，并通过概率值更新粒子权重。
重采样：权重低的粒子将被去除，并以权重高的粒子为中心产生更多相同的高权重粒子，以保证总粒子数不变。
除了第一步初始化外，第二部到第四部将在整个过程中重复执行。在粒子数量足够的强况下，蒙特卡罗定位的精度将高于上述的扩展卡尔曼滤波和无损卡尔曼滤波。但过大的粒子数目也会造成运算量过大、实时性较差的情况，需要在不同移动状态中不断调整粒子数以满足定位需求。

2.3 SLAM在ROS中实现

2.3.1 SLAM整体框架

如前文所说，ROS是个开源的机器人操作系统，为了减少算法及代码的重复编写，不同内容的功能包被开发者上传到ROS社区中。 ROS中涉及SLAM的功能包包括gmapping、cartographer和rtabmap等，本文使用目前最为流行的基于激光传感器的gmapping功能包在ROS中实现SLAM。
在ROS中实现SLAM对移动机器人硬件有相应的要求。最基础的要求为移动机器人能够在地面上实现平移及旋转操控，其次要通过航位推测法推算机器人移动的距离和转角，最后为了实现多检测元件融合的精度定位，还需安装可以检测距离信息的传感器。下图为ROS中基于gmapping功能包实现SLAM的整体流程。
图2.3 基于ROS的SLAM流程图

2.3.2移动机器人模型

为了在ROS中实现对SLAM的构建与仿真，首先需要搭建一个满足SLAM硬件要求的移动机器人。本节创建了一个配备激光雷达的移动四轮移动机器人模型并发布其坐标变换、里程信息及传感器信息。
图2.4 移动机器人模型建立步骤 图2.5 移动机器人模型

1. URDF文件：标准化机器人描述格式(Unified Robot Description Format, URDF)是一种描绘机器人其内部结构、关节、连接的XML格式文件。其中关键代码包括定义机器人本体和四个轮子几何形状及相对位置的、 以及定义本体与轮子之间连接的。在连接关节定义中，将basic_link设置为父节点，wheel_link设置为子节点，并设置其为revolute可旋转关节。
2. Xacro文件：使用宏简化URDF文件，并添加模拟Hokuyo激光传感器，在之后的模拟中激光发射源将被从gazebo_ros_demo包中调用。
3. tf坐标变换：由于传感器测量的仅是障碍物及其自身的距离值，并非实际移动机器人与障碍物之间的距离。为了实现机器人的精确定位，需要坐标变化实现雷达坐标系向机器人坐标系的位置转换，并在ROS中以坐标变换树的形式展现。本文创建的机器人坐标变换如下图所示。其中map为固定的地图坐标系，odom为里程计坐标系。在运动一开始map与odom相互重合，但由于运动过程中的航位推算误差，两者之间将会产生里程计误差，通过map–>odom->base_link得到机器人在map中的坐标。base_link为机器人主体坐标系，而hokuyo_link为传感器坐标系，通过base_link–>hokuyo_link定义传感器与机器人本体的相对位置。下图为创建的四轮移动机器人tf坐标变换树。
   图2.6 tf坐标变换树
4. 发布里程信息：里程信息指移动过程中相对起始点的距离，在SLAM中则是base_link到原点odom的距离。里程信息由机器人移动时的线速度及角速度计算得出。
5. 发布激光传感器信息：在180°激光传感器hokuyo_link坐标系中以/robot/laser/scan为主题发布激光数据。
6. 创建基础控制器：基础控制器是唯一有效地控制机器人的方法，基础控制器通过订阅cmd_vel主题，生成正确的线速度和角速度命令来控制机器人。

2.3.3 ROS中模拟SLAM

在配置好移动机器人模型及发布其里程计、传感器和tf坐标变换后，就可以使用激光传感器信息和里程计信息来完成定位和创建地图。
首先将创建的移动机器人模型放置在模拟的三维地图中，并使用Gazebo仿真软件打开。Gazebo适用于复杂室内和室外环境的机器人仿真，能生成实际传感器的反馈以及物体之间的物理交互。下图为Gazebo模拟三维环境中的四轮移动机器人模型：

图2.7 Gazebo中模拟SLAM过程
通过发布hukoyu激光传感器信息，获取障碍物距离数据，并在Rviz软件中显示。图2.8中红色线条为激光传感器点云：
图2.8 Rviz中激光点云图
此时启动SLAM配置文件map.launch，调用gmapping功能包完成地图构建。同时发布基础控制器命令，使用键盘控制机器人在模拟环境中运动。在Rviz可以看到随着机器人的移动，占用栅格地图逐步被创建：图2.10(a)为初始时刻由180°激光传感器创建的地图，图2.10(b)为机器人在三维模型中不断移动后的占用栅格地图。
图2.9 SLAM配置文件map.launch
(a)
(b)
图2.10 Rviz中显示地图创建过程
完成地图构建后，使用map_server功能包命令生成并保存地图，该命令创建一个名为map.pgm的占据栅格地图文件和一个map.yaml的地图配置文件。最终整个SLAM过程创建的地图如下图所示：
图2.11 map.pgm占据栅格地图
图2.12 map.yaml地图配置文件
其中0.05的resolution为地图的分辨率，表示地图中的每个栅格为5*5cm的正方形。机器人在栅格的初始位置由origin表示。最后occupied_thresh和free_thresh分别为每个栅格被占据和未被占据的临界值。在此地图中，障碍物和边界出现在占据概率大于0.65的栅格中，而概率小于0.196的栅格都为机器人可以移动的区域。

2.4本章小结

本章的主要内容是对移动机器人同时定位与地图构建的研究。首先介绍了在机器人学中常用的三种地图表达方式，并选用占用栅格地图作为本文的环境地图。
然后介绍了基于贝叶斯公式的卡尔曼滤波器和基于粒子滤波器的蒙特卡罗定位两种自主定位方法。最终，通过在ROS中创建配有激光传感器的移动机器人，完成在陌生环境中的同时定位与地图构建模拟，验证了算法与SLAM整体框架的可行性。

第三章 路径规划

移动机器人在未知环境中通过SLAM获取自身位置并创建占据栅格地图后，为了实现自主导航，还需要根据自身位置和目标点的位置关系，自主规划一条安全路径，这便是第一章提出的第三个问题“我怎样到达”。路径规划包括两个子问题：全局路径规划和局部路径规划。全局路径规划指在已获得的环境地图中规划一条可以绕开所有障碍物的最短路径。当机器人按照全局路径规划的路线移动时，难免会有动态物体出现在规划好的路线上，这时机器人就需要使用局部路径规划算法实现实时避障。

3.1 A*全局路径规划算法

1972年斯坦福人工智能研究所首先在自己的移动机器人Shakey中使用了A算法，使其可以在含障碍物的房间中进行导航。A算法是一种基于Dijkstra算法和最佳优先搜索(Best First Search, BFS)的启发式路径规划算法，也是目前在已知环境地图中求取最短路径的常用方法[20] 。
在路径规划研究的初期，最早提出的两种算法为深度优先算法和广度优先算法。深度优先算法指在规划路径时朝一个方向不断前进，直到碰到障碍物或者边界，再开始另一个方向的尝试。即先完全探索一条分支，然后再探索另一条分支。其算法实现较为简单，但计算出的路径通常不为最短且计算时间较长。广度优先算法则是以移动机器人为起点，不断向外辐射，一层层推进，直到找到目标位置，并在所有可能的路线中选取最短的一条。相对于深度优先算法，广度优先算法虽然可以找到最短路径，但其过程将会遍历地图中大多数点，导致运算量巨大。
Dijkstra算法的原理与广度优先算法相同，也是一种发散式搜索算法。在找到目标点之前，该算法计算了移动机器人起始点到地图中所有点的代价。使用大量计算机资源遍历所有节点以找到最短路径。
最佳优先搜索则是一种基于贪心策略的算法。相对于Dijkstra算法计算起始点到所有点的最短距离，最佳优先搜索通过使用启发函数(Heuristics)不断搜索距离目标最近的点，完成点对点的计算。该算法指忽略了之前过程中所花费的代价，只考虑当前点到目标点之间的代价。由于这种特性，最佳优先搜索的计算量与计算时间远远小于Dijkstra算法，但通常无法保证在有障碍物的环境中找到最短路径。
A*算法综合了Dijkstra算法和最佳优先搜索的优点，在Dijkstra算法的基础上引入了最佳优先搜索使用的启发函数，在减少运算量的同时仍能找到最短路径。以下公式为其其数学描述：

其中为总的代价估计, 为起始点到当前节点的代价，为启发函数，代表当前节点到目标点的代价预估。当为0时，此时算法仅存在，忽略了启发函数的A算法就转变为Dijkstra算法。当远大于时，A算法则将近似于最佳优先搜索。
可由下列公式计算得出：

将S定义为起始点，如果n是S的下一个节点，那么为两点间的距离。如果n不是S的子节点，那么则累积上一过程的距离。
启发函数表示当前的n节点到目标的代价，不同的启发函数也会产生不同的路径规划结果。对于SLAM所建立的占据栅格地图，基于曼哈顿距离的启发函数较为常用：

使用曼哈顿距离的时候，忽略当前点和目标点连线间的障碍物，仅计算两点间的坐标差值。
A算法的具体流程如下：
1. 从起始点S开始，将此点加入开放列表中，开放列表为一系例将要被处理的节点。
2. 查看起始点S四周的节点，将其中未被障碍物和墙壁占用的可用节点加入开放列表中，并将四周节点设置为起始点的子节点。
3. 在开放列表中选择f值最小的节点N，并将起始点S从开放列表中取出，不再关注此点情况。
4. 计算N四周节点的f、g、h值。如果该节点在开放列表中，以g值为参考检查该节点周围节点是否存在更好的路径。如果新节点M的g值更小，则把当前路径设为新节点，旧节点N为新节点M的父节点，并重新计算新节点M的f和g值
5. 重复第三步和第四步，直到目标点加入开放列表中，将所有父节点相连接，此时父节点的连线就为最佳路径。
下图显示了在两种不同障碍物情况下Dijkstra算法、最佳优先搜索和使用曼哈顿距离的A算法路径规划结果，其中图(a)和图(d)使用Dijkstra算法，图(b)和图(e)使用最佳优先搜索，图©和图(f)使用将前两者结合的A*算法。图中左下角红色五角星为起始点，右上角紫色叉表示目标点，深紫色线条表示最短路径：