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.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
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
pkg_share = 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 # ... 其他參數
注意：
機器人與傳感器參數：
odom_frame、base_frame必須與你的TF樹完全一致。scan_topic確保訂閱正確的數據。節點添加策略：
minimum_travel_distance和minimum_travel_heading決定了地圖的稠密程度。值越小，節點越多，地圖細節越豐富，但計算量也越大。對于大場景，可以適當增大。閉環檢測：
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_publisherslam_bringup.launch.py：包含硬件 + SLAM Toolboxnav_bringup.launch.py：包含硬件 + 定位 + Nav2核心
第七章：總結與展望
7.1 本文總結
從環境搭建、URDF建模、SLAM建圖、Nav2導航到基于米爾RK3576開發板的實體部署，全面覆蓋了ROS2 Humble下SLAM Toolbox的自主機器人系統構建過程。
7.2 下一步研究方向
多機器人SLAM與地圖合并
語義導航（目標檢測+導航）
強化學習局部規劃器
3D導航（3D激光雷達+體素網格）
附錄：常用命令速查表
評論