基于gmapping地图与cartographer地图测试蒙特卡罗定位

最近这一段时间一直在调节基于蒙特卡罗定位的导航，前提是基于已经开源的基于激光雷达的SLAM算法。地图的建立，主要是参考网上的开源教程，根据实际情况，调整相关的参数文件。最终是实现了建图。建立的地图的精度还没有具体的分析，但从直观上看，应该是没什么问题。因此计划通过AMCL来测试建立地图的精度。要运行AMCL首要的工作是运行move_base导航包；本文以rplidar_A2雷达为例，介绍AMCL的定位性能。

调节move_base节点,相关的参数文件:

(1) 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>

(2) rplidar_costmap_params.yaml

(3) 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>

 

(4) costmap_common_params.yaml

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

  #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

  #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"

 

(5) 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"}

(6)  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"}

 

(7) 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?

 

 

(8) move_base_params.yaml

#  http://www.ros.org/wiki/move_base

shutdown_costmaps: false

 

controller_frequency: 5.0

controller_patience: 3.0

 

 

planner_frequency: 1.0

planner_patience: 5.0

 

oscillation_timeout: 10.0

oscillation_distance: 0.2

 

# local planner - default is trajectory rollout

base_local_planner: "dwa_local_planner/DWAPlannerROS"

 

base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

 

 

#We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted.

## recovery behaviors; we avoid spinning, but we need a fall-back replanning

#recovery_behavior_enabled: true

 

#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      

 

(9) global_planner_params.yaml

GlobalPlanner:                                  # Also see: http://wiki.ros.org/global_planner

  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false

  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true

  use_dijkstra: true                            # Use dijkstra's algorithm. Otherwise, A*, default true

  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false

 

  allow_unknown: true                           # Allow planner to plan through unknown space, default true

                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work

  planner_window_x: 0.0                         # default 0.0

  planner_window_y: 0.0                         # default 0.0

  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0

 

  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100

  planner_costmap_publish_frequency: 0.0        # default 0.0

 

  lethal_cost: 253                              # default 253

  neutral_cost: 50                              # default 50

  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0

  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true

                                                                                                                                               

(10)     navfn_global_planner_params.yaml            

NavfnROS:

  visualize_potential: false    #Publish potential for rviz as pointcloud2, not really helpful, default false

  allow_unknown: false          #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true

                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work

  planner_window_x: 0.0         #Specifies the x size of an optional window to restrict the planner to, default 0.0

  planner_window_y: 0.0         #Specifies the y size of an optional window to restrict the planner to, default 0.0

 

  default_tolerance: 0.0        #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0

                                #The area is always searched, so could be slow for big values

                         

(11) smoother.yaml

# Default parameters used by the yocs_velocity_smoother module.

# This isn't used by minimal.launch per se, rather by everything

# which runs on top.

 

# Mandatory parameters

speed_lim_v: 0.8

speed_lim_w: 5.4

 

accel_lim_v: 1.0 # maximum is actually 2.0, but we push it down to be smooth

accel_lim_w: 2.0

 

# Optional parameters

frequency: 20.0

decel_factor: 1.5

 

# Robot velocity feedback type:

#  0 - none (default)

#  1 - odometry

#  2 - end robot commands

robot_feedback: 2

这里面包含了运行amcl所需设定的所以参数，具体参数怎么调节，需要根据实际运行环境以及激光雷达的参数来设定。

本人经过测试在安装激光雷达的时候一定要保证激光雷达的X轴正方向与kobuki行走的正方向保持一致，这是保持运行AMCL时不会撞墙的根本保证。如果将激光雷达安装的不与机器人行走的方向一致，将导致导航定位不准确。