【cartographer】机器人使用vlp-16多线雷达建图参数配置

环境：
ubuntu 20.04 + ros1 noetic
gazebo 11
机器人有urdf，tf树最上层为base_link，无里程计和imu
直接使用vlp-16点云数据建cartographer 2d图

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

1、lua文件配置

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.min_range = 0.5
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.min_z = -0.03
TRAJECTORY_BUILDER_2D.max_z = 1.5
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.5)

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35.

POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimize_every_n_nodes = 70
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

option部分参数修改

一般主要改这几个参数，

  tracking_frame = "base_link",
  published_frame = "base_link",
  provide_odom_frame = true,
  use_odometry = false,

tracking_frame一般是base_footprint或者base_link，有时也可能是laser_link（比如手持雷达建图的情况）。
published_frame最好选机器人tf树最上层那个frame_id。
如果你的机器人有里程计，那一般tf树最上层就是odom，provide_odom_frame应改为false，use_odometry应改为true。
注意，因为vlp-16的gazebo插件发布的是点云数据，所以num_point_clouds改为你雷达的数量，别在num_laser_scans里改。
一般建图报错说找不到map都是lua文件参数配置的不对，可以去参考一下参数详细说明

TRAJECTORY_BUILDER_2D部分参数修改

TRAJECTORY_BUILDER_2D.min_range = 0.5
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.min_z = -0.03
TRAJECTORY_BUILDER_2D.max_z = 1.5

这四个参数规定了cartographer读取的点云数据范围，如果建图时无障碍物的地方也出现了很多的雷达波纹（地上一圈一圈的），就要调整min_z参数，因为可能扫到地面上，把地板检测为障碍物了。

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35.

如果建出来的图边缘重影较多，可以尝试调小这个参数，推荐20-40

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.5)

如果建图时移动机器人发现无障碍物的空地总有灰色区域，经过好几次都还是扫不上，那就可能是这个参数太大了，将math.rad括号里的数调小，但不建议低于0.1。

POSE_GRAPH部分参数修改

POSE_GRAPH.optimize_every_n_nodes = 70

推荐设为TRAJECTORY_BUILDER_2D.submaps.num_range_data的2倍，如果不要后端优化，设置为0。

2、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.lua"
      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" />
      
  <node name="rviz" pkg="rviz" type="rviz" required="true"  
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" /> 
</launch>

由于我启动机器人gazebo仿真时会发布robot_description和robot_state_publisher，所以这里不再重复发布，如果是实机要加上，或者声明tf变换。

需要修改的参数

-configuration_basename后面要改为你自己的lua文件名字
<remap from="points2" to="/velodyne_points" /> to里面的话题去找下面这个插件里的topicName，保持一致。

<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
  <topicName>/velodyne_points</topicName>
  <frameName>laser_link</frameName>
  <organize_cloud>false</organize_cloud>
  <min_range>0.1</min_range>
  <max_range>30.0</max_range>
  <gaussianNoise>0.008</gaussianNoise>
</plugin>

---

更新：
我在最近的一次尝试里发现下图的草坪不能很好被判定为障碍物建图效果如下：
能看出来这个线很虚，如果小车走近些，会变得更深一些，线也会多一些，但是我走远以后，这个草坪如果又进入了雷达的扫描范围，这根线就消失了。但是如果我把点云数据转成laserscan数据，就不会有这种现象。
以我目前的理解，是因为这个草坪模型垂直面太少了，而cartographer的2d建图不太能处理斜坡（或者说类斜坡？）的数据，可能会把它当成不太平整的地面处理。我的解决办法是把点云数据转成laserscan数据或者直接改用3d建图。
如果对这个问题有什么想法和建议欢迎和我交流！