Cartographer算法调参

根据Cartographer_ros文档翻译
Cartographer是一个复杂的系统，调整它需要很好地理解其内部工作。此页面试图直观地概述Cartographer使用的不同子系统及其配置值。如果您对Cartographer的介绍不仅仅感兴趣，还应参考Cartographer论文。它仅描述了2D SLAM，但它严格定义了此处描述的大多数概念。这些概念通常也适用于3D。

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可以看作是两个独立但相关的子系统。第一个是LocalSLAM（有时也称为前端或局部轨迹构建器）。它的工作是建立一系列子图。每个子图都是本地一致的，但我们接受LocalSLAM随着时间的推移而漂移。大多数地方SLAM选项中可以找到install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua 为2D和install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua为3D。（对于本页的其余部分，我们将参考TRAJECTORY_BUILDER_nD作为常用选项）

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

在更高的抽象上，LocalSLAM的工作是生成良好的子图，而全局SLAM的工作是将它们最一致地结合在一起。

输入

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

TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range

注意

在2D中，Cartographer将比max_range更换范围TRAJECTORY_BUILDER_2D.missing_data_ray_length。它还提供了将3D点云过滤为2D切割的值max_z和min_z值。

注意

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

距离是在一段时间内测量的，而机器人实际上正在移动。但是，距离是由大型ROS消息中的“批量”传感器提供的。Cartographer可以独立考虑每个消息的时间戳，以考虑机器人运动引起的畸变。Cartographer进行测量频率越高，测量结果组合成一个可以立即捕获的单个相干扫描就越好。因此，强烈建议通过扫描提供尽可能多的rangedata（ROS消息）。

TRAJECTORY_BUILDER_nD.num_accumulated_range_data

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

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

TRAJECTORY_BUILDER_nD.voxel_filter_size

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

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配置文件中，每次定义值都以秒为单位

Local SLAM

一旦扫描组装并从多个范围数据中过滤，就可以为LocalSLAM算法做好准备。LocalSLAM 使用来自位姿估计器的初始估计通过扫描匹配将新扫描插入其当前子图构造中。位姿估计器背后的想法是使用除测距仪之外的其他传感器的传感器数据来预测下一次扫描应该插入子图的位置。

有两种扫描匹配策略：

• 该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

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

TRAJECTORY_BUILDER_nD.submaps.num_range_data

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

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

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

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

注意

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

TODO：记录TSDF配置

Global SLAM

当LocalSLAM生成其连续的子图时，全局优化（通常称为“ 优化问题 ”或“ 稀疏位姿调整 ”）任务在后台运行。它的作用是重新安排彼此之间的子图，以便它们形成一个连贯的全局地图。例如，该优化负责改变当前构建的轨迹以正确地对准关于环闭合的子图。

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

POSE_GRAPH.optimize_every_n_nodes

注意

将POSE_GRAPH.optimize_every_n_nodes设置为0是禁用全局SLAM并专注于LocalSLAM行为的便捷方法。这通常是调整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扫描匹配器以改进位姿。

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系统）。可以按照LocalSLAM部分中的说明配置权重和Ceres选项。

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