17. StoppedGoalChecker

归属 §3.4 StoppedGoalChecker · Autonomy ✅ 已实现

StoppedGoalChecker 继承 SimpleGoalChecker：在 XY + 航向达标后，还要求线速度与角速度低于停止阈值，用于充电对接、电梯等须停稳才算到达的场景。

---

1. 背景

仅位姿进入容差时，机器人仍可能因惯性或控制器输出非零速度而微动。对接类任务要求「几何到位 且 静止」。Nav2 StoppedGoalChecker 在 Simple 判定之上叠加速度门限，常与 §20 VelocitySmoother 联用以更快满足停止条件。

---

2. 问题

任务. 判定位姿到达且底盘速度接近零。

输入 / 输出. IsGoalReached(query, goal, velocity) → bool；必须使用 velocity 中的 \(v_x,v_y,\omega_z\)。

在线形式. 每周期：先位姿（含 stateful XY 锁定），再速度；全部满足才返回 true。

---

3. 位姿与速度模型

位姿误差. 与 §15 SimpleGoalChecker 相同：\(d_{xy}\)、\(\Delta\theta=\mathrm{AngleDiff}(\theta,\theta_g)\)。

速度量.

\[ v_{trans} = \sqrt{v_x^2 + v_y^2}, \qquad \omega = \omega_z. \]

• \(v_{trans}^{stop}\)：线速度停止阈值（默认 0.25 m/s）

• \(\omega_{rot}^{stop}\)：角速度停止阈值（默认 0.25 rad/s）

---

4. 数学问题定义

位姿到达 \(G_p\). SimpleGoalChecker 语义（XY + yaw，含 stateful）为真。

速度停止 \(G_v\).

\[ v_{trans} \leq v_{trans}^{stop}, \qquad |\omega| \leq \omega_{rot}^{stop}. \]

总到达条件.

\[ \text{reached} \Leftrightarrow G_p \land G_v. \]

时空联合控制器（TEB/NMPC）规划的速度剖面在终点应趋于零；若仅几何跟踪，须依赖 Smoother 或控制器减速区。

---

5. 判定算法

算法 1 StoppedGoalChecker::IsGoalReached

1. 若 SimpleGoalChecker::IsGoalReached(\(q,g,v\)) 为 false → 返回 false

2. \(v_{trans} \leftarrow \mathrm{hypot}(v_x, v_y)\)

3. 若 \(v_{trans} \leq v_{trans}^{stop}\) ∧ \(|\omega_z| \leq \omega_{rot}^{stop}\) → 返回 true

4. 否则 返回 false

GetTolerances 在 Simple 基础上写入线/角速度容差。精密对接可收紧至 \(0.05\) m/s 量级；见 §6.14 选型矩阵 中 StoppedGoalChecker 行。

---

6. 参考文献

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