4.Cartographer ros配置参数介绍

cartographer_ros   lua 文件参数配置说明

参数解析常用值
map_frame用于发布submaps的ROS坐标系ID,位姿的父坐标系,通常使用“map”“map”
tracking_frame由SLAM算法追踪的ROS坐标系ID,如果使用IMU,应该使用其坐标系,通常选择是 “imu_link”"base_footprint"
published_frame用于发布位姿子坐标系的ROS坐标系ID,例如“odom”坐标系,如果一个“odom”坐标系由系统的不同部分提供,在这种情况下,map_frame中的“odom”姿势将被发布。 否则,将其设置为“base_link”可能是合适的"odom"
odom_frame在provide_odom_frame为真才启用,坐标系在published_frame和map_frame之间用于发布局部SLAM结果,通常是“odom”"odom"
provide_odom_frame如果启用,局部,非闭环,持续位姿会作为odom_frame发布在map_frame中发布。true
use_odometry如果启用,订阅关于“odom”话题的nav_msgs/Odometry消息。里程信息会提供,这些信息包含在SLAM里false
num_laser_scans

订阅的激光扫描话题数量。

在一个激光扫描仪的“scan”话题上订阅sensor_msgs/LaserScan

或在多个激光扫描仪上订阅话题“scan_1”,“scan_2”等

1
num_multi_echo_laser_scans

订阅的多回波激光扫描主题的数量。

在一个激光扫描仪的“echoes”话题上订阅sensor_msgs/MultiEchoLaserScan

或者为多个激光扫描仪订阅话题“echoes_1”,“echoes_2”等。

0
num_subdivisions_per_laser_scan

将每个接收到的(多回波)激光扫描分成的点云数。

细分扫描可以在扫描仪移动时取消扫描获取的扫描。

有一个相应的轨迹构建器选项可将细分扫描累积到将用于扫描匹配的点云中。

1
num_point_clouds

要订阅的点云话题的数量。

在一个测距仪的“points2”话题上订阅sensor_msgs/PointCloud2

或者为多个测距仪订阅话题“points2_1”,“points2_2”等。

0

lookup_transform_timeout_sec

使用tf2查找变换的超时秒数0.2
submap_publish_period_sec发布submap的间隔(以秒为单位),例如, 0.3秒0.3
pose_publish_period_sec发布姿势的间隔(以秒为单位),例如 5e-3,频率为200 Hz。5e-3

trajectory_publish_period_sec

以秒为单位发布轨迹标记的时间间隔,例如, 30e-3持续30毫秒30e-3

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

TRAJECTORY_BUILDER_2D.use_imu_data = false

 

cartographer 底层算法配置

map_builder.lua 配置

include "pose_graph.lua"

MAP_BUILDER = {
  use_trajectory_builder_2d = false,
  use_trajectory_builder_3d = false,
  num_background_threads = 4,
  pose_graph = POSE_GRAPH,
}

pose_graph.lua 配置


POSE_GRAPH = {
  optimize_every_n_nodes = 90,
  constraint_builder = {
    sampling_ratio = 0.3,
    max_constraint_distance = 15.,
    min_score = 0.55,
    global_localization_min_score = 0.6,
    loop_closure_translation_weight = 1.1e4,
    loop_closure_rotation_weight = 1e5,
    log_matches = true,
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },
    ceres_scan_matcher = {
      occupied_space_weight = 20.,
      translation_weight = 10.,
      rotation_weight = 1.,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
    fast_correlative_scan_matcher_3d = {
      branch_and_bound_depth = 8,
      full_resolution_depth = 3,
      min_rotational_score = 0.77,
      min_low_resolution_score = 0.55,
      linear_xy_search_window = 5.,
      linear_z_search_window = 1.,
      angular_search_window = math.rad(15.),
    },
    ceres_scan_matcher_3d = {
      occupied_space_weight_0 = 5.,
      occupied_space_weight_1 = 30.,
      translation_weight = 10.,
      rotation_weight = 1.,
      only_optimize_yaw = false,
      ceres_solver_options = {
        use_nonmonotonic_steps = false,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
  },
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1e3,
    rotation_weight = 3e5,
    consecutive_node_translation_weight = 1e5,
    consecutive_node_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    log_solver_summary = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },
  max_num_final_iterations = 200,
  global_sampling_ratio = 0.003,
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10.,
}

trajectory_builder.lua 配置
 

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
  pure_localization = false,
}

trajectory_builder_2d.lua 配置


TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,
  min_range = 0.,
  max_range = 30.,
  min_z = -0.8,
  max_z = 2.,
  missing_data_ray_length = 5.,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.025,

  adaptive_voxel_filter = {
    max_length = 0.5,
    min_num_points = 200,
    max_range = 50.,
  },

  loop_closure_adaptive_voxel_filter = {
    max_length = 0.9,
    min_num_points = 100,
    max_range = 50.,
  },

  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.1,
    angular_search_window = math.rad(20.),
    translation_delta_cost_weight = 1e-1,
    rotation_delta_cost_weight = 1e-1,
  },

  ceres_scan_matcher = {
    occupied_space_weight = 1.,
    translation_weight = 10.,
    rotation_weight = 40.,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 1,
    },
  },

  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

  imu_gravity_time_constant = 10.,

  submaps = {
    resolution = 0.05,
    num_range_data = 90,
    range_data_inserter = {
      insert_free_space = true,
      hit_probability = 0.55,
      miss_probability = 0.49,
    },
  },
}

 

  • 4
    点赞
  • 27
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值