Cartographer-[2]-系统参数配置说明

Catalogue
  1. 1. 1. Algorithm walkthrough for tuning
    1. 1.1. 1.1. 两个子系统
    2. 1.2. 1.2. Local SLAM
      1. 1.2.1. 1.2.1. CeresScanMatcher
      2. 1.2.2. 1.2.2. RealTimeCorrelativeScanMatcher
      3. 1.2.3. 1.2.3. 运动间隔
      4. 1.2.4. 1.2.4. 子图配置
    3. 1.3. 1.3. Global SLAM
      1. 1.3.1. 1.3.1. 全局BA优化频率
      2. 1.3.2. 1.3.2. 约束及搜索窗口
      3. 1.3.3. 1.3.3. 全局BA的Cost function相关
  2. 2. 2. 其他配置项
    1. 2.1. 2.1. 激光配置项
    2. 2.2. 2.2. 体素滤波器
      1. 2.2.1. 2.2.1. 固定szie体素滤波
      2. 2.2.2. 2.2.2. 自适应体素滤波
    3. 2.3. 2.3. IMU

1. Algorithm walkthrough for tuning

1.1. 两个子系统

第一个是局部 SLAM (有时也称为前端或局部轨迹构建器)。 它的工作是构建一系列子地图。 每个子地图都意味着本地一致性,但我们接受本地 SLAM 随着时间的推移而漂移。

另一个子系统是全局 SLAM (有时称为后端)。 它在后台线程中运行,其主要任务是找到闭环约束。 它通过对子图进行扫描匹配扫描(聚集在节点中)来实现这一点。 它还结合了其他传感器数据,以获得更高层次的看法,并确定最一致的全局解决方案。 在3D 中,它还试图找到重力的方向。

上述两个模块的基本选项配置文件可以参见

1.2. Local SLAM

当一帧扫描从激光数据经过滤波和嵌入,就可以开始执行Local SAM,Local SLAM通过使用pose extrapolator 来得到位姿的初始估计进行扫描匹配,得到优化的位姿后,将新的扫描插入到子图中。

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

  • CeresScanMatcher将最初的猜测作为优先值,并找到扫描匹配子图的最佳位置。这是通过插值子图和子像素对齐扫描来实现的。 这很快,但不能修复明显大于子图分辨率的错误。(如果您的传感器设置和时间是合理的,只使用CeresScanMatcher通常是最好的选择)
  • RealTimeCorrelativeScanMatcher如果您没有其他传感器或者您不信任它们,则可以启用它,它使用的方法类似于如何将扫描与的子图进行匹配形成闭环(后面将描述) ,一种用法是使用RealTimeCorrelativeScanMatcher作为先验的初值,提供给CeresScanMatcher进行位姿估计。

无论哪种方式,CeresScanMatcher 都可以配置为给每个输入赋予一定的权重。 权重是对数据的信任度量,这可以看作是一个静态的协方差。 权重参数的单位是无量纲量,不能相互比较。 一个数据源的权重越大,制图师在进行扫描匹配时就越重视这个数据源。 数据源包括占据空间(扫描点)、来自pose extrapolator(或 RealTimeCorrelativeScanMatcher)的平移和旋转

权重配置选项为

1
2
3
4
5
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

在三维空间中,占用空间权重为0和占用空间权重为1的参数分别与高分辨率和低分辨率滤波点云有关。

1.2.1. CeresScanMatcher

Ceresscanmatcher的名字来源于谷歌开发的用于解决非线性最小二乘问题的 ceressolver 库。 扫描匹配问题被建模为这样一个问题的最小化,扫描之间的运动(变换矩阵)是一个参数确定。 使用梯度下降法来优化位姿,它可以调整通过配置调整收敛的速度

参数项为:

1
2
3
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

1.2.2. RealTimeCorrelativeScanMatcher

RealTimeCorrelativeScanMatcher可以根据您对传感器的信任进行切换,它的工作原理是在一个搜索窗口中搜索相似的扫描,该窗口由最大距离半径和最大角度半径定义。当寻找到一个良好的匹配位姿时,平移和旋转的部分可以各自拥有不同的权重,比如你知道你的机器人实际上并没有旋转这么大的角度,则可以调小权重。

参数项为:

1
2
3
4
5
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

1.2.3. 运动间隔

为了避免每个子图插入太多的扫描,一旦扫描匹配程序发现两个扫描之间的运动,它就会通过运动过滤器。 如果导致扫描的运动被认为不够重要,那么扫描就会被丢弃。 只有当扫描的运动超过一定的距离、角度或时间阈值时,扫描才会插入当前子图。

配置项:

1
2
3
TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians

1.2.4. 子图配置

当Local SLAM 接收到给定数量的激光数据时,子地图被认为是完整的。 随着时间的推移,局部 SLAM 漂移,全局 SLAM 被用来修正这种漂移。 子图必须足够小,使其内部的漂移低于分辨率,以便它们在局部是正确的。 另一方面,它们应该足够大,以便使闭环能够正常工作。

配置项:

1
TRAJECTORY_BUILDER_nD.submaps.num_range_data

子图可以将它们的范围数据存储在两个不同的数据结构中: 最广泛使用的表示方式称为概率网格。 但是,在2D 中,也可以选择使用截断有符号距离字段(TSDF)

配置项:

1
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

概率网格将空间切割成二维或三维表格,其中每个单元格都有固定的大小,每个单元格的值表示被占据的概率。这个概率根据“命中”(测量距离数据的地方)和“未命中”(传感器和测量点之间的空闲空间)更新,在占用概率计算中,命中和未命中可能具有不同的权重,从而给予占用空间或自由空间测量或多或少的信任。

配置项:

1
2
3
4
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 中,每个子图只存储一个概率网格。

在三维中,由于扫描匹配性能的原因,采用了两种混合概率网格。 (术语“混合”仅指内部树状数据表示形式) :

  • a low resolution hybrid grid for far measurements 用于远距离测量的低分辨率混合网格
  • a high resolution hybrid grid for close measurements 用于近距离测量的高分辨率混合网格

扫描匹配首先用低分辨率混合网格对齐低分辨率点云的远端点,然后用高分辨率混合网格对齐高分辨率点,从而改进姿态。

配置项

1
2
3
4
5
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 的左侧面板提供了在低分辨率和高分辨率混合网格可视化之间切换的选项

1.3. Global SLAM

当局部 SLAM 生成一系列子地图时,一个全局优化(通常称为“最佳化问题”或“稀疏姿态调整”)任务在后台运行。 它的作用是重新安排彼此之间的子图,以便它们形成一个连贯的全局地图。 例如,这个优化负责改变当前构建的轨迹,使子图与闭环链接正确对齐。

1.3.1. 全局BA优化频率

一旦插入了一定数量的轨迹节点,就可以分批进行优化。 根据需要运行它的频率,可以调优这些批处理的大小。

参数项:

1
POSE_GRAPH.optimize_every_n_nodes

提示: 将POSE_GRAPH.optimize_every_n_nodes设置为0是禁用全局 SLAM 和关注Local SLAM 行为的一种简便方法。 这通常是调整制图师的第一件事情。

1.3.2. 约束及搜索窗口

Global SLAM 是一种“图 SLAM” ,它本质上是一种姿态图优化,通过在节点和子图之间建立约束,然后优化产生的约束图来实现。 约束可以直观地被认为是将所有节点绑在一起的小绳索。 稀疏位置调整将这些绳子一起扣紧。 由此产生的网络称为“姿态图”。

  • 非全局约束: 也称内部子图约束,是根据轨迹上的每个位姿自动连接的,直观地说,这些非全局约束保持了轨迹的局部结构一致
  • 全局约束:也称闭环约束,通过对新的子图和旧的位姿顶点(取认为比较接近的),然后在搜索窗口中进行匹配,如果足够的匹配,则认为形成闭环。

参数项

1
2
3
4
5
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 的速度并阻碍实时闭环。参数设置:

1
POSE_GRAPH.constraint_builder.sampling_ratio

当考虑为一个节点和子图之间建立约束时,将使用名为FastCorrelativeScanMatcher的第一个扫描匹配程序,依靠“分枝定界”机制在不同的网格分辨率下工作,并有效地消除不正确的匹配。它工作在一棵可以控制深度的扩展树上。参数设置:

1
2
3
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 来优化这个姿态。参数为:

1
2
3
POSE_GRAPH.constraint_builder.min_score
POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d
POSE_GRAPH.constraint_builder.ceres_scan_matcher

1.3.3. 全局BA的Cost function相关

当Cartographer求解优化问题时,Ceres被用来根据多个残差来重新排列子图,残差使用加权的cost function,全局优化的cost function包括了全局(闭环)约束,非全局约束,IMU加速度和角速度,Local SLAM的粗略姿态估计,里程计测量,(或者GPS固定坐标系的测量) 这些项。

参数项:

1
2
3
4
5
6
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

在残差分析中,异常值的影响由一个配置有一定胡贝尔标度的Huber损失函数来处理。 Huber 尺度越大,(潜在的)异常值的影响越大。

核函数配置:

1
POSE_GRAPH.optimization_problem.huber_scale

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

参数:

1
POSE_GRAPH.max_num_final_iterations

2. 其他配置项

2.1. 激光配置项

测距传感器(例如: LIDARs)在多个方向上提供深度信息。 然而,有些测量结果与 SLAM 无关。 如果传感器部分被灰尘覆盖,或者如果它指向机器人的一部分,一些测量的距离可以被认为是 SLAM 的噪音。

另一方面,一些最远距离的测量也可以来自不希望得到的源(反射、传感器噪声) ,与 SLAM 也无关。 为了解决这些问题,Cartographer首先应用带通滤波器,并只保持范围值之间的一定最小和最大范围。 这些最小值和最大值应根据您的机器人和传感器的规格选择。

1
2
TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range

在2D情况下,将会使用TRAJECTORY_BUILDER_2D.missing_data_ray_length来替换超过最大范围的数据,对于3D则提供max_zmin_z来裁剪3D点云。

(配置文件中,每个距离都以米为单位定义)

当机器人实际上在移动的时候,距离是在一段时间内测量的。 然而,距离是通过传感器“批量”传递的大型 ROS 消息。每条消息的时间戳都可以被Cartographer独立地考虑,用来考虑机器人运动引起的变化。Cartographer得到的传感器观测的次数越多,就越能修正地图的弯曲,消除累计误差。因此,强烈推荐提供尽可能多的回环以进行建图。

2.2. 体素滤波器

2.2.1. 固定szie体素滤波

距离数据通常是从机器人上的一个单点测量,但在多个角度。 这意味着靠近的路面(例如道路)经常被撞击并提供许多点。 相反,远处的物体被击中的次数更少,点数也更少。 为了减少点处理的计算量,通常需要对点云进行子采样。 然而,一个简单的随机抽样会从我们已经有一个低密度测量的地区移除点,而高密度的地区仍然会有比需要的更多的点。 为了解决这个密度问题,我们可以使用体素滤波器,将原始点降格为常量大小的立方体,并且只保留每个立方体的质心。

小的立方体大小将导致更密集的数据表示,从而导致更多的计算。 大的多维数据集大小会导致数据丢失,但是速度会快得多。

配置选项为

1
TRAJECTORY_BUILDER_nD.voxel_filter_size

2.2.2. 自适应体素滤波

在应用了固定大小的体素滤波器之后,制图师还应用了自适应的体素滤波器。 这个过滤器试图确定最佳体素大小(在最大长度之下)以实现目标点数。 在3D 中,两个自适应体素滤波器用于生成高分辨率和低分辨率点云

配置选项为

1
2
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points

2.3. IMU

惯性导航系统可以成为 SLAM 的一个有用的信息来源,因为它提供了一个准确的重力方向(因此,地面)和一个嘈杂但很好的机器人旋转的整体指示。 为了过滤 IMU 噪声,需要在一定时间内观测重力。 如果您使用2D SLAM,距离数据可以实时处理,不需要额外的信息源,因此您可以选择是否要制图员使用 IMU。 使用3D SLAM,您需要提供一个 IMU,因为它用作扫描方向的初始猜测,大大降低了扫描匹配的复杂性。

配置选项为

1
2
TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant

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