Engineering Notebook // Build Log

2026.03.19

22:33:49

NOTEBOOK_ENTRY

slam

基于 ROS2 的 SLAM 导航小车完全教程本教程面向有 ROS2 基础但对 SLAM 和计算机视觉不太熟悉的开发者，以导航小车为载体，系统讲解 SLAM 原理与实践。目录 1. 引言 1 引言 2. SLAM 核心原理 2 slam 核心原理 3. 激光 SLAM 实战 3 激光 slam 实战 4. 视觉 SLAM 入门 4 视觉 slam 入门 5. Nav2 导航栈 5 nav2 导航栈 6. 导航小车硬件搭建 6 导航小…

Notebook Time

10 min

Image Frames

View Tracks

296

FIELD_GUIDE

FIELD GUIDE

Use the guide rail to jump between sections.

基于 ROS2 的 SLAM 导航小车完全教程

本教程面向有 ROS2 基础但对 SLAM 和计算机视觉不太熟悉的开发者，以导航小车为载体，系统讲解 SLAM 原理与实践。

1. 引言

1.1 什么是导航小车

导航小车是一种能够在环境中自主移动的移动机器人。它的核心能力包括：

感知：通过传感器（激光雷达、摄像头、IMU 等）获取周围环境信息
定位：确定自身在环境中的位置和姿态
建图：构建环境的地图表示
路径规划：计算从当前位置到目标位置的最优路径
运动控制：控制电机驱动小车沿规划路径行驶

典型应用场景包括：仓储物流机器人、服务机器人、巡检机器人、扫地机器人等。

1.2 教程目标

完成本教程后，你将能够：

理解 SLAM 的数学原理和工程实现
使用 slam_toolbox 进行激光 SLAM 建图
了解视觉 SLAM 的基本原理和 RTAB-Map 的使用
使用 Nav2 实现自主导航
在 Gazebo 仿真环境中完成完整的建图-导航流程

1.3 前置知识

ROS2 基础（节点、话题、服务、Action、Launch 文件）
Linux 基本操作
Python 或 C++ 基础
基本的线性代数知识（矩阵运算、坐标变换）

1.4 环境准备

本教程基于以下环境：

# 推荐系统
Ubuntu 22.04 LTS + ROS2 Humble Hawksbill

# 安装核心依赖
sudo apt update
sudo apt install -y \
  ros-humble-slam-toolbox \
  ros-humble-navigation2 \
  ros-humble-nav2-bringup \
  ros-humble-turtlebot3* \
  ros-humble-rtabmap-ros \
  ros-humble-gazebo-ros-pkgs \
  ros-humble-robot-localization \
  ros-humble-joint-state-publisher-gui

# 设置 TurtleBot3 模型（用于仿真示例）
echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
source ~/.bashrc

2. SLAM 核心原理

SLAM（Simultaneous Localization and Mapping，同时定位与建图）是移动机器人领域最核心的问题之一。简单来说：机器人在一个未知环境中移动，同时构建环境地图并确定自身在地图中的位置。

这是一个"鸡生蛋、蛋生鸡"的问题——要定位需要地图，要建图需要知道位置。SLAM 就是同时解决这两个相互依赖的问题。

2.1 SLAM 问题的数学描述

SLAM 问题可以用概率框架来描述。设：

x_t：t 时刻机器人的位姿（位置 + 朝向）
u_t：t 时刻的控制输入（如轮速）
z_t：t 时刻的传感器观测（如激光扫描）
m：环境地图

SLAM 要求解的是后验概率分布：

P(x_{1:t}, m | z_{1:t}, u_{1:t})

即：给定所有的观测和控制输入，求机器人轨迹和地图的联合概率分布。

2.2 SLAM 系统架构

一个完整的 SLAM 系统通常包含以下模块：

传感器数据
    │
    ▼
┌─────────┐
│  前 端   │ ← 处理传感器数据，估计短期运动
│ Frontend │   （特征提取、帧间匹配、里程计）
└────┬────┘
     │ 位姿估计 + 约束关系
     ▼
┌─────────┐
│  后 端   │ ← 全局优化，消除累积误差
│ Backend  │   （图优化 / 滤波器）
└────┬────┘
     │
     ▼
┌──────────┐
│ 回环检测  │ ← 识别"回到曾经去过的地方"
│Loop Close│   提供额外约束，修正漂移
└────┬─────┘
     │
     ▼
┌─────────┐
│ 地图构建  │ ← 生成最终地图
│ Mapping  │   （栅格图/点云/拓扑图）
└─────────┘

2.3 前端：传感器数据处理

前端的任务是从原始传感器数据中提取有用信息，估计机器人的相对运动。

2.3.1 激光雷达前端

激光雷达（LiDAR）发射激光束并测量反射时间，得到周围环境的距离信息。

扫描匹配（Scan Matching） 是激光 SLAM 前端的核心：

ICP（Iterative Closest Point，迭代最近点）：将两帧点云对齐，找到最优的旋转和平移变换
NDT（Normal Distributions Transform）：将点云转换为正态分布表示，通过优化分布的匹配度来对齐
相关性扫描匹配（Correlative Scan Matching）：在搜索空间中暴力搜索最佳匹配位置

 $帧 t - 1 的激光扫描帧 t 的激光扫描$ 
·  ·  ·                    ·  ·  ·
·        ·                 ·        ·
·    墙壁   ·     ──ICP──▶  ·    墙壁   ·
·        ·      求解 ΔT    ·        ·
·  ·  ·                    ·  ·  ·

 $Δ T = [Δ x, Δ y, Δ θ] \leftarrow 两帧之间的相对运动$

2.3.2 视觉前端

视觉前端从图像中提取特征点，通过特征匹配估计相机运动。

特征点法的基本流程：

特征检测：在图像中找到角点、斑点等显著特征（ORB、SIFT、SURF）
特征描述：为每个特征点生成描述子（一个向量，描述该点周围的图像信息）
特征匹配：在两帧图像之间匹配相似的特征点
运动估计：根据匹配的特征点对，计算相机的运动（本质矩阵 / PnP）

图像帧 t-1                    图像帧 t
┌──────────────┐            ┌──────────────┐
│  ★        ◆  │            │   ★       ◆  │
│     ●        │  特征匹配   │      ●       │
│  ▲      ■   │ ─────────▶ │   ▲     ■    │
│        ◆    │            │        ◆     │
└──────────────┘            └──────────────┘
   ★──★  ◆──◆  ●──●  ▲──▲  ■──■  ← 匹配对

通过匹配对求解相机运动 R, t

直接法不提取特征点，而是直接利用像素灰度信息：

假设同一个空间点在不同视角下的灰度值不变（灰度不变假设）
通过最小化光度误差来估计相机运动
优点：能利用更多像素信息，在纹理缺乏的场景也能工作
缺点：对光照变化敏感，计算量较大

2.4 后端：状态估计与优化

前端提供的是局部的、相对的运动估计，会不可避免地累积误差（漂移）。后端的任务是利用全局信息来优化整个轨迹和地图。

2.4.1 基于滤波的方法

扩展卡尔曼滤波（EKF-SLAM）：

将机器人位姿和所有路标点的位置组成一个大的状态向量
通过预测（运动模型）和更新（观测模型）两步迭代
优点：理论优雅，实时性好
缺点：状态向量随路标数量增长，计算复杂度为 O(n²)，不适合大规模环境
假设高斯分布，对非线性系统的近似可能不准确

 $状态向量 X = [x, y, θ, l 1_{x}, l 1_{y}, l 2_{x}, l 2_{y}, ...]$ 
机器人位姿    路标1位置    路标2位置

协方差矩阵 P = ┌                              ┐
│ P_xx  P_xl1  P_xl2  ...      │
│ P_l1x P_l1l1 P_l1l2 ...      │
│ P_l2x P_l2l1 P_l2l2 ...      │
│ ...                           │
└                              ┘

粒子滤波（Particle Filter SLAM）：

用一组带权重的粒子来表示概率分布
每个粒子代表一种可能的机器人轨迹
GMapping 就是基于 Rao-Blackwellized 粒子滤波的经典 SLAM 算法
优点：能处理非高斯、非线性问题
缺点：粒子数量多时计算量大，高维空间中粒子退化严重

2.4.2 基于图优化的方法（主流）

图优化（Graph-based SLAM） 是目前最主流的 SLAM 后端方法：

节点（Vertex）：代表机器人在各时刻的位姿
边（Edge）：代表两个位姿之间的约束关系（来自里程计、扫描匹配、回环检测等）

图优化示意：

x1 ──边(里程计)──▶ x2 ──边(里程计)──▶ x3
│                                      │
│              边(回环检测)              │
└──────────────────────────────────────┘

优化目标：调整所有节点的位姿，使得所有边的约束误差之和最小

 $min \sum e_{i} j^{T} Ω_{i} j e_{i} j$ 

其中：
 $e_{i} j = 位姿 i 和 j 之间的实际差值 - 观测到的差值$ 
 $Ω_{i} j = 信息矩阵（约束的置信度）$

常用的图优化库：

g2o（General Graph Optimization）
GTSAM（Georgia Tech Smoothing and Mapping）
Ceres Solver（Google 的非线性优化库）

2.5 回环检测（Loop Closure）

回环检测是 SLAM 中消除累积漂移的关键。当机器人回到之前去过的地方时，回环检测能识别出这一事实，并提供一个强约束来修正整个轨迹。

没有回环检测时（漂移累积）：

  起点 ──▶ ──▶ ──▶ ──▶ ──▶ ──▶ ┐
                                 │
  实际回到起点，但估计位置偏了 ◀──┘
  （起点和终点之间有明显间隙）

有回环检测时：

  起点 ──▶ ──▶ ──▶ ──▶ ──▶ ──▶ ┐
    ▲                            │
    └── 检测到回环！添加约束 ◀────┘
  （整条轨迹被优化，间隙消除）

激光 SLAM 的回环检测：

将当前扫描与历史扫描进行匹配
使用分支定界（Branch and Bound）加速搜索（如 Cartographer）

视觉 SLAM 的回环检测：

词袋模型（Bag of Words, BoW）：将图像特征量化为"视觉单词"，通过比较"单词"的分布来判断两幅图像是否相似
DBoW2 / DBoW3：常用的视觉词袋库

2.6 地图表示

不同的地图表示方式适用于不同的场景：

地图类型	描述	适用场景	示例
栅格地图	将环境划分为网格，每个格子标记为占据/空闲/未知	2D 导航、路径规划	Nav2 使用的 costmap
点云地图	由大量 3D 点组成的地图	3D 环境表示、精细建模	PCL 点云
特征地图	只保存特征点/路标的位置	定位、稀疏表示	ORB-SLAM 的地图
拓扑地图	用节点和边表示环境的连通关系	大规模环境、高层规划	房间-走廊连接图
八叉树地图	用八叉树结构表示 3D 占据信息	3D 导航、无人机	OctoMap

对于导航小车，最常用的是 2D 占据栅格地图（Occupancy Grid Map）：

栅格地图示例（0=空闲, 1=占据, -1=未知）：

  -1 -1 -1 -1 -1 -1 -1 -1
  -1  1  1  1  1  1  1 -1
  -1  1  0  0  0  0  1 -1
  -1  1  0  0  0  0  1 -1
  -1  1  0  0  R  0  1 -1    R = 机器人位置
  -1  1  0  0  0  0  1 -1
  -1  1  1  1  D  1  1 -1    D = 门
  -1 -1 -1 -1 -1 -1 -1 -1

2.7 主流 SLAM 算法对比

算法	传感器	方法	特点	ROS2 支持
slam_toolbox	2D 激光	图优化	ROS2 原生支持，功能全面	✅ 官方推荐
Cartographer	2D/3D 激光	子图+回环	Google 出品，精度高	✅ 有移植
GMapping	2D 激光	粒子滤波	经典算法，简单易用	⚠️ 仅 ROS1
Hector SLAM	2D 激光	扫描匹配	不需要里程计	⚠️ 有限支持
ORB-SLAM3	单目/双目/RGB-D	特征点法	精度高，支持多传感器	⚠️ 需自行适配
RTAB-Map	视觉+激光	图优化	多传感器融合，功能强大	✅ 官方支持
LIO-SAM	3D 激光+IMU	因子图	紧耦合激光惯性里程计	✅ 有移植
FAST-LIO2	3D 激光+IMU	迭代卡尔曼	极快速度，适合嵌入式	✅ 有移植

对于导航小车入门，推荐路线：

先用 slam_toolbox（激光 SLAM）快速上手
再学习 RTAB-Map（视觉 SLAM）拓展能力

3. 激光 SLAM 实战

激光 SLAM 是导航小车最成熟、最常用的建图方案。本章以 slam_toolbox 为主，辅以 Cartographer 的介绍。

3.1 激光雷达基础

3.1.1 工作原理

激光雷达通过发射激光脉冲并测量反射时间来计算距离：

距离 = (光速 × 飞行时间) / 2

2D 激光雷达通过旋转扫描，在一个平面内获取 360°（或指定角度范围）的距离数据，形成一帧"激光扫描"。

3.1.2 常见 2D 激光雷达选型

型号	测距范围	扫描频率	角分辨率	参考价格	适用场景
RPLidar A1	12m	5.5Hz	1°	~500 元	入门学习
RPLidar A2	18m	10Hz	0.45°	~1200 元	室内导航
思岚 RPLidar S1	40m	10Hz	0.39°	~2000 元	室内外通用
YDLIDAR X4	10m	7Hz	0.5°	~400 元	低成本入门
北醒 TFmini Plus	12m（单点）	1000Hz	-	~200 元	避障辅助

入门推荐：RPLidar A1 或 YDLIDAR X4，性价比高，ROS2 驱动成熟。

3.1.3 ROS2 中的激光数据

激光雷达在 ROS2 中发布 sensor_msgs/msg/LaserScan 消息：

# LaserScan 消息结构
header:
  stamp: {sec: 1234567890, nanosec: 0}
  frame_id: "laser_frame"
angle_min: -3.14159      # 起始角度（弧度）
angle_max: 3.14159       # 终止角度
angle_increment: 0.0175  # 角度增量（约1°）
time_increment: 0.0001   # 测量时间增量
scan_time: 0.1           # 完整扫描时间
range_min: 0.15          # 最小测距
range_max: 12.0          # 最大测距
ranges: [1.2, 1.3, ...]  # 距离数组
intensities: [47, 52, ...]  # 强度数组（可选）

3.2 slam_toolbox 详解

slam_toolbox 是 ROS2 官方推荐的 2D SLAM 解决方案，由 Steve Macenski（Nav2 的主要开发者）开发。

3.2.1 核心特性

基于图优化的 SLAM
支持在线（实时）和离线（回放）建图
支持增量建图（Lifelong SLAM）：可以在已有地图上继续建图
支持序列化：保存和加载地图及位姿图
支持多种模式：建图模式、纯定位模式、持续建图模式

3.2.2 安装与配置

# 安装（如果之前没有安装）
sudo apt install ros-humble-slam-toolbox

# 查看默认配置文件位置
ros2 pkg prefix slam_toolbox
# 配置文件在：/opt/ros/humble/share/slam_toolbox/config/

3.2.3 核心参数详解

创建自定义配置文件 my_slam_params.yaml：

slam_toolbox:
  ros__parameters:
    # ===== 基本设置 =====
    solver_plugin: solver_plugins::CeresSolver  # 优化求解器
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY  # Ceres 线性求解器类型
    ceres_preconditioner: SCHUR_JACOBI            # 预条件器
    ceres_trust_strategy: LEVENBERG_MARQUARDT     # 信赖域策略
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None                     # 损失函数（鲁棒核）

    # ===== 话题和坐标系 =====
    odom_frame: odom            # 里程计坐标系
    map_frame: map              # 地图坐标系
    base_frame: base_footprint  # 机器人基座坐标系
    scan_topic: /scan           # 激光扫描话题
    use_map_saver: true         # 是否使用地图保存服务

    # ===== 建图模式 =====
    # mapping: 建图模式（默认）
    # localization: 纯定位模式（使用已有地图）
    mode: mapping

    # ===== 扫描匹配参数 =====
    # 这些参数直接影响建图质量
    resolution: 0.05            # 地图分辨率（米/像素），0.05 = 5cm
    max_laser_range: 12.0       # 最大激光使用范围（超出的数据丢弃）
    minimum_travel_distance: 0.5  # 最小移动距离才添加新节点（米）
    minimum_travel_heading: 0.5   # 最小旋转角度才添加新节点（弧度）
    scan_buffer_size: 10          # 扫描缓冲区大小
    scan_buffer_maximum_scan_distance: 10.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  # 高斯模糊偏差

    # ===== 更新间隔 =====
    transform_publish_period: 0.02  # TF 发布周期（秒）
    map_update_interval: 5.0        # 地图更新间隔（秒）

    # ===== 性能调优 =====
    enable_interactive_mode: true   # 启用 RViz 交互模式
    use_scan_matching: true         # 使用扫描匹配（强烈建议开启）
    use_scan_barycenter: true       # 使用扫描重心

    # ===== 调试 =====
    debug_logging: false
    throttle_scans: 1  # 每 N 帧处理一次（1=全部处理）

关键参数调优建议：

参数	调大效果	调小效果	建议
`resolution`	地图粗糙，计算快	地图精细，计算慢	室内 0.05，室外 0.1
`minimum_travel_distance`	节点少，地图稀疏	节点多，地图密集	0.3~0.5
`max_laser_range`	使用更远的数据	只用近处数据	设为雷达实际量程的 80%
`loop_match_minimum_response_coarse`	回环检测更严格	回环检测更宽松	0.3~0.5

3.2.4 Launch 文件

创建 slam_launch.py：

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 声明参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    slam_params_file = LaunchConfiguration('slam_params_file', default='')

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'
        ),
        DeclareLaunchArgument(
            'slam_params_file',
            default_value=os.path.join(
                get_package_share_directory('my_robot_slam'),
                'config',
                'my_slam_params.yaml'
            ),
            description='Full path to the SLAM Toolbox parameters file'
        ),

        # slam_toolbox 节点
        Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',  # 异步模式（推荐）
            name='slam_toolbox',
            output='screen',
            parameters=[
                slam_params_file,
                {'use_sim_time': use_sim_time}
            ],
        ),

        # RViz2 可视化
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', os.path.join(
                get_package_share_directory('my_robot_slam'),
                'rviz',
                'slam.rviz'
            )],
            parameters=[{'use_sim_time': use_sim_time}],
        ),
    ])

async vs sync 模式：

async_slam_toolbox_node（异步）：扫描处理不阻塞，适合实时建图

sync_slam_toolbox_node（同步）：每帧都处理，适合离线建图或高精度需求

3.2.5 实际建图流程

# 终端 1：启动机器人底盘和激光雷达
ros2 launch my_robot_bringup robot.launch.py

# 终端 2：启动 SLAM
ros2 launch my_robot_slam slam_launch.py use_sim_time:=false

# 终端 3：用键盘遥控机器人建图
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# 终端 4：建图完成后保存地图
ros2 run nav2_map_server map_saver_cli -f ~/maps/my_map

建图技巧：

慢速移动：移动速度不要太快，给 SLAM 足够的处理时间
多次经过：重要区域多走几遍，增加约束
闭合回路：尽量走回起点，触发回环检测
避免长走廊：长走廊特征少，容易漂移，可以蛇形走位
注意动态障碍物：建图时尽量减少环境中的移动物体

保存的地图包含两个文件：

~/maps/my_map.pgm   # 地图图像（PGM 格式）
~/maps/my_map.yaml   # 地图元数据

地图 YAML 文件内容：

image: my_map.pgm
resolution: 0.050000          # 分辨率（米/像素）
origin: [-10.0, -10.0, 0.0]  # 地图原点 [x, y, yaw]
negate: 0
occupied_thresh: 0.65         # 占据阈值
free_thresh: 0.196            # 空闲阈值

3.3 Cartographer 简介

Google Cartographer 是另一个优秀的 SLAM 方案，采用子图（Submap）+ 回环检测的架构。

3.3.1 核心思想

Cartographer 架构：

  激光扫描 ──▶ Local SLAM ──▶ 子图 (Submap)
                  │                │
                  │                ▼
                  │         Global SLAM
                  │           (回环检测 + 优化)
                  │                │
                  ▼                ▼
              局部轨迹          全局一致的地图

Local SLAM：将连续的激光扫描插入到当前子图中，使用 Ceres 优化扫描匹配
Global SLAM：在后台运行，检测子图之间的回环，优化全局位姿图

3.3.2 ROS2 中使用 Cartographer

# 安装
sudo apt install ros-humble-cartographer ros-humble-cartographer-ros

# 配置文件示例 my_robot.lua

-- 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.,
}

-- 2D SLAM 配置
MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 12.0
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.0
TRAJECTORY_BUILDER_2D.use_imu_data = false  -- 如果没有 IMU
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.0)

-- 子图参数
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05

-- 回环检测
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

return options

3.4 slam_toolbox vs Cartographer 对比

特性	slam_toolbox	Cartographer
ROS2 支持	原生支持，官方推荐	社区移植，维护较少
配置难度	YAML，简单直观	Lua，学习曲线较陡
增量建图	✅ 支持 Lifelong SLAM	❌ 不支持
纯定位模式	✅ 内置	✅ 支持
3D SLAM	❌ 仅 2D	✅ 支持 3D
建图质量	优秀	优秀（大环境略优）
计算资源	较低	较高
社区活跃度	活跃	Google 已停止维护

结论：对于 2D 导航小车，优先选择 slam_toolbox。

4. 视觉 SLAM 入门

视觉 SLAM 使用摄像头作为主要传感器，成本低、信息丰富，但算法复杂度更高。本章帮助你理解视觉 SLAM 的核心概念，并通过 RTAB-Map 在 ROS2 中实践。

4.1 相机模型与标定

4.1.1 针孔相机模型

相机将 3D 世界投影到 2D 图像平面，最基本的模型是针孔模型：

3D 世界点 P = [X, Y, Z]

投影到图像平面：
  u = fx * (X/Z) + cx
  v = fy * (Y/Z) + cy

相机内参矩阵 K：
  K = ┌ fx  0  cx ┐
      │  0  fy cy │
      └  0   0  1 ┘

其中：
  fx, fy = 焦距（像素单位）
  cx, cy = 主点（通常接近图像中心）

4.1.2 相机类型对比

类型	优点	缺点	深度信息	代表产品
单目相机	成本低、体积小	无法直接获取深度，存在尺度不确定性	❌ 需要运动估计	普通 USB 摄像头
双目相机	可计算深度，成本适中	计算量大，基线限制测距范围	✅ 视差计算	ZED 2, Intel T265
RGB-D 相机	直接输出深度图，使用简单	室外受阳光干扰，范围有限	✅ 结构光/ToF	Intel RealSense D435, Azure Kinect

导航小车推荐：Intel RealSense D435i（RGB-D + IMU），性价比高，ROS2 支持好。

4.1.3 相机标定

相机标定是视觉 SLAM 的前提，用于确定相机内参和畸变系数。

# 安装标定工具
sudo apt install ros-humble-camera-calibration

# 打印棋盘格标定板（8x6 内角点，方格边长 0.025m）
# 可以从网上下载打印

# 启动相机节点（以 RealSense 为例）
ros2 launch realsense2_camera rs_launch.py

# 启动标定工具
ros2 run camera_calibration cameracalibrator \
  --size 8x6 \
  --square 0.025 \
  --ros-args --remap image:=/camera/color/image_raw \
  --remap camera:=/camera/color

标定步骤：

在不同角度、距离、位置展示标定板
工具会自动检测角点并采集样本
当 X、Y、Size、Skew 四个进度条都变绿时，点击 CALIBRATE
标定完成后点击 SAVE，保存标定文件
点击 COMMIT 将标定参数写入相机驱动

4.2 视觉 SLAM 核心概念

4.2.1 特征点法 vs 直接法

特征点法（以 ORB-SLAM3 为代表）：

图像 ──▶ 特征检测 ──▶ 特征描述 ──▶ 特征匹配 ──▶ 运动估计
         (ORB)       (二进制描述子)  (汉明距离)    (PnP/对极几何)

优点：
  - 对光照变化有一定鲁棒性
  - 匹配准确，定位精度高
  - 回环检测成熟（词袋模型）

缺点：
  - 特征提取耗时
  - 纹理缺乏的场景（白墙）特征点少
  - 只利用了图像中的少量信息

直接法（以 LSD-SLAM、DSO 为代表）：

图像 ──▶ 选取像素 ──▶ 光度误差最小化 ──▶ 运动估计
         (梯度大的像素)  (灰度不变假设)

优点：
  - 不需要特征提取，速度可以更快
  - 能利用更多像素信息
  - 在弱纹理场景也能工作（半稠密/稠密方法）

缺点：
  - 对光照变化非常敏感
  - 需要好的初始值
  - 回环检测较困难

4.2.2 视觉里程计（Visual Odometry, VO）

视觉里程计是视觉 SLAM 的前端，估计相机的帧间运动：

单目 VO 流程：

帧 t-1          帧 t
  │               │
  ▼               ▼
特征提取        特征提取
  │               │
  └──── 特征匹配 ──┘
          │
          ▼
    本质矩阵 E = t^ · R
          │
          ▼
    分解得到 R, t（旋转和平移）
          │
          ▼
    三角化得到 3D 点

RGB-D VO 流程（更简单）：

帧 t-1 (RGB + Depth)     帧 t (RGB + Depth)
  │                         │
  ▼                         ▼
特征提取 + 深度关联        特征提取 + 深度关联
  │                         │
  └──── 3D-3D 匹配 ────────┘
          │
          ▼
    ICP 或 PnP 求解 R, t

4.2.3 关键帧策略

不是每一帧都需要处理，视觉 SLAM 通常使用关键帧策略：

当相机移动足够距离或旋转足够角度时，插入新关键帧
当跟踪到的特征点数量下降到阈值以下时，插入新关键帧
关键帧用于后端优化和地图维护
非关键帧只用于实时跟踪

4.3 RTAB-Map 实战

RTAB-Map（Real-Time Appearance-Based Mapping）是一个多传感器 SLAM 方案，支持激光雷达、RGB-D 相机、双目相机等，在 ROS2 中有良好的支持。

4.3.1 RTAB-Map 架构

传感器输入
├── RGB-D 相机 ──▶ 视觉里程计 (Visual Odometry)
├── 双目相机   ──▶ 视觉里程计
├── 激光雷达   ──▶ ICP 里程计
└── IMU        ──▶ 预积分

        │
        ▼
  短期记忆 (STM)  ← 最近的关键帧
        │
        ▼
  工作记忆 (WM)   ← 活跃的关键帧（用于回环检测）
        │
        ▼
  长期记忆 (LTM)  ← 不活跃的关键帧（存入数据库）

回环检测：基于视觉词袋模型，在 WM 中搜索相似帧
图优化：检测到回环后，优化全局位姿图

RTAB-Map 的独特之处在于其内存管理机制：通过 STM → WM → LTM 的转移，保证实时性，适合大规模环境。

4.3.2 安装与配置

# 安装
sudo apt install ros-humble-rtabmap-ros

# 安装 RealSense 驱动（如果使用 RealSense 相机）
sudo apt install ros-humble-realsense2-camera

4.3.3 使用 RGB-D 相机建图

创建 rtabmap_rgbd_launch.py：

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='false'),

        # RealSense 相机节点
        Node(
            package='realsense2_camera',
            executable='realsense2_camera_node',
            name='camera',
            parameters=[{
                'enable_color': True,
                'enable_depth': True,
                'enable_infra1': False,
                'enable_infra2': False,
                'enable_gyro': True,
                'enable_accel': True,
                'align_depth.enable': True,  # 对齐深度到彩色
                'rgb_camera.profile': '640x480x30',
                'depth_module.profile': '640x480x30',
            }],
        ),

        # RTAB-Map 视觉里程计
        Node(
            package='rtabmap_odom',
            executable='rgbd_odometry',
            name='rgbd_odometry',
            output='screen',
            parameters=[{
                'use_sim_time': use_sim_time,
                'frame_id': 'base_link',
                'odom_frame_id': 'odom',
                'publish_tf': True,
                'approx_sync': True,
                'Odom/Strategy': '0',       # 0=Frame-to-Map, 1=Frame-to-Frame
                'OdomF2M/MaxSize': '1000',   # Frame-to-Map 最大特征数
                'Vis/MaxFeatures': '600',    # 最大视觉特征数
                'Vis/MinInliers': '20',      # 最小内点数
            }],
            remappings=[
                ('rgb/image', '/camera/color/image_raw'),
                ('rgb/camera_info', '/camera/color/camera_info'),
                ('depth/image', '/camera/aligned_depth_to_color/image_raw'),
            ],
        ),

        # RTAB-Map SLAM 节点
        Node(
            package='rtabmap_slam',
            executable='rtabmap',
            name='rtabmap',
            output='screen',
            parameters=[{
                'use_sim_time': use_sim_time,
                'frame_id': 'base_link',
                'odom_frame_id': 'odom',
                'map_frame_id': 'map',
                'subscribe_depth': True,
                'subscribe_rgb': True,
                'approx_sync': True,
                'queue_size': 10,

                # 建图参数
                'Mem/IncrementalMemory': 'true',   # 增量建图模式
                'Mem/InitWMWithAllNodes': 'false',

                # 回环检测
                'Kp/MaxFeatures': '400',           # 关键点最大数量
                'Kp/DetectorStrategy': '6',        # 6=ORB
                'RGBD/NeighborLinkRefining': 'true',
                'RGBD/ProximityBySpace': 'true',
                'RGBD/OptimizeFromGraphEnd': 'false',

                # 栅格地图生成（用于导航）
                'Grid/FromDepth': 'true',          # 从深度图生成栅格
                'Grid/MaxGroundHeight': '0.05',    # 地面最大高度
                'Grid/MaxObstacleHeight': '1.5',   # 障碍物最大高度
                'Grid/CellSize': '0.05',           # 栅格分辨率
                'Grid/RangeMax': '5.0',            # 最大范围
            }],
            remappings=[
                ('rgb/image', '/camera/color/image_raw'),
                ('rgb/camera_info', '/camera/color/camera_info'),
                ('depth/image', '/camera/aligned_depth_to_color/image_raw'),
            ],
        ),

        # RViz2
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            parameters=[{'use_sim_time': use_sim_time}],
        ),
    ])

4.3.4 建图与定位切换

# 建图模式（默认）
ros2 launch my_robot_slam rtabmap_rgbd_launch.py

# 遥控机器人建图...

# 建图完成后，RTAB-Map 自动保存数据库到 ~/.ros/rtabmap.db

# 定位模式（使用已有地图）
ros2 launch my_robot_slam rtabmap_rgbd_launch.py \
  localization:=true

在定位模式下，需要将 RTAB-Map 参数修改为：

# 定位模式参数
'Mem/IncrementalMemory': 'false',   # 关闭增量建图
'Mem/InitWMWithAllNodes': 'true',   # 加载所有节点到工作记忆

4.3.5 RTAB-Map 可视化工具

RTAB-Map 提供了独立的可视化工具 rtabmap-databaseViewer，可以查看和分析建图结果：

# 安装可视化工具
sudo apt install rtabmap

# 打开数据库查看器
rtabmap-databaseViewer ~/.ros/rtabmap.db

在查看器中可以：

查看所有关键帧和它们的特征点
查看回环检测结果
查看位姿图和优化结果
导出 3D 点云和 2D 栅格地图

4.4 ORB-SLAM3 简介

ORB-SLAM3 是目前最完整的视觉 SLAM 系统之一，支持单目、双目、RGB-D 相机以及 IMU 融合。

4.4.1 系统架构

ORB-SLAM3 三线程架构：

线程 1：跟踪 (Tracking)
  - ORB 特征提取
  - 初始位姿估计（恒速模型 / 参考关键帧 / 重定位）
  - 局部地图跟踪
  - 关键帧决策

线程 2：局部建图 (Local Mapping)
  - 处理新关键帧
  - 局部 BA（Bundle Adjustment）
  - 地图点筛选
  - 新地图点创建

线程 3：回环检测与地图合并 (Loop Closing & Map Merging)
  - DBoW2 词袋检测回环
  - Sim(3) / SE(3) 位姿计算
  - 全局 BA 优化
  - 多地图合并（Atlas 系统）

4.4.2 在 ROS2 中使用

ORB-SLAM3 没有官方的 ROS2 包，需要使用社区移植版本：

# 安装依赖
sudo apt install libopencv-dev libeigen3-dev libpangolin-dev

# 克隆社区 ROS2 wrapper
cd ~/ros2_ws/src
git clone https://github.com/zang09/ORB_SLAM3_ROS2.git

# 编译
cd ~/ros2_ws
colcon build --packages-select orbslam3

# 运行（以 RGB-D 模式为例）
ros2 run orbslam3 rgbd \
  --ros-args \
  -p voc_file:=path/to/ORBvoc.txt \
  -p settings_file:=path/to/RealSense_D435i.yaml

注意：ORB-SLAM3 的 ROS2 适配可能需要根据具体版本调整，建议优先使用 RTAB-Map。

4.5 视觉 SLAM 常见问题

问题	可能原因	解决方案
跟踪丢失	快速运动、特征点不足	降低运动速度，增加特征点数量
尺度漂移（单目）	单目相机无法确定绝对尺度	使用 RGB-D 或双目相机，或融合 IMU
建图不一致	回环检测失败	确保回到已建图区域，调整回环检测阈值
深度图噪声大	反光表面、透明物体	调整深度滤波参数，避免对着玻璃/镜面
计算资源不足	分辨率过高、特征点过多	降低图像分辨率，减少最大特征点数

5. Nav2 导航栈

Nav2（Navigation2）是 ROS2 的官方导航框架，提供了从路径规划到运动控制的完整导航能力。SLAM 建好地图后，Nav2 负责让小车在地图中自主移动。

5.1 Nav2 架构概览

                    ┌─────────────────────────────────┐
                    │         BT Navigator             │
                    │   （行为树，协调整个导航流程）      │
                    └──────────┬──────────────────────┘
                               │
            ┌──────────────────┼──────────────────┐
            ▼                  ▼                  ▼
    ┌──────────────┐  ┌──────────────┐  ┌──────────────┐
    │   Planner    │  │  Controller  │  │  Recovery    │
    │  Server      │  │  Server      │  │  Server      │
    │ （全局规划）  │  │ （局部控制）  │  │ （恢复行为）  │
    └──────┬───────┘  └──────┬───────┘  └──────┬───────┘
           │                 │                 │
           ▼                 ▼                 ▼
    ┌──────────────┐  ┌──────────────┐  ┌──────────────┐
    │  NavFn /     │  │  DWB /       │  │  Spin /      │
    │  Smac /      │  │  MPPI /      │  │  BackUp /    │
    │  Theta*      │  │  RPP         │  │  Wait        │
    └──────────────┘  └──────────────┘  └──────────────┘
           │                 │
           ▼                 ▼
    ┌─────────────────────────────────┐
    │         Costmap 2D              │
    │  ┌───────────┐ ┌────────────┐  │
    │  │ Global    │ │ Local      │  │
    │  │ Costmap   │ │ Costmap    │  │
    │  └───────────┘ └────────────┘  │
    └─────────────────────────────────┘
           │                 │
           ▼                 ▼
    ┌─────────────────────────────────┐
    │    Map Server + AMCL            │
    │  （地图服务 + 自适应蒙特卡洛定位）│
    └─────────────────────────────────┘

5.2 核心组件详解

5.2.1 Map Server（地图服务器）

加载 SLAM 建好的地图，提供给导航系统使用：

# map_server 参数
map_server:
  ros__parameters:
    yaml_filename: "/home/user/maps/my_map.yaml"
    topic_name: "map"
    frame_id: "map"

5.2.2 AMCL（自适应蒙特卡洛定位）

AMCL 使用粒子滤波在已知地图中定位机器人：

AMCL 工作原理：

1. 初始化：在地图上撒大量粒子（每个粒子代表一种可能的位姿）

   ·  ·  ·  ·  ·  ·  ·  ·  ← 粒子均匀分布
   ·  ·  ·  ·  ·  ·  ·  ·
   ·  ·  ·  ·  ·  ·  ·  ·

2. 预测：根据里程计移动所有粒子

3. 更新：用激光扫描数据评估每个粒子的权重
   - 粒子位姿与实际观测匹配度高 → 权重大
   - 粒子位姿与实际观测匹配度低 → 权重小

4. 重采样：丢弃低权重粒子，复制高权重粒子

   ·                       ← 粒子聚集到正确位置
   · · ·
   · · · · ·  ← 机器人真实位置附近
   · · ·
   ·

5. 重复 2-4，粒子逐渐收敛到真实位姿

# AMCL 关键参数
amcl:
  ros__parameters:
    # 粒子数量
    min_particles: 500
    max_particles: 2000

    # 里程计模型
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    alpha1: 0.2   # 旋转噪声（由旋转引起）
    alpha2: 0.2   # 旋转噪声（由平移引起）
    alpha3: 0.2   # 平移噪声（由平移引起）
    alpha4: 0.2   # 平移噪声（由旋转引起）
    alpha5: 0.2   # 平移噪声（原地旋转）

    # 激光模型
    laser_model_type: "likelihood_field"
    laser_max_range: 12.0
    max_beams: 60
    z_hit: 0.5
    z_rand: 0.5
    sigma_hit: 0.2

    # 更新阈值
    update_min_d: 0.25    # 最小移动距离才更新（米）
    update_min_a: 0.2     # 最小旋转角度才更新（弧度）

    # 坐标系
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    global_frame_id: "map"

    # 初始位姿（可选，也可在 RViz 中设置）
    set_initial_pose: true
    initial_pose:
      x: 0.0
      y: 0.0
      z: 0.0
      yaw: 0.0

5.2.3 Costmap 2D（代价地图）

Costmap 是导航的核心数据结构，将环境信息转化为"代价值"供规划器使用：

代价值含义：
  0        = 空闲空间（Free）
  1-252    = 代价递增（离障碍物越近代价越高）
  253      = 内切障碍物（Inscribed）
  254      = 致命障碍物（Lethal）
  255      = 未知（No Information）

代价地图可视化（俯视图）：

  ████████████████████████
  █                      █
  █   ░░░░░              █    █ = 致命障碍物 (254)
  █   ░░██░░             █    ░ = 膨胀区域 (1-253)
  █   ░░░░░░             █      = 空闲空间 (0)
  █          ░░░░░       █
  █          ░░██░░      █
  █          ░░░░░░      █
  █                      █
  ████████████████████████

Costmap 由多个**层（Layer）**叠加而成：

# Global Costmap 配置
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.15          # 机器人半径
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

      # 静态层：加载 SLAM 建好的地图
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true

      # 障碍物层：实时传感器数据
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          raytrace_max_range: 5.0
          obstacle_max_range: 4.5

      # 膨胀层：在障碍物周围添加安全距离
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0    # 代价衰减速度
        inflation_radius: 0.55      # 膨胀半径

# Local Costmap 配置
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      robot_radius: 0.15
      resolution: 0.05
      rolling_window: true        # 滚动窗口模式
      width: 3                    # 窗口宽度（米）
      height: 3                   # 窗口高度（米）
      plugins: ["voxel_layer", "inflation_layer"]

      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

5.2.4 Planner Server（全局路径规划）

全局规划器在 Global Costmap 上计算从起点到终点的路径：

规划器	算法	特点	适用场景
NavFn	Dijkstra / A*	经典算法，稳定可靠	通用场景
Smac Planner 2D	A* 变体	支持多种启发式	2D 导航
Smac Hybrid-A*	Hybrid A*	考虑运动学约束	阿克曼转向车辆
Smac Lattice	状态格子	精确运动学模型	复杂运动学
Theta*	Theta*	任意角度路径，更平滑	全向移动机器人

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5            # 目标容差（米）
      use_astar: true           # 使用 A*（false 则用 Dijkstra）
      allow_unknown: true       # 允许规划经过未知区域

5.2.5 Controller Server（局部运动控制）

局部控制器根据全局路径生成实时速度指令：

控制器	特点	适用场景
DWB	动态窗口法，经典可靠	差速驱动小车
MPPI	模型预测路径积分，效果好	各种底盘类型
RPP	纯追踪法，简单高效	路径跟踪
Rotation Shim	先原地旋转对准方向	配合其他控制器使用

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    controller_plugins: ["FollowPath"]

    # DWB 控制器配置
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: true
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26            # 最大前进速度
      max_vel_y: 0.0             # 差速驱动无侧向速度
      max_vel_theta: 1.0         # 最大旋转速度
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5             # 前进加速度限制
      acc_lim_y: 0.0
      acc_lim_theta: 3.2         # 旋转加速度限制
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20             # 速度采样数
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7              # 模拟时间
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25    # 到达目标的位置容差
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: true
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle",
                "GoalAlign", "PathAlign", "PathDist", "GoalDist"]

5.2.6 Recovery Server（恢复行为）

当导航遇到困难时（如被卡住），恢复行为帮助机器人脱困：

recoveries_server:
  ros__parameters:
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"

5.3 完整导航 Launch 文件

import os
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

def generate_launch_description():
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    my_robot_dir = get_package_share_directory('my_robot_navigation')

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    map_file = LaunchConfiguration('map', default=os.path.join(
        my_robot_dir, 'maps', 'my_map.yaml'))
    params_file = LaunchConfiguration('params_file', default=os.path.join(
        my_robot_dir, 'config', 'nav2_params.yaml'))

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='false'),
        DeclareLaunchArgument('map', default_value=map_file),
        DeclareLaunchArgument('params_file', default_value=params_file),

        # 启动 Nav2
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
            ),
            launch_arguments={
                'use_sim_time': use_sim_time,
                'map': map_file,
                'params_file': params_file,
            }.items(),
        ),

        # RViz2
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')
            ),
            launch_arguments={
                'use_sim_time': use_sim_time,
            }.items(),
        ),
    ])

5.4 导航实操流程

# 步骤 1：启动机器人底盘和传感器
ros2 launch my_robot_bringup robot.launch.py

# 步骤 2：启动导航
ros2 launch my_robot_navigation navigation_launch.py \
  map:=/home/user/maps/my_map.yaml \
  use_sim_time:=false

# 步骤 3：在 RViz 中操作
# 3a. 点击 "2D Pose Estimate" 设置初始位姿
#     （在地图上点击并拖动，指示机器人当前位置和朝向）
# 3b. 观察 AMCL 粒子收敛
# 3c. 点击 "Nav2 Goal" 设置导航目标
#     （在地图上点击并拖动，指示目标位置和朝向）
# 3d. 机器人开始自主导航！

5.5 通过代码发送导航目标

#!/usr/bin/env python3
"""通过代码发送导航目标点"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from rclpy.duration import Duration

def main():
    rclpy.init()
    navigator = BasicNavigator()

    # 等待 Nav2 启动完成
    navigator.waitUntilNav2Active()

    # --- 设置初始位姿 ---
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 0.0
    initial_pose.pose.position.y = 0.0
    initial_pose.pose.orientation.w = 1.0
    navigator.setInitialPose(initial_pose)

    # --- 单点导航 ---
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 2.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0

    navigator.goToPose(goal_pose)

    # 等待导航完成
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        if feedback:
            # 打印剩余距离
            print(f'剩余距离: {feedback.distance_remaining:.2f}m')

        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=120.0):
            navigator.cancelTask()
            print('导航超时，取消任务')
            break

    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('导航成功！')
    elif result == TaskResult.CANCELED:
        print('导航被取消')
    elif result == TaskResult.FAILED:
        print('导航失败')

    # --- 多点巡航（Waypoint Following）---
    waypoints = []
    for pt in [(1.0, 0.0), (2.0, 1.0), (1.0, 2.0), (0.0, 1.0)]:
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = navigator.get_clock().now().to_msg()
        pose.pose.position.x = pt[0]
        pose.pose.position.y = pt[1]
        pose.pose.orientation.w = 1.0
        waypoints.append(pose)

    navigator.followWaypoints(waypoints)

    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        if feedback:
            print(f'正在前往第 {feedback.current_waypoint + 1}/{len(waypoints)} 个点')

    print('巡航完成！')
    navigator.lifecycleShutdown()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

5.6 TF 坐标系关系

Nav2 依赖正确的 TF 树，这是导航能否正常工作的关键：

TF 树（导航小车典型结构）：

  map
   │
   │  (由 AMCL 或 SLAM 发布)
   ▼
  odom
   │
   │  (由里程计/轮式编码器发布)
   ▼
  base_footprint
   │
   │  (静态 TF)
   ▼
  base_link
   ├──────────────┬──────────────┐
   │              │              │
   ▼              ▼              ▼
 laser_frame   camera_link    imu_link
               │
               ▼
            camera_optical_frame

关键变换：
  map → odom        : 全局定位修正（AMCL 发布）
  odom → base_link  : 里程计（连续但有漂移）
  base_link → 传感器 : 静态变换（URDF 定义）

确保 TF 正确的检查方法：

# 查看 TF 树
ros2 run tf2_tools view_frames

# 查看特定变换
ros2 run tf2_ros tf2_echo map base_link

# 检查 TF 是否有断裂
ros2 run tf2_tools view_frames  # 生成 frames.pdf

6. 导航小车硬件搭建

6.1 硬件清单

一个典型的 ROS2 导航小车包含以下硬件：

组件	推荐选型	参考价格	说明
主控	Jetson Nano / Raspberry Pi 4B	500-800 元	运行 ROS2 和 SLAM
底盘	两轮差速底盘	200-500 元	带编码器电机
电机驱动	L298N / TB6612 / STM32 驱动板	30-100 元	PWM 控制
激光雷达	RPLidar A1 / YDLIDAR X4	400-500 元	2D SLAM 建图
RGB-D 相机	Intel RealSense D435i（可选）	1500-2000 元	视觉 SLAM / 避障
IMU	MPU6050 / BNO055（可选）	20-150 元	姿态辅助
电池	12V 锂电池	100-200 元	供电
降压模块	DC-DC 降压模块	20 元	5V 给主控供电

入门最小配置：主控 + 底盘 + 激光雷达，约 1200 元即可开始。

6.2 系统架构

┌─────────────────────────────────────────────┐
│              上位机 (Jetson / RPi)            │
│                                             │
│  ROS2 节点：                                 │
│  ├── slam_toolbox（SLAM 建图）               │
│  ├── nav2（导航）                            │
│  ├── rplidar_ros（激光雷达驱动）              │
│  ├── realsense2_camera（相机驱动，可选）      │
│  └── micro_ros_agent（与下位机通信）          │
│                                             │
│         USB/UART                            │
└──────────┬──────────────────────────────────┘
           │
           ▼
┌─────────────────────────────────────────────┐
│            下位机 (STM32 / Arduino)          │
│                                             │
│  功能：                                      │
│  ├── 电机 PWM 控制                           │
│  ├── 编码器读取 → 里程计计算                  │
│  ├── IMU 数据读取（可选）                     │
│  └── micro-ROS 通信                         │
│                                             │
│  发布话题：/odom, /imu/data                  │
│  订阅话题：/cmd_vel                          │
└─────────────────────────────────────────────┘

6.3 URDF 机器人模型

URDF（Unified Robot Description Format）描述机器人的物理结构：

<?xml version="1.0"?>
<robot name="my_nav_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 基座 -->
  <link name="base_footprint"/>

  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.25 0.20 0.07"/>
      </geometry>
      <material name="blue">
        <color rgba="0.0 0.0 0.8 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.25 0.20 0.07"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0" ixz="0"
               iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </joint>

  <!-- 左轮 -->
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder radius="0.033" length="0.026"/>
      </geometry>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.033" length="0.026"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0" ixz="0"
               iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin xyz="0 0.11 -0.017" rpy="-1.5708 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!-- 右轮（与左轮对称） -->
  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder radius="0.033" length="0.026"/>
      </geometry>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.033" length="0.026"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0" ixz="0"
               iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>

  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin xyz="0 -0.11 -0.017" rpy="-1.5708 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!-- 万向轮（前方支撑） -->
  <link name="caster_wheel">
    <visual>
      <geometry>
        <sphere radius="0.016"/>
      </geometry>
      <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.016"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0" ixz="0"
               iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_wheel"/>
    <origin xyz="0.1 0 -0.034" rpy="0 0 0"/>
  </joint>

  <!-- 激光雷达 -->
  <link name="laser_frame">
    <visual>
      <geometry>
        <cylinder radius="0.035" length="0.04"/>
      </geometry>
      <material name="red">
        <color rgba="0.8 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.035" length="0.04"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.15"/>
      <inertia ixx="0.0001" ixy="0" ixz="0"
               iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_frame"/>
    <origin xyz="0 0 0.06" rpy="0 0 0"/>
  </joint>

  <!-- RGB-D 相机（可选） -->
  <link name="camera_link">
    <visual>
      <geometry>
        <box size="0.025 0.09 0.025"/>
      </geometry>
      <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
  </link>

  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.12 0 0.04" rpy="0 0 0"/>
  </joint>

  <link name="camera_optical_frame"/>
  <joint name="camera_optical_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_optical_frame"/>
    <!-- 相机光学坐标系：Z 朝前，X 朝右，Y 朝下 -->
    <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
  </joint>

</robot>

6.4 差速驱动里程计

差速驱动小车的运动学模型：

两轮差速模型：

┌─────────┐
v_L  │         │  v_R
◄──── │  base   │ ────►
左轮  │         │  右轮
└─────────┘
L（轮距）

 $线速度： v = (v_{R} + v_{L}) /2$ 
 $角速度： ω = (v_{R} - v_{L}) / L$ 

位姿更新（欧拉积分）：
 $x (t + 1) = x (t) + v * cos (θ) * d t$ 
 $y (t + 1) = y (t) + v * s in (θ) * d t$ 
 $θ (t + 1) = θ (t) + ω * d t$

下位机里程计计算示例（伪代码）：

// STM32 / Arduino 端里程计计算
void updateOdometry() {
    // 读取编码器脉冲
    int left_ticks = readEncoder(LEFT);
    int right_ticks = readEncoder(RIGHT);

    // 转换为距离（米）
    double left_dist = left_ticks * METERS_PER_TICK;
    double right_dist = right_ticks * METERS_PER_TICK;

    // 计算线速度和角速度
    double linear = (right_dist + left_dist) / 2.0;
    double angular = (right_dist - left_dist) / WHEEL_BASE;

    // 更新位姿
    theta += angular;
    x += linear * cos(theta);
    y += linear * sin(theta);

    // 发布 odom 话题（通过 micro-ROS）
    publishOdom(x, y, theta, linear / dt, angular / dt);
}

7. Gazebo 仿真环境

在没有实体硬件的情况下，Gazebo 仿真是学习 SLAM 和导航的最佳方式。

7.1 使用 TurtleBot3 快速开始

TurtleBot3 是 ROS2 官方推荐的教学机器人平台，提供了完整的仿真支持：

# 确保已安装
sudo apt install ros-humble-turtlebot3* ros-humble-gazebo-ros-pkgs

# 设置模型
export TURTLEBOT3_MODEL=burger

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

# 终端 2：启动 SLAM（slam_toolbox）
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=true
# 或者使用 slam_toolbox：
ros2 launch slam_toolbox online_async_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_world

7.2 自定义 Gazebo 世界

创建一个简单的室内环境 my_world.sdf：

<?xml version="1.0" ?>
<sdf version="1.8">
  <world name="my_world">
    <!-- 光照 -->
    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <!-- 地面 -->
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
          </material>
        </visual>
      </link>
    </model>

    <!-- 外墙 -->
    <model name="wall_north">
      <static>true</static>
      <pose>0 5 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>10 0.1 1</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>10 0.1 1</size></box></geometry>
          <material><ambient>0.6 0.6 0.6 1</ambient></material>
        </visual>
      </link>
    </model>

    <model name="wall_south">
      <static>true</static>
      <pose>0 -5 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>10 0.1 1</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>10 0.1 1</size></box></geometry>
          <material><ambient>0.6 0.6 0.6 1</ambient></material>
        </visual>
      </link>
    </model>

    <model name="wall_east">
      <static>true</static>
      <pose>5 0 0.5 0 0 1.5708</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>10 0.1 1</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>10 0.1 1</size></box></geometry>
          <material><ambient>0.6 0.6 0.6 1</ambient></material>
        </visual>
      </link>
    </model>

    <model name="wall_west">
      <static>true</static>
      <pose>-5 0 0.5 0 0 1.5708</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>10 0.1 1</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>10 0.1 1</size></box></geometry>
          <material><ambient>0.6 0.6 0.6 1</ambient></material>
        </visual>
      </link>
    </model>

    <!-- 室内隔墙 -->
    <model name="inner_wall_1">
      <static>true</static>
      <pose>-1 2 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>4 0.1 1</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>4 0.1 1</size></box></geometry>
          <material><ambient>0.7 0.7 0.7 1</ambient></material>
        </visual>
      </link>
    </model>

    <!-- 障碍物（桌子） -->
    <model name="table">
      <static>true</static>
      <pose>2 -2 0.35 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry><box><size>1.0 0.6 0.7</size></box></geometry>
        </collision>
        <visual name="visual">
          <geometry><box><size>1.0 0.6 0.7</size></box></geometry>
          <material><ambient>0.5 0.3 0.1 1</ambient></material>
        </visual>
      </link>
    </model>

  </world>
</sdf>

7.3 仿真中的完整 SLAM + 导航流程

# 步骤 1：启动仿真环境
ros2 launch my_robot_gazebo simulation.launch.py world:=my_world.sdf

# 步骤 2：启动 SLAM 建图
ros2 launch my_robot_slam slam_launch.py use_sim_time:=true

# 步骤 3：遥控建图
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# 步骤 4：保存地图
ros2 run nav2_map_server map_saver_cli -f ~/maps/my_world_map

# 步骤 5：关闭 SLAM，启动导航
# Ctrl+C 关闭 SLAM 节点
ros2 launch my_robot_navigation navigation_launch.py \
  use_sim_time:=true \
  map:=$HOME/maps/my_world_map.yaml

# 步骤 6：在 RViz 中设置初始位姿（2D Pose Estimate）
# 步骤 7：在 RViz 中设置导航目标（2D Goal Pose）

7.4 Gazebo 仿真注意事项

注意事项	说明
`use_sim_time`	仿真中必须设为 `true`，使用 Gazebo 的仿真时钟
物理引擎	默认使用 ODE，复杂场景可切换到 DART
传感器噪声	Gazebo 可以为传感器添加高斯噪声，更接近真实
实时因子	如果仿真卡顿，降低世界复杂度或减少传感器频率
GPU 加速	相机传感器需要 GPU，确保安装了显卡驱动

8. 进阶拓展

8.1 多传感器融合

实际应用中，单一传感器往往不够可靠。多传感器融合可以显著提升系统的鲁棒性和精度。

8.1.1 激光 + IMU 融合

IMU（惯性测量单元）提供高频率的角速度和加速度数据，与激光雷达互补：

激光雷达：低频（10-20Hz）、高精度、无漂移
IMU：     高频（100-1000Hz）、短期精确、长期漂移

融合策略：
  - IMU 提供帧间的高频运动预测（预积分）
  - 激光扫描匹配修正 IMU 的累积漂移
  - 结果：高频率 + 高精度的位姿估计

在 ROS2 中使用 robot_localization 包进行传感器融合：

# ekf.yaml - 扩展卡尔曼滤波融合配置
ekf_filter_node:
  ros__parameters:
    frequency: 50.0
    sensor_timeout: 0.1
    two_d_mode: true          # 2D 导航小车设为 true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # 里程计输入（来自轮式编码器）
    odom0: /wheel_odom
    odom0_config: [false, false, false,   # x, y, z
                   false, false, false,   # roll, pitch, yaw
                   true,  true,  false,   # vx, vy, vz
                   false, false, true,    # vroll, vpitch, vyaw
                   false, false, false]   # ax, ay, az

    # IMU 输入
    imu0: /imu/data
    imu0_config: [false, false, false,   # x, y, z
                  false, false, true,    # roll, pitch, yaw
                  false, false, false,   # vx, vy, vz
                  false, false, true,    # vroll, vpitch, vyaw
                  true,  false, false]   # ax, ay, az
    imu0_differential: true              # 使用差分模式
    imu0_remove_gravitational_acceleration: true

# 启动传感器融合
ros2 launch robot_localization ekf.launch.py

8.1.2 激光 + 视觉融合

RTAB-Map 天然支持激光和视觉的融合：

# RTAB-Map 激光+视觉融合配置
rtabmap:
  ros__parameters:
    subscribe_depth: true
    subscribe_scan: true        # 同时订阅激光数据

    # 视觉用于回环检测
    # 激光用于精确的扫描匹配和栅格地图生成

    Grid/FromDepth: false       # 从激光而非深度图生成栅格
    Grid/RangeMax: 12.0

    # ICP 参数（激光扫描匹配）
    Icp/MaxCorrespondenceDistance: 0.1
    Icp/MaxTranslation: 0.5
    Icp/Strategy: 1             # 1=libpointmatcher

8.1.3 融合架构对比

融合方式	说明	代表算法
松耦合	各传感器独立处理，结果在后端融合	robot_localization (EKF)
紧耦合	传感器原始数据在同一优化框架中联合处理	LIO-SAM, VINS-Fusion

松耦合实现简单，紧耦合精度更高但复杂度也更高。导航小车入门建议从松耦合开始。

8.2 3D SLAM 简介

当导航场景涉及多层建筑、复杂地形或无人机时，需要 3D SLAM。

8.2.1 3D 激光 SLAM

常用 3D 激光雷达：Velodyne VLP-16、Livox Mid-360、Ouster OS1

代表算法：

算法	特点	适用场景
LOAM	经典的激光里程计和建图	通用 3D 建图
LeGO-LOAM	轻量级 LOAM，利用地面分割	地面车辆
LIO-SAM	激光+IMU 紧耦合，因子图优化	高精度建图
FAST-LIO2	迭代卡尔曼滤波，极快速度	嵌入式平台
Point-LIO	逐点处理，超高频率	高动态场景

8.2.2 3D 视觉 SLAM

ORB-SLAM3：支持单目/双目/RGB-D 的 3D 视觉 SLAM
RTAB-Map：支持 3D 点云地图和 OctoMap 生成
VINS-Fusion：视觉惯性 SLAM，支持 GPS 融合

8.3 语义 SLAM

传统 SLAM 只关注几何信息（哪里有障碍物），语义 SLAM 还理解"障碍物是什么"。

传统 SLAM 地图：
  ████████████
  █   ██     █
  █          █    ← 只知道哪里有障碍物
  █     ██   █
  ████████████

语义 SLAM 地图：
  ████████████
  █   桌子    █
  █          █    ← 知道障碍物的类别
  █     椅子  █
  ████████████

实现方式：

使用深度学习目标检测（YOLO、Mask R-CNN）识别物体
将语义标签与 SLAM 地图中的几何信息关联
构建语义地图，支持更智能的导航决策

# 简单的语义 SLAM 概念示例（伪代码）
# 结合 YOLO 检测和 RTAB-Map

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from vision_msgs.msg import Detection2DArray

class SemanticMapper(Node):
    def __init__(self):
        super().__init__('semantic_mapper')

        # 订阅 YOLO 检测结果
        self.det_sub = self.create_subscription(
            Detection2DArray, '/yolo/detections', self.detection_cb, 10)

        # 订阅深度图
        self.depth_sub = self.create_subscription(
            Image, '/camera/depth/image_raw', self.depth_cb, 10)

        self.semantic_objects = []  # 语义物体列表

    def detection_cb(self, msg):
        for det in msg.detections:
            label = det.results[0].hypothesis.class_id
            # 结合深度信息计算 3D 位置
            # 将语义标签和位置存入语义地图
            self.get_logger().info(f'检测到: {label}')

    def depth_cb(self, msg):
        # 处理深度图
        pass

8.4 深度学习在 SLAM 中的应用

深度学习正在改变 SLAM 的多个环节：

应用方向	说明	代表工作
深度估计	从单目图像预测深度图	MiDaS, Depth Anything
特征提取与匹配	学习更鲁棒的特征描述子	SuperPoint + SuperGlue
位姿估计	端到端学习相机位姿	PoseNet, MapNet
回环检测	基于深度学习的场景识别	NetVLAD, Patch-NetVLAD
动态物体去除	识别并去除动态物体的影响	DynaSLAM
NeRF/3DGS	神经辐射场/3D 高斯泼溅用于场景重建	iMAP, NICE-SLAM, SplaTAM

趋势：传统几何方法 + 深度学习的混合方案是当前主流方向，纯端到端的深度学习 SLAM 仍在研究阶段。

9. 常见问题与调试技巧

9.1 SLAM 建图问题

问题	可能原因	解决方案
地图扭曲/重影	里程计漂移严重，回环检测失败	检查里程计精度，降低移动速度，确保走回路触发回环
地图有大量噪点	激光雷达数据质量差	检查雷达安装是否稳固，调整 `max_laser_range` 过滤远处噪声
建图时地图不更新	TF 变换缺失或话题名不匹配	用 `ros2 topic list` 和 `ros2 run tf2_tools view_frames` 检查
长走廊建图漂移	特征不足，扫描匹配退化	蛇形走位增加特征，或融合 IMU
建图速度慢	分辨率过高或计算资源不足	降低分辨率，增大 `minimum_travel_distance`

9.2 导航问题

问题	可能原因	解决方案
机器人不动	AMCL 未定位成功，costmap 未就绪	在 RViz 中设置正确的初始位姿
路径规划失败	起点或终点在障碍物内	检查 costmap 膨胀参数，确保机器人位置正确
机器人撞墙	膨胀半径太小，控制器跟踪不准	增大 `inflation_radius`，调整控制器参数
机器人原地打转	到达目标附近但角度不满足	调大 `xy_goal_tolerance` 和 `yaw_goal_tolerance`
机器人走奇怪的路径	costmap 代价值不合理	检查 costmap 层配置，调整 `cost_scaling_factor`
恢复行为频繁触发	空间狭窄或参数不合适	调整恢复行为参数，或在狭窄区域降低速度

9.3 TF 问题排查

TF 问题是 ROS2 导航中最常见的错误来源：

# 1. 查看完整 TF 树
ros2 run tf2_tools view_frames
# 生成 frames.pdf，检查是否有断裂

# 2. 检查特定变换是否存在
ros2 run tf2_ros tf2_echo map base_link
# 如果报错说找不到变换，说明 TF 链断裂

# 3. 查看 TF 发布频率
ros2 run tf2_ros tf2_monitor

# 4. 常见 TF 错误及解决
# "Could not find a connection between 'map' and 'base_link'"
#   → 检查 AMCL 或 SLAM 是否在运行（它们发布 map→odom）
#   → 检查里程计是否在发布 odom→base_link

# "Lookup would require extrapolation into the future"
#   → 时间同步问题，检查 use_sim_time 设置
#   → 仿真中必须所有节点都设置 use_sim_time:=true

9.4 性能优化建议

场景：Jetson Nano 上运行 SLAM + Nav2

优化措施：
1. 降低激光扫描频率：throttle_scans: 2（每2帧处理1帧）
2. 降低地图分辨率：resolution: 0.1（10cm）
3. 减少 AMCL 粒子数：max_particles: 500
4. 降低 costmap 更新频率：update_frequency: 2.0
5. 使用异步 SLAM 模式：async_slam_toolbox_node
6. 关闭不必要的可视化：不运行 RViz（或在远程电脑运行）
7. 降低相机分辨率：320x240 足够用于避障

9.5 实用调试命令速查

# === 话题调试 ===
ros2 topic list                          # 列出所有话题
ros2 topic echo /scan --once             # 查看一帧激光数据
ros2 topic hz /scan                      # 查看话题频率
ros2 topic info /scan                    # 查看话题类型和发布/订阅者

# === 节点调试 ===
ros2 node list                           # 列出所有节点
ros2 node info /slam_toolbox             # 查看节点详情
ros2 param list /slam_toolbox            # 列出节点参数
ros2 param get /slam_toolbox resolution  # 获取参数值

# === 服务调试 ===
ros2 service list                        # 列出所有服务
ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: '/path/to/map.yaml'}"

# === TF 调试 ===
ros2 run tf2_tools view_frames           # 生成 TF 树图
ros2 run tf2_ros tf2_echo map base_link  # 实时查看变换

# === 录包与回放 ===
ros2 bag record -a                       # 录制所有话题
ros2 bag record /scan /odom /tf /tf_static  # 录制指定话题
ros2 bag play my_bag --clock             # 回放（发布仿真时钟）

# === 可视化 ===
ros2 run rviz2 rviz2                     # 启动 RViz2
ros2 run rqt_graph rqt_graph             # 查看节点关系图
ros2 run rqt_tf_tree rqt_tf_tree         # 查看 TF 树

10. 参考资源

10.1 书籍

书名	作者	说明
《视觉 SLAM 十四讲》	高翔	视觉 SLAM 入门必读，理论+实践
《概率机器人》	Thrun 等	SLAM 理论经典教材
《机器人学中的状态估计》	Barfoot	深入理解状态估计和优化
《ROS 机器人开发实践》	胡春旭	ROS/ROS2 实践指南

10.2 在线课程与教程

古月居 ROS2 教程：中文 ROS2 入门系列
鱼香 ROS：ROS2 中文社区，教程丰富
Articulated Robotics：YouTube 上优质的 ROS2 导航小车教程
Nav2 官方文档：navigation.ros.org
slam_toolbox Wiki：GitHub 上的详细文档

10.3 开源项目参考

项目	说明
TurtleBot3	ROS2 官方教学机器人，完整的 SLAM + 导航示例
Linorobot2	开源 ROS2 导航小车项目，硬件+软件完整方案
Nav2 示例	nav2_bringup 包中的完整导航配置示例
RTAB-Map 示例	rtabmap_demos 包中的多种传感器配置示例

10.4 常用 ROS2 包索引

包名	功能	安装命令
`slam_toolbox`	2D 激光 SLAM	`sudo apt install ros-humble-slam-toolbox`
`navigation2`	导航栈	`sudo apt install ros-humble-navigation2`
`rtabmap_ros`	多传感器 SLAM	`sudo apt install ros-humble-rtabmap-ros`
`robot_localization`	传感器融合	`sudo apt install ros-humble-robot-localization`
`cartographer_ros`	Google SLAM	`sudo apt install ros-humble-cartographer-ros`
`rplidar_ros`	RPLidar 驱动	`sudo apt install ros-humble-rplidar-ros`
`realsense2_camera`	RealSense 驱动	`sudo apt install ros-humble-realsense2-camera`
`teleop_twist_keyboard`	键盘遥控	`sudo apt install ros-humble-teleop-twist-keyboard`