Cartographer参数有很多,主要分为前端、后端和ROS等的基本配置。Ubuntu20.04版本的Cartographer安装流程可以参考:【安装学习】安装Cartographer ROS(noetic)_ros-noetic-cartographer-CSDN博客
安装完后就可以运行测试代码了。作者建的图如下:
前端:
前端代码在工作空间/src/cartographer/configuration_files:
-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
-- http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.
TRAJECTORY_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,
--当没有其他传感器,纯用激光雷达进行SLAM计算时,需要使用RealTimeCorrelative ScanMatcher来进行初始位置的估计。
--RealTimeCorrelative ScanMatcher通过在由最大距离半径和最大角度半径定义的搜索窗口中搜索相似的扫描来工作。当使用此窗口中的Scan进行扫描匹配时,可以为平移和旋转分量选择不同的权重。
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匹配的一些配置参数
-- 这个参数的含义是是否在扫描匹配前面先进行一次位姿计算,如果设置为true会很大的提高计算量,如果使用单线雷达数据建图时建图效果不好,总是叠图的话可以打开这个参数,可以优化建图效果。但是会付出一定的计算量。
ceres_scan_matcher = {
occupied_space_weight = 1., -- 云与地图匹配的权重
translation_weight = 10., -- 匹配的位姿与先验位姿的平移权重
rotation_weight = 40., -- 匹配的位姿与先验位姿的旋转权重
ceres_solver_options = { -- ceres内部的一些参数
use_nonmonotonic_steps = false,
max_num_iterations = 20,
num_threads = 1,
},
},
-- 为了防止子图里插入太多数据, 在插入子图之前之前对数据进行过滤
-- 为了避免每个submap插入过多的scan,可以通过运动过滤器来过滤不需要的Scan。如果两次Scan之间的运动被认为不够重要,则会放弃该Scan。只有当超过某个距离、角度或时间的阈值时,才会将Scan插入当前submap。
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,
-- 当Local SLAM已经接收到给定量的测距数据时,会生成一个完整的submap。Local SLAM随时间漂移,而Global SLAM用于修复此漂移。因此,submap必须足够小,以使其内部的漂移低于分辨率,从而使其局部正确。另一方面,它们又应该足够大,以使Loop Closure能够正常工作(太小了容易造成错误的Loop Closure检测)。
grid_options_2d = {
grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
resolution = 0.05, -- 地图的单位珊格大小 0.05m
},
range_data_inserter = {
range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
-- 概率占用栅格地图的一些配置
-- 概率网格将空间切割成2D或3D的表格,其中每个单元格都有固定的大小,并标记了该单元格有障碍物的概率。根据每一次的Scan“命中”(测量距离数据)和“未命中”(传感器和测量点之间的自由空间)来更新概率。命中和未命中在Occupacy概率计算中可能具有不同的权重。
probability_grid_range_data_inserter = {
insert_free_space = true,
hit_probability = 0.55, -- 被击中的概率,其值应大于0.5
miss_probability = 0.49, -- 未击中的概率,其值应小于0.5
},
-- 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,
},
},
},
}
后端:
后端代码在工作空间/src/cartographer/configuration_files:
-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
-- http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.
POSE_GRAPH = {
optimize_every_n_nodes = 90, -- 后端的优化可以根据插入Node的数量来决定是否运行。如果把该参数设为0,则等于disable Global SLAM。对于调优过程来说,一般第一步就是将其设为0,以首先调试Local SLAM
-- 约束构建器
constraint_builder = {
-- 为了限制约束的数量,以限制计算量,Cartographer从靠近的节点找一个子采样集来构建约束,这由采样率常数控制。采样节点太少可能会导致缺少约束和无效的回环。采样过多的节点则会降低全局SLAM的速度,无法完成实时的回环闭合。
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,
-- 当一个节点和一个submap被用于构建约束构建时,它们首先会经过一个名为FastCorrelativeScanMatcher的Scan Matcher。该Scan Matcher采用分支定界机制以不同的网格分辨率工作,并有效地消除不正确的匹配,大幅降低了计算复杂度,从而使得实时的Scan Matcher成为可能。分支定界的分支树的深度作为参数可配。
fast_correlative_scan_matcher = {
linear_search_window = 7., -- 线性搜索窗口
angular_search_window = math.rad(30.), -- 角度搜索窗口
branch_and_bound_depth = 7, -- 分支定界的分支树的深度
},
-- 一旦FastCorrelativeScanMatcher的结果超过某一限定的最小匹配分值,该结果被送到Ceres Scan Matcher以进一步优化位姿。
ceres_scan_matcher = {
occupied_space_weight = 20., -- 出现空间的权重
translation_weight = 10., -- 平移权重
rotation_weight = 1., -- 旋转权重
-- ceres解决器的参数配置
ceres_solver_options = {
use_nonmonotonic_steps = true,
max_num_iterations = 10,
num_threads = 1,
},
},
-- 一旦FastCorrelativeScanMatcher的结果超过某一限定的最小匹配分值,该结果被送到Ceres Scan Matcher以进一步优化位姿。
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_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, -- 在残差计算中,异常值的影响由Huber损失函数处理,该函数配置有一定的Huber尺度。Huber尺度越大,(潜在)异常值的影响越大。
acceleration_weight = 1.1e2, -- 加速度权重
rotation_weight = 1.6e4, -- 旋转权重
local_slam_pose_translation_weight = 1e5, -- 局部slam平移姿态权重
local_slam_pose_rotation_weight = 1e5, -- 局部slam旋转姿态权重
odometry_translation_weight = 1e5, -- 里程计平移权重
odometry_rotation_weight = 1e5, -- 里程计旋转权重
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,
},
},
-- 轨迹完成后,Cartographer还会运行一个新的全局优化,通常比以前的全局优化迭代次数要多得多。这样做是为了进一步优化最终结果,该过程通常不需要实时。
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,
-- },
}
基本配置:
基本配置代码在工作空间/src/cartographer_ros/cartographer_ros/configuration_files:
-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
-- http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map", --为 cartographer中使用的全局坐标系,也是位姿的父坐标系,通常为map。
tracking_frame = "base_link", --tracking_frame指的是由SLAM算法追踪的ROS坐标系的ID,如果在使用IMU的情况下,也可以(也最好)替换成“imu_link”。
published_frame = "odom", --cartographer正在发布pose的坐标,一般就是小车
odom_frame = "odom", --cartographer的里程计坐标系
provide_odom_frame = false, -- cartographer是否发布里程计坐标。当 bag 或者机器人中本身存在 odom 坐标系, 而 provide_odom_frame 又设置成了 true, 就会导致 odom 坐标系重复发布,会导致机器人位姿发生来回的跳动。
publish_frame_projected_to_2d = false, --是否无滚动、俯仰或z偏移
use_pose_extrapolator = false, --是否使用位姿推测器。这个参数的含义是发布TF时使用位姿推测器推测出来的位姿还是使用算法计算出来的位姿,那一般肯定是算法计算出来的更为准确。推测出来的会有误差。
use_odometry = true, -- 是否使用里程计,如果使用要求一定要有odom的tf
use_nav_sat = false, -- 是否使用gps
use_landmarks = false, -- 是否使用landmark
--num_laser_scans、num_multi_echo_laser_scans、num_point_clouds三者不能同时为0
num_laser_scans = 1, -- 是否使用单线激光数据并设置订阅topic的数量
num_multi_echo_laser_scans = 0, -- 是否订阅多线激光雷达的数据
num_subdivisions_per_laser_scan = 1, -- 将每个接收到的(多回波)激光扫描分成的点云数,说白了就是把激光一帧的数据拆分成几次发出来,正常为1即可。
num_point_clouds = 0, -- 是否使用16线点云数据
lookup_transform_timeout_sec = 0.2, -- 这个指使用tf2查找变换的超时秒数,默认都为0.2s。
submap_publish_period_sec = 0.3, -- 这个指发布submap的间隔,默认为 0.3s。
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., -- imu消息的固定比率采样。
landmarks_sampling_ratio = 1., -- landmark消息的固定比率采样。
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 50
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 10.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 10
POSE_GRAPH.constraint_builder.min_score = 0.65
return options