沉默杀手HomeArchivesTagsCategories
(五) 后端 pose_graph.lua 参数
|激光SLAMCartographer原理和配置|
Word count: 1.7k|Reading time: 5 min

Local SLAM生成一系列子图时，一个global优化（通常被称为“优化问题”或者“sparse pose adjustment”）在后台运行，其主要工作是找到回环约束。它是通过scan-matching的方法用节点中收集到的scans与子地图进行匹配。它用于重新整理子地图，以便他们可以内联成一个全局地图。比如改变当前建成的轨迹以适应于回环检测的子地图对齐(align submaps with regards to loop closures)

  • optimize_every_n_nodes: 一旦一定的trajectory nodes插入，就会分批执行优化，由你决定多么频繁的运行优化和调整大小。如果设置为0，是手动关闭全局SLAM，主要精力集中于局部SLAM。这是调试cartographer的第一件事情。 optimize_every_n_nodes里对应优化标志kRunOptimization，这个标志一共出现三次，还有FinishTrajectory和RunFinalOptimization。 前者的根源是node.FinishAllTrajectories();，     后者的根源在node.RunFinalOptimization();

全局SLAM是一种基于图优化的SLAM，本质上是位姿图优化，其是通过构建节点之间的约束，子地图之间的约束，然后优化产生的约束图。约束可以认为是一根连接所有节点的绳子。sparse pose adjustment加速这些绳子的结合。产生的网称为位姿图(Pose Graph)

约束可以在rviz中观察，这样调整global SLAM很方便

constraint_builder

1
2
3
4
5
6
7
8
9
10
POSE_GRAPH = {
  optimize_every_n_nodes = 90, # 每几个节点执行一次优化，设为0会关闭后端优化
  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,     # 回环约束的日志
  • sampling_ratio: 就是个采样器，用于 MaybeAddConstraint，值越小， 计算约束的频率越小

  • max_constraint_distance: 非常重要 局部子图进行回环检测时，能成为约束的距离阈值，在 MaybeAddConstraint开头。如果设置过小，即使建图时走回同一个位置，也不会计算回环，函数里return。如果设置太大，计算量会增大很多，因为节点会和所有完成的子图匹配。

  • log_matches: 得到 constraints builder(回环约束)的日志，默认是true。 在ConstraintBuilder2D::ComputeConstraint

  • min_score: 局部子图进行回环检测时，扫描匹配分数的阈值，低于该阈值时不考虑匹配。 匹配得分不要太高，不然在有的地方不能回环。决定了哪些constraint添加到哪些节点间，这个会很大影响CPU。可以稍微增大到0.75 The histograms printed while Cartographer is running shows that you have a ton of loop closures constraints already, so lowering the score (which will raise the number of constraints) is probably not too useful. I’d rather ramp it up.

  • global_localization_min_score: 用于有多条轨迹的情况，比如纯定位。不能设置太小，否则会出现超出地图的约束；也不能太大，否则难以出现约束。一般设置为 0.6

非全局约束（也称为子地图内部约束）是在节点之间自动建立，这些节点是轨迹上相对较近的节点。直观上，这些非全局约束保持了轨迹的内连关系。

全局约束（也称为回环检测约束或者子地图之间的约束）通常是在一个新的子地图和先前的节点之间进行搜索，那些先前的节点在空间上是足够的近（部分是在某个搜索window）。直观上，这些全局约束在结构上引进了一个结（打结），固定把两股子地图靠拢。

以下几项均为constraint_builder的子项

fast_correlative_scan_matcher

分支定界

1
2
3
4
5
6
7
fast_correlative_scan_matcher = 
{
  # 这里的window大小明显比前端 real_time_correlative_scan_matcher 的大
  linear_search_window = 7.,      # 依靠“分支定界”机制在不同的网格分辨率下工作，并有效地消除不正确的匹配
  angular_search_window = math.rad(30.), # 一旦找到足够好的分数（高于最低匹配分数），它（scan）就会被送入Ceres扫描匹配器以优化姿势。
  branch_and_bound_depth = 7,    # 至少为1，应当是值越大，后端的作用越强
},

branch_and_bound_depth必须大于3，否则相当于暴力搜索，没有效果。和分辨率也有关系，0.05就用6和7，也就是可以不改。

ceres_scan_matcher

闭环检测的第二步，优化位姿图

1
2
3
4
5
6
7
8
9
10
11
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,
  },
},

这里和前端的csm不同了，occupied_space_weight较大，位移权重较小，旋转权重更小。

use_nonmonotonic_steps：是否允许计算cost时，短暂增大。false会计算到局部最小; true则增加计算量，可越过局部最小。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  # 残差方程的参数配置
  optimization_problem = { 
  # Huber损失函数的尺度因子，核函数，用于filter outliers。该值越大，
  # (potential) outliers（潜在）异常值 的影响越大。
    huber_scale = 1e1, 
    acceleration_weight = 1e3, # IMU加速度的权重
    rotation_weight = 3e5,     # IMU旋转项的权重
    # 基于local SLAM的姿势在连续节点之间进行平移的权重
    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全局优化的结果并用于改进您的外部校准
    log_solver_summary = false,
    # 作为IMU残差的一部分
    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,
  --  },
}

global_sampling_ratio只在PoseGraph2D::AddTrajectoryIfNeeded中的FixedRatioSampler机制使用

overlapping_submaps_trimmer_2d 机制

源码在PoseGraph2D的构造函数里，对应has_overlapping_submaps_trimmer_2d参数

Trimmer是一个删除子图的操作，其具体参数在cartographer/configuration_files/pose_graph.lua中

1
2
3
4
5
6
overlapping_submaps_trimmer_2d =
{
  fresh_submaps_count = 1,
  min_covered_area = 2,
  min_added_submaps_count = 5,
}

这段被注释的部分，如果放开就会发现，建图时重复走一条路，submap会不连续，很多相似的被删除了。上述参数主要是调整参数某数submap是否达到被删除的阈值。具体实现在overlapping_submaps_trimmer_2d.cc，可以简单理解为，删除与最新的1个submaps覆盖后剩余栅格小于2个的子图submap。

全局优化中的里程计

如果local SLAM 使用了单独的里程计(use_odometry = true), 我们可以相应地调整全局SLAM

有四个参数允许我们在优化中调整局部SLAM和里程计的各个权重:

1
2
3
4
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight
POSE_GRAPH.optimization_problem.odometry_translation_weight
POSE_GRAPH.optimization_problem.odometry_rotation_weight

可以根据我们对本地SLAM或odometry的信任程度来设置这些权重。默认情况下，里程计被加权到类似于本地slam（扫描匹配）姿势的全局优化中。然而，来自车轮里程计的旋转通常具有很高的不确定性，因此，旋转权重可以减小，甚至降低到0

参考：后端测试报告