之前博客《ROS学习笔记之——激光雷达SLAM建图》已经介绍过如何通过激光雷达SLAM建图了，本博文讲一下ROS机器人的导航相关
目录
导航相关理论介绍
导航的概述
costmap
AMCL
Dynamic Window Approach(DWA)
导航的四要素
地图
测量或估计机器人姿态的功能
识别障碍物，如墙壁和物体
计算最优路径和行驶功能
导航的步骤
导航所需的信息
基于gazebo的turtlebot3导航仿真（SLAM建图与导航）
参考资料
导航相关理论介绍
导航的概述
costmap
机器人的位置是根据从编码器和惯性传感器（IMU传感器）获得的测位来估计的。然后，通过安装在机器人上的距离传感器来计算机器人与障碍物之间的距离。导航系统将机器人位置、传感器姿态、障碍物信息和作为SLAM地图的结果而获得的占用网格地图调用到固定地图（static map），用作占用区域（occupied area）、自由区域（free area）和未知区域（unknown area）。
在导航中，基于上述四种因素，计算障碍物区域、预计会和障碍物碰撞的区域以及机器人可移动区域，这被称为成本地图（costmap）。根据导航类型，成本地图又被分成两部分。一个是global_costmap，在全局移动路径规划中以整个区域为对象建立移动计划，其输出结果就是global_costmap。而另一个被称为local_costmap，这是在局部移动路径规划中，在以机器人为中心的部分限定区域中规划移动路径时，或在躲避障碍物时用到的地图。
costmap用0到255之间的值来表示。数值的含义如下图所示。根据该值可以知道机器人是位于可移动区域还是位于可能与障碍物碰撞的区域。
导航使用占用网格地图。基于这种网格地图，根据机器人的位置和从传感器获得的周围信息，将各像素计算为障碍物、不可移动区域和可移动区域。这时用到的概念就是costmap。costmap由三种文件组成：公共的costmap_common_params.yaml文件、全局区域运动规划所需的global_costmap_params.yaml文件以及本地所需的local_costmap_params.yaml文件。TurtleBot3根据型号分为costmap_common_params_burger.yaml文件和costmap_common_params_waffle.yaml文件。
例如，实际的costmap如下图所示。具体的，机器人模型位于中间，其周围的黑色边框对应于机器人的外表面。当这个轮廓线碰到实际的墙壁时，意味着机器人会发生碰撞。绿色用从激光传感器获得的距离传感器值表示的障碍物，灰度的颜色越深的位置意味
着碰撞的可能性越高。这同样适用于用颜色表示的情况，粉红色区域是实际的障碍物，蓝色区域是机器人中心位置进入该区域则会发生碰撞的位置，且边框用粗红色像素绘制。这些颜色可以由用户在RViz中修改，因此可以说没有太大的意义。
AMCL
蒙特卡罗定位（MCL）位置估计算法在位置估计领域中被广泛运用。AMCL（自适应蒙特卡罗定位）可以被看作蒙特卡罗位置估计的改进版本，它通过在蒙特卡罗位置估计算法中使用少量样本来减少执行时间，以此提高实时性能。
由于这部分设计的数学推导比较多，后面会写博客专门针对这个进行分析。
Dynamic Window Approach(DWA)
动态窗口方法（Dynamic Windows Approach，DWA）是在规划移动路径和躲避障碍物时常用的方法，具体是指在机器人的速度搜索空间（velocity search space）中选择适当的速度，以躲避可能碰撞的障碍物的同时能迅速到达目的地。在ROS中，局部移动规
划中曾广泛使用Trajectory planner，而最近由于DWA的性能优越，因此DWA在逐渐代替Trajectory planner。
导航的四要素
导航是指机器人运动到一个指定的目的地，这说起来很容易，但完成它所需的技术一个个都不是容易的任务：要知道机器人本身在哪里，并要有一个给定的周围环境的地图，在各种路径中找出最优路径，在行驶中避免障碍物（如墙壁、家具、物体）等。也就是说导航的实现有四要素
- where the robot itself is
- have a map of the given environment
- find the optimized route among the various routing options
- avoid obstacles such as walls and furniture
地图
第一是地图。导航仪从购买时起就配备有非常准确的地图，并且可以定期下载更新的地图，以便可以根据地图将汽车引导到目的地。但是在使用服务机器人的房间里是否会有地图呢？服务机器人也像导航仪一样，需要一个地图，所以需要人创建一个地图，并把它
给到机器人，或者需要机器人自己创建一个地图。
SLAM（Simultaneous localization and mapping）就是为了让机器人自己（或接受人的一些帮助）绘制地图而出现的技术。用中文应该是“同步定位和绘制地图”。这是在机器人移动到未知空间时通过探测周围环境来估计当前位置并同时绘制地图的方法。
测量或估计机器人姿态的功能
第二，机器人需要自己能够测量和估计姿态（位置+方向）。汽车会用GPS估计自己的位置，但在室内无法使用GPS。即使说可以在室内使用，误差较大的GPS无法用于测量精细的移动。最近，虽然有DGPS5等高精度的定位系统，但在室内还是无法使用的。为了克服这种问题，人们引进了标志识别方式及室内定位系统等技术，但在成本或精确度方面还不足以投入实际应用。当前的室内机器人用的最多的是导航推测（dead reckoning）。它的缺点是只能估算相对位置，但因为仅用廉价的传感器就能实现，且已有较长时间的研究进展，因此可以得到一定水平的位置估计值，因此被广泛使用。导航推测技术用机器人的车轮的旋转量来估计机器人本身的移动量。但车轮的旋转量具有不少的误差。因此还利用IMU传感器等获取惯性信息来补偿位置和方向值，以此减小误差。
在ROS中，机器人的位置（position：x，y，z）和方向（orientation：x，y，z，w）被定义为姿态。
识别障碍物，如墙壁和物体
第三是一种利用传感器检测墙壁和物体等障碍物的方法。此时用到距离传感器、视觉传感器等多种传感器。其中距离传感器有基于雷达的距离传感器（常用的是LDS、LRF和LiDAR）、超声波传感器和红外距离传感器等，而视觉传感器包括立体相机、单镜相机、360度相机，以及经常用作深度摄像头的RealSense、Kinect和Xtion也都用于识别障碍物。
计算最优路径和行驶功能
第四是导航（Navigation）功能，这是计算到达目的地的最优路径，并且驱动机器人按照最优路径到达目的地的功能。实现这个功能的算法有很多种：称为路径搜索和规划的A*算法、势场算法、粒子过滤算法10和RRT（Rapidly-exploring Random Tree）算法等。
导航的步骤
导航是在给定的环境中将机器人从当前位置移动到指定的目的地。为此，需要有包含给定环境的家具、物体和墙壁的几何信息（geometry，geo-：土地，metry：测量）的地图， 机器人可以从自己的姿态信息和从传感器获得的距离信息来获得地图（SLAM）。在导航中，机器人利用这个地图和机器人的编码器、惯性传感器和距离传感器等资源，从当前位置移动到地图上的指定目的地。这个过程如下。
- 感知（sensing）——在地图上，机器人利用编码器和惯性传感器（IMU传感器）更新其测位（odometry）信息，并测量从距离传感器的位置到障碍物（墙壁、物体、家具等）的距离。
- 姿态估计（localization / pose estimation）——基于来自编码器的车轮旋转量、来自惯性传感器的惯性信息以及从距离传感器到
- 障碍物的距离信息，估计机器人在已经绘制的地图上的姿态（localization / pose estimation）。此时用到的姿态估计方法有很多种，常用的主要为粒子滤波器定位（particle filter localization），以及蒙特卡罗定位（Monte Carlo Localization）的变体ACML（Adaptive Monte Carlo Localization，自适应蒙特卡罗定位）。
- 运动规划——也称为路径规划，它创建一个从当前位置到地图上指定的目标点的轨迹。对整个地图进行全局路径规划，以及以机器人为中心对部分地区进行局部路径规划。可以使用基于一种避障算法Dynamic Window Approach（DWA）的ROS move_base和nav_core等路径规划功能包。
- 移动/躲避障碍物（move / collision avoidance）——如果按照在运动规划中创建的移动轨迹向机器人发出速度命令，则机器人会根据移动轨迹移动到目的地。由于感应、位置估计和运动规划在移动时仍在被执行，因此使用动态窗口方法（DWA）算法可避免突然出现的障碍物和移动物体。
导航所需的信息
下图7显示了运行ROS导航功能包所需的必要节点和话题之间的关系。
测位（‘/odom’，nav_msgs/Odometry）——机器人的测位信息用于局部路径规划，利用接收到的机器人的当前速度等信息，产生局部移动路径或避开障碍物。
坐标变换（‘/tf , tf/tfMessage）——由于机器人传感器的位置根据机器人的硬件配置而变化，所以ROS使用tf相对坐标变换。这只是简单地利用测位获得机器人的位置，例如测位描述“以机器人的位置为原点，传感器在x、y、z坐标坐标系中位于某某位置”。例如，经过odom→base_footprint→base_link→base_scan的变换后以话题来发布。它从move_base节点接收这些信息，并根据机器人的位置和传感器的位置执行移动路径规划。
距离传感器（‘/scan , sensor_msgs/LaserScan or sensor_msgs/PointCloud）——意味着从传感器测量得到的距离值。通常使用LDS和RealSense、Kinect、Xtion等。该距离传感器使用AMCL（adaptive Monte Carlo localization，自适应蒙特卡罗定位）来估计机器人的当前位置，且规划机器人的运动。
地图(‘/map , nav_msgs/GetMap）——导航使用占用网格地图（occupancy grid map）。将使用map_server功能包来发布slam建立的“map.pgm”和“map.yaml”文件（这两个文件是在slam建图后生成的）。
目标坐标（‘/move_base_simple/goal , geometry_msgs/PoseStamped）——目标坐标由用户直接指定。如在ROS的可视化工具RViz中设置目标坐标。目标坐标由二维坐标（x，y）和姿态θ组成。
速度命令（‘/cmd_vel , geometry_msgs/Twist）——根据最终规划的移动轨迹发布移动机器人的速度命令，而机器人根据该命令移动到目的地。
基于gazebo的turtlebot3导航仿真（SLAM建图与导航）
（https://www.ncnynl.com/archives/201702/1398.html）
（http://learn.turtlebot.com/）
由于放假，目前身边没有机器人，只能用gazebo做一下turtlebot的导航仿真了~~~
由于要进行导航，首先就需要先建立了环境的地图。故此，先基于gazebo来进行turtlebot3的slam
首先运行roscore，然后打开gazebo环境
roslaunch turtlebot3_gazebo turtlebot3_world.launch如下图所示
然后运行slam
roslaunch turtlebot3_slam turtlebot3_slam.launch
下面调用rviz（运行slam后就会自动打开rviz了）
roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch然后用键盘来控制
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
由于可能更改过turtlebot3burger的urdf文件。好像有点怪怪的。下面换一个机器人型号（运行SLAM、gazebo和rviz前要运行下面一句）
export TURTLEBOT3_MODEL=waffle
那么地图很快就可以建立好了
通过下面命令来保存地图
rosrun map_server map_saver -f ~/map2/map2保存地图如下所示
然后就可以运行导航了。
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map2/map2.yaml
初始位置估计
首先，要做的是估计机器人的初始位置。当在RViz的菜单中按下[2D Pose Estimate]时，会出现一个非常大的绿色箭头。将其移动到机器人在给定的地图中所在的位置，并按住鼠标键的同时，拖动绿色箭头使其指向机器人的前方。这是一种在初期为了估计机器人位置的命令。然后用turtlebot3_teleop_keyboard节点等来回移动机器人，搜集周围的环境信息，找出机器人当前位于地图上的位置。经过了这个过程后，机器人将绿色箭头指定的位置和方向作为初始位置，推定自己的位置和方向。
设置目的地且移动机器人
一切准备就绪后，下面下达移动命令。如果在RViz的菜单中按[2D Nav Goal]，会出现一个非常大的绿色箭头。该绿色箭头是指定机器人的目的地的标记，箭头的起点是机器人的x、y位置，箭头方向是机器人的i方向。将此箭头移动到机器人的目的地，然后拖
动，以设置方向。机器人将根据创建的地图躲避障碍物，移动到目的地
最终结果
运行rqt结果如下图所示（类似于section1中的结构图）
navigation package介绍
http://wiki.ros.org/navigation#Tutorials
A 2D navigation stack that takes in information from odometry（里程计）, sensor streams, and a goal pose and outputs safe velocity （速度） commands that are sent to a mobile base.
参考资料
http://wiki.ros.org/Books/ROS_Robot_Programming_English
http://wiki.ros.org/navigation#Tutorials