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作为基础操作系统。请确保你的系统已更新至最新状态。

# 设置locale
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

# 添加ROS2 apt仓库
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
sudo curl -sSL
https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "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" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

# 安装ROS2 Humble Desktop（包含核心库、rqt、rviz2等）
sudo apt update
sudo apt install ros-humble-desktop

# 安装开发工具和依赖
sudo apt install python3-colcon-common-extensions python3-rosdep python3-argcomplete python3-vcstool git

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

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

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

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

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

# 安装Gazebo与ROS2接口包
sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros2-control

# 安装TurtleBot3相关包
sudo apt install ros-humble-turtlebot3* ros-humble-teleop-twist-keyboard

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

# 安装SLAM Toolbox
sudo apt install ros-humble-slam-toolbox

# 安装Nav2导航栈及其启动文件
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

# 安装其他实用工具（用于后续调试）
sudo apt install ros-humble-tf2-tools ros-humble-rqt-tf-tree

验证安装是否成功：

ros2 pkg list | grep slam_toolbox
ros2 pkg list | grep nav2_bringup

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

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build --symlink-install
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

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

export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

图1：Gazebo中TurtleBot3仿真环境

键盘遥控：

# 新终端
export TURTLEBOT3_MODEL=waffle
ros2 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示例结构（部分）：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">
<!-- 定义颜色、尺寸等常量 -->
<xacro:property name="base_length" value="0.3" />
<xacro:property name="base_radius" value="0.1" />

<!-- 底盘 link -->
<link name="base_link">
<visual>
<geometry><cylinder length="${base_length}" radius="${base_radius}"/></geometry>
<material name="blue"/>
</visual>
<collision>
<geometry><cylinder length="${base_length}" radius="${base_radius}"/></geometry>
</collision>
<inertial>
<mass value="2.0"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</inertial>
</link>

<!-- 左轮关节 -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 ${base_radius+wheel_width/2} 0" rpy="-1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>

<!-- Gazebo 差分驱动插件 -->
<gazebo>
<plugin name="gazebo_ros_diff_drive" filename="libgazebo_ros_diff_drive.so">
<ros><namespace>/</namespace></ros>
<update_rate>50</update_rate>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>${base_radius*2 + wheel_width}</wheel_separation>
<wheel_diameter>${wheel_radius*2}</wheel_diameter>
<command_topic>cmd_vel</command_topic>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_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树：

ros2 run 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 示例：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from xacro import process_file

def generate_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()
return LaunchDescription([
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.yaml
slam_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：仿真

export TURTLEBOT3_MODEL=waffle
ros2 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：键盘遥控

ros2 run teleop_twist_keyboard teleop_twist_keyboard

保存地图：

ros2 run nav2_map_server map_saver_cli -f ~/maps/my_map
ros2 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：仿真

ros2 launch 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: localization
map_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:=true params_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激光雷达+体素网格）

附录：常用命令速查表