工作小笔记——使用Cartographer建图和调优
文章目录
- 前言
- 1. 前提
- 2. 运行Demo数据
- 3. 处理自己录制的数据
-
- 3.1 录制数据
- 3.2 验证数据
- 3.3 构建.lua配置文件
- 3.4 构建urdf文件
- 3.5 构建launch文件
- 3.6 运行
- 4. 参数调优
-
- 4.1 基本概念
- 4.2 输入scan数据的处理参数
-
- 4.2.1 有效距离
- 4.2.2 用于形成一个观测的scan累积数
- 4.2.2 体素滤波器大小
- 4.2.3 自适应体素滤波器的参数配置
- 4.2.4 IMU使用
- 4.3 Local SLAM的参数
-
- 4.3.1 CeresScanMatcher
- 4.3.2 RealTimeCorrelativeScanMatcher
- 4.4 Global SLAM的参数
- 5. 定位模式的参数调整
- 6. 全局优化时对里程计的使用
- 7. 使用rviz查看优化结果
- 参考文献
前言
本文讲述使用cartographer进行数据回放和参数调优的过程，属于官方文档的阅读笔记，加入了部分自己的理解。
1. 前提
- Ubuntu 18.04
- 已安装ROS Melodic
- cartographer和cartographer_ros的安装目录位于：~/carto_ws
2. 运行Demo数据
- 下载Demo数据
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
- 以backpack_2d为例，对应的launch文件的功能如下：
- backpack_2d.launch：用于使用实时数据进行SLAM操作的launch文件；
- demo_backpack_2d.launch：用于离线回放数据的launch文件，包含了backpack_2d.launch文件，并增加了rosbag回放节点和rviz节点；
- offline_backpack_2d.launch：和demo_backpack_2d.launch非常类似，但用于快速的直行SLAM，同时支持多个rosbag文件的回放；
- demo_backpack_2d_localization.launch：和demo_backpack_2d.launch非常类似，但可以选择一个.pbstream文件作为地图输入，cartographer模块在该地图上仅做定位；
- assets_writer_backpack_2d.launch：该launch文件用于把之前运行得到的pbstream数据存成不同的文件。
3. 处理自己录制的数据
3.1 录制数据
以下操作在机器人的ros系统上运行：
- 在真机上运行ros和激光雷达的驱动节点
roscore
roslaunch your_lidar_package your_lidar_package.launch
- 使用rosbag记录对应的消息
rosbag record -O my_robot.bag your_scan_topic_name
3.2 验证数据
从此处开始，以下全部操作可以都在开发电脑上运行。
首先，验证rosbag数据包，确认没有问题：
cartographer_rosbag_validate -bag_filename my_robot.bag
3.3 构建.lua配置文件
- 对于2D激光雷达数据，以demo中的backpack_2d.lua文件作为参考：
cp install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua
- 修改my_robot.lua文件，根据rosbag中TF的对应FrameID修改 map_frame, tracking_frame, published_frame和odom_frame的内容；
3.4 构建urdf文件
- 用backpack_2d.urdf文件作为参考：
cp install_isolated/share/cartographer_ros/urdf/backpack_2d.urdf install_isolated/share/cartographer_ros/urdf/my_robot.urdf
- 根据自身机器人的激光雷达在底盘上的位置，修改my_robot.urdf中的相应link定义。
3.5 构建launch文件
- 对于2D激光雷达数据，复制cartographer_ros包中提供的demo的launch文件作为参考：
cp install_isolated/share/cartographer_ros/launch/backpack_2d.launch install_isolated/share/cartographer_ros/launch/my_robot.launch
cp install_isolated/share/cartographer_ros/launch/demo_backpack_2d.launch install_isolated/share/cartographer_ros/launch/demo_my_robot.launch
cp install_isolated/share/cartographer_ros/launch/offline_backpack_2d.launch install_isolated/share/cartographer_ros/launch/offline_my_robot.launch
cp install_isolated/share/cartographer_ros/launch/demo_backpack_2d_localization.launch install_isolated/share/cartographer_ros/launch/demo_my_robot_localization.launch
cp install_isolated/share/cartographer_ros/launch/assets_writer_backpack_2d.launch install_isolated/share/cartographer_ros/launch/assets_writer_my_robot.launch
- 修改demo_my_robot.launch, offline_my_robot.launch, demo_my_robot_localization.launch和assets_writer_my_robot.launch文件，将其中的"backpack_2d.launch"修改为“my_robot.launch"；
- 修改my_robot.launch文件，将”backpack_2d.urdf"修改为"my_robot.urdf"；
- 修改my_robot.launch文件，根据rosbag文件中的LaserScan topic的名称，修改对应的remap语句。
3.6 运行
roslaunch cartographer_ros demo_my_robot.launch bag_filename:=/path/to/your_bag.bag
4. 参数调优
4.1 基本概念
Cartographer可以被视为如下两个独立但相关的子系统：
- Local SLAM（又称前端）。它的工作是构建一系列的submap。每个submap都是本地一致的，但local SLAM会随时间漂移。Local SLAM的相关参数可以参考文件：
- install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua文件（用于2D）
- install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua文件（用于3D）
- Global SLAM（又称后端）。它在后台运行，主要用于构建submap间的约束，包括回环约束，以进行非线性优化。后端还会接入其他传感器数据进行优化以形成更加一致的全局I解。Global SLAM的相关参数可以参考文件：
- install_isolated/share/cartographer/configuration_files/pose_graph.lua
4.2 输入scan数据的处理参数
根据使用的激光雷达型号的不同，进行不同的参数配置。
4.2.1 有效距离
TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range
4.2.2 用于形成一个观测的scan累积数
TRAJECTORY_BUILDER_nD.num_accumulated_range_data
4.2.2 体素滤波器大小
TRAJECTORY_BUILDER_nD.voxel_filter_size
4.2.3 自适应体素滤波器的参数配置
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points
4.2.4 IMU使用
TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant
4.3 Local SLAM的参数
Local SLAM的过程包括两部分：
- 扫描匹配（Scan Match）：通过对LaserScan的消息的匹配将Scan插入到当前子图中去
- 位置外推（Pose Extrapolator）：使用其他传感器数据进行位置外推，得到当前的估计位置，作为Scan Match的初始位置
扫描匹配（Scan Match）有两种方式：
- CeresScanMatcher
- RealTimeCorrelativeScanMatcher
4.3.1 CeresScanMatcher
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
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
4.3.2 RealTimeCorrelativeScanMatcher
当没有其他传感器，纯用激光雷达进行SLAM计算时，需要使用RealTimeCorrelative ScanMatcher来进行初始位置的估计。
- RealTimeCorrelativeScanMatcher通过在由最大距离半径和最大角度半径定义的搜索窗口中搜索相似的扫描来工作。当使用此窗口中的Scan进行扫描匹配时，可以为平移和旋转分量选择不同的权重。
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
- 为了避免每个submap插入过多的扫描，可以通过运动过滤器来过滤不需要的Scan。如果两次Scan之间的运动被认为不够重要，则会放弃该Scan。只有当超过某个距离、角度或时间的阈值时，才会将Scan插入当前submap。
TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians
- 当Local SLAM已经接收到给定量的测距数据时，会生成一个完整的submap。Local SLAM随时间漂移，而Global SLAM用于修复此漂移。因此，submap必须足够小，以使其内部的漂移低于分辨率，从而使其局部正确。另一方面，它们又应该足够大，以使Loop Closure能够正常工作（太小了容易造成错误的Loop Closure检测）。
TRAJECTORY_BUILDER_nD.submaps.num_range_data
- submap可以将其测距数据存储在两种不同的数据结构中：最广泛使用的是概率网格图。对于2D应用，也可以选择使用TSDF格式。
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type
- 概率网格将空间切割成2D或3D的表格，其中每个单元格都有固定的大小，并标记了该单元格有障碍物的概率。根据每一次的Scan“命中”（测量距离数据）和“未命中”（传感器和测量点之间的自由空间）来更新概率。命中和未命中在Occupacy概率计算中可能具有不同的权重。
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
- Scan Match算法首先对齐低分辨率点云的远点与低分辨率混合网格，然后通过将近高分辨率点与高分辨率混合网格对准来细化姿态。
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
4.4 Global SLAM的参数
- 后端的优化可以根据插入Node的数量来决定是否运行。如果把该参数设为0，则等于disable Global SLAM。对于调优过程来说，一般第一步就是将其设为0，以首先调试Local SLAM。
POSE_GRAPH.optimize_every_n_nodes
- 约束分为两种：
- 非全局约束（也称为submap内约束）是在轨迹上相互紧随的节点之间自动构建的。直觉上，这些“非全局绳索”保持了轨迹的局部结构的连贯性。
- 全局约束（也称为回环约束或submap间约束）是在新的submap和之前的node之间定期搜索，这些node被认为在空间上“足够接近”（某个搜索窗口的一部分），并且具有强匹配性（运行扫描匹配时很匹配）。直觉上，这些“全局绳索”在结构中引入了结，并牢固地将两股绳拉近。
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
- 当一个节点和一个submap被用于构建约束构建时，它们首先会经过一个名为FastCorrelativeScanMatcher的Scan Matcher。该Scan Matcher采用分支定界机制以不同的网格分辨率工作，并有效地消除不正确的匹配，大幅降低了计算复杂度，从而使得实时的Scan Matcher成为可能。分支定界的分支树的深度作为参数可配。
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根据多个残差重新排列submap。残差使用加权代价函数进行计算。全局优化使用的代价函数考虑了大量来源：全局（回环闭合）约束、非全局（匹配器）约束、IMU加速度和角速度、局部SLAM的粗略姿态估计、里程计数据或者固定点位标签（如GPS系统）。权重和Ceres选项可以按照Local 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
- 在残差计算中，异常值的影响由Huber损失函数处理，该函数配置有一定的Huber尺度。Huber尺度越大，（潜在）异常值的影响越大。
POSE_GRAPH.optimization_problem.huber_scale
- 轨迹完成后，Cartographer还会运行一个新的全局优化，通常比以前的全局优化迭代次数要多得多。这样做是为了进一步优化最终结果，该过程通常不需要实时。
POSE_GRAPH.max_num_final_iterations
5. 定位模式的参数调整
给定一张已有地图，可以进行单纯的定位操作。在定位操作下，参数配置的要求有如下改变：
- Local和Global的SLAM都需要更低的延迟，以保障快速输出当前位置
- Global SLAM通常会有大量在已有的轨迹（即已有的地图）和当前轨迹之间的约束
要进入定位模式，首先需要设置：
TRAJECTORY_BUILDER.pure_localization = true
同时，需要大幅减小POSE_GRAPH.optimize_every_n_nodes以保障结果的快速输出。在这样的设置下，Global SLAM的计算资源要求大增，可能无法满足实时要求，为此，需要大幅调低global_sampling_ratio和constraint_builder.sampling_ratio。
6. 全局优化时对里程计的使用
设置use_odometry = true，可以在Local SLAM中使用里程计。同样也可以通过配置如下几个参数来使Global SLAM也从里程计的数据获得一定收益。
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight
POSE_GRAPH.optimization_problem.odometry_translation_weight
POSE_GRAPH.optimization_problem.odometry_rotation_weight
我们可以根据我们对Local SLAM或里程计的信任程度来设置这些权重。默认情况下，在全局优化中里程计的权重和Local SLAM的位姿相同。然而，来自轮速编码器的里程计的角速度通常具有很高的不确定性。在这种情况下，旋转的权重可以减小，甚至降低到零。
7. 使用rviz查看优化结果
使用rviz可以在constraints一栏中打开或者关闭约束和残差的显示，对于可视化的参数优化很有帮助。
参考文献
- https://google-cartographer-ros.readthedocs.io/