SLAM 实践（三）：理解Nav2的Navigation Concepts与完整导航链路

上一篇文章把二维激光扫描、里程计约束、回环检测和位姿图优化连接成了一张 ROS 2 地图。这一篇继续追问：地图建好以后，机器人怎样知道自己在哪里、规划哪条路，并最终把路径变成底盘速度？

写在前面

上一篇文章使用 ROS 2、TurtleBot3、Gazebo 和 slam_toolbox 跑通了二维激光 SLAM 流程，重点观察了三件事：

1. 激光雷达数据怎样变成二维占据栅格地图；
2. 里程计误差为什么会让地图逐渐变形；
3. 回环约束加入以后，位姿图优化为什么会让地图“跳一下”。

完成这些工作后，我们得到了 /map，也得到了 SLAM 发布的 map → odom 坐标变换。但这还不能让机器人独立完成导航。

地图只回答了“环境是什么样的”。机器人还需要继续回答：

• 我现在在哪里？
• 目标点在哪里？
• 哪些区域可以通行？
• 从当前位置到目标位置应该走哪条路？
• 路上突然出现障碍物怎么办？
• 规划失败或机器人被卡住时，系统应该怎样恢复？

这些问题正是 Nav2 导航系统要解决的。

本文以 Nav2 官方文档中的 Navigation Concepts 章节为主线，不急着修改参数，也不从头运行一遍仿真，而是先把 SLAM、状态估计、TF、代价地图、路径规划器、控制器、行为树和 ROS 2 通信机制放进同一条数据链中。

需要先说明：Navigation Concepts 是 Nav2 官方文档中的章节，不是专门介绍 SLAM 算法的文档。 SLAM 是完整自主导航系统中的一个组成部分。本文把它放进 SLAM 系列，是因为只有理解 SLAM 的输出怎样被下游模块使用，我们才能真正理解一张地图在机器人系统中的价值。

---

1. 本文要解决什么问题

上一篇文章的主线可以概括为：

激光扫描 + 里程计
        ↓
扫描匹配与相对位姿约束
        ↓
回环检测与位姿图优化
        ↓
二维占据栅格地图 /map
        ↓
全局坐标修正 map → odom

本文从这里继续向下：

/map + 机器人位姿 + 实时传感器
                 ↓
              Costmap
                 ↓
              Planner
                 ↓
             全局路径 Path
                 ↓
             Controller
                 ↓
              /cmd_vel
                 ↓
                底盘

完成本文后，我们应该能够：

• 解释 SLAM 与 Nav2 的职责边界；
• 说清楚 map → odom → base_link → sensor 的来源；
• 理解为什么导航同时需要 map 和 odom；
• 区分 Occupancy Grid、Global Costmap 与 Local Costmap；
• 区分 Planner、Controller、Smoother 与 Behavior Server；
• 理解 Action、Lifecycle Node 与 Behavior Tree 在 Nav2 中的作用；
• 从目标点出发，完整描述一条导航任务怎样变成底盘速度。

---

2. SLAM 与 Nav2 的职责边界

可以先用两个问题区分它们：

SLAM 负责回答：我在哪里，环境是什么样的？

Nav2 负责回答：我要怎样安全地到达目标？

2.1 SLAM 通常提供什么

以 slam_toolbox 为例，二维激光 SLAM 通常向系统提供：

• /map：二维占据栅格地图；
• map → odom：从全局地图坐标系到局部里程计坐标系的修正；
• 机器人在地图中的全局位姿；
• 位姿图、回环和地图保存等相关接口。

2.2 Nav2 继续完成什么

Nav2 接收地图、机器人位姿、实时传感器和导航目标，继续完成：

• 构建全局与局部代价地图；
• 计算从当前位置到目标位置的路径；
• 跟踪路径并输出速度指令；
• 根据实时障碍物调整运动；
• 在规划失败、路径阻塞或机器人卡住时执行恢复行为；
• 通过行为树组织完整导航流程。

两者不是互相替代的关系，而是上下游关系：

传感器与里程计
      ↓
     SLAM
      ↓
地图与全局位姿
      ↓
     Nav2
      ↓
路径与速度指令

这也解释了为什么“地图已经建好”和“机器人已经能够自主导航”不是一回事。

---

3. ROS 2 Action：导航为什么不能只用 Topic 或 Service

Nav2 建立在 ROS 2 之上，因此先要理解它怎样接收一个导航任务。

ROS 2 中常见的三类通信方式是：

通信方式	适合的任务	典型例子
Topic	持续传输数据	/scan、/odom、/cmd_vel
Service	快速完成一次请求并返回	清除代价地图、保存地图
Action	持续时间较长、需要反馈且允许取消	导航到目标点、跟随一组路径点

导航可能持续几十秒甚至几分钟。执行过程中，调用者通常还需要知道：

• 机器人是否仍在移动；
• 距离目标还有多远；
• 当前任务是否失败；
• 是否需要取消或替换目标。

所以 Nav2 使用 Action 接收导航任务。一个 Action 可以简单理解为：

Goal：我要去哪里
Feedback：任务执行到哪里了
Result：最终成功还是失败

当用户在 RViz 中点击目标点时，通常会触发 NavigateToPose Action。上层应用不需要直接命令 Planner 或 Controller，而是把目标交给 BT Navigator，由它组织后面的流程。

可以用下面的命令查看当前系统中的 Action：

ros2 action list

查看导航 Action 的类型与客户端、服务器数量：

ros2 action info /navigate_to_pose

不同系统可能使用命名空间，因此实际 Action 名称应以 ros2 action list 的输出为准。

---

4. Lifecycle Node：为什么节点启动了却不一定在工作

Nav2 中的大部分核心服务器使用 ROS 2 Lifecycle Node，也叫 Managed Node。

普通节点启动以后通常会直接开始工作，而 Lifecycle Node 会经历明确的状态变化：

Unconfigured
      ↓ configure
   Inactive
      ↓ activate
    Active
      ↓ deactivate
   Inactive
      ↓ cleanup / shutdown
Unconfigured / Finalized

几个主要状态的含义是：

状态	含义
Unconfigured	节点已经创建，但尚未完成参数和资源配置
Inactive	配置已经完成，但节点没有正式处理导航任务
Active	节点开始订阅数据、响应请求并产生输出
Finalized	节点已经结束，等待销毁或用于故障检查

这种设计让导航系统能够按照确定的顺序启动。例如，Planner Server 不应该在代价地图和相关插件尚未配置完成时就开始规划。

Nav2 使用 Lifecycle Manager 协调多个服务器的状态，并通过 Bond 连接监控它们是否仍然存活。如果一个关键服务器异常退出，管理器就能感知故障，避免系统继续在不完整的状态下运行。

因此，排查 Nav2 时要记住：

ros2 node list 中能够看到节点，不代表它已经进入 active 状态。

可以查看系统中的生命周期节点：

ros2 lifecycle nodes

查看某个节点的状态：

ros2 lifecycle get /controller_server

节点名称和命名空间可能因启动文件而不同，应根据实际输出修改命令。

---

5. Behavior Tree：谁来组织完整导航流程

Planner 只负责计算路径，Controller 只负责跟踪路径。如果路径计算失败、前方出现新障碍物，或者机器人原地卡住，系统还需要决定下一步做什么。

Nav2 使用 Behavior Tree，也就是行为树，组织这些高层逻辑。

一棵简化后的导航行为树可以理解为：

接收目标
   ↓
计算路径
   ↓
路径是否有效？
   ├── 是：跟随路径
   │        ↓
   │     是否到达目标？
   │        ├── 是：返回成功
   │        └── 否：继续控制
   │
   └── 否：执行恢复流程
            ↓
       清除代价地图
            ↓
         重新规划

行为树中的节点被 tick 后，通常返回三种状态：

• SUCCESS：任务已经成功；
• FAILURE：任务失败；
• RUNNING：任务仍在执行。

在 Nav2 中，BT Navigator 解析行为树 XML，并通过行为树节点调用 Planner、Controller、Smoother 和 Behavior 等服务器提供的 Action。

可以把行为树理解为导航系统的“流程编排者”：

行为树不负责计算最短路径，也不直接控制电机；它负责决定什么时候规划、什么时候控制、失败以后尝试什么。

---

6. Nav2 的核心服务器分别负责什么

Nav2 将导航能力拆分成多个服务器，每个服务器可以加载不同算法插件。

模块	主要职责	典型输入	典型输出
BT Navigator	组织完整导航任务	目标位姿、行为树	对其他服务器的 Action 调用
Planner Server	计算全局路径	当前位姿、目标位姿、Global Costmap	nav_msgs/msg/Path
Controller Server	跟踪路径并避开局部障碍物	全局路径、当前位姿、Local Costmap	/cmd_vel
Smoother Server	改善路径平滑度和可执行性	原始路径、环境信息	平滑后的路径
Behavior Server	执行旋转、后退、等待等行为	行为目标、局部环境	行为执行结果
Route Server	在预定义导航图上计算路线	起点、终点、导航图	图上的路线

6.1 Planner：决定“走哪条路”

Planner Server 的基本任务是：根据当前位置、目标位置和全局环境表示，计算一条有效路径。

当前位姿 + 目标位姿 + Global Costmap
                 ↓
              Planner
                 ↓
               Path

不同规划器可以追求不同目标，例如：

• 路径更短；
• 距离障碍物更远；
• 符合差速、全向或阿克曼底盘的运动学约束；
• 沿仓库通道或预定义路线移动；
• 覆盖整个可通行区域。

6.2 Controller：决定“这一刻怎样运动”

Controller 在 ROS 1 中常被称为 Local Planner。它接收全局路径，并结合机器人当前状态与局部环境，持续计算底盘控制指令。

Path + 当前位姿 + Local Costmap
              ↓
          Controller
              ↓
           /cmd_vel

Planner 输出的是“应该经过哪些位置”，Controller 输出的则是“当前应该以多快的线速度和角速度运动”。

因此可以这样记忆：

Planner 决定走哪条路，Controller 决定此刻怎样走。

6.3 Smoother：让路径更适合执行

搜索算法生成的路径不一定足够平滑，可能存在锯齿、突然转向或离障碍物过近等问题。Smoother 接收一条路径，对它进行进一步优化，再把结果交给 Controller。

Smoother 并不是所有系统都必须单独使用。有些 Planner 自带平滑能力，有些应用则希望把规划与平滑解耦，以便组合不同插件。

6.4 Behavior：在异常情况下恢复

机器人可能遇到：

• 新障碍物挡住路径；
• 定位或传感器短暂异常；
• Controller 无法产生有效速度；
• 机器人陷入局部死锁；
• 目标位置本身不可达。

Behavior Server 可以提供原地旋转、后退、等待等行为。清除代价地图、重新规划以及多次重试等策略，则通常由行为树组织。

6.5 Route：沿规定道路移动

普通 Planner 在自由空间或代价地图中搜索路径。Route Server 则使用预先定义的节点和有向边组成导航图，更适合：

• 仓库固定通道；
• 园区道路；
• 物流运输路线；
• Teach-and-Repeat；
• 只允许机器人在指定区域内通行的场景。

对于刚开始理解 Nav2 的读者，先掌握 Planner 与 Controller 即可，Route Server 可以在后续项目需要时再深入。

---

7. 状态估计：理解 map → odom → base_link

对于 SLAM 和 Nav2，最重要的坐标链通常是：

map → odom → base_link → sensor_frames

以 TurtleBot3 为例，末端可能是：

map → odom → base_footprint → base_link → base_scan

不同机器人可能使用 base_laser、laser_frame 或 lidar_link，具体名称应以 URDF 和 /scan.header.frame_id 为准。

7.1 map → odom：全局修正

map → odom 通常由全局定位系统提供，例如：

• slam_toolbox；
• AMCL；
• 视觉或激光定位系统；
• GPS 与其他全局定位方案。

它的任务是把存在漂移的局部里程计坐标系放回全局地图中。

在上一篇文章里，当机器人回到起点并建立回环约束时，位姿图优化会重新调整历史轨迹。对导航系统来说，这种全局修正通常体现在 map → odom 的变化上。

7.2 odom → base_link：局部连续运动

odom → base_link 由里程计系统提供。里程计可以来自：

• 轮式编码器；
• IMU；
• Visual Odometry；
• LiDAR Odometry；
• 多种传感器经过 robot_localization 融合后的结果。

odom 的核心特点是平滑、连续，适合短时间运动控制；它允许长期漂移，但不应该因为一次回环或全局定位修正而突然跳变。

7.3 base_link → sensor_frames：传感器外参

机器人底盘到激光雷达、相机和 IMU 的变换通常由 URDF 或静态 TF 发布器提供。例如：

base_link → base_scan
base_link → camera_link
base_link → imu_link

这些变换描述传感器安装在机器人什么位置、朝向哪里。外参错误会直接影响扫描匹配、障碍物位置和代价地图，因此它不是单纯的显示问题。

7.4 为什么同时需要 map 和 odom

map 与 odom 分别解决两种不同需求：

坐标系	优点	局限
map	具有全局一致性，适合定位和目标表达	回环或重新定位时可能发生修正
odom	平滑连续，适合局部控制和短时间运动估计	误差会随时间累积

机器人在地图中的位姿可以理解为两段变换的组合：

T_map_base = T_map_odom × T_odom_base

SLAM 调整 T_map_odom 以修正全局误差；里程计持续更新 T_odom_base 以描述平滑运动。这样既能保持局部控制连续，又能让机器人长期位于正确的地图位置。

可以检查两段关键变换：

ros2 run tf2_ros tf2_echo map odom
ros2 run tf2_ros tf2_echo odom base_footprint

如果机器人使用 base_link 而不是 base_footprint，应根据实际 TF 树修改命令。

---

8. SLAM、Localization 与 Odometry 不要混为一谈

三者都在估计机器人状态，但职责不同。

8.1 Mapping / SLAM

在未知环境中，机器人一边估计自己的位置，一边更新地图。以 slam_toolbox 为例，它接收激光扫描和里程计信息，维护位姿图并发布 /map 与 map → odom。

8.2 Localization

地图已经存在时，机器人只需要估计自己在地图中的位置，不再持续修改静态地图。Nav2 常见的二维定位方案是 AMCL；slam_toolbox 也提供基于序列化位姿图的定位模式。

虽然算法不同，但对 Nav2 来说，它们至少需要完成同一项关键工作：

提供 map → odom

8.3 Odometry

里程计根据轮子、IMU、视觉或激光等局部运动信息，提供平滑连续的：

odom → base_link

因此，不要指望 SLAM 替代底层里程计，也不要指望轮式里程计独自提供长期无漂移的全局位置。

可以把它们的分工记成：

Odometry：短时间内，我怎样连续地移动？
SLAM：未知环境中，我在哪里，地图是什么样的？
Localization：地图已知时，我在地图的什么位置？

---

9. 从 Occupancy Grid 到 Costmap

上一篇文章得到的 /map 通常是 nav_msgs/msg/OccupancyGrid，其中每个栅格表示未知、空闲或被占用。

但是 Nav2 不能只使用一张静态图片完成安全导航。真实环境中可能出现行人、纸箱、椅子或临时封闭区域。因此，Nav2 会把静态地图、实时传感器和安全策略组合成代价地图。

SLAM / 静态地图
      │
      ├── Static Layer
      │
LiDAR / Depth Camera
      │
      ├── Obstacle Layer / Voxel Layer
      │
机器人尺寸与安全距离
      │
      └── Inflation Layer
               │
               ▼
            Costmap

代价地图中的栅格不只是“能走”与“不能走”，还可以表达从低风险到高风险的不同代价值。规划器和控制器会利用这些代价，尽量选择更安全、更合理的路径。

9.1 Global Costmap

Global Costmap 通常覆盖较大的范围，用于全局路径规划。它可能包含：

• SLAM 或 Map Server 提供的静态地图；
• 障碍物信息；
• Inflation Layer；
• 禁行区、限速区等过滤信息。

9.2 Local Costmap

Local Costmap 通常以机器人为中心，在较小范围内滚动更新，主要用于：

• 处理实时传感器数据；
• 发现临时和动态障碍物；
• 为 Controller 提供局部环境；
• 支持实时避障和轨迹评估。

可以简单记忆：

Global Costmap 决定大方向怎样走，Local Costmap 决定眼前怎样安全地走。

9.3 Costmap Filter

除了障碍物以外，工业和服务机器人还需要遵守空间规则。Costmap Filter 可以借助过滤掩膜表达：

• 禁止进入区域；
• 限速区域；
• 推荐通道；
• 特定位置触发的行为变化。

这些规则不是 SLAM 地图本身的一部分，而是导航系统在地图之上增加的业务语义。

---

10. Robot Footprint：规划的是机器人，不是一个点

地图上的路径看起来可能没有穿过墙壁，但机器人具有真实的宽度和长度。Nav2 需要用 Footprint 描述机器人在地面上的占用形状。

常见表示方法有两种：

• 使用 robot_radius 把机器人近似成圆；
• 使用 footprint 多边形描述非圆形底盘。

规划器和控制器会结合 Footprint 判断机器人是否能够安全通过狭窄空间。Inflation Layer 则在障碍物周围生成逐渐变化的代价值，让规划结果主动与墙壁和障碍物保持距离。

所以：

SLAM 地图中的空白区域，不一定真的足够让机器人通过。

如果机器人带有机械臂、托盘或其他会改变外形的机构，还可以动态更新 Footprint，使碰撞检查反映机器人当前形态。

---

11. 一次 NavigateToPose 怎样变成 /cmd_vel

现在把前面的模块连接起来。假设用户在 RViz 中设置了一个目标位姿，一次完整导航大致会经历：

第一步：发送目标

RViz 或上层应用通过 NavigateToPose Action 发送目标位姿。目标通常表达在 map 坐标系中。

第二步：BT Navigator 接收任务

BT Navigator 加载行为树，根据树中的逻辑决定先计算路径、跟随路径，还是执行其他检查。

第三步：获得机器人当前位姿

系统通过 TF 查询：

map → odom → base_link

从而得到机器人在地图中的当前位置。

第四步：更新 Global Costmap

静态地图、障碍物、Footprint、Inflation Layer 和过滤规则被组合成全局环境表示。

第五步：Planner 计算路径

Planner Server 根据当前位姿、目标位姿和 Global Costmap 生成全局路径。

第六步：可选的路径平滑

如果行为树中配置了 Smoother，原始路径会被进一步处理，使它更平滑或更符合运动学约束。

第七步：Controller 跟踪路径

Controller Server 持续读取：

• 全局路径；
• 当前机器人位姿；
• Local Costmap；
• 速度与运动学限制。

然后输出 /cmd_vel，驱动底盘运动。

第八步：传感器持续更新局部环境

LiDAR、深度相机或其他传感器不断把新障碍物加入 Local Costmap。Controller 根据最新环境调整速度。

第九步：失败时执行恢复策略

如果路径不可达、控制失败或机器人被障碍物阻塞，行为树可以组织：

• 重新规划；
• 清除代价地图；
• 等待障碍物离开；
• 原地旋转；
• 向后移动；
• 多次尝试后返回失败。

第十步：返回 Action 结果

机器人到达目标后，Action 返回成功；如果所有恢复策略都无法解决问题，则返回失败原因。

整条链路可以画成：

导航目标 NavigateToPose
          │
          ▼
     BT Navigator
          │
          ├───────────────┐
          ▼               │
    Planner Server        │ 失败与恢复逻辑
          │               │
       Global Path        │
          │               │
          ▼               │
    Smoother Server       │
          │               │
     Smoothed Path        │
          │               │
          ▼               │
   Controller Server      │
          │               │
       /cmd_vel           │
          │               │
          ▼               │
        机器人 ────────────┘

---

12. 用少量命令观察这条链路

本文以概念为主，但可以使用一些只读命令检查系统中的关键接口。

12.1 查看地图

ros2 topic echo /map --once

12.2 查看激光扫描的坐标系

ros2 topic echo /scan --once

重点观察消息头中的 frame_id。

12.3 检查全局与局部 TF

ros2 run tf2_ros tf2_echo map odom
ros2 run tf2_ros tf2_echo odom base_footprint

12.4 查看导航 Action

ros2 action list
ros2 action info /navigate_to_pose

12.5 查看生命周期状态

ros2 lifecycle nodes
ros2 lifecycle get /planner_server
ros2 lifecycle get /controller_server

12.6 查看速度指令

ros2 topic info /cmd_vel -v

不同 ROS 2 发行版、启动文件和机器人可能使用不同命名空间、Frame 或速度话题，以上命令应根据实际系统调整。

---

13. 几个容易混淆的概念

13.1 建好地图不等于完成导航

SLAM 输出地图和全局位姿，但导航还需要 Costmap、Planner、Controller 和任务管理逻辑。

13.2 /map 不等于 Costmap

/map 通常是 SLAM 或 Map Server 发布的占据栅格地图；Costmap 在它的基础上加入实时障碍物、膨胀、安全区域和其他导航规则。

13.3 Planner 不直接控制电机

Planner 输出路径，Controller 才根据路径和局部环境计算 /cmd_vel。

13.4 Behavior Tree 不是路径规划算法

行为树负责组织任务流程。A*、Hybrid-A*、MPPI、RPP 等算法运行在对应的 Planner 或 Controller 插件中。

13.5 map 不应该承担局部连续控制

map 可能因为回环或重新定位发生全局修正；平滑连续的局部运动通常由 odom 提供。

13.6 Nav2 不强制要求使用 LiDAR

LiDAR 是成熟而常见的选择，但 Nav2 的核心要求是系统能够提供符合约定的 TF、状态估计和环境表示。视觉、深度相机、雷达或其他传感器也可以参与定位和避障。

---

14. 从 SLAM 地图到自主导航的完整认识

现在回头看第二篇文章，二维激光 SLAM 并不是一条孤立的算法链。它的输出会进入更大的机器人系统：

SLAM 中的结果	Nav2 中的用途
/map 占据栅格地图	构建 Global Costmap 的静态层
map → odom	确定机器人在全局地图中的位置
激光扫描 /scan	既可用于 SLAM，也可更新局部障碍物
里程计 odom → base_link	为局部控制提供平滑连续的运动估计
机器人与传感器 TF	把不同传感器数据转换到统一坐标系

完整导航链路可以总结为：

传感器观测
    ↓
SLAM / Localization
    ↓
地图与全局位姿
    ↓
Global / Local Costmap
    ↓
Planner 计算路径
    ↓
Smoother 改善路径
    ↓
Controller 计算速度
    ↓
底盘执行运动
    ↓
传感器产生新的观测
    └──────────────→ 再次更新系统

这是一条持续闭环，而不是从地图到电机的一次性流水线。机器人每移动一步，状态估计和局部环境都会变化，规划与控制也可能随之更新。

---

15. 总结

本文沿着 Nav2 官方文档的 Navigation Concepts 章节，把 SLAM 的输出放进了完整自主导航系统。

最重要的概念可以归纳成四个问题：

SLAM / Localization：机器人在哪里？环境是什么样的？
Costmap：哪些区域安全，哪些区域代价更高？
Planner：从当前位置到目标位置走哪条路？
Controller：这一时刻应该怎样运动？

而 Action、Lifecycle Node 与 Behavior Tree 分别解决：

Action：怎样发送、反馈和取消长时间导航任务？
Lifecycle：怎样可靠地启动、管理和关闭导航服务器？
Behavior Tree：怎样组织规划、控制、重试和恢复流程？

上一篇文章结束在 /map 和 map → odom；本文则继续解释了这两项输出怎样进入 Costmap、Planner 和 Controller，最终变成 /cmd_vel。

下一篇可以真正进入 Nav2 实践：加载上一轮保存的地图，使用 AMCL 完成定位，在 RViz 中发送目标点，并观察 Planner、Controller、Costmap 与 Behavior Tree 怎样共同完成一次导航任务。

---

参考资料

1. Nav2 官方文档：Navigation Concepts
2. Nav2 官方文档：Getting Started
3. Nav2 官方文档：First-Time Robot Setup Guide
4. ROS 2 Design：Managed Nodes / Lifecycle Nodes
5. BehaviorTree.CPP：Introduction to Behavior Trees
6. ROS REP-105：Coordinate Frames for Mobile Platforms
7. SLAM Toolbox 官方仓库

版本提示：Nav2、ROS 2 与相关插件会持续更新，不同发行版的节点名称、默认插件、参数和命名空间可能不同。本文重点介绍稳定的系统概念；文中的命令应以实际运行环境中的 Topic、Action、Lifecycle Node 和 TF 名称为准。