18. SimpleProgressChecker

归属 §3.5 SimpleProgressChecker · Autonomy ✅ 已实现（movement_time_allowance 待接）

SimpleProgressChecker 检测机器人在 FollowPath 中是否产生足够 XY 位移；无位移则返回 false，累计后 Navigator 抛出 FailedToMakeProgress，触发 Recovery。

1. 背景

局部控制器可能因障碍、错误 lookahead 或定位异常而原地振荡或卡住，Goal Checker 长期 false 却难以区分「仍在努力」与「已失效」。Progress Checker 维护基线位姿，若位移低于阈值则判为无进度，与 Goal Checker 并行运行（语义相反：true = 有进度）。

2. 问题

任务. 每周期判断自上次基线以来是否移动足够远。

输入 / 输出. Check(current_pose) → bool；true = 有进度，false = 无进度。

在线形式. 首次调用或检测到足够位移时更新基线 \((x_b,y_b)\)；Autonomy 当前未实现 Nav2 的 movement_time_allowance 时间窗口（proto 已预留）。

3. 位姿与位移模型

当前位姿. \(p=(x,y,\theta)^\top\)；基线 \(p_b=(x_b,y_b,\theta_b)^\top\)（仅 XY 参与 Simple 判定）。

平面位移.

\[ d_{xy} = \mathrm{hypot}(x - x_b,\; y - y_b). \]

  • \(r\)：required_movement_radius（默认 0.5 m）。

4. 数学问题定义

有进度条件.

\[ d_{xy} > r \quad \Longrightarrow \quad \text{更新基线，返回 true}. \]

无进度. \(d_{xy} \leq r\) 且非首次调用 → 返回 false。

Nav2 完整语义还包括：在 \(T_{allow}\)（movement_time_allowance）内必须至少一次满足 \(d_{xy}>r\)，否则失败。Autonomy C++ 待实现该计时逻辑。

5. 判定算法

算法 1 SimpleProgressChecker::Check

  1. 若 首次调用（baseline_pose_set_ 为 false）→ ResetBaselinePose(\(p\)) → 返回 true

  2. \(d_{xy} \leftarrow \mathrm{hypot}(x-x_b, y-y_b)\)

  3. 若 \(d_{xy} > r\) → ResetBaselinePose(\(p\)) → 返回 true

  4. 否则 返回 false（无进度）

原地大角度旋转而 XY 几乎不变时，Simple 会误判无进度 → 改用 §19 PoseProgressChecker。FollowPath 集成时序见 §3.9。

6. 参考文献

  1. Navigation2 Controller Server — Progress Checker plugins: configuring-controller-server