🗺️ 第16课：地图构建(SLAM)

📋 课程目标

• 理解SLAM（同时定位与建图）的核心原理
• 掌握slam_toolbox的配置与使用
• 学会地图的保存、加载与编辑
• 理解不同SLAM算法的特点与适用场景
• 完成SLAM建图的完整实操流程

🧠 什么是SLAM？

SLAM（Simultaneous Localization and Mapping，同时定位与建图）是机器人领域最核心的技术之一。机器人在未知环境中，一边构建环境地图，一边利用已构建的地图确定自身位置——这就是"鸡与蛋"问题的经典解法。

📐 SLAM算法分类

🛠️ slam_toolbox详解

slam_toolbox是ROS2中最常用的2D SLAM方案，基于图优化方法，支持在线建图和离线地图优化。

安装slam_toolbox

# 安装slam_toolbox
sudo apt install ros-humble-slam-toolbox

# 验证安装
ros2 pkg list | grep slam_toolbox

slam_toolbox的两种模式

# 启动在线SLAM
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=./config/mapper_params_online_async.yaml \
  use_sim_time:=true

# 启动离线SLAM
ros2 launch slam_toolbox offline_launch.py \
  slam_params_file:=./config/mapper_params_offline.yaml

# 回放录制的bag
ros2 bag play recorded_bag --clock

⚙️ slam_toolbox配置文件

# mapper_params_online_async.yaml - slam_toolbox在线异步配置
slam_toolbox:
  ros__parameters:
    # ROS参数
    solver_plugin: solver_plugins::CeresSolver  # 图优化求解器
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # 地图参数
    resolution: 0.05                # 地图分辨率 m/pixel
    max_laser_range: 20.0           # 激光最大有效范围 m
    minimum_time_interval: 0.5      # 最小扫描间隔 s
    transform_timeout: 0.2          # TF查询超时 s
    tf_buffer_duration: 30.0        # TF缓冲时长 s
    stack_size_to_use: 40000000     # 线程栈大小

    # 扫描匹配参数
    use_scan_matching: true         # 启用扫描匹配
    use_scan_barycenter: true       # 使用扫描重心
    minimum_travel_distance: 0.5    # 触发更新的最小移动距离 m
    minimum_travel_heading: 0.5     # 触发更新的最小旋转角度 rad
    scan_buffer_size: 10            # 扫描缓冲区大小
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true           # 启用回环检测
    loop_match_minimum_chain_size: 10
    loop_match_maximum_variance_coarse: 3.0
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45

    # 相关性搜索参数
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1

    # 回环检测参数
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # 扫描匹配参数
    distance_variance_penalty: 0.5
    angle_variance_penalty: 1.0
    fine_search_angle_offset: 0.00349
    coarse_search_angle_offset: 0.349
    coarse_angle_resolution: 0.0349
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

    # 地图更新
    update_factor_free: 0.4         # 自由空间更新因子
    update_factor_occupied: 0.9     # 占用空间更新因子

🐍 Python：SLAM状态监控节点

#!/usr/bin/env python3
"""SLAM建图状态监控节点 - 实时监控地图质量与定位状态"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav_msgs.msg import OccupancyGrid, Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped
from std_msgs.msg import String
import math
import time


class SLAMMonitorNode(Node):
    """监控SLAM建图状态：地图覆盖率、定位质量、建图速度"""

    def __init__(self):
        super().__init__('slam_monitor')

        # 参数
        self.declare_parameter('map_coverage_threshold', 0.5)
        self.declare_parameter('localization_quality_threshold', 0.7)

        # QoS配置
        map_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        # 订阅地图
        self.map_sub = self.create_subscription(
            OccupancyGrid, '/map', self.map_callback, map_qos
        )

        # 订阅里程计
        self.odom_sub = self.create_subscription(
            Odometry, '/odom', self.odom_callback, 10
        )

        # 订阅定位协方差
        self.amcl_sub = self.create_subscription(
            PoseWithCovarianceStamped, '/amcl_pose', self.amcl_callback, 10
        )

        # 发布状态
        self.status_pub = self.create_publisher(String, '/slam_status', 10)

        # 状态变量
        self.map_received = False
        self.total_cells = 0
        self.known_cells = 0
        self.occupied_cells = 0
        self.map_coverage = 0.0
        self.last_map_time = 0.0
        self.map_update_count = 0
        self.robot_x = 0.0
        self.robot_y = 0.0
        self.localization_cov = float('inf')

        # 定时发布状态
        self.timer = self.create_timer(2.0, self.publish_status)

        self.get_logger().info('🗺️ SLAM监控节点已启动')

    def map_callback(self, msg: OccupancyGrid):
        """处理地图更新"""
        self.total_cells = msg.info.width * msg.info.height
        self.known_cells = 0
        self.occupied_cells = 0

        for cell in msg.data:
            if cell != -1:  # -1表示未知
                self.known_cells += 1
                if cell > 50:  # 占用阈值
                    self.occupied_cells += 1

        if self.total_cells > 0:
            self.map_coverage = self.known_cells / self.total_cells

        self.map_received = True
        self.map_update_count += 1
        self.last_map_time = time.time()

    def odom_callback(self, msg: Odometry):
        """更新机器人位置"""
        self.robot_x = msg.pose.pose.position.x
        self.robot_y = msg.pose.pose.position.y

    def amcl_callback(self, msg: PoseWithCovarianceStamped):
        """评估定位质量"""
        # 从6x6协方差矩阵提取位置方差
        cov = msg.pose.covariance
        pos_var = cov[0] + cov[7]  # x方差 + y方差
        self.localization_cov = math.sqrt(pos_var)

    def publish_status(self):
        """定时发布SLAM状态"""
        status = String()

        if not self.map_received:
            status.data = '⏳ 等待地图数据...'
            self.status_pub.publish(status)
            return

        # 计算各项指标
        coverage_pct = self.map_coverage * 100
        map_age = time.time() - self.last_map_time if self.last_map_time > 0 else -1

        # 定位质量评估
        if self.localization_cov < 0.1:
            loc_quality = '🟢 优秀'
        elif self.localization_cov < 0.5:
            loc_quality = '🟡 良好'
        else:
            loc_quality = '🔴 较差'

        status.data = (
            f'📊 SLAM状态 | '
            f'覆盖率: {coverage_pct:.1f}% | '
            f'已知区域: {self.known_cells}/{self.total_cells} | '
            f'障碍物: {self.occupied_cells} | '
            f'更新次数: {self.map_update_count} | '
            f'定位: {loc_quality} | '
            f'位置: ({self.robot_x:.2f}, {self.robot_y:.2f})'
        )

        self.status_pub.publish(status)
        self.get_logger().info(status.data)


def main(args=None):
    rclpy.init(args=args)
    node = SLAMMonitorNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

🔧 C++：地图质量分析节点

// map_quality_analyzer.cpp - 地图质量分析C++节点
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "std_msgs/msg/string.hpp"

#include <cmath>
#include <string>
#include <algorithm>
#include <chrono>

class MapQualityAnalyzer : public rclcpp::Node {
public:
    MapQualityAnalyzer() : Node("map_quality_analyzer") {
        // 订阅地图
        map_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
            "/map", rclcpp::QoS(1).reliable(),
            std::bind(&MapQualityAnalyzer::map_callback, this, std::placeholders::_1));

        // 发布分析结果
        analysis_pub_ = this->create_publisher<std_msgs::msg::String>(
            "/map_quality_report", 10);

        RCLCPP_INFO(this->get_logger(), "🗺️ 地图质量分析节点已启动");
    }

private:
    struct MapMetrics {
        double coverage_ratio;       // 覆盖率
        double occupancy_ratio;      // 障碍物占比
        double free_ratio;           // 自由空间占比
        size_t total_cells;
        size_t known_cells;
        size_t occupied_cells;
        size_t free_cells;
        double entropy;              // 地图信息熵
    };

    void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
        auto start = std::chrono::high_resolution_clock::now();

        MapMetrics metrics;
        metrics.total_cells = msg->data.size();
        metrics.known_cells = 0;
        metrics.occupied_cells = 0;
        metrics.free_cells = 0;
        metrics.entropy = 0.0;

        for (const auto& cell : msg->data) {
            if (cell == -1) continue;  // 未知区域

            metrics.known_cells++;
            double p = static_cast<double>(cell) / 100.0;  // 占用概率

            if (p > 0.5) {
                metrics.occupied_cells++;
            } else {
                metrics.free_cells++;
            }

            // 计算信息熵 H = -p*log(p) - (1-p)*log(1-p)
            if (p > 0.01 && p < 0.99) {
                metrics.entropy -= p * std::log2(p) + (1 - p) * std::log2(1 - p);
            }
        }

        metrics.coverage_ratio = static_cast<double>(metrics.known_cells) / metrics.total_cells;
        metrics.occupancy_ratio = static_cast<double>(metrics.occupied_cells) / metrics.known_cells;
        metrics.free_ratio = static_cast<double>(metrics.free_cells) / metrics.known_cells;

        // 平均信息熵
        if (metrics.known_cells > 0) {
            metrics.entropy /= metrics.known_cells;
        }

        auto end = std::chrono::high_resolution_clock::now();
        double elapsed_ms = std::chrono::duration<double, std::milli>(end - start).count();

        // 评估地图质量
        std::string quality;
        if (metrics.coverage_ratio < 0.1) {
            quality = "🔴 初始阶段 - 覆盖率不足";
        } else if (metrics.coverage_ratio < 0.5) {
            quality = "🟡 建图中 - 需要继续探索";
        } else if (metrics.entropy > 0.5) {
            quality = "🟡 不确定性高 - 需要重访区域";
        } else {
            quality = "🟢 地图质量良好";
        }

        // 发布报告
        auto report = std_msgs::msg::String();
        report.data = fmt_report(metrics, quality, elapsed_ms, *msg);
        analysis_pub_->publish(report);

        RCLCPP_INFO(this->get_logger(), "地图分析完成: %s (耗时%.1fms)",
                    quality.c_str(), elapsed_ms);
    }

    std::string fmt_report(const MapMetrics& m, const std::string& quality,
                           double elapsed_ms, const nav_msgs::msg::OccupancyGrid& map) {
        char buf[512];
        snprintf(buf, sizeof(buf),
            "📊 地图质量报告\n"
            "  分辨率: %.3f m/pixel\n"
            "  尺寸: %dx%d (%.1fx%.1f m)\n"
            "  覆盖率: %.1f%% (%zu/%zu)\n"
            "  障碍物: %.1f%% (%zu)\n"
            "  自由空间: %.1f%% (%zu)\n"
            "  平均信息熵: %.3f\n"
            "  质量: %s\n"
            "  分析耗时: %.1f ms",
            map.info.resolution,
            map.info.width, map.info.height,
            map.info.width * map.info.resolution,
            map.info.height * map.info.resolution,
            m.coverage_ratio * 100, m.known_cells, m.total_cells,
            m.occupancy_ratio * 100, m.occupied_cells,
            m.free_ratio * 100, m.free_cells,
            m.entropy, quality.c_str(), elapsed_ms);
        return std::string(buf);
    }

    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr analysis_pub_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MapQualityAnalyzer>());
    rclcpp::shutdown();
    return 0;
}

💾 地图保存与加载

使用nav2_map_server保存地图

# 安装map_server
sudo apt install ros-humble-nav2-map-server

# 保存当前地图
ros2 run nav2_map_server map_saver_cli -f ~/maps/my_map

# 生成两个文件：
# my_map.pgm - 地图图像（灰度图）
# my_map.yaml - 地图元数据

地图YAML文件解析

# my_map.yaml - 地图元数据文件
image: my_map.pgm          # 地图图像文件名
mode: trinary              # 模式: trinary(三值)/scale(缩放)/raw(原始)
resolution: 0.05           # 分辨率 m/pixel
origin: [-10.0, -10.0, 0] # 地图左下角在世界坐标系的位姿 [x, y, yaw]
negate: 0                  # 是否反转颜色
occupied_thresh: 0.65      # 占用阈值 (概率>此值为障碍物)
free_thresh: 0.196         # 自由阈值 (概率<此值为自由空间)

加载已保存的地图

# 方法1：使用map_server节点
ros2 run nav2_map_server map_server \
  --ros-args \
  -p yaml_filename:=$HOME/maps/my_map.yaml

# 方法2：在launch文件中加载
# map_server_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    map_dir = os.path.join(get_package_share_directory('my_nav_pkg'), 'maps')
    return LaunchDescription([
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{
                'yaml_filename': os.path.join(map_dir, 'my_map.yaml'),
                'use_sim_time': True
            }]
        )
    ])

🗺️ 地图编辑与处理

#!/usr/bin/env python3
"""地图编辑工具 - 修改已保存的地图文件"""

import cv2
import numpy as np
import yaml
import os


class MapEditor:
    """地图编辑器：裁剪、膨胀、修补地图"""

    def __init__(self, map_yaml_path: str):
        self.yaml_path = map_yaml_path
        self.load_map(map_yaml_path)

    def load_map(self, yaml_path: str):
        """加载地图"""
        with open(yaml_path, 'r') as f:
            self.metadata = yaml.safe_load(f)

        # 地图图像路径（相对YAML文件）
        img_path = os.path.join(os.path.dirname(yaml_path), self.metadata['image'])
        self.map_img = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
        self.get_logger().info(f'加载地图: {self.map_img.shape}')

    def inflate_obstacles(self, radius_pixels: int):
        """膨胀障碍物（增加安全边距）"""
        # 创建二值障碍物图
        occupied_thresh = int(self.metadata.get('occupied_thresh', 0.65) * 255)
        binary = (self.map_img < (255 - occupied_thresh)).astype(np.uint8) * 255

        # 膨胀操作
        kernel = cv2.getStructuringElement(
            cv2.MORPH_ELLIPSE, (2 * radius_pixels + 1, 2 * radius_pixels + 1)
        )
        inflated = cv2.dilate(binary, kernel, iterations=1)

        # 合并回原图
        result = self.map_img.copy()
        result[inflated > 0] = 0  # 膨胀区域标记为障碍

        self.map_img = result
        return self

    def crop_map(self, x: int, y: int, w: int, h: int):
        """裁剪地图区域"""
        self.map_img = self.map_img[y:y+h, x:x+w]
        # 更新origin
        self.metadata['origin'][0] += x * self.metadata['resolution']
        self.metadata['origin'][1] += y * self.metadata['resolution']
        return self

    def fill_holes(self):
        """填充地图中的小孔洞"""
        # 将自由空间（白色）的小区域填充
        free_mask = (self.map_img > 200).astype(np.uint8)
        # 查找轮廓
        contours, _ = cv2.findContours(
            255 - free_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
        )
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area < 100:  # 小于100像素的孔洞
                cv2.drawContours(self.map_img, [cnt], 0, 205, -1)
        return self

    def save(self, output_path: str):
        """保存修改后的地图"""
        # 保存图像
        img_dir = os.path.dirname(output_path)
        img_name = os.path.basename(output_path).replace('.yaml', '.pgm')
        cv2.imwrite(os.path.join(img_dir, img_name), self.map_img)

        # 更新YAML
        self.metadata['image'] = img_name
        with open(output_path, 'w') as f:
            yaml.dump(self.metadata, f, default_flow_style=False)

        print(f'地图已保存: {output_path}')


# 使用示例
if __name__ == '__main__':
    editor = MapEditor('~/maps/my_map.yaml')
    editor.inflate_obstacles(radius_pixels=5)  # 膨胀5像素
    editor.fill_holes()                          # 填充孔洞
    editor.save('~/maps/my_map_inflated.yaml')

🔄 SLAM与Nav2集成

完整SLAM+Nav2启动文件

#!/usr/bin/env python3
"""SLAM导航启动文件 - 建图+导航一体化"""

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    # 获取包路径
    slam_dir = get_package_share_directory('slam_toolbox')
    nav_dir = get_package_share_directory('my_nav_pkg')

    # Launch参数
    use_sim_time = DeclareLaunchArgument(
        'use_sim_time', default_value='true'
    )
    slam_params = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(nav_dir, 'config', 'mapper_params_online_async.yaml')
    )

    # SLAM节点
    slam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(slam_dir, 'launch', 'online_async_launch.py')
        ),
        launch_arguments={
            'slam_params_file': LaunchConfiguration('slam_params_file'),
            'use_sim_time': LaunchConfiguration('use_sim_time'),
        }.items()
    )

    # Nav2导航
    nav_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav_dir, 'launch', 'navigation_launch.py')
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time'),
            'params_file': os.path.join(nav_dir, 'config', 'nav2_params.yaml'),
        }.items()
    )

    # 地图保存服务
    map_saver = Node(
        package='nav2_map_server',
        executable='map_saver_server',
        name='map_saver',
        output='screen',
        parameters=[{
            'save_map_timeout': 5000,
            'use_sim_time': LaunchConfiguration('use_sim_time'),
        }]
    )

    return LaunchDescription([
        use_sim_time,
        slam_params,
        slam_launch,
        nav_launch,
        map_saver,
    ])

📊 SLAM调优指南

🖥️ SLAM命令行工具

# 启动SLAM
ros2 launch slam_toolbox online_async_launch.py

# 查看地图话题
ros2 topic echo /map --once

# 查看TF树
ros2 run tf2_tools view_frames

# 保存地图
ros2 run nav2_map_server map_saver_cli -f ~/maps/my_map

# 序列化SLAM状态（暂停/恢复建图）
ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph "{filename: '/tmp/slam_state'}"

# 反序列化恢复
ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/tmp/slam_state', match_type: 1}"

🎯 练习题

# 终端1：启动Gazebo仿真
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

# 终端2：启动SLAM
ros2 launch turtlebot3_cartographer cartographer.launch.py \
  use_sim_time:=True

# 终端3：遥控建图
ros2 run turtlebot3_teleop teleop_keyboard

# 终端4：保存地图
ros2 run nav2_map_server map_saver_cli -f ~/maps/turtlebot3_map

# 录制数据
ros2 bag record /scan /tf /tf_static /odom -o slam_recording

# 离线处理
ros2 launch slam_toolbox offline_launch.py
ros2 bag play slam_recording --clock

🏆 成就解锁