Cartographer与ROS集成实战：构建机器人自主导航系统

【免费下载链接】cartographer Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations. 项目地址: https://gitcode.com/gh_mirrors/ca/cartographer

引言：解决自主导航的核心挑战

你是否在机器人导航项目中遇到过这些问题：SLAM建图漂移严重、地图与机器人位姿不匹配、传感器数据融合困难？本文将系统讲解如何将Cartographer（实时同步定位与地图构建系统）与ROS（机器人操作系统）无缝集成，构建稳定可靠的自主导航系统。通过本文，你将掌握从环境配置、参数调优到导航测试的全流程，解决实际应用中的常见痛点。

读完本文后，你将能够：

• 正确配置Cartographer与ROS的开发环境
• 理解并优化Cartographer的核心参数
• 实现传感器数据（激光雷达、IMU、里程计）的有效融合
• 构建高精度地图并进行自主导航测试
• 诊断和解决集成过程中的常见问题

1. 系统架构与核心概念

1.1 Cartographer与ROS集成架构

Cartographer与ROS的集成架构主要包含以下组件：

核心数据流：

• 传感器数据通过ROS驱动节点发布为标准ROS消息
• Cartographer节点订阅这些消息，进行SLAM计算
• 输出地图数据和机器人位姿
• 导航节点利用地图和位姿进行路径规划
• 控制节点将规划结果转换为机器人控制指令

1.2 关键坐标系(Frame)

Cartographer定义了多个关键坐标系，理解这些坐标系是正确集成的基础：

坐标系名称	描述	特点
global map frame	全局地图坐标系	固定不变，包含所有回环检测和优化结果
local map frame	局部地图坐标系	不包含回环优化结果，与其他坐标系关系固定
submap frame	子图坐标系	每个子图有独立的固定坐标系
tracking frame	跟踪坐标系	传感器数据表达的坐标系，随时间变化
gravity-aligned frame	重力对齐坐标系	仅用于2D，与跟踪坐标系原点相同，z轴与重力方向对齐

坐标变换关系：

• local_pose: 跟踪坐标系到局部地图坐标系的变换
• global_pose: 跟踪坐标系到全局地图坐标系的变换
• local_submap_pose: 子图坐标系到局部地图坐标系的变换
• global_submap_pose: 子图坐标系到全局地图坐标系的变换

2. 环境配置与安装

2.1 硬件要求

Cartographer对硬件有一定要求，特别是CPU性能和内存：

组件	最低配置	推荐配置
CPU	四核处理器	八核及以上处理器
内存	4GB RAM	8GB RAM及以上
激光雷达	单线激光雷达	多线激光雷达(16线及以上)
IMU	6轴IMU	9轴IMU(带磁场传感器)
里程计	-	轮式里程计或视觉里程计

2.2 软件环境准备

以下是在Ubuntu 20.04上安装Cartographer和ROS Noetic的步骤：

# 安装ROS Noetic (如果尚未安装)
sudo sh -c 'echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt install ros-noetic-desktop-full
source /opt/ros/noetic/setup.bash

# 创建工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src

# 克隆Cartographer仓库
git clone https://gitcode.com/gh_mirrors/ca/cartographer.git
git clone https://gitcode.com/gh_mirrors/ca/cartographer_ros.git

# 安装依赖
cd ~/catkin_ws
rosdep install --from-paths src --ignore-src -r -y

# 安装abseil-cpp
cd ~/catkin_ws/src/cartographer/scripts
./install_abseil.sh

# 编译工作空间
cd ~/catkin_ws
catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

3. 核心参数配置与优化

3.1 配置文件结构

Cartographer的配置主要通过Lua脚本文件实现，核心配置文件包括：

configuration_files/
├── map_builder.lua        # 地图构建器配置
├── map_builder_server.lua # 地图构建器服务器配置
├── pose_graph.lua         # 位姿图配置
├── trajectory_builder.lua # 轨迹构建器配置
├── trajectory_builder_2d.lua # 2D轨迹构建器配置
└── trajectory_builder_3d.lua # 3D轨迹构建器配置

3.2 关键参数优化

3.2.1 激光雷达参数

在trajectory_builder_2d.lua中配置激光雷达参数：

TRAJECTORY_BUILDER_2D = {
  min_range = 0.3,          -- 最小有效距离
  max_range = 30.,          -- 最大有效距离
  min_z = -0.8,             -- 最小z坐标
  max_z = 2.,               -- 最大z坐标
  missing_data_ray_length = 5., -- 缺失数据的射线长度
  
  -- 体素滤波配置
  voxel_filter_size = 0.05, -- 体素滤波器大小
  
  adaptive_voxel_filter = {
    max_length = 0.5,       -- 体素最大边长
    min_num_points = 200,   -- 最小点数量
    max_range = 50.,        -- 最大范围
  },
}

参数调整建议：

• 对于室内环境，可减小max_range至5-10米
• 对于噪声较大的激光雷达，可增大voxel_filter_size
• 对于稀疏环境，减小adaptive_voxel_filter.min_num_points

3.2.2 扫描匹配参数

TRAJECTORY_BUILDER_2D = {
  ceres_scan_matcher = {
    occupied_space_weight = 1.,    -- 占据空间权重
    translation_weight = 10.,      -- 平移权重
    rotation_weight = 40.,         -- 旋转权重
    ceres_solver_options = {
      use_nonmonotonic_steps = false, -- 是否使用非单调步长
      max_num_iterations = 30,      -- 最大迭代次数
      num_threads = 1,              -- 线程数
    },
  },
  
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.15,    -- 线性搜索窗口
    angular_search_window = math.rad(20.), -- 角度搜索窗口
    translation_delta_cost_weight = 1e-1, -- 平移增量成本权重
    rotation_delta_cost_weight = 1e-1,    -- 旋转增量成本权重
  },
}

参数调整建议：

• 提高translation_weight和rotation_weight可增加扫描匹配稳定性
• 对于动态环境，可适当减小linear_search_window和angular_search_window
• 如计算资源充足，可增加max_num_iterations提高匹配精度

3.2.3 运动滤波器参数

TRAJECTORY_BUILDER_2D = {
  motion_filter = {
    max_time_seconds = 5.,         -- 最大时间间隔
    max_distance_meters = 0.2,     -- 最大距离间隔
    max_angle_radians = math.rad(1.), -- 最大角度间隔
  },
}

参数调整建议：

• 对于慢速移动机器人，减小max_distance_meters
• 对于旋转较快的机器人，增大max_angle_radians
• 对于动态环境，减小max_time_seconds以提高响应速度

3.2.4 回环检测参数

在pose_graph.lua中配置回环检测：

POSE_GRAPH = {
  optimize_every_n_nodes = 35,    -- 每n个节点优化一次
  constraint_builder = {
    sampling_ratio = 0.3,          -- 采样比例
    max_constraint_distance = 15., -- 最大约束距离
    min_score = 0.55,              -- 最小匹配分数
    loop_closure_translation_weight = 1.1e4, -- 回环平移权重
    loop_closure_rotation_weight = 1e5,      -- 回环旋转权重
  },
  
  optimization_problem = {
    huber_scale = 1e1,             -- Huber损失尺度
    acceleration_weight = 1.1e2,   -- 加速度权重
    rotation_weight = 1.1e2,       -- 旋转权重
  },
  
  global_sampling_ratio = 0.003,   -- 全局采样比例
  log_residual_histograms = false, -- 是否记录残差直方图
}

参数调整建议：

• 对于大型环境，增加max_constraint_distance
• 如出现错误回环，提高min_score
• 对于高精度要求，减小global_sampling_ratio

3.3 传感器融合配置

3.3.1 IMU配置

TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,            -- 是否使用IMU数据
  imu_gravity_time_constant = 10., -- IMU重力时间常数
  
  pose_extrapolator = {
    use_imu_based = true,         -- 是否使用基于IMU的位姿外推
    imu_based = {
      pose_queue_duration = 5.,   -- 位姿队列持续时间
      gravity_constant = 9.806,   -- 重力常数
      imu_acceleration_weight = 1., -- IMU加速度权重
      imu_rotation_weight = 1.,   -- IMU旋转权重
    },
  },
}

参数调整建议：

• 确保IMU与激光雷达时间同步
• 对于噪声较大的IMU，减小imu_acceleration_weight和imu_rotation_weight

4. ROS节点与话题接口

4.1 核心节点

Cartographer ROS提供以下核心节点：

节点	功能
cartographer_node	主要SLAM节点，处理传感器数据并构建地图
cartographer_occupancy_grid_node	将Cartographer地图转换为ROS占用网格地图
cartographer_offline_node	离线处理传感器数据
cartographer_pbstream_to_ros_map	将pbstream文件转换为ROS地图

4.2 话题接口

4.2.1 订阅话题

话题	类型	描述
scan	sensor_msgs/LaserScan	激光雷达扫描数据
points2	sensor_msgs/PointCloud2	点云数据
imu	sensor_msgs/Imu	IMU数据
odom	nav_msgs/Odometry	里程计数据
tf	tf2_msgs/TFMessage	坐标变换

4.2.2 发布话题

话题	类型	描述
map	nav_msgs/OccupancyGrid	占用网格地图
pose	geometry_msgs/PoseStamped	机器人位姿
trajectory_node_list	cartographer_ros_msgs/TrajectoryNodeList	轨迹节点列表
submap_list	cartographer_ros_msgs/SubmapList	子图列表
tf	tf2_msgs/TFMessage	坐标变换

4.3 坐标系配置

在launch文件中配置坐标系转换关系：

<node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="
  -configuration_directory $(find cartographer_ros)/configuration_files
  -configuration_basename trajectory_builder_2d.lua"
  output="screen">
  
  <remap from="scan" to="laser_scan" />
  
  <param name="robot_description" textfile="$(find my_robot_description)/urdf/robot.urdf" />
  
  <param name="/use_sim_time" value="true" />
  
  <!-- 坐标系配置 -->
  <rosparam param="tracking_frame">base_link</rosparam>
  <rosparam param="published_frame">odom</rosparam>
  <rosparam param="odom_frame">odom</rosparam>
  <rosparam param="provide_odom_frame">true</rosparam>
  <rosparam param="use_odometry">true</rosparam>
  <rosparam param="use_imu_data">true</rosparam>
</node>

关键坐标系参数：

• tracking_frame: 跟踪坐标系，通常设为base_link
• published_frame: 发布的坐标系，通常设为map或odom
• odom_frame: 里程计坐标系，如果使用里程计则设为odom

5. 地图构建与保存

5.1 启动SLAM建图

创建并启动launch文件：

# 创建launch文件
mkdir -p ~/catkin_ws/src/my_robot_bringup/launch
nano ~/catkin_ws/src/my_robot_bringup/launch/cartographer_demo.launch

launch文件内容：

<launch>
  <param name="/use_sim_time" value="false" />
  
  <!-- 启动Cartographer节点 -->
  <node name="cartographer_node" pkg="cartographer_ros" 
        type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename trajectory_builder_2d.lua"
        output="screen">
        
    <!-- 坐标系参数 -->
    <rosparam param="tracking_frame">base_link</rosparam>
    <rosparam param="map_frame">map</rosparam>
    <rosparam param="odom_frame">odom</rosparam>
    <rosparam param="publish_frame_projected_to_2d">true</rosparam>
    <rosparam param="use_pose_extrapolator">true</rosparam>
    <rosparam param="use_odometry">true</rosparam>
    <rosparam param="use_imu_data">true</rosparam>
    <rosparam param="num_laser_scans">1</rosparam>
    <rosparam param="num_multi_echo_laser_scans">0</rosparam>
    <rosparam param="num_subdivisions_per_laser_scan">1</rosparam>
    <rosparam param="num_point_clouds">0</rosparam>
    
    <!-- 话题重映射 -->
    <remap from="scan" to="/my_robot/laser/scan" />
    <remap from="imu" to="/my_robot/imu/data" />
    <remap from="odom" to="/my_robot/odom" />
  </node>
  
  <!-- 启动占用网格地图节点 -->
  <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" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

启动建图：

roslaunch my_robot_bringup cartographer_demo.launch

5.2 保存地图

建图完成后，保存地图数据：

# 保存pbstream格式地图
rosservice call /finish_trajectory 0
rosservice call /write_state "{filename: '${HOME}/map.pbstream', include_unfinished_submaps: true}"

# 转换为ROS地图格式
rosrun cartographer_ros cartographer_pbstream_to_ros_map \
  -pbstream_filename ~/map.pbstream \
  -map_filestem ~/my_robot_map

这将生成两个文件：

• my_robot_map.pgm: 地图图像文件
• my_robot_map.yaml: 地图元数据文件

5.3 地图评估指标

评估地图质量的关键指标：

指标	描述	理想值
轨迹一致性	闭环后轨迹的重合程度	误差<5%
地图分辨率	地图的细节程度	0.05-0.1m
回环数量	检测到的有效回环数	取决于环境大小
定位精度	机器人实际位置与估计位置的偏差	<0.1m

使用Cartographer提供的工具评估地图：

# 运行评估工具
rosrun cartographer_ros evaluate_trajectory \
  --reference_frames=map \
  --evaluation=position_error_mse,rotation_error_mse \
  --pose_graph_filename=${HOME}/map.pbstream \
  --save_plot=/tmp/evaluation_plot.pdf

6. 自主导航实现

6.1 导航系统架构

6.2 集成AMCL定位

使用ROS的AMCL(自适应蒙特卡洛定位)包进行定位：

<launch>
  <!-- 地图服务器 -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find my_robot_bringup)/maps/my_robot_map.yaml" />
  
  <!-- AMCL定位 -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <!-- 激光雷达参数 -->
    <param name="laser_min_range" value="0.1" />
    <param name="laser_max_range" value="30.0" />
    <param name="laser_max_beams" value="60" />
    
    <!-- 粒子滤波参数 -->
    <param name="min_particles" value="500" />
    <param name="max_particles" value="2000" />
    <param name="kld_err" value="0.05" />
    <param name="update_min_d" value="0.2" />
    <param name="update_min_a" value="0.5" />
    
    <!-- 初始位姿估计 -->
    <param name="initial_pose_x" value="0.0" />
    <param name="initial_pose_y" value="0.0" />
    <param name="initial_pose_a" value="0.0" />
    <param name="initial_cov_xx" value="0.5*0.5" />
    <param name="initial_cov_yy" value="0.5*0.5" />
    <param name="initial_cov_aa" value="(π/12)*(π/12)" />
    
    <!-- 坐标系参数 -->
    <param name="odom_frame_id" value="odom" />
    <param name="base_frame_id" value="base_link" />
    <param name="global_frame_id" value="map" />
    
    <!-- 话题订阅 -->
    <remap from="scan" to="/my_robot/laser/scan" />
  </node>
</launch>

6.3 导航栈配置

配置ROS导航栈：

<launch>
  <!-- 启动move_base节点 -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    
    <!-- 全局规划器参数 -->
    <rosparam file="$(find my_robot_bringup)/param/global_costmap_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_robot_bringup)/param/local_costmap_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find my_robot_bringup)/param/base_local_planner_params.yaml" command="load" />
    
    <!-- 规划器配置 -->
    <param name="base_global_planner" value="navfn/NavfnROS" />
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    
    <!-- 恢复行为 -->
    <param name="recovery_behaviors" value="[{'name':'clear_costmap_recovery/ClearCostmapRecovery', 'type':'clear_costmap_recovery/ClearCostmapRecovery'},{'name':'rotate_recovery/RotateRecovery', 'type':'rotate_recovery/RotateRecovery'}]" />
    <param name="clearing_rotation_allowed" value="true" />
  </node>
</launch>

全局代价地图参数配置示例：

# global_costmap_params.yaml
global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 1.0
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  resolution: 0.05
  transform_tolerance: 1.0
  plugins:
    - {name: static_layer, type: 'costmap_2d::StaticLayer'}
    - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
    - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}

7. 常见问题与解决方案

7.1 建图漂移

症状：地图出现明显错位或重影

解决方案：

1. 优化传感器同步：确保激光雷达、IMU和里程计时间同步
2. 调整运动滤波器参数：

   motion_filter = {
     max_time_seconds = 2.,
     max_distance_meters = 0.1,
     max_angle_radians = math.rad(0.5),
   }
   

3. 增强环境特征：在空旷环境中添加人工特征
4. 提高IMU权重：如果IMU质量较好，增加IMU相关权重

7.2 回环检测失败

症状：长时间建图后累积误差大，无回环校正

解决方案：

1. 调整回环检测参数：

   constraint_builder = {
     sampling_ratio = 0.5,
     max_constraint_distance = 20.,
     min_score = 0.5,
   }
   

2. 增加环境特征：回环检测依赖环境特征，特征少的环境难以检测回环
3. 降低loop_closure_adaptive_voxel_filter.max_length以保留更多特征点
4. 增加global_sampling_ratio提高全局采样概率

7.3 导航避障问题

症状：机器人导航时频繁碰撞或过度保守

解决方案：

1. 调整代价地图膨胀参数：

   inflation_layer:
     inflation_radius: 0.5
     cost_scaling_factor: 10.0
   

2. 优化局部规划器参数：

   DWAPlannerROS:
     max_vel_x: 0.5
     min_vel_x: 0.1
     max_vel_theta: 1.0
     min_vel_theta: -1.0
     acc_lim_x: 0.2
     acc_lim_theta: 0.5
     xy_goal_tolerance: 0.2
     yaw_goal_tolerance: 0.1
   

3. 增加传感器覆盖范围：确保机器人周围360°无感知盲区

7.4 系统性能问题

症状：Cartographer节点CPU占用过高，导致延迟

解决方案：

1. 降低传感器数据频率：减少激光雷达扫描频率
2. 增加体素滤波器大小：voxel_filter_size: 0.1
3. 减少优化迭代次数：

   ceres_solver_options = {
     max_num_iterations = 15,
     num_threads = 1,
   }
   

4. 调整优化频率：optimize_every_n_nodes: 50

8. 实战案例与性能优化

8.1 室内环境应用

场景特点：结构化环境，特征丰富，空间有限

配置优化：

• 激光雷达参数：max_range: 8.0，voxel_filter_size: 0.05
• 回环检测：max_constraint_distance: 10.0，min_score: 0.6
• 计算资源：单核CPU即可满足需求

效果评估：

• 地图分辨率：5cm
• 定位精度：±3cm
• 建图时间：100平方米约5分钟

8.2 室外环境应用

场景特点：非结构化环境，特征稀疏，空间大

配置优化：

• 激光雷达参数：max_range: 30.0，voxel_filter_size: 0.1
• 回环检测：max_constraint_distance: 30.0，min_score: 0.5
• 增加IMU权重以提高运动估计稳定性

效果评估：

• 地图分辨率：10cm
• 定位精度：±10cm
• 建图时间：500平方米约30分钟

8.3 多机器人协作建图

架构设计：

实现步骤：

1. 每个机器人运行独立Cartographer实例
2. 通过ROS服务将子图数据发送到中央服务器
3. 中央服务器进行全局优化
4. 将优化结果分发回各个机器人

9. 总结与展望

9.1 关键知识点回顾

• Cartographer与ROS集成的核心是理解坐标系变换和参数配置
• 传感器数据质量和时间同步对建图精度至关重要
• 参数优化应根据具体硬件和环境特点进行调整
• 地图评估是提高系统性能的关键步骤

9.2 进阶方向

1. 多传感器融合：融合视觉、激光、惯导等多种传感器数据
2. 动态环境处理：研究动态物体检测与过滤算法
3. 语义SLAM：将语义信息融入地图构建
4. 终身SLAM：实现长期运行的地图维护与更新

9.3 学习资源

• Cartographer官方文档：深入理解算法原理
• ROS导航教程：掌握导航栈使用与配置
• 《概率机器人》：理解SLAM核心算法
• 开源项目实践：通过实际项目巩固知识

通过本文介绍的方法，你应该能够构建一个稳定可靠的Cartographer与ROS集成系统，实现机器人的自主导航功能。记住，SLAM系统的优化是一个迭代过程，需要根据实际环境和需求不断调整参数和配置。

祝你在机器人导航项目中取得成功！

【免费下载链接】cartographer Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations. 项目地址: https://gitcode.com/gh_mirrors/ca/cartographer