移动机器人激光SLAM导航（五）：Cartographer SLAM 篇

Robot_Yue2024-02-18 15:12

参考

1. Cartographer 安装

1.1 前置条件

1.2 依赖库安装

1.3 编译

本文只编译 cartographer_ros，以下为同时开三个终端操作

powershell 复制代码 
$ mkdir -p cartographer_ws/src
powershell 复制代码 
$ cd ~
$ git clone https://github.com/xiangli0608/cartographer_detailed_comments_ws
$ mv ~/cartographer_detailed_comments_ws/src/cartographer_ros ~/cartographer_ws/src
powershell 复制代码 
$ cd ~/cartographer_ws
$ catkin_make

2. Cartographer 运行

2.1 数据集下载

2.2 配置文件

  • lx_rs16_2d_outdoor.launch

    xml 复制代码 
    <launch>
  <!-- bag 的地址与名称（根据自己情况修改，建议使用绝对路径） -->
  <arg name="bag_filename" default="/home/yue/bag/rslidar-outdoor-gps.bag"/>

  <!-- 使用 bag 的时间戳 -->
  <param name="/use_sim_time" value="true" />

  <!-- 启动 cartographer -->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename lx_rs16_2d_outdoor.lua"
      output="screen">
    <remap from="points2" to="rslidar_points" />
    <remap from="scan" to="front_scan" />
    <remap from="odom" to="odom_scout" />
    <remap from="imu" to="imu" />
  </node>

  <!-- 生成 ros 格式的地图 -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

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

  <!-- 启动 rosbag -->
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />

</launch>

  • lx_rs16_2d_outdoor.lua

    lua 复制代码 
    include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER，               -- map_builder.lua的配置信息
  trajectory_builder = TRAJECTORY_BUILDER， -- trajectory_builder.lua的配置信息
  
  map_frame = "map"，                       -- 地图坐标系的名字
  tracking_frame = "imu_link"，             -- 将所有传感器数据转换到这个坐标系下
  published_frame = "footprint"，           -- tf: map -> footprint
  odom_frame = "odom"，                     -- 里程计的坐标系名字
  provide_odom_frame = false，              -- 是否提供odom的tf，如果为true,则tf树为map->odom->footprint
                                            -- 如果为false tf树为map->footprint
  publish_frame_projected_to_2d = false，   -- 是否将坐标系投影到平面上
  --use_pose_extrapolator = false，           -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿

  use_odometry = false，                    -- 是否使用里程计,如果使用要求一定要有odom的tf
  use_nav_sat = false，                     -- 是否使用gps
  use_landmarks = false，                   -- 是否使用landmark
  num_laser_scans = 0，                     -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0，          -- 是否使用multi_echo_laser_scans数据
  num_subdivisions_per_laser_scan = 1，     -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 1，                    -- 是否使用点云数据
  
  lookup_transform_timeout_sec = 0.2，      -- 查找tf时的超时时间
  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.use_imu_data = true
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2
--TRAJECTORY_BUILDER_2D.max_z = 1.4
--TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02

--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200.
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.

--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12

--TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
--TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004
--TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1.

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1

POSE_GRAPH.optimize_every_n_nodes = 160.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

return options

  • trajectory_builder_2d.lua

    lua 复制代码 
    TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,            -- 是否使用imu数据
  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,      -- 体素滤波的立方体的边长

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离原点超过max_range 的点被移除
  },

  -- 闭环检测的自适应体素滤波器, 用于生成稀疏点云 以进行 闭环检测
  loop_closure_adaptive_voxel_filter = {
    max_length = 0.9,
    min_num_points = 100,
    max_range = 50.,
  },

  -- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息
  -- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果
  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,   -- 用于计算各部分score的权重
    rotation_delta_cost_weight = 1e-1,
  },

  -- ceres匹配的一些配置参数
  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.),
  },

  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10.,

  -- 位姿预测器
  pose_extrapolator = {
    use_imu_based = false,
    constant_velocity = {
      imu_gravity_time_constant = 10.,
      pose_queue_duration = 0.001,
    },
    imu_based = {
      pose_queue_duration = 5.,
      gravity_constant = 9.806,
      pose_translation_weight = 1.,
      pose_rotation_weight = 1.,
      imu_acceleration_weight = 1.,
      imu_rotation_weight = 1.,
      odometry_translation_weight = 1.,
      odometry_rotation_weight = 1.,
      solver_options = {
        use_nonmonotonic_steps = false;
        max_num_iterations = 10;
        num_threads = 1;
      },
    },
  },

  -- 子图相关的一些配置
  submaps = {
    num_range_data = 90,          -- 一个子图里插入雷达数据的个数的一半
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
      resolution = 0.05,
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      -- 概率占用栅格地图的一些配置
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地图的一些配置
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },
}

  • pose_graph.lua

    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,                   -- 打印约束计算的log
    
    -- 基于分支定界算法的2d粗匹配器
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },

    -- 基于ceres的2d精匹配器
    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,
      },
    },

    -- 基于分支定界算法的3d粗匹配器
    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的3d精匹配器
    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 = 1.1e2,      -- 3d里imu的线加速度的权重
    rotation_weight = 1.6e4,          -- 3d里imu的旋转的权重
    
    -- 前端结果残差的权重
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    -- 里程计残差的权重
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    -- gps残差的权重
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    fixed_frame_pose_use_tolerant_loss = false,
    fixed_frame_pose_tolerant_loss_param_a = 1,
    fixed_frame_pose_tolerant_loss_param_b = 1,

    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,
    fix_z_in_3d = 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., -- 纯定位时多少秒执行一次全子图的约束计算

  --  overlapping_submaps_trimmer_2d = {
  --    fresh_submaps_count = 1,
  --    min_covered_area = 2,
  --    min_added_submaps_count = 5,
  --  },
}

2.3 运行演示

powershell 复制代码 
$ source devel/setup.bash
$ roslaunch cartographer_ros lx_rs16_2d_outdoor.launch

3. Cartographer 调参总结

3.1 降低延迟与减小计算量

  • 前端

    • 减小 max_range 即减小了需要处理的点数，在激光雷达远距离的数据点不准时一定要减小这个值
    • 增大 voxel_filter_size，相当于减小了需要处理的点数
    • 增大 submaps.resolution，相当于减小了匹配时的搜索量
    • 对于自适应体素滤波 减小 min_num_points 与 max_range，增大 max_length，相当于减小了需要处理的点数

  • 后端

    • 减小 optimize_every_n_nodes, 降低优化频率, 减小了计算量
    • 增大 MAP_BUILDER.num_background_threads, 增加计算速度
    • 减小 global_sampling_ratio, 减小计算全局约束的频率
    • 减小 constraint_builder.sampling_ratio, 减少了约束的数量
    • 增大 constraint_builder.min_score, 减少了约束的数量
    • 减小分枝定界搜索窗的大小, 包括linear_xy_search_window,inear_z_search_window, angular_search_window
    • 增大 global_constraint_search_after_n_seconds, 减小计算全局约束的频率
    • 减小 max_num_iterations, 减小迭代次数

3.2 降低内存

  • 增大子图的分辨率 submaps.resolution

3.3 常调的参数

cpp 复制代码 
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2 -- / -0.8
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 -- / 0.02
POSE_GRAPH.optimize_every_n_nodes = 160. -- 2倍的num_range_data以上
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

4. Cartographer 工程化建议

4.1 如何提升建图质量

  • 选择频率高（25 Hz 以上）、精度高的激光雷达
  • 如果只能用频率低的激光雷达
    • 使用高频、高精度 IMU，且让机器人运动慢一点
    • 调 ceres 的匹配权重，将地图权重调大，平移旋转权重调小
    • 将代码中平移和旋转的残差注释掉
  • 里程计
    • Cartographer 中对里程计的使用不太好
    • 可以将 karto 与 GMapping 中使用里程计进行预测的部分拿过来进行使用，改完后就可达到比较好的位姿预测效果
  • 粗匹配
    • 将 karto 的扫描匹配的粗匹配放过来，karto 的扫描匹配的计算量很小，当做粗匹配很不错
  • 地图
    • 在最终生成地图的时候使用后端优化后的节点重新生成一次地图，这样生成的地图效果会比前端地图的叠加要好很多

4.2 降低计算量与内存

  • 体素滤波与自适应体素滤波的计算量（不是很大）
  • 后端进行子图间约束时的计算量很大
  • 分支定界算法的计算量很大
  • 降低内存，内存的占用基本就是多分辨率地图，每个子图的多分辨率地图都进行保存是否有必要

4.3 纯定位的改进建议

  • 将纯定位模式与建图拆分开，改成读取之前轨迹的地图进行匹配.
  • 新的轨迹只进行位姿预测，拿到预测后的位姿与之前轨迹的地图进行匹配，新的轨迹不再进行地图的生成与保存，同时将整个后端的功能去掉.
  • 去掉后端优化会导致没有重定位功能，这时可将 cartographer 的回环检测（子图间约束的计算）部分单独拿出来，做成一个重定位功能，通过服务来调用这个重定位功能，根据当前点云确定机器人在之前地图的位姿

4.4 去 ros 的参考思路

  • 仿照 cartographer_ros 里的操作：获取到传感器数据，将数据转到 tracking_frame 坐标系下并进行格式转换，再传入到 cartographer 里就行

5. 源码注释

相关推荐
智驾机器人技术前线18 小时前
近期两篇NeRF/3DGS-based SLAM方案赏析：TS-SLAM and MBA-SLAM
3d·slam·nerf·3dgs
CA7274 天前
【视觉SLAM】2-三维空间刚体运动的数学表示
slam·三维旋转·四元数
CA7276 天前
【视觉SLAM】4b-特征点法估计相机运动之PnP 3D-2D
slam
大山同学6 天前
RA-L开源：Light-LOAM: 基于图匹配的轻量级激光雷达里程计和地图构建
语言模型·机器人·去中心化·slam·感知定位
大山同学7 天前
DPGO:异步和并行分布式位姿图优化 2020 RA-L best paper
人工智能·分布式·语言模型·去中心化·slam·感知定位
OAK中国_官方8 天前
OAK相机：纯视觉SLAM在夜晚的应用
人工智能·机器学习·slam
极客代码11 天前
【计算机视觉】深入浅出SLAM技术原理
人工智能·python·算法·计算机视觉·机器人·slam·地图构建
大山同学12 天前
最新开源DCL-SLAM:一种用于机器人群体的分布式协作激光雷达 SLAM 框架
人工智能·分布式·机器人·开源·slam·感知定位
大山同学13 天前
多机器人图优化：2024ICARA开源
人工智能·语言模型·机器人·去中心化·slam·感知定位
Lusix194925 天前
Realsense相机驱动在使用imu数据时出现Qos问题
数码相机·slam·realsense
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06RAG 实践- Ollama+RagFlow 部署本地知识库07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO1004 - matlab m_map地学绘图工具基础函数 - 设置网格