G2 放球任务优化 · 优化 · 示范数据运动学加速

示范数据运动学加速 · 设计方案

目标:基于运动学和 G2 的实测机械能力,把慢速遥操示范重定时成接近可执行上限的轨迹,用于训练, 让 π0.5 直接学到更快的放球策略。本页是方案设计,尚未实现。

0. 结论

做法:保持示范的几何路径不变,只重新分配时间(time re-parameterization)。三条硬约束:
  1. 分段加速:停顿压缩、空中搬运加速;夹取与入筒两个接触段基本不加速。
  2. 上限来自实测:在 G2 上做系统辨识,得到可跟踪的速度 / 加速度包络,乘安全系数。
  3. 先真机回放再训练:加速后的动作序列先在 h7 开环回放,能稳定入筒才进训练集。
最大且最安全的收益来自压缩停顿与犹豫,其次才是加速运动本身。
示范 period(松开→松开)
8.37 s
P10 4.67 · 最快整条 ep ≈4.3 s
其中 松开→再抓
≈4.6 s
取球 / 对准 / 犹豫 / 停顿
真机 fp4_ep0 本场
6.85 s
抓→入桶 2.85 s · 8/8 一次成功
目标 period
≈4.5–5 s
逼近示范最快 episode

1. 现状与约束

1.1 示范实际用到的运动包络(30hz_full,平滑后)

右臂关节|v| p95|v| p99|a| p95|a| p99
R11.001.796.312.4
R20.651.294.18.3
R31.212.047.714.7
R40.991.946.513.8
R52.364.0415.323.5
R61.422.7110.719.4
R71.923.4613.021.5

单位 rad/s、rad/s²;单关节 max 可达 12–18 rad/s,为开局跳变等毛刺,不能当上限。

1.2 已知约束

2. 核心思路:同路径、重分配时间

设原始示范时间为路径参数 \(s\),新时间为 \(t\),求单调映射 \(s(t)\)。所有通道(动作、状态、视频帧)都用同一条映射, 几何路径不变,只改变「何时到达哪个点」。

原始 停顿 取球接近 夹取 搬运 入筒 停顿 ≈ 8.4 s 加速后 ×1.6 ×1.0 ×1.6 ×1.0 ≈ 4.5–5 s 停顿:压缩到 ~0.1 s 非接触:受 v/a/jerk 包络约束加速 接触:倍率 ≤1.1,保成功率 示意比例,非实测。
为什么抽帧是成立的:放球是准静态操作 —— 球在夹爪里、筒在左手里、桌面静止,世界状态只由机器人位姿决定。 所以新时刻 \(t\) 用原始时刻 \(s(t)\) 的真实相机帧,在几何上一致。接触段不加速,正好避开这个假设失效的地方。

3. 流水线

①
约束标定

SDK 限位 + h7 系统辨识:v/a/jerk 包络、跟踪模型、夹爪时间

②
分段

夹爪循环 + 速度,切出停顿 / 取球 / 夹取 / 搬运 / 入筒

③
路径清洗

样条 / 零相位低通,先去抖再加速

④
时间重参数化

分段倍率上限 + TOPP-RA 求 s(t)

⑤
重采样 30Hz

动作 / 状态 / 视频帧共用 s(t);只抽帧不插图

⑥
验证闸门

运动学 · 抖动 · 真机开环回放

⑦
数据集与训练

多档速度 + 速度条件化 prompt,从 30hz_89999 续训

⑧
部署与评测

倍速回 1.0、调 ZP 截止、固定协议 N≥10

① 约束标定:「理论上限」从哪来

  1. SDK 限位:部署代码已调用 robot.get_joint_limits(),确认是否带速度 / 加速度字段。
  2. 系统辨识(h7):每个关节做梯形 / 正弦扫频,逐级加 v、a,记录跟踪误差、超调、稳定时间。 以「跟踪误差 p95 < 0.05 rad 且无振荡」的最大值作为包络 \(v_{max}, a_{max}, j_{max}\)。
  3. 跟踪模型:用已有 control_trace 的指令 vs 实测,拟合「纯延迟 \(\tau_d\) + 一阶滞后 \(\tau_1\)」,供 ⑤ 合成 state。
  4. 夹爪:单独测最短闭合 / 张开时间、夹稳所需最短保持时间。
  5. 设计上限 = 实测包络 × 0.7–0.8。

② 分段(复用 FAST60 夹爪循环)

阶段判定倍率上限 k
停顿 / 犹豫双臂速度 <0.15 rad/s 持续 ≥0.3 s压缩到 ~0.1 s
取球接近(远段)松开后、距下次夹住 >0.8 s1.5–2.0
夹取窗口夹住前 0.8 s ~ 夹住后 0.3 s1.0–1.1
搬运夹住后 0.3 s ~ 松开前 0.6 s1.5–2.0
入筒窗口松开前 0.6 s ~ 松开后 0.3 s1.0–1.1
左臂持筒基本静止跟随同一时间映射

接触段不加速:夹取与插入的物理结果随速度变化,且决定成功率(示范一次成功 91%,真机已 97%)。

③ 路径清洗:先去抖,再加速

加速会把频率按 \(k\) 倍放大:示范里 2 Hz 的遥操抖动,1.5× 后变成 3 Hz,正好落进 hf3。因此:

④ 时间重参数化(核心算法)

  1. 按分段表得到速度上限曲线 \(\dot s(t) \le k_{seg}(s)\)。
  2. 在此上限下,用 TOPP-RA(toppra 库)求满足关节约束的最快 \(s(t)\):
    \(|q'(s)\,\dot s| \le v_{max}\),\(|q''(s)\,\dot s^2 + q'(s)\,\ddot s| \le a_{max}\)
  3. 对 \(\dot s\) 做 jerk 限制平滑;段间倍率过渡 ≥0.2 s,防止新增加加速度阶跃。
  4. 停顿段直接压到最短驻留(夹爪动作前后保留夹爪所需时间)。

输出:单调映射 \(s(t)\),后面所有通道共用。

⑤ 重采样到 30Hz(一致性关键)

通道做法注意
action · 手臂\(a_{new}(t)=\text{spline}(s(t))\)路径不变
action · 夹爪按 \(s(t)\) 映射,强制最短闭合 / 张开时长不得短于实测夹爪能力
视频帧取原始帧 \(\text{round}(30\,s(t))\),只抽帧不插值每帧都是真实拍到的位姿
state(推荐 B)action 过 ① 的跟踪模型合成保持「指令领先实测」的时间关系
state(A,简单)原 state 在 \(s(t)\) 处取值滞后按帧被压缩,模型会学到偏紧的跟随关系

视觉域差:原始画面是慢速拍摄、运动模糊少;训练时可按速度加轻度运动模糊增强。

⑥ 验证闸门(三道,缺一不可)

闸门判据不通过怎么办
运动学重定时后 v / a / jerk 全部在设计包络内降低该段倍率
抖动hf3 / hf7 / 换向率 ≤ 源数据(按频率换算)加强 ③ 平滑
真机开环回放抽 20–30 条在 h7 直接回放 action(不经模型):gap p95、是否入筒、每球耗时;成功率 ≥ 原始示范回放该倍率档不合格,回退一档

真机都放不进去的数据,模型学了也没用 —— 回放是这套方案的「物理真值」。

⑦ 数据集与训练

⑧ 部署调整与评测

指标当前基线(fp4_ep0 本场)目标
抓→入桶 mean2.85 s≤2.5 s
period(松开→松开)6.85 s≤5 s
一次成功率100%(8/8)≥95%
跟踪 gap p95 / hf7记录不劣于基线

4. 风险与对策

风险表现对策
动力学失真加速后球在爪内晃动 / 滑脱、筒被带偏接触段不加速;真机回放验证
超出跟踪能力gap 变大、甩动(同 1.2× 倍速现象)系统辨识包络 × 安全系数;分段倍率
抖动放大遥操低频抖被推到 3 Hz+先平滑再重定时;源数据用 clean_DG
state–action 时序错位模型学到不真实的跟随关系用跟踪模型合成 state(方案 B)
视觉域差训练图像比真机快速运动时更清晰按速度加运动模糊增强
停顿并非无意义部分停顿是在视觉对准只压缩夹取窗口外的停顿;保留最短驻留
部署滤波吃掉速度ZP 3 Hz 削掉快动作截止频率随速度上调

5. 分期落地

阶段内容风险预期收益
P1只压缩停顿 / 犹豫,运动不加速低 路径、速度都不变period 8.4 → ~6 s
P2系统辨识 + 搬运 / 取球远段 ~1.3×,真机回放验证中period → ~5 s
P3多档 + 速度条件化 + 跟踪模型合成 state + 运动模糊增强中高逼近示范最快 ep(~4.3 s)

6. 实现清单

模块来源说明
夹爪循环 / 静止段检测tools/make_fast_subset.py复用 grip_cycles、runs
写 LeRobot 数据集(真实文件)tools/materialize_keep_copy.py复用落盘与校验
重定时主程序tools/retime_demos.py(新增)分段 → 平滑 → s(t) → 重采样 → ffmpeg 抽帧重编码
系统辨识scripts/sysid_g2_arm.py(新增,h7)扫频运动 + 包络 / 跟踪模型拟合
开环回放robot_bridge 回放模式读重定时 action,记录 gap / 入筒
抽查页tools/fast_spotcheck_html.py原始 vs 加速 时间线 + 视频对照
# P1 示例(设计稿,待实现)
python3 tools/retime_demos.py \
  --src /data/badminton651/merged/30hz_full_fast60 \
  --dst /data/badminton651/merged/30hz_fast60_retime_p1 \
  --mode idle-compress --idle-speed 0.15 --idle-min-s 0.3 --dwell-s 0.1 \
  --protect-grasp-s 0.8,0.3 --protect-release-s 0.6,0.3

# P2:加速非接触段(需先有 sysid 包络)
python3 tools/retime_demos.py ... --mode topp \
  --limits results/sysid/g2_arm_envelope.json --safety 0.75 \
  --k-free 1.3 --k-contact 1.0