【movebase】机器人使用vlp-16多线雷达cartographer纯定位+movebase导航

环境：
ubuntu 20.04 + ros1 noetic
gazebo 11
机器人有urdf，tf树最上层为base_link，无里程计和imu
使用vlp-16点云数据生成激光里程计数据

没有用过imu，所以文章内容不太能给使用imu的人提供参考
文章内容如有错误，欢迎指出交流。

1、cartographer纯定位

launch文件相关修改

参考链接文章，修改occupancy_grid_node_main文件
launch文件参考

<launch>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename robot_simple_2d_localization.lua
          -load_state_filename /home/letitia/robot_simple_ws/src/robot_simple_navigation/maps/warehouse/warehouse.pbstream"
      output="screen">
    <remap from="/points2" to="/velodyne_points" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05
      							                    -pure_localization 1" />

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

</launch>

lua文件相关修改

复制backpack_2d_localization.lua，将复制的文件改为自己的名字，并打开修改里面参数，可参考链接文章

include "修改为建图时的lua文件名.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3, 
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

2、激光里程计

两个包都放到工作空间的src文件里面

pointcloud2转laserscan

无论是激光里程计还是move_base，直接使用点云数据的效果都不好，所以先将pointcloud2转laserscan，使用到pointcloud_to_laserscan工具，github地址（如果直接git clone，下载的是ros2版本的，ros1需要在分支里找一个合适的版本下载zip）
launch配置参考（中文记得删）

<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
  # remap到自己的话题
  <remap from="cloud_in" to="/velodyne_points"/>
  <remap from="scan" to="/scan"/>
  <rosparam>
      # target_frame: camera_link # Leave disabled to output scan in pointcloud frame
      transform_tolerance: 0.01
      # z轴读取范围，根据自己的情况改下面两个
      min_height: -0.03
      max_height: 1.5
      # 扫描角度，根据自己的情况改下面两个
      angle_min: -1.4
      angle_max: 1.4
      angle_increment: 0.0087 # M_PI/360.0
      scan_time: 0.1
      range_min: 0.5
      range_max: 30
      use_inf: true
      inf_epsilon: 1.0

      # Concurrency level, affects number of pointclouds queued for processing and number of threads used
      # 0 : Detect number of cores
      # 1 : Single threaded
      # 2->inf : Parallelism level
      concurrency_level: 1
  </rosparam>

</node>

里程计

因为laser_scan_matcher没有发布odom话题，这里用的是laser_scan_matcher_odometry，github链接。还有一个激光雷达工具是rf2o_laser_odometry，我感觉在无imu时，效果差不太多，不过这个工具运行时在终端里有很多输出，建议单独写在一个launch文件里并在另一个终端打开，这样不影响看move_base的输出。
launch配置参考

<node pkg="laser_scan_matcher_odometry" type="laser_scan_matcher_odometry_node"
    name="laser_scan_matcher_odometry_node" output="screen">

  <param name="fixed_frame" value="odom" />
  <param name="base_frame" value="base_link" />
  <param name="max_iterations" value="10" />
</node>

3、movebase

movebase主要是4个参数文件

costmap_common_params.yaml

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
# robot_radius: 0.12 #圆形
footprint: [[-0.389, -0.228], [-0.389, 0.228], [0.389, 0.228], [0.389, -0.228]] #其他形状

#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

obstacle_range: 1.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 2.0 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物

#导航包所需要的传感器
observation_sources: scan # points2试着配过，自己感觉不好用
scan:
  {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

global_costmap_params.yaml

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_link #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 8.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  #膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
  inflation_radius: 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: map #这里如果放odom会报错或警告的话，就改为map
  robot_base_frame: base_link #机器人坐标系

  update_frequency: 15.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  #膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
  inflation_radius: 0.2

  obstacle_range: 1.5 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
  raytrace_range: 2.0 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 1.5 # 局部地图宽度 单位是 m
  height: 1.5 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致（carto建图的那个系数

  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

局部路径规划teb_local_planner.yaml（我用的teb）

recovery_behavior_enabled: False
clearing_rotation_allowed: False

TebLocalPlannerROS:
 
 odom_topic: odom
 map_frame: /map
 
 #Trajectory
 teb_autosize: True 
 dt_ref: 0.3 
 dt_hysteresis: 0.03
 global_plan_overwrite_orientation: True 
 allow_init_with_backwards_motion: False 
 max_global_plan_lookahead_dist: 3.0 
 feasibility_check_no_poses: 5 
 
 # Robot
 max_vel_x: 10.0
 max_vel_y: 0.0 
 max_vel_x_backwards: 1.0 
 max_vel_theta: 10.0 
 acc_lim_x: 0.15 
 acc_lim_y: 0  
 acc_lim_theta: 2.5 
 
 min_turning_radius: 0.5 #差动小车不想总是原地转，可以把这里不设为0
 
 cmd_angle_instead_rotvel: False
 
 footprint_model:
  type: "polygon"
  vertices: [[-0.389, -0.228], [-0.389, 0.228], [0.389, 0.228], [0.389, -0.228]]
 
 
 xy_goal_tolerance: 0.2  
 yaw_goal_tolerance: 0.2
 free_goal_vel: False 
 complete_global_plan: True
 
 # Obstacles
 min_obstacle_dist: 0.1 
 include_costmap_obstacles: True 
 costmap_obstacles_behind_robot_dist: 1.5  # default:1.5  0.0~20.0
 obstacle_poses_affected: 15   
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 include_dynamic_obstacles: True 
 dynamic_obstacle_inflation_dist: 0.6 
 
 # Optimization  
 no_inner_iterations: 5 
 no_outer_iterations: 4  
 optimization_activate: True 
 optimization_verbose: False 
 penalty_epsilon: 0.05
 obstacle_cost_exponent: 4
 weight_max_vel_x: 1  
 weight_max_vel_theta: 1 
 weight_acc_lim_x: 2  
 weight_acc_lim_theta: 1  
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1 
 weight_optimaltime: 1 # must be > 0 
 weight_shortest_path: 0 
 weight_obstacle: 100
 weight_inflation: 0.2   
 weight_dynamic_obstacle: 10
 weight_dynamic_obstacle_inflation: 0.2  
 weight_viapoint: 1  
 weight_adapt_factor: 2 
 
 # Homotopy Class Planner
 enable_homotopy_class_planning: True 
 enable_multithreading: False 
 max_number_classes: 5 
 selection_cost_hysteresis: 1.0 
 selection_prefer_initial_plan: 0.95
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False
 
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False
 
 # Recovery
 
 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: False 
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

4、launch文件

<node name="map_server" pkg="map_server" type="map_server" args="$(find robot_simple_navigation)/maps/warehouse/warehouse.yaml"/>

<include file="$(find pointcloud_to_laserscan)/launch/point_to_scan.launch"/>

<node pkg="laser_scan_matcher_odometry" type="laser_scan_matcher_odometry_node"
  name="laser_scan_matcher_odometry_node" output="screen">

  <param name="fixed_frame" value="odom" />
  <param name="base_frame" value="base_link" />
  <param name="max_iterations" value="10" />
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="10.0" />

    <rosparam file="$(find robot_simple_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot_simple_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robot_simple_navigation)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_simple_navigation)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_simple_navigation)/config/teb_local_planner.yaml" command="load" />
</node>

总结

尝试了几种激光里程计，但都会出现偏移甚至漂移出地图的情况，虽然多设置几次目标地点让机器人走一走可以后期修正回来，但未修正期间是有可能撞到障碍物的，所以在无里程计和imu的时候，如果是cartographer建的图，建议纯定位时还是直接用cartographer提供的odom（cartographer查阅TF-tree上的base_scan相对于map的关系，根据激光点云定位base_scan，并根据base_link与base_scan的关系追踪base_link的位姿，最终推算odom相对于map的位姿关系并发布tf，发布的不是话题哦），基本上没有漂移的问题。

参考

cartographer pure_localization 纯定位修改 +初始化位置
【移动机器人技术】Cartographer使用流程-建图-纯定位-导航