引言
Nav2 是 ROS 2 的标准导航框架，它把「给一个目标点，把机器人安全开过去」拆成定位、全局规划、局部控制、恢复行为四件事，每件事由一个独立的生命周期服务器承担，再用行为树把它们编排起来。理解 Nav2 的关键不是记住某个参数，而是理解这组服务器之间的数据流与生命周期依赖。
工程上的第一个难点是「参数太多且互相耦合」。一个完整的 Nav2 配置有 200 个以上的参数，膨胀半径、代价衰减、控制器容差、进度检查阈值彼此影响。现场调参时改一个参数导致另一个场景退化是常态，因此必须有系统的调参顺序与回归方法，而不是逐个试。
第二个难点是行为树。Nav2 的默认行为树只是起点，真实产品几乎都要改：加急停分支、加多段恢复、加充电桩对接、加等待电梯。读懂默认的 navigate_to_pose_w_replanning_and_recovery.xml 并能安全地裁剪它，是从「跑通 demo」到「能上线」的分水岭。
第三个难点是与定位的接口。Nav2 不负责定位，它消费 /map 与 map → odom 变换。定位漂移、TF 跳变、地图过期都会表现为「导航行为诡异」，但根因不在 Nav2。本文按「架构 → 行为树 → 代价地图 → 规划器 → 控制器 → 恢复 → 生命周期 → 过滤器 → 定位接口 → 验证」的顺序展开，通用规划算法本身见运动规划与轨迹优化
，本文只讲 Nav2 的工程落地。
目录
- Nav2 架构与服务器拓扑
- 行为树：节点类型与编排
- 自定义行为树节点与插件
- 代价地图：层、语义与更新
- 膨胀层与代价衰减调参
- 全局规划器与插件选型
- 局部控制器与速度平滑
- 恢复行为、进度检查与目标检查
- 生命周期、启动顺序与参数组织
- 过滤器：keepout、速度限制与安全监视
- 与 SLAM 和 AMCL 的接口
- 仿真回归验证与现场调参
1. Nav2 架构与服务器拓扑
Nav2 的每个能力都是一个生命周期节点，由 nav2_bringup 的 launch 一次性拉起，彼此通过话题与动作通信。
核心服务器与职责：
map_server ：发布静态地图与 /map 元数据
amcl ：在已知地图上定位，发布 map->odom
planner_server ：全局规划（ComputePathToPose 动作）
controller_server ：局部控制（FollowPath 动作），输出 /cmd_vel
behavior_server ：恢复行为（Spin、BackUp、Wait、DriveOnHeading）
bt_navigator ：行为树执行器（NavigateToPose 动作入口）
waypoint_follower ：多点巡航（FollowWaypoints 动作）
smoother_server ：路径平滑
velocity_smoother ：速度指令限幅与平滑
collision_monitor ：基于传感器与多边形区域的安全减速/急停
一次 NavigateToPose 的数据流是：目标 → bt_navigator → 行为树 → planner_server 产出全局路径 → controller_server 逐段跟踪并输出 /cmd_vel → velocity_smoother → 底盘驱动。bt_navigator 是唯一的对外入口，它不直接规划，只负责按行为树的逻辑调用其他服务器。这样设计的价值是「策略与算法分离」：换规划器改插件参数，换任务逻辑改行为树 XML，两者互不干扰。服务器之间用动作而非话题协作，因为规划与跟踪都是长耗时、可取消、有反馈的操作。
2. 行为树：节点类型与编排
行为树是一种从根向下执行、返回 SUCCESS/FAILURE/RUNNING 三态的树形控制结构，Nav2 用 BehaviorTree.CPP 实现。
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<SequenceStar name="RecoveryActions">
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
</SequenceStar>
</RecoveryNode>
</BehaviorTree>
关键节点类型分三类：
控制节点（决定子节点如何执行）：
Sequence / SequenceStar ：依次执行，任一失败即失败；Star 版重试时不重跑已完成节点
Fallback ：依次尝试，任一成功即成功（OR 语义）
PipelineSequence ：前者 RUNNING 时后者也并行跑，用于边规划边跟踪
RecoveryNode ：先跑第一个子节点，失败则跑第二个（恢复），可重试 n 次
RateController ：限制子节点执行频率
动作节点（叶节点，调用服务器）：
ComputePathToPose、FollowPath、Spin、BackUp、Wait、ClearEntireCostmap
条件节点（只读黑板，无副作用）：
GoalUpdated、IsPathValid、TimeExpired
PipelineSequence 是 Nav2 的巧妙设计：它让「规划」与「跟踪」并行，当规划节点返回 RUNNING（还没算完）时，跟踪节点继续用旧路径跑。这样重规划不会打断运动，避免了「每次重规划都停一下」的抖动。
3. 自定义行为树节点与插件
默认行为树覆盖不了真实产品逻辑，必须能加自己的节点。
// 自定义动作节点：在门口等待，直到条件满足或超时
#include "behaviortree_cpp/action_node.h"
class WaitForDoorOpen : public BT::StatefulActionNode {
public:
WaitForDoorOpen(const std::string & name, const BT::NodeConfiguration & cfg)
: BT::StatefulActionNode(name, cfg) {
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
}
static BT::PortsList providedPorts() {
return { BT::InputPort<double>("timeout", 30.0, "最长等待秒数") };
}
BT::NodeStatus onStart() override {
getInput("timeout", timeout_);
start_ = node_->now();
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus onRunning() override {
if (door_open_) return BT::NodeStatus::SUCCESS;
if ((node_->now() - start_).seconds() > timeout_) return BT::NodeStatus::FAILURE;
return BT::NodeStatus::RUNNING;
}
void onHalted() override {}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Time start_;
double timeout_ = 30.0;
bool door_open_ = false;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<WaitForDoorOpen>("WaitForDoorOpen");
}
两条必须遵守的纪律。第一，黑板 key 要唯一且有文档，{path}、{goal} 这类默认 key 被多个子树共享，自己新增的 key 加前缀（如 door_retry_count）避免冲突。第二，自定义节点不能阻塞，onRunning 必须快速返回，长等待用计时器与状态机表达，不要在里面 sleep，否则会卡死整个执行器。bt_navigator 的 XML 路径由参数 default_nav_to_pose_bt_xml 指定，产品里通常维护多套 XML（正常导航、充电、电梯、窄通道），运行时通过 NavigateToPose 的 behavior_tree 字段切换。
4. 代价地图：层、语义与更新
Nav2 有全局与局部两张代价地图，都由若干可插拔的层叠加而成。
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0 # 全局图更新慢
resolution: 0.05 # 5 cm/格
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
observation_sources: scan
scan: { topic: /scan, clearing: true, marking: true,
raytrace_max_range: 3.5, obstacle_max_range: 3.0 }
inflation_layer:
inflation_radius: 0.55
cost_scaling_factor: 3.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0 # 局部图高频更新
rolling_window: true # 跟随机器人滚动
width: 6 # 6m × 6m
plugins: ["voxel_layer", "inflation_layer"]
代价的语义是固定分级的，理解它才能读懂规划器行为：
代价分级（0~255）：
0 ：空闲，可自由通过
1~252 ：膨胀产生的代价，越高越靠近障碍
253 (INSCRIBED)：内切圆会碰到障碍，不可通行
254 (LETHAL) ：致命障碍，绝对不可通行
255 (NO_INFORMATION)：未知区域，取决于 track_unknown_space
全局图与局部图分工明确：全局图追求稳定与全环境覆盖，用静态层加低更新频率；局部图追求实时，用滚动窗口加体素层加高更新频率。局部图不做 raytrace 清除会让幽灵障碍残留，做了又可能把真实障碍误清，clearing 与 raytrace_max_range 是这组矛盾的调节旋钮。
5. 膨胀层与代价衰减调参
膨胀层是 Nav2 最难调的层，因为它同时决定「安全性」与「可通过性」。
三个几何量的关系（必须满足）：
inscribed_radius ：机器人内切圆半径（由 footprint 算出）
inflation_radius ：代价扩散半径，通常 = 2~3 × inscribed_radius
cost_scaling_factor：衰减指数，值越大代价下降越快
经验判据：
狭窄通道可通行条件：inflation_radius < 通道宽度/2 - 机器人半径
若环境有大量窄通道，只能减小 inflation_radius，
改由局部控制器的避障与安全裕度兜底
cost_scaling_factor 的效果（inflation_radius = 0.55 m）：
1.0 ：衰减极慢，距障碍 0.5m 处仍高代价 → 远离墙走
3.0 ：中等衰减，0.3m 处代价约 100 → 推荐起点
10.0 ：衰减极快，只有贴障处有代价 → 敢贴墙但易刮蹭
footprint 的配置有两种形式，选择会显著影响性能：
robot_radius: 0.35 # 圆形：碰撞检测最快
# 或精确多边形：长条形机器人必须用
footprint: "[[0.35,0.25],[0.35,-0.25],[-0.35,-0.25],[-0.35,0.25]]"
长条形机器人（如叉车、清洁车）必须用多边形，用圆形近似会导致「明明能过的地方规划失败」。但多边形足迹让碰撞检测成本上升数倍，一个折中是「全局规划用圆形近似、局部控制用精确多边形」。
6. 全局规划器与插件选型
全局规划器在全局代价地图上找一条从起点到目标的路径，输出路径点序列。
planner_server:
ros__parameters:
expected_planner_frequency: 1.0
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5 # 目标容差
use_astar: true # true 用 A*，false 用 Dijkstra
allow_unknown: true # 允许穿越未知区域
Nav2 内置全局规划器对比：
NavFn ：Dijkstra/A* 在代价栅格上搜索，快、成熟，不考虑运动学
Smac 2D ：状态格搜索，把朝向纳入状态，路径更可执行
Smac Hybrid-A* ：支持阿克曼与差速的运动学约束（最低转弯半径）
Smac State Lattice：用预计算运动原语，路径最平滑，适合阿克曼
Theta* ：任意角度路径，减少栅格锯齿
选型规则：
差速轮 + 通道宽 ：NavFn 够用，最快
差速轮 + 通道窄 ：Smac 2D，路径更贴运动学
阿克曼/车式 ：Smac Hybrid-A* 或 State Lattice（必需）
allow_unknown 是最容易踩坑的参数。设为 true 时规划器愿意穿越未知区域，在建图不完整的环境里能出路径，但可能一头扎进实际存在的障碍；设为 false 时路径只走已知安全区，更保守但可能无解。建图阶段设 true，运行阶段设 false 是常见策略。tolerance 决定「多接近目标算到达」，太小（0.1 m）时若目标点在障碍膨胀区内会永远规划失败，太大（1.0 m）时机器人停在离目标很远的地方，经验值是机器人半径的量级。
7. 局部控制器与速度平滑
局部控制器以高频（10~20 Hz）跟踪全局路径并输出速度指令，是避障的最后一道防线。
controller_server:
ros__parameters:
controller_frequency: 20.0
failure_tolerance: 0.3 # 超过 0.3s 无有效控制则失败
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05 # 预测步长 50ms
batch_size: 2000 # 每周期采样轨迹数
vx_max: 0.8
wz_max: 1.9
motion_model: "DiffDrive"
局部控制器对比：
DWB ：DWA 的 ROS2 版，采样速度空间+评分，参数直观，调参量大
TEB ：时间弹性带，把路径当橡皮筋优化，适合阿克曼与窄通道
RPP ：纯追踪，简单、平滑、无避障
MPPI ：模型预测路径积分，采样+加权，避障与平滑俱佳，算力高
Graceful：优雅控制，适合全向移动
选型：差速+算力足用 MPPI（当前社区默认推荐）；阿克曼用 TEB 或 MPPI CarLike；
极简场景用 RPP（避障交给代价地图）
velocity_smoother 是独立于控制器的一层，对 cmd_vel 做限幅与低通滤波。它的价值是「把控制器的理想指令变成底盘能执行的指令」：限制最大加速度、限制速度变化率、统一不同频率的指令源。核心参数是 max_velocity（如 [0.8, 0.0, 1.9]，对应 vx/vy/wz）、max_accel、max_decel 与 feedback（OPEN_LOOP 或 CLOSED_LOOP）。不要用控制器内部的限幅替代 velocity_smoother：控制器限幅针对自身算法，smoother 是面向底盘的统一保护层。
8. 恢复行为、进度检查与目标检查
恢复行为是「正常路径走不通时的兜底」，是 Nav2 可靠性的关键。
behavior_server 内置恢复行为：
Spin ：原地旋转，用于重新观测、脱离局部极小
BackUp ：后退固定距离，用于脱离贴障状态
DriveOnHeading ：沿指定朝向直行一段
Wait ：原地等待，用于让行人先过
AssistedTeleop ：人工遥操作辅助脱困
进度检查器：若在 movement_time_allowance 秒内移动距离小于
required_movement_radius，判定为卡住并触发恢复。典型：0.5m / 10s
目标检查器：用 xy_goal_tolerance 与 yaw_goal_tolerance 判断到达，
到达后需在容差内稳定 stopped_velocity_tolerance 秒才确认
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.15
yaw_goal_tolerance: 0.15
stateful: true # 到达后保持，避免抖动
恢复行为的编排逻辑在行为树里而不在服务器里，这意味着「什么情况用什么恢复」是策略问题。常见编排是「清除代价地图 → 旋转 → 后退 → 等待 → 重新规划」，重试若干次后放弃并上报。恢复行为不能无限重试，RecoveryNode 的重试次数必须有限，否则机器人会永远转圈。另一个常见现场问题是恢复行为本身触发安全：BackUp 后退时若后方有人会撞上去，因此应配合后向传感器与碰撞监视器使用。
9. 生命周期、启动顺序与参数组织
Nav2 的服务器都是生命周期节点，由 lifecycle_manager 统一驱动，启动顺序有严格依赖。
推荐启动顺序与依赖：
1. map_server 激活（先有地图）
2. amcl 激活（发布 map->odom）
3. planner_server 激活（需全局代价地图）
4. controller_server 激活（需局部代价地图）
5. behavior_server 激活
6. bt_navigator 激活（最后，依赖以上全部）
7. waypoint_follower、smoother 等按需激活
lifecycle_manager:
ros__parameters:
autostart: true
node_names: ["map_server", "amcl", "planner_server",
"controller_server", "behavior_server", "bt_navigator"]
bond_timeout: 4.0
bond_timeout 是容易被忽视的可靠性机制：lifecycle_manager 与各服务器之间维持一个心跳（bond），若某服务器崩溃或卡死，bond 断开，manager 会尝试重启或触发降级。这个超时不能设太短，嵌入式平台启动慢时误判会导致反复重启。参数组织上强烈建议分层覆盖：nav2_bringup 的默认参数作为基线，产品参数用独立 YAML 在 launch 里覆盖，绝不直接改上游文件，这样升级 Nav2 版本时能清楚看到自己改了哪些值。
10. 过滤器：keepout、速度限制与安全监视
Nav2 的过滤器机制允许在地图上叠加「业务规则」，而不改算法代码。
代价地图过滤器（costmap_filter_info_server）：
Keepout Filter ：指定区域标记为不可通行（禁区、维修区）
Speed Limit Filter：指定区域限制最大速度（人行区、门口）
数据来源：掩膜图像（PGM/PNG）+ YAML 元数据
碰撞监视器（collision_monitor）：
独立于代价地图，用多边形区域 + 实时传感器数据判断碰撞风险，
可发布停止/减速指令，响应比代价地图快，是安全功能的正确落点
collision_monitor:
ros__parameters:
base_frame_id: "base_footprint"
cmd_vel_in_topic: "cmd_vel_smoothed"
cmd_vel_out_topic: "cmd_vel"
polygons: ["StopZone"]
StopZone:
type: "polygon"
points: "[[0.6,0.6],[0.6,-0.6],[-0.6,-0.6],[-0.6,0.6]]"
action_type: "stop"
slowdown_ratio: 0.3
collision_monitor 与代价地图的分工是「快与慢」：代价地图更新周期是 100 ms 量级，用于规划；collision_monitor 直接读传感器，响应在毫秒量级，用于最后的安全兜底。安全相关的停止逻辑应放在 collision_monitor，而不是指望局部规划器及时反应。Keepout 区域的掩膜要用与地图相同的分辨率与原点，否则区域会错位，元数据里的 origin、resolution、negate 三个字段必须与地图一致。
11. 与 SLAM 和 AMCL 的接口
Nav2 消费定位结果，定位的质量直接决定导航的质量，接口只有两处：/map 话题与 map → odom 变换。
TF 树（Nav2 期望的结构）：
map ──(定位发布)──> odom ──(里程计发布)──> base_link
map → odom ：由 amcl 或 SLAM 发布，代表「定位对里程计的修正」
odom → base_link：由底盘发布，连续但会漂移
分离的意义：局部平滑（odom）与全局准确（map）解耦，
定位跳变时只影响 map→odom，不会让局部控制突然抖动
两种定位模式：
建图模式：slam_toolbox 发布 map→odom 与 /map
导航模式：amcl 在已知地图上发布 map→odom，map_server 提供 /map
建图与导航不能同时开：两个节点都发布 map → odom 会导致 TF 冲突。切换的正确流程是「SLAM 保存地图 → 关闭 SLAM → 启动 map_server + AMCL」。
ros2 run nav2_map_server map_saver_cli -f /etc/robot/maps/factory \
--ros-args -p save_map_timeout:=10000.0
ros2 launch nav2_bringup localization_launch.py \
map:=/etc/robot/maps/factory.yaml use_sim_time:=false
地图更新是长期运行机器人的必修课。环境变化（货架移位、新增隔断）后旧地图会误导规划。做法有两条：定期重新建图并替换地图文件；或让 AMCL 在定位的同时用激光数据对静态地图做局部更新（这一路径见SLAM 与定位建图 ）。地图的版本与生效时间必须纳入管理，否则「用错地图版本」会成为难以复现的现场问题。
12. 仿真回归验证与现场调参
Nav2 的参数改动必须经过回归验证，否则改一处坏一处。
回归验证的最小场景集：
1. 空旷直行（验证基本跟踪与速度平滑）
2. 窄通道通过（验证膨胀半径与通道判据）
3. 动态障碍绕行（验证局部避障）
4. 目标不可达（验证恢复行为与放弃逻辑）
5. 定位跳变注入（验证鲁棒性）
6. 长时间运行（验证内存与地图一致性）
记录四个指标：到达率、到达时间、重规划次数、恢复触发次数
# 简化的回归断言：一次导航任务的成功与效率
def test_narrow_corridor_navigation():
nav = BasicNavigator()
nav.setInitialPose(start_pose)
nav.waitUntilNav2Active()
nav.goToPose(goal_pose)
assert nav.isTaskComplete(), "导航未完成"
assert nav.getResult().error_code == 0
assert stats.replans < 20 # 重规划次数在阈值内
assert stats.recoveries <= 2 # 恢复次数在阈值内
现场调参的顺序建议固定为：footprint → 膨胀 → 规划器容差 → 控制器速度与加速度 → 恢复行为阈值。这个顺序的原因是前面的决定「哪里能走」，后面的决定「怎么走」，先调后面的会掩盖前面的问题。回归验证在仿真里用重放数据做，把最小场景集自动化成 pytest 断言，每次改参都跑一遍。
ros2 topic echo /plan --once # 看全局路径是否合理
ros2 topic echo /local_costmap/costmap --once # 看局部代价地图
ros2 topic hz /cmd_vel # 控制频率是否达标
ros2 topic echo /behavior_server/transition_event # 恢复行为何时触发
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 全局规划器 | NavFn：快、差速轮够用 | Smac Hybrid-A*：阿克曼必需 |
| 局部控制器 | MPPI：平滑避障好、算力高 | DWB：参数直观、算力低 |
| 代价地图形状 | 圆形：快、适合圆机器人 | 多边形：精确、长条机器人必需 |
| 膨胀半径 | 大：安全、窄道过不去 | 小：能过窄道、靠局部避障兜底 |
| 未知区域 | 允许穿越：建图期能出路径 | 禁止穿越：运行期更安全 |
| 定位来源 | SLAM：建图期 | AMCL：导航期、需已有地图 |
| 安全兜底 | 局部规划器：集成度高 | collision_monitor：响应快、独立 |
核心原则：策略放行为树，算法放插件，安全放独立层。把任务逻辑写进行为树 XML，把规划/控制算法通过插件参数配置，把与安全相关的停止放在 collision_monitor。三层职责清晰，改一层不会意外影响另外两层。
常见坑清单
- footprint 用圆形近似长条机器人，明明能过的窄道规划失败——长条机器人必须用多边形足迹。
- 膨胀半径大于通道宽度一半，狭窄区域永远无解——按通道宽度定半径，余量交给局部避障。
allow_unknown运行期仍为 true，机器人扎进未知区域的真实障碍——建图期 true、运行期 false。- SLAM 与 AMCL 同时发布
map → odom，TF 树冲突——切换定位模式必须先关旧节点。 - 恢复行为无限重试，机器人原地转圈永不放弃——
RecoveryNode的重试次数必须有限。 progress_checker阈值过松，机器人卡住十几秒才触发恢复——按实际移动能力设定半径与时长。- keepout 掩膜分辨率与原地图不一致，禁区错位——掩膜的 origin/resolution 必须与地图相同。
- 只靠局部规划器做安全停止，响应不及时——安全停止用 collision_monitor。
- 直接用
cmd_vel不经 velocity_smoother，加速度超底盘能力——smoother 是统一保护层。 - 改参数不做回归，修好一个场景退化另一个——维护最小场景集并自动化断言。
小结
Nav2 的骨架是「一组生命周期服务器 + 行为树编排 + 两层代价地图」。理解数据流（谁依赖谁）与职责边界（策略、算法、安全三层）比记住参数更重要。行为树是产品化最需要改动的部分，代价地图与膨胀是现场调参最花时间的部分，定位接口是最容易被误判为 Nav2 问题的部分。
下一步建议沿两条线深入：定位侧深入 AMCL 与地图更新的细节，理解它们如何影响导航质量；仿真侧读机器人仿真 ，把回归验证搭起来。全局与局部规划算法本身见运动规划与轨迹优化 ，Nav2 只是这些算法的一个工程封装。
最后一句经验：任何「导航行为诡异」的问题，先看 TF 树与定位质量，再怀疑 Nav2 参数。九成的诡异行为根因在 map → odom 的跳变或地图过期，而不在规划器。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。