这篇文档的目标是建立一套能分析 Nav2 的思维方式：机器人为什么能从 A 点走到 B 点？为什么需要地图、定位、规划、控制、行为树、恢复行为？这些概念在源码中分别落在哪里？

读完后，能回答三个问题：

1. 给机器人下发一个目标点后，Nav2 内部到底发生了什么。
2. 行为树导航器、全局规划服务器、局部控制服务器、速度平滑器、碰撞监控器，各自负责什么，为什么要拆成这些节点。
3. 为什么 Nav2 大量使用生命周期、动作、插件加载机制、代价地图、行为树这些机制。

navigation2 源码地址：https://github.com/ros-navigation/navigation2

术语约定

为了让没有基础的读者更容易阅读，本文使用中文名称。第一次读源码时，需要知道这些中文名称对应的 Nav2 源码模块：

中文名称	源码中的名称
行为树导航器	bt_navigator
全局规划服务器	planner_server
局部控制服务器	controller_server
速度平滑器	velocity_smoother
碰撞监控器	collision_monitor
全局代价地图	global_costmap
局部代价地图	local_costmap
动作	ROS2 action
生命周期节点	ROS2 lifecycle node
插件加载机制	plugin
导航函数规划器	NavFnPlanner
流水线顺序节点	PipelineSequence
恢复节点	RecoveryNode
频率控制节点	RateController

导航不是“画一条线”

很多人会把机器人导航理解成“从当前位置画一条线到目标点，然后让机器人沿着线走”。真实系统复杂得多，因为机器人面对的是一个不断变化的物理世界：

• 地图可能不完整。
• 定位会有误差。
• 传感器会有噪声。
• 前方可能突然出现人或障碍物。
• 底盘不能瞬间达到任意速度。
• 目标点可能贴墙、落在障碍物附近，甚至不可达。
• 机器人可能卡住、打滑、转不过去。

所以 Nav2 的核心思路是把“导航”拆成一组更小的问题：

问题	Nav2 中的答案
我在哪里？	定位系统，如 AMCL、SLAM、里程计、TF
周围哪里能走？	代价地图
从当前位置到目标点大概走哪条路？	全局规划服务器负责全局规划
当前这一瞬间该发什么速度？	局部控制服务器负责局部控制
速度是否太突兀，底盘能不能执行？	速度平滑器负责速度平滑
最终速度是否会导致马上碰撞？	碰撞监控器负责碰撞监控
失败后先清地图、等待、旋转还是后退？	行为树导航器负责任务调度

Nav2 不是一个“大导航算法”，而是一套导航任务编排框架。它把复杂导航拆成多个服务器和插件，再用行为树把它们组织起来。

ROS2 基础概念

节点

ROS2 中的节点可以理解为一个独立功能单元。Nav2 不是单个节点，而是很多节点协作：

• 行为树导航器：接收导航目标，执行行为树。
• 全局规划服务器：提供全局路径规划动作。
• 局部控制服务器：提供路径跟踪动作。
• 地图服务器：提供静态地图。
• amcl：根据激光和地图估计机器人在地图中的位置。
• 速度平滑器：平滑速度。
• 碰撞监控器：最终碰撞保护。

这样拆分的好处是每个节点职责清楚，可以单独替换、调试、重启和测试。

话题

话题像“广播频道”，适合持续发布的数据。例如：

• /scan：激光雷达数据。
• /odom：里程计数据。
• /cmd_vel：速度指令。
• /plan：规划出来的路径。

话题的特点是发布者不等待接收者处理完成，适合传感器、速度、状态这类连续数据。

服务

服务像函数调用，适合短任务。例如清理代价地图：

• global_costmap/clear_entirely_global_costmap
• local_costmap/clear_entirely_local_costmap

调用者发出请求，服务端处理后返回结果。

动作

导航、规划、路径跟踪都不是瞬间完成的，所以 Nav2 大量使用动作。动作同时支持：

• 发送目标。
• 周期反馈。
• 成功或失败结果。
• 取消任务。

典型动作：

• NavigateToPose：用户对行为树导航器下发“去某个目标点”。
• ComputePathToPose：行为树请求全局规划服务器规划路径。
• FollowPath：行为树请求局部控制服务器跟踪路径。

可以这样理解：话题是广播，服务是短函数，动作是可取消的长任务。

坐标变换

机器人导航离不开坐标系。常见坐标系有：

坐标系	含义
map	全局地图坐标系，目标点和全局路径通常在这里
odom	里程计坐标系，短时间连续但会慢慢漂移
base_link	机器人本体坐标系
laser / camera	传感器坐标系

Nav2 经常需要做坐标转换。例如全局路径在 map 下，控制器需要知道机器人在路径附近的位置，就要通过 TF 把机器人位姿转换到对应坐标系。

很多导航失败不是算法失败，而是 TF 不通、时间戳不对、坐标系名称配置错。

Navigation2 的主链路

先看一个目标点从上层进入到底盘执行的完整链路：

这条链路中最重要的分工是：

• 行为树决定“现在该做什么”。
• 规划器决定“从大局看该走哪条路”。
• 控制器决定“这一瞬间该怎么动”。
• 速度平滑器让速度更符合底盘能力。
• 碰撞监控在最后一刻兜底。

Nav2 的核心架构：服务器 + 插件 + 行为树

Nav2 的设计可以拆成三层：

第一层：行为树负责任务逻辑

行为树负责回答：

• 什么时候规划？
• 什么时候控制？
• 规划失败后做什么？
• 控制失败后做什么？
• 新目标来了是否打断当前任务？
• 是否需要清理代价地图、旋转、等待、后退？

行为树不直接计算路径，也不直接控制底盘。它像一个调度器。

第二层：服务器负责稳定接口

全局规划服务器、局部控制服务器这类服务器提供稳定的动作接口，并管理资源：

• 读取参数。
• 管理代价地图。
• 加载插件。
• 接收动作请求。
• 捕获异常并返回错误码。
• 发布结果或状态。

服务器关心“流程和工程稳定性”，不把具体算法写死。

第三层：插件负责具体算法

具体规划或控制算法通过插件加载机制加载。例如：

• 全局规划器：NavFn、Smac、ThetaStar。
• 控制器：DWB、Regulated Pure Pursuit、MPPI。
• 目标检查器：判断是否到达目标。
• 进度检查器：判断机器人是否卡住。
• 路径处理器：截取、转换、筛选局部路径段。

所以 Nav2 的扩展方式通常不是改服务器，而是写一个实现统一接口的插件。

为什么到处都是 configure 和 activate

Nav2 中很多节点都是生命周期节点。普通节点一启动就直接工作，而生命周期节点有明确状态：

生命周期的意义是把“准备资源”和“正式工作”分开：

阶段	做什么
on_configure()	读取参数、创建插件、初始化代价地图、创建动作服务器 / publisher / subscriber
on_activate()	激活动作服务器和生命周期 publisher，开始对外提供能力
on_deactivate()	暂停服务或停止发布，必要时发布零速度
on_cleanup()	释放插件、线程、代价地图、通信对象

以全局规划服务器为例，PlannerServer::on_configure() 会：

1. 配置全局代价地图。
2. 创建代价地图线程。
3. 读取规划器参数。
4. 使用插件加载机制加载 nav2_core::GlobalPlanner 插件。
5. 创建 compute_path_to_pose 和 compute_path_through_poses 动作服务器。

这说明全局规划服务器不关心你使用 NavFn、Smac 还是别的规划器。它只关心插件是否实现了统一的 createPlan() 接口。

代价地图

代价地图就是 Nav2 如何把世界变成“能不能走”，规划器和控制器都离不开代价地图。代价地图可以理解成一张“带危险程度的栅格地图”。

栅格地图是什么

机器人把连续世界切成很多小格子，每个格子代表一小块区域。每个格子有一个代价值：

• 空地：代价低，可以走。
• 障碍物：代价极高，不能走。
• 障碍物附近：代价较高，能走但不推荐。
• 未知区域：是否能走取决于参数。

全局规划并不是在连续空间里随便画线，而是在这些格子中找一条连通、代价较低的路径。

为什么障碍物附近也有代价

如果只标记障碍物本身，规划器可能贴着墙走。真实机器人有体积，定位也有误差，贴墙很容易碰撞。所以代价地图会把障碍物向外“膨胀”一圈，这叫膨胀层。

越靠近障碍物，代价越高；离障碍物越远，代价越低。规划器会倾向于选择更安全的路线。

全局代价地图和局部代价地图的区别

类型	作用	关注范围	典型用途
全局代价地图	给全局规划器用	整张地图或较大区域	找从起点到终点的大路线
局部代价地图	给控制器用	机器人附近一小片区域	处理近距离障碍物和动态避障

全局规划器回答“应该往哪里走”，局部控制器回答“现在怎么走才不撞”。

行为树

行为树是导航任务的总调度器，用户通常不会直接调用全局规划服务器和局部控制服务器，而是给行为树导航器发送 NavigateToPose 动作。行为树导航器会加载行为树 XML，然后不断 tick 行为树。

默认的单点导航行为树文件之一是：

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

简化后的结构如下：

行为树节点怎么理解

行为树中的节点会返回三种状态：

• SUCCESS：这个动作或判断成功了。
• FAILURE：失败了。
• RUNNING：还在执行。

例如 FollowPath 在机器人移动过程中通常是 RUNNING，到达目标后返回 SUCCESS，控制失败时返回 FAILURE。

流水线顺序节点为什么重要

普通顺序节点通常是“第一个成功后再执行第二个”。但导航需要一边走一边重新规划。流水线顺序节点的作用就是让前面的规划节点可以周期性重新 tick，同时后面的控制节点持续运行。

默认行为树里会用频率控制节点让全局规划大约 1 Hz 更新一次，而控制器可以用更高频率持续输出速度。这样系统既能稳定控制，又能在环境变化时更新路线。

恢复节点做了什么

恢复节点通常有两个孩子：

1. 正常任务。
2. 恢复动作。

如果正常任务失败，它会执行恢复动作，然后重试。默认导航中常见恢复动作有：

• 清理全局代价地图。
• 清理局部代价地图。
• 原地旋转。
• 等待。
• 后退。

行为树的价值在这里很明显：规划器和控制器只需要报告失败，失败后的处理策略由行为树统一安排。

全局规划服务器：把目标点变成全局路径

全局规划服务器对外提供 ComputePathToPose 和 ComputePathThroughPoses 动作。它的输入和输出很简单：

输入	输出
起点、目标点、规划器编号	nav_msgs/Path 全局路径

如果上层没有指定起点，全局规划服务器会从 TF 中获取当前机器人位姿。

全局规划服务器的工作流程

源码上可以抓住两个位置：

• nav2_planner/src/planner_server.cpp：服务器的动作、插件、代价地图管理。
• nav2_core::GlobalPlanner：全局规划器插件统一接口。

在 PlannerServer::on_configure() 中，代码会通过 gp_loader_.createUniqueInstance(…) 创建规划器插件，再调用插件的 configure(…)。在真正规划时，服务器通过 planners_[planner_id]->createPlan(…) 调用具体算法。

这就是 Nav2 解耦的关键：服务器不实现算法，算法在插件里。

导航函数规划器

导航函数规划器是 Nav2 中经典的全局规划器，适合理解全局规划的基本原理。它的源码主要在：

nav2_navfn_planner/src/navfn_planner.cpp
nav2_navfn_planner/src/navfn.cpp

输入和输出

导航函数规划器的输入：

1. 起点：机器人当前在哪里。
2. 目标点：机器人要去哪里。
3. 代价地图：每个栅格的通行代价。

输出：

• 一串连续位姿点，也就是 nav_msgs/Path。

先算代价场，再沿低代价方向回溯

NavFn 的思路可以这样理解：

1. 把目标点放到栅格地图上。
2. 从目标点开始向外传播“到目标的代价”。
3. 障碍物代价极高，不可通过。
4. 空地代价低，可以传播。
5. 起点附近也会得到一个代价值。
6. 从起点开始，沿着代价越来越低的方向走，最后就能走到目标点。

这有点像把目标点看成山谷，把起点放在山坡上。路径就是从起点顺着“高度下降”的方向走到山谷。当然这里的“高度”不是海拔，而是到目标的累计代价。

Dijkstra 和 A*

NavFn 可以使用 Dijkstra，也可以使用 A*：

• Dijkstra：从目标向外均匀扩展，能找到最短代价路径，但搜索范围可能较大。
• A*：加入启发式估计，更倾向朝目标方向搜索，通常更快。

在 NavfnPlanner::makePlan() 中可以看到：

• calcNavFnAstar(…)
• calcNavFnDijkstra(…)

具体用哪个由参数控制。

为什么目标点不可达时还要找附近点

真实使用中，用户点的目标可能刚好贴墙或落在障碍物上。如果严格要求目标点本身可达，导航会很容易失败。

导航函数规划器会根据 tolerance 在目标点周围寻找一个“离目标足够近且可达”的点。如果目标点本身不可达，但附近有合法点，就规划到这个合法点。

这是工程上很重要的容错：导航系统不能因为用户点歪了几厘米就完全失败。

局部控制服务器

局部控制把路径变成速度，全局路径只是“路线”，底盘不能直接执行路线。底盘真正能执行的是速度，例如：

• linear.x：前进或后退速度。
• linear.y：横向速度，差速底盘通常不用。
• angular.z：旋转速度。

局部控制服务器对外提供 FollowPath 动作。它的输入是路径，输出是速度指令。

局部控制服务器的控制循环

简化流程如下：

源码入口在：

nav2_controller/src/controller_server.cpp

关键函数：

• ControllerServer::computeControl()：FollowPath 动作的主循环。
• ControllerServer::computeAndPublishVelocity()：获取位姿、截取路径、调用控制器插件、发布速度。
• ControllerServer::isGoalReached()：判断是否到达目标。

控制器插件到底做什么

控制器插件实现的是“路径跟踪 + 局部避障 + 速度生成”。不同控制器算法思路不同：

• DWB：采样很多速度，模拟短时间轨迹，给每条轨迹打分，选择最好的一条。
• Regulated Pure Pursuit：找路径前方一个追踪点，根据曲率生成速度，并根据障碍物和转弯情况调节速度。
• MPPI：采样大量控制序列，用代价函数选择更优控制，适合更复杂的动态约束。

但从局部控制服务器看，它们都统一成一个接口：

computeVelocityCommands(...)

这就是插件架构的好处：上层不关心算法细节，只关心“给我一个速度”。

目标检查器和进度检查器

控制器不仅要算速度，还要判断任务状态。

目标检查器负责判断是否到达目标。现实机器人不可能精确停在数学上的目标点，所以通常允许一定位置误差和角度误差。

进度检查器负责判断是否卡住。例如机器人一直在发速度，但实际位置长时间没有变化，就说明可能被障碍物挡住、轮子打滑或控制失败。此时局部控制服务器会返回失败，让行为树触发恢复动作。

速度平滑器

控制器算出的速度不一定适合直接发给底盘。例如上一刻速度是 0，下一刻突然要求 0.8 m/s，真实底盘可能做不到，也会导致运动突兀。

速度平滑器的作用是把速度变得更可执行。它主要处理：

• 最大速度限制。
• 最小速度限制。
• 最大加速度限制。
• 最大减速度限制。
• 死区速度处理。
• 超时未收到速度时输出停止速度。

源码入口：

nav2_velocity_smoother/src/velocity_smoother.cpp

它订阅输入速度，发布平滑后的速度。常见链路是：

局部控制服务器 -> cmd_vel_nav -> 速度平滑器 -> cmd_vel_smoothed

可以把它理解成控制器和底盘之间的“运动学限幅器”。

碰撞检测

即使局部控制器已经考虑了障碍物，Nav2 仍然可以在最终速度发到底盘前加一层碰撞监控器。

它的作用是检查“如果继续执行当前速度，是否马上会撞”。如果有风险，它可以：

• 不处理：速度安全。
• 减速：有风险但还没到必须停。
• 限速：把速度限制在安全范围。
• 停止：障碍物太近，直接输出 0。

它可以使用多种数据源：

• 激光雷达。
• 点云。
• 距离传感器。
• 代价地图。

源码入口：

nav2_collision_monitor/src/collision_monitor_node.cpp

它和控制器的区别是：

• 控制器负责“规划下一步怎么走”。
• 碰撞监控器负责“最终速度是否安全”。

这是一层安全兜底，不应该替代正常的规划和控制。

恢复行为

机器人导航中失败很常见：

• 目标点不可达。
• 地图里有错误障碍物。
• 动态障碍物挡路。
• TF 暂时不可用。
• 控制器找不到安全速度。
• 机器人卡住。

Nav2 的策略不是一失败就结束，而是先尝试恢复。

默认行为树中的恢复动作包括：

恢复动作	解决的问题
清理全局代价地图	全局地图中可能有错误障碍物，清理后重新规划
清理局部代价地图	局部地图中可能有传感器噪声或过期障碍物
原地旋转	重新观察周围环境
等待	等待动态障碍物离开
后退	后退一点，给控制器重新规划空间

恢复行为的本质是：把“失败原因可能是暂时的”这件事纳入任务逻辑。

多点导航

单点导航是从当前位置到一个目标点。多点导航则是从当前位置依次经过多个目标点。

Nav2 中对应动作是 NavigateThroughPoses，行为树中会调用 ComputePathThroughPoses。规划思路通常是：

1. 当前位姿到第一个目标点。
2. 第一个目标点到第二个目标点。
3. 第二个目标点到第三个目标点。
4. 把每段路径拼成完整路径。

控制器仍然负责跟踪最终路径，只是目标检查、路径截取、反馈会围绕多目标任务处理。

误区

行为树不是算法

行为树不是路径规划算法，也不是控制算法。它是任务编排逻辑。真正算路径的是规划器插件，真正算速度的是控制器插件。

全局规划服务器不是只负责 NavFn

全局规划服务器不绑定 NavFn。NavFn 只是一个插件。换成 Smac、ThetaStar 或自己写的规划器，服务器主体逻辑不需要变。

全局规划器基本不负责动态避障

全局规划器主要负责大范围路径。动态障碍物通常更多由局部代价地图、控制器和碰撞监控器处理。全局重规划可以绕开变化后的障碍物，但它不是最高频的避障模块。

局部控制服务器不只是沿路径走

控制器不只是“跟着线走”。它还要考虑机器人当前位置、局部障碍物、当前速度、目标容差、是否卡住、是否还有合法速度。

碰撞监控器有了也要调控制器

碰撞监控器是最后保护层，不是正常避障策略。如果它频繁触发停止，通常说明局部代价地图、控制器参数、传感器配置或速度限制需要调整。

总结每个核心模块

模块	我的理解
行为树导航器	导航任务调度器，决定规划、控制和恢复的顺序
全局规划服务器	把目标点变成全局路径
导航函数规划器	一个经典全局规划插件，用代价传播和回溯生成路径
局部控制服务器	把路径变成连续速度
目标检查器	判断机器人是否到达目标
进度检查器	判断机器人是否卡住
全局代价地图	给全局规划用的大范围代价地图
局部代价地图	给局部控制用的近距离代价地图
速度平滑器	限制速度和加速度，让命令更适合底盘
碰撞监控器	速度发到底盘前的最后碰撞保护
生命周期	让节点启动、激活、暂停、清理过程可控
插件加载机制	让算法可插拔，换算法不用改服务器

分析问题时看数据链

调 Nav2 时不要只问“为什么导航失败”，要沿数据链定位问题：

1. 目标点是否在正确坐标系？
2. TF 是否完整，map -> odom -> base_link 是否正常？
3. 定位是否稳定？
4. 全局代价地图是否正确显示障碍物和膨胀层？
5. 规划器是否能生成全局路径？
6. 局部代价地图是否能看到近距离障碍物？
7. 控制器是否能输出合理速度？
8. 速度平滑器是否把速度限制得过小？
9. 碰撞监控器是否频繁拦截速度？
10. 行为树是否进入了恢复分支？

Nav2 不是记住所有参数，而是知道每个模块的输入、输出、职责边界，以及失败时应该检查哪条数据链。