cartographer算法调优演练

作者 易水 2025年06月15日 14:55 阅读 545
0 0 0

算法调优演练Cartographer 是一个复杂的系统，调优需要充分理解其内部工作原理。本页面旨在直观地概述 Cartographer 所使用的不同子系统及其配置值。如果您对 Cartographer 的更多内容感兴趣，请参阅 Cartographer 论文。该论文仅介绍了 2D SLAM，但对本文中提到的大多数概念进行了严格的定义。这些概念通常也适用于 3D SLAM。W. Hess, D. Kohler, H. Rapp 和 D. Andor， 《2D LIDAR SLAM 中的实时环路闭合》，载于 《机器人与自动化（ICRA）》，2016 IEEE 国际会议。IEEE，2016 年，第 1271-1278 页。概述Cartographer 可以看作是两个独立但又相互关联的子系统。第一个是本地 SLAM（有时也称为前端或本地轨迹构建器）。它的作用是构建一系列子图。每个子图都应保持局部一

算法调优演练

Cartographer 是一个复杂的系统，调优需要充分理解其内部工作原理。本页面旨在直观地概述 Cartographer 所使用的不同子系统及其配置值。如果您对 Cartographer 的更多内容感兴趣，请参阅 Cartographer 论文。该论文仅介绍了 2D SLAM，但对本文中提到的大多数概念进行了严格的定义。这些概念通常也适用于 3D SLAM。

W. Hess, D. Kohler, H. Rapp 和 D. Andor， 《2D LIDAR SLAM 中的实时环路闭合》，载于 《机器人与自动化（ICRA）》，2016 IEEE 国际会议。IEEE，2016 年，第 1271-1278 页。

概述

https://raw.githubusercontent.com/cartographer-project/cartographer/master/docs/source/high_level_system_overview.png

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）中找到。（在本页面的其余部分，我们将使用TRAJECTORY_BUILDER_nD来了解常用选项。）

另一个子系统是全局 SLAM（有时称为后端）。它在后台线程中运行，主要任务是查找回环约束。它通过将扫描结果（在**节点**中收集）与子地图进行扫描匹配来实现此目的。它还会整合其他传感器数据，以获得更高级别的视图并确定最一致的全局解决方案。在 3D 环境中，它还会尝试查找重力方向。其大部分选项可在install_isolated/share/cartographer/configuration_files/pose_graph.lua中找到。

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

输入

测距传感器（例如：激光雷达）提供多个方向的深度信息。然而，有些测量值与 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。它还提供了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

惯性测量单元 (IMU) 可以成为 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_2D.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

子图可以将其范围数据存储在几种不同的数据结构中：最广泛使用的表示形式称为概率网格。然而，在二维空间中，也可以选择使用截断符号距离场 (TSDF)。

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

概率网格将空间划分成二维或三维表格，每个单元格大小固定，并包含被遮挡的概率。概率会根据“命中”（测量距离数据的位置）和“未命中”（传感器与测量点之间的自由空间）进行更新。命中和未命中在占用概率计算中具有不同的权重，从而决定对已占用或自由空间测量结果的可信度。

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

在二维中，每个子图仅存储一个概率网格。在三维中，出于扫描匹配性能方面的考虑，使用两个混合概率网格。（“混合”一词仅指内部树状数据表示，对用户而言是抽象的。）

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

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

扫描匹配首先将低分辨率点云的远点与低分辨率混合网格对齐，然后通过将近点高分辨率点与高分辨率混合网格对齐来细化姿态。

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 左侧窗格中提供了一些选项，用于在低分辨率和高分辨率混合网格可视化之间切换。

TODO：记录 TSDF 配置

全局 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依赖于“分支定界”机制，可以在不同的网格分辨率下工作，并有效消除错误匹配。该机制在本文前面介绍的 Cartographer 论文中进行了详细介绍。它适用于深度可控的探索树。

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.log_residual_histograms

作为 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

使用弹性数据平台 MongoDB Atlas 开发和启动现代应用程序。


上一篇： 在自己的数据包上运行 CartographerROS
下一篇： 初识Python

发表评论