Google Cartographer SLAM 参数调整

 1 overview 

cartographer可以看作是两个独立但相关的子系统。第一个是本地SLAM（有时候 也称为前端或局部轨迹构建器）。它的工作是建立一系列submap。每个submap都意味着在短时间内一致，但我们接受local SLAM随着时间的推移而漂移。大多数local SLAM选项都在install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua for 2D and install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua for 3D 

  另一个子系统是global SLAM（有时称为后端）。它运行在后台线程及其中主要工作是找到循环闭包约束。它通过扫描匹配扫描（在节点中收集）来实现子图。它还结合了其他传感器数据，以获得更高级别的视图并确定最一致的视图全球解决方案在3D中，它还试图找到重力方向。它的大多数选项都在install_isolated/share/cartographer/configuration_files/pose_graph.lua 

  抽象来说，本地SLAM的工作是生成良好的子图，全局SLAM的工作是将它们联系起来。

2 input

  测距传感器（例如：LIDAR）提供多个方向的深度信息。但是，有些 测量与SLAM无关。如果传感器部分被灰尘覆盖或者朝向某个部件 对于机器人，一些测量距离可以被认为是SLAM的噪声。另一方面一些最远的测量也可能来自不需要的源（反射，传感器噪声），并且与SLAM无关 同样。为了解决这些问题，Cartographer首先应用带通滤波器，并仅在a之间保持范围值 一定的最小和最大范围。应根据机器人的规格选择最小值和最大值 和传感器。

min_range = 0.,--传感器测量深度距离范围
max_range = 30.,

  它还提供max_z和min_z值，以将3D点云过滤为2D切割。

min_z = -0.8,--传感器测量高度值范围，应用在3d
max_z = 2.,

  距离是在一段时间内测量的，而机器人实际上正在移动。但是，距离是由大型ROS消息中的“批量”传感器提供的。每个消息的时间戳都可以独立考虑 由制图师考虑机器人运动引起的变形。制图师得到的频率越高 测量结果越好，就可以在不扭曲测量值的情况下组装单个相干扫描 已被捕获。因此，强烈建议提供尽可能多的范围数据（ROS消息） 扫描（一组可以与另一个扫描匹配的范围数据）尽可能。

num_accumulated_range_data = 1,

  范围数据通常从机器人上的单个点测量，但是以多个角度测量。这意味着关闭表面 （例如道路）经常被击中并提供很多积分。相反，远处的物体不常被击中 提供更少的积分。为了减少点处理的计算权重，我们通常需要子采样点 云。但是，简单的随机抽样会从我们已经具有低密度的区域中移除点 测量和高密度区域仍然会有比所需更多的点。为了解决密度问题， 我们可以使用一个体素滤镜，将原始点下采样到一个恒定大小的立方体中，并且只保留每个立方体的质心 立方体。

较小的立方体大小将导致更密集的数据表示，从而导致更多计算。一个大的立方体尺寸导致数据丢失，但会更快。

voxel_filter_size = 0.025,

  在应用了固定尺寸的体素滤镜后，Cartographer还应用了自适应体素滤镜。此过滤器尝试 确定最佳体素大小（在最大长度下）以达到目标点数。在3D中，两个自适应体素滤波器用于生成高分辨率和低分辨率点云，它们的使用将在Local中阐明 SLAM。

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

 

  惯性测量单元可以是SLAM的有用信息源，因为它提供了准确的信息 重力方向（因此，地面）和机器人旋转的嘈杂但良好的整体指示。为了过滤IMU噪声，在一定时间内观察到重力。如果您使用2D SLAM，则可以实时处理范围数据而无需额外的信息来源，因此您可以选择是否要让Cartographer使用IMU。使用3D SLAM，您需要提供IMU，因为它被用作初始猜测 扫描方向，大大降低了扫描匹配的复杂性。

use_imu_data = true,
imu_gravity_time_constant = 10.,

 3 local SLAM

​​​​​  一旦扫描组装并从多个范围数据中过滤，就可以为本地SLAM算法做好准备。本地SLAM使用来自姿势外推器的初始猜测通过扫描匹配将新扫描插入其当前子图构造中。姿势外推器背后的想法是使用除测距仪之外的其他传感器的传感器数据来预测下一次扫描应该插入子图的位置。
有两种扫描匹配策略：
•CeresScanMatcher将初始猜测作为先验，并找到扫描匹配所适合的最佳位置 子图。它通过插入子图和子像素对齐扫描来实现。这很快，但无法修复明显大于子图分辨率的错误。如果您的传感器设置和时序合理，那么仅使用CeresScanMatcher通常是最佳选择。
•如果您没有其他传感器或者您不信任它们，则可以启用RealTimeCorrelativeScanMatcher。它使用的方法类似于在循环闭包中将扫描与子图匹配的方式（稍后描述），但它与当前子图匹配。然后将最佳匹配用作CeresScanMatcher的先验。这种扫描匹配器非常昂贵，并且基本上会覆 盖来自其他传感器但除了测距仪的任何信号，但它在功能丰富的环境中非常强大。
  无论哪种方式，CeresScanMatcher都可以配置为为每个输入赋予一定的权重。权重是衡量对数据的信任度，可以将其视为静态协方差。重量参数的单位是无量纲的数量并不能相互比较。数据源的权重越大，制图师在进行扫描匹配时就会越强调这个数据源。数据来源包括占用空间（扫描点），姿势外推器（或RealTimeCorrelativeScanMatcher）的平移和旋转

  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,
    },
  },   

      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 1,

 

  可以根据您对传感器的信任来切换RealTimeCorrelativeScanMatcher。 它的工作原理是在搜索窗口中搜索类似的扫描，搜索窗口由最大距离半径和a定义 最大角度半径。当使用此窗口中的扫描执行扫描匹配时，可以使用不同的权重 选择用于平移和旋转组件。例如，如果你知道的话，你可以玩这些重量 你的机器人不会旋转很多。

  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,
  },

 

  为避免每个子图插入太多扫描，一旦扫描匹配器找到两次扫描之间的运动，它就会发生 通过运动过滤器。如果导致它的运动不够重要，则扫描将被删除。一个 仅当扫描的运动高于某个距离，角度或时间阈值时，才会将扫描插入到当前子图中。

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

 

  当本地SLAM已经接收到给定量的范围数据时，子映射被认为是完整的。随着时间的推移，本地SLAM漂移，全球SLAM用于解决这种漂移问题。子图必须足够小，以使其内部的漂移低于分辨率，以便它们在本地正确。另一方面，它们应该足够大以使环路闭合能够正常工作

 submaps = {
    num_range_data = 90,

 

  子图可以将它们的范围数据存储在几个不同的数据结构中：最广泛使用的表示称为概率网格。但是，在2D中，还可以选择使用截断的有符号距离场（TSDF）

    grid_options_2d = {
      grid_type = "PROBABILITY_GRID",
      resolution = 0.05,
    },

 

  概率网格将空间划分为2D或3D表格，其中每个单元格具有固定大小并包含被阻挡的几率。根据“命中”（测量范围数据）和“未命中”（传感器和测量点之间的自由空间）更新赔率。命中和未命中在占用概率计算中可以具有不同的权重，从而对占用或自由空间测量给予或多或少的信任。

    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,
      },

 

  在2D中，每个子图仅存储一个概率网格。在3D中，出于扫描匹配性能的原因，使用两个混合概率网格。 （术语“混合”仅指内部树状数据表示并被抽象给用户）

•用于远距离测量的低分辨率混合网格

•高分辨率混合网格，用于近距离测量

  通过将低分辨率点云的远点与低分辨率混合网格对齐来开始扫描匹配 然后通过将高分辨率点与高分辨率混合网格对齐来细化姿势。

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution
TRAJECTORY_BUILDER_3D.submaps.high_resolution
TRAJECTORY_BUILDER_3D.submaps.low_resolution
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range

 

  制图师ROS提供了一个可视化子图的RViz插件。您可以选择要查看的子图 从他们的号码。在3D中，RViz仅显示3D混合概率网格的2D投影（灰度）。 RViz左侧窗格中提供了选项，可在低分辨率和高分辨率混合网格可视化之间切换

4 Global SLAM

  当本地SLAM生成其连续的子图时，全局优化（通常称为“优化问题”或“稀疏姿势调整”）任务在后台运行。它的作用是重新安排彼此之间的子图，以便它们形成一个连贯的全球地图。例如，该优化负责改变当前构建的轨迹以正确地对准关于环闭合的子图。
  一旦插入了一定数量的轨迹节点，就会批量运行优化。根据您运行它的频率，您可以调整这些批次的大小。

  optimize_every_n_nodes = 90,

 

  将POSE_GRAPH.optimize_every_n_nodes设置为0是禁用全局SLAM和集中注意力的便捷方法 关于本地SLAM的行为。这通常是调整Cartographer的第一件事。

 

  全局SLAM是一种“GraphSLAM”，它本质上是一种姿势图优化，它通过在节点和子图之间构建约束然后优化得到的约束图来工作。约束可以直观地进行 被认为是将所有节点捆绑在一起的小绳索。稀疏姿势调整完全固定这些绳索。该 结果网被称为“姿势图”

 

  约束可以在RViz中可视化，调整全局SLAM非常方便。人们也可以切换 POSE_GRAPH.constraint_builder.log_matches以获取格式为直方图的约束生成器的常规报告。

 

•非全局约束（也称为inter子图约束）是在节点之间自动构建的 在轨道上紧密相互追随。直觉上，那些“非全球性的绳索”保留了当地的结构 轨迹连贯。

•定期搜索全局约束（也称为循环闭包约束或子内图约束） 在新的子图和在空间中被认为“足够接近”的先前节点之间（某个搜索的一部分） 窗口）和强大的匹配（运行扫描匹配时的良好匹配）。直觉上，那些“全球绳索”引入 在结构中结，并牢牢地使两股更紧密

    max_constraint_distance = 15.,

    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.),
    },

  实际上，全局约束不仅可以在单个轨迹上查找循环闭包。他们也可以对齐由多个机器人记录的不同轨迹，但我们将保留此用法以及与“全局本地化”相关的参数超出本文档的范围。

  为了限制约束（和计算）的数量，Cartographer仅考虑构建约束的所有关闭节点的子采样集。这由采样率常数控制。采样太少的节点可能导致错过约束和无效的循环闭包。采样太多节点会降低全局SLAM的速度并阻止实时循环闭包。

  

  global_sampling_ratio = 0.003,

  当考虑节点和子图构建约束时，它们会通过名为FastCorrelativeScanMatcher的第一个扫描匹配器。此扫描匹配器专为制图师和制作而设计实时循环闭包扫描匹配可能。 FastCorrelativeScanMatcher依靠“分支定界”机制在不同的网格分辨率下工作，并有效地消除不正确的匹配。这个机制在本文件前面提到的制图论文中广泛提出了这一点。它适用于探索树其深度可以控制。

    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },

一旦FastCorrelativeScanMatcher有足够好的提议（高于最低匹配分数），然后将其送入Ceres扫描匹配器以改善姿势。

    min_score = 0.55,

    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,
      },
    },
    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,
      },
    },
  },

  当Cartographer运行优化问题时，Ceres用于根据多个残差重新排列子图。残差是使用加权成本函数计算的。全局优化需要考虑成本函数大量数据源：全局（循环闭包）约束，非全局（匹配器）约束，IMU加速和旋转测量，局部SLAM粗糙姿态估计，测距源或固定帧（如aGPS系统）。可以按照本地SLAM部分中的说明配置权重和Ceres选项。

  loop_closure_translation_weight = 1.1e4,
  loop_closure_rotation_weight = 1e5,

  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,

  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1e3,
    rotation_weight = 3e5,
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,

    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
}，

通过切换POSE_GRAPH.max_num_final_iterations，可以找到有关优化问题中使用的残差的有用信息

作为其IMU残差的一部分，优化问题为IMU姿势提供了一些灵活性，默认情况下，Ceres是可以自由优化IMU和跟踪框架之间的外部校准。如果你不相信你的IMU姿势，那么可以记录Ceres全局优化的结果并用于改进外部校准。如果Ceres没有
正确优化您的IMU姿势并且您完全信任您的外在校准，您可以使此姿势保持不变

    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,

 

在残差中，异常值的影响由配置有某个Huber量表的Huber损失函数处理。该Huber量表越大，（潜在）异常值的影响越大。

    huber_scale = 1e1,

一旦轨迹完成，Cartographer将运行一个新的全局优化，通常比迭代更多以前的全局优化。这样做是为了完善制图师的最终结果，通常不需要实时所以大量的迭代通常是正确的选择。

 max_num_final_iterations = 200,