机器人最基本功能之一就是在环境中运动。为了有效地做到这一点，机器人需要知道它在哪里以及应该去哪里。通常情况下，需要为机器人提供环境地图、起始位置和目标位置来实现导航。在上一教程中，我们介绍了如何根据传感器数据构建环境地图。本教程，我们将讲解如何使用此地图和ROS导航包让您的机器人从世界的一个地方自动导航到另一个地方。
（1）ROS导航包
参考链接：http://wiki.ros.org/navigation
ROS导航包的功能是将机器人安全地从一个位置移动到另一个位置，而不会碰撞或丢失。它从里程计和传感器获取信息，根据目标位姿计算安全的速度命令，并发送给机器人。
图1 ROS Navigation Stack
表1 Navigation Stack Main Components
| Package/Component | Description |
|---|---|
| map_server | offers map data as a ROS Service |
| gmapping | provides laser-based SLAM |
| amcl | a probabilistic localization sySTEM |
| global_planner | implementation of a fast global planner for navigation |
| local_planner | implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation |
| move_base | links together the global and local planner to accomplish the navigation task |
（2）导航的主要步骤
图2 导航的主要步骤
安装导航包
ROS导航包不是ROS Kinetic默认安装的，因此需要额外安装，命令如下：
$ sudo apt-get install ros-kinetic-navigation
导航包的要求
三个主要硬件要求：ROS导航包只能处理差动驱动器和完整约束的轮式机器人，只要机器人不发生侧面移动，它也可以用于双足机器人的定位等；激光雷达必须安装在机器人的移动底座上，以创建地图和实现定位，或借助其他传感器（例如Kinect），生成与激光扫描相当的信息；它的性能最适用于近似方形或圆形的机器人。
导航规划器
本教程的机器人使用两种类型的导航规划器以实现地图中移动，分别是全局导航规划器和局部导航规划器。全局导航规划器用于为地图中的终点目标或远距离点目标创建路径，而局部导航规划器用于为近距离目标点，创建路径并实现避障。
全局导航规划器
NavFn（http://wiki.ros.org/navfn）提供了为移动基座创建规划的快速插值导航功能；全局规划在机器人开始向下一个目标点前进之前就应该计算出来；规划器使用Dijkstra算法在代价图上运行，以便从网格中的起点到终点找到一条代价最低的规划路径；全局导航规划器为局部导航规划器生成一系列航路点，作为后者的输入。
局部导航规划器
为机器人选择适当的速度命令以遍历全局路径的当前段。根据传感器、里程计数据以及全局、局部代价地图，能够在机器人运动过程中重新计算路径，以防止机器人撞击物体，同时保证到达目的地；实现了Trajectory Rollout and Dynamic算法。
图3 Trajectory Rollout Algorithm
取自ROSwiki：http://wiki.ros.org/base_local_planner
在机器人的控制空间（dx，dy，dθ）中进行离散采样；对于每个采样速度，从机器人的当前状态执行前向模拟，以预测如果在某些（短）时间段内应用采样速度会发生什么；使用包含以下特征的度量评估前向模拟产生的每个轨迹：接近障碍物、接近目标、接近全局路径和速度；丢弃非法轨迹（与障碍物碰撞的轨迹）；选择得分最高的轨迹并将相关的速度发送到移动基底；重复上述步骤。
局部导航规划器参数
base_local_planner.yaml文件包含大量ROS参数，设置这些参数可以用来自定义基本局部导航规划器的行为，这些参数分为如下几类：机器人配置、目标容忍度、前向模拟、轨迹评分、振荡预防、全局规划。
1.7.2 代价地图
代价地图（costmap）是一种数据结构，表示机器人在网格单元中的安全位置；它基于环境的占据网格图和用户指定的膨胀半径；ROS中有两种类型的代价地图：全局代价地图用于全局导航，局部代价地图用于局部导航；代价地图中的每个单元格都有一个[0（FREE_SPACE），255（UNKNOWN）]范围内的整数值，由costmap_2d包管理。
图4 Costmap Example
取自ROS wiki: http://wiki.ros.org/costmap_2d
膨胀
膨胀是指从占据网格单元中传播代价数值的过程，随着距离增大而减小。
图5 Inflation
地图更新
代价地图以’update frequency‘参数指定的速率执行周期的地图更新。在每个周期中：传感器数据输入，在代价地图的底层占用结构中执行标记和清除操作，该结构被投影到代价地图中，其中如上所述分配适当的代价值，使用LETHAL_OBSTACLE值对每个单元格执行障碍膨胀，这包括将成本值从每个被占用的单元向外传播到用户指定的膨胀半径。
代价地图参数文件
代价地图有3个配置文件：costmap_common_params.yaml、global_costmap_params.yaml、local_costmap_params.yaml。
参考：http://wiki.ros.org/costmap_2d/hydro/obstacles
1.7.3 定位与导航
定位是估计机器人相对于地图的姿势的问题。定位问题对物体的确切位置不是非常敏感，因此它可以处理物体位置的小变化。ROS使用AMCL包实现定位。
AMCL
AMCL是用于机器人在2D中移动的概率定位系统，它实现了自适应蒙特卡罗定位方法，该方法使用粒子滤波器来跟踪机器人相对已知地图的位姿，该算法及其参数在Thrun，Burgard和Fox的Probabilistic Robotics一书中有所描述（http://www.probabilistic-robotics.org/，公众号后台回复Probabilistic Robotics即可获取），目前AMCL仅适用于激光扫描，但是，它可以扩展到与其他传感器。
AMCL算法的输入包括基于激光的地图、激光扫描和变换消息，输出姿势估计；
订阅的话题包括：‘scan’、‘tf’、‘initialpose’、‘map’。
scan – Laser scans tf – Transforms initialpose – Mean and covariance with which to (re-) initialize the particle filter map – the map used for laser-based localization
发布的话题包括：‘amcl_pose’、‘Particlecloud’
amcl_pose – Robot's estimated pose in the map, with covariance.
Particlecloud – The set of pose estimates being maintained by the filter
move_base
通过move_base使用导航包能够将机器人移动到所需位置，move_base节点将全局规划器和局部规划器链接在一起以完成其导航任务，当机器人检测自己处于卡住状态时，它可以选择性地执行恢复行为。
TurtleBot导航
turtlebot_navigation包中包含使用gmapping进行地图构建、使用amcl进行定位以及运行导航包的demo。在param子目录中，它包含TurtleBot导航的配置文件。
表2 Navigation Configuration Files
| Configuration File | Description |
|---|---|
| global_planner_params.yaml | global planner configuration |
| navfn_global_planner_params.yaml | navfn configuration |
| dwa_local_planner_params.yaml | local planner configuration |
| costmap_common_params.yaml | costmap configuration files |
| move_base_params.yaml | move base configuration |
| amcl.launch.xml | amcl configuration |
已知地图的自主导航
导航demo的启动文件：amcl_demo.launch
<launch>
<!-- Map server -->
<arg name="map_file" default="$(env TURTLEBOT_GAZEBO_MAP_FILE)"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- Localization -->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<include file="$(find turtlebot_navigation)/launch/includes/amcl.launch.xml">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>
<!-- Move base -->
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
</launch>
启动带有turtlebot的Gazebo：
$ roslaunch turtlebot_gazebo turtlebot_world.launch
运行导航demo：
$ roslaunch turtlebot_gazebo amcl_demo.launch
图6 Autonomous Navigation of a Known Map
带导航包的rviz
此处，rviz的功能包括：提供机器人的大致位置（启动时，机器人不知道自己在哪里），将目标发送到导航包，显示与导航相关的所有可视化信息（规划路径，代价地图等）。
启动rviz：
$ roslaunch turtlebot_rviz_launchers view_navigation.launch
图7 rviz with Navigation Stack
定位TurtleBot
启动时，TurtleBot不知道它在哪里。例如，让我们将Gazebo中的机器人移动到（-1，-2）；现在在地图上提供它的大致位置：单击“2D Pose Estimate”按钮，单击TurtleBot在地图中的大致位置，然后沿TurtleBot指向的方向拖动；您将看到一组箭头，这些箭头是TurtleBot位置的假设；激光扫描应与地图中的墙壁大致对齐，否则，可以重复上述步骤，直到满意为止。
图8(a) Localize the TurtleBot
图8(b) Localize the TurtleBot
图8(c) Localize the TurtleBot
图8(d) Localize the TurtleBot
rviz中的粒子云
粒子云面板显示机器人定位系统使用的粒子云；云的传播代表了定位系统关于机器人位姿估计的不确定性；随着机器人在环境中移动，这个云应该缩小尺寸，因为AMCL使用新增的扫描数据改进其对机器人位置和方向的估计；要在rviz中观看粒子云：首先，单击“Add”并选择“Pose Array”，然后将话题名称设置为/particlecloud。
远程操控
远程操控可以与导航包同时运行，如果发送命令，它将覆盖自主行为，初步定位后，远程操作机器人以确保其收敛到对位置的良好估计，这通常是一个好主意。
发送导航目标
当确定TurtleBot的位置后，它可以自动规划通过环境。首先是发送目标：单击“2D Nav goal”按钮，然后在地图上点击希望TurtleBot到达的位置，并沿指向末尾的方向拖动。如果您想在机器人到达目标之前停止它，请在它的当前位置作为终点发送给它。
图9 Send a Navigation Goal
图10 Robot Moves to Destination
图11 Final Pose
图12 Final Pose In Gazebo
rqt_reconfigure
用于更改动态配置值的工具，要启动rqt_reconfigure，请运行：
$ rosrun rqt_reconfigure rqt_reconfigure
导航包参数将显示在move_base_node下，如下图所示：
图13 rqt_reconfigure
练习题
实现一个简单的导航算法（基于A *），该算法将使用地图的网格表示来计算机器人从其当前位置到给定目标位置的路线。
转自公众号：
Robot404