ROS1机器人SLAM系列（五）：Cartographer算法详解

本文将深入介绍Google开源的Cartographer算法，包括其核心原理、配置方法和实战应用。Cartographer支持2D/3D SLAM，并具备强大的回环检测能力。

1. Cartographer简介

1.1 什么是Cartographer？

Cartographer是Google于2016年开源的SLAM库，支持2D和3D激光SLAM。它采用了**图优化（Graph-based SLAM）**的方法，具有以下特点：

• 支持2D和3D SLAM
• 内置回环检测（Loop Closure）
• 支持多传感器融合（激光、IMU、里程计）
• 可处理大规模环境
• 支持在线和离线建图
• 代码质量高，工业级实现

1.2 与Gmapping的对比

特性	Gmapping	Cartographer
算法框架	粒子滤波	图优化
维度支持	仅2D	2D和3D
回环检测	不支持	支持
大场景能力	一般	优秀
计算资源	较低	较高
配置复杂度	简单	复杂

2. 算法原理

2.1 系统架构

2.2 Local SLAM（前端）

Local SLAM负责实时处理传感器数据，构建局部地图（Submap）。

核心步骤：

1. 传感器数据预处理

   • 激光数据时间同步
   • IMU数据积分
   • 里程计数据融合

2. 扫描匹配（Ceres Scan Matching）

   使用Ceres Solver进行非线性优化：

   // 优化目标：找到最优位姿ξ
   argmin_ξ Σ (1 - M_smooth(T_ξ h_k))²
   

   其中：

   • M_smooth：平滑后的概率栅格
   • T_ξ：位姿变换
   • h_k：扫描点

3. Submap构建

   每个Submap由固定数量的扫描组成，完成后不再更新：

   Submap状态：
   - Active：正在构建中，可以添加扫描
   - Frozen：构建完成，进入全局优化
   

2.3 Global SLAM（后端）

Global SLAM负责回环检测和全局优化。

1. 回环检测

使用Branch and Bound算法进行快速扫描匹配：

// 搜索窗口
SearchWindow = (x ± Δx, y ± Δy, θ ± Δθ)

// 分支定界搜索
while (candidate_nodes not empty) {
    node = pop_best_candidate()
    if (is_leaf(node)) {
        if (score > threshold) {
            add_constraint()
        }
    } else {
        branch_and_bound(node)
    }
}

2. 位姿图优化（Sparse Pose Adjustment, SPA）

构建约束图并进行全局优化：

约束类型：
- 相邻约束：连续帧之间的约束
- 回环约束：回环检测产生的约束
- 固定约束：GPS等外部信息

优化目标：

argmin Σ ρ(||f(ξ_i, ξ_j) - z_ij||²_Σ)

2.4 子图（Submap）机制

Submap是Cartographer的核心概念：

Submap结构包含：

• 概率栅格（Probability Grid）
• 位姿（Local Pose）
• 状态（Active/Frozen）
• 包含的扫描帧列表

3. ROS集成与话题

3.1 订阅的话题

话题	消息类型	说明
/scan	sensor_msgs/LaserScan	2D激光数据
/points2	sensor_msgs/PointCloud2	3D点云数据
/imu	sensor_msgs/Imu	IMU数据
/odom	nav_msgs/Odometry	里程计（可选）

3.2 发布的话题

话题	消息类型	说明
/map	nav_msgs/OccupancyGrid	2D栅格地图
/submap_list	cartographer_ros_msgs/SubmapList	Submap列表
/trajectory_node_list	visualization_msgs/MarkerArray	轨迹节点
/constraint_list	visualization_msgs/MarkerArray	约束可视化
/scan_matched_points2	sensor_msgs/PointCloud2	匹配后的点云

3.3 提供的服务

服务	类型	说明
start_trajectory	StartTrajectory	开始新轨迹
finish_trajectory	FinishTrajectory	结束轨迹
write_state	WriteState	保存状态
get_trajectory_states	GetTrajectoryStates	获取轨迹状态

4. 配置文件详解

Cartographer使用Lua配置文件，主要分为三部分：

4.1 轨迹构建器配置（trajectory_builder）

2D配置示例（backpack_2d.lua）：

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",  -- 跟踪的坐标系
  published_frame = "odom",     -- 发布的坐标系
  odom_frame = "odom",
  provide_odom_frame = false,   -- 是否提供odom
  publish_frame_projected_to_2d = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,          -- 激光雷达数量
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  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 = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

-- 2D轨迹构建器配置
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

return options

4.2 关键参数说明

Local SLAM参数：

TRAJECTORY_BUILDER_2D = {
  -- 激光范围
  min_range = 0.1,              -- 最小有效距离
  max_range = 30.,              -- 最大有效距离
  missing_data_ray_length = 5., -- 无效数据替换距离

  -- 体素滤波
  voxel_filter_size = 0.025,    -- 体素大小

  -- 自适应体素滤波
  adaptive_voxel_filter = {
    max_length = 0.5,
    min_num_points = 200,
    max_range = 50.,
  },

  -- IMU使用
  use_imu_data = true,
  imu_gravity_time_constant = 10.,

  -- 扫描匹配
  use_online_correlative_scan_matching = true,  -- 在线相关性匹配
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.1,     -- 线性搜索窗口
    angular_search_window = math.rad(20.),  -- 角度搜索窗口
    translation_delta_cost_weight = 1e-1,
    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.),
  },

  -- Submap配置
  submaps = {
    num_range_data = 90,            -- 每个Submap包含的扫描数
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID",
      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,    -- 未命中概率
      },
    },
  },
}

Global SLAM参数（MAP_BUILDER）：

MAP_BUILDER = {
  use_trajectory_builder_2d = true,
  num_background_threads = 4,       -- 后台线程数
}

POSE_GRAPH = {
  -- 优化频率
  optimize_every_n_nodes = 90,      -- 每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,

    -- 快速相关性匹配器
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },

    -- Ceres匹配器
    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,
      },
    },
  },

  -- 全局优化
  optimization_problem = {
    huber_scale = 1e1,
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    log_solver_summary = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },

  -- 最大约束数
  max_num_final_iterations = 200,
}

5. 实战：使用Cartographer建图

5.1 安装Cartographer

# 方法1：apt安装（推荐）
sudo apt install ros-noetic-cartographer ros-noetic-cartographer-ros

# 方法2：源码编译
cd ~/catkin_ws/src
git clone https://github.com/cartographer-project/cartographer.git
git clone https://github.com/cartographer-project/cartographer_ros.git
# 安装依赖
rosdep install --from-paths src --ignore-src -r -y
# 编译
cd ~/catkin_ws
catkin_make_isolated --install --use-ninja

5.2 创建配置文件

创建目录结构：

mkdir -p ~/catkin_ws/src/my_robot_cartographer/config
mkdir -p ~/catkin_ws/src/my_robot_cartographer/launch

配置文件 my_robot.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",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = 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,
  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 = 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.min_range = 0.12
TRAJECTORY_BUILDER_2D.max_range = 10.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
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.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)

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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05

POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

return options

5.3 创建Launch文件

cartographer.launch：

<launch>
  <!-- Cartographer节点 -->
  <node name="cartographer_node" pkg="cartographer_ros"
        type="cartographer_node" args="
            -configuration_directory $(find my_robot_cartographer)/config
            -configuration_basename my_robot.lua"
        output="screen">
    <remap from="scan" to="/scan"/>
    <remap from="odom" to="/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" required="true"
        args="-d $(find my_robot_cartographer)/rviz/cartographer.rviz"/>
</launch>

5.4 使用TurtleBot3测试

# 设置机器人型号
export TURTLEBOT3_MODEL=burger

# 终端1：启动仿真
roslaunch turtlebot3_gazebo turtlebot3_world.launch

# 终端2：启动Cartographer
roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=cartographer

# 终端3：键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5.5 保存地图

方法1：使用map_server保存

# 等待建图完成后
rosrun map_server map_saver -f ~/maps/cartographer_map

方法2：保存Cartographer状态（pbstream）

# 完成轨迹
rosservice call /finish_trajectory "trajectory_id: 0"

# 保存状态
rosservice call /write_state "{filename: '${HOME}/maps/cartographer_state.pbstream'}"

方法3：从pbstream转换为栅格地图

rosrun cartographer_ros cartographer_pbstream_to_ros_map \
  -map_filestem=/home/user/maps/map \
  -pbstream_filename=/home/user/maps/cartographer_state.pbstream \
  -resolution=0.05

6. 纯定位模式

Cartographer支持在已有地图上进行纯定位（Localization Only）：

6.1 定位配置

创建 my_robot_localization.lua：

include "my_robot.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}

POSE_GRAPH.optimize_every_n_nodes = 20

return options

6.2 定位Launch文件

<launch>
  <arg name="load_state_filename"/>

  <node name="cartographer_node" pkg="cartographer_ros"
        type="cartographer_node" args="
            -configuration_directory $(find my_robot_cartographer)/config
            -configuration_basename my_robot_localization.lua
            -load_state_filename $(arg load_state_filename)"
        output="screen">
    <remap from="scan" to="/scan"/>
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05"/>
</launch>

使用：

roslaunch my_robot_cartographer localization.launch \
  load_state_filename:=/home/user/maps/cartographer_state.pbstream

7. 参数调优指南

7.1 提高建图精度

-- 减小体素滤波大小
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.015

-- 增加Submap分辨率
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.03

-- 增加扫描匹配权重
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.

-- 更严格的回环检测
POSE_GRAPH.constraint_builder.min_score = 0.7

7.2 提高实时性

-- 减少优化频率
POSE_GRAPH.optimize_every_n_nodes = 150

-- 减少约束采样
POSE_GRAPH.constraint_builder.sampling_ratio = 0.1

-- 使用更大的体素滤波
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.05

-- 减少Submap包含的扫描数
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 50

7.3 处理动态环境

-- 减小最大有效距离
TRAJECTORY_BUILDER_2D.max_range = 8.

-- 增加运动滤波
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.5)

7.4 长走廊环境

-- 增加搜索窗口
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.3
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(45.)

-- 增加回环检测范围
POSE_GRAPH.constraint_builder.max_constraint_distance = 25.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 15.

8. 常见问题排查

8.1 无法启动

问题：Could not find a configuration directory

解决：确保配置目录路径正确

roslaunch my_robot_cartographer cartographer.launch
# 检查 -configuration_directory 路径

8.2 建图漂移

问题：地图出现严重漂移

解决：

-- 启用在线扫描匹配
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

-- 如果有IMU，启用IMU
TRAJECTORY_BUILDER_2D.use_imu_data = true

-- 检查TF变换是否正确

8.3 回环检测失败

问题：回环检测不触发

解决：

-- 降低最小得分阈值
POSE_GRAPH.constraint_builder.min_score = 0.5
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55

-- 增加搜索范围
POSE_GRAPH.constraint_builder.max_constraint_distance = 20.

8.4 CPU占用过高

问题：建图时CPU占用100%

解决：

-- 减少后台线程
MAP_BUILDER.num_background_threads = 2

-- 减少优化频率
POSE_GRAPH.optimize_every_n_nodes = 200

-- 增加采样间隔
POSE_GRAPH.constraint_builder.sampling_ratio = 0.2

9. 3D Cartographer简介

Cartographer也支持3D SLAM，主要区别：

-- 启用3D
MAP_BUILDER.use_trajectory_builder_3d = true

TRAJECTORY_BUILDER_3D = {
  min_range = 0.5,
  max_range = 30.,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.15,

  high_resolution_adaptive_voxel_filter = {
    max_length = 2.,
    min_num_points = 150,
    max_range = 15.,
  },

  low_resolution_adaptive_voxel_filter = {
    max_length = 4.,
    min_num_points = 200,
    max_range = 60.,
  },

  -- 需要IMU数据
  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {...},
  ceres_scan_matcher = {...},

  submaps = {
    high_resolution = 0.10,
    high_resolution_max_range = 20.,
    low_resolution = 0.45,
    num_range_data = 160,
  },
}

10. 总结

本文详细介绍了Cartographer算法：

1. 算法架构：Local SLAM + Global SLAM的双层结构
2. 核心技术：扫描匹配、Submap构建、回环检测、位姿图优化
3. 配置方法：Lua配置文件的详细参数说明
4. 实战应用：从安装到建图保存的完整流程
5. 调优技巧：针对不同场景的参数调整建议

Cartographer是目前最强大的开源SLAM算法之一，虽然配置相对复杂，但其出色的建图质量和回环检测能力使其成为大规模环境建图的首选。

---

系列导航：

• 上一篇：Gmapping算法详解与实战
• 下一篇：Hector SLAM算法详解

---

版权声明：本文为原创文章，转载请注明出处