4.算法调试

Posted 2021-02-10 gary-guo

cartographer是一个复杂的系统，调试需要对内部工作有很好的理解。下面将给出cartographer所使用的子系统以及不同的配置值的一个直观的概览。如果想进一步了解cartographer需要参考cartographer的论文。但论文中之定义了2D slam，大部分概念在这里都有介绍，且在3D slam中概念类似。

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的大多数选项可以在install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua for 2D and install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua for 3D中找到。

另一个子系统是全局SLAM（有时称为后端）。它在内部进程中运行，且其主要工作是找到回环约束。它是通过scan-matching的方法用节点中收集到的scans与子地图进行匹配。它也融合其他传感器数据来获得一个更高水平的view以及验证全局解决方式的一致性。在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用TRAJECTORY_BUILDER_2D.missing_data_ray_length来替代了max_range。它也提供了max_z和min_z值来3d点云到2d。在cartographer的配置文件中，每一个距离单位都是m。

当机器人在实际移动的时候，距离测量是在某一个时间段内的。然而，距离值的传递是以ROS格式通过传感器捆绑的方式来实现的，cartographer会独立地把每一个信息的时间戳考虑进去，同时也会考虑由于机器人的运动而带来的畸变。cartographer获取的测量越频繁，它解决测量的畸变来组装成一个内部紧相关的scan就越好，该scan可能是立即被获取的。因此强烈建议在一帧scan中提供尽可能多的距离数据（ROS messages的形式）,

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，距离数据可以被实时处理而不需要另外的信息源，因此可以选择是否使用IMU，对于3D SLAM，必须提供一个IMU，因为它提供了激光方向的一个初始猜测值，巨大地减少了scan matching的复杂度,

TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant

注意，在cartographer配置文件中，时间的单位是s。

局部SLAM

一旦把多个距离传感器的一帧激光数据被组装且滤波后，就可以用于局部SLAM算法。局部slam通过scan-matching来插入一帧激光到当前子地图中，scan-matching是使用pose extrapolate来估计该帧激光的初始位姿。在pose extrapolate背后的想法是使用除了距离传感器之外的传感器数据来预测下一帧激光在将要插入的子地图的那个位置。

有两个scan matching 策略可使用：

1.ceresscanMatcher将初始猜测值作为先验信息，并找到最佳的点，该点就是通过scan match获得的在子地图中的位置，是通过插值子地图和与激光亚像素对齐的方式实现。这种方式快，但是不能修复错误，该错误会比子地图的分辨率要大。如果传感器装置和时间是合理的，只使用ceresscanmatcher是最佳选择。

2.RealTimeCorrelativeScanMatcher可以被使能，如果没有其他传感器或者你不信任其他传感器。它使用类似于在回环检测中激光与子地图匹配的方式，但是它是与当前子地图进行匹配。最佳匹配会用于ceresScanMatcher的先验值。这个scan matcher是非常耗时的，它本质上会覆盖其他传感器的信息除了激光传感器，但在特征点丰富的地方非常鲁邦。

CeresScanMatcher可以配置对每一个输入给定某一个权重。该权重是对数据的一个trust，这可以被看做一个静态的协方差。权重参数的单位是一个无单位的量，彼此之间不能相互比较。源数据的权重越大，当做scan matching的时候cartographer就越重视该数据源。数据源包括了占据的空间（来自于一帧激光的点），平移和旋转来自于pose extrapolator(或者 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 and occupied_space_weight_1参数是相关的，分别对应滤过波点云的高分辨率和低分辨率。

CeresScanMatcher的名字来自于Ceres Solver,该库来自于google用于解决非线性最小二乘问题.扫描匹配问题被模拟为最小化这样一个问题，该问题是在两帧激光之间的运动，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可以被拴牢通过依靠你对传感器的信任度。它的工作方式是在一个搜索window里面寻找类似的激光，该搜索window通过一个最大的距离半径和一个最大的角度半径来定义。当在window里面执行scan matching的时候，对于平移和旋转部分不同的权重可以被选择。如果你直到你的机器人旋转不多，可以适当修改权重。

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

在每个子地图中为了避免插入太多的激光帧，由scan matcher找到的一个运动之间的两帧激光，其进行运动滤波。如果由于运动而导致一帧激光丢掉，这并不是那么重要。如果机器人的运动有一定距离，角度或者时间阈值，那么一帧激光就被插入到当前的子地图中。

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用于修复这个漂移。子地图必须足够小以便于在子地图内部的漂移低于分辨率，以至于他们是局部正确的。另一方面，子地图应当足够大到比较明显，对于回环检测可以正常工作，

TRAJECTORY_BUILDER_nD.submaps.num_range_data

子地图可以存储他们的距离数据用不同的数据结构：最常用的表示方式成为概率栅格，然而，在2D中，也可以选择使用Truncated Signed Distance Fields (TSDF).

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

概率栅格把空间剪切成一个2D或者3D的表格，该表格的每一个cell有一个固定的大小，并包含了被构造的可能性。可能性是根据hits和misses来进行更新，hits和misses二者在占据概率计算中可能有不同的权重，该占据概率计算或多或少相信占据或者自由空间的测量，

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中，由于scan matching性能的原因，两个混合的概率栅格被使用。(the term “hybrid” only refers to an internal tree-like data representation and is abstracted to the user)

a、对于远距离测量，一个低分辨率的混合栅格；

b、对于近距离测量，一个高分辨率的混合栅格；

scan matching先使用低分辨率的点云来与低分辨率的栅格进行对齐，然后在精细位姿，使用高分辨率的点云与高分辨率的栅格进行比对，

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

全局SLAM

尽管局部slam生成了连续的子地图，但是一个全局优化（通常称为优化问题或者sparse pose adjustment）任务在后台运行，它的角色是去重新整理子地图，以便他们可以内联成一个全局地图。例如，这个优化负责改变当前建立的轨迹来适应于回环检测的子地图对齐。

一旦一定数量的轨迹节点被插入，优化任务被成批地执行。取决于你需要多快运行它，可以调试这些批次的大小，

POSE_GRAPH.optimize_every_n_nodes

把POSE_GRAPH.optimize_every_n_nodes设置为0，是手动关闭全局SLAM，主要精力集中于局部SLAM。这是调试cartographer的第一件事情。

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

约束可以在RVIZ中可视化，调试全局地图特别方便，可以拴牢POSE_GRAPH.constraint_builder.log_matches来获得作为直方图格式的约束构建器的常规报告。

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

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

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

注意，实际上，全局约束不仅仅是在单一的轨迹上寻找回环检测。