ROS1云课→28机器人代价地图配置

Posted 2022-09-20 zhangrelay

ROS1云课→27机器人SLAM小结

---

在前面做的所有工作都成了现在项目的铺垫，而最大的乐趣也即将开始，这是赋予机器人生命的时刻。

 

后续学习以下内容：

• 应用程序包开发。
• 理解导航功能包集及其工作方式。
• 配置所有必要文件。
• 创建启动文件并开始导航。

---

机器人配置启动文件：

<!--
  Turtlebot navigation simulation:
  - stdr
  - move_base
  - amcl
  - map_server
  - rviz view
 -->
<launch>
  <arg name="base"       default="$(optenv TURTLEBOT_BASE kobuki)"/>  <!-- create, rhoomba -->
  <arg name="stacks"     default="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons -->
  <arg name="3d_sensor"  default="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro -->
  <arg name="laser_topic" default="robot0/laser_0"/> <!-- default laser topic in stdr for 1 robot -->
  <arg name="odom_topic" default="robot0/odom"/>
  <arg name="odom_frame_id" default="map"/>
  <arg name="base_frame_id" default="robot0"/>
  <arg name="global_frame_id" default="world"/>
  <!-- Name of the map to use (without path nor extension) and initial position -->
  <arg name="map_file"       default="$(env TURTLEBOT_STDR_MAP_FILE)"/>
  <arg name="initial_pose_x" default="2.0"/>
  <arg name="initial_pose_y" default="2.0"/>
  <arg name="initial_pose_a" default="0.0"/>
  <arg name="min_obstacle_height" default="0.0"/>
  <arg name="max_obstacle_height" default="5.0"/>

  <!--  ******************** Stdr********************  -->
  <include file="$(find stdr_robot)/launch/robot_manager.launch" />
  <!-- Run STDR server with a prefedined map-->
  <node pkg="stdr_server" type="stdr_server_node" name="stdr_server" output="screen" args="$(arg map_file)"/>
  <!--Spawn new robot at init position 2 2 0-->
  <node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn)" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml $(arg initial_pose_x) $(arg initial_pose_y) 0"/>
  <!-- Run Gui  -->
  <include file="$(find stdr_gui)/launch/stdr_gui.launch"/>
  <!-- Run the relay to remap topics -->
  <include file="$(find turtlebot_stdr)/launch/includes/relays.launch.xml"/>

  <!--  ***************** Robot Model *****************  -->
  <include file="$(find turtlebot_bringup)/launch/includes/robot.launch.xml">
    <arg name="base" value="$(arg base)" />
    <arg name="stacks" value="$(arg stacks)" />
    <arg name="3d_sensor" value="$(arg 3d_sensor)" />
  </include>
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="true"/>
  </node>

  <!-- Command Velocity multiplexer -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml"/>
    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
  </node>

  <!-- ****** Maps ***** -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
   <param name="frame_id" value="$(arg global_frame_id)"/>
  </node>


  <!--  ************** Navigation  ***************  -->
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
   <arg name="odom_topic" value="$(arg odom_topic)"/>
   <arg name="laser_topic" value="$(arg laser_topic)"/>
   <arg name="odom_frame_id"   value="$(arg odom_frame_id)"/>
   <arg name="base_frame_id"   value="$(arg base_frame_id)"/>
   <arg name="global_frame_id" value="$(arg global_frame_id)"/>
  </include>

  <!-- ***************** Manually setting some parameters ************************* -->
    <param name="move_base/local_costmap/obstacle_layer/scan/min_obstacle_height" value="$(arg min_obstacle_height)"/>
    <param name="move_base/local_costmap/obstacle_layer/scan/max_obstacle_height" value="$(arg max_obstacle_height)"/>
    <param name="move_base/global_costmap/obstacle_layer/scan/min_obstacle_height" value="$(arg min_obstacle_height)"/>
    <param name="move_base/global_costmap/obstacle_layer/scan/max_obstacle_height" value="$(arg max_obstacle_height)"/>

  <!--  ************** AMCL ************** -->
  <include file="$(find turtlebot_navigation)/launch/includes/amcl/amcl.launch.xml">
    <arg name="scan_topic" value="$(arg laser_topic)"/>
    <arg name="use_map_topic" value="true"/>
    <arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
    <arg name="base_frame_id" value="$(arg base_frame_id)"/>
    <arg name="global_frame_id" value="$(arg global_frame_id)"/>
    <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>

 <!-- ********** Small tf tree connector between robot0 and base_footprint********* -->
  <node name="tf_connector" pkg="turtlebot_stdr" type="tf_connector.py" output="screen"/>

  <!--  **************** Visualisation ****************  -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_stdr)/rviz/robot_navigation.rviz"/>




</launch>

重点关注move_base：

<!-- 
    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>

配置全局和局部代价地图

好的，现在将要开始配置导航功能包集和所有启动时必需的文件。为了开始进行配置，首先将会学习什么是代价地图（costmaps）和代价地图的作用。机器人将使用两种导航算法在地图中移动——全局导航（global）和局部导航（local）。

• 全局导航用于建立到地图上最终目标或一个远距离目标的路径。
• 局部导航用于建立到近距离目标和为了临时躲避障碍物的路径。例如，机器人四周一个4×4m²的方形窗户。

这些导航算法通过使用代价地图来处理地图中的各种信息。全局代价地图用于全局导航，局部代价地图用于局部导航。

代价地图的参数用于配置算法计算行为。它们也有最基本的通用参数，这些参数会保存在共享的文件中。

可以在三个最基本的配置文件中设定不同的参数。这三个文件如下：

• costmap_common_params.yaml
• global_costmap_params.yaml
• local_costmap_params.yaml

只需要看一下这三个文件的名称，就能大致猜出来它们的用途。现在可能刚刚对代价地图的作用有一个初步的认识，来创建这些配置文件并解释这些配置参数的作用。

基本参数的配置

从最基本的参数讲起。以costmap_common_params.yaml为名称创建一个新文件，并添加以下代码。

下面的脚本在costmap_common_params.yaml呈现：

max_obstacle_height: 0.60  # 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.20  # 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:  0.6
  origin_z:             0.0
  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.5
  raytrace_range: 3.0
  origin_z: 0.0
  z_resolution: 0.2
  z_voxels: 2
  publish_voxel_map: false
  observation_sources:  scan bump
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  # for debugging only, let's you see the entire voxel grid

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

static_layer:
  enabled:              true
  

这个文件主要用于配置基本参数。这些参数会被用于local_costmap和global_costmap。对代码进行详细的解释。

obstacle_range和raytrace_range参数用于表示传感器的最大探测距离并在代价地图中引入探测的障碍信息。前者主要用于障碍的探测。在我们的示例中，如果机器人检测到一个距离小于2.5m的障碍物，就会将这个障碍物引入到代价地图中。后者则用于在机器人运动过程中，实时清除代价地图中的障碍物，并更新可移动的自由空间数据。请注意，我们只能检测到障碍物对雷达激光信号投影或者声呐的回波，我们无法感知完整的物体形状和大小。但在实际应用中，这样的测量结果就足够我们完成地图的绘制和机器人自身的定位了。

footprint参数用于将机器人的几何参数告知导航功能包集。这样就能在机器人和障碍物之间保持一个合理的距离，或者说提前获知机器人能否穿越某个门等。

inflation_radius参数则给定了机器人与障碍物之间必须要保持的最小距离。

cost_scaling_factor参数修改机器人绕过障碍物的行为，可以通过修改参数设计一个激进或保守的行为。

还需要配置：

observation_sources参数来设定导航功能包集所使用的传感器，以获取实际环境的数据并计算路径。

在示例中，会在stdr中使用一个模拟的LIDAR。

通过下面的代码，会对传感器的坐标系和数据进行配置：

  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  # for debugging only, let's you see the entire voxel grid

上面配置的这些参数也会被用于在代价地图中添加和清除障碍。例如，可以添加一个探测范围较大的传感器用于在代价地图中寻找障碍物，再添加一个传感器用于导航和清除障碍物。在上面还会配置主题的名称，这是不能被遗漏的，如果不进行配置，那么导航功能包集会使用默认的主题以保证程序能够正常运行，那么一旦机器人移动起来，很可能就会撞到墙上或者障碍物上。

全局代价地图的配置

下一个用于配置的文件主要是全局代价地图global costmap。在以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"

其中，global_frame和robot_base_frame参数用于定义地图和机器人之间的坐标变换。建立全局代价地图必须要使用这个变换。

还能配置代价地图更新的频率。在示例中，这个频率为1Hz。参数static_map用于配置是否使用一个地图或者地图服务器来初始化代价地图。如果不准备使用静态的地图，那么这个参数应该为false。

局部代价地图的配置

下面的这个文件用于配置局部代价地图。在以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::VoxelLayer"
    - name: inflation_layer,     type: "costmap_2d::InflationLayer"

其中，global_frame、robot_base_frame、update_frequency和static_map参数和上一节描述的内容一致，用来配置全局代价地图。参数publish_frequency定义了发布信息的频率。参数rolling_window用于保持在机器人运动过程中，代价地图始终以机器人为中心。

通过transform_tolerance参数配置转换的最大延迟，在本例中为0.5s。通过planner_frequency参数配置规划循环的频率（以Hz为单位），planner_ patiente参数配置在执行空间清理操作前，规划器寻找一条有效路径的等待时间（以s为单位）

能够通过width、height和resolution参数来配置代价地图的尺寸和分辨率。这些参数都是以m为单位的。

底盘局部规划器配置

一旦我们完成代价地图的配置，那么就能够开始进行一个底盘规划器的配置了。这个底盘规划器会产生一个速度命令来移动我们的机器人。在以dwa_local_planner_params.yaml为名的新文件，并添加以下代码：

DWAPlannerROS:

# Robot Configuration Parameters - Kobuki
  max_vel_x: 0.5  # 0.55
  min_vel_x: 0.0 

  max_vel_y: 0.0  # diff drive robot
  min_vel_y: 0.0  # diff drive robot

  max_trans_vel: 0.5 # choose slightly less than the base's capability
  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
  trans_stopped_vel: 0.1

  # Warning!
  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
  #   are non-negligible and small in place rotational velocities will be created.

  max_rot_vel: 5.0  # choose slightly less than the base's capability
  min_rot_vel: 0.4  # this is the min angular velocity when there is negligible translational velocity
  rot_stopped_vel: 0.4
  
  acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 
  acc_lim_theta: 2.0
  acc_lim_y: 0.0      # diff drive robot

# Goal Tolerance Parameters
  yaw_goal_tolerance: 0.3  # 0.05
  xy_goal_tolerance: 0.15  # 0.10
  # latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 1.0       # 1.7
  vx_samples: 6       # 3
  vy_samples: 1       # diff drive robot, there is only one sample
  vtheta_samples: 20  # 20

# Trajectory Scoring Parameters
  path_distance_bias: 64.0      # 32.0   - weighting for how much it should stick to the global path plan
  goal_distance_bias: 24.0      # 24.0   - wighting for how much it should attempt to reach its goal
  occdist_scale: 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
  forward_point_distance: 0.325 # 0.325  - how far along to place an additional scoring point
  stop_time_buffer: 0.2         # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
  scaling_speed: 0.25           # 0.25   - absolute velocity at which to start scaling the robot's footprint
  max_scaling_factor: 0.2       # 0.2    - how much to scale the robot's footprint when at speed.

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true
  global_frame_id: odom


# Differential-drive robot configuration - necessary?
#  holonomic_robot: false

这个配置文件设定了机器人的最大和最小速度限值，同时也设定了加速度的限值。

当使用的是一台完整约束的平台（holonomic platform）时，那么参数holonomic_robot就应设为true。在示例中使用的不是完整约束的运动载体，所以这个参数是false。全向移动机器人指的是它能够以从任意位置向已配置空间移动。换句话说，如果机器人可以到达的位置已经以x、y轴坐标值的形式定义，那么全向移动机器人能够从任意位置出发向该位置移动。举例来说，如果机器人能够前后左右移动，那么就是全向移动机器人。一种典型的非全向移动的例子就是汽车，因为它就不能直接沿左右方向移动到一个给定的位置。如果从某个位置（和位姿）开始运动的话，它并不能一次到达地图上的每一个地点，也不能保证到达时的位姿。类似地，差分移动机器人平台就是一个非全向移动机器人平台。

---