Files
Robot_Arm/docs/Transformer轨迹拟合训练方案.md
T

532 lines
40 KiB
Markdown
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# Transformer 避障运动策略:无老师闭环部署方案(审查稿 v2.2)
> - v1(2026-10-17 20:06,`40af3b4`):模型一次生成整条 32 点路径,失败回落到原规划器("有老师部署")。
> - v1.1(20:25,`c51f8f9`):加入后训练挖错循环。
> - **v2(本稿)**:按用户决定改为 **无老师部署 + 每个控制周期重算(闭环)**,删除全部"回落原规划器"设计;
> 加入具身智能领域主流做法的调研与对比(§1–§2),并据此调整模型输出、后训练方法和验收标准。
> - v2.1(2026-10-17 20:58):训练机定为 RTX 3090 24 GB;near_column 验收改为 **30/30**,新增 §8.5「为什么会超时、无老师怎样做到 30/30」;新增 §8.6 控制周期业界调研与评估。
> - v2.2(2026-10-17 21:10):调研 2024–2026 前沿生成式策略(流匹配/MeanFlow 一步生成、实时动作块 RTC、代价引导扩散规划、流策略强化学习微调、π0.5/FAST 等),逐项判断是否适用,新增 §2.4,并据此把动作头升级为**一步流匹配头 + 碰撞代价引导 + 前缀条件**(§6.1A)。
> - 基于 `main@3c7c814`(贴立柱修复后的规划器)。本文只是方案,代码尚未编写。
> - 审查重点:§0 定位、§2 差异与优化、§5 输出表示、§7 后训练(APG)、§8 闭环部署与安全层、§9 验收。
---
## 0. 定位与边界
| 项目 | 决定 |
|---|---|
| 部署形态 | **无老师**:运行时只有「神经策略 + 碰撞安全层」,不加载 IK 大表、PRM 路网、A*/RRT,也不调用 `solveDyn` / `routeRT` |
| 运行方式 | **闭环滚动重算**(MPC 式):每个控制周期(默认 50 Hz,20 ms)读当前关节角和目标,重新推理一段短动作块,只执行最前面一小段,下一周期再算。某一周期算错,下一周期有机会纠正 |
| 碰撞安全层 | **保留**。它不是老师:不做搜索,只对模型给出的下一小段做胶囊碰撞检查(已有 `collideFree` + 包围球预筛),不安全就换候选或刹停 |
| 老师的角色 | 只在**离线训练**时出现:生成预训练数据。后训练阶段用可微碰撞代价(§7),基本不再需要老师 |
| 不做的事 | 不学动力学(速度/加速度上限由插补器保证);不用相机/点云(环境固定:台架 + P01 立柱,几何已知) |
| 收益目标 | 冷启动(新目标到第一个动作)< 10 ms;每周期推理 + 安全检查 < 10 ms;拖拽重规划 P99(现状约 321 ms)不再存在 |
| 底线 | 实时拖拽**碰撞帧 = 0**(安全层保证);到达率、行程在 §9 的指标上不低于现有规划器 |
为什么安全层必须保留:调研里最好的神经运动策略在静态场景里仍有约 10% 的轨迹发生碰撞(§1 DRP 附录表 6、ELMP 表 I)。
我们的环境几何完全已知、检查只要几毫秒,保留它代价极小,却把"偶尔撞一下"变成"偶尔停一下"。
---
## 1. 业界调研:具身智能里的神经运动规划
以下数据均来自论文原文(链接见 §13)。
| 工作 | 年份 / 会议 | 输入 | 输出 | 老师与数据量 | 关键做法 | 运行方式 |
|---|---|---|---|---|---|---|
| **MπNets**(NVIDIA) | 2022 CoRL | 点云 + 当前关节角 + 目标位姿 | 下一步关节位置 | 规划器生成 300 万+ 问题、50 万+ 环境;8×V100 训练一周 | 端到端行为克隆 | 闭环 9 Hz,每次执行约 70% 推出的路径,1 kHz PD 跟踪 |
| **Neural MP**(CMU) | 2024 / IROS 2025 | 点云 + 关节角 + 目标 | GMM(LSTM) | AIT* 等,100 万–300 万条 | **测试时优化(TTO)**:采样多条,选与场景穿透最少的 | 开环为主;冷启动约 2970 ms |
| **MπFormer / Avoid Everything**(NVIDIA) | 2024 CoRL WS | 点云 | 关节动作 | 规划器数据 | 师生微调 + **困难负样本挖掘** | 闭环 |
| **DRP / IMPACT**(CMU) | 2025 CoRL | 点云 + 当前关节角 + **目标关节角** | **动作块**:10 步关节增量,MSE 回归 | cuRobo 生成 **1000 万条**;4090 训练 3 天 | ① 大规模预训练 ② **迭代师生微调**(局部控制器 Geometric Fabrics 当老师,不用全局规划器,因为每步都规划太贵)③ 局部反应层 DCP-RMP 改写目标 ④ 加入"目标被挡→停在最近安全处"的数据 | 闭环;4.67 M 参数,300 Hz(4090),冷启动 3.48 ms |
| **ELMP**(ETH) | 2026 arXiv | 点云 + 关节角 + 目标位姿(12 维) | 关节增量 | AIT* 约 60 万条 | 行为克隆后做 **APG 微调**:可微运动学展开,碰撞/到达/平滑三项损失直接反传,**不再需要老师数据** | MPC 式闭环 30 Hz;冷启动 7.8 ms |
| Diffusion Policy / ACT 一类 | 2023– | 视觉 + 本体 | 动作块(扩散 / CVAE) | 人类示教 | 动作块 + 滚动执行(receding horizon)+ 时间集成 | 闭环 |
几个关键数字:
- DRP:迭代师生微调使成功率比纯预训练**提高 45%**;纯预训练的 IMPACT "经常有轻微碰撞"。
- ELMP:只用 1 万条老师轨迹 + APG(成功率 72.0%、碰撞 19.4%)就超过 60 万条纯行为克隆(57.9%、31.9%),**老师数据少 60 倍**;APG 把环境碰撞率降到原来的 1/3 以下。新环境下,采样 20 万个 APG 问题约 4.5 CPU 小时,而生成同样数量的老师轨迹要 1194 CPU 小时以上。
- 即便最好的方法,静态场景碰撞率也约 9–10%(ELMP-APG 8.9%,DRP 10.16%)——所以实际部署都配有某种检查或反应层。
**行业共识**:
1. 都是**闭环**策略,每个周期根据当前状态重新出动作,而不是一次性出整条路径。
2. 输出**短动作块**(10 步左右)并滚动执行,兼顾平滑和反应速度。
3. 老师只用于离线造数据;微调阶段倾向于**便宜的老师**(局部控制器)或**不要老师**(可微代价)。
4. 纯模仿有"分布偏移、误差累积"问题,必须做后训练(DAgger / 师生微调 / APG)。
---
## 2. 我们的方案和主流的差异,以及据此做的优化
### 2.1 我们的独特条件
| 条件 | 主流(DRP/ELMP 等) | 本项目 | 意味着 |
|---|---|---|---|
| 环境 | 未知、多变,靠点云感知 | **固定且精确已知**(台架 + 立柱,1936 个胶囊) | 不需要点云编码器;模型可以小很多;碰撞检查是**精确**的 |
| 机器人 | Franka 7 轴 | 自研 6 轴,J1/J3/J6 连续转 | 需处理连续轴展开 |
| 碰撞模型 | SDF / 球近似,有感知噪声 | 胶囊解析距离,可微 | 可以直接做 APG;安全层可信 |
| 运行平台 | 台式 GPU | 浏览器(后续真机) | 模型要小,ONNX + WebGPU/wasm |
结论:我们的问题比主流**窄得多也"干净"得多**,主流用来对付感知噪声和环境泛化的大部分复杂度(点云、千万级数据、多环境)都可以省掉,资源应投在**闭环稳定性、多解选择和安全层**上。
### 2.2 v1 方案与主流的差异 → v2 的调整
| # | v1 做法 | 主流做法 | v2 调整 | 理由 |
|---|---|---|---|---|
| 1 | 一次生成整条 32 点路径,开环执行 | 闭环 + 短动作块滚动执行 | **改为动作块策略**:每周期输出未来 16 步(320 ms),只执行前 2–4 步 | 用户要求每周期重算;可纠错、可应对拖拽目标变化 |
| 2 | 失败回落原规划器 | 无回落;靠后训练提升 + 局部反应/检查 | **删除回落**,改为安全层:换候选 → 刹停保持 → 下周期重算 | 用户决定无老师部署 |
| 3 | 512 桶离散化 + 自回归 186 token | DRP 用 MSE 回归动作块;Neural MP 用 GMM | **K 个模式的动作块头(K=4)+ 模式分类**,非自回归一次前向 | 闭环下多峰主要出现在"刚出发选左绕还是右绕",K 模式足够;一次前向比 186 步自回归快一个数量级 |
| 4 | 以目标位姿为条件 | DRP 用目标关节角(IK 另算);ELMP 用目标位姿 | **用目标位姿**(位置 + 6D 旋转 + 模式标志),末段可选局部牛顿精修 | 无老师就没有全量 IK;位姿条件让模型连 IK 一起学。局部牛顿精修只是梯度下降几步,不属于搜索 |
| 5 | 后训练:挖错 + 老师重标(每轮 1–2 小时老师计算) | DRP:便宜局部老师;ELMP:APG 无老师 | **主力换成 APG**(可微胶囊碰撞),挖错循环保留但不再需要老师重标 | 我们的胶囊模型可微且精确,正好适合 APG;省掉绝大部分老师计算 |
| 6 | 拒判头(REJECT) | DRP:目标被挡时学会"停在最近安全处" | **改为"停靠"行为**:目标不可达时,数据里老师路径停在离目标最近的安全位形 | 闭环下"停在最近处"比"拒绝"更有用(目标常在拖拽中短暂进入立柱区) |
| 7 | 20 万条老师数据 | 主流百万–千万条,但它们要泛化到任意环境 | **降到 5 万条**(+ 切片增广),其余靠 APG | ELMP 显示 APG 下 1 万条已胜过 60 万条纯克隆;我们的环境固定,需求更小 |
| 8 | 无 | DRP 的 DCP-RMP 局部反应层 | 暂不做;安全层已覆盖静态环境。未来真机有人/动态障碍时再加 | 当前环境静态 |
### 2.3 v2 相对主流的额外优化点
1. **精确安全层**:主流因感知噪声做不到精确检查,我们可以。每周期对即将执行的动作段做 1° 细验 + 半格复验,保证**只执行检查过的动作**。
2. **可刹停不变式**:只有当"执行这一段之后,仍存在一条已检查过的刹停轨迹"时才执行(见 §8.3),保证任何周期推理失败都能安全停住。
3. **模式粘滞**:闭环每周期重算最怕左右摇摆(这一帧选左绕、下一帧选右绕)。部署时对"切换模式"加代价,除非当前模式不安全(§8.2)。
4. **APG 损失直接用我们的胶囊距离**(`segSegDist` 的可微版本),训练和部署检查同一套几何,消除"训练用球、检查用网格"的口径差(ELMP 的做法有这个差)。
5. **数据便宜**:环境固定,老师离线 5 万条在沙箱约 5 小时即可,不需要集群。
### 2.4 2024–2026 前沿生成式策略逐项评估(v2.2)
用户问:"能不能用更先进的大模型算法?"先说结论:**大模型本身(VLA,30 亿参数级)不适合这个任务**,但它们背后的几项**动作生成技术**很适合,而且能直接解决我们 v2 的三个弱点:K=4 固定模式太粗、动作块拼接处有顿挫、贴柱难例要靠规则兜底。
| 技术(代表工作) | 核心做法 | 公开结果 | 用在我们这里? |
|---|---|---|---|
| **流匹配动作头**(π0 / π0.5 action expert,2024–25) | 用流匹配从噪声生成连续动作块,天然多模态 | π0 以 50 Hz 输出 50 步动作块 | ✅ **采纳**,替换 K=4 WTA 头:不再只有 4 个固定"模式",左绕/右绕/上绕及其细分都能表达 |
| **一步生成**:MeanFlow(MP1,AAAI 2026)、OFP(2026)、条件 OT 耦合(CoRL 2025)、Consistency Policy / OneDP | 学"平均速度场"或自蒸馏,1 次前向就出动作,不需迭代去噪 | MP1 推理约 6.8 ms;OFP 一步超过 100 步扩散,并在 π0.5 上一步胜过原 10 步;OneDP 1.5 Hz → 62 Hz | ✅ **采纳 OFP/MeanFlow**:OFP 从零自蒸馏,**不需要预训练老师模型**,与"无老师"原则一致;推理仍是 1–2 次前向 |
| **代价引导采样**(MPD、EDMP、GPD,2023–25) | 采样过程中加入碰撞代价梯度,把轨迹推离障碍 | MPD/EDMP 引导显著提高成功率;GPD 约 0.8 s | ✅ **采纳,但只在 1–2 步内做**:我们已有可微胶囊距离(APG 要用),在去噪最后一步加一次梯度修正,几乎零额外训练成本。不采纳整条轨迹扩散(MPD 约 1 s,太慢) |
| **扩散种子 + 优化**(DiffusionSeeder + cuRobo,CoRL 2024) | 扩散给初值,GPU 轨迹优化几轮 | 真机规划 26 ms,比 cuRobo 成功率高 51% | ⚠️ 部分采纳:只保留"对已生成动作块做 2–3 次局部梯度修正",不引入全局轨迹优化器(那等于又加回一个老师) |
| **实时动作块执行 RTC**(Black 等,NeurIPS 2025) | 生成新动作块时,把已提交但未执行的动作当作"已知前缀"补全,异步推理不卡顿 | 解决动作块拼接处的停顿与跳变 | ✅ **采纳**:以"已提交的 3 步"为前缀条件生成新块,替代 v2 的时间集成;推理偶发超时也不会顿挫 |
| **流/扩散策略的强化学习微调**(DPPO,ICLR 2025;ReinFlow,NeurIPS 2025) | 在仿真里用 PPO 类方法微调生成式策略 | ReinFlow 在操作任务上成功率平均提升 40%,可在 1–4 步去噪下微调,比 DPPO 省 60%+ 时间 | ✅ **采纳为第三阶段(可选)**:APG 擅长连续可微目标;"到达/卡死/安全层刹停次数"这类离散目标交给 ReinFlow。仿真就是我们的 GPU 批量 FK + 胶囊检测,3090 上可跑 |
| **FAST 动作分词 / π0.5 知识隔离**(2025) | DCT 压缩动作块成离散 token,供大语言模型骨干学习 | FAST 训练提速最多 5 倍 | ❌ 分词本身不采纳(为语言大模型服务,自回归推理慢);⚠️ 借鉴"频域/样条表示":动作块改用 **B 样条控制点**输出,平滑性由表示保证 |
| **VLA 大模型**(π0/π0.5、GR00T、OpenVLA) | 视觉语言骨干 + 动作专家 | 3B 模型单次预填充 46 ms(4090) | ❌ 不采纳:我们没有图像、没有语言指令,环境固定精确;30 亿参数只会更慢,不会更准 |
**结论:v2.2 采纳"一步流匹配头 + 碰撞代价引导 + 前缀条件 + B 样条表示 + 可选 ReinFlow"。** 这正是 π0.5、MP1/OFP、MPD、RTC 这几条前沿线在"低维状态、精确几何、实时控制"场景下的组合,规模仍然是几百万参数。
---
## 3. 训练环境
| 阶段 | 在哪里跑 | 说明 |
|---|---|---|
| 预训练数据生成 | Hark 沙箱(2 vCPU / 7 GB,Node)或用户机器 | 纯 JS,无需 GPU |
| 预训练 + APG 微调 | **用户的 GPU 机器** | 沙箱无 GPU、无 PyTorch。**已定:RTX 3090 24 GB**(显存对 3–4 M 参数模型绰绰有余,可用 batch 1024–2048) |
| 部署推理 | 浏览器(onnxruntime-web,WebGPU 优先,wasm 兜底);后续真机控制器 | 与现有查看器同页 |
```bash
conda create -n armtf python=3.11 -y && conda activate armtf
pip install torch --index-url https://download.pytorch.org/whl/cu121 # 按本机 CUDA 版本选
pip install numpy pyarrow tqdm tensorboard onnx onnxruntime
```
---
## 4. 第一步:离线造预训练数据(老师只在这里出现)
### 4.1 新增文件
| 文件 | 作用 |
|---|---|
| `ml/gen/gen_dataset.cjs` | 采样任务 → 调老师 → 校验 → 时间参数化 → 写分片 |
| `ml/gen/sampler.cjs` | 起点/目标采样(4.3) |
| `ml/gen/run_parallel.sh` | 多进程,不同种子 |
| `ml/data/shard_XXXX.jsonl` | 原始样本(不入库) |
复用 `tests/route_bench_setup.cjs` 装配求解器(关节表、`docs/travel.json`、`docs/collide.json`)。
### 4.2 一条原始样本
```json
{
"id": "s0003_000127", "mode": "pos" | "pose",
"q0": [6], "T": [x,y,z], "R": [[3x3]] | null, "tool": 0,
"path": [[6], ...], // 老师折线 q0 → via... → q_end
"reached": true, // false = 目标不可达,q_end 为离目标最近的安全位形("停靠")
"travel_deg": 123.4, "plan_ms": 812.0,
"fp": "collide.json 指纹"
}
```
### 4.3 采样分布(5 万条任务)
关节限位(`docs/travel.json`):J1/J3/J6 连续 ±180°;J2 [−92, 9],J4 [−111, 16],J5 [−93, 34]。
| 类别 | 占比 | 采样方式 |
|---|---:|---|
| A 均匀 | 45% | q0、目标位形 b 均匀采样且无干涉;目标 = `fk(b)`(pose 模式加 `orient(b)`) |
| B 直线被挡 | 30% | q0→b 关节直线 1° 细验有干涉(route_bench 口径) |
| C 贴立柱 | 15% | b 满足 `nearColumn`,q0 在立柱另一侧(near_column_bench 口径) |
| D 目标不可达 | 10% | 目标在立柱内/所有分支都撞/工作空间外;老师标签 = 停在离目标最近的安全位形 |
- pos / pose 各半;老师用 `solveDyn`(松手口径)。
- **测试集隔离**:固定种子另造 2000 条留出;三个基准案例不进训练集。
### 4.4 过滤
1. 整条路径 1° 细验 + 半格复验,任一段干涉即丢弃。
2. `poseRelaxed` 样本只当 pos 模式用。
3. 行程 > 同任务 `routeRT` 1.5 倍的视为绕远(先统计,再定是否丢弃)。
### 4.5 时间参数化 → 闭环训练样本(v2 新增,关键)
闭环策略学的是"在某个状态下,接下来 16 步怎么走",所以要把整条路径切成很多个"状态 → 动作块"样本:
1. 按现有 `animTo` 的轴速比 `VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8]` 做时间参数化(与查看器动画同口径),控制周期 **dt = 20 ms**。
2. 得到状态序列 q_0, q_1, …, q_N。对每个 t(每隔 3 步取 1 个,减少冗余)生成样本:
- 输入:当前 q_t、目标 (T, R, mode)
- 标签:未来 S = 16 步的增量 Δq_{t+1..t+16} = q_{t+k} − q_t(到终点后补零,即"停住")
3. 每条路径平均约 50–150 个样本 → 5 万条任务约 300 万个训练样本。
4. **路径后缀天然覆盖"从半路出发"**,等于免费的 DAgger 种子数据。
### 4.6 耗时与存储
- `solveDyn` 约 0.7 s/条:5 万条在沙箱 2 进程约 5 小时;用户 8 核机器约 1.5 小时。
- 300 万样本 × 约 0.5 KB ≈ 1.5 GB 原始;Parquet 约 300 MB。
### 4.7 检查点
- [ ] `ml/data/stats.json`:类别、到达率、行程、样本数分布
- [ ] 抽 20 条在查看器回放(新增 `?replay=` URL 参数)
- [ ] 测试集与训练集、基准案例无重叠
---
## 5. 第二步:表示与预处理
文件 `ml/prep/build_dataset.py` → `train/val/test.parquet`、`norm.json`。
### 5.1 输入(条件)
| 量 | 维度 | 编码 |
|---|---:|---|
| 当前 q_t | 6 | 每轴 (sin, cos) → 12;限位轴另加归一化值 → +3 |
| 当前 TCP 位置与姿态 | 3 + 6 | `fk(q_t)`、`orient(q_t)` 的 6D 表示(给模型笛卡尔线索) |
| 目标位置 T | 3 | /600 mm |
| 目标姿态 R | 6 | 6D 表示;pos 模式填 0 |
| 目标 − 当前 TCP | 3 | 位置差,/600 mm |
| 模式 | 2 | one-hot [pos, pose] |
| 上一周期选的模式 k_prev | K+1 | one-hot(含"无"),用于模式粘滞 |
| 关键胶囊对最小距离 | 8 | 当前位形下臂各段到立柱的胶囊距离(cheap,现成函数),给模型"离柱子多近"的直接信号 |
### 5.2 输出(动作块)
- **K = 4 个模式**,每个模式一整块 16 步 × 6 轴的关节增量 Δq(连续值,按每轴最大步长归一化)。
- 一个模式分类头:4 类 logits。
- 训练时 **winner-takes-all**:只对与标签最接近的那个模式回归,分类头学"选哪个"(MTR / 多模态轨迹预测的常用做法)。这样左绕、右绕会落到不同模式里,不会被平均到穿过立柱。
- 附加头:到达剩余时间估计(帮助排序和早停)。
### 5.3 连续轴
J1/J3/J6 统一用 `unwrapTo` 口径展开后再做增量;预处理和部署共用一份实现,§6.4 对拍时一并验证。
---
## 6. 第三步:模型与预训练(行为克隆)
### 6.1 结构(参考 IMPACT,去掉点云部分)
```
条件特征(约 60 维)──MLP──▶ 8 个条件 token (d=192)
│
4 层 Transformer encoder
│ (memory)
K×16 个可学习动作 token ──▶ 4 层 Transformer decoder(非自回归,一次前向)
│
├─ 动作头:每 token 输出 6 维 Δq
├─ 模式分类头:K=4
├─ 价值头(剩余时间/代价):1 ← v2.1,§8.5
└─ 子目标头:2 个关节空间途经点 + 是否需要(仅冷启动用)← v2.1,§8.5
```
| 超参 | 值 |
|---|---|
| d_model / 头数 / FFN | 192 / 8 / 768 |
| encoder / decoder 层数 | 4 / 4 |
| 参数量 | 约 3–4 M(DRP 为 4.67 M,含点云编码) |
| 推理 | 一次前向得到 4 条候选动作块 |
### 6.1A v2.2 动作头升级:一步流匹配(默认)与 WTA(基线)
| | v2 WTA 头(保留为基线) | **v2.2 一步流匹配头(默认)** |
|---|---|---|
| 多模态 | 固定 K=4 个模式 | 连续分布;每周期并行采 K=8 个噪声 → 8 条候选 |
| 训练 | WTA-L1 + 模式 CE | OFP 自蒸馏(或 MeanFlow),从零训练,无需预训练老师模型 |
| 输出 | 16 步 Δq | **B 样条**:6 个控制点 × 6 关节,再采样成 16 步;自带平滑 |
| 前缀条件 | 无,靠时间集成 | 输入"已提交前 3 步",新块从前缀接着走(RTC 思路) |
| 碰撞引导 | 无 | 最后一步加一次可微胶囊距离梯度修正:a ← a + η·∇d_capsule(d < 安全距离时) |
| 推理成本 | 1 次前向 | 1–2 次前向 + 1 次解析梯度(JS 实现,不走 ONNX) |
| APG 兼容 | ✅ | ✅(固定噪声后生成器是确定性可微映射) |
模式粘滞改为"噪声粘滞":上一周期选中候选的噪声向量保留下来,本周期作为 8 个噪声之一,相邻周期自然延续同一绕行方向。子目标头、价值头(§8.5)保持不变。
M4 同时实现两种头,M5 预训练后在 near_column + held-out 上 A/B;若一步流匹配不优于 WTA,回退 WTA(风险可控)。
### 6.2 预训练损失
```
L = WTA-L1(动作块, 最近模式) 权重 1.0
+ CE(模式分类) 权重 0.5
+ FK 损失:预测与标签位形下各连杆胶囊端点的位置差 权重 0.5 ← 借鉴 ELMP
+ 平滑:二阶差分 权重 0.05
+ L1(剩余时间) 权重 0.1
```
### 6.3 训练配置
| 项 | 值 |
|---|---|
| 优化器 | AdamW,β=(0.9, 0.95),wd 0.01 |
| 学习率 | 1e-3,预热 1000 步,余弦退火(同 DRP 量级) |
| batch | 1024(3090 24 GB) |
| 精度 | bf16 混合精度 |
| 步数 | 约 20 万步(300 万样本约 30 epoch),验证集闭环成功率不再上升即停 |
| 预计耗时 | RTX 3090 约 1.5–3 小时(估算;DRP 4.67 M 参数在 4090 上预训练 100 万步用 3 天,我们的样本少、无点云编码,步数约其 1/5,跑完首轮再校正) |
### 6.4 对拍与验证(每 epoch)
- `ml/eval/collide_torch.py`:胶囊距离的 PyTorch 批量版(**可微**,§7 要用),先与 JS `collideFree` 对拍 10 万个位形,判定 0 误差。
- 验证用**闭环仿真**而不是单步误差:从 1000 个验证任务的 q0 出发,每 20 ms 推理一次、提交前 3 步,直到到达或超时(超时 = 老师同任务路径动画时长 × 2,最少 3 s),统计到达率、碰撞步数(安全层关闭时)、行程比。
---
## 7. 第四步:后训练(无老师为主)
### 7.1 主力:APG 可微微调(借鉴 ELMP)
不再请老师标注,直接让模型在可微仿真里闭环展开,用代价反传:
```
for 每个 batch(随机采样 q0 和目标,82 ms 级别的便宜采样,不需要老师):
q = q0
for t in 0..H-1 (H = 60 个周期 ≈ 1.2 s,长任务分段展开):
chunk, logits = π(obs(q, 目标))
Δq = chunk[选中模式][0:3] # 执行前 3 步,与部署一致
q = clamp(q + Δq) # 限位
累计 L_col(q), L_smooth(Δq)
L = w_tgt·L_target(q_H) + w_col·ΣL_col + w_smooth·ΣL_smooth (+ w_len·行程)
反传(BPTT),梯度裁剪 ≤ 1.0
```
- **碰撞项** L_col = Σ_胶囊对 ReLU(margin − 胶囊距离),margin 取 `collide.json` 的 `margin_mm` 基础上再加 5 mm 训练余量;包含臂×立柱、臂×台架、臂×臂(非相邻段)。
- **到达项**:位置误差² + λ_rot·‖R − R*‖_F²(弦距离,ELMP 同款,避免四元数梯度消失)。
- **平滑项**:速度² + 加速度²。
- 可加 **行程项**,直接把"关节行程短"写进目标(ELMP 用笛卡尔路径长度项做过同类事情)。
- 模式选择不可导:用 Gumbel-Softmax,或对 K 个模式都展开 1 步再取最小代价(K=4,代价可接受)。
- 采样任务分布同 §4.3,但 B/C/D 类提到 60%。
- 预期:参照 ELMP,APG 主要降低碰撞率、提高末端精度;**不需要任何老师计算**。
### 7.2 辅助:闭环挖错 + 门禁(v1.1 循环的无老师版)
每轮:
1. **出题**:新种子 5 万任务 + 2000 段模型在环拖拽仿真(目标沿随机曲线移动)。
2. **闭环作答**:完全按部署方式运行(含模式粘滞,**关闭安全层**以暴露真实错误)。
3. **验算**:GPU 批量胶囊检查;记录到达、碰撞、行程、摇摆次数。
4. **错题分类**:E1 撞了 / E2 没到(超时或卡在局部极小)/ E3 绕远(> 1.2× 参考)/ E4 末端精度差 / E5 模式来回切换 ≥ 3 次 / E6 拖拽失稳。
5. **针对性训练**:错题的起点和目标(以及 ±5 mm / ±3° 的 10 个邻居)加入 APG 的采样池,按类别加权(E1、E6 ×2)。
- 只有 E2(卡在局部极小——可微代价本身解决不了的"需要绕远路"问题)才可选地请老师离线给一条路径做行为克隆补充。这是唯一残留的老师用途,量很小,可以在沙箱慢慢跑。
6. **门禁**:留出集闭环到达率上升、三个基准不退化、上一轮修好的错题 95% 仍对;否则回滚。
7. **停止**:连续 2 轮提升 < 0.5 个百分点,或满 6 轮,或达到 §9 全部标准。
### 7.3 可选:拒绝采样自蒸馏
对每个任务采样多条闭环轨迹(不同模式、不同温度),只保留验算通过且最短的一条做行为克隆标签——模型自己当老师。适合 APG 收敛后进一步缩短行程。
---
## 8. 第五步:无老师闭环部署
### 8.1 每个控制周期做什么(默认 50 Hz)
```
每 20 ms:
1. 读当前 q(真机读编码器;查看器读当前插补位形)和最新目标(拖拽时每周期都在变)
2. 推理一次:得到 K=4 条 16 步动作块 + 模式概率 ≈ 2–5 ms(估算)
3. 按"模式概率 − 切换代价"排序(§8.2)
4. 安全层依次检查候选的前 n=3 步(60 ms 的运动)+ 刹停段(§8.3)
第一个通过的 → 把这 3 步"提交"给插补器;记下 k_prev
(实际上 20 ms 后只走完第 1 步,第 2、3 步是已验证的"缓冲":
下一周期的新结果会覆盖它们;若下一周期推理超时或全部不安全,就沿缓冲 + 刹停段安全停下)
5. 全部不通过 → 进入刹停:按已验证的刹停轨迹减速停下,保持;下一周期继续推理
6. 到达判据:TCP 误差 < 1 mm(pose 另需 < 0.5°)且速度≈0 → 结束;
接近目标(< 5 mm)时可选做 1–3 次局部牛顿精修(solveTo 单步,不是全量 IK)
```
插补器在两个周期之间做平滑过渡(下一块与正在执行的块做时间集成/交叉淡入,ACT 的 temporal ensembling 思路),避免每 20 ms 一顿。
### 8.2 模式粘滞(防左右摇摆)
- 排序分数 = log p(k) − c·[k ≠ k_prev],c 初值 2.0(在 §7.2 的 E5 统计上调)。
- 训练时 k_prev 作为输入(§5.1),模型自己也学会"保持当前绕行方向"。
### 8.3 安全层(唯一的硬保证)
- 检查内容:候选前 3 步的连续运动做 1° 细验 + 半格复验(与现有 `pathFine` 同口径),外加关节限位。
- **可刹停不变式**:执行前还要检查"从第 3 步末状态按最大减速度停下"的刹停段也无干涉。于是任何时候系统都处在"有一条已验证的停车路线"的状态,下一周期就算模型完全失效,也只会停下、不会撞。
- 耗时:3 步 × 4 候选,最坏约 4 × 10 个采样位形 × 约 1.8 ms ≈ 70 ms(太慢)→ 实际做法:
- 先用现有包围球预筛(大多数位形直接判安全,见 2026-10-15 记录 collideFree 2.9→1.8 ms);
- 按排序**逐个**检查,第一个通过就停(多数周期只检查 1 条);
- 目标:每周期安全检查 < 5 ms(需在 M6 实测;不达标则把检查放到 Web Worker 与推理流水线并行)。
- 现有 `ik-glue.js` 的"每帧碰撞守卫"保留,作为第二道保险。
### 8.4 部署包里有什么、没有什么
| 有 | 没有 |
|---|---|
| `traj_v2.onnx`(fp16,约 7 MB)+ `norm.json` | PRM 路网(`collide.json.roadmap`)、IK 候选大表 |
| 胶囊碰撞检查(现有 `collideFree`、包围球预筛、`pair2d` 表) | `solveDyn` / `routeRT` / RRT / A* |
| 正运动学 `fk` / `orient`、局部牛顿精修 `solveTo` 单步 | 全量 IK 枚举 |
`collide.json` 指纹写进模型;不一致时前端**拒绝运行并提示重训**(不再回落旧规划器)。
### 8.5 为什么会"超时",以及无老师怎样做到 30/30
**超时不是算得慢。** 模型每周期只要几毫秒,比原规划器快得多;"超时"指的是**机械臂在规定时间内没走到目标**,是决策错了,不是计算慢了。无老师闭环下有三种典型情形:
| 失效形态 | 现象 | 为什么更快的推理救不了 |
|---|---|---|
| 局部极小 / 活锁 | 目标在立柱背后,策略每周期都想"直着过去",安全层每周期都刹停,原地抖动 | 每周期都做同样的错误决定,算得再快也只是更快地重复 |
| 模式摇摆 | 这一周期选左绕、下一周期选右绕,来回切换,进度为零 | 同上 |
| 绕远 | 选了一条可行但很长的路,在时限内走不完 | 方向对了但路线差 |
原规划器不会这样,是因为它做**全局搜索**(PRM + A*)。去掉老师后,全局"想清楚往哪绕"这件事必须由模型自己承担。v2.1 用五个手段(全部在运行时不需要老师)把 near_column 要求提到 **30/30**:
1. **全局子目标头(分层)**:模型在冷启动(新目标到达的第一个周期)额外输出 1–2 个途经点(关节空间),作为"绕柱方向"的锚点;之后每周期的动作块以"当前子目标"为条件,到达子目标后切到下一个。训练标签直接取自老师路径的拐点(离线已有)。这相当于把老师的全局搜索结果蒸馏成一次前向。
2. **价值头(剩余代价估计)**:模型额外预测"从这个状态到目标还要多少时间"。APG 展开时有真实值可监督,不需要老师。部署时用它给 K 个模式打分,并作为"有没有进展"的判据。
3. **短前瞻选模式**:每周期对 K=4 个模式各自再往前推一块(共 2 次批量前向 ≈ 2 × 2–5 ms),用 胶囊碰撞 + 价值头 给 32 步后的终态打分,选最好的。比只看第一块的概率更能避开"走两步就撞墙"的分支。
4. **卡死检测 + 强制换向**:0.3 s 内价值头没有下降、或连续 2 个周期刹停 → 判定卡死,强制切到另一个绕行方向(另一个子目标/模式)并锁定 1 s(防止又切回来)。规则很简单、确定性、可测试。
5. **训练侧针对性加强**:贴柱类任务在 APG 采样里占 40%;APG 展开长度覆盖整段绕行(约 3 s = 150 个周期,分段截断反传);对贴柱任务做拒绝采样自蒸馏(§7.3),模型自己成功的路线成为新标签。
验收相应改为(§9.2):near pos / near rt 都必须 **30/30**,且到达用时 ≤ 老师路径动画时长 × 1.3。
### 8.6 控制周期:业界案例与评估
**工业臂 / 研究臂的外部实时接口(伺服层)**
| 产品 / 接口 | 周期 | 说明 |
|---|---|---|
| Franka FCI(libfranka) | **1 ms(1 kHz)** | 关节位置/速度/力矩、笛卡尔位姿;每周期往返 + 计算必须 < 1 ms |
| Universal Robots e-Series RTDE | **2 ms(500 Hz)** | `servoj` 默认 t = 0.002 |
| Universal Robots CB3 RTDE | **8 ms(125 Hz)** | `servoj` 默认 t = 0.008 |
| ABB EGM | **4 ms(250 Hz)**,可设为 4 ms 的整数倍 | 控制滞后 10–20 ms(随机型) |
| KUKA RSI | **4 ms**(Fast)或 **12 ms**(Normal) | |
| FANUC DPM | 约 **8 ms** | R-30iB 无 1–2 ms 通道 |
| MoveIt Servo(ROS 2) | 默认 **10 ms**(新版);旧版示例 34 ms | 常设为 ros2_control 更新频率的倒数(如 50 Hz → 20 ms) |
| 舵机(RC PWM) | **20 ms(50 Hz)** 帧周期 | 数字舵机可到更高帧率 |
**学习型策略的推理周期(策略层)**
| 方法 | 策略频率 | 动作块 | 执行方式 |
|---|---|---|---|
| MπNets | 9 Hz | 单步 | 线性插值,1 kHz PD 跟踪 |
| Diffusion Policy | 10 Hz | 16 步 | 线性插值到 125 Hz 交给 UR5 |
| ELMP | 30 Hz(真机) | 单步 Δq | 每步 3 个航点线性插值给 Franka 位置控制器 |
| π0 | 50 Hz 动作,约每 0.5 s 推理一次 | 50 步(1 s) | 执行 25 步后重算 |
| ACT | 50 Hz | 100 步 | 时间集成 |
| DRP / IMPACT | 可跑 300 Hz(4090) | 10 步 | 闭环 |
**结论:业界一律是"两层"结构**——策略层 10–50 Hz(少数到 300 Hz)给出关节目标,伺服/插补层 125 Hz–1 kHz 负责平滑跟踪。没有人让神经网络直接跑 1 kHz 伺服环。
**对本方案的评估:**
1. **策略层 50 Hz(20 ms)合适**:与 π0、ACT 同档,高于 ELMP 的 30 Hz 和 MπNets 的 9 Hz;我们模型小(3–4 M)、无点云,单次前向 + 安全检查预计 < 10 ms,留有一倍余量。浏览器里 `requestAnimationFrame` 是 60 Hz,50 Hz 也能稳定跑。
2. **"提交 3 步(60 ms)"合适,且应按刹停距离定**:它的作用是给推理抖动和通信滞后留缓冲。ABB EGM 自身就有 10–20 ms 滞后;60 ms 足以覆盖一次推理超时 + 一次滞后。真机上应改成规则:提交步数 n ≥ ⌈(通信滞后 + 1 次推理最坏耗时) / 20 ms⌉ + 1,且刹停段另行验证。
3. **伺服层不归本方案管,但要匹配**:策略输出 50 Hz 关节目标,由插补器升采样到电机控制器的频率(舵机 50 Hz 直接用;总线舵机/步进/FOC 驱动通常 ≥ 250 Hz–1 kHz,做三次样条插值)。
4. **做成可配置**:`dt`(默认 20 ms)、提交步数 n(默认 3)、动作块长度 S(默认 16 = 320 ms)。训练数据按 dt = 20 ms 生成;若真机必须改到 10 ms(100 Hz),只需把动作块重采样,或用 dt = 10 ms 重造数据(约 5 小时)。
5. 你的电机/驱动型号确定后,按第 2、3 条把 n 和插补频率定下来。
---
## 9. 第六步:评估与验收
### 9.1 测试集合
| 集合 | 来源 |
|---|---|
| route_bench | `tests/route_bench.cjs`,60 例 |
| near_column_bench | `tests/near_column_bench.cjs`,30 例(pos / pose / rt) |
| rt_drag_bench | `tests/rt_drag_bench.cjs`,12 例(改为模型在环逐周期运行) |
| held-out | §4.3 留出 2000 条 |
新增 `tests/policy_bench.cjs`:①现有规划器 ②策略(关安全层)③策略 + 安全层(部署形态),输出对比表。
### 9.2 验收标准(部署形态 = 策略 + 安全层)
| 指标 | 要求 | 现状参考 |
|---|---|---|
| 实际执行的碰撞帧 | **= 0** | rt_drag_bench 0 |
| 到达率 | route **60/60**,near pos **30/30**,near rt **30/30**,held-out ≥ 98% | 60/60、30/30、30/30 |
| 到达用时 | ≤ 老师同任务路径动画时长 × 1.3 | — |
| 关节行程 | ≤ 现有规划器 × 1.10 | route 平均 467° |
| 冷启动(新目标→第一个动作) | < 10 ms | 首次全量 IK 最差约 2 s |
| 每周期耗时(推理+安全层)P99 | < 15 ms | 拖拽重规划 P99 约 321 ms |
| 刹停发生率 | < 2% 的周期 | — |
| 模式摇摆(≥3 次切换)| < 1% 的任务 | — |
| 策略单独(关安全层)碰撞率 | < 3%(参考:ELMP-APG 8.9%,DRP 10.16%) | — |
说明:v2 曾提议 near_column 允许 29/30,用户否决(2026-10-17)。为保证 30/30,部署侧加入子目标头、价值头、短前瞻和卡死换向(§8.5)。
---
## 10. 风险与维护
1. **局部极小**:可微代价和闭环策略都可能卡在立柱前"想直穿过去又被安全层挡住"。对策:B/C 类数据加权、模式头学左右绕、E2 错题少量老师补数据(§7.2)。这是无老师方案最大的风险。
2. **碰撞模型/立柱变动 = 重训**:指纹不一致即拒绝运行。重训流程:§4(约 5 h)→ §6(约 4 h)→ §7(约 2–4 h)。
3. **安全层耗时**:最坏周期可能超 5 ms,见 §8.3 的流水线方案。
4. **老师缺陷**:`ci27` 姿态根枚举漏分支(姿态偏差约 97.5°)会进入预训练数据;APG 的姿态到达项可以部分纠正,必要时先修老师。
5. **真机迁移**:浏览器的"执行"是理想插补;真机需加速度/力矩限制(TOPP 类时间参数化)和跟踪误差余量(把 margin 再加大)。
---
## 11. 目录规划
```
ml/
gen/ gen_dataset.cjs sampler.cjs run_parallel.sh
data/ (不入库)shard_*.jsonl stats.json *.parquet norm.json
prep/ build_dataset.py
model/ policy_transformer.py
sim/ closed_loop.py # 闭环仿真(训练验证、挖错、APG 展开共用)
eval/ collide_torch.py # 可微胶囊距离,与 JS 对拍
apg/ apg_finetune.py
mining/ mine_round.py gate.py loop.sh
configs/ base.yaml apg.yaml
train.py export_onnx.py README.md
webapp/js/ik-policy.js # 闭环推理 + 模式粘滞 + 安全层 + 刹停
webapp/models/traj_v2.onnx traj_v2.norm.json
tests/policy_bench.cjs
```
## 12. 里程碑
| # | 内容 | 产出 | 在哪跑 |
|---|---|---|---|
| M1 | 数据生成器 + 时间参数化 + 1000 条试跑 | `ml/gen/*`、stats.json | 沙箱 |
| M2 | 可微胶囊距离(PyTorch)与 JS 对拍 | `collide_torch.py`,10 万位形 0 误差 | 沙箱(CPU 即可) |
| M3 | 5 万条老师数据 → 约 300 万闭环样本 | Parquet | 沙箱约 5 h 或用户机器 |
| M4 | 模型 + 训练工程 + 闭环仿真;小数据过拟合测试 | `train.py`、`closed_loop.py` | 沙箱 CPU 验证可跑 |
| M5 | 预训练(行为克隆) | checkpoint | **用户 GPU** |
| M6 | APG 微调 + 挖错循环 3–6 轮 | 每轮 report.md | **用户 GPU** |
| M6b | (可选)ReinFlow 强化学习微调:奖励 = 到达 − 刹停次数 − 卡死 − 用时 | 对比 M6 | **用户 GPU** |
| M7 | 导出 ONNX + `ik-policy.js` 闭环部署 + 安全层实测 + `policy_bench` | 对比表 | 沙箱 + 浏览器 |
| M8 | 移除查看器里对 `solveDyn`/`routeRT`/路网的运行时依赖(保留在 `ml/gen` 离线使用) | 部署包瘦身 | 沙箱 |
## 13. 参考文献
- Fishman et al., *Motion Policy Networks*, CoRL 2022. https://arxiv.org/abs/2210.12209 ;闭环细节见补充材料 https://proceedings.mlr.press/v205/fishman23a/fishman23a-supp.pdf
- Dalal et al., *Neural MP: A Generalist Neural Motion Planner*, 2024 / IROS 2025. https://arxiv.org/abs/2409.05864
- Yang, Liu et al., *Deep Reactive Policy: Learning Reactive Manipulator Motion Planning for Dynamic Environments*, CoRL 2025. https://arxiv.org/abs/2509.06953
- Fishman et al., *Avoid Everything: Model-Free Collision Avoidance with Expert-Guided Fine-Tuning*, CoRL 2024 Workshop(DRP 文中引用为 MπFormer)
- Li et al., *ELMP: Efficient Learning for Motion Planning via Analytical Policy Gradients*, arXiv 2607.00215, 2026. https://arxiv.org/abs/2607.00215
- 控制周期:Franka FCI https://frankarobotics.github.io/docs/overview.html ;UR RTDE https://www.universal-robots.com/articles/ur/interface-communication/remote-control-via-tcpip ;ABB EGM 应用手册 3HAC073318;KUKA RSI https://docs.quanser.com/quarc/documentation/kuka_rsi_block.html ;MoveIt Servo servo_parameters.yaml;Diffusion Policy https://arxiv.org/abs/2303.04137 ;π0 https://arxiv.org/abs/2410.24164 ;实时动作块执行 https://arxiv.org/abs/2506.07339
- 前沿生成式策略(v2.2):π0 https://arxiv.org/abs/2410.24164 ;π0.5 https://arxiv.org/abs/2504.16054 ;知识隔离 https://arxiv.org/abs/2505.23705 ;FAST https://arxiv.org/abs/2501.09747 ;MP1 https://arxiv.org/abs/2507.10543 ;OFP https://arxiv.org/abs/2603.12480 ;条件 OT 流策略 https://proceedings.mlr.press/v305/sochopoulos25a.html ;Consistency Policy https://arxiv.org/abs/2405.07503 ;OneDP https://proceedings.mlr.press/v267/wang25ba.html ;MPD https://arxiv.org/abs/2308.01557 ;EDMP https://arxiv.org/abs/2309.11414 ;GPD https://arxiv.org/abs/2501.18229 ;DiffusionSeeder https://arxiv.org/abs/2410.16727 ;RTC https://arxiv.org/abs/2506.07339 ;DPPO https://arxiv.org/abs/2409.00588 ;ReinFlow https://arxiv.org/abs/2505.22094
- Ross, Gordon, Bagnell, *A Reduction of Imitation Learning and Structured Prediction to No-Regret Online Learning*(DAgger),AISTATS 2011
## 14. 需要用户确认
1. ~~训练机显卡~~:已定 RTX 3090 24 GB。
2. ~~near_column 29/30~~:已否决,要求 30/30(§8.5)。
3. 控制周期:按 §8.6 评估保持策略层 50 Hz、提交 3 步;**请告知真机电机/驱动类型与通信方式**,用于确定插补频率和提交步数。