TB2解决定位导航撞人问题(修改三个yaml文件)

问题描述

here
按照这里的教程进行编写luanch文件，luanchAMCL和movebase的node之后发现竟然局部规划貌似不起作用，设置地点导航之后直接按照global planner的路线一直往前走，没有进行局部的避障。
由于现在movebase出现貌似localplanner没有进行规划，但是经过测试发现local的costmap数据并不是全0的情况(有可能是因为碰撞发生recovery 行为的时候才不为0，其他仍然0)

分析

我猜测有两种原因:

1. dwa_local_planner_params.yaml和local_costmap_params.yaml文件的参数问题(机器人高度，膨胀地图的大小，膨胀数据来源这些数据需要调整，因为默认是用kinect3的点云做的----凯大)
2. 可能是因为我没有添加一个发布odom和map之间tf关系的node(在此篇文章中，貌似launch文件有加一个发布odom和map之间tf关系的node)

<node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

可以在运行的时候tf_monitor看一下有没有发布，或者rqt_tf_tree

PS:关于导航的参数可以去找一本书
翻译文档项目github主页

---

ROS与navigation教程-基本导航调试指南

原数据

里程计线速度测试

里程计角速度测试

amcl测试

测试OK

---

获取最大速度和最大加速度

速度和加速度的最大/最小值是移动基座的两个基本参数。正确设置这两个数值对优化局部规划器
的移动行为帮助非常大。在 ROS 的导航中,我们需要知道平移和旋转的速度及加速度。
turtlebot2的IMU只有陀螺仪，没有加速度计。
平移速度：最高速度为70厘米/秒
最大旋转速度：180度/秒 （14）陀螺仪：工厂校准，1轴（110度/秒）

---

修改yaml文件

在 ROS 中,代价地图由静态地图层static map layer、障碍物图层obstacle map layer和膨胀层inflation layer组成。静态地图层直接给导航堆栈提供静态 SLAM 地图解释。障碍物图层包含 2D 障碍物和 3D 障碍物(体素层voxel_grid)。膨胀层是将障碍物膨胀来计算每个 2D 代价地图单元的代价。

对比一下，先看一下turbot是怎么用的
首先turbot是roslaunch turbot_slam laser_amcl_rplidar.launch map_file:=/home/ubuntu/map/zhizaokongjian.yaml

laser_amcl_rplidar.launch

<launch>
  <!-- Define laser type-->
  <arg name="laser_sensor" default="$(env TURTLEBOT_LASER_SENSOR)"/>  <!-- eai, rplidar -->
  <!-- laser driver -->
  <include file="$(find turtlebot_navigation)/laser/driver/$(arg laser_sensor)_laser.launch" />

  <!-- Map server -->
  <arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- AMCL -->
  <arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg laser_sensor)_amcl.launch.xml"/>
  <arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
  <arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
  <arg name="initial_pose_a" default="0.0"/>
  <include file="$(arg custom_amcl_launch_file)">
    <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 -->
  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg laser_sensor)_costmap_params.yaml"/>
  <include file="$(find turtlebot_navigation)/launch/includes/move_base/laser_move_base.launch.xml">
    <arg name="custom_param_file" value="$(arg custom_param_file)"/>
  </include>

</launch>

rplidar_amcl.launch.xml

<launch>
  <arg name="use_map_topic"   default="false"/>
  <arg name="scan_topic"      default="scan"/> 
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="map"/>

  <node pkg="amcl" type="amcl" name="amcl">
    <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="$(arg odom_frame_id)"/> 
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <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"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>
</launch>

rplidar_costmap_params.yaml

#A dummy file loaded when no custom param file is given 

laser_move_base.launch.xml

<!-- 
    ROS navigation stack with velocity smoother and safety (reactive) controller
-->
<launch>
  <include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/>
  <include file="$(find turtlebot_navigation)/launch/includes/safety_controller.launch.xml"/>
  
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="map"/>
  <arg name="odom_topic" default="odom" />
  <arg name="laser_topic" default="scan" />
  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />
    <!-- external params file that could be loaded into the move_base namespace -->
    <rosparam file="$(arg custom_param_file)" command="load" />
    
    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
  </node>
</launch>

看了一下其他的yaml都差不多，就是costmap那边因为原来是用kinetic所以现在需要改成rplidar的yaml文件。

costmap_common_params.yaml

#传感器读数的最大有效高度，单位为 meters。通常设置为略高于机器人的高度。
max_obstacle_height: 2.0  # assume something like an arm is mounted on top of the robot

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
robot_radius: 0.18  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
# footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  # if the robot is not circular

#map_type: voxel

obstacle_layer:
  enabled:              true
  max_obstacle_height:  2.0
  min_obstacle_height:  0.0
  #origin_z:             0.0 #VoxelCostmapPlugin ;rplidar is not VoxelCostmap
  #z_resolution:         0.2
  #z_voxels:             2
  #unknown_threshold:    15
  #mark_threshold:       0
  combination_method:   1
  track_unknown_space:  true    #true needed for disabling global path planning through unknown space
  obstacle_range: 2.0 #指定可将多大范围空间内障碍物插入到代价地图中这个参数指定范围上限边界
  raytrace_range: 5.0 #指定raytrace可以清除障碍物的最大范围
  #origin_z: 0.0
  #z_resolution: 0.2
  #z_voxels: 2
  publish_voxel_map: false
  observation_sources:  scan #观察源列表,以空格分割表示
  scan:
    data_type: LaserScan
    topic: "/scan"
    marking: true
    clearing: true
    expected_update_rate: 0


#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.25  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  map_topic:            "/map"

global_costmap_params.yaml

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 1.0
   publish_frequency: 0.5
   static_map: true
   transform_tolerance: 0.5
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

local_costmap_params.yaml

local_costmap:
   global_frame: odom
   robot_base_frame: /base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
   width: 4.0
   height: 4.0
   resolution: 0.05
   transform_tolerance: 0.5
   plugins:
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

然后真就costmap能够接受laser产生正确的local costmap啦，只不过还是容易撞到，可能膨胀系数还需要修改，不过我们先来看看为什么之前没有正确的costmap产生呢？
不同点:

costmap_common_params.yaml

#传感器读数的最大有效高度，单位为 meters。通常设置为略高于机器人的高度。
max_obstacle_height: 0.60 ->2.0
robot_radius: 0.20->0.18  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
#voxel是3d的栅格地图，所以在这里不能用
map_type: voxel ->#
obstacle_layer:
  max_obstacle_height:  0.6->2.0
  origin_z:             0.0->#
  z_resolution:         0.2->#
  z_voxels:             2->#
  unknown_threshold:    15->#
  mark_threshold:       0->#
  obstacle_range: 2.5->2.0
  raytrace_range: 3.0->5.0
  origin_z: 0.0->#
  z_resolution: 0.2->#
  z_voxels: 2->#
  observation_sources:  scan bump->observation_sources:  scan
  scan:
    data_type: LaserScan
    topic: "/scan"
    marking: true
    clearing: true
    #多了一个expected_update_rate
    expected_update_rate: 0 
  #bump数据源没了
  bump:->#
inflation_layer:
  cost_scaling_factor:  5.0 -> 10.0# exponential rate at which theobstacle cost drops off (default: 10)
  inflation_radius:     0.5 ->0.25 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
  #多了map topic
  map_topic:            "/map"



-----------------------------------------
global_costmap_params.yaml
没有变化
-----------------------------------------
local_costmap_params.yaml
   width: 6.0->4.0
   height: 6.0->4.0
plugins:
    - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
变成
   plugins:
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

膨胀参数

inflation_radius 和 cost_scaling_factor 是决定膨胀的主要参数。 inflation_radius 控制零代价点距离障碍物有多远。 cost_scaling_factor 与单元 (cell) 的代价值成反比,设置高值将使衰减曲线更为陡峭。
Pronobis 博士认为,最佳代价图的衰减曲线应为斜率相对较低的曲线,这样可使得最佳路径尽可能远离每侧的障碍物。优点是机器人更趋向于在障碍物中间移动。如图 8 和图 9 所示,在具有相同的起点和目标点情况下,当代价图的曲线非常陡峭时,机器人往往会靠近障碍物。在图 14 中,膨胀半径 inflation_radius = 0.55,代价比例因子cost_scaling_factor = 5.0;在图 15 中,膨胀半
径 inflation_radius = 1.75,代价比例因子 cost_scaling_factor = 2.58。
根据衰变曲线图,我们需设定这两个参数值,以使得膨胀半径几乎覆盖走廊,并且代价值的衰减速度相对中等,这意味着要降低代价的比例因子cost_scaling_factor 的值。

max_obstacle_height

以米为单位插入代价图中的障碍物的最大高度。该参数应设置为略高于机器人的最大高度。对于体素层,这本质含义是指体素网格的高度。

obstacle_range

插入代价地图的障碍物应距离机器人的最大默认距离,以米为单位。它可以在每个传感器的基础上进行覆盖操作。

raytrace_range

使用传感器数据从地图中扫描出障碍物的最大距离范围(以米为单位) 。同样,该值可以在每个传感器的基础上进行覆盖。

voxel_grid

体素网格 voxel_grid 是一个 ROS 包,它提供了一个高效的三维体素网格数据结构的实现,该存
储了体素的三种状态:标记状态 (marked)、自由状态 (free)、未知状态 (unknown)。 voxel_grid 占据了代价地图区域内的体积。在每次更新体素边界期间,体素层根据传感器的数据来标记或移除体素网格中的一些体素。

下面的这些参数仅适用于体素层(VoxelCostmapPlugin)
• origin_z : 地图的 Z 轴原点,以米为单位
• z_resolution : 地图的 Z 轴分辨率,以米/单元格 (meters/cell) 为单位。
• z_voxels : 每个垂直列中的体素数目,网格的高度为 z_resolution * z_voxels 。
• unknown_threshold : 当整列的 voxel 是“已知”(“known”) 的时候,含有的未知 cell(“unknown”
) 的最大数量。
• mark_threshold : 整列 voxel 认为是“自由”(“free”) 的时候,含有的已标记的 cell(“marked”)
的最大数目。
实验观察情况:
实验进一步阐明了体素层参数的影响。我们使用 ASUS Xtion Pro 作为深度传感器。我们发现 Xtion 的位置很重要,它决定了“盲区”的范围,即深度传感器看不到任何东西的区域。

看来在common_costmap_params.yaml里面去掉和voxel相关的参数以及在local_costmap_params.yaml中在plugins一项中可以设置Layer的种类，可以多层叠加。需要设置成我们2d的 - {name: obstacle_layer, type: “costmap_2d::ObstacleLayer”}

最后，让我修改这四个参数来调整navigation效果。

cost_scaling_factor: 5.0 -> 10.0
inflation_radius: 0.5 ->0.25
obstacle_range: 2.5->2.0
raytrace_range: 3.0->5.0

cost_scaling_factor: 5.0
inflation_radius: 0.6
obstacle_range: 2.5
raytrace_range: 5.0

一些参考

movebase参数调整

中文csdn解释参数 1
中文csdn解释参数 3
中文csdn解释参数 2

dwa_local_planner_params.yaml解读
base local planner ROSwiki

AMCL参数调整

As currently implemented, this node works only with laser scans and laser maps. It could be extended to work with other sensor data.
amcl takes in a laser-based map, laser scans, and transform messages, and outputs pose estimates.On startup, amcl initializes its particle filter according to the parameters provided. Note that, because of the defaults, if no parameters are set, the initial filter state will be a moderately sized particle cloud centered about (0,0,0).
中文csdn解释参数
amcl ROSwiki