前言

文檔定位與目標(biāo)讀者

本文檔面向具備一定ROS基礎(chǔ)、希望深入理解并在實(shí)際項(xiàng)目中部署ROS2 Humble + SLAM Toolbox + Nav2完整建圖與導(dǎo)航系統(tǒng)的機(jī)器人工程師。我們將從零開始，基于米爾RK3576開發(fā)板逐步構(gòu)建一個(gè)功能完備的自主移動(dòng)機(jī)器人系統(tǒng)，涵蓋環(huán)境搭建、機(jī)器人建模、SLAM建圖、自主導(dǎo)航以及生產(chǎn)級(jí)系統(tǒng)的優(yōu)化與排錯(cuò)。

為什么選擇SLAM Toolbox + Nav2？

在ROS2生態(tài)中，SLAM（同時(shí)定位與建圖）與導(dǎo)航（Navigation）是機(jī)器人自主移動(dòng)的核心技術(shù)。SLAM Toolbox由Steve Macenski主導(dǎo)開發(fā)，是基于成熟Karto SLAM的改進(jìn)版本，相比傳統(tǒng)的Gmapping、Hector SLAM或Cartographer，它具有以下顯著優(yōu)勢(shì)：

• 圖優(yōu)化框架：采用基于圖優(yōu)化的后端，而非簡(jiǎn)單的濾波器，在大場(chǎng)景下地圖一致性更好。
• 生命周期管理：支持終身地圖（LifeLong Mapping），即可以在已有地圖基礎(chǔ)上繼續(xù)優(yōu)化或更新，甚至能夠移除動(dòng)態(tài)物體留下的痕跡。
• 多種運(yùn)行模式：同步/異步建圖、純定位模式（可作為AMCL的高精度替代品）、地圖序列化與反序列化。
• RViz交互插件：提供豐富的RViz工具，支持手動(dòng)修正地圖、操作圖節(jié)點(diǎn)。
• 性能卓越：經(jīng)過優(yōu)化，能夠在數(shù)十萬平方英尺的場(chǎng)景中實(shí)時(shí)運(yùn)行。

而Nav2作為ROS2的官方導(dǎo)航框架，繼承了ROS1 Navigation Stack的優(yōu)點(diǎn)并進(jìn)行了完全的重構(gòu)，支持行為樹、更靈活的插件化架構(gòu)和更好的實(shí)時(shí)性保障。將SLAM Toolbox與Nav2結(jié)合，我們可以基于RK3576開發(fā)板構(gòu)建一套從建圖到定位導(dǎo)航的無縫銜接系統(tǒng)，甚至可以在導(dǎo)航過程中邊建圖邊導(dǎo)航（Navigation while Mapping）。

核心技術(shù)棧概覽

• 操作系統(tǒng)：Ubuntu 22.04 LTS (Jammy)
• ROS發(fā)行版：ROS2 Humble Hawksbill (長(zhǎng)期支持版)
• 仿真環(huán)境：Gazebo Classic 11 (與ROS2 Humble官方集成)
• 機(jī)器人建模：URDF / Xacro
• SLAM庫：slam_toolbox (版本 ≥ 2.6.10)
• 導(dǎo)航棧：Nav2 (navigation2, nav2_bringup)
• 可視化與調(diào)試：Rviz2, tf2_tools, rqt_graph

第一章：環(huán)境搭建與準(zhǔn)備工作

1.1 操作系統(tǒng)與ROS2 Humble安裝

我們選擇Ubuntu 22.04作為基礎(chǔ)操作系統(tǒng)。請(qǐng)確保你的系統(tǒng)已更新至最新狀態(tài)。

# 設(shè)置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
# 安裝開發(fā)工具和依賴sudoapt install python3-colcon-common-extensions python3-rosdep python3-argcomplete python3-vcstool git

安裝完成后，配置環(huán)境變量以便每次打開終端時(shí)自動(dòng)加載ROS2環(huán)境：

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

注意：如果你管理多個(gè)工作空間，建議在工作空間的install目錄下使用local_setup.bash，而非全局覆蓋。后續(xù)我們會(huì)在項(xiàng)目工作空間中具體說明。

1.2 安裝仿真環(huán)境（Gazebo）與機(jī)器人模型

為了在不依賴實(shí)體硬件的情況下進(jìn)行算法驗(yàn)證，我們需要安裝Gazebo仿真環(huán)境以及經(jīng)典的TurtleBot3機(jī)器人模型，盡量在x86 虛擬機(jī)安裝仿真，arm64架構(gòu)turtlebot3支持不足。

# 安裝Gazebo與ROS2接口包sudoapt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros2-control
# 安裝TurtleBot3相關(guān)包sudoapt install ros-humble-turtlebot3* ros-humble-teleop-twist-keyboard

1.3 安裝核心算法包：SLAM Toolbox與Nav2

# 安裝SLAM Toolboxsudoapt install ros-humble-slam-toolbox
# 安裝Nav2導(dǎo)航棧及其啟動(dòng)文件sudoapt install ros-humble-navigation2 ros-humble-nav2-bringup
# 安裝其他實(shí)用工具（用于后續(xù)調(diào)試）sudoapt install ros-humble-tf2-tools ros-humble-rqt-tf-tree

驗(yàn)證安裝是否成功：

ros2 pkg list |grepslam_toolboxros2 pkg list |grepnav2_bringup

1.4 創(chuàng)建工作空間與測(cè)試安裝

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

測(cè)試仿真環(huán)境：打開新終端，運(yùn)行Gazebo仿真世界和TurtleBot3機(jī)器人：

exportTURTLEBOT3_MODEL=waffleros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

圖1：Gazebo中TurtleBot3仿真環(huán)境

鍵盤遙控：

# 新終端exportTURTLEBOT3_MODEL=waffleros2 run teleop_twist_keyboard teleop_twist_keyboard

第二章：機(jī)器人建模與仿真集成

2.1 URDF/Xacro基礎(chǔ)與傳感器配置

URDF (Unified Robot Description Format) 是ROS中描述機(jī)器人幾何、慣性、關(guān)節(jié)關(guān)系的XML格式。Xacro則是URDF的宏語言，允許我們使用變量、數(shù)學(xué)運(yùn)算和模塊化包含。

一個(gè)典型的差分驅(qū)動(dòng)機(jī)器人模型的核心部分：link、joint、transmission與gazebo插件。

下面是一個(gè)簡(jiǎn)化的差分驅(qū)動(dòng)+激光雷達(dá)的Xacro示例結(jié)構(gòu)（部分）：

"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 坐標(biāo)系變換（TF）樹詳解：map -> odom -> base_link -> sensor_link

關(guān)鍵坐標(biāo)系：

• map：世界固定坐標(biāo)系。
• odom：里程計(jì)坐標(biāo)系，連續(xù)但不穩(wěn)定。
• base_link：機(jī)器人基座坐標(biāo)系。
• laser_link等：傳感器坐標(biāo)系。

變換關(guān)系：base_link->sensor_link（靜態(tài)），odom->base_link（里程計(jì)發(fā)布），map->odom（定位系統(tǒng)發(fā)布）。

驗(yàn)證TF樹：

ros2run tf2_tools view_frames # 生成frames.pdf

2.3 自定義機(jī)器人描述文件與啟動(dòng)

標(biāo)準(zhǔn)包結(jié)構(gòu)：

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深度實(shí)踐與建圖

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 # ... 其他參數(shù)

注意：1.機(jī)器人與傳感器參數(shù)：odom_frame、base_frame必須與你的TF樹完全一致。scan_topic確保訂閱正確的數(shù)據(jù)。
2.節(jié)點(diǎn)添加策略：minimum_travel_distance和minimum_travel_heading決定了地圖的稠密程度。值越小，節(jié)點(diǎn)越多，地圖細(xì)節(jié)越豐富，但計(jì)算量也越大。對(duì)于大場(chǎng)景，可以適當(dāng)增大。
3.閉環(huán)檢測(cè)：loop_search_space是閉環(huán)檢測(cè)的搜索半徑。如果你的環(huán)境有很多相似的結(jié)構(gòu)（如長(zhǎng)走廊），需要適當(dāng)減小這個(gè)值以避免錯(cuò)誤的閉環(huán)；反之，如果傳感器噪聲大或里程計(jì)漂移嚴(yán)重，需要增大搜索空間。

3.3 手動(dòng)建圖流程與保存地圖

終端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：實(shí)體機(jī)器人建圖現(xiàn)場(chǎng)

3.4 高級(jí)話題：終身地圖與位姿圖序列化

啟用終身地圖：mode: mapping+enable_life_long_mapping: true。序列化文件(.posegraph)可保存圖節(jié)點(diǎn)信息，用于后續(xù)繼續(xù)建圖或定位模式。

第四章：Nav2導(dǎo)航系統(tǒng)構(gòu)建與配置

4.1 Nav2架構(gòu)與核心組件

• 地圖服務(wù)器、AMCL、代價(jià)地圖（全局/局部）、規(guī)劃器（Planner）、控制器（DWB）、行為樹導(dǎo)航器（BT Navigator）。

4.2 Nav2參數(shù)配置實(shí)戰(zhàn)（nav2_params.yaml節(jié)選）

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 啟動(dòng)Nav2：基于已有地圖的導(dǎo)航

終端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默認(rèn)視圖)

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

圖5：Nav2仿真導(dǎo)航界面

使用“2D Pose Estimate”初始化位姿，然后“2D Goal Pose”發(fā)送目標(biāo)。

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

修改SLAM配置文件：

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

啟動(dòng)SLAM Toolbox（定位模式）：

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

啟動(dòng)Nav2（不含AMCL）：

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

步驟1 ：在地圖上設(shè)置小車初始位置和方向；
步驟2：在地圖上設(shè)置小車單點(diǎn)導(dǎo)航：

圖6：實(shí)體機(jī)器人Nav2導(dǎo)航

第五章：高級(jí)整合與調(diào)試

5.1 邊建圖邊導(dǎo)航（Navigation while Mapping）

啟動(dòng)仿真 + SLAM建圖模式 + navigation_launch.py（不含map_server/amcl），然后通過RViz設(shè)定目標(biāo)，機(jī)器人一邊探索一邊建圖。

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

Panels -> Add Panel -> SlamToolboxPlugin 可手動(dòng)保存、清除節(jié)點(diǎn)、強(qiáng)制閉環(huán)。

5.3 性能分析與優(yōu)化

• 分析CPU/內(nèi)存：top -p `pgrep -d',' -f 'ros2|slam_toolbox|nav2'`
• 檢查話題頻率：ros2 topic hz /scan
• SLAM優(yōu)化：使用snap版slam-toolbox；增大map_update_interval；增大節(jié)點(diǎn)添加閾值。
• Nav2優(yōu)化：降低controller_frequency；增大局部代價(jià)地圖分辨率；減少DWB采樣。

5.4 常見錯(cuò)誤排解指南

第六章：實(shí)體機(jī)器人部署指南

6.1 硬件抽象與驅(qū)動(dòng)層

• 激光雷達(dá)驅(qū)動(dòng)：例如ros2 launch sllidar_ros2 view_sllidar_a1_launch.py
• 里程計(jì)融合：使用robot_localization的ekf_node融合編碼器與IMU。

6.2 參數(shù)調(diào)整：從仿真到現(xiàn)實(shí)

• 精確測(cè)量footprint
• 降低最大速度/加速度
• 增大inflation_radius(如0.5m)
• 調(diào)大SLAM的minimum_travel_distance和loop_search_space

6.3 啟動(dòng)系統(tǒng)：Bringup的模塊化設(shè)計(jì)

• harware_bringup.launch.py：底層驅(qū)動(dòng) + robot_state_publisher
• slam_bringup.launch,py：包含硬件 + SLAM Toolbox
• nav_bringup.launch.py：包含硬件 + 定位 + Nav2核心

第七章：總結(jié)與展望

7.1 本文總結(jié)

從環(huán)境搭建、URDF建模、SLAM建圖、Nav2導(dǎo)航到基于米爾RK3576開發(fā)板的實(shí)體部署，全面覆蓋了ROS2 Humble下SLAM Toolbox的自主機(jī)器人系統(tǒng)構(gòu)建過程。

7.2 下一步研究方向

• 多機(jī)器人SLAM與地圖合并
• 語義導(dǎo)航（目標(biāo)檢測(cè)+導(dǎo)航）
• 強(qiáng)化學(xué)習(xí)局部規(guī)劃器
• 3D導(dǎo)航（3D激光雷達(dá)+體素網(wǎng)格）

附錄：常用命令速查表