Transformer 避障运动策略：最终方案

基于 main@3c7c814（贴立柱修复后的规划器）。本文是方案，代码尚未编写。 训练机：RTX 3090 24 GB。部署：浏览器（后续真机）。

0. 定位与边界

关于"风险"： 论文证据只能说明"别人在相近条件下做成过"，没有一篇论文的条件（低维状态、单根固定立柱、约 4M 参数、浏览器推理、30/30）与本项目完全一致。因此本方案中只有安全层是硬保证（碰撞与否不依赖模型好坏）；其余每一项都配有证据等级、本仓库可测的门禁（不过不进下一步）和回退（§10）。

1. 方案总览

证据等级：A 有直接先例与数字 · B 有相近先例，需本仓库验证 · C 仅推断。

2. 依据

2.1 业界神经运动规划

• ELMP：1 万条老师轨迹 + APG（成功率 72.0%）超过 60 万条纯行为克隆（57.9%）；APG 把碰撞率降到 1/3 以下。
• 最好的方法在静态场景仍有约 9–10% 碰撞（ELMP-APG 8.9%，DRP 10.16%），实际部署都配检查或反应层。
• 共识：闭环、短动作块滚动执行、老师只离线用、必须后训练。

2.2 本项目的条件

资源应投在闭环稳定性、左右绕行选择和安全层上，而不是感知和泛化。

2.3 前沿生成式技术取舍

2.4 沙箱双模态对照实验

2D 点从柱左侧到右侧，示教一半从上绕一半从下绕，16 步动作块、每 3 步重推理，与本方案同构。代码 ml/toy/toy_bimodal.py，150 个"起终点都贴近中轴"的难例：

• 朴素 1 步流匹配会把模态平均进柱子；MeanFlow 1 步达到 10 步流匹配的质量，推理快约 8 倍。
• 安全层在所有方法上把穿柱降为 0，代价是刹停。
• 该任务过于简单（闭环中状态一偏离中轴即打破对称），分不出一步头与 WTA 谁更好，需在真实数据上 A/B（M5）。
• 训练仅 300 步的 MSE 在同一难例上 100% 穿柱、加安全层后 0% 到达，说明欠拟合时平均模态的风险真实存在。

3. 训练环境

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 文件

复用 tests/route_bench_setup.cjs 装配求解器（关节表、docs/travel.json、docs/collide.json）。

4.2 原始样本

{
  "id": "s0003_000127", "mode": "pos" | "pose",
  "q0": [6], "T": [x,y,z], "R": [[3x3]] | null, "tool": 0,
  "path": [[6], ...],
  "reached": true,
  "travel_deg": 123.4, "plan_ms": 812.0,
  "fp": "collide.json 指纹"
}

reached=false 表示目标不可达，路径终点为离目标最近的安全位形（"停靠"）。

4.3 采样分布（5 万条任务）

关节限位：J1/J3/J6 连续 ±180°；J2 [−92, 9]，J4 [−111, 16]，J5 [−93, 34]。

• pos / pose 各半；老师用 solveDyn，固定种子；同类任务优先取行程最短的绕行同伦类，保证左右选择一致。
• 另造三个互不重叠的集合：门禁集 2000 条（挖错循环每轮用）、held-out 验收集 2000 条、near_column 扩展验收集 ≥1000 条；两个验收集只在最终验收时用一次。现有三个基准不进训练集。

4.4 过滤

1. 整条路径用距离下界认证（与 §8.3 同口径）复核，任一段不过即丢弃。仓库 tests/route_bench.cjs 上现有规划器有 1/40 例路径细验不过，这类样本必须丢掉。
2. poseRelaxed 样本只当 pos 模式用。
3. 行程 > 同任务 routeRT 1.5 倍的先统计，再定是否丢弃；按类别统计丢弃率（C 类 > 20% 说明老师在贴柱场景不可靠）。

4.5 时间参数化 → 闭环训练样本

1. 时间律：按轴速比 VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8] 匀速巡航，梯形加减速，速度只取决于"当前速度 + 剩余路程"（每轴加速度上限 a_max 为待定参数，真机型号确定后设定），dt = 20 ms。
2. 得到状态序列 q_0 … q_N，每隔 3 步取一个样本：
   • 输入：§5.1 全部字段（含当前速度、已提交前缀）
   • 标签：未来 16 步 Δq_{t+k} = q_{t+k} − q_t（到终点后补零）
3. 每条路径约 40–50 个样本 → 约 200 万样本（以 stats.json 为准）。
4. 协变量偏移对策：关节噪声增广（σ 0.5–1°，噪声状态由老师离线重标）；偏离状态的进一步覆盖靠 §7。

4.6 耗时与存储

• solveDyn 约 0.7 s/条：沙箱 2 进程约 5 小时；8 核机器约 1.5 小时。
• 200 万样本原始约 1 GB；Parquet 约 200 MB。

4.7 检查点

• ml/data/stats.json：类别、到达率、行程、样本数、补零样本占比
• 老师一致性：同任务 5 个种子，同伦类一致率 ≥ 90%，行程变异系数 < 10%
• 抽 20 条在查看器回放（新增 ?replay= 参数）
• 各集合与训练集无重叠

5. 表示与预处理

文件 ml/prep/build_dataset.py → train/val/test.parquet、norm.json。

5.1 输入

5.2 输出

• 默认一步生成头：每条候选 16 步 × 6 轴 Δq（按每轴最大步长归一化）。
• WTA 基线头：K=4 条 16 步 Δq + 4 类模式 logits。
• 两种头共用：价值头（剩余时间/代价）、子目标头（2 个关节途经点 + 是否需要）。

5.3 连续轴

J1/J3/J6 统一用 unwrapTo 展开后再做增量；预处理与部署共用一份实现，对拍时一并验证。

6. 模型与预训练

6.1 结构

条件特征（约 90 维）──MLP──▶ 8 个条件 token (d=192)
                               │
               4 层 Transformer encoder
                               │ (memory)
16 个动作 token（一步头：注入噪声 + 时间嵌入；WTA：K×16 个可学习 token）
                               ▼
               4 层 Transformer decoder（非自回归，一次前向）
         ├─ 动作输出：每 token 6 维 Δq
         ├─ （WTA）模式分类头：4
         ├─ 价值头：1
         └─ 子目标头：2 个关节途经点 + 是否需要

子目标触发：新目标或目标跳变 > 50 mm 时输出；到达当前子目标 < 5° 后切下一个。

6.2 动作头

M4 同时实现两种头，M5 预训练后在 near_column 与门禁集上 A/B（穿柱率、到达率、摇摆率），择优部署。

6.3 损失

一步生成头：

L = MeanFlow 损失（或 Shortcut 自一致 + FM 锚定）          1.0
  + FK 损失：一步输出（固定噪声）的各连杆胶囊端点位置差      0.5
  + 平滑：二阶差分                                         0.05
  + 价值头 L1 + 子目标头 L1                                 0.1 / 0.1

WTA 头：

L = WTA-L1(动作块, 最近模式)   1.0
  + CE(模式分类)               0.5
  + FK 损失                    0.5
  + 平滑：二阶差分             0.05
  + 价值头 L1 + 子目标头 L1    0.1 / 0.1

6.4 训练配置

6.5 对拍与验证

• ml/eval/collide_torch.py：可微胶囊距离 PyTorch 批量版，与 JS collideFree 对拍 10 万位形：布尔判定 0 差异，距离误差 < 1e-4 mm，梯度与有限差分相对误差 < 1e-3。
• 每 epoch 闭环验证：从 1000 个验证任务出发，按部署节奏（每 60 ms 推理、提交 3 步）运行到到达或超时（老师路径时长 × 2，最少 3 s），统计到达率、用时 ≤ 老师 × 1.3 的达标率、碰撞步数（关安全层）、行程比、左/右/穿柱三类比例。

7. 后训练

7.1 主力：APG 可微微调

for 每个 batch（随机采样 q0 和目标，不需要老师）:
    q = q0
    for t in 0..H-1  (H = 60 次推理 × 3 步 = 180 步 ≈ 3.6 s；长任务分段截断反传):
        chunk = π(obs(q, 目标, 前缀))       # 一步头固定噪声；WTA 见 §6.2
        Δq = chunk[0: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·行程) + w_bc·L_BC
    反传，梯度裁剪 ≤ 1.0，lr 1e-4

• 碰撞项 L_col = Σ ReLU(margin − 胶囊距离)，margin = margin_mm + 训练余量（3–6 mm，覆盖安全层认证的保守量）；含臂×立柱、臂×台架、臂×臂（非相邻段）。
• 到达项：位置误差² + λ_rot·‖R − R*‖_F²。
• 平滑项：速度² + 加速度²。
• BC 锚定项：保留预训练损失，防止多模态被压缩；每轮监控左右比例。
• 采样分布同 §4.3，B/C/D 类提到 60%，其中贴柱类 40%。

7.2 辅助：无老师挖错循环

每轮：

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. 门禁（用门禁集，不用验收集）：到达率上升、A 类每轮降幅 ≤ 0.5 个百分点、上一轮修好的错题 95% 仍对；否则回滚。
7. 停止：连续 2 轮提升 < 0.5 个百分点，或满 6 轮。

7.3 可选：拒绝采样自蒸馏

每个任务采样多条闭环轨迹，在"验算通过且最小间隙 ≥ 阈值"的样本中取最短的一条做行为克隆标签。门禁：C 类间隙中位数不低于自蒸馏前的 80%，刹停率不升。

7.4 可选：ReinFlow 微调

奖励 = 到达 − 刹停次数 − 卡死 − 用时，按左/右绕行分组平衡奖励，防止模式分布被重塑。

8. 部署

8.1 执行循环

执行：50 Hz，每步 20 ms，按已提交步走
每 60 ms：
  1. 读当前 q、当前速度、已提交未执行前缀、最新目标
  2. 推理：一步头 8 条候选（WTA 为 4 条）16 步动作块
  3. 每条候选做 1–2 步胶囊距离梯度投影（仅 d < 余量时）
  4. 排序（§8.2）
  5. 安全层依次认证候选前 3 步 + 刹停段（§8.3），第一个通过的提交给执行器
  6. 全部不通过 → 沿上一次已认证的刹停段停下，下次继续推理
  7. 到达：TCP 误差 < 1 mm（pose 另需 < 0.5°）且速度≈0；
     接近目标（< 5 mm）时做 1–3 次局部牛顿精修（solveTo 单步），精修段同样过安全层认证

执行的始终是安全层认证过的那一块，不做新旧块混合。执行时钟放在 Web Worker、按实际 dt 推进，避免 requestAnimationFrame（60 Hz）与 20 ms 不整除及后台节流造成的抖动。

8.2 候选排序（防左右摇摆）

• 一步头：分数 = −Σ w_k‖候选第 k 步 − 已提交块第 k 步‖²（重叠步加权）− λ·价值头剩余代价。
• WTA 头：分数 = log p(k) − c·[k ≠ k_prev] − λ·价值头剩余代价，c 初值 2.0。

8.3 安全层（唯一的硬保证）

• 距离下界认证：对一段运动 q_a → q_b，若两端到障碍的最小距离之和 d(q_a) + d(q_b) > Σ_j r_j·|Δθ_j|（r_j 为每轴实测杠杆上界，J1 约 9.0 mm/°，六轴合计最大约 24.3 mm/°），则整段安全；否则二分，直到全部子段通过或判不安全。臂×立柱/台架用胶囊距离；自碰撞对复用 pair2d 杠杆界（J3 3.24、J4 2.54 mm/°）。外加关节限位。
• 离散采样检查（现有 pathFine，1.5° + 半格）只作快速预筛，不作保证依据。
• 可刹停不变式：只有当"候选前 3 步 + 从第 3 步末按每轴 a_max 减速到停"整段都通过认证时才提交。任何时候系统都持有一条已认证的停车路线，下一次推理即使完全失效也只会停下。
• 耗时：每段一次距离查询覆盖整段，不通过才二分（通常 2–6 次）；按排序逐个认证，第一个通过即停；包围球预筛先行。目标每次 < 5 ms，否则放 Web Worker 与推理流水线并行。
• ik-glue.js 的每帧碰撞守卫保留为第二道保险。

8.4 部署包

collide.json 指纹写进模型；不一致时前端拒绝运行并提示重训。

8.5 near_column 30/30 的保障手段

"超时"指机械臂在规定时间内没走到目标，是决策问题而非计算速度问题。典型失效：局部极小/活锁（每次都想直穿、每次被安全层挡住）、左右摇摆、绕远。对策（运行时均不需要老师）：

1. 子目标头：给出 1–2 个关节途经点作为绕柱方向锚点，标签取老师路径拐点（离线）。
2. 价值头：预测剩余时间/代价，用于候选打分与进展判断；APG 中用 TD 自举监督。
3. 短前瞻：对每条候选再推一块，用胶囊认证 + 价值头给 32 步后终态打分。
4. 卡死检测 + 强制换向：0.3 s 内价值不降或连续 2 次刹停 → 切到另一绕行方向并锁定 1 s。
5. 训练侧加强：贴柱类占 APG 采样 40%；展开覆盖整段绕行；贴柱任务做拒绝采样自蒸馏。

8.6 控制周期

伺服层（参考）：Franka FCI 1 ms；UR e-Series 2 ms、CB3 8 ms；ABB EGM 4 ms（滞后 10–20 ms）；KUKA RSI 4/12 ms；FANUC DPM 约 8 ms；MoveIt Servo 默认 10 ms；RC 舵机 20 ms。

策略层（参考）：MπNets 9 Hz；Diffusion Policy 10 Hz、预测 16 执行 8；ELMP 30 Hz；π0 50 Hz、约每 0.5 s 推理一次；DRP 300 Hz。业界均为"策略层 10–50 Hz + 伺服层 125 Hz–1 kHz"两层结构。

本方案：执行 50 Hz、每 60 ms 推理一次、提交 3 步。真机上提交步数 n ≥ ⌈(通信滞后 + 一次推理最坏耗时) / 20 ms⌉ + 1，刹停段另行认证；策略输出 50 Hz 关节目标，由插补器升采样到电机控制器频率。dt、n、动作块长度均可配置。

9. 评估与验收

9.1 测试集合

新增 tests/policy_bench.cjs：①现有规划器 ②策略（关安全层）③策略 + 安全层，输出对比表。

9.2 验收标准（部署形态 = 策略 + 安全层）

说明：30 例全对时，失败率的 95% 置信上界约 9.5%；扩展集 1000 例 0 失败时约 0.3%，因此以扩展集作为 30/30 的统计支撑。

10. 当前存在的问题与风险

10.1 只能靠实测消除的风险（按里程碑顺序）

10.2 已知缺陷

1. 现有规划器（老师）在 tests/route_bench.cjs 上有 1/40 例到达但路径细验不过；§4.4 的认证过滤会丢弃这类样本。
2. ci27 姿态根枚举漏分支（姿态偏差约 97.5°）会影响 pose 数据；APG 姿态项可部分纠正，必要时先修老师。
3. 胶囊模型或立柱变动即需重训：数据约 5 h + 预训练 1.5–5.6 h + 后训练 2–4 h。

10.3 需要用户确认

1. 真机电机/驱动型号与通信方式：决定每轴加速度上限 a_max、插补频率与提交步数。
2. 运行时多候选打分是否算"运行时老师"：本方案的理解是不算（只对模型自己的候选做距离认证和打分，不做搜索）。

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   # 一步生成头 + WTA 头
  sim/        closed_loop.py          # 闭环仿真（验证、挖错、APG 共用）
  eval/       collide_torch.py        # 可微胶囊距离，与 JS 对拍
  apg/        apg_finetune.py
  mining/     mine_round.py  gate.py  loop.sh
  toy/        toy_bimodal.py          # 2D 双模态对照实验
  configs/    base.yaml  apg.yaml
  train.py  export_onnx.py  README.md
webapp/js/ik-policy.js               # 闭环推理 + 排序 + 安全认证 + 刹停
webapp/models/traj_policy.onnx  traj_policy.norm.json
tests/policy_bench.cjs

12. 里程碑

13. 参考文献

• MπNets：https://arxiv.org/abs/2210.12209
• Neural MP：https://arxiv.org/abs/2409.05864
• Deep Reactive Policy / IMPACT：https://arxiv.org/abs/2509.06953
• ELMP：https://arxiv.org/abs/2607.00215
• Diffusion Policy：https://arxiv.org/abs/2303.04137
• π0：https://arxiv.org/abs/2410.24164 ；π0.5：https://arxiv.org/abs/2504.16054
• FAST：https://arxiv.org/abs/2501.09747
• MP1 / MeanFlow：https://arxiv.org/abs/2507.10543
• One-Step Flow Policy：https://arxiv.org/abs/2603.12480
• 条件 OT 流策略：https://proceedings.mlr.press/v305/sochopoulos25a.html
• Kinematic MeanFlow：https://arxiv.org/abs/2610.00864
• Consistency Policy：https://arxiv.org/abs/2405.07503 ；OneDP：https://arxiv.org/abs/2410.21257
• 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
• Real-Time Chunking：https://arxiv.org/abs/2506.07339 ；训练时动作条件化：https://arxiv.org/abs/2512.05964
• Bidirectional Decoding（BID）：https://arxiv.org/abs/2408.17355
• DPPO：https://arxiv.org/abs/2409.00588 ；ReinFlow：https://arxiv.org/abs/2505.22094
• Schwarzer, Saha, Latombe, Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments, IEEE T-RO 2005
• Thumm & Althoff, Provably Safe Deep Reinforcement Learning for Robotic Manipulation in Human Environments, ICRA 2022
• Zhou et al., On the Continuity of Rotation Representations in Neural Networks, CVPR 2019
• AWE（航点提取）：https://arxiv.org/abs/2307.14326 ；SHAC：https://arxiv.org/abs/2204.07137
• 控制周期：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