ROS2 AGV SLAM导航仿真系统技术详解与踩坑实录

1. 项目背景与目标

随着物流自动化和智能制造的快速发展，自动导引车（AGV）在仓储、工厂等场景的应用越来越广泛。AGV系统的核心技术包括环境感知、地图构建、自主定位和路径规划。为了降低开发成本和风险，基于仿真环境的AGV系统开发成为业界主流方案。

本文将详细介绍如何从零搭建一个基于ROS2（Robot Operating System 2）的AGV SLAM导航仿真系统。该系统集成了激光SLAM建图、自主导航、避障等核心功能，可用于室内仓储场景的AGV开发和测试。

项目地址：

https://github.com/HulinCal/SLAM-Navigation-Simulation-System

1.1 项目目标

本项目的主要目标包括：

1. 搭建基于Gazebo的室内仓储仿真环境，包含100平方米的仓库场景和TurtleBot3 Waffle机器人模型

2. 实现基于Cartographer的2D激光SLAM建图功能，支持子图分块优化和回环检测

3. 集成Nav2导航栈，实现自主路径规划、避障和速度平滑控制

4. 实现基于AMCL的自适应蒙特卡洛定位，支持初始位姿估计

5. 提供完整的RVIZ可视化界面，显示地图、代价地图、机器人模型和路径规划结果

6. 构建完整的TF坐标变换树，确保仿真时间同步

1.2 技术选型

本项目采用以下核心技术栈：

• ROS2 Jazzy Jalisco：机器人操作系统2的最新稳定版本，提供更好的性能和QoS支持

• Gazebo Garden：新一代物理仿真引擎，通过ros_gz_sim与ROS2集成

• Cartographer：Google开源的激光SLAM算法，支持2D/3D建图

• **Nav2**：ROS2官方导航框架，包含AMCL、Planner、Controller等模块

• TurtleBot3 Waffle：开源移动机器人平台，配备激光雷达、IMU、摄像头

• RVIZ：ROS可视化工具，用于显示地图、机器人模型等信息

2. 系统架构设计

系统采用分层架构设计，从底层到顶层依次为：仿真层、传感器数据层、SLAM层、导航层和可视化层。各层之间通过ROS2话题（Topic）和服务（Service）进行通信。

2.1 整体架构

系统架构如下所示：

┌─────────────────────────────────────────────────────────────┐
│ ROS2 Jazzy 系统架构 │
├─────────────────────────────────────────────────────────────┤
│ Gazebo Simulation │
│ ├─ agv_warehouse.world (室内仓储环境) │
│ └─ TurtleBot3 Waffle (机器人模型) │
├─────────────────────────────────────────────────────────────┤
│ Sensor Data │
│ ├─ LaserScan (激光雷达) → /scan │
│ ├─ IMU (惯性测量单元) → /imu │
│ ├─ Odometry (里程计) → /odom │
│ └─ Camera (摄像头) → /camera/image_raw │
├─────────────────────────────────────────────────────────────┤
│ SLAM Module (Cartographer) │
│ ├─ 激光扫描匹配 │
│ ├─ 子图构建与优化 │
│ ├─ 回环检测 │
│ └─ 地图发布 → /map │
├─────────────────────────────────────────────────────────────┤
│ Navigation Module (Nav2) │
│ ├─ AMCL (自适应蒙特卡洛定位) │
│ ├─ Planner (路径规划) │
│ ├─ Controller (运动控制) │
│ └─ BT Navigator (行为树导航) │
└─────────────────────────────────────────────────────────────┘

2.2 项目结构

项目采用标准的ROS2包结构，包含launch文件、配置文件、world文件等：

agv_slam_navigation/
├── launch/
│ ├── gazebo_sim.launch.py # Gazebo仿真环境启动

│ ├── slam_launch.py # Cartographer SLAM启动

│ ├── navigation_launch.py # Nav2导航启动

│ ├── save_map_launch.py # 地图保存启动

│ └── full_simulation_launch.py # 完整仿真启动

├── config/

│ ├── cartographer_agv_2d.lua # Cartographer配置

│ ├── nav2_params.yaml # Nav2配置

│ └── agv_bridge.yaml # ROS-Gazebo桥接配置

├── worlds/

│ └── agv_warehouse.world # 室内仓储仿真环境

├── src/

│ └── agv_lifecycle_node.cpp # AGV生命周期节点

├── scripts/

│ └── navigate_to_pose.py # 导航到目标点脚本

├── CMakeLists.txt # CMake构建配置

└── package.xml # ROS包配置

3. 核心模块详解

3.1 仿真环境搭建

仿真环境是整个系统的基础，包括物理世界和机器人模型。本项目使用Gazebo Garden作为仿真引擎，通过ros_gz_sim包与ROS2集成。

仿真环境包含以下关键组件：

• agv_warehouse.world：室内仓储场景，包含货架、墙壁、障碍物等

• TurtleBot3 Waffle模型：配备360度激光雷达、IMU、摄像头和差分驱动

• ros_gz_bridge：ROS与Gazebo之间的消息桥接，实现话题双向传输

• clock同步：确保所有节点使用统一的仿真时间

启动仿真环境的命令如下：

export TURTLEBOT3_MODEL=waffle
ros2 launch agv_slam_navigation gazebo_sim.launch.py

3.2 SLAM建图模块

SLAM（Simultaneous Localization and Mapping）即同时定位与建图，是AGV系统的核心技术之一。本项目采用Cartographer作为SLAM算法，它是Google开源的2D/3D激光SLAM库。

Cartographer的核心特点包括：

• 基于子图的建图方式，支持大规模环境建图

• 实时回环检测，提高地图一致性

• 支持多种传感器融合（激光、IMU、里程计）

• 提供ROS2接口，易于集成

Cartographer的配置文件`cartographer_agv_2d.lua`包含以下关键参数：

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- 地图分辨率

TRAJECTORY_BUILDER_2D.min_range = 0.12 -- 激光最小测距

TRAJECTORY_BUILDER_2D.max_range = 3.5 -- 激光最大测距

use_imu_data = false -- 是否使用IMU

use_odometry = false -- 是否使用里程计

启动SLAM建图的命令如下：

ros2 launch agv_slam_navigation slam_launch.py

3.3 导航模块

导航模块是AGV自主运行的核心，本项目集成了Nav2导航栈，包含以下子模块：

• **AMCL**：自适应蒙特卡洛定位，通过粒子滤波估计机器人在地图中的位置

• **Planner**：全局路径规划器，使用A*或Dijkstra算法规划最优路径

• **Controller**：局部控制器，使用MPPI算法实现轨迹跟踪和避障

• **BT Navigator**：行为树导航器，管理导航任务的执行流程

• **Velocity Smoother**：速度平滑器，过滤突变速度指令

• **Collision Monitor**：碰撞监测器，检测障碍物并调整速度

启动导航模块的命令如下：

ros2 launch agv_slam_navigation navigation_launch.py map:=/path/to/map.yaml

3.4 RVIZ可视化

RVIZ是ROS官方的可视化工具，用于显示地图、机器人模型、激光扫描、路径规划等信息。本项目使用nav2_bringup提供的默认RVIZ配置文件`nav2_default_view.rviz`，包含以下显示组件：

• **Map**：显示SLAM建图或加载的地图

• **RobotModel**：显示机器人模型和TF坐标变换

• **LaserScan**：显示激光雷达扫描数据

• **Global Costmap**：显示全局代价地图

• **Local Costmap**：显示局部代价地图

• **Path**：显示规划的全局路径

• **AMCL Pose**：显示AMCL定位结果和粒子云

4. 技术挑战与解决方案

在项目开发过程中，遇到了多个技术挑战，包括QoS不匹配、TF链断裂、桥接配置错误、AMCL初始化问题等。以下详细介绍每个问题的原因分析和解决方案。

4.1 QoS不匹配问题

**问题描述**：启动导航模块后，RVIZ中无法显示地图。通过`ros2 topic echo /map`可以看到地图数据正常发布，但RVIZ始终显示空白。

**原因分析**：ROS2引入了QoS（Quality of Service）机制，用于定义消息传输的可靠性、持久性等属性。map_server发布地图时使用`Durability: TRANSIENT_LOCAL`（瞬态本地持久化），而RVIZ默认订阅使用`Durability: VOLATILE`（非持久化），导致QoS不匹配，RVIZ无法接收地图数据。

**解决方案**：修改RVIZ配置文件，将地图Topic的QoS设置为`Durability: TRANSIENT_LOCAL`。最初尝试独立QoS块配置未生效，最终替换为nav2_bringup提供的默认配置文件`nav2_default_view.rviz`，采用嵌套的Topic QoS结构解决问题。

同时，在AMCL配置中添加QoS覆盖，确保AMCL能正确接收map_server发布的地图数据：

amcl:
ros__parameters:
qos_overrides:
/map:
publisher:
reliability: reliable
durability: transient_local

4.2 TF链断裂问题

**问题描述**：RVIZ中提示"No transform from base_footprint to map"，机器人模型无法显示。

**原因分析**：完整的TF链应该是`map → odom → base_footprint → base_link → ...`。经过检查，发现缺少`map → odom`变换，这是因为AMCL需要先接收初始位姿才能发布该变换。

**解决方案**：在RVIZ中使用"2D Pose Estimate"工具设置机器人的初始位姿，或通过命令行发布初始位姿消息：

ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, covariance: 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0685}}" --qos-reliability reliable --qos-durability transient_local

设置初始位姿后，AMCL会发布`map → odom`变换，完整的TF链建立，机器人模型可以正常显示。

4.3 tf_static冲突问题

**问题描述**：RVIZ中机器人模型和代价地图无法显示，日志中出现QoS冲突警告。

**原因分析**：存在两个tf_static发布者------robot_state_publisher（ROS端）和ros_gz_bridge（Gazebo桥接），导致QoS冲突（一个使用TRANSIENT_LOCAL，一个使用VOLATILE），RVIZ无法正确接收静态TF变换。

**解决方案**：移除`agv_bridge.yaml`中tf_static的桥接配置，仅保留robot_state_publisher作为tf_static的发布者。修改后的桥接配置不再包含tf_static话题。

4.4 速度指令链路问题

**问题描述**：点击"2D Nav Goal"工具后，机器人没有自动运行，控制器服务器显示"Reached the goal!"但机器人并未移动。

**原因分析**：速度指令链路存在两个问题：

• 控制器服务器发布的cmd_vel消息类型（Twist）与桥接配置的ros_type_name（TwistStamped）不匹配

• velocity_smoother和collision_monitor的cmd_vel话题输出冲突，导致速度指令未正确传递到Gazebo

**解决方案**：

修改agv_bridge.yaml，将cmd_vel的消息类型改为Twist

• ros_topic_name: "cmd_vel_final"

gz_topic_name: "cmd_vel"

ros_type_name: "geometry_msgs/msg/Twist"

gz_type_name: "gz.msgs.Twist"

direction: ROS_TO_GZ

调整velocity_smoother和collision_monitor的话题配置，形成完整的速度指令链路：

• controller_server → cmd_vel

• cmd_vel → velocity_smoother → cmd_vel_smoothed

• cmd_vel_smoothed → collision_monitor → cmd_vel_final

• cmd_vel_final → ros_gz_bridge → Gazebo

4.5 AMCL时间同步问题

**问题描述**：AMCL日志中出现"Failed to transform initial pose in time"警告，提示时间戳差异过大。

**原因分析**：在仿真环境中，不同节点的时间同步存在延迟，AMCL的`transform_tolerance`参数（默认1.0秒）过小，导致时间同步失败。

**解决方案**：修改`nav2_params.yaml`，将AMCL的`transform_tolerance`增加到5.0秒，并添加`max_publish_freq`参数：

amcl:
ros__parameters:
transform_tolerance: 5.0 # 时间容差增加到5秒

max_publish_freq: 10.0 # 最大发布频率

4.6 Gazebo进程自动关闭问题

**问题描述**：启动Gazebo仿真环境后，进程在几秒钟后自动关闭。

**原因分析**：`gazebo_sim.launch.py`中gzserver和gzclient配置了`on_exit=Shutdown()`，导致create进程结束后整个launch被关闭。

**解决方案**：移除gzserver和gzclient的`on_exit=Shutdown()`配置，确保Gazebo进程正常运行。

5. 开发与调试流程

本项目采用迭代开发和测试驱动的方式，开发流程包括环境准备、模块开发、集成测试和问题修复四个阶段。

5.1 环境准备

环境准备阶段包括以下步骤：

1. 安装Ubuntu 24.04 LTS操作系统

2. 安装ROS2 Jazzy Jalisco

3. 安装Gazebo Garden和ros_gz_sim包

4. 安装依赖包：turtlebot3_gazebo、turtlebot3_cartographer、turtlebot3_navigation2、nav2_bringup

5. 设置环境变量：export TURTLEBOT3_MODEL=waffle

6. 创建ROS2工作空间并编译项目

编译命令：

source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

5.2 模块开发

模块开发阶段按以下顺序进行：

1. 仿真环境搭建：创建world文件和桥接配置

2. SLAM模块集成：配置Cartographer并测试建图功能

3. 导航模块集成：配置Nav2并测试定位和导航功能

4. RVIZ配置：设置可视化组件和QoS参数

5. 一键启动脚本：编写launch文件简化启动流程

5.3 集成测试

集成测试阶段包括以下测试项：

• **仿真环境测试**：检查Gazebo是否正常启动，机器人模型是否显示

• **传感器数据测试**：检查/scan、/odom、/imu等话题是否有数据发布

• **TF变换测试**：检查TF链是否完整：map→odom→base_footprint→base_link

• **SLAM建图测试**：使用遥控节点控制机器人移动，检查地图是否正确生成

• **导航测试**：设置初始位姿和导航目标，检查机器人是否自动导航

• **避障测试**：在机器人路径上放置障碍物，检查是否自动避开

5.4 调试工具

常用的调试工具包括：

• **ros2 topic list**：查看当前所有话题

• **ros2 topic echo /topic**：查看话题数据

• **ros2 topic hz /topic**：查看话题发布频率

• **ros2 node list**：查看当前所有节点

• **ros2 node info /node**：查看节点详细信息

• **ros2 service list**：查看当前所有服务

• **ros2 lifecycle list /node**：查看生命周期节点状态

• **ros2 tf tree**：查看TF变换树

• **gz topic -l**：查看Gazebo话题列表

6. 使用指南

6.1 分步启动

分步启动方式适合调试和学习，可以逐步启动各个模块。

**步骤1：启动仿真环境**

source /opt/ros/jazzy/setup.bash
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 launch agv_slam_navigation gazebo_sim.launch.py

**步骤2：启动SLAM建图**

source install/setup.bash
ros2 launch agv_slam_navigation slam_launch.py

**步骤3：启动遥控节点**

ros2 run turtlebot3_teleop teleop_keyboard

**步骤4：保存地图**

ros2 launch agv_slam_navigation save_map_launch.py

**步骤5：启动自主导航**

ros2 launch agv_slam_navigation navigation_launch.py map:=/home/hl/ros2_learning/ros2_mapping_navigation_simulation/agv_slam_navigation/maps/map.yaml

6.2 一键启动

一键启动方式适合快速部署和演示，自动启动所有模块。

**启动仿真+SLAM建图**

source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 launch agv_slam_navigation full_simulation_launch.py slam:=true nav:=false

**启动仿真+导航**

source install/setup.bash
export TURTLEBOT3_MODEL=waffle
ros2 launch agv_slam_navigation full_simulation_launch.py slam:=false nav:=true map:=agv_slam_navigation/maps/map.yaml

6.3 RVIZ操作步骤

启动导航模块后，需要在RVIZ中进行以下操作：

**步骤1：设置初始位姿**（必须先执行）

操作方式：点击工具栏的"2D Pose Estimate"工具（图标是带箭头的机器人）
快捷键：按P键
操作：在地图上点击机器人所在位置，拖动鼠标设置朝向

设置完成后，会看到：

• 机器人模型出现在地图上
• AMCL粒子云（红色/绿色圆点）
• 全局/局部代价地图

**步骤2：设置导航目标**

操作方式：点击工具栏的"2D Nav Goal"工具（图标是目标旗帜）
快捷键：按G键
操作：在地图上点击目标位置，拖动鼠标设置目标朝向

设置完成后，会看到：

• 全局路径规划（绿色路径）
• 机器人开始移动
• 局部代价地图更新