2. 导航栈核心概念：全局路径规划、局部路径规划、代价地图、行为树、生命周期节点
好，咱们直接进入正题。Nav2 导航栈听起来挺唬人，拆开来看其实就五个核心零件。我当年刚接触 ROS 导航时，也是被这些概念绕得晕头转向。后来在项目里一个个踩坑，才真正搞明白它们各自管什么事。
说白了，这五个东西就是机器人的「大脑、小脑、眼睛、决策逻辑和开关」。下面我一个一个讲清楚。
2.1 全局路径规划（Global Planner）
全局路径规划，就是给机器人画一条从起点到终点的「大路线」。它只管宏观，不管细节。
我个人习惯把全局规划器比作「地图导航」。你从北京开到上海，高德地图给你规划一条高速路线。它不会管你路上哪个路口有个坑、哪条车道在施工——那些是局部规划器的事。
核心任务：在已知的全局代价地图上，搜索出一条从起点到目标点的无碰撞路径。
Nav2 里常用的全局规划器有：
- NavFn：基于 Dijkstra 或 A* 算法，稳定可靠，适合大多数场景。
- Smac Planner：支持 Hybrid-A*，适合阿克曼底盘，能生成更平滑的路径。
- Theta*：比 A* 路径更短，但计算量稍大。
我在项目中遇到过一个问题：全局路径总是贴着墙走，稍微有点误差机器人就撞上。后来发现是 inflater_layer 的膨胀半径设得太小。嗯，这里要注意——全局规划器的路径质量，很大程度上取决于代价地图的配置。
调优小技巧：全局路径的平滑度比最短更重要。宁可绕一点，也别让路径贴着障碍物走。我一般会在 planner_server 的配置里把 tolerance 设到 0.5 米左右，给局部规划器留点余量。
2.2 局部路径规划（Local Planner）
局部路径规划，负责「跟着全局路线走，同时避开眼前的障碍物」。它只看机器人周围几米的范围，实时性要求很高。
你想想看，全局规划告诉你「前面右转」，但右转的路上突然出现一个纸箱。局部规划器就要立刻决定：减速、绕过去、还是停下来等？
Nav2 里常见的局部规划器：
| 规划器 | 适用底盘 | 特点 |
|---|---|---|
| DWB（Dynamic Window Approach） | 差速、全向 | 成熟稳定，参数多，可调性强 |
| MPPI（Model Predictive Path Integral） | 差速、阿克曼 | 基于采样，路径更平滑，适合高速 |
| Regulated Pure Pursuit | 阿克曼 | 简单高效，适合走廊等窄通道 |
我曾经在一个仓储项目里吃过亏。机器人速度一快，局部规划器就频繁急刹，货物全倒了。后来把 DWB 的 max_vel_x 从 1.0 降到 0.6，同时把 min_vel_x 设成 -0.2 允许微调倒退，问题就解决了。
避坑指南：局部规划器的更新频率很关键。默认 20Hz 够用，但如果你的传感器噪声大，可以降到 10Hz。我曾经把频率调到 50Hz，结果 CPU 跑满，机器人直接罢工。
2.3 代价地图（Costmap）
代价地图，就是机器人的「环境感知模型」。它把传感器数据（激光雷达、深度相机）转换成一张栅格地图，每个格子标一个代价——0 表示完全自由，255 表示致命障碍。
Nav2 里代价地图分两层：
- 全局代价地图（Global Costmap）：用于全局规划，通常较大，更新频率低。
- 局部代价地图（Local Costmap）：用于局部规划，范围小，更新频率高。
代价地图的核心是「层（Layer）」的概念。每个层负责一种信息源：
# 典型的代价地图层配置
layers:
- static_layer: # 静态地图（来自 SLAM 或已知地图）
- obstacle_layer: # 动态障碍物（激光雷达实时数据）
- inflation_layer: # 膨胀层（给障碍物加安全距离）
- sonar_layer: # 声纳数据（可选）
我个人觉得，膨胀层是最容易被忽视但又最关键的一层。它决定了机器人离障碍物多远就开始减速。设太小，机器人容易擦碰；设太大，窄通道过不去。
经验值：对于直径 0.5 米的机器人，inflation_radius 设 0.3 米比较稳妥。cost_scaling_factor 默认 10.0，如果机器人频繁在障碍物附近犹豫，可以降到 5.0。
2.4 行为树（Behavior Tree）
行为树是 Nav2 的「决策大脑」。它用树状结构组织机器人的行为逻辑：先做什么、后做什么、出错了怎么办。
你可能会问：为什么不用有限状态机？我以前也这么想。直到我在一个项目里需要处理「导航中突然没电→回充→继续导航」这种复杂流程，状态机的状态图画得跟蜘蛛网一样。行为树就清爽多了。
行为树的四个基本节点类型：
| 节点类型 | 作用 | 类比 |
|---|---|---|
| Sequence（顺序节点） | 从左到右依次执行，全部成功才算成功 | 「先开门，再进门」 |
| Fallback（回退节点） | 从左到右尝试，一个成功就停止 | 「先走大路，大路不通走小路」 |
| Parallel（并行节点） | 同时执行多个子节点 | 「一边导航，一边播放语音」 |
| Decorator（装饰节点） | 修改子节点的行为（如重试、超时） | 「尝试 3 次，不行就放弃」 |
Nav2 默认的行为树文件在 nav2_bt_navigator 包里，叫 navigate_to_pose.xml。我建议你直接打开看看，结构非常清晰。
实战建议：不要一上来就自己写行为树。先用默认的，跑通了再改。我见过太多人把行为树改得乱七八糟，最后机器人原地打转。
2.5 生命周期节点（Lifecycle Node）
生命周期节点，是 ROS2 里一个很实用的机制。它让节点有明确的「状态机」：未配置→未激活→激活→去激活。每个状态切换都有回调函数，方便你管理资源。
Nav2 里的核心组件（planner_server、controller_server、costmap_2d 等）都是生命周期节点。这样做的好处是：
- 启动顺序可控：先加载代价地图，再启动规划器，最后激活控制器。
- 运行时重置：出错了可以反初始化再重新激活，不用重启整个导航栈。
- 资源管理：在未激活状态下释放内存，降低功耗。
我记得第一次用 Nav2 时，看到终端里一堆 on_configure、on_activate 的日志，还以为是报错。后来才明白，这是生命周期节点在按顺序启动。
注意：如果你自己写插件（比如自定义规划器），一定要继承 nav2_core::Planner 或 nav2_core::Controller，并实现生命周期回调。否则 Nav2 不会认你的插件。
2.6 五者关系总览
这五个概念不是孤立的。它们协同工作的流程大致如下：
- 生命周期节点按顺序启动所有组件。
- 全局代价地图提供环境信息，全局规划器生成宏观路径。
- 局部代价地图实时更新障碍物信息，局部规划器跟踪全局路径并避障。
- 行为树控制整个流程：规划→执行→重试→恢复。
下面这张图可以帮你理清它们的关系：
嗯，这张图基本把五个核心概念的关系说清楚了。你可能会注意到，代价地图被全局和局部规划器共同使用，但它们的配置是独立的。这也是 Nav2 设计上的一个巧妙之处——全局地图可以粗糙一点、更新慢一点，局部地图必须精细、实时。