ROS 2 导航深层实践：TF 时序、代价地图与控制稳定性

1. 「规划器有问题」多半是估计链错了

真机上 80% 的「规划器有问题」复盘到最后，根因在 TF 时序、定位跳变或 costmap 与动力学不匹配——不是 Nav2 算法本身。会启动 Nav2 不等于会做导航。本文按数据流写：估计（TF/定位）→ 表征（costmap）→ 决策（BT/规划）→ 执行（控制器），每层有可核对验收。

2. map/odom 分工（REP-105）

text

map → odom → base_link → sensors

odom → base_link：可微、高频、局部一致，允许全局漂移。
map → odom：低频、全局修正（AMCL/SLAM），可以跳变。
控制器主要在 odom 下平滑执行；全局目标在 map 下解释。

反模式：控制器直接追 map 系下剧烈跳变的位姿 → 震荡急转；两个节点同时发 map→odom；TF 树出现多父节点。

3. TF 外推失败从哪来

tf2 在 t_query 需要两端变换，extrapolation into the future 常见根因：

传感器 stamp 用到达时间而非采集时间
时钟未同步（未用 use_sim_time / 硬件 PTP）
transform_tolerance 过紧，定位发布过慢
回放 bag 多时钟源

bash

ros2 run tf2_ros tf2_monitor
ros2 topic delay /scan

看 /scan header stamp 与 /tf 最新 stamp 的差。稳定系统通常数毫秒到十几毫秒；经常 > 100 ms，先修时间，别调膨胀半径。

4. 建图与导航必须分阶段

在线 SLAM 持续改图时跑 Nav2：costmap 基于移动的墙、回环瞬间路径失效。推荐状态机：

MAPPING：slam_toolbox online_async + teleop，禁 Nav2
SERIALIZE：保存 pose-graph / map.pgm
LOCALIZATION：AMCL 或 slam_toolbox localization
NAVIGATION：Nav2 全开

地图质量检查：长直走廊是否弯（里程计尺度）；回环处是否重影；动态物体是否写成永久墙。

5. Costmap 与控制窗口

全局/局部分工：global 管长距离拓扑，local 管实时避障（更新 ≥ 控制频率）。局部滚动窗口半径应大于制动距离 $d_{stop} \approx v^{2} / (2 a_{brake})$ ——高速下窗口太小，障碍刚进就来不及减速。

膨胀与 footprint 同审：有效通过宽度 ≈ 门宽 − 2×inflation_radius − footprint 裕量。cost_scaling_factor 过陡窄道抖振，过缓贴墙擦碰。

6. 控制器是带延迟的闭环

链路延迟 = 感知 + costmap 更新 + 控制器计算 + 通信 + 电机响应。延迟过大，避障会「绕到障碍物里」。差速车选 DWB/MPPI/RPP 等看约束，lookahead 经验 $L \approx \clamp (k \cdot v, L_{m i n}, L_{m a x})$ ，并与 $ω_{m a x} \cdot Δ t$ 联立检查单步转角可行性。仿真增益不可照搬真机——延迟与滑移不同。

7. BT 与定位门禁

恢复行为须有次数与时间上限；定位置信低时进 relocalization，不是无限 FollowPath。发导航目标前建议软件门禁：AMCL 协方差/匹配分数、map→odom 跳变量。低于阈值拒绝接单。

8. 验收指标与故障树

指标	室内服务示例
到点误差	< 0.15 m, < 10°
窄门通过率	> 95%（50 次）
定位丢失恢复	< 5 s

高频故障树：路径抖动 → 定位跳变 / costmap 噪声 / 增益；穿墙 → footprint 偏小 / 膨胀不足；TF 报错 → stamp 错 / 发布频率低 / 多父节点。A/B 实验只改一参数，用同一 bag/路线复测。

9. 与 SLAM 研究的接口

SLAM 节点只发布 map→odom + 地图；odom→base_link 走标准 nav_msgs/Odometry。算法核心与 ROS 解耦；评测同时记录 ATE（定位）与任务成功率（导航）——二者不可互相替代。

8. track_unknown 与全局规划

allow_unknown: true 时 planner 可穿越未知格——适合探索，但可能把机器人导向未建图区域。false 时未知当障碍，窄道旁未知区过大导致无解。与 SLAM 阶段对齐：定位模式用 false，已知地图调试可临时 true。改此参数必须附全局路径截图，不是只报成功率。

9. AMCL 与窄廊粒子退化

AMCL 在长直窄廊容易粒子退化——update_min_d 过大时，激光匹配分缓慢下降，直到某次更新 map→odom 跳变。症状：路径突然偏 90°、控制器急转。门禁：匹配分连续 N 帧低于阈值则暂停导航；recovery_alpha_fast/slow 与 laser_max_range 按廊道宽度标定，不是默认值上车。

10. 延迟预算与控制相位

把控制回路看成带延迟的闭环：感知 stamp 偏差 + costmap 更新周期 + planner/controller 计算 + DDS 传输 + 电机响应。任一段过大，等价于在「过去的世界」里做决策。用 ros2 topic delay 量每段，总和超过控制周期一半时，先减延迟再调增益。

bash

ros2 topic delay /scan
ros2 topic hz /local_costmap/costmap
ros2 topic hz /cmd_vel

窄道通过时，local costmap 更新频率应 ≥ controller 频率；否则障碍进窗口时 map 还是旧格。

11. CollisionMonitor 与安全层

Nav2 Humble+ 的 CollisionMonitor 在 controller 输出前做区带检测（stop/slow/limit polygon）。与 progress checker 分工：progress 管卡死，collision 管危险几何。安全停最终应能绕过 BT 直接切断 /cmd_vel——软件急停链路 < 100 ms 应写进验收。

12. 窄门与定位门禁的联动

窄廊 AMCL 粒子退化时，map→odom 会跳变——控制器追跳变位姿画圈。门禁：协方差超阈或激光匹配分低于阈值时，拒绝新目标，触发 relocalization。回环后短暂抑制高速（max_vel_x 限幅 30 s），等协方差收敛再放开。

调参之前先测量延迟与定位质量；否则所有参数优化都在拟合错误的时间与坐标基础。