示范数据运动学加速 · 设计方案
目标:基于运动学和 G2 的实测机械能力,把慢速遥操示范重定时成接近可执行上限的轨迹,用于训练, 让 π0.5 直接学到更快的放球策略。本页是方案设计,尚未实现。
0. 结论
做法:保持示范的几何路径不变,只重新分配时间(time re-parameterization)。三条硬约束:
- 分段加速:停顿压缩、空中搬运加速;夹取与入筒两个接触段基本不加速。
- 上限来自实测:在 G2 上做系统辨识,得到可跟踪的速度 / 加速度包络,乘安全系数。
- 先真机回放再训练:加速后的动作序列先在 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 |
|---|---|---|---|---|
| R1 | 1.00 | 1.79 | 6.3 | 12.4 |
| R2 | 0.65 | 1.29 | 4.1 | 8.3 |
| R3 | 1.21 | 2.04 | 7.7 | 14.7 |
| R4 | 0.99 | 1.94 | 6.5 | 13.8 |
| R5 | 2.36 | 4.04 | 15.3 | 23.5 |
| R6 | 1.42 | 2.71 | 10.7 | 19.4 |
| R7 | 1.92 | 3.46 | 13.0 | 21.5 |
单位 rad/s、rad/s²;单关节 max 可达 12–18 rad/s,为开局跳变等毛刺,不能当上限。
1.2 已知约束
- 仓库里没有 G2 URDF,也没有关节速度 / 加速度上限;部署侧已去掉限速,只保留位置限位与健康检查。
- 部署端统一倍速
execution_speed1.2× 时 gap p95 0.028→0.254、出现甩动,被禁用 —— 统一加速存在硬天花板,必须分段并以实测跟踪能力为界。 - 示范每球:抓→入桶 mean 3.75 s,松开→再抓 ≈4.6 s。耗时大头在取球与停顿,不在搬运。
2. 核心思路:同路径、重分配时间
设原始示范时间为路径参数 \(s\),新时间为 \(t\),求单调映射 \(s(t)\)。所有通道(动作、状态、视频帧)都用同一条映射, 几何路径不变,只改变「何时到达哪个点」。
为什么抽帧是成立的:放球是准静态操作 —— 球在夹爪里、筒在左手里、桌面静止,世界状态只由机器人位姿决定。
所以新时刻 \(t\) 用原始时刻 \(s(t)\) 的真实相机帧,在几何上一致。接触段不加速,正好避开这个假设失效的地方。
3. 流水线
①
约束标定
SDK 限位 + h7 系统辨识:v/a/jerk 包络、跟踪模型、夹爪时间
②
分段
夹爪循环 + 速度,切出停顿 / 取球 / 夹取 / 搬运 / 入筒
③
路径清洗
样条 / 零相位低通,先去抖再加速
④
时间重参数化
分段倍率上限 + TOPP-RA 求 s(t)
⑤
重采样 30Hz
动作 / 状态 / 视频帧共用 s(t);只抽帧不插图
⑥
验证闸门
运动学 · 抖动 · 真机开环回放
⑦
数据集与训练
多档速度 + 速度条件化 prompt,从 30hz_89999 续训
⑧
部署与评测
倍速回 1.0、调 ZP 截止、固定协议 N≥10
① 约束标定:「理论上限」从哪来
- SDK 限位:部署代码已调用
robot.get_joint_limits(),确认是否带速度 / 加速度字段。 - 系统辨识(h7):每个关节做梯形 / 正弦扫频,逐级加 v、a,记录跟踪误差、超调、稳定时间。 以「跟踪误差 p95 < 0.05 rad 且无振荡」的最大值作为包络 \(v_{max}, a_{max}, j_{max}\)。
- 跟踪模型:用已有
control_trace的指令 vs 实测,拟合「纯延迟 \(\tau_d\) + 一阶滞后 \(\tau_1\)」,供 ⑤ 合成 state。 - 夹爪:单独测最短闭合 / 张开时间、夹稳所需最短保持时间。
- 设计上限 = 实测包络 × 0.7–0.8。
② 分段(复用 FAST60 夹爪循环)
| 阶段 | 判定 | 倍率上限 k |
|---|---|---|
| 停顿 / 犹豫 | 双臂速度 <0.15 rad/s 持续 ≥0.3 s | 压缩到 ~0.1 s |
| 取球接近(远段) | 松开后、距下次夹住 >0.8 s | 1.5–2.0 |
| 夹取窗口 | 夹住前 0.8 s ~ 夹住后 0.3 s | 1.0–1.1 |
| 搬运 | 夹住后 0.3 s ~ 松开前 0.6 s | 1.5–2.0 |
| 入筒窗口 | 松开前 0.6 s ~ 松开后 0.3 s | 1.0–1.1 |
| 左臂持筒 | 基本静止 | 跟随同一时间映射 |
接触段不加速:夹取与插入的物理结果随速度变化,且决定成功率(示范一次成功 91%,真机已 97%)。
③ 路径清洗:先去抖,再加速
加速会把频率按 \(k\) 倍放大:示范里 2 Hz 的遥操抖动,1.5× 后变成 3 Hz,正好落进 hf3。因此:
④ 时间重参数化(核心算法)
- 按分段表得到速度上限曲线 \(\dot s(t) \le k_{seg}(s)\)。
- 在此上限下,用 TOPP-RA(
toppra库)求满足关节约束的最快 \(s(t)\):
\(|q'(s)\,\dot s| \le v_{max}\),\(|q''(s)\,\dot s^2 + q'(s)\,\ddot s| \le a_{max}\) - 对 \(\dot s\) 做 jerk 限制平滑;段间倍率过渡 ≥0.2 s,防止新增加加速度阶跃。
- 停顿段直接压到最短驻留(夹爪动作前后保留夹爪所需时间)。
输出:单调映射 \(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、是否入筒、每球耗时;成功率 ≥ 原始示范回放 | 该倍率档不合格,回退一档 |
真机都放不进去的数据,模型学了也没用 —— 回放是这套方案的「物理真值」。
⑦ 数据集与训练
- 分档:1.0×(原始)· ~1.3× · ~1.6× · 实测上限档。
- 速度条件化:prompt 末尾加标签,如
speed: fast,部署可切换、可回退;或只用快档微调。 - 数据量补偿:加速后每条帧数变少(1.6× 约少 37%),用采样权重或多档混合补足。
- 训练:从
30hz_89999续训;norm stats 按新集重算(位置分位基本不变,确认无退化维);action_horizon仍 30(1 s),加速后一个 chunk 覆盖更多任务进度。
⑧ 部署调整与评测
- 不叠加倍速:速度已学进模型,
execution_speed回到 1.0。 - ZP 低通截止上调:3 Hz 对加速轨迹偏保守,按 \(3k\) Hz 或扫 3/4/5 Hz 重选,否则「学快了但执行不快」。
- 延迟预算:动作变快后 114 ms 延迟对应位置误差变大;先 fp4_ep0 4+4 + pa,gap 变大再试 3+3 / ds8。
- 固定评测协议:每档 ≥10 球,口径同 真机抓球汇总。
| 指标 | 当前基线(fp4_ep0 本场) | 目标 |
|---|---|---|
| 抓→入桶 mean | 2.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