ROS 2 Nav2 自主导航实战
系列导航：
前言
把 Nav2 想成给机器人配的一位"认路的老司机"。你刚到手一台机器人，它对世界一无所知——墙在哪、门朝哪开，它全不知道。第一步你得带它在环境里逛一圈，画成一张地图（SLAM 建图）；地图有了，之后你只要在屏幕上点一个目标点，老司机就自己规划路线、躲开桌椅行人，稳稳开过去。
读完本文，你能让一台差速驱动机器人在 Gazebo 里从零完成"建图 → 定位 → 自主导航到指定点"，并且知道每个参数大概管什么、卡住了怎么调。全文以一台差速驱动机器人为例，分四块走：
- SLAM 建图 — 带机器人逛一圈，画出环境地图
- 导航配置 — 代价地图、规划器、控制器怎么调
- 行为树定制 — 给导航加"卡住就换办法"的智能
- 实车部署 — 从仿真搬到真机的注意点
先泼盆冷水：Nav2 不是「贴份 yaml 就能跑」的傻瓜相机，而是一整套会互相拉扯的模块——地图不准，定位就飘；代价地图太胖，机器人寸步难行；行为树没配恢复，卡一下就躺平。本文不打算把配置直接甩给你，而是每一步先讲清「这一步在解决什么、哪几个数最要命、改错了会怎样」，让你真机卡住时知道往哪拧。把上面四块吃透，你拿到任何一台机器人都能照葫芦画瓢。
动手前先把术语混个脸熟，后面不再逐个解释：
SLAM：同时定位与建图，边走边画地图。代价地图 (Costmap)：标着「撞上去代价」的网格图，规划和控都看它。全局路径 / 局部路径：全局是从起点到终点的整条线（粗）；局部是眼前几米怎么走（细）。planner / controller：规划算线，控制给速度。AMCL：自适应蒙特卡洛定位，用粒子猜自己在地图哪。行为树 (BT)：把「规划→跟随→脱困」编成可失败重试的流程。DWB：本文用的局部控制器，试走一堆轨迹挑最好的。TF：坐标系变换树，把各传感器位置统一到同一个世界。cmd_vel：底盘最终收到的速度指令话题，导航的「输出端」。use_sim_time：是否用仿真时钟，真机一定关。
记住一条主线：地图(建) → 定位(我在哪) → 规划(去哪条线) → 控制(怎么走) → 脱困(卡了咋办)，全文都绕这五步。
一、环境准备
1.1 安装 Nav2
# Ubuntu 24.04 + ROS 2 Jazzy
sudo apt install ros-jazzy-nav2-bringup \
ros-jazzy-nav2-msgs \
ros-jazzy-nav2-util \
ros-jazzy-nav2-map-server \
ros-jazzy-nav2-costmap-2d \
ros-jazzy-nav2-planner \
ros-jazzy-nav2-controller \
ros-jazzy-nav2-behavior-tree \
ros-jazzy-nav2-recoveries \
ros-jazzy-slam-toolbox \
ros-jazzy-gazebo-ros-pkgs
# 验证安装
ros2 pkg list | grep nav2装完上面一串 nav2-* 包，快速对一下它们是干嘛的，免得报错时不知道去哪找：nav2_bringup（launch 和示例配置，本文直接复用）、nav2_planner / nav2_controller / nav2_recoveries（三个服务器本体）、nav2_bt_navigator（行为树）、nav2_map_server / nav2_amcl（地图与定位）、nav2_costmap_2d（代价地图算法库）、slam_toolbox（建图，不算 Nav2 但导航前必用）。记住「带 nav2_ 前缀的是导航，slam_toolbox 是建图」这条线，看日志报错时一眼知道是哪块。
1.2 项目结构
动手前先看清家底。下面这棵树就是后面所有文件的大本营：description 管机器人长什么样，gazebo 管仿真世界，nav2 管导航的大脑（一堆 config + launch）。你不用现在背下来，只记一个原则——模型、仿真、导航三类东西分包裹，改底盘只动 description，调导航只动 nav2，互不连坐。后面每个小标题对应的文件，都能在这棵树里找到位置。
my_robot_nav/ # 工作空间
└── src/
├── my_robot_description/ # 机器人模型 (URDF/XACRO)
│ ├── urdf/
│ │ ├── my_robot.xacro
│ │ ├── diff_drive.xacro
│ │ ├── lidar.xacro
│ │ └── camera.xacro
│ ├── launch/
│ │ └── display.launch.py
│ ├── config/
│ │ └── gazebo.yaml
│ └── CMakeLists.txt
├── my_robot_gazebo/ # Gazebo 仿真
│ ├── launch/
│ │ ├── spawn_robot.launch.py
│ │ └── empty_world.launch.py
│ └── worlds/
│ └── my_world.sdf
├── my_robot_nav2/ # 导航配置
│ ├── config/
│ │ ├── nav2_params.yaml
│ │ ├── costmap_common.yaml
│ │ ├── local_costmap.yaml
│ │ ├── global_costmap.yaml
│ │ ├── dwa_controller.yaml
│ │ └── behavior_tree.xml
│ ├── launch/
│ │ ├── slam.launch.py
│ │ ├── navigation.launch.py
│ │ └── nav_bringup.launch.py
│ ├── maps/
│ │ └── (SLAM 生成的地图存放于此)
│ └── CMakeLists.txt
└── my_robot_custom_bt/ # 自定义行为树节点
├── include/
├── src/
└── plugins/导航再聪明，也得先有「身体」。这一章从零把机器人建模：先把骨架和传感器立起来，后面所有导航配置才有挂载的对象。
建模时最该建立的一个直觉是坐标系层级：base_footprint（地面投影点，导航算位置就用它）→ base_link（机器人几何中心）→ odom（里程计累积的临时原点）→ map（建好的地图原点）。它们一层套一层，靠 TF 变换连起来。
怎么确认这层没断？
补一句：TF 不是凭空来的，每个变换都由某个节点发布。robot_state_publisher 发布 URDF 里的关节（base→wheel），amcl 发布 map→odom，控制器 / 里程计发布 odom→base_footprint。所以「变换断了」本质是「某个发布者没起来或话题不对」。排错时顺着这条责任链，比盲改参数快——比如激光在代价地图里不显示，先查是不是 base_link→lidar_link 的 TF 没发（URDF 里雷达 joint 漏了）。
跑起来后执行 ros2 run tf2_tools view_frames，它会生成一张变换树 PDF。正常应该看到 map → odom → base_footprint → base_link → lidar_link 一条链，中间没有断点、也没有「俩父节点」。断在哪一层，哪一层对应的配置（URDF 关节 / 发布 TF 的节点）就出错了。这是排错第一招，比看日志快。
后面但凡看到「机器人走歪」「目标对不上」，第一反应都是「这四层变换哪一层断了」，而不是去改算法。
二、机器人模型
2.1 完整 XACRO 模型
my_robot_description/urdf/my_robot.xacro：
机器人「本体」长这样。URDF 是骨架说明书：几个轮子、雷达装多高、基座多重，全写在这。XACRO 是带变量的 URDF——先把 wheel_radius、base_radius 这些尺寸定义成常量（文件开头那一排 xacro:property），下面到处引用，换机器人只改这几个数。
你不用逐行读它，跑导航时只确认三件事：轮子半径和轮距填对了（决定里程计准不准）、base_footprint 到 base_link 的抬高量对了（决定坐标系对不对）、Gazebo 差速插件接上了（决定能不能动）。这三件错一件，后面全歪。
<robot xmlns:xacro="https://www.ros.org/wiki/xacro" name="my_robot">
<!-- ====== 常量定义 ====== -->
<xacro:property name="PI" value="3.14159265"/>
<xacro:property name="base_radius" value="0.25"/>
<xacro:property name="base_height" value="0.12"/>
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_width" value="0.04"/>
<xacro:property name="wheel_separation" value="0.40"/>
<xacro:property name="wheel_y_offset" value="${wheel_separation/2}"/>
<xacro:property name="lidar_height" value="0.10"/>
<xacro:property name="camera_height" value="0.08"/>
<!-- ====== 差速驱动插件 ====== -->
<xacro:include filename="$(find my_robot_description)/urdf/diff_drive.xacro"/>
<!-- ====== LiDAR ====== -->
<xacro:include filename="$(find my_robot_description)/urdf/lidar.xacro"/>
<!-- ====== 相机 ====== -->
<xacro:include filename="$(find my_robot_description)/urdf/camera.xacro"/>
<!-- ====== 基座 ====== -->
<link name="base_footprint"/>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_radius}" length="${base_height}"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.4 0.8 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${base_radius}" length="${base_height}"/>
</geometry>
</collision>
<inertial>
<mass value="10.0"/>
<origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
<cylinder_inertia m="10.0" r="${base_radius}" h="${base_height}"/>
</inertial>
</link>
<joint name="base_footprint_to_base" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 ${wheel_radius + 0.01}" rpy="0 0 0"/>
</joint>
<!-- ====== 里程计帧 ====== -->
<link name="odom"/>
<joint name="base_to_odom" type="fixed">
<parent link="base_link"/>
<child link="odom"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- 实例化组件 -->
<xacro:diff_drive prefix="left" x="0" y="${wheel_y_offset}"/>
<xacro:diff_drive prefix="right" x="0" y="${-wheel_y_offset}"/>
<xacro:rplidar parent="base_link" height="${lidar_height}"/>
<xacro:camera parent="base_link" height="${camera_height}"/>
</robot>写 URDF 最容易踩的坑，先列出来省你两小时：
- 单位用米：ROS 里长度全是米、质量全是千克，写
0.25不是25cm，写错机器人会「瞬间飞出地图」。 - joint 的 parent/child 别反：父子关系决定坐标怎么传，反了 TF 树就倒。
- 质量 / 惯量别留 0：惯量为 0 的刚体在物理引擎里会被当成无穷大质量，Gazebo 里机器人会抖或不动。给个合理值即可。
差速驱动的运动学一句话：左轮快、右轮慢 → 向右转；两轮同速 → 直行。所以「转向」本质是两轮转速差，这也解释了为什么轮距和轮半径填错，里程计必然漂——它直接决定「转一度轮，机器人转多少度」。
2.2 差速驱动 XACRO
urdf/diff_drive.xacro：
轮子怎么转、底盘怎么动，全在这个宏里。它干两件大事：第一，用 type="continuous" 的 joint 定义左右轮旋转关节——差速驱动就是靠两轮转速差来转向；第二，挂上 Gazebo 的差速插件（libgazebo_ros_diff_drive.so），把「左右轮转速」翻译成「机器人实际位移和里程计」。
最该盯的是 wheel_separation 和 wheel_radius（必须和 2.1 里定义的常量一致），以及 odom_frame / robot_base_frame（定位系统就靠这两个坐标系对齐）。这两个填错，典型症状是「转一圈实际没转够」或里程计肉眼可见地飘。
<robot xmlns:xacro="https://www.ros.org/wiki/xacro">
<xacro:macro name="diff_drive" params="prefix x y">
<link name="${prefix}_wheel_link">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
<material name="black">
<color rgba="0.1 0.1 0.1 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
</collision>
<inertial>
<mass value="0.5"/>
<cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
</inertial>
</link>
<joint name="${prefix}_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_wheel_link"/>
<origin xyz="${x} ${y} ${-base_height/2}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- Gazebo 差速驱动插件 -->
<gazebo reference="${prefix}_wheel_link">
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<material>Gazebo/DarkGrey</material>
</gazebo>
</xacro:macro>
<!-- 差速驱动控制器 -->
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<!-- 运动学参数 -->
<wheel_separation>${wheel_separation}</wheel_separation>
<wheel_radius>${wheel_radius}</wheel_radius>
<!-- 里程计 -->
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
<!-- 速度和加速度限制 -->
<max_wheel_torque>20.0</max_wheel_torque>
<max_wheel_acceleration>5.0</max_wheel_acceleration>
<!-- 发布配置 -->
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_source>world</odometry_source>
</plugin>
</gazebo>
<!-- 里程计 IMU -->
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu/data</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
</gazebo>
</robot>2.3 LiDAR XACRO
urdf/lidar.xacro：
激光雷达是导航的「眼睛」。这个宏定义一个圆柱当雷达外壳，再用 Gazebo 的 ray 传感器插件模拟 360° 扫描，话题名 scan，类型 LaserScan。
两个地方最影响建图质量：samples（一圈打多少点，越多越密）、range 里的 max（最远测距，要盖过你环境的最大尺度）。noise 那段是仿真噪声，真机不用管；真机换型号时只改 samples / range 和话题名即可，结构不用动。
<robot xmlns:xacro="https://www.ros.org/wiki/xacro">
<xacro:macro name="rplidar" params="parent height">
<link name="lidar_link">
<visual>
<geometry>
<cylinder radius="0.04" length="0.05"/>
</geometry>
<material name="red">
<color rgba="0.8 0.2 0.2 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.05"/>
</geometry>
</collision>
</link>
<joint name="lidar_joint" type="fixed">
<parent link="${parent}"/>
<child link="lidar_link"/>
<origin xyz="0 0 ${base_height + height}" rpy="0 0 0"/>
</joint>
<!-- Gazebo LiDAR 仿真插件 -->
<gazebo reference="lidar_link">
<sensor type="ray" name="lidar_sensor">
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.0</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.12</min>
<max>12.0</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="lidar_plugin" filename="libgazebo_ros_ray_sensor.so">
<ros>
<namespace>/</namespace>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>lidar_link</frame_name>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>如果你从 ROS 1 过来，一句话对应：本文的 bt_navigator + planner + controller + recoveries 这套，就是 ROS1 里 move_base 的「拆开重做版」。区别是 Nav2 把规划 / 控制 / 脱困拆成独立服务器，用行为树编排，比 move_base 那根写死的 recovery 链条灵活得多，也更好加自己的逻辑。老用户最大的不适应是「找不到 move_base 节点了」——它变成了上面那五个。
三、SLAM 建图
SLAM 听着吓人，拆开就两步。第一步「定位」：机器人每走一点，靠里程计（轮子里程）和 IMU 估个大概位置，但这估法会越走越漂，叫累积误差。第二步「建图」：激光雷达扫到墙，把墙的轮廓和「我刚估的位置」绑在一起，画进地图。麻烦在于这俩互相依赖——位置不准，墙就画歪；墙画歪，又反过来让位置更不准。SLAM 求解器就是在每一帧里把两者一起优化。
你会看到地图「重影」「漂移」，多半是里程计太漂（轮子打滑、轮距填错），求解器太信轮子、不信雷达，墙就被画到错的位置。slam_toolbox 用回环检测（loop closing）来救：
SLAM 内部还分「前端」和「后端」：前端负责每一帧把激光点和上一帧对上（帧间匹配），算出「我刚挪了多少」；后端把一路上的相对位移拼成全局一致的地图，并做回环校正。你调的 odom_alpha* 影响前端信不信轮子，max_laser_range 和噪声影响前端点云质量，do_loop_closing 是后端的杀手锏。理解这层，建图飘时你就知道该拧参数还是该走慢点，而不是瞎改。
当你走回见过的墙，它发现「这墙我见过」，把整张图拉正。所以建图时慢走、多走回头路，比快冲一圈有用得多。
本文用 slam_toolbox 的异步模式：边走边建，还能在导航时继续微调地图。参数都在 3.1 那份 yaml，你只要把住四个旋钮：resolution（精度）、max_laser_range（有效测距）、do_loop_closing（回环开关）、odom_alpha*（信雷达还是信轮子）。
3.1 SLAM 配置文件
my_robot_nav2/config/slam_params.yaml：
上面那段参数直接存成这个文件。建图时你几乎不用改它，唯一常动的是 map_file_name（地图存哪）和 resolution。跑起来后盯 RViz：地图边缘清晰、没有重影就对了；有重影先回头调 odom_alpha* 或走慢点，别急着改别的。
slam_toolbox:
ros__parameters:
# 使用异步模式（推荐，支持在线地图保存）
mode: mapping
# 里程计话题
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
# 地图更新频率
map_update_interval: 2.0
resolution: 0.05 # 地图分辨率 5cm/格
max_laser_range: 12.0
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
# 相关性搜索（性能优化）
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# 扫描匹配器
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
# 循环闭合检测
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# 扫描器设置
laser_model_type: likelihood_field
laser_likelihood_max_dist: 2.0
laser_min_range: 0.12
laser_max_range: 12.0
laser_sample_step: 0.05
# 运动模型
odom_model_type: diff
odom_alpha1: 0.2 # 旋转引起的旋转噪声
odom_alpha2: 0.2 # 平移引起的旋转噪声
odom_alpha3: 0.2 # 平移引起的平移噪声
odom_alpha4: 0.2 # 旋转引起的平移噪声
odom_alpha5: 0.1 # 平移修正
# 在线异步（支持在导航的同时更新地图）
use_scan_matching: true
use_scan_barycenter: true
# 地图保存路径
map_file_name: /home/user/my_robot_nav/src/my_robot_nav2/maps/my_map
map_start_pose: [0.0, 0.0, 0.0]
debug_logging: false
throttle_scans: 13.2 SLAM Launch 文件
这个 launch 文件一次性把仿真环境、机器人、SLAM 节点和 RViz 全部拉起来。运行它，你会看到 Gazebo 里站着的机器人和 RViz 中逐渐浮现的地图——这就是建图现场。
my_robot_nav2/launch/slam.launch.py：
#!/usr/bin/env python3
"""SLAM 建图 Launch 文件"""
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
pkg_nav2 = get_package_share_directory('my_robot_nav2')
pkg_gazebo = get_package_share_directory('my_robot_gazebo')
# 启动 Gazebo 仿真环境
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo, 'launch', 'empty_world.launch.py'))
)
# 启动机器人
spawn_robot = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo, 'launch', 'spawn_robot.launch.py'))
)
# SLAM Toolbox 节点（在线异步建图）
slam_toolbox = Node(
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen',
parameters=[
os.path.join(pkg_nav2, 'config', 'slam_params.yaml'),
# 覆盖地图保存路径
{'map_file_name': os.path.join(pkg_nav2, 'maps', 'my_map')},
],
)
# RViz2 建图可视化
rviz = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(pkg_nav2, 'config', 'slam.rviz')],
name='rviz2',
)
return LaunchDescription([
gazebo_launch,
spawn_robot,
slam_toolbox,
rviz,
])建图质量 80% 取决于你怎么走，不是参数。三条经验：
- 先在原地转一圈：让激光扫满 360°，把初始位姿和周围第一圈墙固定住，后面才不漂。
- 慢且匀：0.2-0.3 m/s 匀速推进，急转弯时激光容易丢特征，地图就裂。
- 关键区域来回走：走廊、门口、岔路多走两遍，给回环检测更多「我见过了」的证据，地图更闭合。
建完别急着关，先在 RViz 看整张图有没有明显断裂或重影；有的话回到那段重新走，比后面在导航里 debug 省事十倍。
3.3 手动遥控建图
# 终端 1：启动 SLAM
ros2 launch my_robot_nav2 slam.launch.py
# 终端 2：遥控机器人扫描环境
ros2 run teleop_twist_keyboard teleop_twist_keyboard
# 操作技巧：
# 1. 先在原地 360° 旋转一圈，初始化地图
# 2. 以 0.2-0.3 m/s 速度缓慢移动
# 3. 走廊、门口等关键区域要来回走几遍
# 4. 避免急转弯，保持匀速建图「飘 / 重影 / 建不出」先自查这三点，十有八九中：
- 地图在转但墙不累积：激光话题名不对（
scan_topic填错），SLAM 收不到点云。ros2 topic echo /scan看有没有数据。 - 走直线地图却歪：里程计漂，回 2.2 核轮距 / 轮半径，或走更慢。
- 回到原点对不上（不闭合）：关了回环，把
do_loop_closing设回 true，或环境特征太少（空白墙）导致无特征可匹配，多加点家具 / 标志物。
补充：激光看不到的东西（玻璃门、纯色反光墙、强日光下的深色面）在建图时会「穿透」，地图出现空洞。对策：环境里加点纹理 / 标志物帮匹配；真机用抗强光雷达。建图时某面墙始终画不上，别硬调参数，先怀疑是传感器物理极限。
确认地图清晰后再保存，不然后面导航全程建立在歪地图上。
还有个隐蔽坑：导航用的地图必须和建图时是同一份、同一坐标系。很多人建图后改了机器人尺寸或雷达高度，没重新建图，结果「地图对、激光不对」——激光打到的墙和地图里的墙差了半米。铁律：动过 URDF 的传感器 / 底盘，重新建图再用。地图文件用 git 管理，每次重建提交一次，出问题能回滚到「那次还能用」的版本。
3.4 保存地图
# 方式 1：使用 slam_toolbox 的服务保存
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap \
"{name: {data: 'my_map'}}"
# 方式 2：使用 map_saver 手动保存
ros2 run nav2_map_server map_saver_cli \
-f ~/my_robot_nav/src/my_robot_nav2/maps/my_map
# 生成文件：
# my_map.pgm (灰度地图图像)
# my_map.yaml (地图元数据)保存地图会落下两个文件：my_map.pgm 是灰度图（白=空、黑=墙、灰=未知），my_map.yaml 是元数据。yaml 里最该懂的是 origin（地图左下角在 map 坐标系的位置，建图起点不对时整图偏移）和 occupied_thresh / free_thresh（黑白判定阈值，环境反光强可调）。导航时只读这个 yaml，pgm 当图片加载，你一般不用手动改 pgm。
3.5 地图 YAML 示例
image: my_map.pgm
resolution: 0.05 # m/pixel
origin: [-10.0, -10.0, 0.0] # 左下角在 map 坐标系中的位置
negate: 0 # 0=白色=空闲, 1=黑色=空闲
occupied_thresh: 0.65 # > 65% 黑 = 障碍物
free_thresh: 0.25 # < 25% 黑 = 空闲地图有了，机器人也知道自己在哪了。现在的问题是：给它一个目标点，它怎么安全开过去？这就是 Nav2 的核心——导航配置。
代价地图（Costmap）是什么？ 你可以把它想成一张"危险热力图"。地图每个格子标着撞上去的代价：墙边是深红（千万别碰），离墙半米是浅黄（尽量别来），空地是绿色（随便走）。规划器和控制器都盯着这张图决定路线。
四、Nav2 导航配置（核心）
代价地图是导航的「眼睛」，本质是一张「危险热力图」：每个格子标着撞上去的代价，墙边深红（别碰）、离墙半米浅黄（尽量别来）、空地绿色（随便走）。它分三层叠起来：
- static_layer（静态层）：从建好的地图来，墙永久标红。
- obstacle_layer（障碍层）：从实时激光来，动态行人 / 椅子随时标红、走了就清掉。
- inflation_layer（膨胀层）：在红区外面再画一圈「渐变的黄」，离障碍越近代价越高，逼机器人自觉保持距离。
planner 看全局代价地图决定大方向，controller 看局部代价地图躲眼前障碍，两份配置都继承下面这份公共地基。
4.1 公共代价地图参数
config/costmap_common.yaml：
代价地图的「公共地基」，local 和 global 两层都继承它。重点看三块：footprint（机器人轮廓，碰撞判定就靠它，填成你底盘的矩形）、inflation_layer 的 inflation_radius 和 cost_scaling_factor（这俩决定机器人和墙保持多远，调参篇会反复拧）、observation_sources（告诉代价地图「激光从哪来」）。voxel_layer 默认关着，纯 2D 室内用不到，别开。
observation_sources 下的 marking / clearing 两个开关常被忽略：marking=true 表示激光扫到障碍就往地图标红；clearing=true 表示激光「穿过空地」时把原来的红清掉（障碍走了）。两个都得开，否则会出现「人走了墙还在」的鬼打墙。raytrace_range 要比 obstacle_range 大，清障才清得远——这是调静态 / 动态障碍分离的关键。
footprint 不是随便画的：必须比你底盘实际外廓略小或相等，错了有两种后果——画大了，机器人以为自己比实际胖，明明能过的缝不过；画小了，会贴墙蹭甚至卡住。简单机器人用矩形（本文四角坐标），异形底盘用多边形逐点描。实用技巧：RViz 打开 Footprint 图层，看轮廓是不是正好压在模型上，偏了回去改坐标。
进阶一点：如果你的机器人不是方形（比如圆形底盘带突出传感器），用多边形逐点写 footprint 更准——按激光实际能扫到的外缘描点，而不是套个大矩形。代价是点多了计算稍重，但能少撞。反例：圆形扫地机用方形 footprint，四角永远标红，开阔地也绕着走。形状贴近真实，代价地图才贴近真实。
# 公共参数（local 和 global 共享）
costmap_common:
ros__parameters:
# 坐标帧
robot_base_frame: base_footprint
global_frame: map
odom_topic: /odom
use_sim_time: false
# 代价地图分辨率和尺寸
resolution: 0.05 # 5cm/格
footprint: "[[-0.20, -0.20], [0.20, -0.20], [0.20, 0.20], [-0.20, 0.20]]"
footprint_padding: 0.03 # 膨胀 padding
# 传感器数据源
observation_sources: scan
scan:
topic: /scan
data_type: "LaserScan"
marking: true # 标记障碍物
clearing: true # 清除障碍物
obstacle_range: 5.0 # 障碍物检测最大距离
raytrace_range: 8.0 # 射线追踪清除距离
inf_is_valid: false
max_obstacle_height: 2.0
clearing_allowed: true
# ====== 各层配置 ======
# 静态地图层
static_layer:
enabled: true
map_subscribe_transient_local: true
map_topic: /map
# 障碍物层（激光扫描）
obstacle_layer:
enabled: true
max_obstacle_height: 2.0
combination_method: 1 # 1=max (取最高代价)
# 膨胀层（关键调参！）
inflation_layer:
enabled: true
cost_scaling_factor: 3.0 # 代价衰减系数（越小膨胀越大）
inflation_radius: 0.55 # 膨胀半径（机器人半径 + 安全余量）
# 体素层（3D 障碍物，可选）
voxel_layer:
enabled: false
voxel_size: 0.05
origin_z: 0.0
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
unknown_threshold: 15
# Always True 层（强制自由区域）
always_send_full_costmap: truelocal 和 global 两份代价地图不是复制粘贴，而是分工：global 覆盖整张地图、更新慢（1Hz），负责大方向；local 是机器人周围 6m×6m 的滚动窗口、更新快（5Hz），负责躲眼前障碍。
所以你改 inflation_radius 时要分清改哪份：local 的膨胀直接决定「敢不敢贴墙过窄道」，global 的膨胀决定「规划出的线离墙多远」。窄道卡住优先动 local，整体绕远路优先动 global。两份的 resolution 通常一致（5cm），改小更精细也更吃内存。
分辨率怎么选：5cm 是室内甜点区，细节够、内存合理；仓库等大场景用 0.1-0.2m 更省资源，反正墙都大。注意分辨率和膨胀要配套——分辨率粗了（格子大），inflation_radius 也得按格子数相应调，否则「看起来膨胀很小实际贴墙」。改分辨率后整份代价地图参数建议一起复算，别只改一个数。
inflation_layer 的代价公式直觉：离障碍距离 d，代价 ≈ exp(-cost_scaling_factor × (d - 机器人半径))，超过 inflation_radius 就归零（纯绿）。所以 cost_scaling_factor 越大，黄区越薄、机器人越「敢贴墙」；inflation_radius 越大，黄区越厚、离墙越远。调这两个就是调「保守 vs 激进」——室内拥挤环境偏保守（大半径），开阔仓库可偏激进（小半径）。
一个常被问的问题：建图和导航用同一份代价地图吗？不用。建图时只有静态地图（slam 产出）；导航时代价地图才叠加上实时激光（obstacle_layer）。所以你会在 global 里开 static_layer、local 里关它——global 看建好的整图，local 看眼前实时障碍。理解这个区别，就不会疑惑「为什么 local 代价地图里地图是空的」。
一句话记住代价地图的「三层两图」：三层是 static / obstacle / inflation，两图是 global（整图 + 静态）和 local（窗口 + 实时）。调参时认准你动的在哪层、作用于哪张图，就不会改了没感觉。这条心法配合第七章的表，基本覆盖九成调参场景——剩下的一成，才是真要去读 Nav2 源码的硬骨头。
4.2 局部代价地图
config/local_costmap.yaml：
local_costmap:
ros__parameters:
update_frequency: 5.0 # 5Hz 更新
publish_frequency: 2.0 # 2Hz 发布
global_frame: odom # 局部地图在 odom 帧下
rolling_window: true # 滚动窗口
width: 6 # 6m x 6m
height: 6
resolution: 0.05
# 插件栈顺序（从下到上叠加）
plugins: ["voxel_layer", "inflation_layer"]
# 不使用静态地图层（局部用实时传感器数据）
static_layer:
enabled: false
obstacle_layer:
enabled: false4.3 全局代价地图
config/global_costmap.yaml：
global_costmap:
ros__parameters:
update_frequency: 1.0 # 1Hz 更新（全局不需要高频）
publish_frequency: 1.0
global_frame: map # 全局地图在 map 帧下
robot_base_frame: base_footprint
rolling_window: false # 不滚动，覆盖整张地图
# 插件栈
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]DWB 是「握方向盘的人」——给定一条全局路径，它算出每瞬间该以多快的速度前进、转多大弯。它不发明路线，只在路径旁边「试走」一堆小轨迹，再用一组**评分器（critics）**打分挑最好的那条。
参数分三群：max_vel_x / max_vel_theta（速度上限，室内别给太大）、sim_time + *_samples（向前模拟多少种走法再挑最好的，越大越聪明也越吃 CPU）、critics（评分器，决定「贴墙走」还是「走中间」，权重越大越被看重）。新手九成调参只动 max_vel_x、sim_time 和 xy_goal_tolerance（到点容差）。
critics 列表里每个带 scale 的项就是一票。打分逻辑是「代价 = 各 critic 输出 × 权重 求和」，分低的轨迹胜出。常用几个：BaseObstacle（离障碍多近，权重越大越怕撞）、PathDist / GoalDist（离全局路径 / 目标多远，越大越贴线）、Oscillation（惩罚左右横跳，专治振荡）。想更贴路径就调大 PathDist，想更敢靠近障碍就调小 BaseObstacle。改一个记一个，别全调。
本文用 DWB，Nav2 还有另一主流局部控制器 TEB（Timed-Elastic-Band）。区别一句话：DWB 是「采样一堆轨迹挑最好的」，简单稳、调参直观，适合差速 / 阿克曼；TEB 是「优化一条带时间的最优带」，更平滑、能处理倒车 / 非完整约束，但配置更繁、更吃算力。新手先用 DWB 跑通，真遇到「必须倒车入库」这类场景再换 TEB，别一上来折腾。
4.4 DWB 控制器
config/dwa_controller.yaml：
这份就是 DWB 的落地配置。
参数怎么被加载的，搞清能少踩坑：launch 里把 params_file（即 nav2_params.yaml）整体喂给每个 Composable 节点，节点再按自己关心的段（如 controller_server 下的 FollowPath）读值。所以同一份 yaml 被所有节点共享，改一个段只影响对应节点。若某参数「改了不生效」，先确认改的段名和节点实际读的段名一致（大小写 / 缩进），再确认 launch 真指向这份文件、没有别处覆盖。很多「不生效」其实是改错段或路径指错。
看的时候抓住三行：max_vel_x（前进上限，室内 0.3-0.5 稳妥）、sim_time（轨迹预测时长，大空间调到 2.0+，小空间降到 1.0 更跟手）、critics 列表里的权重（PathDist / GoalDist 越大越贴路径，BaseObstacle 越大越怕撞）。其余默认即可。
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
# 跟随路径控制器
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: true
min_vel_x: 0.0 # 最小前进速度
max_vel_x: 0.5 # 最大前进速度 (m/s)
max_vel_y: 0.0 # 差速驱动，y 方向速度为 0
max_vel_theta: 1.5 # 最大旋转速度 (rad/s)
min_speed_xy: 0.1 # 低于此速度将停止
max_speed_xy: 0.5
min_speed_theta: 0.2
# 加速度限制
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
# 轨迹生成参数
sim_time: 1.7 # 轨迹模拟时间 (s)
sim_granularity: 0.025 # 轨迹点间距 (m)
vx_samples: 20 # x 速度采样数
vy_samples: 1 # y 速度采样数
vtheta_samples: 40 # 角速度采样数
# 目标容差
xy_goal_tolerance: 0.25 # 到达目标的距离容差
yaw_goal_tolerance: 0.25 # 到达目标的角度容差
latch_xy_goal_tolerance: false
# 轨迹评分器（按优先级排列）
critics: [
"RotateToGoal", "Oscillation", "BaseObstacle",
"GoalAlign", "PathAlign", "PathDist", "GoalDist"
]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
Oscillation.scale: 1.0Nav2 不是一个程序，是五个各管一摊的服务器，由行为树当总指挥调度。记住这五个，那一堆 yaml 就不吓人了：
- planner_server（规划）：拿到「我在哪、目标在哪」，算出一条全局路径。像高德给你规划路线。本文用 NavfnPlanner，底层是 Dijkstra 或 A*（
use_astar切换：A* 快但可能不平滑，Dijkstra 稳但慢）。 - controller_server（控制 / 局部规划）：拿全局路径，结合实时障碍算「这一刻给轮子什么速度」。像老司机看眼前路况打方向盘。本文用 DWB。
- recoveries_server（脱困）：卡住时执行旋转、后退、清地图。
- bt_navigator（行为树）：把上面几个编排成「规划→跟随→卡了就脱困」，是导航的大脑。
- lifecycle_manager（生命周期）：统一管理这些节点的启动 / 关闭顺序，免得「地图还没好就让机器人走」。
代价地图是 planner 和 controller 共用的眼睛。理解了这条链，调参时你就知道该动哪份文件。
planner 怎么选：本文用 NavfnPlanner，底层是 Dijkstra（默认，找全局最短、稳但慢）或 A*（use_astar: true，快但路径偏「格子感」、不够平滑）。室内小场景两者差别不大；大 warehouse 用 A* 省时间。想要更平滑的轨迹，Nav2 还有 Smac 规划器（支持圆弧 / 混合 A*），代价是配置更繁。新手先用默认 Dijkstra 跑通，再按需换。
用一张「谁调谁」的图把五个服务器串起来，比记定义更牢：行为树（bt_navigator）是总指挥，它调用 planner 算路、调用 controller 跟线、调用 recoveries 脱困；lifecycle_manager 只管这些节点的生命周期（配置→激活→关闭），不参与算法。map_server 和 amcl 是「基础设施」，给 planner / controller 提供地图和定位。所以行为树一启动就拉起整条链——你点目标，真正干活的是被它调度的那几个服务器，不是行为树自己。
4.5 Nav2 主参数文件
config/nav2_params.yaml：
这是 Nav2 的「总开关」，把上面五个服务器的参数收在一份里。planner_server / controller_server 大多沿用前面分文件的值；真正要你心里有数的是 plugin_lib_names 那一长串——行为树能调用的「技能清单」，标准件，照抄即可；还有 lifecycle_manager 的 node_names，列了哪些节点一起启动。**这片文件别手敲，复制后只改路径和个别阈值。
行为树里还有个隐藏旋钮 default_server_timeout（4.5 顶部，默认 20s）：单个动作（如规划）超过这么久没回，行为树就当失败进脱困。真机算力慢、规划偶尔卡，把它调大（比如 30-60s）能避免「明明在算却被误判失败」。但调太大也会让真卡死时反应迟钝，按真机实测取中值。
**
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom
bt_loop_duration: 10 # 行为树循环间隔 (ms)
default_server_timeout: 20 # Action 超时 (s)
# 行为树 XML
default_nav_to_pose_bt_xml: ""
default_nav_through_poses_bt_xml: ""
# 下面一长串是行为树可用节点的标准清单，照抄即可，不必逐行理解
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_node
# 规划器服务器
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: false
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false # true=A*, false=Dijkstra
allow_unknown: true
# 控制器服务器
controller_server:
ros__parameters:
use_sim_time: false
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
progress_checker_plugins: ["progress_checker"]
goal_checker_plugins: ["general_goal_checker"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: true
# 恢复服务器
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_footprint
transform_timeout: 0.1
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
# 生命周期节点管理器
lifecycle_manager:
ros__parameters:
use_sim_time: false
autostart: true
node_names:
['controller_server', 'planner_server', 'recoveries_server',
'bt_navigator', 'waypoint_follower']行为树（Behavior Tree）是给导航装「遇事不慌的脑子」。它不像普通 if/else 写死流程，而是用节点组合出「先规划、再跟线、卡住就换招」的策略，哪个分支失败就自动滑到下一个，天然适合「失败了怎么办」。
核心节点记三个：PipelineSequence（把「规划→跟随」串成流水线，边走边重规划，不用等走完再算下一段）、RecoveryNode（主流程失败就进恢复分支，重试 N 次）、ReactiveFallback（里面挂清地图、旋转、后退三招）。下面这份 XML 就是一棵自定义树，直接对应这三层。
4.6 自定义行为树
config/navigation_behavior_tree.xml：
照着上面三层写：外层 RecoveryNode 包住「规划+跟随」，内层 ReactiveFallback 挂三种脱困。想加新招（比如「绕行」），往 ReactiveFallback 里再塞一个节点即可，不用动别的。
恢复行为（recoveries）是导航的「安全气囊」。本文默认三个：Spin（原地转一圈，扫掉盲区障碍）、BackUp（后退一点，脱离卡死）、Wait（停一下，等动态障碍走开）。它们挂在行为树的 ReactiveFallback 里，主流程（规划+跟随）一失败就按顺序试。注意恢复也会失败（真被四面围住），这时行为树放弃本次目标并报错——这不是 bug，是「确实过不去」的正常反馈。真机常把 Spin 角度调小，避免大空间里无意义猛转。
注意节点名要和 4.5 里 plugin_lib_names 清单对得上，否则行为树找不到这个技能。
<!--
自定义导航行为树
核心逻辑：规划 → 跟随 → 检查目标 → 恢复（失败时）
-->
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<!-- 尝试规划并跟随路径 -->
<PipelineSequence name="NavigateWithReplanning">
<!-- 1. 计算路径 -->
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<!-- 2. 跟随路径（带速度控制） -->
<FollowPath path="{path}" controller_id="FollowPath"/>
</PipelineSequence>
<!-- 恢复策略（PipelineSequence 失败时触发） -->
<ReactiveFallback name="RecoveryFallback">
<!-- 策略 1：清除局部代价地图（处理动态障碍物） -->
<ClearEntireCostmap name="ClearLocalCostmap"
service_name="local_costmap/clear_entirely_local_costmap"/>
<!-- 策略 2：旋转扫描（处理盲区障碍物） -->
<Spin spin_dist="1.57"/>
<!-- 策略 3：后退（处理卡死情况） -->
<BackUp backup_dist="0.3" backup_speed="0.15"/>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>配置都齐了，但还散着。这一章用一个 launch 把它们「装配成厂」：地图、定位、规划、控制一次性拉起，点目标就能动。
先把整条链路串一遍，卡住时你就知道去哪查：
- 你在 RViz 点目标 →
bt_navigator收到 NavigateToPose 目标。 - 行为树先调
planner_server算全局路径（基于全局代价地图 + 地图）。 - 再调
controller_server，它每帧看局部代价地图，算速度指令发给底盘（/cmd_vel）。 - 底盘动了 → 里程计 + 激光更新 → 代价地图刷新 → 控制器下一帧再算，循环。
amcl持续把「激光匹配地图」的结果写成map→odom变换，让全局路径和真实位置对齐。- 一旦 controller 报告「卡住 / 进度不足」，行为树切到
recoveries_server脱困，重试。
所以「机器人不动」先问：目标发出去了吗、路径算出来了吗、速度发出来了吗、代价地图有障碍吗、坐标变换断了吗——顺着这条链五分钟定位。
一个新手必踩的坑：use_sim_time。仿真时所有节点要用仿真时钟（Gazebo 发布的 /clock），所以 launch 里 use_sim_time 多为 true；真机没有 /clock，必须全设 false，且所有节点要一致——只要有一个节点还用 sim time，它和其他节点的时间对不上，TF 和定位立刻崩。移植真机第一件事就是把所有 use_sim_time 关掉。
五、导航 Launch 文件
导航模式的总装配车间。
本文的行为树够日常用了，但真机常要加自己的脱困逻辑，比如「检测到前方是人就停」。加法是三步：
- 写一个继承行为树节点的 C++/Python 插件，实现
tick()逻辑（返回成功 / 失败 / 运行中三态之一）。 - 把它编进一个包，在 4.5 的
plugin_lib_names清单里登记名字。 - 在 4.6 的 XML 里，往
ReactiveFallback或主流程塞一个节点标签。
关键点：自定义节点必须正确返回三态，行为树靠它决定下一步；返回不对，整棵树会卡死或疯狂重试。
把 4.6 的 XML 节点和密码本对一下：ComputePathToPose = 调规划器算路径；FollowPath = 调控制器跟线；ClearEntireCostmap = 清局部地图（动态障碍卡住时很有用）；Spin / BackUp = 两种脱困。这些名字必须和 4.5 的 plugin_lib_names 清单一字不差，否则行为树启动找不到节点直接报错。改 XML 后若启动报「missing node」，第一反应就是去核对这俩地方的名字。
行为树节点的三态是它和状态机最大的不同：RUNNING 表示「还在干」，让父节点保持等待；SUCCESS / FAILURE 才推进分支。正是这个 RUNNING，让「跟随路径」能一边走一边被评价，而不是「要么完要么败」。写自定义节点时，长时间任务要持续返回 RUNNING，干完了再返回 SUCCESS，否则行为树会以为你瞬间完了或瞬间挂了，流程就乱了。
为什么 Nav2 选行为树而不是老式状态机（FSM）？FSM 把「状态」和「转移」写死在代码里，加一个脱困逻辑要改一堆 case；行为树把逻辑变成可组合节点树，加招只需往 XML 塞一个节点，不改框架。这对导航特别重要——真实环境千奇百怪，「卡住怎么办」永远列不全，行为树让你用配置而非改码去扩展。理解这点，你就明白为什么本文 4.6 让你改 XML 而不是改 C++。
第一次建议只加一个「打印日志」的节点练手，确认能挂上再写真逻辑。
地图服务器、AMCL 定位、各个 Nav2 节点都在这儿组装成一个可启动的系统。跑起来后，你在 RViz 里点个目标点，机器人就真的动起来了。
my_robot_nav2/launch/navigation.launch.py：
#!/usr/bin/env python3
"""Nav2 自主导航 Launch 文件"""
import os
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node, LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
pkg_share = get_package_share_directory('my_robot_nav2')
pkg_desc = get_package_share_directory('my_robot_description')
pkg_gazebo = get_package_share_directory('my_robot_gazebo')
use_sim = LaunchConfiguration('use_sim')
map_yaml = LaunchConfiguration('map')
params_file = LaunchConfiguration('params_file')
bt_xml = LaunchConfiguration('bt_xml')
autostart = LaunchConfiguration('autostart')
# === 声明启动参数 ===
declare_use_sim = DeclareLaunchArgument(
'use_sim', default_value='true',
description='是否使用仿真时间')
declare_map_yaml = DeclareLaunchArgument(
'map', default_value=os.path.join(pkg_share, 'maps', 'my_map.yaml'),
description='地图文件路径')
declare_params_file = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(pkg_share, 'config', 'nav2_params.yaml'),
description='Nav2 参数文件')
declare_bt_xml = DeclareLaunchArgument(
'bt_xml',
default_value=os.path.join(pkg_share, 'config',
'navigation_behavior_tree.xml'),
description='行为树 XML 文件')
declare_autostart = DeclareLaunchArgument(
'autostart', default_value='true',
description='自动启动导航节点生命周期')
# === 设置 use_sim_time ===
set_sim_time = SetEnvironmentVariable(
name='ROS_DOMAIN_ID', value='0')
# === 地图服务器 ===
map_server = Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[{
'yaml_filename': map_yaml,
'use_sim_time': use_sim,
}],
)
# === AMCL 定位 ===
amcl = Node(
package='nav2_amcl',
executable='amcl',
name='amcl',
output='screen',
parameters=[{
'use_sim_time': use_sim,
'min_particles': 200,
'max_particles': 2000,
'update_min_d': 0.2,
'update_min_a': 0.5,
'alpha1': 0.2,
'alpha2': 0.2,
'alpha3': 0.2,
'alpha4': 0.2,
'alpha5': 0.1,
'kld_err': 0.05,
'kld_z': 0.99,
'odom_model_type': 'diff',
'odom_frame_id': 'odom',
'base_frame_id': 'base_footprint',
'global_frame_id': 'map',
'scan_topic': '/scan',
'tf_broadcast': True,
}],
)
# === 生命周期管理器 ===
lifecycle_manager = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'use_sim_time': use_sim,
'autostart': autostart,
'node_names': [
'map_server', 'amcl',
'controller_server', 'planner_server',
'recoveries_server', 'bt_navigator',
],
}],
)
# === Nav2 核心节点（composeable） ===
nav2_composable = LoadComposableNodes(
target_container='nav2_container',
composable_node_descriptions=[
ComposableNode(
package='nav2_controller',
plugin='nav2_controller::ControllerServer',
name='controller_server',
parameters=[params_file, {'use_sim_time': use_sim}],
),
ComposableNode(
package='nav2_planner',
plugin='nav2_planner::PlannerServer',
name='planner_server',
parameters=[params_file, {'use_sim_time': use_sim}],
),
ComposableNode(
package='nav2_recoveries',
plugin='nav2_recoveries::RecoveryServer',
name='recoveries_server',
parameters=[params_file, {'use_sim_time': use_sim}],
),
ComposableNode(
package='nav2_bt_navigator',
plugin='nav2_bt_navigator::BtNavigator',
name='bt_navigator',
parameters=[params_file, {'use_sim_time': use_sim}],
),
ComposableNode(
package='nav2_waypoint_follower',
plugin='nav2_waypoint_follower::WaypointFollower',
name='waypoint_follower',
parameters=[params_file, {'use_sim_time': use_sim}],
),
],
)
# === Composable 节点容器 ===
container = Node(
package='rclcpp_components',
executable='component_container_mt',
name='nav2_container',
output='screen',
parameters=[{'use_sim_time': use_sim}],
)
return LaunchDescription([
# 参数
declare_use_sim,
declare_map_yaml,
declare_params_file,
declare_bt_xml,
declare_autostart,
set_sim_time,
# 节点
container,
nav2_composable,
map_server,
amcl,
lifecycle_manager,
])跑 launch 前，先认两个关键角色。map_server 把建好的地图读进系统；amcl 是定位器——它用一堆「粒子」猜机器人可能在哪（粒子越多越准也越吃 CPU），靠激光和里程计不断修正，输出 map→odom 的变换。没有 AMCL，机器人就不知道「我在地图的哪个点」，点了目标也白点。
下面这个 launch 用生命周期管理器
为什么要有 lifecycle（生命周期）这一层？Nav2 的节点启动有顺序依赖：地图服务器得先加载完地图，amcl 才能开始定位；定位没好，规划器算的路径是错的。lifecycle 用「未配置→非活跃→活跃」三态，让 lifecycle_manager 按 node_names 顺序逐个激活，避免「半吊子启动」。真机调试时如果某个节点卡在「非活跃」，就是它前一级没就绪，看它日志比瞎调参数有用。
autostart 这个开关很实用：设 true，lifecycle_manager 拉起后自动把所有节点从「未配置」激活到「活跃」，你不用手动逐个 configure / activate；设 false，则停在计算资源、等你远程激活，适合「先起系统、人到场再开导航」的真机流程。调试时建议 false，能单独激活某个节点看它日志，定位更快。
AMCL 最该懂的两个数：min_particles / max_particles（粒子数上下限）和 alpha*（里程计噪声模型）。粒子少 → 定位快但不准（容易跟丢）；粒子多 → 准但吃 CPU。室内小场 200/2000 够用，大 warehouse 加到 500/5000。alpha* 越大表示「越不信任里程计、越信激光」，轮子打滑的机器人在瓷砖上就把这几个调大。定位抖 → 加粒子；定位慢慢飘走 → 调大 alpha*。
为什么 RViz 里「设初始位姿」那么关键？AMCL 启动时粒子默认堆在 (0,0) 或上次位姿附近，你设的 2D Pose Estimate 等于告诉它「我大概在这」，粒子瞬间重撒到那一片。初值差太远（差了半张地图），粒子要花很久才收敛，期间定位是飘的，导航自然歪。所以「先设准初值，再点目标」是铁律；自动化场景由上游（如二维码定位）自动完成，原理一样。
把 map_server、amcl 和 Nav2 各节点串起来一次拉起。注意 Nav2 核心节点走的是 Composable Nodes（component_container_mt）——把多个节点塞进一个进程，省掉进程间通信开销，真机上帧率更稳。你读代码时看到 LoadComposableNodes 就知道这是「合体节点」。
5.1 一键启动完整系统
launch/nav_bringup.launch.py：
这是「总开关」launch，用 mode 一个参数切换建图 / 导航两种模式，再按需 include 对应子 launch。真机部署时你大概率只跑这个文件，传 mode:=nav 就进导航。它用 LaunchConfigurationEquals 按模式条件加载，同一份文件兼顾两种场景，不用维护两份。
#!/usr/bin/env python3
"""一键启动：仿真 + SLAM/导航"""
import os
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
)
from launch.conditions import IfCondition, LaunchConfigurationEquals
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
pkg_nav2 = get_package_share_directory('my_robot_nav2')
pkg_gazebo = get_package_share_directory('my_robot_gazebo')
mode = LaunchConfiguration('mode')
use_rviz = LaunchConfiguration('use_rviz')
use_sim = LaunchConfiguration('use_sim')
declare_mode = DeclareLaunchArgument(
'mode', default_value='slam',
choices=['slam', 'nav'],
description='slam=建图模式, nav=导航模式')
declare_use_rviz = DeclareLaunchArgument(
'use_rviz', default_value='true')
declare_use_sim = DeclareLaunchArgument(
'use_sim', default_value='true')
# Gazebo 仿真环境
sim_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo, 'launch', 'empty_world.launch.py')),
condition=IfCondition(use_sim),
)
# SLAM 模式
slam_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_nav2, 'launch', 'slam.launch.py')),
condition=LaunchConfigurationEquals('mode', 'slam'),
)
# 导航模式
nav_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_nav2, 'launch', 'navigation.launch.py')),
condition=LaunchConfigurationEquals('mode', 'nav'),
)
return LaunchDescription([
declare_mode, declare_use_rviz, declare_use_sim,
LogInfo(msg='=== 巡检机器人导航系统 ==='),
LogInfo(msg='模式: %s' % mode),
sim_launch,
slam_launch,
nav_launch,
])系统跑起来后怎么用？这一章给一套可照抄的操作流程。先说 RViz 里你该盯哪两块：
- 建图时盯
Map图层——地图边缘清晰、没有重影就对了；再开LaserScan看激光是不是贴着真实墙。 - 导航时盯
Path（全局路径，蓝线）、Costmap（障碍红 / 膨胀黄）、TF（坐标系树不断）。这三块正常，机器人基本就能动。
一个省时间的小技巧：RViz 配置（图层、视角、订阅的话题）可以存成 .rviz 文件，下次直接 rviz2 -d 你的配置.rviz 一键还原，不用每次手动点。本文 launch 里已经传了 slam.rviz，导航版你也可以存一份 nav.rviz。团队协同时把这份文件提交进仓库，所有人视角一致，排错时「你看到的和我看到的一样」能省一堆沟通。
第一次点目标容易犯的错：没先设初始位姿就点。RViz 里一定要先点 2D Pose Estimate，在地图上戳一下机器人真实位置和朝向，让 AMCL 的种子粒子落在正确地方；然后再点 2D Goal Pose。如果设完位姿机器人「啪」地跳一下，说明初始位姿和 AMCL 估的不符，重设一次。目标点也别设在墙里或离墙太近，否则规划器算不出可行路径，行为树会直接进脱困。
第一次成功的长什么样，心里要有数：点目标后，RViz 里蓝线（全局路径）立刻出现并从机器人连到目标；机器人平稳起步、沿蓝线走、遇障碍自然绕；到点后停下不抖；状态从「ACTIVE」变「SUCCEEDED」。只要这四步顺，后面调参只是微调。如果路径出现了但机器人迟迟不动，回到「速度发出来了吗」那一问，八成是 controller 参数卡死。
下面每条命令都标了「终端几、干什么」，照顺序敲。
六、操作指南
6.1 建图流程
# 1. 启动 SLAM 模式
ros2 launch my_robot_nav2 nav_bringup.launch.py mode:=slam
# 2. 在 RViz 中设置初始位姿
# - 点击 "2D Pose Estimate"
# - 在地图上点击机器人的大致位置和朝向
# 3. 遥控机器人扫描环境
ros2 run teleop_twist_keyboard teleop_twist_keyboard
# 4. 保存地图
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap \
"{name: {data: 'warehouse_map'}}"6.2 导航流程
# 1. 启动导航模式
ros2 launch my_robot_nav2 nav_bringup.launch.py mode:=nav \
map:=$(ros2 pkg prefix my_robot_nav2)/share/my_robot_nav2/maps/my_map.yaml
# 2. 在 RViz 中设置初始位姿（与地图对齐）
# 3. 发送导航目标
# - RViz: 点击 "2D Goal Pose" → 在地图上点击目标位置
# - 或命令行：
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
"{pose: {header: {frame_id: 'map'},
pose: {position: {x: 3.0, y: 2.0, z: 0.0},
orientation: {w: 1.0}}}}"多点巡航适合「巡检 / 送货」这类按固定路线跑的场景，本质是把多个 NavigateToPose 串起来顺序执行。两个注意点：航点之间要有合理间隔，别两个点在墙两侧导致规划失败；最后一个点建议回到起点附近，方便循环。断在某个点不走了，先单独 ros2 action send_goal /navigate_to_pose 测那一个点，大概率是该点周围代价地图有问题。
再多一句：航点多了建议写成 yaml 文件用 nav2_msgs 的 WaypointFollower 加载，而不是手敲一长串 goal。文件里每个点带 frame_id（必须是 map）和位姿，提交进仓库就能复现同一条巡检路线。调试时先拿两个点验证链路通，再批量加，别一上来塞二十个点然后不知道卡哪个。
6.3 多点巡航
# 通过 Waypoint Follower 发送多点巡航任务
ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints \
"{waypoints: [
{header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 0.0}, orientation: {w: 1.0}}},
{header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 3.0}, orientation: {w: 1.0}}},
{header: {frame_id: 'map'}, pose: {position: {x: 0.0, y: 3.0}, orientation: {w: 1.0}}},
{header: {frame_id: 'map'}, pose: {position: {x: 0.0, y: 0.0}, orientation: {w: 1.0}}}
]}"前面给的是「能跑」的默认参数。真机一上，八成要调——机器人要么在窄道里来回抖（振荡），要么明明能过却绕大远路，要么动不动就触发脱困。
调参的核心心法是一次只动一个数，看现象再决定下一个，别一口气改五个。下面这张表把「最常拧的数 ↔ 对应症状」列出来，卡住了对号入座；7.1 再给三对典型问题的现成改法。
七、代价地图调参指南
| 参数 | 默认值 | 作用 | 调参建议 |
|---|---|---|---|
resolution |
0.05 | 地图精度 | 室内 0.03-0.05，室外 0.1-0.2 |
inflation_radius |
0.55 | 膨胀半径 | 机器人半径 + 0.1-0.15m 安全余量 |
cost_scaling_factor |
3.0 | 膨胀衰减 | 越小膨胀越"硬"（不容易穿过去） |
obstacle_range |
5.0 | 障碍检测距离 | 室内 3-5m，开阔区域可增大 |
raytrace_range |
8.0 | 清除障碍距离 | 应大于 obstacle_range |
max_vel_x |
0.5 | 最大前进速度 | 室内 0.3-0.5，走廊 0.8-1.0 |
sim_time |
1.7 | 轨迹预测时间 | 大空间增大(2.0+)，小空间减小(1.0) |
xy_goal_tolerance |
0.25 | 到达容差 | 精确定位 0.1，粗略 0.5 |
三对高频问题的「现象→根因→改法」。记住根因比背改法重要：振荡多半是膨胀太狠或速度太快导致控制器在窄道里左右横跳；绕远路是规划器太保守或代价衰减太软；频繁恢复是目标容差太小、机器人觉得自己「没到」。
7.1 常见问题与调参
光给表不够直观，演示一次完整调参。假设机器人进窄走廊后左右横跳、走不动：
- 现象：RViz 里局部路径在走廊中线附近剧烈左右摆，
cmd_vel角速度正负跳。 - 第一反应：膨胀太狠，机器人觉得两边都是墙、没路可走，于是振荡。把 local 的
inflation_radius从 0.55 降到 0.45，cost_scaling_factor从 3.0 提到 5.0（膨胀更「软」、更愿意靠近墙）。 - 没好透：速度上限太高，DWB 在窄道里还敢转大弯。把
max_vel_x从 0.5 降到 0.3，max_vel_theta从 1.5 降到 1.0。 - 仍偶尔卡：目标容差太小，机器人觉得自己「没精确到点」反复微调。
xy_goal_tolerance放到 0.35。 - 验证：同一段走廊重跑，路径变直、不再摆，到达目标不再原地转。
整个过程「一次动一个数、跑一遍看现象」，十分钟定位，比同时改五个数然后猜是哪个生效靠谱得多。
一个比参数更阴的坑：话题名 / QoS 不匹配。Nav2 内部话题默认「可靠 + 稳定」QoS，如果你的激光驱动用了「最佳努力」且历史不同，代价地图可能收不到数据，表现就是「地图全绿、机器人不动」。同理 scan / odom / cmd_vel 名字必须全局一致——launch 里用 remappings 对齐最稳。所以「改了不生效 / 收不到」先 ros2 topic list + ros2 topic echo 确认数据真在流，再谈调参。
问题：机器人在狭窄通道中不停振荡
# 增大膨胀半径 + 降低最大速度
inflation_radius: 0.65 # 增大
max_vel_x: 0.3 # 降低
max_vel_theta: 1.0 # 降低旋转速度「频繁触发恢复」和前面「振荡」容易混，但根因不同：振荡是控制器在窄道左右横跳（调速度 / 膨胀）；频繁恢复是机器人觉得自己「没在前进」（进度检查器超时），于是反复 Spin / BackUp。先判断属于哪种——看 RViz 里机器人是在抖还是真在原地打转。后者把 movement_time_allowance 调大（允许更久不出 progress），或把目标容差放宽；前者去调控制器。误诊就会南辕北辙。
问题：机器人频繁触发恢复行为
# 放宽目标容差 + 降低卡死检测灵敏度
xy_goal_tolerance: 0.35
yaw_goal_tolerance: 0.35
# progress_checker 中增加等待时间
movement_time_allowance: 15.0「绕远路」和「振荡」是两种不同病：绕远路是规划器太保守（把大片区域当成障碍）或代价衰减太软（机器人不敢靠近任何东西）；振荡是控制器在窄道里左右横跳。验收标准记一条：能走直线就不绕，能一次到就不抖。调完跑同一段已知路线三遍，三次表现一致才算稳，别被「刚好一次成了」骗了。
问题：路径规划绕远路
# 切换为 A*（比 Dijkstra 快但路径可能不够平滑）
GridBased:
use_astar: true
# 或降低代价衰减（让机器人更愿意靠近障碍物）
cost_scaling_factor: 5.0仿真里跑通了，最后一步是搬到真机。真机和仿真最大的区别是：传感器是真的、物理是真的、bug 也是真的。
真机比仿真多出三座大山，提前知道能省一天：
- 坐标系对不齐：
base_footprint/base_link/odom/map四层变换少一层，机器人就「地图在地图上、机器人在机器上」。用ros2 run tf2_tools view_frames看变换树，断在哪一层一眼看出。 - 传感器时间不同步：激光和里程计时间对不上，AMCL 会把你定位到墙里。真机务必统一时间源，且各驱动
use_sim_time一致（通常都设 false）。 - 里程计太漂：轮子里程在打滑地面（瓷砖、地毯）误差极大，AMCL 粒子会散开。解决是加 IMU +
robot_localization做融合，或上视觉 / 激光里程计。
移植顺序建议：先 use_sim:=false 把仿真底盘换成真驱动
真机传感器三类最常见：激光雷达（主力，给 2D 障碍）、深度相机（能给 3D 体素，但价格高、怕强光）、IMU（补姿态，防打滑漂移）。室内平整地面用单激光 + IMU 足够；室外或不平整地面必须加 IMU 甚至视觉里程计。换传感器只动两处：URDF 里的传感器插件（话题名 / 参数）和代价地图的 observation_sources（告诉它数据从哪来）。
，确认 /cmd_vel 能让人动；再调 AMCL 让定位稳；最后才动代价地图参数。别一上来全改。
八、实车部署要点
8.1 硬件适配清单
| 真机部件 | 需要修改的配置 |
|---|---|
| LiDAR 型号不同 | lidar.xacro 中 sensor 参数 + scan_topic |
| 里程计源不同 | 替换 Gazebo 插件为实际驱动（如 ros2_control） |
| 底盘不同 | diff_drive.xacro 中 wheel_separation / wheel_radius |
| IMU | robot_localization 包融合里程计 + IMU |
真机第一个坑几乎都是里程计漂，而里程计准不准首先看你填的轮距对不对。wheel_separation（左右轮中心距）和 wheel_radius 必须量实物、别抄示例：用尺量两轮触地点间距、量轮直径，填进 2.1 / 2.2。
填错 1cm，走 10m 就能偏半米，AMCL 再强也救不回。验证方法：让机器人直行 2m，用尺量实际走了多远、偏了多少；差太多就回炉改轮距。这一步不做，后面所有「定位飘」的锅都会错扣在 AMCL 头上。
8.2 仿真到真机移植步骤
# 1. 替换 Gazebo 插件为 ros2_control
# （参考上一篇 "ROS 2 机器人仿真实战"）
# 2. 启动真实传感器驱动
# 如 RPLidar A3:
ros2 launch sllidar_ros2 sllidar_a3_launch.py
# 3. 使用 robot_localization 融合里程计+IMU
ros2 launch robot_localization ekf.launch.py
# 4. 导航时关闭仿真时间
ros2 launch my_robot_nav2 navigation.launch.py use_sim:=false真机算力紧时，三处最吃资源：
- 局部代价地图更新频率：降到 3Hz 通常无感，但 CPU 省一截。改
local_costmap.yaml的update_frequency。 - DWB 的轨迹采样数：
vx_samples/vtheta_samples越大越聪明也越慢，室内降到 15 / 30 够用。 - Composable Nodes：本文已用
component_container_mt把节点合进一个进程，减少通信开销；别拆回多个独立 Node，否则单板电脑容易掉帧。
调性能的原则是「先降频率、再降采样、最后动算法」，别一上来换控制器。NAV2_USE_SIMD 这类 SIMD 加速开关，支持的平台打开能再快些。
8.3 性能优化
# GPU 加速代价地图计算（如果支持）
export NAV2_USE_SIMD=true
# 降低局部代价地图更新频率（节省 CPU）
# local_costmap.yaml: update_frequency: 3.0
# 使用 Composable Nodes 减少进程间通信开销
# （已在本文 launch 文件中使用 component_container_mt）真机上线前，三条保命习惯：
- 先低速、再加急停：
max_vel_x真机首跑给 0.2，人在旁随时按急停（键盘或物理按钮），确认不撞再提速。 - 先在空场跑：别一上来进满屋家具，空地先验证「能走直线、能到点、能避静态障碍」三件事。
- 膨胀宁大勿小：第一次真机把
inflation_radius调大些，宁可绕远也别贴墙刮花，稳了再收紧。
导航出事几乎都发生在「提速太快 + 环境太杂 + 没人盯」三者叠加，避开它就避开九成事故。
急停怎么接才靠谱：真机务必在硬件层留一个物理急停（断开电机使能），别只靠软件停 cmd_vel——软件卡死时它不保证停。软件层再加一道：导航 launch 里监听一个急停话题，收到就停发速度。两层都有的机器人才算「能让人放心站旁边」。第一次真机跑，人在 1 米内、手搭急停，是铁律。
三个真实翻车现场，对应前面知识点：
- 案例 A：机器人到点后疯狂原地转。根因
xy_goal_tolerance太小 + 地面打滑导致永远「差一点没到」。改大到 0.35 解决。 - 案例 B：RViz 里路径是直的，机器人却贴墙走。根因 local
inflation_radius太大，控制器不敢走中线。调小解决。 - 案例 C：建图清楚，导航却总撞同一面墙。根因建图后改了雷达高度没重建设图，激光和地图差半米。重建设图解决。
共同点：都是「一个参数 / 一份地图」的锅，没有一个是算法坏了。按本文链路和速查卡对号入座，半小时都能定位。
排错锦囊：导航不动先查这五处
导航「点了目标不动 / 乱走 / 飘」，按这个顺序查，九成能定位：
- 目标真发出去了吗？ 看 RViz 有没有蓝色箭头，或
ros2 topic echo /goal_pose。没发出去，先设好初始位姿（2D Pose Estimate）——AMCL 不知道你在哪，目标会被当无效。 - 全局路径算出来了吗？
ros2 topic echo /plan看有没有路径点。空的，多半是代价地图没障碍信息（激光没进来）或起点被当成障碍。ros2 topic echo /scan确认激光有数。 - 速度指令发出来了吗？
ros2 topic echo /cmd_vel看线 / 角速度。有路径没速度，问题在 controller（DWB 参数卡死，如max_vel_x太小或sim_time太长算不出轨迹）。 - 代价地图有障碍吗？ RViz 勾 Costmap 图层，墙是不是标红了。全绿说明激光没进代价地图（话题名 /
observation_sources不对）。 - 坐标变换断了吗？
ros2 run tf2_tools view_frames看map→odom→base_footprint是否连续。断一层，全局路径和真实位置就对不上，表现为「地图对但走歪」。
这五步顺着「目标→路径→速度→地图→坐标」往下捅，和第五章那条链路一一对应。
走到这，你已经能让一台差速机器人在仿真和真机里自主导航。下一步建议：把行为树换成官方 Nav2 默认更复杂版本，体验 docking / 巡线等高级行为；或接深度相机做动态行人避障；再进一步就是多机协同（系列后续会讲）。
把本文的「五步主线」刻进脑子——地图 → 定位 → 规划 → 控制 → 脱困——后面所有 Nav2 玩法都是在这上面做加法，而不是推倒重来。
学完自测：能不翻文档回答下面问题，就算真懂了——
- 代价地图三层分别来自哪、干嘛用？
- planner 和 controller 各管什么，差在哪？
- 行为树里 RecoveryNode / ReactiveFallback 分别在什么时机触发？
- 机器人不动，五步排查顺序是什么？
- 轮距填错会导致什么现象？
答不上哪个，回对应小节再看一遍，比直接上手调参稳。
上线前做一遍回归，确认没被你某次改动搞坏：
- 重新建图，看地图清晰、无重影。
- 设初值 → 点目标，机器人能到点且不抖。
- 故意挡一下激光，看它绕行而非硬撞。
- 多点巡航跑完不卡。
- 关掉再启动，lifecycle 能自动拉起全部节点。
五条全绿，这版配置就能提交。任何一条翻车，回到对应小节，别带着「差不多」的配置上真机。
速查卡：把全文收成一张表
下面这张表不是结语，是速查——卡在哪个环节，直接翻对应参数去拧，不用重读全文。
补一张「一页纸参数速查」，卡住时直接对：
- 定位飘 → AMCL 粒子数 /
alpha*，或回 2.2 核轮距。 - 撞墙 →
inflation_radius调大 /cost_scaling_factor调小。 - 窄道振荡 →
inflation_radius调小、max_vel_x降、xy_goal_tolerance放宽。 - 绕远路 →
use_astar: true、cost_scaling_factor调大。 - 不动 / 卡 → 五步排查（目标→路径→速度→地图→坐标）。
- 掉帧 → 降
update_frequency/*_samples，开 Composable。
把这张和第五章那条链路一起看，你已经是能独立救场的人了。
顺手存一组最常用命令，调试时直接抄：
- 看 TF：
ros2 run tf2_tools view_frames - 看代价地图：
ros2 topic echo /local_costmap/costmap - 强制重定位：
ros2 topic pub /initialpose geometry_msgs/PoseWithCovarianceStamped ... - 单独测规划：
ros2 action send_goal /compute_path_to_pose ... - 看节点是否都起：
ros2 node list
把这几条设成 shell 别名，排错速度翻倍。
| 环节 | 工具/技术 | 关键配置 |
|---|---|---|
| 建图 | SLAM Toolbox (异步模式) | mode: mapping, loop closing |
| 定位 | AMCL (蒙特卡洛) | min/max_particles, KLD采样 |
| 规划 | NavfnPlanner (A*/Dijkstra) | use_astar, tolerance |
| 控制 | DWB LocalPlanner | max_vel_x, sim_time, critics |
| 避障 | 代价地图 (膨胀层) | inflation_radius, cost_scaling_factor |
| 行为树 | BehaviorTree.CPP | 自定义恢复策略 |
| 多点巡航 | WaypointFollower | FollowWaypoints action |
下一篇我们将进入 ROS 2 机械臂抓取与视觉感知实战，结合深度相机和 MoveIt2 实现完整的 Pick & Place 工作流。
本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。
