机器人 / 系统课时
调节 Goal Checker 到达阈值
调节 Goal Checker 到达阈值
机器人接近导航目标后,可能反复小幅移动却迟迟不报告成功,也可能距离目标还有一段距离就提前结束。本课使用 ROS 2 Jazzy 的 Nav2 SimpleGoalChecker,分别调整位置与朝向容差,通过终点附近的轨迹和导航结果判断阈值是否合适。
学习目标
- 理解位置容差、朝向容差和
stateful的到达判定机制。 - 在 Nav2 参数文件中配置 Goal Checker,并验证参数已经生效。
- 根据终点附近的运动表现,区分阈值过严、过宽和控制器跟踪问题。
理解位置与朝向的到达条件
Nav2 的 Controller Server 可以加载不同的 Goal Checker 插件。ROS 2 Jazzy 中,nav2_controller::SimpleGoalChecker 根据机器人当前位姿与目标位姿之间的平面距离和偏航角误差判断是否到达,不要求位置与朝向完全一致。
它主要使用以下参数:
| 参数 | 单位 | Jazzy 默认值 | 作用 |
|---|---|---|---|
xy_goal_tolerance |
m | 0.25 | 允许的平面位置误差半径 |
yaw_goal_tolerance |
rad | 0.25 | 允许的最小绝对偏航角误差 |
stateful |
bool | true | 首次满足位置条件后是否记住该结果 |
位置误差使用欧氏距离,而不是分别判断 X、Y 是否小于阈值。例如目标位置为 (2.00, 1.00),机器人当前位置为 (2.11, 1.06),两点距离约为 0.125 m。如果位置容差设置为 0.15 m,这个位置已经满足要求。
朝向误差使用两个航向角之间的最短角距离。设置 yaw_goal_tolerance: 0.20 表示允许约 ±11.5° 的偏差。机器人只有在位置条件与朝向条件都满足时,才会被 SimpleGoalChecker 判为到达。
stateful: true 时,一旦某次检查满足位置容差,插件就会记住位置已经到达,后续主要检查朝向。这样可以避免机器人原地旋转时因轻微定位漂移反复退出位置容差,但也意味着旋转期间位置明显漂移后,仍可能得到成功结果。设置为 false 则每次都重新检查位置和朝向。

在 Controller Server 中配置阈值
以 Gazebo Harmonic 中运行的 TurtleBot3 仿真为例,可以先尝试 0.15 m 的位置容差和 0.20 rad 的朝向容差。这是一组便于调试的起始值,并非所有底盘都适用。实际阈值还应考虑底盘尺寸、地图分辨率、定位抖动和任务精度。
打开启动 Nav2 时实际加载的 nav2_params.yaml,找到 controller_server.ros__parameters。以下是片段示例:把这些参数合并到已有的 Controller Server 配置中,保留原来的控制器、代价地图和进度检查器设置,不要重复定义同名 YAML 键。
controller_server:
ros__parameters:
goal_checker_plugins: ["goal_checker"]
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.20
stateful: true这里的 goal_checker 是插件实例名,nav2_controller::SimpleGoalChecker 才是插件类型。如果现有配置使用 general_goal_checker 等其他实例名,应保持 goal_checker_plugins 中的名称与下方配置块名称一致,并确认导航行为树或控制器动作选择的是该 Goal Checker。
调整时建议先固定一项,再改变另一项:
- 保持
yaw_goal_tolerance: 0.20,调整xy_goal_tolerance,观察机器人是否能稳定进入目标附近。 - 确认位置误差可接受后,再调整朝向容差,观察终点原地旋转是否能正常结束。
- 最后保持两项阈值不变,对比
stateful: true和stateful: false下的终点行为。
验证配置是否真正生效
保存参数文件后,使用项目现有的启动方式重新启动 Nav2,确认加载的是刚修改的参数文件。对于默认未使用命名空间的 Controller Server,在另一终端执行:
# 分别读取正在运行的 Goal Checker 的两个容差
ros2 param get /controller_server goal_checker.xy_goal_tolerance
ros2 param get /controller_server goal_checker.yaw_goal_tolerance预期输出:
Double value is: 0.15
Double value is: 0.2这两行分别对应两条命令。如果使用 ROS 命名空间,节点路径可能不是 /controller_server,应使用 ros2 node list 确认实际名称。
如果读出的值仍是 0.25,优先检查启动文件传入的参数路径、插件实例名以及 YAML 缩进。不要仅凭文件已经保存就认为配置生效。
参数确认后,在 RViz 中使用 Nav2 Goal 向距离机器人约 1.5~2 m 的空旷位置发送导航目标。先确保地图、定位、TF 和局部控制器正常工作,再观察终点附近的移动轨迹、最终朝向和导航是否返回成功。
对同一个目标位置重复测试三次,并尽量使用相近的初始位姿。建议记录以下数据:
| 记录项 | 示例结果 | 判定依据 |
|---|---|---|
| 目标位置误差 | 0.12 m | 与目标的平面距离 |
| 目标朝向误差 | 0.16 rad | 最短绝对偏航角误差 |
| 导航结果 | 成功 | Nav2 导航动作的最终状态 |
| 终点轨迹 | 接近后小角度旋转 | 是否反复前后修正 |
对于 stateful: true,成功瞬间的位置误差并不一定仍小于 0.15 m,因为位置条件可能已在之前的控制周期被锁存。因此,既要记录第一次进入容差范围的位置,也要检查最终停止位置。
根据终点轨迹修正阈值
使用相同地图、目标点和控制器,分别比较较严、适中和较宽的阈值,可以把 Goal Checker 的影响与其他导航问题区分开。
| 观察到的现象 | 可能原因 | 调整方向 |
|---|---|---|
| 终点附近来回微动,迟迟不成功 | 位置容差小于实际可稳定达到的精度 | 例如从 0.05 m 放宽到 0.15 m,重新测试 |
| 位置已经接近,但持续小角度旋转 | 朝向容差过严或角速度控制不稳定 | 例如从 0.05 rad 调到 0.20 rad,检查旋转表现 |
| 距离目标明显偏远就报告成功 | 位置容差过宽 | 逐步缩小 xy_goal_tolerance |
| 朝向明显不符但已成功 | 朝向容差过宽或选用了忽略朝向的检查器 | 检查插件类型并减小 yaw_goal_tolerance |
| 旋转时漂移,最后停在容差圈外 | stateful 锁存位置或底盘运动存在漂移 |
对比 stateful: false,检查定位与控制 |
例如设置 xy_goal_tolerance: 0.15、yaw_goal_tolerance: 0.20 时,如果机器人当前位置距离目标 0.18 m,即使角度误差只有 0.06 rad,首次位置检查仍然不能通过。相反,如果距离只有 0.08 m,但朝向误差达到 0.29 rad,也不能通过朝向检查。
需要注意,SimpleGoalChecker 不检查机器人是否已经完全停止。如果任务要求到达后速度也低于指定阈值,Jazzy 还提供 nav2_controller::StoppedGoalChecker;它增加了线速度和角速度的停止判定参数。只关心位置、不要求最终朝向的任务,则可以考虑 nav2_controller::PositionGoalChecker,但不能将它用于有明确朝向要求的停靠场景。
对于一般室内仿真导航,可以从 0.15 m、0.20 rad 开始调试。最终采用哪组数值,应由重复实验的到达成功率、最终位姿误差和实际任务要求共同决定;导航报告成功,只代表当前 Goal Checker 的条件得到满足。