机器人 / 系统课时
用误差指标评估导航任务结果
用误差指标评估导航任务结果
导航任务结束后,RViz 显示到达并不等于仿真验收通过。本篇把 Nav2 动作状态、终点位置误差和机器人到障碍物的最小净间距整理为逐次试验记录,计算可复测的成功率,并输出验收报告。
学习目标
- 区分 Nav2 动作成功率与同时满足精度、安全条件的任务通过率。
- 正确计算终点二维误差和全程最小障碍净间距。
- 用同一组 CSV 数据重复运行评估脚本,得到可追溯的验收结果。
先固定指标与门槛
把一次 NavigateToPose 目标执行定义为一次试验:从发出目标开始,到动作结束或达到 120 s 截止时间为止。截止仍未结束时先取消导航,并在本次记录中标为 TIMEOUT;TIMEOUT 是评测标签,不是 ROS 2 的动作状态常量。
- Nav2 动作成功率:
SUCCEEDED次数除以全部试验次数;ABORTED、CANCELED、TIMEOUT都计入分母。 - 终点位置误差:在统一的
map坐标系下,。例如目标为(2.00, 1.00),终点为(2.06, 1.08),误差为 0.10 m。 - 全程最小净间距:导航期间每一时刻,机器人真实足迹边界到最近障碍物边界的距离,再取全程最小值。发生接触按 0 m 记;不能直接把激光雷达的最小量测距离当作底盘净间距。
- 综合通过率:动作为
SUCCEEDED、终点误差不超过 0.25 m、全程净间距不小于 0.15 m 的试验数,除以全部试验数。
以上是本例的工程验收门槛,不是 Nav2 强制标准。使用 SimpleGoalChecker 时还应核对实际的 xy_goal_tolerance 和 yaw_goal_tolerance,不能用动作成功取代独立精度检查;参数可参考 Nav2 Jazzy 配置说明。
在 Gazebo 记录同一轮试验
测试前固定世界文件、地图、底盘足迹、Nav2 参数、起终点和动态障碍物脚本;每轮恢复相同初始位姿并重新初始化定位。建议先做 5 轮验证评估流程,再做至少 30 轮正式统计,并记录使用的随机种子。
在已启动定位、Nav2 和仿真时间的 ROS 2 Jazzy 环境里,先检查话题,再为第一轮录制证据:
ros2 topic list
ros2 bag record -o nav_eval_01 --topics /tf /tf_static /amcl_pose /scan /clock预期结果:话题列表包含当前系统实际发布的相关话题;录制期间出现订阅状态,按 Ctrl+C 结束后生成 nav_eval_01/metadata.yaml 和数据文件。/amcl_pose 仅在使用 AMCL 且已发布该话题时适用;下一轮换用 nav_eval_02 等独立目录。
每轮还要记录 NavigateToPose 的最终状态与目标坐标。终点坐标优先采用 Gazebo 真值位姿转换到 map 坐标系 后的结果,以检验整体导航精度;用 map -> base_link 的定位估计值则属于另一种评估口径,不能与真值混用。净间距需从仿真机器人足迹和障碍物几何位置逐帧计算,保留最小值;上述 rosbag 命令本身不会自动生成该数值或试验 CSV。

用五次试验构造可复算样本
将以下示例存为 UTF-8 编码的 trials.csv。每行是一轮试验,终点是在动作终止或超时瞬间采集的位置,min_clearance_m 是该轮全程的最小净间距,单位均为米。
trial_id,status,goal_x,goal_y,end_x,end_y,min_clearance_m
R01,SUCCEEDED,2.00,1.00,2.06,1.08,0.20
R02,SUCCEEDED,3.00,0.00,3.12,0.09,0.17
R03,SUCCEEDED,1.00,-1.00,1.05,-1.12,0.09
R04,ABORTED,2.00,2.00,1.40,2.20,0.08
R05,SUCCEEDED,0.00,3.00,-0.12,3.05,0.18R03 虽然动作成功、误差约 0.13 m,却因净间距只有 0.09 m 而未通过;R04 的动作失败也必须保留在分母中。实际采集时,缺少终点真值或障碍几何数据的试验不可被默认为通过,应补采并说明原因。
运行评估并保存结果
把以下完整 Python 3 脚本保存为 evaluate.py,与 trials.csv 放在同一目录。它不依赖第三方库,分别计算两种通过率,并将阈值和每轮判定写入 report.json。
import csv
import json
import math
from pathlib import Path
from statistics import mean
MAX_ERROR_M = 0.25
MIN_CLEARANCE_M = 0.15
MIN_PASS_RATE = 0.95
with Path("trials.csv").open(newline="", encoding="utf-8") as file:
trials = list(csv.DictReader(file))
if not trials:
raise SystemExit("trials.csv 没有试验数据")
evaluated = []
success_errors = []
for trial in trials:
status = trial["status"].strip().upper()
if status not in {"SUCCEEDED", "ABORTED", "CANCELED", "TIMEOUT"}:
raise ValueError(f"未知状态: {status}")
gx, gy, fx, fy, clearance = (
float(trial[key]) for key in
("goal_x", "goal_y", "end_x", "end_y", "min_clearance_m")
)
if not all(math.isfinite(v) for v in (gx, gy, fx, fy, clearance)):
raise ValueError(f"存在缺失或非有限值: {trial['trial_id']}")
error = math.hypot(fx - gx, fy - gy)
succeeded = status == "SUCCEEDED"
# 动作成功仍需独立检查终点精度和全程安全间距
accepted = succeeded and error <= MAX_ERROR_M and clearance >= MIN_CLEARANCE_M
if succeeded:
success_errors.append(error)
evaluated.append({
"trial_id": trial["trial_id"],
"status": status,
"position_error_m": round(error, 3),
"min_clearance_m": clearance,
"accepted": accepted,
})
count = len(evaluated)
nav_ok = sum(r["status"] == "SUCCEEDED" for r in evaluated)
task_ok = sum(r["accepted"] for r in evaluated)
nav_rate = nav_ok / count
pass_rate = task_ok / count
minimum_clearance = min(r["min_clearance_m"] for r in evaluated)
avg_error = mean(success_errors) if success_errors else None
# 安全底线单独约束,不能被较高的平均成功率掩盖
passed = pass_rate >= MIN_PASS_RATE and minimum_clearance >= MIN_CLEARANCE_M
report = {
"thresholds": {
"max_position_error_m": MAX_ERROR_M,
"min_clearance_m": MIN_CLEARANCE_M,
"min_task_pass_rate": MIN_PASS_RATE,
},
"trials": evaluated,
"summary": {
"nav_success_rate": nav_rate,
"task_pass_rate": pass_rate,
"mean_success_position_error_m": avg_error,
"minimum_clearance_m": minimum_clearance,
"accepted": passed,
},
}
Path("report.json").write_text(
json.dumps(report, ensure_ascii=False, indent=2) + "\n",
encoding="utf-8",
)
print(f"Nav2 动作成功率: {nav_ok}/{count} ({nav_rate:.1%})")
print(f"综合通过率: {task_ok}/{count} ({pass_rate:.1%})")
print(f"成功动作平均终点误差: {avg_error:.3f} m" if avg_error is not None
else "成功动作平均终点误差: 无")
print(f"全程最小净间距: {minimum_clearance:.3f} m")
print(f"验收结论: {'通过' if passed else '未通过'}")
print("报告已写入: report.json")执行:
python3 evaluate.py预期输出:
Nav2 动作成功率: 4/5 (80.0%)
综合通过率: 3/5 (60.0%)
成功动作平均终点误差: 0.128 m
全程最小净间距: 0.080 m
验收结论: 未通过
报告已写入: report.json根据失败指标定位问题
| 观测现象 | 优先核查 | 可验证的修正 |
|---|---|---|
ABORTED 多,终点误差大 |
TF 连通性、定位跳变、规划失败原因 | 修复后以相同起终点再跑 30 轮 |
| 动作成功但终点偏差大 | 真值与 map 的坐标变换、定位误差、目标容差 |
校准坐标变换并复算误差,不直接放宽门槛 |
| 净间距低于 0.15 m | 足迹尺寸、局部代价图障碍层与膨胀参数 | 更新真实足迹后重放相同避障场景 |
正式验收可约定 至少 30 轮、综合通过率不低于 95%、任何一轮都不得低于 0.15 m 安全净间距。30 轮中至少需要 29 轮综合通过;但只要发生一次低于安全底线的事件,本次验收仍不通过。保留每轮 bag、试验 CSV、report.json 和地图及参数版本,才能在调参后判定性能是否确实改善。