机器人 / 系统课时

用误差指标评估导航任务结果

用误差指标评估导航任务结果

导航任务结束后,RViz 显示到达并不等于仿真验收通过。本篇把 Nav2 动作状态、终点位置误差和机器人到障碍物的最小净间距整理为逐次试验记录,计算可复测的成功率,并输出验收报告。

学习目标

  • 区分 Nav2 动作成功率与同时满足精度、安全条件的任务通过率。
  • 正确计算终点二维误差和全程最小障碍净间距。
  • 用同一组 CSV 数据重复运行评估脚本,得到可追溯的验收结果。

先固定指标与门槛

把一次 NavigateToPose 目标执行定义为一次试验:从发出目标开始,到动作结束或达到 120 s 截止时间为止。截止仍未结束时先取消导航,并在本次记录中标为 TIMEOUT;TIMEOUT 是评测标签,不是 ROS 2 的动作状态常量。

  • Nav2 动作成功率:SUCCEEDED 次数除以全部试验次数;ABORTED、CANCELED、TIMEOUT 都计入分母。
  • 终点位置误差:在统一的 map 坐标系下,exy=(xf−xg)2+(yf−yg)2e_{xy}=\sqrt{(x_f-x_g)^2+(y_f-y_g)^2}。例如目标为 (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 环境里,先检查话题,再为第一轮录制证据:

bash
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 是该轮全程的最小净间距,单位均为米。

csv
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.18

R03 虽然动作成功、误差约 0.13 m,却因净间距只有 0.09 m 而未通过;R04 的动作失败也必须保留在分母中。实际采集时,缺少终点真值或障碍几何数据的试验不可被默认为通过,应补采并说明原因。

运行评估并保存结果

把以下完整 Python 3 脚本保存为 evaluate.py,与 trials.csv 放在同一目录。它不依赖第三方库,分别计算两种通过率,并将阈值和每轮判定写入 report.json。

python
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")

执行:

bash
python3 evaluate.py

预期输出:

text
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 和地图及参数版本,才能在调参后判定性能是否确实改善。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用误差指标评估导航任务结果

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通