ROS导航功能包Navigation Stack

Navigation Stack功能包介绍

该功能包是常用于移动机器人导航与避障过程。其可以分为定位组件、代价地图组价、路径规划器组件、异常处理组件。

定位组件 AMCL

算法介绍请参考自适应蒙特卡罗粒子滤波定位算法。
安装方法

sudo apt-get install ros-kinnect-amcl

需要提供的话题：

1. 二维激光传感器 LaserScan 话题
2. 激光雷达以及机器人的坐标tf转换，通常有robot_state_publisher 发布
3. 二维的栅格地图，通过建图算法获得，由map_server提供地图
4. （不常用）机器人的初始化位姿点

通过AMCL节点推算出机器人位姿，会发布话题：

1. 机器人的amcl位姿估计
2. 机器人坐标odom与map之间的tf相对位姿
3. （可视化常用）粒子滤波的分布状态

代价地图 Cost_map2d

我其实想把map_server也归结到这里，都是提供地图的作用。

• map_server将二维栅格地图不加改变的发布出去，代价地图分为全局代价地图和局部代价地图。
• 全局代价地图是在map_server提供的地图上进行地图障碍物的安全距离膨胀，以保证机器人在全局路径规划时，不至于过于靠近障碍边缘。
• 局部代价地图稍微不同，他是通过设置的探索源进行动态障碍物的膨胀，以保证对于地图上新增或者未知区域进行探测，并对探测源障碍物进行膨胀。探测源可以为激光雷达的LaserScan 或者三维的PointCloud2 甚至是深度相机。

规划器 Planner

规划器分为全局规划器和局部规划器

• 全局规划器，在全局代价地图中进行起始点与目标点的最优路径搜索，获得全局的路径点。
• 局部规划器则是根据局部代价地图、全局路径的路径点、速度约束等进行局部的路径规划，并获得对机器人的速度实际控制指令，这样保证了在全局路径点的约束下到达目标点，在局部代价地图的约束下躲避了障碍物，在速度约束下更加合理的发送对机器人的控制指令。

异常处理 Recovery

异常处理又常被叫做错误恢复，有时候由于进入死角或者传感器数据阻塞、错误，导致机器人的局部路径规划不出合理的路径。常用的恢复多为原地旋转、倒车逃逸等方法，来清除机器人周围的局部代价地图产生的障碍物。

核心节点 Move_Base

作为Navigation stack的核心，move_base起到了多个组件的融合，实现了整个移动机器人的路径规划、导航避障等功能。

这个图中，涵盖了所有的组件，白色方框为需要自己提供的节点，灰色为可选节点。这里想提醒下关于可选节点的含义：

• AMCL的定位方法可以被其他的SLAM算法替代或者能够估算位姿的都行，但是大多情况下都是选用AMCL，所以当你提供了其他定位的就可以不用
• map_server加载二维栅格地图，是基于已知地图进行导航，当未来做到探索SLAM或者动态场景导航时，就无法事先获得地图，所以是可选的，基于已知地图导航，那么也不可缺少这一部分

接下来就是使用起来啦！！！

Navigation stack使用

先来看下launch文件

<launch> 
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="cloister_gmapping.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_navigation)/launch/amcl.launch" />

    <!-- 对于定位，需要设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>
</launch>

这里可以看到上面的那些组件都在文件中得到使用。

这里内部还涵盖了两个launch文件。

AMCL launch文件

<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.2"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.25"/>
        <param name="update_min_a" value="0.2"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

具体的参数意义参考 ros_wiki AMCL。

move_base launch文件

<?xml version="1.0"?>
<launch>

    <arg name="env_map" default="/home/wj/ros_map/navigation.yaml" />
  <!-- MAP SERVER -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg env_map)"/>

 <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /map /odom 100"/>
 
 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <!-- Costmaps -->
    <rosparam file="$(find robot_navigation)/config/move_base/costmaps/costmap_common.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot_navigation)/config/move_base/costmaps/costmap_common.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robot_navigation)/config/move_base/costmaps/costmap_local.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/move_base/costmaps/costmap_global.yaml" command="load" />

    <!-- Global Planner -->
    <!-- NavFn -->
    <param name="base_global_planner" value="navfn/NavfnROS"/>
    <rosparam file="$(find robot_navigation)/config/move_base/global_planner/NavFnROS/navfnros_planner.yaml" command="load" />
    <!-- SBPL Lattice -->
    <!--param name="base_global_planner" value="SBPLLatticePlanner"/-->
    <!--rosparam file="$(find robot_navigation)/config/move_base/global_planner/SBPLLattice/sbpllattice_planner.yaml" command="load" /-->

    <!-- Local Planner -->
    <!-- EBand -->
    <!-->
    <param name="base_local_planner" value="eband_local_planner/EBandPlannerROS"/>
    <rosparam file="$(find robot_navigation)/config/move_base/local_planner/eband_local_planner/eband_planner.yaml" command="load" />
    <-->
    <!-- Teb -->
    <!-->
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>
    <rosparam file="$(find robot_navigation)/config/move_base/local_planner/teb_local_planner/teb_planner.yaml" command="load" />
    <-->
    <!-- DWA -->   
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
    <rosparam file="$(find robot_navigation)/config/move_base/local_planner/dwa_local_planner/dwa_planner.yaml" command="load" />
   
    <!-- Common -->
    <rosparam file="$(find robot_navigation)/config/move_base/move_base/move_base.yaml" command="load" />
  </node>

 <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

可以看到主要加载了几个文件，分为代价地图（局部代价地图和全局代价地图) 规划器（全局规划器和局部规划器） 以及move——base的配置文件，和rviz节点。至于具体细节配置可以参考其他文献以及rowiki中的参数解释。

局部规划器和全局规划器可以选择其中一个，其他的进行注释，然后在配置相对应的参数文件。
一般直接拿别人的参数在场景开阔的地方应该可以跑出不错的效果了，至于在狭小地方，需要根据自己机器人的特性，配置好膨胀半径和速度限制。

下面是使用Cargrapher 建图后使用cartograther 替代AMCL进行定位，DWA 和Nav Global作为规划器。
具体的Cartographer 建图过程参考SLAM专栏。
首先通过Cartographer 建立2D的栅格地图，包括一个map.png map.yaml 和 cartograhpher 的位姿地图map.pbstream.
为了降低运算量，我虽然使用的是三维激光雷达，我用pointcould_to_scan节点转成了二维的单线激光雷达，其定位效果相差不大。

配置好cargrapher 定位所需的 urdf、2d_location.lua 我将激光雷达 、PointCLoud2Scan、以及Cartographer 定位放在同一个文件夹下启动。
Launch文件大致如下，根据自己的实际情况修改文件路径和名称。

<launch>
  <param name="/use_sim_time" value="false" />
 <!--- lidar  --> 
  <include file="$(find rslidar_pointcloud)/launch/cloud_nodelet.launch" />
  <include file="$(find pointcloud_to_laserscan)/launch/point_to_scan.launch" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/dcz_robot.urdf" />
  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename sick_2d_localization.lua
          -load_state_filename /home/ys/ros_map/dcz_lab_2d.pbstream"
      output="screen"> 
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<!--RVIZ-->
<!--
 <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

       <arg name="env_map" default="/home/ys/ros_map/lab/map.yaml" />
  
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg env_map)"/>
-->
</launch>

• 关于机器人控制，我有一个订阅，cmd_vel话题，然后进行速度分解udp下发至stm32或者其他的底层控制器，还有一个通过键盘按键，发送cmd_vel消息的节点。
• cartographer在定位初始化的时候，默认是建图起点，所以导航时做好将机器人放在建图起点，可以快速定位，否则可能需要前后左右 旋转一下，才能使定位到达一个良好的状态。
• move_base.launch 中将AMCL定位的组件注释掉就行了。

当定位完成后，可以通过RVIZ中的2D Navigation Go 发布目标点，注意在发布目标点前，需要关闭自己键盘手动控制机器人的节点，避免多节点发布同一个/cmd_vel话题，导致相互影响。如果没有问题，机器人应该可以实现导航到目标点。

参数解析

move_base

~base_global_planner (string, default: "navfn/NavfnROS" )
~base_local_planner (string, default: "base_local_planner/TrajectoryPlannerROS")
全局规划器和局部规划器，这里可以根据自己安装的插件 进行修改

recovery_behavior_enabled: false    #是否打开异常恢复插件，如果需要修改下方的恢复距离和恢复行为
#recovery_behaviors:
  #- name: 'super_conservative_reset1'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'conservative_reset1'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'aggressive_reset1'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'clearing_rotation1'
    #type: 'rotate_recovery/RotateRecovery'
  #- name: 'super_conservative_reset2'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'conservative_reset2'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'aggressive_reset2'
    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'clearing_rotation2'
    #type: 'rotate_recovery/RotateRecovery'
#super_conservative_reset1:
  #reset_distance: 3.0
#conservative_reset1:
  #reset_distance: 1.5
#aggressive_reset1:
  #reset_distance: 0.0
#super_conservative_reset2:
  #reset_distance: 3.0
#conservative_reset2:
  #reset_distance: 1.5
#aggressive_reset2:
  #reset_distance: 0.0

~controller_frequency (double, default: 20.0) 对机器人下发速度的频率
~planner_patience (double, default: 5.0)等待规划出新路径的最大时长
~controller_patience (double, default: 15.0) 控制器对于未收到机器人有效控制等待的最大时长
~conservative_reset_dist (double, default: 3.0) 对于多大范围进行costmap的reset，启用recorvery_beheavor时候有用
~clearing_rotation_allowed (bool, default: true) 旋转清除障碍物
~shutdown_costmaps (bool, default: false)关闭costmap
~oscillation_timeout (double, default: 0.0) 震荡多久时长后启用异常恢复，默认0代表inf
~oscillation_distance (double, default: 0.5) 机器人走多远距离，重置上面的计时器
~planner_frequency (double, default: 0.0) 全局规划器的规划频率，0表示全局规划器至进行一次规划
~max_planning_retries (int32_t, default: -1) 在执行恢复行为之前允许计划重试的次数，-1代表无限次

个人调试记录

当然还有很多细节问题需要去解决，接下来，将针对Nav的全局规划器和DWA的局部规划器在使用中遇到的问题进行总结。

• 首先应该知道，Nav是基于贪心算法的迪杰斯特拉的A Star算法。
• DWA的局部规划，通过对速度约束、障碍物约束、加速度约束、障碍物距离约束等获得速度解。

局部避障的动态窗口算法（DWA）及其调试的方法

Dynamic Window Approach（DWA）是重要的局部轨迹规划算法，ROS中使用了DWA算法获得了很好的局部路径规划的效果。具体的教程可参考官方的导航调试资料Navigation Tuning Guide。ROS的DWA（dwa_local_planner）应用到新机器人调试时，有一些技巧需要注意的，这里做一些总结。

1.设置坐标系和话题

需要订阅的话题：

里程计话题：/odom

需要设置的坐标系：包含/map，/odom，/base_link

2.参数调试

加速度的限制

加速度限制（acc_lim_x，acc_lim_y，acc_lim_th）非常的重要，如果不知道机器人的加速度，可以尽量的往大的设置，因为如果设置太小了，往往会出现机器人往前跑断断续续的，转弯转过头（看似加速度太大了，实际是加速度太小，以至于机器人想把机器人掰回来而掰不及），从而导致反复的震荡（oscillating over and over），例如下面的错误提示：

Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

~/acc_lim_x (double, default: 2.5)

The x acceleration limit of the robot in meters/sec^2
开始调试尽量往大的设置

~/acc_lim_y (double, default: 2.5)

The y acceleration limit of the robot in meters/sec^2
如果是差分轮，设置为0即可

~/acc_lim_th (double, default: 3.2)

The rotational acceleration limit of the robot in radians/sec^2
开始调试尽量往大的设置

oscillation_timeout: 5.0
oscillation_distance: 0.01

如果上面的方法依然报错，建议增大timeout时长，减小distance距离，避免原地旋转的时候时钟触发错误的Aborting oscillation

仿真的时间

本地规划器将机器人控制空间中的速度采样，并检查由这些速度样本表示的圆形轨迹，并最终消除不良速度（轨迹与障碍物相交）。在机器人的一段时间间隔内，每个速度样本由仿真时间控制及仿真。
我们可以将模拟时间视为允许机器人以采样速度移动的时间。

通过实验，我们观察到仿真时间越长，计算负荷越大。此外，当仿真时间变长后，本地路径规划器产生路径的时间也会变长，这是合理的。这里有一些关于如何调整仿真时间参数的建议。

如何设置仿真时间：如果将仿真时间设置为非常低的值（≤2.0）将导致性能有限，特别是当机器人需要通过狭窄的门口或家具之间的间隙时，因为没有足够的时间来获得最佳轨迹来通过狭窄的通道。另一方面，由于使用了 DWA 本地规划器，所有的轨迹都是简单的圆弧，如果将仿真时间设置的非常高（≥5.0），将导致长曲线不是非常灵活。这个问题并不是不可避免的，因为规划器在每个时间间隔后都会积极地重新规划，可以进行小的调整。对于高性能的计算机，4.0 秒的值也是足够的。

局部路径模拟的时间不用太长也不用太短，一般一秒左右就差不多了，如果太多会容易导致偏离全局的路径，特别启动的时候会转较大的半径，如果想启动的时候基本原地旋转摆正机器人的方向和全局路径的方向一致，那么就把模拟的时间设置短点。如果太小的仿真时间也不好，容易导致频繁的路径规划消耗资源，甚至也会出现震荡的可能。

~/sim_time (double, default: 1.7)

The amount of time to forward-simulate trajectories in seconds

轨迹的规划

主要是一些设置生成轨迹的参数。其中以下的两个参数比较重要。

~/path_distance_bias (double, default: 32.0)

The weighting for how much the controller should stay close to the path it was given
刻画局部路径和全局路径的贴合程度，该权重参数越大说明越贴合，越小说明不用那么贴合。

~/goal_distance_bias (double, default: 24.0)

The weighting for how much the controller should attempt to reach its local goal, also controls speed
达到局部目标点的权重参数，也用来控制速度。权重如果设置为0表示要求完全到达目标点，这会导致机器人走动缓慢和震荡的现象，因为要求达到目标点的精度太高，所对机器人的控制比较苛刻

速度采样
在其它几个参数中，vx_sample，vy_sample 确定在 x，y 方向上取多少平移速度样本,vth_sample 控制旋转速度样本的数量。样本的数量取决于你的计算能力。在大多数情况下，我们倾向于设置 vth_sample 高于平移速度样本，因为通常旋转比直线前进更复杂。如果将 y 向最大速度设置为零，则没必要在 y 方向提取速度样本，因为没有可用的样本。我们设置 vx_sample=20,vth_samples=40。

仿真粒度

sim_granularity 是在轨迹上的点之间采取的步长。它意味着要多频繁的检查轨迹上的点（检测它们是否与障碍物相交）。
较低的值意味着高频率，这需要更多的计算能力。对于 turtlebot 机器人来说，0.025 的默认值是足够的。

轨迹得分
如上所述，DWA 本地规划器最大化目标函数来获得最佳速度对。在其论文中，目标函数的值依赖于三个组成部分：到目标点的过程、清除障碍物和前进速度。在 ROS 中，目标函数的计算公式如下：

cost = path_distance_bias*(distance(m) to path from the endpoint of the trajectory)+ goal_distance_bias (distance(m) to local goal from the endpoint of the trajectory)+ occdist_scale*(maximu m obstacle cost along the trajectory in obstacle cost (0-254))

目标是获得最小的代价。path_distance_bias 是本地规划器与全局路径保持一致的权重。较大的值将使本地规划器更倾向于跟踪全局路径。goal_distance_bias 是机器人尝试到达目标点的权重。实验显示增加 goal_distance_bias 值将会使机器人与全局路径的一致性偏低。occdist_scale 是机器人尝试躲避障碍物的权重，这个值偏大将导致机器人陷入困境。

path_distance_bias 为 32，goal_distance_bias 为 20，occdist_sacle 为 0.02 仿真结果良好。

其他参数

目标距离公差（Goal distance tolerance)：

yaw_goal_tolerance（double,默认值：0.05），实现目标时，偏航/旋转中控制器弧度公差
xy_goal_tolerance（double,，默认值：0.10），实现目标时，在 x y 方向的距离公差
latch_xy_goal_tolerance（bool，默认：false）如果目标公差被锁定，即使在目标公差之前结束，如果机器人到达目标 xy 位置，它会简单旋转到位

振荡复位（Oscilation reset）：在通过门口的情况下，机器人可能会来回振荡，是因为本地规划器正在产生通过两个相反方向的路径。如果机器人保持振荡，导航堆栈将让机器人尝试恢复行为。

oscillation_reset_dist（double，默认值：0.05）在振荡标志复位之前，机器人以米为单位行走多远

# units are in meters/sec

DWAPlannerROS:

  xy_goal_tolerance: 0.3
  yaw_goal_tolerance: 0.1

  sim_time: 2.5 # The amount of time to forward-simulate trajectories in seconds
  sim_granularity: 0.1 # The step size, in meters, to take between points on a given trajectory 
  angular_sim_granularity: 0.1
  vx_samples: 8.0
  vy_samples: 0.0
  vth_samples: 20.0

  min_vel_trans: 0.1
  max_vel_trans: 0.4
  acc_lim_theta: 18.0
  acc_lim_trans: 9.0

  forward_point_distance: 0.0
  acc_lim_x: 9.0
  acc_lim_y: 0.0
  max_vel_y: 0.0
  min_vel_y: 0.0
  max_vel_x: 0.4 # m/sec (agv_default = 0.4)
  min_vel_x: 0.1 # m/sec
  max_vel_theta: 3.0
  min_vel_theta: 1.0
  latch_xy_goal_tolerance: true
  path_distance_bias: 0.8
  goal_distance_bias: 0.6
  # occdist_scale: 0.02
  oscillation_reset_dist: 0.05 # How far the robot must travel in meters before oscillation flags are reset.
  prune_plan: true
  scaling_speed: 0.8

参考文章：

• ROS机器人底盘(16)-move_base(4)-planner - 简书 https://www.jianshu.com/p/6d15ab640f34
• ROS与navigation教程-dwa_local_planner（DWA） - 创客智造 https://www.ncnynl.com/archives/201708/1906.html
• ROS 机器人导航调参手册 - 知乎 https://zhuanlan.zhihu.com/p/143202720
• ROS与navigation教程——基本导航调整指南_沧海一升的博客-CSDN博客 https://blog.csdn.net/qq_21842097/article/details/88604781
• GitHub - zkytony/ROSNavigationGuide: A guide for ROS navigation tuning | demo: https://youtu.be/1-7GNtR6gVk https://github.com/zkytony/ROSNavigationGuide