Cartographer工作流程及参数解析

Cartographer工作流程及参数解析

参考Cartographer_ros官方教程
Cartographer 是一个复杂的系统，调整它需要对其内部工作有很好的了解。 这里试图直观地概述 Cartographer 使用的不同子系统及其参数配置值。这里只描述了 2D SLAM，但它包含了Cartographer使用的大多数概念。 这些概念通常也适用于 3D。

Cartographer的详细细节可以参考W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016. pp. 1271–1278.

系统概述

Cartographer 可以被视为两个独立但相关的子系统。 第一个是局部SLAM（有时也称为前端或本地轨迹构建器）。 它的工作是构建一系列子图。 每个子图都是局部一致的，但我们接受局部 SLAM 随时间漂移。 大多数局部 SLAM 选项可以在 install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua（对于 2D）和 install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua（对于 3D）中找到。

另一个子系统是全局 SLAM（有时称为后端）。 它在后台线程中运行，其主要工作是寻找闭环约束。 它通过扫描匹配方法匹配当前的扫描（储存在节点中）和子图来实现这一点。 它还结合其他传感器数据以获得更高级别的层次的视角并确定最一致的全局解。 在 3D 中，它还试图找到重力的方向。 它的大部分选项可以在install_isolated/share/cartographer/configuration_files/pose_graph.lua中找到。

在更高的抽象层次上，局部 SLAM 的工作是生成好的子图，而全局 SLAM 的工作是将它们最一致地联系在一起。

测距传感器（例如：LIDAR）提供多个方向的深度信息。 然而，一些测量与 SLAM 无关。 如果传感器部分被灰尘覆盖或指向机器人的一部分，则部分测量距离可被视为 SLAM 的噪声。 另一方面，一些最远的测量也可能来自不期望的信息源（反射、传感器噪声），并且也与 SLAM 无关。 为了解决这些问题，Cartographer 首先应用带通滤波器，并且将范围值保持在某个最小和最大范围之间。 应根据机器人和传感器的规格选择这些最小值和最大值。

TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range

在 2D 中，Cartographer 用·TRAJECTORY_BUILDER_2D.missing_data_ray_length 替换了比 max_range 更远的距离测量值。 它还提供了max_z和 min_z值以将 3D 点云过滤为 2D 切片。

在 Cartographer 配置文件中，每个距离都以米为单位定义。

距离是在机器人实际移动的一段时间内测量的。 然而，距离是由传感器在一个大的 ROS 消息中“批量”传递的。 Cartographer 可以独立考虑每个消息的时间戳，以考虑由机器人运动引起的畸变。 Cartographer 获得测量值的次数越多，就越能更好地对测量值进行去畸变以获得可以立即捕获的单个扫描。 因此，强烈建议通过扫描（一组可以与另一个扫描匹配的范围数据）提供尽可能多的范围数据（ROS 消息）。

TRAJECTORY_BUILDER_nD.num_accumulated_range_data

市面上的商用雷达不提供这个功能，可以忽略掉这个功能

范围数据通常是从机器人上的单个点以多个角度测量的。 这意味着靠近的表面（例如道路）经常被击中并提供很多点。 相反，远处的物体被击中的频率较低，提供的点也较少。 为了减少点处理的计算量，我们通常需要对点云进行二次采样。 但是，简单的随机抽样会从已经是低测量密度的区域中删除点，而高测量密度区域的点仍会比需要的多。 为了解决这个测量值密度分布不均的问题，我们可以使用体素滤波器将原始点下采样为恒定大小体素，并且只保留每个体素的质心。

较小的体素尺寸将导致更稠密的数据表示，从而导致更多的计算。 大的体素尺寸会导致数据丢失，但速度会快得多。

TRAJECTORY_BUILDER_nD.voxel_filter_size

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

TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points

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

TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant

在 Cartographer 配置文件中，每个时间值都以秒为单位定义

局部SLAM

一旦从多个范围数据中集成和过滤了扫描，就可以为局部 SLAM 算法做好准备。 局部 SLAM 通过使用位姿外推器的初始猜测进行扫描匹配来将新扫描插入到其当前的子图构造中。 位姿外推器背后的想法是使用测距仪之外的其他传感器的传感器数据来预测下一次扫描应该插入到子图中的位置。

有两种扫描匹配策略可用：

• CeresScanMatcher 将初始猜测作为先验，并找到当前扫描与子图匹配的最佳位置。 它通过对子图进行插值和亚像素对齐扫描来实现。 这很快，但不能修复明显大于子图分辨率的错误。 如果您的传感器设置和时序是合理的，则仅使用CeresScanMatcher 通常是最佳选择。
• 如果您没有其他传感器或不信任它们，则可以启用 RealTimeCorrelativeScanMatcher。 它使用的方法类似于如何在闭环检测中将扫描与子图匹配，但它是与当前子图匹配。 然后将最佳匹配用作 CeresScanMatcher 的先验。 这种扫描匹配器计算代价非常昂贵，并且基本上会覆盖来自除测距仪之外的其他传感器的任何信号，但它在特征丰富的环境中非常鲁棒。

无论哪种方式，CeresScanMatcher 都可以配置为为其每个输入赋予一定的权重。 权重是对数据信任度的衡量标准，这可以看作是静态协方差。 权重参数的单位是无量纲量，不能相互比较。 一个数据源的权重越大，Cartographer在做扫描匹配时就会越重视这个数据源。 数据源包括占据空间（来自扫描的点）、来自位姿外推器（或 RealTimeCorrelativeScanMatcher）的平移和旋转。

TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight

在 3D 中，occupied_space_weight_0 和 occupied_space_weight_1 参数分别与高分辨率和低分辨率过滤后的点云相关。

CeresScanMatcher 得名于 Ceres Solver，这是谷歌开发的用于解决非线性最小二乘问题的库。 扫描匹配问题被建模为这种问题的最小化，两次扫描之间的运动（变换矩阵）是要确定的参数。 对于给定的迭代次数，Ceres 使用梯度下降算法优化运动。 Ceres 可以配置为根据您自己的需要调整收敛速度。

TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps	#是否允许短暂的代价函数上升
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.max_num_iterations
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.num_threads

可以根据您对传感器的信任度来决定是否使用 RealTimeCorrelativeScanMatcher。 它的工作原理是在由最大距离半径和最大角度半径定义的搜索窗口中搜索类似的扫描。 当与在此窗口中找到的扫描执行扫描匹配时，可以为平移和旋转分量选择不同的权重。 例如，如果您知道您的机器人不会经常旋转，您就可以调整这些权重。

TRAJECTORY_BUILDER_nD.use_online_correlative_scan_matching
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.linear_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.angular_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.translation_delta_cost_weight
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.rotation_delta_cost_weight

为了避免为每个子图插入过多的扫描，一旦扫描匹配器发现两次扫描之间的运动，它就会通过运动滤波器。 如果扫描匹配得到的的运动被认为不够显著，则扫描被丢弃。 仅当其运动超过特定距离、角度或时间阈值时，才将扫描插入到当前子图中。

TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians

当局部SLAM 接收到给定数量的距离数据时，子图被认为是构建完整的。 局部 SLAM 随时间产生漂移，全局 SLAM 用于修复此漂移。 子图必须足够小，以便使它们内部的漂移低于分辨率，从而保证它们在局部是正确的。 另一方面，它们应该足够大，使得它们之间可以被区分开来，这样闭环检测才能正常工作。

TRAJECTORY_BUILDER_nD.submaps.num_range_data

子图可以将其范围数据存储在几种不同的数据结构中：最广泛使用的表示称为概率栅格地图。 但是，在 2D 中，也可以选择使用截断符号距离场 (TSDF)。

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

概率栅格将空间切割成 2D 或 3D 栅格，其中每个单元格具有固定大小并包含被占据的对数概率。 对数概率根据“击中”（测量范围数据）和“未击中”（传感器和测量点之间的自由空间）进行更新。 击中和未击中都可以在占据概率计算中具有不同的权重，从而或多或少地信任占据或空闲空间的测量值。

TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.miss_probability

在 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

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

全局SLAM

当局部 SLAM 生成其连续的子图时，全局优化（通常称为“优化问题”或“稀疏位姿调整”）任务在后台运行。 它的作用是重新排列彼此之间的子图，使它们形成一个一致的全局地图。 例如，此优化负责更改当前构建的轨迹，从而根据回环检测正确对齐子图。

一旦插入了一定数量的轨迹节点，优化就会批量运行。 根据您需要运行它的频率，您可以调整这些批次的大小。

POSE_GRAPH.optimize_every_n_nodes

将POSE_GRAPH.optimize_every_n_nodes 设置为 0 是一种禁用全局 SLAM 并专注于局部 SLAM 行为的便捷方法。 这通常是调整 Cartographer 要做的第一件事。

全局 SLAM 是一种“GraphSLAM”，它本质上是一种位姿图优化，它通过在节点和子图之间建立约束然后优化得到的约束图来工作。 约束可以直观地被认为是将所有节点绑在一起的小绳索。 稀疏位姿调整降这些绳索系在一起。 生成的网络称为“位姿图”。

约束可以在 RViz 中可视化，这对于调整全局 SLAM 非常方便。 还可以切换 POSE_GRAPH.constraint_builder.log_matches 以获取格式为直方图的约束构建器的定期报告。

• 非全局约束（也称为子图内约束）在轨迹上彼此紧密跟随的节点之间自动构建。 直观地说，那些“非全局约束”使轨迹的局部结构保持一致。
• 全局约束（也称为闭环约束或子图间约束）在新子图和被认为在空间上“足够接近”（某个搜索窗口的一部分）和强拟合（当运行扫描匹配时得到良好的匹配结果）的先前节点之间定期搜索。 直观地说，那些“全局约束”在结构中引入了结，并牢固地将两股绳索拉近。

POSE_GRAPH.constraint_builder.max_constraint_distance
POSE_GRAPH.fast_correlative_scan_matcher.linear_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_xy_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_z_search_window
POSE_GRAPH.fast_correlative_scan_matcher*.angular_search_window

在实践中，全局约束可以做的不仅仅是在单个轨迹上找到闭环。 它们还可以对齐多个机器人记录的不同轨迹。

为了限制约束的数量（和计算量），Cartographer 只考虑所有接近节点的子采样集合来构建约束。 这是由采样比率常数控制的。 采样太少的节点可能会导致错过约束和无效的闭环。 采样过多的节点会减慢全局 SLAM 的速度并阻止实时闭环。

POSE_GRAPH.constraint_builder.sampling_ratio

当考虑节点和子图进行约束构建时，它们会通过称为 FastCorrelativeScanMatcher 的第一个扫描匹配器。 此扫描匹配器专为 Cartographer 设计，使实时闭环扫描匹配成为可能。 FastCorrelativeScanMatcher 依靠“分支定界”机制在不同的栅格分辨率下工作并有效地消除不正确的匹配。 它工作在深度可以控制的搜索树中。

POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth

一旦 FastCorrelativeScanMatcher 有一个足够好的候选匹配（高于最低匹配分数），它就会被送入 Ceres Scan Matcher 以优化位姿。

POSE_GRAPH.constraint_builder.min_score
POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d
POSE_GRAPH.constraint_builder.ceres_scan_matcher

当Cartographer运行优化问题时，Ceres根据多个残差重新排列子图。 残差是使用加权代价函数计算的。 全局优化具有成本函数以考虑大量数据源：全局（闭环）约束、非全局（匹配）约束、IMU 加速度和旋转测量、局部 SLAM 粗略位姿估计、里程计或固定位置传感器（例如 GPS 系统）。 权重和 Ceres 选项可以按照局部 SLAM 部分中的描述进行配置。

POSE_GRAPH.constraint_builder.loop_closure_translation_weight
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight
POSE_GRAPH.matcher_translation_weight
POSE_GRAPH.matcher_rotation_weight
POSE_GRAPH.optimization_problem.*_weight
POSE_GRAPH.optimization_problem.ceres_solver_options

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

作为IMU 残差的一部分，优化问题为 IMU 位姿提供了一定的灵活性，默认情况下，Ceres 可以自由地优化 IMU 和被追踪坐标系之间的外参。 如果您不信任您的 IMU 位姿，可以记录 Ceres 全局优化的结果并用于改进您的外参标定。 如果 Ceres 没有正确优化您的 IMU 位姿并且您足够信任您的外参标定，您可以使这个位姿保持不变。

POSE_GRAPH.optimization_problem.log_solver_summary
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d

在残差中，异常值的影响由配置了特定 Huber 尺度的 Huber 损失函数处理。Huber 尺度越大，（潜在）异常值的影响就越大。

POSE_GRAPH.optimization_problem.huber_scale

轨迹构建完成后，Cartographer 会运行新的全局优化，迭代次数通常比以前的全局优化多得多。 这样做是为了完善 Cartographer 的最终结果，通常不需要实时，因此大量迭代通常是正确的选择。

POSE_GRAPH.max_num_final_iterations