调试 Cartographer Laser-SLAM -- Cartographer 算法流程

Cartographer Algorithm walkthrough for tuning

本文章有后续更新版本，csdn排版和插入图片有点麻烦，我更新在我的个人站点了，欢迎阅读！

@author 薛轲翰

@email1 [e-mail]

@email2 [e-mail]

本文参考cartographer官方文档.

cartographer基本思路简介

cartographer基本思路简介

cartographer的主要贡献不是算法,而是工程的实现

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-5RRJBw89-1607313168606)(/home/kehan/Documents/V5PP文档/技术文档/Cartographer/Cartographer_Algorithm_Walkthrough_for_Tuning/Cartographer_Algorithm_Walkthrough_for_Tuning-pics/high_level_system_overview.png)]

cartographer 可以看做两个独立但相关的子系统, Local SLAM 和 Global SLAM.Local SLAM可以被看做是前端(local trajectory builder), 任务是建立一个个的submap. 各个submap是本地一致的, 但是会慢慢的漂移.Golbal SLAM是后端, 用于作 loop closure constraints(回环).通过scans(集合成nodes)与submaps进行matching的方法来工作.并且可以结合其他传感器的数据来进一步提高精度,来确定一致性最强的Global optimization 方案. 3D SLAM中还会使用加速度计提供的重力的方向.

总的来说,Local SLAM 的主要工作是生成更好的 submaps, Global SLAM的工作是将这些 submaps 更好的结合起来.

其中,Local SLAM的配置文件主要为 trajectory_builder_2d.lua 和 trajectory_builder_3d.lua. Global SLAM的配置文件为pose_graph.lua

Input Process

首先是雷达扫描数据的距离范围.参数为下面两个,单位为m

TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range

如果将 3D 雷达用在 2D SLAM的话,提供一个"截断"的参数,就是把一定高度范围内的扫描点映射到 2D 的一个平面上.

TRAJECTORY_BUILDER_2D.max_z
TRAJECTORY_BUILDER_2D.min_z

如果 scan 中的点的 range 大于 max_range 时,会在 max_range 外面生成一个长度为TRAJECTORY_BUILDER_2D.missing_data_ray_length的空白区域.(?不太明白)

TRAJECTORY_BUILDER_nD.num_accumulated_range_data的作用及意义比较麻烦,会在下文range data标题下说到.

较近的表面(如路面)经常扫描得到更多的points,而远处的物体的points经常比较稀少. 为了降低计算量, 需要对点云数据进行下采样, 简单的随机采样仍然会导致低密度区的点更少,而高密度区的点仍然比较多.因此cartographer 采用 voxel_filter (体素滤波)的方法. 即将raw points(如何定义raw points? TODO) 为中心做一个边长为TRAJECTORY_BUILDER_nD.voxel_filter_size的立方体,然后只取立方体的形心.如果立方体较小的话会导致更密集的数据,而较大的话可能会导致数据丢失但是速度会更快.

不仅使用了定大小的 voxel_filter, Cartographer还用了 adaptive_voxel_filter, 该 adaptive_voxel_filter 可以在最大长度TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length的限制下确定最佳的voxel_filter_size来实现目标的points数TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points .

关于IMU数据的作用以及是否使用IMU数据的问题

IMU 数据用于观测精确的重力方向以及机器人的姿态.IMU 数据噪声较大但是整体的方向还是正确的.cartographer 内部有一套它自己处理IMU数据的流程.cartographer 并未使用 sensor_msgs/Imu 消息中的四元数信息,而是直接使用三轴线加速度与角速度.由于 2D SLAM 可以做到无外界信息来源而实时处理数据,所以 2D SLAM 可以选择是否使用 IMU 的消息.但是在 3D SLAM 中需要提供 IMU 数据作为扫描方向的初始猜测,可以大大降低 scan 匹配的复杂性.

cartographer中关于时间的参数单位均为s(秒).

TRAJECTORY_BUILDER_nD.use_imu_data = true
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant = 10.0

下文还有 imu 数据在 Global optimization 中的应用.

cartographer两种match策略

CeresScanMatcher 以及 RealTimeCorrelativeScanMatcher

• CeresScanMatcher把 initial guess 当做先验,寻找 submap 与当前的 scan 最匹配的位置.通过对 submap 进行插值然后与 scan 进行对齐.这种做法比较快速,但是无法修复远大于子图分辨率的误差.如果你的传感器的设置与 timing(?) 都是合理的话,仅仅使用 CeresScanMatcher 通常是最好的选择. CeresScanMatcher 可以为每一个输入配置一个权重,权重就是对其数据的信任度,可以把它视为静态协方差.数据来源可以包括 space(scan points),平移(translation)与旋转(rotation).

  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
  

• RealTimeCorrelativeScanMatcher 在你比较不信任你的其他传感器的情况下可以启用.它的做法类似于回环检测中的做法将 scan 与当前 submap 进行 match.Best match 然后被用作 CeresScanMatcher 的先验.这种 match 的方式对计算资源要求较高,并且会几乎覆盖其他所有传感器的作用.但这种做法在 feature rich 的环境中的鲁棒性非常好.同样 RealTimeCorrelativeScanMatcher 也可以根据对 sensors 的信任度进行切换(即可以配置不同权重/置信度/weight).它的工作原理是滑动窗口(搜索窗口的大小由 a maximum distance radius and a maximum angle radius)定义.在此窗口中进行 scan match 的时候,可以为 translation 和 rotation 选择不同的权重.例如当已知机器人不会旋转过多的话,就可以改变对应 weight.

  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
  

Ceres-Solver in cartographer

scan match 被建模为非线性最小二乘问题,用 google 自家的 Ceres-Slover 来求解. CeresScanMatcher 过程中迭代步长,迭代次数及线程数均可以通过参数配置.

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

motion_filter

为避免将过多的 scan 插入到 submap 里,当两个 scan 成功 match 后,会得到两个 match 之间的运动关系.当两者运动关系不明显的话,这个 match 结果就不会被插入到 submap 中去.这个操作通过运动滤波器中卡time,distance以及angle的阈值来实现.

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

关于range data的理解

看了文档对这个概念的理解还是比较模糊.其中有两个参数与它直接关系比较大,分别是 num_accumulated_range_data 和 num_range_data. range_data 可以理解为一组雷达扫描出的距离数据.拿 VLP 举例,设定 vlp16 的转速为20Hz,但是 vlp16 回传的数据的频率远大于旋转的频率.一次 udp 包回传的可以看做的扫描一圈的点云中的一段扫描距离的子点云. num_accumulated_range_data 参数的作用就是累计 num_accumulated_range_data 个子点云然后合成一个大的点云用于 scan match.并且 merge 的时候还根据(速度不变性?)以及 IMU 等的数据估计出运动量,处理了 merge 时由于运动造成的误差.
num_range_data 与 submap 的生成直接相关.(可以看做 num_range_data 个 range_data 生成一个submap).submap 的大小的选择也比较重要.submap 需要足够小以使得其内部的漂移低于 submap 的分辨率.但又需要足够的大以使得 loop closure 可以正常的工作.

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
TRAJECTORY_BUILDER_nD.submaps.num_range_data

附: PointCloud2消息类型的数据格式

官方点云数据格式

点云数据格式解释

手动解析点云数据时不可以直接解析，因为在field里是以二进制方式存储的，可以通过ros包里的工具来进行解析

附: 关于TRAJECTORY_BUILDER_nD.num_accumulated_range_data参数的作用与意义

cartographer官方关于调参的文档对这个TRAJECTORY_BUILDER_nD.num_accumulated_range_data参数的介绍感觉不太清楚.后来在cartographer文档最后的提问区中的第一个回答对这个参数的解释就比较明了了.

Submap

Submap 可以采用不止一种的数据结构来存储.但是一般都是采用的概率栅格地图的方式来存储.cartographer 的2D SLAM采用一个概率栅格地图.3D SLAM由于 scan match 的性能原因,维护两个hybrid(?) grid,一个是用于远距离测量的低分辨率 hybrid grid,另一个是用于近距离测量的高分辨率 hybrid grid.scan match 首先将远点云与低分辨率 hybrid grid 对齐,然后通过高分辨率近点与高分辨率 hybrid grid对齐来细化得到 pose.
栅格地图 hit 和 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
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

关闭Global SLAM

刚开始调试 cartographer 的时候首先要先关闭 Global SLAM.方法是将POSE_GRAPH.optimize_every_n_nodes 置零.

Global SLAM

Global SLAM 采用的是图优化的方式.“GraphSLAM”.建立轨迹点 node 与 submaps 之间的约束.约束在 Rviz里可以可视化,调试 Global SLAM 非常的方便(具体怎么做后续待定).

POSE_GRAPH.optimize_every_n_nodes

约束分为非全局约束与全局约束.非全局约束也被称作(也被称作是 inter submaps constraints)."离得近"的 nodes 会自动构建它们之间的约束,来一旦 trajectory 完成后, Cartographer 经常会运行一个新的全局优化,迭代的次数通常比之前的要多得多.这样做的原因是:尽可能的去优化最终建图的效果,并且通常并没有实时性的要求.所以选择大量的迭代是正确的选择.使得轨迹的局部结构保持连贯.

Global constraints in Global SLAM

全局约束(也被称作是loop closure constraints或者intra submaps contraints)的运行:通常是在一个新的new submap和之前的nodes之间进行搜索,空间(类似于滑动窗)上足够近以及一个极强的scan match.直观的来讲,相当于在两个绳子(约束)之间打一个结点(使得两个约束更紧密).

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
POSE_GRAPH.constraint_builder.sampling_ratio

全局约束不仅仅可以在一个轨迹上进行 loop closures,还可以对齐多个机器人的多个轨迹.在 global localization 中提到.
为了降低计算量, cartographer 对这些节点进行了下采样.通过POSE_GRAPH.constraint_builder.sampling_ratio参数控制.采样结果过少的话会导致可能错过约束而使得闭环检测失效.过多的话会导致全局 SLAM 的运行效率过低而达不到实时的闭环检测.

FastCorrelativeScanMatcher

用于建立一个 node 与一个 submap 之间的约束.该 scan matcher 可以实现实时的回环检测.依靠 “Branch and bound” 机制(?在 cartographer 的 paper 里有详细介绍,待补充),使得其可以在不同分辨率的栅格上进行工作,并且可以有效的消除误匹配.这个机制适用于深度可控的搜索树.
FastCorrelativeScanMatcher 的 score 大于设定的 POSE_GRAPH.constraint_builder.min_score 时,就会被送进 CeresScanMatcher 以改进 pose 估计.

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_matcInputher_3d.full_resolution_depth

POSE_GRAPH.optimization_problem

当 Cartographer 运行优化问题的时候, Cartographer 会通过多个 residuals 来对 submap 重新排序.Residuals 根据 weighted cost functions 来计算. cost functions 结合了大量的数据源:全局(回环)约束,非全局(scan match)的约束,IMU 的加速度及旋转信息的测量,local SLAM 的粗略的 pose 估计,外部的odometry 信息,以及fixed frame(如GPS).关于 weight 的 described 的描述和 Local SLAM 中的 option 相同.

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

全局优化时对于IMU数据的处理

Global optimization 对 imu 的 pose 信息提供了更多的灵活性.默认的 Ceres 会优化你 IMU 和 tracking frame 之间的外参矩阵.如果你不信任你的 imu 的数据的话, Ceres’ global optimization 的结果可以被 logged 来用来优化它俩之间的外参矩阵.如果 Ceres 不能够很好的优化 IMU 的 pose (它俩之间的外参矩阵)或者你非常信任你校准的它俩之间的外参矩阵的话,可以使用自己标定的外参矩阵并使其保持不变. (?If Ceres doesn’t optimize your IMU pose correctly and you trust your extrinsic calibration enough, you can make this pose constant.)

POSE_GRAPH.optimization_problem.log_solver_summary
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d

Huber loss

(未理解, TODO)
在residuals中,异常值的影响被 Huber loss function 所处理,受参数 huber_scale 所影响.huber_scale 越大,(潜在)异常值的影响就越大.

POSE_GRAPH.optimization_problem.huber_scale

最终的全局优化

一旦 trajectory 完成后, Cartographer 经常会运行一个新的全局优化,迭代的次数通常比之前的要多得多.这样做的原因是:尽可能的去优化最终建图的效果,并且通常并没有实时性的要求.所以选择大量的迭代是正确的选择.

POSE_GRAPH.max_num_final_iterations

Date 2019/04/07