使用激光雷达和rf2o算法在cartographer建图出现漂移重叠
-
使用激光雷达和rf2o算法在cartographer建图出现漂移重叠
背景：
最近在搞激光雷达建图，使用的是思岚a2m12激光雷达，然后尝试直接使用单激光雷达建图，发现地图漂移重叠比较严重，查询后得知可能需要里程计来提高精度，但由于我们的小车结构还在改，没确定最后方案，因此使用rf2o算法来获取里程计数据，结果使用过程就出现了两个问题
首先先将相关代码文件放上来吧雷达的launch文件 def generate_launch_description(): ## ***** Launch arguments ***** use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') ## ***** File paths ****** pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') urdf_dir = os.path.join(pkg_share, 'urdf') urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') with open(urdf_file, 'r') as infp: robot_desc = infp.read() ## ***** Nodes ***** robot_state_publisher_node = Node( package = 'robot_state_publisher', executable = 'robot_state_publisher', parameters=[ {'robot_description': robot_desc}, {'use_sim_time': LaunchConfiguration('use_sim_time')}], output = 'screen' ) cartographer_node = Node( package = 'cartographer_ros', executable = 'cartographer_node', parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], arguments = [ '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', '-configuration_basename', 'backpack_2d.lua'], remappings = [('scan','scan'), ('odom','odom_rf2o'), ('odom','odom_rf2o')],#('scan','scan')('odom','/odom_rf2o')('imu','/camera/camera/imu')echoes,horizontal_laser_2d output = 'screen' ) # 可视化节点 rviz_node = Node( package='rviz2', namespace='rviz2', executable='rviz2', name='rviz2', output='screen') cartographer_occupancy_grid_node = Node( package = 'cartographer_ros', executable = 'cartographer_occupancy_grid_node', parameters = [ {'use_sim_time': False}, {'resolution': 0.05}], ) return LaunchDescription([ use_sim_time_arg, # Nodes robot_state_publisher_node, rviz_node, cartographer_node, cartographer_occupancy_grid_node, ])rf2o的launch文件 def generate_launch_description(): return LaunchDescription([ Node( package='rf2o_laser_odometry', executable='rf2o_laser_odometry_node', name='rf2o_laser_odometry', output='screen', parameters=[{ 'laser_scan_topic' : '/scan', 'odom_topic' : '/odom_rf2o',#/odom_rf2o 'publish_tf' : True, 'base_frame_id' : 'base_link', 'odom_frame_id' : 'odom', 'init_pose_from_topic' : '', 'freq' : 10.0}], ), ])cartograoher的launch文件 def generate_launch_description(): ## ***** Launch arguments ***** use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False') ## ***** File paths ****** pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros') urdf_dir = os.path.join(pkg_share, 'urdf') urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf') with open(urdf_file, 'r') as infp: robot_desc = infp.read() ## ***** Nodes ***** robot_state_publisher_node = Node( package = 'robot_state_publisher', executable = 'robot_state_publisher', parameters=[ {'robot_description': robot_desc}, {'use_sim_time': LaunchConfiguration('use_sim_time')}], output = 'screen' ) cartographer_node = Node( package = 'cartographer_ros', executable = 'cartographer_node', parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}], arguments = [ '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files', '-configuration_basename', 'backpack_2d.lua'], remappings = [('scan','scan'), ('odom','odom_rf2o'), ('odom','odom_rf2o')],#('scan','scan')('odom','/odom_rf2o')('imu','/camera/camera/imu')echoes,horizontal_laser_2d output = 'screen' ) # 可视化节点 rviz_node = Node( package='rviz2', namespace='rviz2', executable='rviz2', name='rviz2', output='screen') cartographer_occupancy_grid_node = Node( package = 'cartographer_ros', executable = 'cartographer_occupancy_grid_node', parameters = [ {'use_sim_time': False}, {'resolution': 0.05}], ) return LaunchDescription([ use_sim_time_arg, # Nodes robot_state_publisher_node, rviz_node, cartographer_node, cartographer_occupancy_grid_node, ])cartographer的lua文件 include "map_builder.lua" include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link",--camera_imu_optical_frame base_link published_frame = "odom",-- laser_frame base_link odom_frame = "odom", provide_odom_frame = false, publish_frame_projected_to_2d = false, use_pose_extrapolator = true, use_odometry = true, use_nav_sat = false, use_landmarks = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1,-- 10 num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, 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 = 0.5, fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 35 --10 TRAJECTORY_BUILDER_2D.min_range = 0.3 TRAJECTORY_BUILDER_2D.max_range = 8. TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. 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 = 35 POSE_GRAPH.constraint_builder.min_score = 0.65 return options问题一：
当我配置好文件然后建图时，cartographer弹出了这样的报错：[cartographer_node-3] F0716 14:47:49.030234 58418 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (638567092688185279 vs. 638567092688185279) [cartographer_node-3] [FATAL] [1721112469.031000008] [cartographer logger]: F0716 14:47:49.000000 58418 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (638567092688185279 vs. 638567092688185279) [cartographer_node-3] F0716 14:47:49.132308 58386 imu_tracker.cc:67] Check failed: (orientation_ * gravity_vector_).z() > 0. (-nan vs. 0) [cartographer_node-3] [FATAL] [1721112469.132508442] [cartographer logger]: F0716 14:47:49.000000 58386 imu_tracker.cc:67] Check failed: (orientation_ * gravity_vector_).z() > 0. (-nan vs. 0) [cartographer_node-3] *** Check failure stack trace: *** [cartographer_node-3] @ 0x757905eb2b03 google::LogMessage::Fail() [cartographer_node-3] @ 0x757905eba9d1 google::LogMessage::SendToLog() [cartographer_node-3] @ 0x757905eb27c2 google::LogMessage::Flush() [cartographer_node-3] @ 0x757905eb478f google::LogMessageFatal::~LogMessageFatal() [cartographer_node-3] @ 0x62b1b0616669 cartographer::mapping::optimization::OptimizationProblem2D::AddOdometryData() [cartographer_node-3] @ 0x62b1b05a4143 _ZNSt17_Function_handlerIFN12cartographer7mapping8WorkItem6ResultEvEZNS1_11PoseGraph2D15AddOdometryDataEiRKNS0_6sensor12OdometryDataEEUlvE_E9_M_invokeERKSt9_Any_data [cartographer_node-3] @ 0x62b1b05a3af9 cartographer::mapping::PoseGraph2D::DrainWorkQueue() [cartographer_node-3] @ 0x62b1b066fac5 cartographer::common::Task::Execute() [cartographer_node-3] @ 0x62b1b0590381 cartographer::common::ThreadPool::DoWork() [cartographer_node-3] @ 0x7579052dc253 (unknown) [cartographer_node-3] @ 0x757904e94ac3 (unknown) [cartographer_node-3] @ 0x757904f26850 (unknown) [ERROR] [cartographer_node-3]: process has died [pid 58386, exit code -6, cmd '/home/murphy/fishbot_ws/install/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/murphy/fishbot_ws/install/cartographer_ros/share/cartographer_ros/configuration_files -configuration_basename backpack_2d.lua --ros-args --params-file /tmp/launch_params_fm5bsshm -r scan:=scan -r odom:=odom_rf2o -r odom:=odom_rf2o'].这似乎是雷达的scan和rf2o的odom信息转换间的时间戳出现了问题，但网上查询后有一个解决方法，就是将cartographer的luan文件中的参数
odometry_sampling_ratio = 1.,，这个odom采样率从1.0降低，降低到0.51左右就不会报错了，但我认为这种方法实际上是丢失了大量的odom信息，值时将原有问题掩盖而已，所以想问问各位是否遇见过这个问题，有没有什么办法解决？问题二：
第二个问题是我在将问题一暂且搁置，采用降低采样率进行建图后出现的问题，就是发现即使加上了里程计，其地图依旧出现漂移和重叠，感觉和没加上odom没有区别，这又是怎么一回事呢？望各位大神解答
图片地址)
图片地址)
图片地址) -
@1039291016 我之前测试的效果都比较不错,建图的时候旋转要慢一些,也可以试试 slam_toobox 方案
-
针对您遇到的问题，我将分别给出可能的解决方案和建议。
问题一：时间戳问题
报错信息表明 Cartographer 在处理来自激光雷达和 RF2O 提供的里程计数据时，时间戳发生了冲突或不一致。
map_by_time.h和imu_tracker.cc中的检查失败通常与数据的时间戳处理有关。-
确保时间同步：首先，确保激光雷达和 RF2O 节点都使用相同的时间源（如果可能，使用硬件时间戳）。如果激光雷达和 RF2O 节点运行在仿真中，请确保仿真时间是同步的，并检查
use_sim_time参数是否正确设置。 -
检查时间戳的生成：确保 RF2O 生成的里程计消息的时间戳是递增的，并且与激光雷达扫描的时间戳保持合理的一致性。如果 RF2O 的输出频率远低于激光雷达的扫描频率，可能需要插值或调整时间戳以匹配。
-
调整采样率：虽然降低
odometry_sampling_ratio可以暂时避免错误，但这并不是长期解决方案。考虑在保持精度的同时增加 RF2O 的输出频率，或者对激光雷达数据进行下采样以匹配 RF2O 的频率。 -
检查配置参数：确保 Cartographer 的配置文件中其他与时间相关的参数（如
lookup_transform_timeout_sec）设置得当。
问题二：地图漂移和重叠
如果在使用里程计数据后仍然出现地图漂移和重叠，这通常是由于以下几个原因：
-
里程计精度：RF2O 提供的里程计数据可能不够精确，导致 Cartographer 无法准确估计机器人的位置。考虑使用其他里程计算法或传感器（如 IMU）来提高精度。
-
配置问题：检查 Cartographer 的配置文件，特别是与扫描匹配、位姿图优化等相关的参数。可能需要调整这些参数以适应您的具体硬件和环境。
-
初始化问题：确保机器人在建图开始前有一个合理的初始位置和朝向。错误的初始位置可能导致整个地图的漂移。
-
环境问题：某些环境特征（如重复的结构、缺乏纹理的区域）可能导致扫描匹配失败，从而导致地图漂移。尝试在不同的环境中测试您的系统，并观察是否仍然出现漂移。
-
TF树和话题关系：确保 TF 树正确设置，并且 Cartographer 节点订阅了正确的里程计和激光雷达话题。您可以使用
rosrun tf view_frames命令来可视化 TF 树，并使用rostopic echo命令来检查话题数据。 -
硬件问题：最后，考虑检查激光雷达和里程计传感器的硬件连接和校准。硬件问题（如松动的连接线、传感器故障等）也可能导致地图漂移。
以上为生成回答，仅供参考～
-
-
-
@小鱼 一开始就是使用carto纯激光建图，但地图也会漂移重叠
-
@1039291016 我之前测试的效果都比较不错,建图的时候旋转要慢一些,也可以试试 slam_toobox 方案
-
@小鱼 ok，我再试试纯激光建图的方案，还有slam_toolbox的方案有相关教程或者例子吗？想学习一下，感谢小鱼
-
@小鱼 在 使用激光雷达和rf2o算法在cartographer建图出现漂移重叠 中说：
@1039291016 在 使用激光雷达和rf2o算法在cartographer建图出现漂移重叠 中说：
rf2o
使用 rf2o 不如直接用carto纯激光建图
还有想问问为啥不推荐使用rf2o，是因为精度很差吗？
-
@1039291016 从本质上来讲,并没有增加传感器
-
@1039291016 直接搜,有很多,社区的fishbot资料中也有
-
@小鱼 ok，感谢小鱼
-
小 小鱼 将这个主题标记为已解决，在
