Turtlebot的建图导航之旅

Turtlebot是一款经典的小车机器人了。它是Melonee Wise和Tully Foote在2010年做出来的，是很多ROS教程的研究对象。也是要感谢Turtlebot的开源，使得我们有了一个系统的玩具，涉及到了机器人中的控制、定位、建图、导航等方方面面的内容，可以说是麻雀虽小五脏俱全。下面是各种Turtlebot相关的链接：

在介绍ROS系统的时候，我们已经简单的初步接触了Turtlebot了，并且在仿真环境下运行了两个demo，分别演示了建图和导航。本系列文章从这两个demo出发，结合概率上的机器人学(Probabilistic Robotics)的基础，来窥视一下SLAM以及导航控制都需要什么技术。内容安排如下：

第一部分：书接上文。这一部分已经在机器人操作系统之ROS系列文章中列举过了，目的是介绍工作环境，以及仿真系统。
第二部分：GMapping的基本原理。本部分结合GMapping的文章，介绍粒子滤波器的基本原理，它是GMapping和AMCL的基础。
第三部分：GMapping的封装和实现。本部分解析GMapping的源码，介绍它的框架设计和业务逻辑。
第四部分：amcl_demo浅析。amcl_demo是一个完整的定位导航例程，本部分浅析导航控制所需要的基本功能。
第五部分: navigation源码解析。本部分详细解析navigation的导航技术栈，重点在于move_base框架和相关功能模块的源码分析。
第六部分: amcl源码解析。在本部分中，我们讨论amcl定位器的源码。 amcl使用了《P.R.》的第5章、第6章、第8章中提到的大量技术。

我们从2019年3月份开始写本系列的文章，到2019年的8月低完成初稿。本意是分析GMapping源码入门机器人定位和SLAM的。在分析这两个demo的相关源码过程中，发现自主导航的过程也需要一个系统化的设计，所以添加了第五步分介绍导航技术栈navigation，以后我们还会写一个专题来介绍路径规划和导航控制。原本还想专门写一个部分来介绍amcl包相关的算法原理，但发现它所用的各个算法都能在《P.R.》一书中找到，所以就不再介绍了。

第一部分：书接上文

本部分中的这些文章都已经在上文中列举过了，从机器人操作系统之ROS系列文章过来的读者可以直接跳过这些内容。本部分主要是介绍一下我们的机器人和工作环境，以及仿真系统。为我们后续从gmapping和amcl两个demo走向SLAM和导航深处做铺垫。

Turtlebot你好!	本文是对网络上各种入门教程的浓缩，介绍一下仿真环境的搭建和运行。这里先跟Turtlebot打个招呼，让它在Gazebo的世界中摩擦摩擦，之后的文章都是讲Turtlebot自由摩擦背后的故事。
Turtlebot的描述模型	建立机器人模型应该是进行仿真的第一步工作，因为它将是我们以后开发算法的控制对象。本文详细分析了Turtlebot的描述模型，以及相关描述文件的组织方式。
Kobuki控制插件	Kobuki控制插件是Gazebo世界中的Turtlebot底盘与ROS的接口，我们通过该插件控制底盘运动，获取底盘上的传感器在仿真世界中的数据。本文详细解析Kobuki控制插件的源码。
Kinect控制插件	Kinect是微软出品的一款双目/深度摄像头，早期与XBox一起用于体感游戏风靡一时。为了在Gazebo环境中模拟Kinect，Gazebo官方为之提供了一个插件。
召唤Turtlebot	本文中我们将详细介绍召唤Turtlebot时的launch文件都做了些什么事情。
Turtlebot眼中的世界	Turtlebot官方提供了在Gazebo环境下仿真建图和定位导航的两个demo，本文介绍运行这两个demo的launch文件，目的是了解ROS系统是如何让这些功能模块运转起来的。至于具体的建图算法我们将在SLAM算法主题中予以介绍。

第二部分：GMapping的基本原理

GMapping是Giorgio Grisetti等人提出的一种从激光距离传感器中学习栅格地图的Rao-Blackwellized粒子滤波器。在概率机器人学中提到过，受限于解空间的维度，粒子滤波器的计算复杂度很高。

在粒子滤波中，每个粒子都有一个独立的环境地图，因此如何有效的降低粒子数量是粒子滤波用于SLAM问题的关键。GMapping是一种能够自适应的约减粒子的技术，它不仅考虑了机器人的运动还考虑了最近的观测值，来计算一个相对准确的建议分布，大大的降低了机器人位姿估计中的不确定性。

本部分详细解读GMapping的两篇文章，介绍它的基本原理。

粒子滤波器

粒子滤波器是一种非参数形式的滤波器，它使用序列蒙特卡洛的方法，以大量的样本来描述状态的概率密度，结合递推贝叶斯理论实现状态估计。这种滤波器不需要对状态空间建立精确的模型，适用于非线性非高斯的系统。在我看来它是一种万能的暴力方法，只要计算资源够用不计较计算时间，能够对任何系统进行估计。

GMapping的粒子滤波框架

GMapping是一种RBPF的实现，它在构建粒子滤波器的建议分布时，考虑了传感器的数据，达到了降低粒子数量，提高准确度的目的。此外，还提出了一个指标用于自适应的进行重采样，在一定程度上缓解了粒子匮乏的问题。

第三部分：GMapping的封装和实现

GMapping根据激光传感器的扫描数据和里程计来学习栅格地图。本部分从ROS的封装开始，了解GMapping的工作过程。然后解析其输入和输出的数据结构。通过对建图引擎的分析，我们把GMapping拆分为运动模型、扫描匹配器、粒子和轨迹树、重采样过程四个部分，并分别予以介绍。

GMapping的ROS封装——初始化	OpenSLAM给出了GMapping的源码，在ROS的环境下以openslam_gmapping包的形式进行维护。 slam_gmapping只是对其的一个封装。本文中介绍其初始化部分。
GMapping的ROS封装——更新	本文中介绍slam_gmapping的更新过程。
GMapping的地图结构	地图是整个SLAM建图过程中维护的对象，对于建图引擎而言，合理的地图表达方式是影响其运行效率的一个重要因素。本文中，我们详细解析GMapping中的扫描匹配地图数据结构ScanMatcherMap。
GMapping的传感器	GMapping以激光扫描的距离数据和里程计作为输入。为了方便管理传感器以及传感器数据，GMapping将传感器模型和传感器数据分开实现，可以在一定程度上提高系统的可扩展性。并且为传感器定义了两个基类，SensorReading和Sensor，提供一些基础的属性，比如传感器名称、传感器数据产生时间等等。
GMapping的建图引擎	建图引擎以激光传感器的扫描数据和里程计位姿作为输入，通过Raw-Blackwellized粒子滤波器完成SLAM任务。总体上它可以拆分为运动模型、扫描匹配器、粒子和轨迹树、重采样过程四个部分。
GMapping的运动模型	根据GMapping的建图引擎的分析，我们知道粒子滤波器需要经过预测和更新两个阶段。运动模型是滤波器实现预测的依据。本文分析MotionModel中实际参与建图的代码。
GMapping的扫描匹配器	扫描匹配是GMapping进行SLAM建图的一个重要环节。它存在的作用主要是，在预测机器人位姿的时候，参考激光传感器的扫描数据，以达到改进粒子滤波器中的建议分布的目的。
GMapping的粒子和轨迹树	建图引擎通过函数updateTreeWeights，完成了粒子权重的归一化操作，计算了用于判定是否需要进行重采样的指标\(N_{eff}\)，还更新了轨迹树中各个节点的权重和累积权重。但是我并不理解轨迹树的权重有什么用处。
GMapping的重采样过程	重采样是粒子滤波器得以成功应用的一个重要环节。它的存在，是为了消除早期SIS粒子滤波器的粒子退化问题。本文以流水账的形式的记录了GMapping的重采样过程。建图引擎的resample函数，不仅仅完成了根据粒子权重相似度进行重采样的操作，还更新了重采样后各个粒子的运动轨迹和地图。

第四部分：amcl_demo浅析

我们所运行的amcl_demo使用的是ROS生态下的一个2D导航技术栈—— Navigation。它用于根据里程计、传感器数据、目标点生成安全的速度控制指令，来控制底盘运动，从而实现机器人的自主导航的功能。 amcl_demo中一共运行了3个节点：map_server, amcl, move_base。本部分将依次从总体上分析它们，介绍它们的功能和使用方法。另外还有两个nodelet:kobuki_safety_controller, navigation_velocity_smoother，由于与导航定位无关，这里不再详细分析它们。

amcl_demo之地图服务器——map_server	map_server在acml_demo中扮演了地图数据提供者的角色。它发布了'/map'和'/map_metadata'两个latched主题，我们可以通过订阅这两个主题来获取地图信息。此外，还可以通过服务'/static_map'获取地图信息。
amcl_demo之机器人定位器——amcl	amcl在acml_demo中扮演了机器人定位器的角色。它订阅了"/scan"和"/tf"来获取激光传感器的参数和里程计的坐标变换，通过主题"/map"获取环境地图。使用粒子滤波器进行机器人定位，并通过主题"/amcl_pose"和"/particlecloud"发布对机器人位姿的估计以及粒子的位姿。
amcl_demo之导航控制器——move_base	move_base是ROS的导航技术栈的核心，它是一个技术框架，将导航技术栈中的各个模块进行组合，实现了机器人的导航控制。它通过全局和局部规划器进行路径规划，并输出控制指令控制机器人沿着规划出的轨迹运动。为了提高系统的可扩展性，move_base使用了插件的机制，用户可以设计自己的全局和局部规划器，以及代价地图的计算方式。这些都可以通过配置文件进行说明。

第五部分: navigation源码解析

move_base只是一个框架。代价地图的计算由costmap_2d包完成，全局和局部的规划器接口定义在nav_core包中定义，我们可以根据自己的需要，实现路径规划器和轨迹跟踪器，但是要符合nav_core中的接口定义。

move_base	浅析amcl_demo的时候，我们已经了解到move_base是一个用于导航控制的框架。它根据全局和局部的代价地图，进行路径规划并控制机器人按照规划的路径运动和避障。本文中我们将分析它的源码，具体了解整个框架的运行机制。
nvfn	nvfn为move_base框架提供了NavfnROS，它是amcl_demo默认的全局规划器。它使用Dijkstra和A*算法进行路径规划。本想在这里分析算法的实现，但源码写的很乱，就取消了。这里只是介绍了基本的工作流程。关于这两个经典的算法，我们将在数据结构与算法系列文章中详细介绍并实现。
dwa_local_planner	dwa_local_planner中的DWAPlannerROS是amcl_demo默认的局部规划器，它使用一种称为动态窗口路径(Dynamic Window Approach)的方法进行局部的路径规划。实际上它并没有实现DWA算法，只是对包base_local_planner进行了封装而已。
base_local_planner	包base_local_planner提供了进行DWA的局部路径规划的工具和基本功能模块。所实现的功能就是给定需要跟踪的轨迹和代价地图的情况下，根据当前的机器人和环境状态，计算用于控制机器人运动的速度指令。
costmap_2d	在move_base的框架下，costmap_2d为全局规划器和局部规划器提供了2D的代价地图。 costmap_2d使用的是占用删格地图，通过多个图层描述环境信息。每个图层描述了一种类型的信息，最终的代价是这些图层叠加的结果。

第六部分: amcl源码解析

amcl使用粒子滤波器在已知地图的情况下定位机器人位姿。它根据粒子集合的统计数据插入一定数量的新粒子，来解决全局定位失效或者说是机器人绑架的问题。并根据KLD采样自适应的调节粒子数量，可以在定位精度和计算代价之间进行权衡。

amcl的总体业务逻辑	本文是amcl源码解析部分的开篇，从总体上分析其源码结构和业务逻辑。由于完成主体业务的laserReceived函数写的太冗长了，我们删减了很多语句。其最核心的就是分别调用里程计对象完成运动更新，调用传感器对象完成测量更新，并在必要时刻进行重采样。
amcl的地图表示	地图是进行定位和导航的基本数据对象之一，amcl使用C语言定义了自己的地图结构map_t。本文分析该数据结构，介绍基本的接口函数，并细述根据地图数据和机器人位姿估计无噪声情况下的传感器测量值的方法。
amcl的传感器模型	和Gmapping一样，amcl使用里程计和激光雷达作为传感器进行定位。amcl实现了四种里程计模型和三种激光传感器模型。其中里程计模型是《Probabilistic Robotics》中的采样里程计运动模型的实现和改进。激光传感器模型则是波束模型和似然场模型的实现和改进。
amcl的粒子滤波器	一个典型的粒子滤波器需要经过"递推样本"、“权重计算”、“重采样”三个阶段。其中"递推样本"的工作是由里程计对象完成的，激光雷达对象则完成了"权重计算"的工作。本文介绍amcl的粒子滤波器框架，和重采样过程。了解amcl是如何自适应的控制粒子数量的。

展开

行军第九

孙子曰：凡处军相敌，绝山依谷，视生处高，战降无登，此处山之军也。绝水必远水，客绝水而来，勿迎之于水内，令半渡而击之利，欲战者，无附于水而迎客，视生处高，无迎水流，此处水上之军也。绝斥泽，唯亟去无留，若交军于斥泽之中，必依水草而背众树，此处斥泽之军也。平陆处易，右背高，前死后生，此处平陆之军也。凡四军之利，黄帝之所以胜四帝也。凡军好高而恶下，贵阳而贱阴，养生处实，军无百疾，是谓必胜。丘陵堤防，必处其阳而右背之，此兵之利，地之助也。上雨水流至，欲涉者，待其定也。凡地有绝涧、天井、天牢、天罗、天陷、天隙，必亟去之，勿近也。吾远之，敌近之；吾迎之，敌背之。军旁有险阻、潢井、蒹葭、小林、翳荟者，必谨覆索之，此伏奸之所也。

敌近而静者，恃其险也；远而挑战者，欲人之进也；其所居者易，利也；众树动者，来也；众草多障者，疑也；鸟起者，伏也；兽骇者，覆也；尘高而锐者，车来也；卑而广者，徒来也；散而条达者，樵来也；少而往来者，营军也；辞卑而益备者，进也；辞强而进驱者，退也；轻车先出居其侧者，陈也；无约而请和者，谋也。奔走而陈兵者，期也；半进半退者，诱也；杖而立者，饥也；汲而先饮者，渴也；见利而不进者，劳也；鸟集者，虚也；夜呼者，恐也；军扰者，将不重也；旌旗动者，乱也；吏怒者，倦也。杀马肉食者，军无粮也；悬甀不返其舍者，穷寇也；谆谆翕翕，徐与人言者，失众也；书赏者，窘也；数罚者，困也；先暴而后畏其众者，不精之至也；来委谢者，欲休息也。兵怒而相迎，久而不合，又不相去，必谨察之。

兵非贵益多，虽无武进，足以并力料敌取人而已。夫惟无虑而易敌者，必擒于人。卒未专亲而罚之，则不服，不服则难用。卒已亲附而罚不行，则不可用。故合之以文，齐之以武，是谓必取。令素行以教其民，则民服；令素不行以教其民，则民不服。令素行者，于众相得也。