参考
1. Cartographer 安装
1.1 前置条件
- 推荐在刚装好的 Ubuntu 16.04 或 Ubuntu 18.04 上进行编译
- ROS 安装:ROS学习1:ROS概述与环境搭建
1.2 依赖库安装
-
资源下载完解压并执行以下指令
powershell$ sudo chmod 777 auto-carto-build.sh $ ./auto-carto-build.sh
1.3 编译
本文只编译 cartographer_ros,以下为同时开三个终端操作
powershell
$ mkdir -p cartographer_ws/src
powershell
$ cd ~
$ git clone https://github.com/xiangli0608/cartographer_detailed_comments_ws
$ mv ~/cartographer_detailed_comments_ws/src/cartographer_ros ~/cartographer_ws/src
powershell
$ cd ~/cartographer_ws
$ catkin_make
2. Cartographer 运行
2.1 数据集下载
- 百度网盘链接(rslidar-outdoor-gps.bag、landmarks_demo_uncalibrated.bag)
2.2 配置文件
-
lx_rs16_2d_outdoor.launch
xml<launch> <!-- bag 的地址与名称(根据自己情况修改,建议使用绝对路径) --> <arg name="bag_filename" default="/home/yue/bag/rslidar-outdoor-gps.bag"/> <!-- 使用 bag 的时间戳 --> <param name="/use_sim_time" value="true" /> <!-- 启动 cartographer --> <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename lx_rs16_2d_outdoor.lua" output="screen"> <remap from="points2" to="rslidar_points" /> <remap from="scan" to="front_scan" /> <remap from="odom" to="odom_scout" /> <remap from="imu" to="imu" /> </node> <!-- 生成 ros 格式的地图 --> <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05" /> <!-- 启动 rviz --> <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/lx_2d.rviz" /> <!-- 启动 rosbag --> <node name="playbag" pkg="rosbag" type="play" args="--clock $(arg bag_filename)" /> </launch>
-
lx_rs16_2d_outdoor.lua
luainclude "map_builder.lua" include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, -- map_builder.lua的配置信息 trajectory_builder = TRAJECTORY_BUILDER, -- trajectory_builder.lua的配置信息 map_frame = "map", -- 地图坐标系的名字 tracking_frame = "imu_link", -- 将所有传感器数据转换到这个坐标系下 published_frame = "footprint", -- tf: map -> footprint odom_frame = "odom", -- 里程计的坐标系名字 provide_odom_frame = false, -- 是否提供odom的tf,如果为true,则tf树为map->odom->footprint -- 如果为false tf树为map->footprint publish_frame_projected_to_2d = false, -- 是否将坐标系投影到平面上 --use_pose_extrapolator = false, -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿 use_odometry = false, -- 是否使用里程计,如果使用要求一定要有odom的tf use_nav_sat = false, -- 是否使用gps use_landmarks = false, -- 是否使用landmark num_laser_scans = 0, -- 是否使用单线激光数据 num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据 num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1 num_point_clouds = 1, -- 是否使用点云数据 lookup_transform_timeout_sec = 0.2, -- 查找tf时的超时时间 submap_publish_period_sec = 0.3, -- 发布数据的时间间隔 pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., -- 传感器数据的采样频率 odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true TRAJECTORY_BUILDER_2D.use_imu_data = true TRAJECTORY_BUILDER_2D.min_range = 0.3 TRAJECTORY_BUILDER_2D.max_range = 100. TRAJECTORY_BUILDER_2D.min_z = 0.2 --TRAJECTORY_BUILDER_2D.max_z = 1.4 --TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02 --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200. --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9 --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100 --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50. TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1. TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1. TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1. --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12 --TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1 --TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004 --TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1. TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80. TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 POSE_GRAPH.optimize_every_n_nodes = 160. POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 POSE_GRAPH.constraint_builder.max_constraint_distance = 15. POSE_GRAPH.constraint_builder.min_score = 0.48 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60 return options
-
trajectory_builder_2d.lua
luaTRAJECTORY_BUILDER_2D = { use_imu_data = true, -- 是否使用imu数据 min_range = 0., -- 雷达数据的最远最近滤波, 保存中间值 max_range = 30., min_z = -0.8, -- 雷达数据的最高与最低的过滤, 保存中间值 max_z = 2., missing_data_ray_length = 5., -- 超过最大距离范围的数据点用这个距离代替 num_accumulated_range_data = 1, -- 几帧有效的点云数据进行一次扫描匹配 voxel_filter_size = 0.025, -- 体素滤波的立方体的边长 -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器 -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配 adaptive_voxel_filter = { max_length = 0.5, -- 尝试确定最佳的立方体边长, 边长最大为0.5 min_num_points = 200, -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数 max_range = 50., -- 距远离原点超过max_range 的点被移除 }, -- 闭环检测的自适应体素滤波器, 用于生成稀疏点云 以进行 闭环检测 loop_closure_adaptive_voxel_filter = { max_length = 0.9, min_num_points = 100, max_range = 50., }, -- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息 -- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果 use_online_correlative_scan_matching = false, real_time_correlative_scan_matcher = { linear_search_window = 0.1, -- 线性搜索窗口的大小 angular_search_window = math.rad(20.), -- 角度搜索窗口的大小 translation_delta_cost_weight = 1e-1, -- 用于计算各部分score的权重 rotation_delta_cost_weight = 1e-1, }, -- ceres匹配的一些配置参数 ceres_scan_matcher = { occupied_space_weight = 1., translation_weight = 10., rotation_weight = 40., ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 20, num_threads = 1, }, }, -- 为了防止子图里插入太多数据, 在插入子图之前之前对数据进行过滤 motion_filter = { max_time_seconds = 5., max_distance_meters = 0.2, max_angle_radians = math.rad(1.), }, -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., -- 位姿预测器 pose_extrapolator = { use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, imu_based = { pose_queue_duration = 5., gravity_constant = 9.806, pose_translation_weight = 1., pose_rotation_weight = 1., imu_acceleration_weight = 1., imu_rotation_weight = 1., odometry_translation_weight = 1., odometry_rotation_weight = 1., solver_options = { use_nonmonotonic_steps = false; max_num_iterations = 10; num_threads = 1; }, }, }, -- 子图相关的一些配置 submaps = { num_range_data = 90, -- 一个子图里插入雷达数据的个数的一半 grid_options_2d = { grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式 resolution = 0.05, }, range_data_inserter = { range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", -- 概率占用栅格地图的一些配置 probability_grid_range_data_inserter = { insert_free_space = true, hit_probability = 0.55, miss_probability = 0.49, }, -- tsdf地图的一些配置 tsdf_range_data_inserter = { truncation_distance = 0.3, maximum_weight = 10., update_free_space = false, normal_estimation_options = { num_normal_samples = 4, sample_radius = 0.5, }, project_sdf_distance_to_scan_normal = true, update_weight_range_exponent = 0, update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5, update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5, }, }, }, }
-
pose_graph.lua
luaPOSE_GRAPH = { -- 每隔多少个节点执行一次后端优化 optimize_every_n_nodes = 90, -- 约束构建的相关参数 constraint_builder = { sampling_ratio = 0.3, -- 对局部子图进行回环检测时的计算频率, 数值越大, 计算次数越多 max_constraint_distance = 15., -- 对局部子图进行回环检测时能成为约束的最大距离 min_score = 0.55, -- 对局部子图进行回环检测时的最低分数阈值 global_localization_min_score = 0.6, -- 对整体子图进行回环检测时的最低分数阈值 loop_closure_translation_weight = 1.1e4, loop_closure_rotation_weight = 1e5, log_matches = true, -- 打印约束计算的log -- 基于分支定界算法的2d粗匹配器 fast_correlative_scan_matcher = { linear_search_window = 7., angular_search_window = math.rad(30.), branch_and_bound_depth = 7, }, -- 基于ceres的2d精匹配器 ceres_scan_matcher = { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, num_threads = 1, }, }, -- 基于分支定界算法的3d粗匹配器 fast_correlative_scan_matcher_3d = { branch_and_bound_depth = 8, full_resolution_depth = 3, min_rotational_score = 0.77, min_low_resolution_score = 0.55, linear_xy_search_window = 5., linear_z_search_window = 1., angular_search_window = math.rad(15.), }, -- 基于ceres的3d精匹配器 ceres_scan_matcher_3d = { occupied_space_weight_0 = 5., occupied_space_weight_1 = 30., translation_weight = 10., rotation_weight = 1., only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 10, num_threads = 1, }, }, }, matcher_translation_weight = 5e2, matcher_rotation_weight = 1.6e3, -- 优化残差方程的相关参数 optimization_problem = { huber_scale = 1e1, -- 值越大,(潜在)异常值的影响就越大 acceleration_weight = 1.1e2, -- 3d里imu的线加速度的权重 rotation_weight = 1.6e4, -- 3d里imu的旋转的权重 -- 前端结果残差的权重 local_slam_pose_translation_weight = 1e5, local_slam_pose_rotation_weight = 1e5, -- 里程计残差的权重 odometry_translation_weight = 1e5, odometry_rotation_weight = 1e5, -- gps残差的权重 fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_use_tolerant_loss = false, fixed_frame_pose_tolerant_loss_param_a = 1, fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = false, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 50, num_threads = 7, }, }, max_num_final_iterations = 200, -- 在建图结束之后执行一次全局优化, 不要求实时性, 迭代次数多 global_sampling_ratio = 0.003, -- 纯定位时候查找回环的频率 log_residual_histograms = true, global_constraint_search_after_n_seconds = 10., -- 纯定位时多少秒执行一次全子图的约束计算 -- overlapping_submaps_trimmer_2d = { -- fresh_submaps_count = 1, -- min_covered_area = 2, -- min_added_submaps_count = 5, -- }, }
2.3 运行演示
powershell
$ source devel/setup.bash
$ roslaunch cartographer_ros lx_rs16_2d_outdoor.launch
3. Cartographer 调参总结
3.1 降低延迟与减小计算量
-
前端
- 减小 max_range 即减小了需要处理的点数,在激光雷达远距离的数据点不准时一定要减小这个值
- 增大 voxel_filter_size,相当于减小了需要处理的点数
- 增大 submaps.resolution,相当于减小了匹配时的搜索量
- 对于自适应体素滤波 减小 min_num_points 与 max_range,增大 max_length,相当于减小了需要处理的点数
-
后端
- 减小 optimize_every_n_nodes, 降低优化频率, 减小了计算量
- 增大 MAP_BUILDER.num_background_threads, 增加计算速度
- 减小 global_sampling_ratio, 减小计算全局约束的频率
- 减小 constraint_builder.sampling_ratio, 减少了约束的数量
- 增大 constraint_builder.min_score, 减少了约束的数量
- 减小分枝定界搜索窗的大小, 包括linear_xy_search_window,inear_z_search_window, angular_search_window
- 增大 global_constraint_search_after_n_seconds, 减小计算全局约束的频率
- 减小 max_num_iterations, 减小迭代次数
3.2 降低内存
- 增大子图的分辨率 submaps.resolution
3.3 常调的参数
cpp
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2 -- / -0.8
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 -- / 0.02
POSE_GRAPH.optimize_every_n_nodes = 160. -- 2倍的num_range_data以上
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60
4. Cartographer 工程化建议
4.1 如何提升建图质量
- 选择频率高(25 Hz 以上)、精度高的激光雷达
- 如果只能用频率低的激光雷达
- 使用高频、高精度 IMU,且让机器人运动慢一点
- 调 ceres 的匹配权重,将地图权重调大,平移旋转权重调小
- 将代码中平移和旋转的残差注释掉
- 里程计
- Cartographer 中对里程计的使用不太好
- 可以将 karto 与 GMapping 中使用里程计进行预测的部分拿过来进行使用,改完后就可达到比较好的位姿预测效果
- 粗匹配
- 将 karto 的扫描匹配的粗匹配放过来,karto 的扫描匹配的计算量很小,当做粗匹配很不错
- 地图
- 在最终生成地图的时候使用后端优化后的节点重新生成一次地图,这样生成的地图效果会比前端地图的叠加要好很多
4.2 降低计算量与内存
- 体素滤波与自适应体素滤波的计算量(不是很大)
- 后端进行子图间约束时的计算量很大
- 分支定界算法的计算量很大
- 降低内存,内存的占用基本就是多分辨率地图,每个子图的多分辨率地图都进行保存是否有必要
4.3 纯定位的改进建议
- 将纯定位模式与建图拆分开,改成读取之前轨迹的地图进行匹配.
- 新的轨迹只进行位姿预测,拿到预测后的位姿与之前轨迹的地图进行匹配,新的轨迹不再进行地图的生成与保存,同时将整个后端的功能去掉.
- 去掉后端优化会导致没有重定位功能,这时可将 cartographer 的回环检测(子图间约束的计算)部分单独拿出来,做成一个重定位功能,通过服务来调用这个重定位功能,根据当前点云确定机器人在之前地图的位姿
4.4 去 ros 的参考思路
- 仿照 cartographer_ros 里的操作:获取到传感器数据,将数据转到 tracking_frame 坐标系下并进行格式转换,再传入到 cartographer 里就行