3. Nav2架构概览

Nav2是ROS2生态中用于移动机器人自主导航的官方框架，其核心设计目标是将传统ROS1 Navigation Stack的紧耦合结构重构为松耦合、可扩展、行为树驱动的导航系统。理解Nav2的架构，不能只停留在“它有哪些模块”的层面，而必须深入到“数据如何在模块间流动”“行为树如何编排这些模块”“插件机制如何让每个模块可替换”这三个维度。本章将从导航栈的四大核心组件（Planner、Controller、Recovery、BT Navigator）入手，剖析其职责边界、数据接口与协作关系，并重点解读行为树（Behavior Tree）作为任务编排中枢的设计哲学，以及插件化架构对工程落地带来的实际价值。

上图展示了Nav2导航栈的核心数据流与组件协作关系。BT Navigator作为行为树执行器，位于架构中枢，向下调用Planner、Controller与Recovery三个执行组件，向上接收来自传感器、代价地图和里程计的数据。值得注意的是，所有组件之间的通信并非直接ROS2 Topic耦合，而是通过BT Navigator内部的黑板（Blackboard）机制传递数据，这种设计使得任务编排逻辑与具体算法实现完全解耦。下面我们将逐一拆解每个组件的职责与实现要点。

3.1 四大核心组件的职责边界与协作机制

3.1.1 Planner：全局路径规划器

核心原理：Planner负责在静态或半静态代价地图上搜索从机器人当前位置到目标点的无碰撞路径。其输出是一条全局路径（nav_msgs/msg/Path），通常由一系列离散的位姿点组成。Nav2中Planner通过nav2_core::GlobalPlanner接口定义，所有具体算法（如NavFn、Smac Planner系列）均实现该接口。

关键实现差异：

• NavFn（Dijkstra/A*）：基于栅格地图的经典搜索算法，计算速度快，但路径平滑度较差，适合简单环境。
• SmacPlannerHybrid：基于Hybrid A*的搜索算法，考虑机器人运动学约束（最小转弯半径），生成的路径更平滑，但计算开销较大。
• Theta*：在A*基础上允许任意角度连接，路径更短，但需要额外的视线检查。

3.1.2 Controller：局部轨迹跟踪控制器

核心原理：Controller接收全局路径、局部代价地图和当前位姿，实时计算速度指令（geometry_msgs/msg/Twist）发送给底盘。其核心任务是跟踪全局路径的同时避开动态障碍物。Nav2中Controller通过nav2_core::Controller接口定义，常用实现包括DWB（Dynamic Window Approach的改进版）、MPPI（Model Predictive Path Integral）和Regulated Pure Pursuit。

控制频率与延迟：Controller通常运行在10-50Hz，其输出直接作用于底盘。因此，Controller的算法复杂度必须与计算资源匹配。例如，MPPI需要GPU或高性能CPU进行并行采样，而Regulated Pure Pursuit则适合低算力平台。

3.1.3 Recovery：故障恢复行为

核心原理：Recovery组件在导航任务失败时（如陷入局部极小值、路径被完全阻塞）执行恢复行为。Nav2内置了三种标准恢复行为：

1. Spin：原地旋转指定角度，用于重新感知周围环境。
2. Backup：沿机器人朝向的反方向后退指定距离，用于脱离卡死状态。
3. Wait：等待指定时间，用于处理临时性障碍（如行人经过）。

恢复策略的编排：恢复行为并非由Controller直接触发，而是由行为树中的RecoveryNode根据子节点返回的失败状态决定执行顺序。例如，当Controller连续多次无法跟踪路径时，行为树会依次尝试Spin、Backup、ClearCostmap等恢复动作。

3.1.4 BT Navigator：行为树任务编排器

核心原理：BT Navigator是Nav2区别于ROS1 Navigation Stack的最核心设计。它将导航任务（如“从A点到B点”）建模为一棵行为树，树的每个节点代表一个原子动作（如计算路径、跟踪路径、恢复旋转）或控制流节点（如序列、选择、并行）。BT Navigator负责加载XML格式的行为树定义，并通过黑板的键值对机制在节点间传递数据。

行为树与状态机的本质区别：传统状态机（如SMACH）的转换逻辑是显式编码的，而行为树通过节点间的父子关系隐式表达决策逻辑。这使得行为树具有更好的模块化、可复用性和可视化调试能力。例如，一个“带恢复的导航”行为树可以表示为：

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
      <Action ID="ComputePathToPose" goal="{goal}" path="{path}"/>
      <RecoveryNode name="follow_path_with_recovery">
        <Action ID="FollowPath" path="{path}" cmd_vel="{cmd_vel}"/>
        <Fallback>
          <Action ID="Spin" spin_dist="1.57"/>
          <Action ID="Backup" backup_dist="0.5"/>
        </Fallback>
      </RecoveryNode>
    </Sequence>
  </BehaviorTree>
</root>

上述XML定义中，RecoveryNode会先尝试执行FollowPath，若返回失败，则依次尝试Spin和Backup。这种编排方式使得导航策略的调整无需修改C++代码，只需编辑XML文件即可。

3.2 数据流与黑板机制

3.2.1 核心数据流路径

Nav2的数据流可以归纳为三条主线：

黑板（Blackboard）的作用：黑板是行为树节点间共享数据的全局存储区。例如，Planner计算出的路径存入黑板的path键，Controller从黑板读取该键。这种设计避免了节点间通过ROS Topic传递中间数据，减少了序列化和网络开销，同时使得节点间的数据依赖关系在行为树XML中显式可见。

3.2.2 代价地图的层级结构

Nav2维护两张代价地图：全局代价地图（用于Planner）和局部代价地图（用于Controller）。两者通过nav2_costmap_2d模块独立维护，但共享传感器数据源。全局地图通常尺寸较大（如10m×10m），分辨率较低（0.05m/cell）；局部地图尺寸较小（如3m×3m），分辨率较高（0.025m/cell）。这种分层设计平衡了计算效率与避障精度。

3.3 插件化设计：从“改代码”到“配参数”

3.3.1 插件化架构的实现原理

Nav2的插件化基于ROS2的pluginlib库实现。每个核心组件（Planner、Controller、Recovery）都定义了抽象接口类，具体算法作为插件注册到系统中。在运行时，通过参数文件（YAML）指定加载哪个插件，例如：

# planner_params.yaml
planner_plugin: "nav2_smac_planner/SmacPlannerHybrid"
smac_planner_hybrid:
  minimum_turning_radius: 0.4
  max_iterations: 1000

这种设计带来的直接收益是：算法升级无需重新编译整个导航栈。例如，当社区发布了新的Planner算法，只需安装对应的插件包，并在YAML中修改一行参数即可切换。

3.3.2 插件化设计的工程价值

• 算法对比效率提升：在选型阶段，可以在同一套导航框架下快速切换DWB和MPPI，通过实车测试对比跟踪精度和计算资源占用。
• 场景定制化：室内场景使用Regulated Pure Pursuit保证平滑性，室外场景切换为MPPI以应对复杂地形。
• 团队协作解耦：算法工程师只需关注插件接口的实现，无需理解行为树编排逻辑；导航集成工程师只需配置XML和YAML，无需修改C++代码。

3.4 实战案例：室内服务机器人导航策略调优

场景背景：某仓储物流机器人项目，采用差速底盘，最大速度1.2m/s，最大加速度1.5m/s²。运行环境为货架密集的仓库，通道宽度约1.5m，存在频繁的叉车动态障碍。初始方案使用NavFn + DWB，但实际运行中出现两个问题：① 在货架转角处频繁陷入局部极小值，导致Spin恢复动作触发过于频繁（平均每5分钟一次）；② 全局路径穿过货架间隙时，由于路径不平滑，DWB跟踪时出现明显抖动。

关键指标采集：

• 全局路径规划平均耗时：35ms（NavFn）
• 局部路径跟踪平均线速度误差：0.18m/s
• 恢复行为触发频率：0.2次/分钟
• 任务完成率（100m导航任务）：82%

分析过程：通过Nav2的BT日志（ros2 run nav2_bt_navigator bt_navigator --ros-args -p bt_xml_filename:=navigate_to_pose_w_recovery.xml）发现，大部分失败发生在FollowPath节点返回FAILURE后，行为树执行Spin恢复。进一步查看局部代价地图，发现DWB在货架转角处由于前方障碍物密集，动态窗口内无可行速度，导致跟踪失败。

优化操作：

1. 更换Planner：将NavFn替换为SmacPlannerHybrid，设置minimum_turning_radius: 0.3（略小于底盘最小转弯半径0.35m），使全局路径在转角处更平滑。
2. 调整Controller参数：将DWB的min_vel_x从0.0改为0.1，避免机器人完全停止；增大path_distance_bias从0.6到0.8，使控制器更倾向于跟踪全局路径而非局部避障。
3. 修改行为树：在RecoveryNode中增加ClearCostmap动作，在Spin之前先清除局部代价地图中的障碍物信息（针对动态障碍物误检测的情况）。

结果对比：

复盘经验：本次调优的核心收益并非来自单一组件的替换，而是通过行为树将Planner、Controller和Recovery的协作逻辑重新编排。SmacPlannerHybrid虽然增加了规划耗时，但显著减少了后续Controller的跟踪压力；而ClearCostmap动作的加入，则针对仓库场景中动态障碍物频繁导致代价地图污染的问题提供了低成本解决方案。这印证了Nav2架构设计的核心思想：导航性能的瓶颈往往不在单个算法，而在于组件间的协作策略。

3.5 常见误区与总结复盘

3.5.1 常见误区

• 误区一：认为BT Navigator只是“状态机的替代品”。实际上，行为树的强大之处在于其支持条件节点（Condition）、装饰节点（Decorator）和并行节点（Parallel），可以表达更复杂的任务逻辑，例如“在导航过程中持续监测电量，低于阈值时切换目标点”。
• 误区二：忽略代价地图的更新频率对Controller的影响。如果局部代价地图的更新频率低于Controller的控制频率，Controller可能会基于过时的障碍物信息规划速度，导致碰撞。通常需要将costmap_2d的update_frequency设置为Controller频率的2倍以上。
• 误区三：将所有恢复行为都堆叠在行为树中。过多的恢复行为会导致任务卡在恢复循环中。建议每个RecoveryNode最多配置2-3个恢复动作，并设置合理的重试次数上限。

3.5.2 架构设计要点总结

Nav2的架构设计可以提炼为三个核心原则：

1. 数据与逻辑分离：传感器数据、代价地图、路径等通过Topic和黑板传递，而任务决策逻辑完全由行为树XML定义，两者互不干扰。
2. 算法与框架分离：所有算法组件通过插件接口接入，框架不关心具体算法实现，只定义数据交互契约。
3. 策略与参数分离：行为树定义“做什么”（任务编排），YAML参数定义“怎么做”（算法参数），两者均可独立修改。

理解这三点，是后续学习Nav2高级功能（如自定义行为树节点、编写新的Planner插件、多机器人导航协调）的基础。在实际工程中，建议团队至少维护一套标准的行为树模板和参数基线，在此基础上针对不同场景（室内、室外、仓储、巡检）进行参数微调，而不是每次从零开始搭建导航配置。