RK3576 + ROS2 SLAM建图与导航实战

前言

文档定位与目标读者

本文档面向具备一定ROS基础、希望深入理解并在实际项目中部署ROS2 Humble + SLAM Toolbox + Nav2完整建图与导航系统的机器人工程师。我们将从零开始，基于米尔RK3576开发板逐步构建一个功能完备的自主移动机器人系统，涵盖环境搭建、机器人建模、SLAM建图、自主导航以及生产级系统的优化与排错。

为什么选择SLAM Toolbox + Nav2？

在ROS2生态中，SLAM（同时定位与建图）与导航（Navigation）是机器人自主移动的核心技术。SLAM Toolbox由Steve Macenski主导开发，是基于成熟Karto SLAM的改进版本，相比传统的Gmapping、Hector SLAM或Cartographer，它具有以下显著优势：

• 图优化框架：采用基于图优化的后端，而非简单的滤波器，在大场景下地图一致性更好。
• 生命周期管理：支持终身地图（LifeLong Mapping），即可以在已有地图基础上继续优化或更新，甚至能够移除动态物体留下的痕迹。
• 多种运行模式：同步/异步建图、纯定位模式（可作为AMCL的高精度替代品）、地图序列化与反序列化。
• RViz交互插件：提供丰富的RViz工具，支持手动修正地图、操作图节点。
• 性能卓越：经过优化，能够在数十万平方英尺的场景中实时运行。

而Nav2作为ROS2的官方导航框架，继承了ROS1 Navigation Stack的优点并进行了完全的重构，支持行为树、更灵活的插件化架构和更好的实时性保障。将SLAM Toolbox与Nav2结合，我们可以基于RK3576开发板构建一套从建图到定位导航的无缝衔接系统，甚至可以在导航过程中边建图边导航（Navigation while Mapping）。

核心技术栈概览

• 操作系统：Ubuntu 22.04 LTS (Jammy)
• ROS发行版：ROS2 Humble Hawksbill (长期支持版)
• 仿真环境：Gazebo Classic 11 (与ROS2 Humble官方集成)
• 机器人建模：URDF / Xacro
• SLAM库：slam_toolbox (版本 ≥ 2.6.10)
• 导航栈：Nav2 (navigation2, nav2_bringup)
• 可视化与调试：Rviz2, tf2_tools, rqt_graph

---

第一章：环境搭建与准备工作

1.1 操作系统与ROS2 Humble安装

我们选择Ubuntu 22.04作为基础操作系统。请确保你的系统已更新至最新状态。

# 设置localesudoapt update &&sudoapt install localessudolocale-gen en_US en_US.UTF-8sudoupdate-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8exportLANG=en_US.UTF-8
# 添加ROS2 apt仓库sudoapt install software-properties-commonsudoadd-apt-repository universesudoapt update &&sudoapt install curl -ysudocurl -sSLhttps://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpgecho"deb [arch=$(dpkg --print-architecture)signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu$(. /etc/os-release && echo $UBUNTU_CODENAME)main"|sudotee/etc/apt/sources.list.d/ros2.list > /dev/null
# 安装ROS2 Humble Desktop（包含核心库、rqt、rviz2等）sudoapt updatesudoapt install ros-humble-desktop
# 安装开发工具和依赖sudoapt install python3-colcon-common-extensions python3-rosdep python3-argcomplete python3-vcstool git

安装完成后，配置环境变量以便每次打开终端时自动加载ROS2环境：

echo"source /opt/ros/humble/setup.bash">> ~/.bashrcsource~/.bashrc

注意：如果你管理多个工作空间，建议在工作空间的install目录下使用local_setup.bash，而非全局覆盖。后续我们会在项目工作空间中具体说明。

1.2 安装仿真环境（Gazebo）与机器人模型

为了在不依赖实体硬件的情况下进行算法验证，我们需要安装Gazebo仿真环境以及经典的TurtleBot3机器人模型，尽量在x86 虚拟机安装仿真，arm64架构turtlebot3支持不足。

# 安装Gazebo与ROS2接口包sudoapt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros2-control
# 安装TurtleBot3相关包sudoapt install ros-humble-turtlebot3* ros-humble-teleop-twist-keyboard

1.3 安装核心算法包：SLAM Toolbox与Nav2

# 安装SLAM Toolboxsudoapt install ros-humble-slam-toolbox
# 安装Nav2导航栈及其启动文件sudoapt install ros-humble-navigation2 ros-humble-nav2-bringup
# 安装其他实用工具（用于后续调试）sudoapt install ros-humble-tf2-tools ros-humble-rqt-tf-tree

验证安装是否成功：

ros2 pkg list |grepslam_toolboxros2 pkg list |grepnav2_bringup

1.4 创建工作空间与测试安装

mkdir-p ~/ros2_ws/srccd~/ros2_wscolcon build --symlink-installecho"source ~/ros2_ws/install/setup.bash">> ~/.bashrcsource~/.bashrc

测试仿真环境：打开新终端，运行Gazebo仿真世界和TurtleBot3机器人：

exportTURTLEBOT3_MODEL=waffleros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

图1：Gazebo中TurtleBot3仿真环境

键盘遥控：

# 新终端exportTURTLEBOT3_MODEL=waffleros2 run teleop_twist_keyboard teleop_twist_keyboard

---

第二章：机器人建模与仿真集成

2.1 URDF/Xacro基础与传感器配置

URDF (Unified Robot Description Format) 是ROS中描述机器人几何、惯性、关节关系的XML格式。Xacro则是URDF的宏语言，允许我们使用变量、数学运算和模块化包含。

一个典型的差分驱动机器人模型的核心部分：link、joint、transmission与gazebo插件。

下面是一个简化的差分驱动+激光雷达的Xacro示例结构（部分）：

"1.0"?> geometry> visual> geometry> collision> inertial> link> joint> /namespace>ros> 50update_rate> left_wheel_jointleft_joint> right_wheel_jointright_joint> ${base_radius*2 + wheel_width}wheel_separation> ${wheel_radius*2}wheel_diameter> cmd_velcommand_topic> odomodometry_topic> odomodometry_frame> base_footprintrobot_base_frame> plugin> gazebo>robot>

2.2 坐标系变换（TF）树详解：map -> odom -> base_link -> sensor_link

关键坐标系：

• map：世界固定坐标系。
• odom：里程计坐标系，连续但不稳定。
• base_link：机器人基座坐标系。
• laser_link等：传感器坐标系。

变换关系：base_link->sensor_link（静态），odom->base_link（里程计发布），map->odom（定位系统发布）。

验证TF树：

ros2run tf2_tools view_frames # 生成frames.pdf

2.3 自定义机器人描述文件与启动

标准包结构：

my_robot_description/├──CMakeLists.txt├──package.xml├──urdf/│ ├──my_robot.urdf.xacro│ └──materials.xacro├──meshes/└──launch/ ├──display.launch.py └──spawn_robot.launch.py

display.launch.py示例：

importosfrom launchimportLaunchDescriptionfrom launch_ros.actionsimportNodefrom xacroimportprocess_file
defgenerate_launch_description(): pkg_share = os.path.join(get_package_share_directory('my_robot_description')) urdf_path = os.path.join(pkg_share,'urdf','my_robot.urdf.xacro') robot_description = process_file(urdf_path).toxml() returnLaunchDescription([ Node(package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': robot_description}]), Node(package='joint_state_publisher_gui', executable='joint_state_publisher_gui'), Node(package='rviz2', executable='rviz2'), ])

---

第三章：SLAM Toolbox深度实践与建图

3.1 SLAM Toolbox的两种核心模式：同步与异步

online_async_launch.py（异步，常用）和online_sync_launch.py（同步）。

3.2 配置文件详解：mapper_params_online_async.yaml

# mapper_params_online_async.yamlslam_toolbox: ros__parameters: odom_frame: odom map_frame: map base_frame: base_footprint scan_topic: /scan mode: mapping
minimum_range:0.2 maximum_range:10.0 minimum_travel_distance:0.1 minimum_travel_heading:0.2 do_loop_closing: true loop_search_space:8.0 map_update_interval:5.0 enable_interactive_mode: true # ... 其他参数

注意：1.机器人与传感器参数：odom_frame、base_frame必须与你的TF树完全一致。scan_topic确保订阅正确的数据。
2.节点添加策略：minimum_travel_distance和minimum_travel_heading决定了地图的稠密程度。值越小，节点越多，地图细节越丰富，但计算量也越大。对于大场景，可以适当增大。
3.闭环检测：loop_search_space是闭环检测的搜索半径。如果你的环境有很多相似的结构（如长走廊），需要适当减小这个值以避免错误的闭环；反之，如果传感器噪声大或里程计漂移严重，需要增大搜索空间。

3.3 手动建图流程与保存地图

终端1：仿真

exportTURTLEBOT3_MODEL=waffleros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

终端2：SLAM Toolbox

ros2 launch slam_toolbox online_async_launch.py \ slam_params_file:=./src/my_robot_navigation/config/mapper_params_online_async.yaml \ use_sim_time:=true

终端3：RViz（添加Map和LaserScan）

图3：Rviz2中可视化激光扫描和建图过程

终端4：键盘遥控

ros2run teleop_twist_keyboard teleop_twist_keyboard

保存地图：

ros2 run nav2_map_server map_saver_cli -f ~/maps/my_mapros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \ "{filename: '/home/your_user/maps/my_pose_graph'}"

图4：实体机器人建图现场

3.4 高级话题：终身地图与位姿图序列化

启用终身地图：mode: mapping+enable_life_long_mapping: true。序列化文件(.posegraph)可保存图节点信息，用于后续继续建图或定位模式。

---

第四章：Nav2导航系统构建与配置

4.1 Nav2架构与核心组件

• 地图服务器、AMCL、代价地图（全局/局部）、规划器（Planner）、控制器（DWB）、行为树导航器（BT Navigator）。

4.2 Nav2参数配置实战（nav2_params.yaml节选）

bt_navigator: ros__parameters: default_nav_to_pose_bt_xml: /opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning.xml
controller_server: ros__parameters: controller_frequency:20.0 FollowPath: plugin:"dwb_core::DWBLocalPlanner" max_vel_x:0.22 max_vel_theta:1.0 path_distance_bias:32.0 goal_distance_bias:24.0
local_costmap: local_costmap: ros__parameters: global_frame: odom rolling_window: true width:3 plugins:["voxel_layer", "inflation_layer"]
global_costmap: global_costmap: ros__parameters: global_frame: map plugins:["static_layer", "obstacle_layer", "inflation_layer"]
amcl: ros__parameters: global_frame_id: map odom_frame_id: odom laser_model_type: likelihood_field min_particles:500 max_particles:2000

4.3 启动Nav2：基于已有地图的导航

终端1：仿真

ros2launch turtlebot3_gazebo turtlebot3_world.launch.py

终端2：Nav2 bringup

ros2 launch nav2_bringup bringup_launch.py \ use_sim_time:=true\ map:=/home/your_user/maps/my_map.yaml \ params_file:=./src/my_robot_navigation/config/nav2_params.yaml

终端3：RViz (Nav2默认视图)

rviz2-d/opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

图5：Nav2仿真导航界面

使用“2D Pose Estimate”初始化位姿，然后“2D Goal Pose”发送目标。

4.4 集成SLAM Toolbox定位模式替代AMCL

修改SLAM配置文件：

mode: localizationmap_file_name: "/home/your_user/maps/my_pose_graph"

启动SLAM Toolbox（定位模式）：

ros2 launch slam_toolbox online_async_launch.py \ slam_params_file:=./config/mapper_params_localization.yaml use_sim_time:=true

启动Nav2（不含AMCL）：

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=trueparams_file:=./config/nav2_params.yaml

步骤1 ：在地图上设置小车初始位置和方向；
步骤2：在地图上设置小车单点导航：

图6：实体机器人Nav2导航

---

第五章：高级整合与调试

5.1 边建图边导航（Navigation while Mapping）

启动仿真 + SLAM建图模式 + navigation_launch.py（不含map_server/amcl），然后通过RViz设定目标，机器人一边探索一边建图。

5.2 RViz插件：SLAM Toolbox图形化工具

Panels -> Add Panel -> SlamToolboxPlugin 可手动保存、清除节点、强制闭环。

5.3 性能分析与优化

• 分析CPU/内存：top -p `pgrep -d',' -f 'ros2|slam_toolbox|nav2'`
• 检查话题频率：ros2 topic hz /scan
• SLAM优化：使用snap版slam-toolbox；增大map_update_interval；增大节点添加阈值。
• Nav2优化：降低controller_frequency；增大局部代价地图分辨率；减少DWB采样。

5.4 常见错误排解指南

---

第六章：实体机器人部署指南

6.1 硬件抽象与驱动层

• 激光雷达驱动：例如ros2 launch sllidar_ros2 view_sllidar_a1_launch.py
• 里程计融合：使用robot_localization的ekf_node融合编码器与IMU。

6.2 参数调整：从仿真到现实

• 精确测量footprint
• 降低最大速度/加速度
• 增大inflation_radius(如0.5m)
• 调大SLAM的minimum_travel_distance和loop_search_space

6.3 启动系统：Bringup的模块化设计

• harware_bringup.launch.py：底层驱动 + robot_state_publisher
• slam_bringup.launch,py：包含硬件 + SLAM Toolbox
• nav_bringup.launch.py：包含硬件 + 定位 + Nav2核心

---

第七章：总结与展望

7.1 本文总结

从环境搭建、URDF建模、SLAM建图、Nav2导航到基于米尔RK3576开发板的实体部署，全面覆盖了ROS2 Humble下SLAM Toolbox的自主机器人系统构建过程。

7.2 下一步研究方向

• 多机器人SLAM与地图合并
• 语义导航（目标检测+导航）
• 强化学习局部规划器
• 3D导航（3D激光雷达+体素网格）

---

附录：常用命令速查表