本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

第 90 章 UMI-on-Legs 精读——移动底盘上的 Diffusion Policy 操作¶

难度: ⭐⭐⭐⭐ | 建议时间: 1.5 周 (35-45 小时) | 前置: 复合/180 Deep-WBC, 足式/190 RL 基础, 复合/20 操作基础, 复合/30 MPC 基础

本章定位
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
复合/180 Deep-WBC ──→ [UMI-on-Legs 精读] ──→ 复合/220 移动操作前沿
复合/20 操作基础   ──→ [Diffusion Policy] ──→ 复合/210 RAMBO
足式/190 RL 基础   ──→ [RL WBC 策略]     ──→ 复合/230 VBC 对比
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

前置自测¶

📋 前置自测（答不出 ≥ 2 题 → 先回复合/180 和复合/20 复习）

什么是末端执行器(EE)轨迹接口？为什么它比关节角接口更适合跨平台复用？
扩散模型(Diffusion Model)的前向过程和反向过程分别做什么？训练目标是什么？
一个 RL 训练的全身控制器(WBC)的观测空间通常包含哪些量？奖励函数如何设计？
手眼标定(hand-eye calibration)要求解什么方程？AX=XB 的几何含义是什么？
在 Sim-to-Real 部署中，动作延迟(action delay)会导致什么问题？常见的缓解方法有哪些？

本章目标¶

学完本章，学员应能：

解释 UMI-on-Legs 的三层架构设计——感知、规划、执行各层的职责边界
从零推导 DDPM/DDIM 的去噪过程——理解条件扩散策略如何生成动作序列
设计 Action Chunking 的时序管线——包括插值、过期检查、延迟补偿
实现任务帧注册——从示教数据中提取任务坐标系并完成部署时的坐标变换
分析 14 物体评估的失败模式——将失败归因到系统的具体层次

90.1 动机：为什么要把 UMI 放在腿上？ ⭐⭐¶

90.1.1 固定基座操作的局限¶

在传统机器人操作研究中，机械臂被安装在固定桌面上。这种设置有一个隐含假设：操作对象永远在臂的工作空间之内。这个假设在工厂产线上成立——零件被传送带精确地送到机器人面前。但在家庭、仓库、户外等非结构化环境中，这个假设彻底失败。

考虑一个具体场景：让机器人从厨房台面上拿起一个杯子，走到客厅放在茶几上。固定基座的臂只能覆盖约 0.8m 半径的工作空间。整个任务需要跨越 5-10m 的距离——这已经超出了任何单臂的物理极限。

传统的解决方案是"移动底盘 + 机械臂"的两阶段方案：先移动到目标附近，停稳，再执行操作。但这种方案有三个根本缺陷：

缺陷	具体表现	根本原因
移动和操作割裂	必须先停下来再操作	底盘控制器和臂控制器各自独立
定位精度要求高	停稳后臂的起始位姿必须精确	两个控制器之间没有闭环耦合
动态环境失效	目标移动时需要重新规划	移动-停止-操作的串行流程太慢

本质洞察：固定基座操作的核心限制不是工作空间大小，而是**移动和操作无法同时发生**。四足机器人 + 机械臂的组合天然解决了这个问题——腿可以提供移动性，同时臂可以持续操作。但代价是控制复杂度急剧上升：腿的运动会扰动臂的基座，臂的负载会改变身体的质心分布。

90.1.2 为什么选四足而不是轮式？¶

轮式底盘更简单、更稳定，为什么 UMI-on-Legs 选择四足？

这个选择不是学术上的炫技，而是工程上的必要。非结构化环境的地形——门槛、台阶、不平地面、柔软地毯——对轮式底盘是灾难性的障碍。四足机器人能跨越这些障碍，代价是引入了一个新问题：基座持续晃动。行走时机身的俯仰(pitch)和滚转(roll)波动可达 ±5°，上下颠簸幅度约 2-3cm。这些扰动会直接传递到安装在背上的机械臂末端。

如果不处理这个问题会怎样？假设操作策略输出的是世界坐标系下的末端目标位置 $\mathbf{p}_{world}^{ee,des}$，而底层控制器只做简单的逆运动学：

\[\mathbf{q}_{arm} = \text{IK}(\mathbf{p}_{world}^{ee,des})\]

当机身向右倾斜 3° 时，如果逆运动学没有考虑基座姿态变化，末端实际位置会偏移约 $L_{arm} \cdot \sin(3°) \approx 0.5 \text{m} \times 0.052 \approx 2.6 \text{cm}$。对于精密操作（如插拔 USB），这个误差是不可接受的。

UMI-on-Legs 的解决方案是：让底层全身控制器(WBC)同时控制腿和臂，在行走的同时主动补偿基座扰动。这就是"manipulation-centric whole-body control"的含义——WBC 的首要目标是让末端稳定，行走只是手段。

90.1.3 UMI 的核心思想：示教数据的跨平台复用¶

UMI (Universal Manipulation Interface, Chi et al., RSS 2024) 的原始版本是一个手持夹爪数据采集工具。操作员手持 UMI 夹爪完成操作示教，GoPro 相机记录腕部视角，SLAM 系统记录夹爪的 6-DoF 轨迹。这些数据被用来训练 Diffusion Policy。

关键洞察在于：UMI 采集的数据完全不涉及机器人本体。数据记录的是"夹爪相对于任务物体应该怎么运动"，而不是"某台机器人的关节应该怎么转"。这意味着同一份示教数据可以部署到任何拥有相似夹爪的机器人上——只要底层控制器能跟踪末端轨迹。

UMI 数据流:
┌──────────────┐    ┌───────────────────┐    ┌──────────────────┐
│ 手持示教      │───→│ 任务帧 EE 轨迹     │───→│ Diffusion Policy │
│ (GoPro+SLAM) │    │ {T_task^ee(t)}    │    │ (条件去噪生成)    │
└──────────────┘    └───────────────────┘    └──────────────────┘
                                                      │
                    ┌────────────────────────────────────┘
                    │ 部署时：任务帧 EE 轨迹
                    ▼
        ┌───────────────────────┐
        │  固定臂 / 四足臂 / ...│  ← 不同机器人，同一接口
        │  底层 IK 或 RL WBC    │
        └───────────────────────┘

UMI-on-Legs (He et al., CoRL 2024) 的贡献是：证明这个接口可以延伸到**移动操作**——把 UMI 数据训练的策略直接部署到四足机器人背上的臂上，由 RL 训练的全身控制器执行末端跟踪。

时间线:
2023.03 ─ Chi et al. ─ Diffusion Policy (RSS 2024 Best Paper)
    │
2024.01 ─ Chi et al. ─ UMI: 手持夹爪通用示教接口 (RSS 2024)
    │
2024.07 ─ He et al. ─ UMI-on-Legs: 移动底盘上的 UMI (CoRL 2024)
    │
2025    ─ UMI-3D ─ 3D 空间感知扩展

90.1.4 论文核心贡献总结¶

贡献	具体内容	为什么重要
跨平台接口	任务帧 EE 轨迹作为操作策略和底层控制器的接口	解耦了上层策略和底层硬件
Manipulation-centric WBC	RL 训练的 WBC 以末端跟踪为首要目标	行走扰动不影响操作精度
零样本迁移	预训练的 Diffusion Policy 无需微调直接用	证明接口设计的有效性
14 物体评估	系统性的真机实验 + 失败分析	暴露了哪些环节是瓶颈

⚠️ Pitfall: 初学者容易把 UMI-on-Legs 理解为"把臂装在狗上然后训练一个端到端策略"。实际上它的核心不是端到端，而是**分层 + 接口设计**——上层策略完全不知道下面是什么机器人，下层 WBC 完全不知道上面在执行什么任务。这种解耦是系统能工作的关键。

练习 90.1a (⭐): 计算一个 0.5m 臂长的机械臂，当基座俯仰角波动 ±5° 时，末端位置在垂直方向的偏移范围。如果任务要求精度为 5mm，WBC 需要在多长时间内完成补偿？（提示：考虑四足行走的步态频率约 2Hz。）

练习 90.1b (⭐⭐): 对比"移动-停止-操作"和"移动中操作"两种范式。列出各自的优势场景和失败场景。在哪些任务中"移动中操作"是不可替代的？

90.2 系统架构总览：感知→规划→执行的三层分工 ⭐⭐¶

90.2.1 整体架构¶

UMI-on-Legs 的系统分为三个层次，每层有明确的输入、输出和职责边界。理解这三层的分工是理解整个系统的基础。

┌─────────────────────────────────────────────────────────────┐
│                     Layer 1: 感知层                          │
│  输入: 腕部相机 RGB 图像, 机器人本体感知                      │
│  输出: 条件向量 c_t (视觉特征 + 位姿历史)                    │
│  频率: 10-15 Hz (受相机帧率和推理速度限制)                    │
├─────────────────────────────────────────────────────────────┤
│                     Layer 2: 规划层                          │
│  输入: 条件向量 c_t                                         │
│  输出: 未来 EE 轨迹 chunk {T_task^ee(t+k·Δt)}_{k=0}^{K-1}  │
│  频率: 2-5 Hz (DDIM 10步去噪约 50-100ms)                    │
│  核心: Diffusion Policy + Action Chunking                   │
├─────────────────────────────────────────────────────────────┤
│                     Layer 3: 执行层                          │
│  输入: body-frame EE 目标 + 本体感知                         │
│  输出: 12+N 个关节 PD 目标 (4腿×3关节 + N臂关节)             │
│  频率: 50-100 Hz (RL WBC 策略推理)                           │
│  核心: RL-trained Whole-Body Controller                     │
└─────────────────────────────────────────────────────────────┘

三层之间的频率差异是设计的核心难点。感知层以 10-15Hz 提供观测，规划层以 2-5Hz 输出动作片段，执行层以 50-100Hz 发送关节命令。这种频率金字塔要求精心设计的缓冲和插值机制来弥合速率差异。

如果三层以相同频率运行会怎样？假设全部以 50Hz 运行：Diffusion Policy 的单次推理需要约 100ms（DDIM 10 步），远超 20ms 的周期要求。降低执行层频率到 5Hz 以匹配规划层？关节控制在 5Hz 下完全无法抑制高频振动——四足行走的足底冲击频率约 20-30Hz，5Hz 的采样率连 Nyquist 条件都不满足。

本质洞察：三层频率不同不是设计缺陷，而是**物理必然**。视觉处理、生成式推理、关节伺服——三者的计算密度和物理时间尺度天然不同。UMI-on-Legs 的架构巧妙地利用了 Action Chunking 来桥接这些时间尺度：规划层一次性输出未来 0.5-1 秒的动作，执行层从缓存中逐帧取用。

90.2.2 接口设计：任务帧 EE 轨迹¶

三层之间的接口是理解系统的关键。上层（规划层）和下层（执行层）之间的接口是**任务帧(task frame)下的末端执行器轨迹**：

\[\mathcal{C}_t = \{(T_{\text{task}}^{ee}(t + k \cdot \Delta t),\ g(t + k \cdot \Delta t))\}_{k=0}^{K-1}\]

其中 $T_{\text{task}}^{ee} \in SE(3)$ 是末端在任务帧中的位姿，$g \in [0,1]$ 是夹爪开合度，$K$ 是 chunk 长度（通常 16-32 步），$\Delta t$ 是规划层的时间步长。

为什么选择任务帧而不是世界帧或机身帧？

坐标系选择	优势	劣势
世界帧	全局一致	示教和部署的世界帧必须对齐
机身帧	不受移动影响	行走时机身晃动，目标相对于物体在抖
任务帧	与任务物体绑定	需要实时估计任务帧位姿

任务帧通常绑定在操作对象附近——例如门把手的坐标系、抽屉的坐标系、杯子的坐标系。Diffusion Policy 学习的是"相对于任务物体，夹爪应该怎么运动"，这与在哪台机器人上执行、机器人站在什么位置完全无关。

从任务帧到执行层需要的机身帧(body frame)目标，需要一条坐标变换链：

\[T_{\text{body}}^{ee} = \left(T_{\text{world}}^{\text{body}}\right)^{-1} \cdot T_{\text{world}}^{\text{task}} \cdot T_{\text{task}}^{ee}\]

每一项的来源和失败模式：

变换	来源	更新频率	典型误差源
$T_{\text{world}}^{\text{body}}$	IMU + 腿部里程计	100Hz+	漂移、冲击跳变
$T_{\text{world}}^{\text{task}}$	视觉定位 / 初始注册	任务开始时一次	标定误差、遮挡
$T_{\text{task}}^{ee}$	Diffusion Policy 输出	2-5Hz	模型泛化误差

90.2.3 与 VBC 的对比预览¶

UMI-on-Legs 使用的是末端轨迹接口——上层输出 $T_{\text{task}}^{ee}$，下层跟踪。另一种方案是 VBC (Velocity-Based Control)，上层输出末端速度指令 $\mathbf{v}_{ee}^{des}$。

两者的设计差异深刻影响了整个系统的行为：

UMI-on-Legs 接口:               VBC 接口:
上层: "到这个位置"               上层: "以这个速度移动"
      ↓ 位置目标                       ↓ 速度指令
下层: 跟踪误差 → 关节命令         下层: 速度映射 → 关节命令
      ↓                               ↓
优势: 可预测、可回放              优势: 响应快、连续
劣势: 对延迟敏感                  劣势: 积分漂移

💡 Insight: 位置接口 vs 速度接口的选择，类似于 PD 控制中"位置环"vs"速度环"的经典争论。位置接口保证了终态正确性（最终会到达目标位置），但对延迟敏感（过时的位置目标会导致突然跳变）。速度接口响应更快但需要积分器，容易漂移。UMI-on-Legs 选择位置接口是因为 Diffusion Policy 天然输出位置序列，而且 Action Chunking 提供了面对延迟的缓冲。

⚠️ Pitfall: 初学者常认为"位置接口比速度接口简单"。实际上位置接口在移动操作场景中更难——因为机身在移动，世界帧下的 EE 位置目标在不断变化。如果坐标变换链有任何一环出错（例如里程计漂移了 5cm），末端就会偏离目标。

练习 90.2a (⭐): 画出从 Diffusion Policy 输出到关节 PD 目标的完整信号流图，标注每一步的数据格式、坐标系和频率。

练习 90.2b (⭐⭐): 如果 $T_{\text{world}}^{\text{body}}$ 的估计存在 3cm 的恒定偏差，这个偏差会如何传递到末端？在什么条件下这个偏差不影响任务成功？

90.3 Diffusion Policy 深度解析 ⭐⭐⭐¶

90.3.1 从生成模型到机器人策略¶

Diffusion Policy (Chi et al., RSS 2024) 的核心思想是把机器人策略建模为一个**条件生成模型**：给定当前的观测，从噪声中"去噪"出一个动作序列。

为什么不直接用回归模型（如 MLP）预测动作？考虑一个简单例子：面前有一个杯子，你可以从左边抓或从右边抓。两种方式都是正确的。如果用回归模型（最小化 MSE），它会预测两种方式的**平均值**——从正上方抓。但正上方可能是一种糟糕的抓取姿态（例如手指无法合拢）。

这就是**多模态性(multi-modality)**问题。示教数据中同一个场景可能有多种正确的操作方式。回归模型只能给出平均解，而扩散模型可以**采样**不同模态。

回归模型 vs 扩散模型:

回归:    观测 ──→ [MLP] ──→ 单一动作 (均值)
                              ↕ 平均了多个模态
                              ↕ 可能是非法动作

扩散:    观测 + 噪声 ──→ [去噪网络 × T步] ──→ 一个模态的动作
                   ↕ 从噪声采样                ↕ 是某个真实模态
                   ↕ 不同噪声给出不同模态

跨领域类比：Diffusion Policy 之于机器人控制，类似 Stable Diffusion 之于图像生成。Stable Diffusion 从随机噪声生成图像，条件是文本描述；Diffusion Policy 从随机噪声生成动作序列，条件是视觉观测。两者用的是同一个数学框架，只是"生成什么"不同。

90.3.2 DDPM 前向过程：逐步加噪¶

**前向过程(forward process)**定义了如何从干净数据逐步添加噪声。设 $\mathbf{a}_0$ 是一个干净的动作序列（从示教数据中采样），前向过程定义一个 Markov 链：

\[q(\mathbf{a}_t | \mathbf{a}_{t-1}) = \mathcal{N}(\mathbf{a}_t;\ \sqrt{1 - \beta_t}\, \mathbf{a}_{t-1},\ \beta_t \mathbf{I})\]

其中 $\beta_t \in (0, 1)$ 是预定义的噪声调度表(noise schedule)，$t = 1, 2, \dots, T$。

直觉理解：每一步把上一步的结果乘以一个略小于 1 的系数（缩小信号），再加上一个小的高斯噪声。经过 $T$ 步之后，原始信号被完全淹没在噪声中。

利用高斯分布的加法性质，可以推导出**任意时刻**的边缘分布（不需要逐步模拟）：

推导过程：

定义 $\alpha_t = 1 - \beta_t$，$\bar{\alpha}_t = \prod_{s=1}^{t} \alpha_s$。

Step 1: 写出一步转移： $$\mathbf{a}_t = \sqrt{\alpha_t}\, \mathbf{a}_{t-1} + \sqrt{1 - \alpha_t}\, \boldsymbol{\epsilon}_{t-1}, \quad \boldsymbol{\epsilon}_{t-1} \sim \mathcal{N}(\mathbf{0}, \mathbf{I})$$

Step 2: 递归展开两步： $$\mathbf{a}_t = \sqrt{\alpha_t}\left(\sqrt{\alpha_{t-1}}\, \mathbf{a}_{t-2} + \sqrt{1-\alpha_{t-1}}\, \boldsymbol{\epsilon}_{t-2}\right) + \sqrt{1-\alpha_t}\, \boldsymbol{\epsilon}_{t-1}$$

\[= \sqrt{\alpha_t \alpha_{t-1}}\, \mathbf{a}_{t-2} + \underbrace{\sqrt{\alpha_t(1-\alpha_{t-1})}\, \boldsymbol{\epsilon}_{t-2} + \sqrt{1-\alpha_t}\, \boldsymbol{\epsilon}_{t-1}}_{\text{两个独立高斯的和}}\]

Step 3: 两个独立高斯 $\mathcal{N}(0, \sigma_1^2 I)$ 和 $\mathcal{N}(0, \sigma_2^2 I)$ 的和服从 $\mathcal{N}(0, (\sigma_1^2 + \sigma_2^2) I)$。验证方差：

\[\alpha_t(1-\alpha_{t-1}) + (1-\alpha_t) = \alpha_t - \alpha_t\alpha_{t-1} + 1 - \alpha_t = 1 - \alpha_t\alpha_{t-1} = 1 - \bar{\alpha}_t\]

Step 4: 归纳到任意 $t$：

\[\boxed{q(\mathbf{a}_t | \mathbf{a}_0) = \mathcal{N}(\mathbf{a}_t;\ \sqrt{\bar{\alpha}_t}\, \mathbf{a}_0,\ (1 - \bar{\alpha}_t) \mathbf{I})}\]

等价地：$\mathbf{a}_t = \sqrt{\bar{\alpha}_t}\, \mathbf{a}_0 + \sqrt{1 - \bar{\alpha}_t}\, \boldsymbol{\epsilon}, \quad \boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0}, \mathbf{I})$

为什么这个公式重要？ 训练时不需要模拟 $T$ 步 Markov 链，而是直接从 $\mathbf{a}_0$ 跳到任意 $t$ 时刻的噪声样本。这让训练效率提高了几个数量级。

当 $t = T$ 且 $\bar{\alpha}_T \approx 0$ 时，$q(\mathbf{a}_T | \mathbf{a}_0) \approx \mathcal{N}(\mathbf{0}, \mathbf{I})$——原始信号几乎完全消失，只剩纯噪声。

90.3.3 DDPM 反向过程：逐步去噪¶

**反向过程(reverse process)**从纯噪声出发，逐步去噪恢复出干净的动作序列。真实的反向条件分布 $q(\mathbf{a}_{t-1} | \mathbf{a}_t)$ 是难以计算的（需要知道全局数据分布），但当 $\beta_t$ 足够小时，它近似为高斯分布。

我们用一个参数化的神经网络 $p_\theta$ 来拟合反向过程：

\[p_\theta(\mathbf{a}_{t-1} | \mathbf{a}_t, \mathbf{c}) = \mathcal{N}(\mathbf{a}_{t-1};\ \boldsymbol{\mu}_\theta(\mathbf{a}_t, t, \mathbf{c}),\ \sigma_t^2 \mathbf{I})\]

其中 $\mathbf{c}$ 是条件信息（视觉观测），$\boldsymbol{\mu}_\theta$ 是网络预测的去噪均值。

关键的参数化技巧：与其让网络直接预测均值 $\boldsymbol{\mu}_\theta$，不如让网络预测**噪声** $\boldsymbol{\epsilon}_\theta$。利用前向过程的公式，可以从预测的噪声反推均值：

\[\boldsymbol{\mu}_\theta(\mathbf{a}_t, t, \mathbf{c}) = \frac{1}{\sqrt{\alpha_t}} \left(\mathbf{a}_t - \frac{\beta_t}{\sqrt{1 - \bar{\alpha}_t}} \boldsymbol{\epsilon}_\theta(\mathbf{a}_t, t, \mathbf{c})\right)\]

推导：

已知 $\mathbf{a}_t = \sqrt{\bar{\alpha}_t}\, \mathbf{a}_0 + \sqrt{1-\bar{\alpha}_t}\, \boldsymbol{\epsilon}$，反解 $\mathbf{a}_0$：

\[\mathbf{a}_0 = \frac{\mathbf{a}_t - \sqrt{1-\bar{\alpha}_t}\, \boldsymbol{\epsilon}}{\sqrt{\bar{\alpha}_t}}\]

真实后验 $q(\mathbf{a}_{t-1} | \mathbf{a}_t, \mathbf{a}_0)$（注意这里条件包含 $\mathbf{a}_0$，是可以精确计算的）的均值为：

\[\tilde{\boldsymbol{\mu}}_t = \frac{\sqrt{\bar{\alpha}_{t-1}} \beta_t}{1-\bar{\alpha}_t} \mathbf{a}_0 + \frac{\sqrt{\alpha_t}(1-\bar{\alpha}_{t-1})}{1-\bar{\alpha}_t} \mathbf{a}_t\]

将 $\mathbf{a}_0$ 的表达式代入，经过整理得到上式。网络 $\boldsymbol{\epsilon}_\theta$ 预测的是 $\boldsymbol{\epsilon}$（加入的噪声），从而间接给出均值。

**训练损失**非常简洁：

\[\boxed{\mathcal{L}_{\text{simple}} = \mathbb{E}_{t, \mathbf{a}_0, \boldsymbol{\epsilon}} \left[\|\boldsymbol{\epsilon} - \boldsymbol{\epsilon}_\theta(\underbrace{\sqrt{\bar{\alpha}_t}\,\mathbf{a}_0 + \sqrt{1-\bar{\alpha}_t}\,\boldsymbol{\epsilon}}_{\mathbf{a}_t},\ t,\ \mathbf{c})\|^2\right]}\]

直觉：随机选一个时间步 $t$，给数据加噪得到 $\mathbf{a}_t$，让网络预测加入的噪声 $\boldsymbol{\epsilon}$，MSE 越小越好。

90.3.4 DDIM 加速采样¶

DDPM 需要 $T=100\text{-}1000$ 步去噪，每步一次网络推理。在机器人上以 100 步跑一个 U-Net 需要约 500ms——完全无法满足实时性要求。

DDIM (Song et al., ICLR 2021) 的关键发现：去噪过程不需要是 Markov 的。DDIM 构造了一个非 Markov 的反向过程，使得可以跳过中间步骤，用 10-25 步达到 DDPM 100 步的质量。

DDIM 的更新公式：

\[\mathbf{a}_{t-1} = \sqrt{\bar{\alpha}_{t-1}} \underbrace{\left(\frac{\mathbf{a}_t - \sqrt{1-\bar{\alpha}_t}\, \boldsymbol{\epsilon}_\theta(\mathbf{a}_t, t, \mathbf{c})}{\sqrt{\bar{\alpha}_t}}\right)}_{\text{"预测的" } \mathbf{a}_0} + \sqrt{1-\bar{\alpha}_{t-1} - \sigma_t^2}\, \boldsymbol{\epsilon}_\theta(\mathbf{a}_t, t, \mathbf{c}) + \sigma_t \boldsymbol{\epsilon}_t\]

当 $\sigma_t = 0$ 时，过程变为**完全确定性**——给定相同的初始噪声和条件，总是生成相同的动作序列。这个性质对调试非常有用。

采样方法	步数	推理时间 (RTX 3090)	推理时间 (Jetson Orin)	质量
DDPM	100	~500ms	~2s	最优
DDIM-25	25	~125ms	~500ms	接近 DDPM
DDIM-10	10	~50ms	~200ms	略降，实用
DDIM-5	5	~25ms	~100ms	明显下降

反事实推理：如果 DDIM 没有被发明，Diffusion Policy 在机器人上根本不可行——500ms 的推理延迟意味着机器人每秒只能做 2 次决策，远不够精细操作。DDIM-10 把延迟压到 50ms，配合 Action Chunking（一次输出 16 步动作），有效控制频率达到 10Hz，这才进入实用区间。

90.3.5 条件扩散策略的网络架构¶

Diffusion Policy 使用 U-Net 或 Transformer 作为去噪网络。在机器人操作中，条件 $\mathbf{c}$ 包括：

条件向量 c 的构成:
┌──────────────────────────────────────────┐
│ 视觉特征 (腕部相机)                       │
│   └─ ResNet-18 提取 → 512维向量          │
│ 本体感知历史                              │
│   └─ 最近 T_obs 步的 EE 位姿和夹爪状态   │
│ 时间步编码                                │
│   └─ sinusoidal embedding of t           │
└──────────────────────────────────────────┘

去噪网络的输入是噪声动作序列 $\mathbf{a}_t \in \mathbb{R}^{K \times d_a}$（$K$ 步，每步 $d_a$ 维），输出是预测噪声 $\boldsymbol{\epsilon}_\theta \in \mathbb{R}^{K \times d_a}$。

⚠️ Pitfall: Diffusion Policy 的去噪网络不是在单个动作上工作，而是在**整个动作序列**上工作。这意味着网络需要同时考虑序列内部的时序一致性。如果把 $K$ 步动作当作独立样本分别去噪，生成的序列会不连续——相邻动作之间可能有大的跳变。

练习 90.3a (⭐⭐): 从 DDPM 的训练损失出发，推导当 $t$ 很大（接近 $T$）时，网络实际上在学习什么；当 $t$ 很小（接近 0）时，网络在学习什么。提示：考虑 $\bar{\alpha}_t$ 在两个极端的值。

练习 90.3b (⭐⭐⭐): 实现一个最小的 1D Diffusion Policy：状态是一个标量 $x$，动作是一个标量 $a$，示教数据是 $a = \sin(x)$ 和 $a = -\sin(x)$ 两个模态的混合。验证扩散模型能采样出两个模态，而 MSE 回归只能给出 $a = 0$。

90.4 Action Chunking：动作片段的时序设计 ⭐⭐⭐¶

90.4.1 为什么需要 Action Chunking？¶

回顾 90.2 中的频率金字塔：Diffusion Policy 以 2-5Hz 输出，执行层以 50-100Hz 运行。如果每次推理只输出**下一步**的动作，那么在推理的 50-200ms 期间，执行层没有新的目标可用——机器人要么停住等待，要么使用过时的目标。

Action Chunking 的解决方案是：每次推理输出未来 K 步的动作序列（一个"chunk"）。执行层从 chunk 中按时间顺序取用动作，直到新的 chunk 到达。

时间轴:
t=0     t=1     t=2     t=3     t=4     t=5     t=6     t=7
│       │       │       │       │       │       │       │
│ 推理开始       │ 推理完成,chunk 到达
│       │       │ [a0, a1, a2, a3, a4, a5, a6, a7]
│       │       │ ←执行→  ←执行→  ←执行→  ←执行→ ...
│       │       │                 │ 新推理开始
│       │       │                 │       │ 新chunk到达
│       │       │                 │       │ [b0, b1, b2, ...]

90.4.2 Chunk 参数设计¶

Action Chunking 有三个关键参数：

参数	符号	典型值	影响
Chunk 长度	$K$	16-32	过短→频繁等待；过长→预测不准
执行步数	$K_{exec}$	8-16	过少→浪费计算；过多→使用过时预测
观测窗口	$T_{obs}$	2-4	过短→缺乏上下文；过长→特征模糊

执行步数 vs Chunk 长度：虽然每个 chunk 有 $K$ 步动作，但通常只执行前 $K_{exec} < K$ 步就丢弃，等待新 chunk。后面的动作预测精度随时间衰减——越远的未来越难准确预测。

跨领域类比：Action Chunking 与 MPC 的 receding horizon 思想异曲同工。MPC 也是每步规划未来 $N$ 步，只执行第一步然后重新规划。区别在于 MPC 通过优化产生预测，Diffusion Policy 通过生成式推理产生预测。两者都遵循同一个原则：规划要看远处，执行只用近处。

90.4.3 Chunk 重叠与时序插值¶

当新 chunk 到达时，旧 chunk 可能还没执行完。这创造了一个重叠区域——同一时刻有两个（甚至更多个）chunk 给出了不同的动作预测。

UMI-on-Legs 采用**指数衰减混合**来处理重叠：

\[\mathbf{a}_{exec}(t) = w \cdot \mathbf{a}_{new}(t) + (1-w) \cdot \mathbf{a}_{old}(t), \quad w = 1 - e^{-\lambda \cdot \Delta t_{new}}\]

其中 $\Delta t_{new}$ 是新 chunk 到达后经过的时间，$\lambda$ 控制过渡速度。

如果不做混合直接切换会怎样？新旧 chunk 的预测在同一时刻可能相差数厘米。直接切换会产生动作跳变，执行层会响应这个跳变并产生大的关节加速度，轻则导致操作不平滑，重则触发安全保护。

无混合切换:                    有混合过渡:
EE位置                         EE位置
  │    旧chunk                   │    旧chunk
  │   /                          │   /
  │  /                           │  / ─── 平滑过渡
  │ /   跳变!                    │ /  /
  │/    │                        │/ /
  │     │ 新chunk                │ /  新chunk
  │     │/                       │/
  └─────────── t                 └─────────── t

90.4.4 过期检查与安全降级¶

Action Chunk 有一个隐含的时间戳。当以下情况发生时，chunk 应被标记为"过期"：

推理延迟：Diffusion Policy 推理时间超过预期，chunk 到达时已"迟到"
感知中断：相机帧丢失，当前 chunk 是基于过时观测生成的
大扰动：机器人被踢或绊倒，当前 chunk 的前提假设已不成立

过期检查机制：

def check_chunk_validity(chunk, current_state, config):
    """检查 action chunk 是否仍然有效"""
    # 1. 时间戳检查：chunk 是否太旧？
    age = current_time - chunk.timestamp
    if age > config.max_chunk_age:  # 典型值: 0.5s
        return EXPIRED, "chunk 超时"

    # 2. 状态偏差检查：当前状态是否偏离 chunk 的起始假设？
    ee_deviation = norm(current_ee_pose - chunk.expected_ee_pose_at(current_time))
    if ee_deviation > config.max_ee_deviation:  # 典型值: 5cm
        return DEVIATED, "末端偏离过大"

    # 3. 安全检查：chunk 中剩余动作是否会导致危险？
    for future_action in chunk.remaining_actions():
        if not is_within_workspace(future_action):
            return UNSAFE, "未来动作超出工作空间"

    return VALID, "chunk 有效"

当 chunk 被判定为无效时，安全降级策略有三种选择：

策略	行为	适用场景
冻结	保持当前 EE 位姿不变，等待新 chunk	短暂延迟
回缩	慢速将 EE 移向安全位姿（如身体上方）	感知中断
紧停	锁定所有关节，触发 E-stop	硬件故障

⚠️ Pitfall: Action Chunking 最隐蔽的 bug 是**时间戳不对齐**。如果 chunk 的时间戳用的是 GPU 的时钟，而执行层用的是 CPU 的时钟，两个时钟之间可能有毫秒级偏差。在 10Hz 的控制频率下，1ms 的偏差看起来不大；但如果 chunk 的时间步 $\Delta t = 50\text{ms}$，1ms 偏差意味着在 50 个 chunk 后累积到 50ms——整整一个时间步的漂移。所有时钟必须同步到同一个时间源（通常是系统单调时钟 CLOCK_MONOTONIC）。

练习 90.4a (⭐⭐): 设计一个实验：比较 $K_{exec} = K$（执行完整 chunk）和 $K_{exec} = K/2$（只执行前半）的表现差异。预测哪种设置在以下场景中更好：(1) 静态环境中的精密操作；(2) 动态环境中的快速抓取。

练习 90.4b (⭐⭐⭐): 实现指数衰减混合的 chunk 融合算法。输入是旧 chunk 和新 chunk 的动作序列以及时间戳，输出是融合后的执行动作。考虑 SE(3) 空间中的插值（位置用线性插值，旋转用 SLERP）。

90.5 任务帧注册：从示教数据中提取任务坐标系 ⭐⭐⭐¶

90.5.1 什么是任务帧？¶

任务帧(task frame)是绑定在操作对象上的坐标系。它的原点通常选在操作的"焦点"——门把手的转轴、杯子的中心、抽屉的把手中心。它的朝向按照任务的自然方向定义——例如开门任务中，任务帧的 x 轴指向门的打开方向。

为什么不直接用世界帧？因为同一个任务（比如"打开门"）在不同位置执行时，世界帧下的 EE 轨迹完全不同。但在任务帧下，无论门在房间的哪个位置，"推门把手"的运动模式是相同的。

反事实推理：如果 Diffusion Policy 在世界帧下训练，它需要记住"门在 (1.2, 3.5) 米处时右手从右边推，门在 (0.5, 2.1) 米处时左手从左边推"——这需要海量数据覆盖所有可能的空间位置。而在任务帧下，一个位置的示教就足以泛化到所有位置。

90.5.2 任务帧的注册流程¶

在部署时，系统需要知道任务帧在世界坐标系中的位置——这个过程叫做"任务帧注册(task frame registration)"。UMI-on-Legs 使用以下流程：

任务帧注册流程:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
Step 1: 操作员手动指定任务帧
   └─ 方式 A: 让机器人的夹爪触碰操作对象的参考点
   └─ 方式 B: 视觉检测物体位姿 (ArUco/AprilTag)
   └─ 方式 C: 使用示教时记录的参考位姿

Step 2: 记录 T_world^task
   └─ 从 Step 1 的结果得到任务帧在世界中的位置

Step 3: 在任务执行期间保持 T_world^task 不变
   └─ 假设操作对象不移动 (静态任务帧)
   └─ 或实时更新 (动态任务帧, 需要持续的视觉追踪)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

静态 vs 动态任务帧：

场景	任务帧类型	注册方式
打开固定的门	静态	开始时注册一次
从移动传送带上取物	动态	视觉持续追踪
在行走中抓桌上物体	准静态	定期重新注册

90.5.3 坐标变换链的数值稳定性¶

完整的坐标变换链中，每一步都可能引入误差。误差在链式乘法中**累积**而非抵消。

设各环节的位置误差为 $\delta_1, \delta_2, \delta_3$，末端的总误差：

\[\|\delta_{total}\| \leq \|\delta_1\| + \|\delta_2\| + \|\delta_3\| + \text{交叉项}\]

旋转误差的情况更复杂。两个旋转矩阵各有小角度误差 $\delta R_1, \delta R_2$ 时：

\[R_{total} = (R_1 \cdot \delta R_1)(R_2 \cdot \delta R_2) = R_1 R_2 \cdot R_2^{-1} \delta R_1 R_2 \cdot \delta R_2\]

交叉项 $R_2^{-1} \delta R_1 R_2$ 表明：上游的旋转误差会被下游的旋转"放大"。如果 $R_2$ 是一个大角度旋转，它会把 $\delta R_1$ 的误差方向旋转到不同的轴上，可能从一个不敏感的方向变到一个敏感的方向。

⚠️ Pitfall: 坐标变换中最常见的 bug 是**四元数未归一化**。经过多次乘法后，浮点误差导致四元数的模不再是 1。模偏离 1 超过 $10^{-6}$ 时，旋转矩阵不再正交，逆运动学会给出错误的关节角。正确做法是每次乘法后重新归一化：q = q / q.norm()。

90.5.4 从 UMI 示教数据中提取任务帧¶

UMI 的示教数据由 GoPro 相机的 SLAM 系统提供 6-DoF 夹爪轨迹。但原始轨迹是在**SLAM 坐标系**下的——这个坐标系的原点和朝向取决于 SLAM 初始化时相机的位置和方向，是随机的。

任务帧提取的关键步骤：

选择参考时刻：通常是操作开始的时刻（如夹爪接触物体的瞬间）
定义任务帧原点：参考时刻的夹爪位置（或由操作员指定的物体上的点）
定义任务帧朝向：从操作轨迹的主方向提取

示例：开门任务
━━━━━━━━━━━━━━
原始 SLAM 轨迹: 手持夹爪从左到右画弧

参考时刻: 夹爪碰到门把手的瞬间
任务帧原点: 门把手的位置
任务帧 x 轴: 从门轴到门把手 (门的打开方向)
任务帧 z 轴: 垂直向上

转换后: 所有 EE 轨迹变成 "相对于门把手, 手怎么动"
        → 这个表示与门的世界坐标位置无关

90.5.5 任务帧的鲁棒性分析¶

任务帧注册的精度直接影响操作成功率。让我们量化分析不同误差源的影响。

设任务帧在世界坐标系中的真实位姿为 $T_{world}^{task,true}$，估计的位姿为 $T_{world}^{task,est}$，误差为 $\delta T = T_{world}^{task,est} \cdot (T_{world}^{task,true})^{-1}$。

对于位置误差 $\delta p$ 和旋转误差 $\delta \theta$，末端轨迹的误差放大取决于操作的**空间范围** $L$：

误差类型	末端误差	量级估计 (L=0.3m)
位置 $\delta p$	$\delta p$	直接传递
旋转 $\delta \theta$	$L \sin(\delta\theta) \approx L \cdot \delta\theta$	$\delta\theta = 2° → 0.3 \times 0.035 = 1.05$cm
位置+旋转耦合	$\delta p + L\delta\theta$	叠加

关键观察：旋转误差通过操作范围 $L$ 被放大。对于大范围操作（如整臂伸展 L=0.5m），即使 2° 的任务帧旋转误差也会导致末端偏差 1.7cm——这已经是很多精细操作的容忍上限。

动态任务帧的更新策略：

对于需要动态更新的任务帧（如目标物体可能被移动），更新策略有三种：

策略 A: 每帧更新 (最精确, 最不稳定)
  每次视觉检测 → 更新 T_world^task
  → 问题: 检测噪声直接传递到目标, 导致 EE 抖动

策略 B: 滤波更新 (折中)
  视觉检测 → Kalman 滤波 → 更新 T_world^task
  → 优势: 抑制噪声, 平滑变化
  → 参数: 过程噪声 Q 和观测噪声 R 需要调参

策略 C: 事件触发更新 (最稳定)
  仅当 ||T_new - T_old|| > threshold 时才更新
  → 优势: 避免小幅抖动
  → 问题: 对大的突变反应延迟

在 UMI-on-Legs 中，大部分任务使用**静态任务帧**（策略 C 的极端情况——只在任务开始时注册一次），因为操作对象通常不会自己移动。

练习 90.5a (⭐⭐): 给定两次不同位置的开门示教数据（SLAM 坐标系下），设计算法提取统一的任务帧。提示：利用示教轨迹中夹爪接触门把手的位置作为原点对齐。

练习 90.5b (⭐⭐⭐): 考虑一个"倒水"任务，任务帧应该绑定在杯子上还是水壶上？如果水壶是固定的（放在桌上），杯子是机器人拿着的，答案是否改变？分析两种选择各自的优劣。

90.6 腕部相机管线：视觉观测流水线 ⭐⭐⭐¶

90.6.1 为什么用腕部相机而不是头部相机？¶

UMI-on-Legs 使用安装在夹爪/机械臂腕部的相机作为主要视觉输入。这个选择背后有深刻的工程考量。

头部相机（或身体固定相机）的问题：四足行走时身体持续晃动，头部相机看到的图像也在晃动。更严重的是，操作物体可能被机械臂自身遮挡——当臂在身体前方操作时，头部相机可能根本看不到操作区域。

腕部相机的优势：它与夹爪刚性连接。无论机身怎么晃动，腕部相机看到的是夹爪"眼前"的场景——操作对象始终在视野中央。这和人类的视觉系统不同（人用头部的眼睛看手操作的物体），但对机器人而言更高效。

相机位置	与操作物体的相对运动	遮挡风险	标定复杂度
头部	大(身体+臂运动叠加)	高(臂可能遮挡)	需要全身运动学
肩部	中(臂运动)	中	需要臂运动学
腕部	小(只有手指运动)	低(始终朝向物体)	只需手眼标定
外部固定	无(但受环境限制)	低	需要世界帧标定

90.6.2 视觉特征提取¶

腕部相机输出 RGB 图像（通常 640×480 或 320×240），需要编码为策略可用的特征向量。UMI-on-Legs 使用 ResNet-18 的前几层作为视觉编码器，输出 512 维特征。

完整的视觉管线：

原始图像 (640×480×3)
    │
    ├── 1. 预处理
    │   ├── 裁剪: 去除不相关区域
    │   ├── 缩放: → 224×224 (ResNet 输入尺寸)
    │   └── 归一化: ImageNet 均值/标准差
    │
    ├── 2. 特征提取
    │   ├── ResNet-18 conv1-conv4
    │   ├── Global Average Pooling
    │   └── → 512维特征向量
    │
    └── 3. 时间堆叠
        ├── 保留最近 T_obs 帧的特征
        └── → (T_obs × 512) 的条件矩阵

90.6.3 时间同步：视觉和本体感知的对齐¶

视觉管线的延迟约 30-50ms（相机曝光 + 图像传输 + 特征提取）。本体感知（关节编码器、IMU）的延迟约 1-2ms。如果不对齐，策略收到的条件向量中，视觉和本体感知描述的是**不同时刻**的状态。

在 10Hz 的操作频率下，50ms 的时间差意味着视觉和本体感知之间差了半个控制步。如果机器人正以 0.3m/s 移动，50ms 对应 1.5cm 的位移——这个量级已经足以影响精密操作。

同步策略：

方案 A: 延迟对齐 (推荐)
━━━━━━━━━━━━━━━━━━━━━
把本体感知数据延迟到与视觉对齐
    t_visual  ──→ 时间戳 t-Δ
    t_proprio ──→ 缓存, 取时间戳 t-Δ 的数据
    → 两者对齐, 但整体观测延迟了 Δ

方案 B: 预测补偿
━━━━━━━━━━━━━━━━━━━━━
用本体感知预测未来 Δ 时刻的状态
    t_visual  ──→ 时间戳 t-Δ
    t_proprio ──→ 当前 + 预测到 t-Δ 的差
    → 减少延迟, 但预测不准确引入新误差

⚠️ Pitfall: 视觉管线中最致命的 bug 不是特征提取不好，而是**图像时间戳标错**。如果系统用的是图像**到达 CPU 的时间**而非**相机曝光的时间**，所有基于时间戳的同步都是错的。USB 相机的传输延迟可以有 10-100ms 的抖动（取决于系统负载），必须使用相机硬件时间戳（V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC）。

90.6.4 图像增广与视觉鲁棒性¶

训练 Diffusion Policy 时，视觉输入需要数据增广来提高鲁棒性。但机器人操作的增广策略与图像分类不同——不能随意翻转或大幅旋转图像，因为这些变换会改变操作的空间语义。

适合机器人操作的增广策略：

增广类型	参数范围	适用	不适用
颜色抖动 (brightness/contrast)	±15%	光照变化	暗环境操作
随机裁剪 (crop)	±10%	相机位置微移	大幅裁剪改变空间关系
高斯模糊	σ ∈ [0, 1.5]	焦距不准	过度模糊丢失关键特征
随机擦除	<10% 面积	遮挡鲁棒	擦除操作物体本身
水平翻转	禁止	—	改变左右方向
大角度旋转	禁止	—	改变重力方向的语义

⚠️ Pitfall: 初学者从计算机视觉（图像分类）迁移过来时，习惯使用激进的数据增广（如 RandAugment）。但在机器人操作中，图像的空间结构承载了关键信息——物体在画面上方还是下方对应了不同的物理位置。随机旋转 90° 会让"向上抓取"变成"向右抓取"，完全破坏了动作标签的正确性。

90.6.5 相机内参与外参的标定¶

腕部相机的内参（焦距、畸变系数）和外参（相对于工具中心点的位姿）都需要标定。

内参标定：使用 OpenCV 的 calibrateCamera 函数，采集 15-30 张棋盘格图像。对于 GoPro 等广角相机，特别注意桶形畸变的校正——未校正的畸变会导致图像边缘的物体位置估计偏差达到 3-5%。

外参标定的在线验证方法：部署后在机器人末端安装一个已知尺寸的标记物（如 10cm 的 ArUco 标记），让机器人移动到已知位置，比较视觉估计的标记物位姿与运动学计算的位姿。如果重投影误差 > 5 pixel（在 640×480 图像上），外参需要重新标定。

# 简化的外参验证代码
def verify_hand_eye(T_cam_ee, camera_matrix, T_base_ee, T_base_marker_gt):
    """验证手眼标定的外参精度"""
    # 从相机检测标记物
    T_cam_marker = detect_aruco(image, camera_matrix)

    # 通过运动学 + 手眼外参计算标记物在基座下的位姿
    T_base_marker_est = T_base_ee @ T_cam_ee.inverse() @ T_cam_marker

    # 计算与真值的误差
    pos_error = np.linalg.norm(
        T_base_marker_est[:3, 3] - T_base_marker_gt[:3, 3]
    )
    rot_error = rotation_angle_error(
        T_base_marker_est[:3, :3], T_base_marker_gt[:3, :3]
    )

    print(f"位置误差: {pos_error*100:.1f} cm")
    print(f"旋转误差: {np.degrees(rot_error):.1f} deg")

    assert pos_error < 0.02, "位置误差 > 2cm, 需重新标定"
    assert rot_error < np.radians(3), "旋转误差 > 3°, 需重新标定"

练习 90.6a (⭐⭐): 设计一个实验来测量视觉管线的端到端延迟。提示：在相机视野中放一个快速闪烁的 LED（由已知时间戳的 GPIO 控制），通过比较 LED 闪烁时刻和图像帧中检测到闪烁的时刻来测量延迟。

练习 90.6b (⭐⭐⭐): 如果腕部相机的桶形畸变系数为 $k_1 = -0.3$（典型的 GoPro 值），计算图像边缘（距中心 200 pixel）处一个点的实际位置偏差。如果不校正，这个偏差在 30cm 操作距离下对应多少毫米的 3D 位置误差？

90.7 RL WBC 底层策略：观测与奖励设计 ⭐⭐⭐¶

90.7.1 WBC 的角色¶

UMI-on-Legs 的底层是一个 RL 训练的全身控制器(Whole-Body Controller)。它的唯一职责是：给定 body frame 下的 EE 目标位姿，输出所有关节的 PD 目标，使得腿稳定行走的同时臂精确跟踪。

这与传统的优化型 WBC (如 TSID) 有本质不同：

维度	优化型 WBC (TSID)	RL WBC
计算方式	在线求解 QP	前向推理神经网络
模型需求	精确的 URDF + 接触力	不需要显式模型
鲁棒性	模型精度敏感	通过 domain randomization 鲁棒
可解释性	可分析约束和优先级	黑盒
计算耗时	1-5ms (CPU)	0.1-0.5ms (GPU)

回顾复合/180 Deep-WBC：在那一章我们讨论了 RL WBC 的基本框架——Privileged Learning + Domain Randomization。教师策略在完美信息下训练，学生策略从受限观测中学习。UMI-on-Legs 的 WBC 正是这个框架的一个实例，不同之处在于它的奖励函数以**末端跟踪精度**为最高优先级。

90.7.2 观测空间设计¶

WBC 策略的观测空间需要包含足够的信息来实现稳定行走 + 精确跟踪。典型设计：

观测向量 o_t ∈ R^{d_obs}:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
[本体感知]
  关节角度     q_joint ∈ R^{12+N}     (12腿关节 + N臂关节)
  关节角速度   dq_joint ∈ R^{12+N}
  基座角速度   ω_body ∈ R^3           (来自 IMU)
  基座方向     R_body ∈ R^6           (重力方向的投影)

[目标信息]
  EE 目标位置   p_ee^des ∈ R^3        (body frame)
  EE 目标朝向   R_ee^des ∈ R^6        (6D rotation representation)
  夹爪目标      g^des ∈ R^1

[命令]
  速度命令     v_cmd ∈ R^3            (行走速度/转向)

[历史]
  上一步动作   a_{t-1} ∈ R^{12+N}

总维度: 约 60-80 维 (取决于臂的自由度 N)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

为什么用 6D 旋转表示而不是四元数？ Zhou et al. (CVPR 2019) 证明了 4 维表示（如四元数）不能连续地映射整个 SO(3)——存在不可避免的不连续性。6D 表示（旋转矩阵的前两列）是连续的，神经网络更容易学习。

⚠️ Pitfall: 观测中的"上一步动作"容易被忽略，但它对平滑性至关重要。没有动作历史，策略可能在两个相似的状态下输出差别很大的动作（因为策略是无记忆的 MLP），导致关节抖动。包含上一步动作让策略能保持一定的时间连贯性。

90.7.3 奖励函数设计¶

UMI-on-Legs 的 WBC 是"manipulation-centric"的——末端跟踪精度是最高优先级。奖励函数反映了这个设计意图：

\[r_t = \underbrace{w_{ee} \cdot r_{ee}}_{\text{末端跟踪 (最重要)}} + \underbrace{w_{base} \cdot r_{base}}_{\text{基座稳定}} + \underbrace{w_{smooth} \cdot r_{smooth}}_{\text{动作平滑}} + \underbrace{w_{alive} \cdot r_{alive}}_{\text{存活奖励}}\]

各分项的详细设计：

奖励分项	公式	权重	含义
EE 位置跟踪	$-\\|p_{ee} - p_{ee}^{des}\\|^2$	高 (10.0)	末端位置误差越小越好
EE 朝向跟踪	$-\\|R_{ee}^T R_{ee}^{des} - I\\|_F^2$	高 (5.0)	末端旋转误差
基座高度	$-	h_{base} - h_{nom}	^2$
基座倾斜	$-\\|\theta_{roll,pitch}\\|^2$	中 (1.0)	身体保持水平
动作变化率	$-\\|a_t - a_{t-1}\\|^2$	中 (1.0)	关节命令平滑
关节力矩	$-\\|\tau\\|^2$	低 (0.1)	能量效率
接触奖励	$+1$ if 足底接触正常	中 (1.0)	鼓励正确步态

权重设计的直觉：末端跟踪的权重远高于其他项。这意味着策略在面临"跟踪末端 vs 保持身体水平"的 trade-off 时，会优先选择跟踪末端——即使这意味着身体稍微倾斜。这就是"manipulation-centric"的具体体现。

如果反过来，基座稳定的权重更高会怎样？策略会优先保持身体水平，当行走扰动来临时，它会牺牲末端跟踪精度来维持身体姿态。这在纯行走任务中是正确的，但在操作任务中会导致末端抖动——每次迈步都会影响操作精度。

90.7.4 Domain Randomization¶

WBC 在仿真中训练，需要 Domain Randomization 来缩小 Sim-to-Real Gap：

随机化参数	范围	作用
地面摩擦系数	[0.3, 1.5]	适应不同地面
负载质量	[0, 2.0] kg	适应不同操作对象
基座质量偏移	[-0.5, 0.5] kg	补偿建模误差
电机延迟	[0, 20] ms	适应通信延迟
PD 增益	±20%	补偿执行器参数
地形坡度	[0, 15°]	适应不平地面
传感器噪声	±5%	适应传感器精度

90.7.5 WBC 训练的 Sim-to-Real 关键因素¶

WBC 的 Sim-to-Real 迁移有几个特别需要注意的因素：

执行器模型：仿真中的理想电机（给力矩立刻产生运动）与真实电机差距很大。真实电机有：

力矩延迟：从命令发出到力矩实际产生，约 5-15ms
力矩带宽限制：高频力矩命令会被电机的电气时间常数衰减
反电动势：高速运动时实际可用力矩减少
摩擦和齿隙：减速器的非线性摩擦

理想电机 vs 真实电机:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
理想:  τ_cmd ──→ τ_actual = τ_cmd (瞬时)

真实:  τ_cmd ──→ [延迟 10ms] ──→ [低通滤波]
              ──→ [摩擦+齿隙] ──→ τ_actual ≠ τ_cmd

仿真中的简化模型:
  τ_actual(t) = τ_cmd(t - Δt) * (1 - friction_coeff * sign(dq))
  Δt ~ Uniform(5, 20) ms  ← Domain Randomization
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

地面接触模型：仿真器（MuJoCo, Isaac）的接触模型使用 penalty 方法或解析接触——真实世界的接触更复杂（弹性变形、接触面积随力变化）。Domain Randomization 通过随机化地面刚度和阻尼来缓解这个 gap。

90.7.6 从 Manipulation-Centric 到 Locomotion-Centric 的切换¶

UMI-on-Legs 的 WBC 是 manipulation-centric 的——末端跟踪优先。但有些场景（如爬楼梯时暂时不操作）需要切换到 locomotion-centric 模式。

一种实现方式是**奖励权重的动态调节**：

\[w_{ee}(t) = \begin{cases} w_{ee}^{high} = 10.0 & \text{if 操作模式} \\ w_{ee}^{low} = 1.0 & \text{if 行走模式} \end{cases}\]

但在训练时不能使用两套固定权重（会导致策略在模式边界处行为不连续）。更好的做法是把模式权重作为观测的一部分，让策略学会根据权重信号调整行为。

⚠️ Pitfall: 模式切换时最常见的问题是**末端位姿跳变**。从行走模式切换到操作模式时，如果末端目标突然从"身体上方的默认位姿"变成"操作物体附近的位姿"，跳变可能导致大的关节加速度。正确做法是在切换时用平滑的插值（如最小 jerk 轨迹）从当前位姿过渡到目标位姿，过渡时间约 0.5-1.0s。

练习 90.7a (⭐⭐): 在 Isaac Lab/Gym 中实现一个简化版的 WBC 训练环境。只需要 2D（sagittal plane）的四足模型 + 1-DoF 臂。设计奖励函数使得臂能在行走时保持末端高度恒定。

练习 90.7b (⭐⭐⭐): 设计一个消融实验：分别移除 EE 跟踪奖励和基座稳定奖励，观察策略行为的变化。预测结果并验证。

90.8 夹爪与工具标定 ⭐⭐¶

90.8.1 手眼标定的经典问题¶

腕部相机安装在机械臂末端，其相对于工具中心点(TCP)的变换是固定但未知的。这个变换称为**手眼标定(hand-eye calibration)**。

经典手眼标定问题的数学形式化：

\[\mathbf{A}_i \mathbf{X} = \mathbf{X} \mathbf{B}_i, \quad i = 1, 2, \dots, n\]

其中 $\mathbf{A}_i$ 是两次机器人运动之间的末端位姿变化（来自运动学），$\mathbf{B}_i$ 是对应的相机观测变化（来自视觉），$\mathbf{X}$ 是相机到末端的未知变换。

几何直觉：想象你在转头（末端运动 $\mathbf{A}$），同时看世界在旋转（相机观测变化 $\mathbf{B}$）。$\mathbf{X}$ 就是眼睛在头上的安装位置——知道了头怎么转、世界看起来怎么变，就能反推眼睛装在哪里。

90.8.2 UMI 特有的标定挑战¶

UMI 的手持夹爪使用 GoPro 相机，安装在 3D 打印的支架上。与工业级安装相比：

挑战	原因	影响
安装精度低	3D 打印公差 ±0.5mm	外参初始偏差大
重复性差	每次安装位置略有不同	每次都需要重新标定
运行中松动	操作时的冲击和振动	外参随时间漂移

标定流程：

UMI 标定流程:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
1. 打印标定靶 (ArUco 板)

2. 手持 UMI 夹爪, 从多个角度拍摄标定靶
   → 至少 15 个不同位姿
   → 确保位姿多样性 (不要只在一个平面上)

3. 用 SLAM 系统记录每个位姿的夹爪 6-DoF 位置

4. 用相机检测每帧中标定靶的位姿

5. 求解 AX=XB 得到手眼变换 X

6. 验证: 用独立的测试位姿检查重投影误差
   → 重投影误差 < 2 pixel 为合格
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

⚠️ Pitfall: 手眼标定中最常见的错误是**旋转和平移的量纲不一致**。$\mathbf{A}$ 矩阵中旋转部分是弧度（无量纲），平移部分是米。如果在求解 $\mathbf{AX} = \mathbf{XB}$ 时用 Frobenius 范数做误差度量，平移误差会被旋转误差淹没（旋转值通常 <1 rad，平移值通常 >0.01 m）。正确做法是对旋转和平移分别求解或使用加权范数。

90.8.3 Tsai 方法求解 AX=XB¶

经典的 AX=XB 求解方法由 Tsai & Lenz (1989) 提出。其核心思想是将旋转和平移分离求解。

Step 1: 求解旋转部分

从 $\mathbf{A}_i \mathbf{X} = \mathbf{X} \mathbf{B}_i$ 中分离旋转：

\[\mathbf{R}_A^{(i)} \mathbf{R}_X = \mathbf{R}_X \mathbf{R}_B^{(i)}\]

利用旋转的轴-角表示。设 $\mathbf{R}_A^{(i)}$ 的旋转轴为 $\hat{\mathbf{n}}_A^{(i)}$、角度为 $\theta_A^{(i)}$，$\mathbf{R}_B^{(i)}$ 的旋转轴为 $\hat{\mathbf{n}}_B^{(i)}$、角度为 $\theta_B^{(i)}$。

Tsai 证明了一个关键性质：$\theta_A^{(i)} = \theta_B^{(i)}$（旋转角相同，因为是同一次运动在两个坐标系下的描述），且：

\[\hat{\mathbf{n}}_A^{(i)} = \mathbf{R}_X \hat{\mathbf{n}}_B^{(i)}\]

利用至少两组运动数据，可以建立旋转方程求解 $\mathbf{R}_X$。

Step 2: 求解平移部分

已知 $\mathbf{R}_X$ 后，从 $\mathbf{A}_i \mathbf{X} = \mathbf{X} \mathbf{B}_i$ 中提取平移方程：

\[\mathbf{R}_A^{(i)} \mathbf{t}_X + \mathbf{t}_A^{(i)} = \mathbf{R}_X \mathbf{t}_B^{(i)} + \mathbf{t}_X\]

整理为：

\[(\mathbf{R}_A^{(i)} - \mathbf{I}) \mathbf{t}_X = \mathbf{R}_X \mathbf{t}_B^{(i)} - \mathbf{t}_A^{(i)}\]

这是关于 $\mathbf{t}_X$ 的线性方程组。至少需要 2 组运动数据（实际使用 10+ 组以提高精度），用最小二乘求解。

⚠️ Pitfall: Tsai 方法要求运动对的旋转轴不平行——如果所有运动都围绕同一轴旋转，方程退化（奇异）。采集标定数据时应确保从多个不同方向观察标定靶，旋转轴至少覆盖 3 个线性无关的方向。

90.8.4 在线外参监控¶

在长时间部署中，外参可能因振动松动而漂移。一个实用的在线监控方法：

# 在线外参漂移检测
class HandEyeMonitor:
    def __init__(self, T_cam_ee_calib, threshold_pos=0.005, threshold_rot=0.05):
        self.T_calib = T_cam_ee_calib  # 标定时的外参
        self.threshold_pos = threshold_pos  # 5mm
        self.threshold_rot = threshold_rot  # ~3°
        self.check_history = []

    def check(self, T_base_ee_fk, T_base_marker_visual, T_marker_ee_known):
        """通过已知参考物体检测外参漂移"""
        # 从视觉+运动学反推当前外参
        T_cam_ee_current = (
            T_base_ee_fk.inverse() @ T_base_marker_visual @ T_marker_ee_known
        )

        # 与标定值比较
        delta = self.T_calib.inverse() @ T_cam_ee_current
        pos_drift = np.linalg.norm(delta[:3, 3])
        rot_drift = rotation_angle(delta[:3, :3])

        self.check_history.append((pos_drift, rot_drift))

        if pos_drift > self.threshold_pos or rot_drift > self.threshold_rot:
            return WARNING, f"外参漂移: pos={pos_drift*100:.1f}cm, rot={np.degrees(rot_drift):.1f}°"
        return OK, "外参稳定"

练习 90.8a (⭐⭐): 如果手眼标定的外参 $\mathbf{X}$ 有 1cm 的平移偏差，计算在不同操作距离（10cm, 30cm, 50cm）下末端位置的相对误差。在哪个距离范围内这个偏差可以忽略？

练习 90.8b (⭐⭐⭐): 实现 Tsai 方法的简化版本：给定 5 组 (A, B) 矩阵对，求解 X。用合成数据（已知 X，生成 A 和 B）验证算法的正确性。添加不同水平的噪声（0.1°, 0.5°, 1° 旋转噪声 + 0.1mm, 0.5mm, 1mm 平移噪声），绘制估计误差随噪声的变化。

90.9 延迟处理：动作片段的延迟随机化 ⭐⭐⭐¶

90.9.1 延迟的来源与量级¶

在真机部署中，从观测到动作的整个管线有多个延迟源：

延迟分解:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
相机曝光        ~10ms
图像传输 (USB3) ~5-15ms
视觉编码        ~10-20ms  (GPU)
Diffusion推理   ~50-100ms (GPU, DDIM-10)
动作传输        ~1-5ms
关节控制延迟     ~5-10ms
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
总计:           ~80-160ms

80-160ms 的延迟意味着：当机器人执行动作时，这个动作是基于 80-160ms 之前的观测做出的。如果机器人正以 0.2m/s 移动，动作的执行位置与决策位置之间差了 1.6-3.2cm。

90.9.2 延迟随机化训练¶

UMI-on-Legs 在训练 Diffusion Policy 时引入**延迟随机化(latency randomization)**：人为在观测和动作之间添加随机延迟，让策略学会在延迟不确定的情况下仍然输出合理的动作。

# 训练时的延迟随机化
def augment_with_latency(obs_buffer, action_buffer, config):
    """在训练数据中添加随机延迟"""
    latency_steps = np.random.randint(
        config.min_latency,    # 典型: 1 步 (50ms)
        config.max_latency + 1  # 典型: 4 步 (200ms)
    )

    # 观测使用 latency_steps 之前的帧
    obs_delayed = obs_buffer[:-latency_steps]
    # 动作保持原始时间
    action_aligned = action_buffer[latency_steps:]

    return obs_delayed, action_aligned

为什么延迟随机化有效？ 当策略在不同延迟下训练时，它被迫学会输出"保守但安全"的动作——不依赖于精确的实时观测，而是基于观测的趋势做出平滑的预测。这与 Domain Randomization 的原理一致：用训练时的多样性换部署时的鲁棒性。

反事实推理：如果训练时不做延迟随机化，策略在仿真中（延迟几乎为零）表现完美。但部署到真机时，100ms 的延迟让策略看到的是"过去"的世界而执行"现在"的动作。对于精确操作（如插入销钉），100ms 意味着目标已经移走了几毫米，策略会反复"追赶"目标而无法收敛。延迟随机化让策略预先适应了这种"看过去做现在"的模式。

90.9.3 动作片段中的延迟补偿¶

Action Chunking 天然提供了一定程度的延迟容忍：因为 chunk 包含未来 $K$ 步的动作，即使 chunk 迟到了几步，执行层仍然可以从 chunk 中取出"应该现在执行"的动作（跳过已过期的前几步）。

延迟补偿示意:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
chunk 应在 t=0 到达, 实际在 t=2 到达:

chunk 内容: [a_0, a_1, a_2, a_3, a_4, a_5, a_6, a_7]
                        ↑
                    跳过 a_0, a_1 (已过期)
                    从 a_2 开始执行

这要求 chunk 的每个动作带有绝对时间戳, 而不仅是序号
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

90.9.4 延迟分布的尾部效应¶

延迟不仅有平均值，更有**尾部分布**。平均延迟 100ms 的系统，可能 99% 的时间在 80-120ms，但 1% 的时间因 GPU 负载波动跳到 200-300ms。这些"延迟尾部"(tail latency) 是真机部署中最大的不确定性来源。

延迟分布示例:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
频率 ▲
     │ ████
     │ ██████
     │ ████████
     │ ██████████
     │ ████████████
     │ ██████████████ ░░     ← 尾部
     │ ██████████████ ░░░
     └─────────────────────→ 延迟(ms)
       50   100  150  200  300

主体: 80-120ms (正常)
尾部: 200-300ms (GPU context switch, GC, 系统中断)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

应对尾部延迟的策略：

策略	做法	效果
异步推理	推理在后台线程，不阻塞控制	消除推理延迟对控制的影响
多 chunk 缓存	缓存 2-3 个 chunk，新的到了再替换	即使一次推理迟到也有备用
降级模式	超时后切换到简单启发式	保证有动作输出

90.9.5 端到端延迟的精确测量¶

在调试延迟问题时，需要精确测量管线每一段的延迟：

import time

class LatencyProfiler:
    """精确测量每段延迟"""
    def __init__(self):
        self.timestamps = {}

    def mark(self, name):
        """在关键节点打时间戳"""
        self.timestamps[name] = time.clock_gettime(time.CLOCK_MONOTONIC)

    def report(self):
        """打印各段延迟"""
        keys = list(self.timestamps.keys())
        for i in range(1, len(keys)):
            dt = (self.timestamps[keys[i]] - self.timestamps[keys[i-1]]) * 1000
            print(f"  {keys[i-1]} → {keys[i]}: {dt:.2f} ms")

# 使用示例
profiler = LatencyProfiler()
profiler.mark("camera_capture")
image = camera.read()
profiler.mark("image_preprocess")
obs = preprocess(image)
profiler.mark("feature_extract")
features = encoder(obs)
profiler.mark("diffusion_start")
chunk = diffusion_policy.inference(features)
profiler.mark("diffusion_end")
profiler.mark("action_execute")
send_to_wbc(chunk[0])
profiler.mark("wbc_complete")

profiler.report()
# 输出:
#   camera_capture → image_preprocess: 2.31 ms
#   image_preprocess → feature_extract: 8.45 ms
#   feature_extract → diffusion_start: 0.12 ms
#   diffusion_start → diffusion_end: 52.67 ms
#   diffusion_end → action_execute: 0.08 ms
#   action_execute → wbc_complete: 0.95 ms

练习 90.9a (⭐⭐): 实现一个延迟补偿的 chunk 执行器。输入是 chunk（带时间戳）和当前时间，输出是应该执行的动作。处理以下边界情况：(1) chunk 整体过期；(2) chunk 部分过期。

练习 90.9b (⭐⭐⭐): 设计一个延迟敏感度分析实验。在仿真中分别测试 0ms, 50ms, 100ms, 200ms 的恒定延迟对抓取成功率的影响。找到使成功率降至 50% 的临界延迟。

90.10 部署验证：14 物体评估的方法论与失败分析 ⭐⭐⭐¶

90.10.1 评估方法论¶

UMI-on-Legs 在 14 种不同物体上进行了系统性评估，包含抓取(prehensile)、非抓取(non-prehensile)和动态(dynamic)三类任务。评估方法论的设计值得仔细学习。

任务类别	示例物体	挑战
抓取	杯子、瓶子、工具	形状多样、质量差异
非抓取	推盘子、翻书页	需要精确力控
动态	抛接球	高速、时间窗口窄

每个物体每种配置重复 10 次试验。成功标准明确定义——不是主观判断。

90.10.2 失败模式分类¶

论文中报告的失败模式可以归类到系统的不同层次：

失败模式分层:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
Layer 1 (感知): 视觉特征误匹配 → 错误的条件输入
  │ 症状: 策略输出与场景不匹配的动作
  │ 诊断: 检查视觉编码器的激活图
  │
Layer 2 (规划): Diffusion Policy 生成不合理动作
  │ 症状: EE 轨迹不连续或超出工作空间
  │ 诊断: 回放 chunk 的 EE 轨迹, 检查平滑性
  │
Layer 3 (执行): WBC 跟踪误差过大
  │ 症状: 末端实际轨迹偏离目标
  │ 诊断: 比较 EE 目标和实际位姿的误差曲线
  │
Layer 4 (硬件): 夹爪打滑/电机过热
  │ 症状: 物体已到手但没抓住
  │ 诊断: 检查夹爪力传感器和电机温度
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

90.10.3 日志记录与回放¶

有效的失败分析依赖于全面的日志记录。UMI-on-Legs 的日志系统需要记录以下内容：

层次	记录内容	频率	格式
感知	原始图像 + 视觉特征	10Hz	HDF5
规划	Chunk 内容 + 时间戳	2-5Hz	JSON
执行	关节角/角速度/力矩	50Hz+	CSV/ROS2 bag
目标	EE 目标 + 实际位姿	50Hz+	CSV
安全	安全层状态/裁剪事件	每事件	Log

**固定输入回放**是最强大的调试工具：把日志中的观测重新输入策略，检查输出是否与原始一致。如果一致，问题在执行层或硬件；如果不一致，问题在策略（可能是随机种子或时间戳问题）。

90.10.4 零样本跨平台迁移¶

UMI-on-Legs 的一个亮点是：把在固定臂上训练的 Diffusion Policy checkpoint **零样本**迁移到四足臂上。这证明了任务帧 EE 接口的有效性——只要接口语义一致，上层策略可以跨平台使用。

但零样本迁移有边界条件：

条件	要求	不满足时后果
EE 工作空间重叠	新平台的 EE 可达空间覆盖任务需要的区域	部分轨迹不可达
夹爪相似	夹爪的开合范围和力相近	抓取力度不匹配
视角一致	腕部相机的安装角度和视场角相近	视觉特征域偏移
动态能力	新平台的运动速度和加速度够快	跟不上高速操作

💡 Insight: 零样本迁移的成功说明，接口设计比算法本身更重要。即使用最普通的 Diffusion Policy（没有任何针对移动操作的特殊设计），只要接口正确，它就能在全新的平台上工作。这是一个强有力的工程教训：好的抽象层比好的算法更有持久价值。

90.10.5 关键评估指标¶

在评估移动操作系统时，单一的"成功率"指标远远不够。需要一组结构化的指标来全面评估系统性能：

指标类别	具体指标	计算方式	基线值 (UMI-on-Legs)
任务成功率	总体成功率	成功次数/总次数	>70%
末端精度	EE 位置 RMSE	$\sqrt{\frac{1}{T}\sum_t \\|p_{ee} - p_{ee}^{des}\\|^2}$	~2-4cm
末端精度	EE 旋转误差	$\frac{1}{T}\sum_t \angle(R_{ee}, R_{ee}^{des})$	~5-10°
行走稳定	基座高度波动	std$(h_{base})$	<1.5cm
行走稳定	基座倾斜	max$(\theta_{roll,pitch})$	<8°
平滑性	关节加速度	$\frac{1}{T}\sum_t \\|\ddot{q}\\|$	—
延迟	策略推理时间	95th percentile	<150ms
鲁棒性	扰动后恢复时间	从推扰到 EE 误差 <5cm 的时间	<0.5s

统计学注意事项：每种配置至少 10 次试验才有统计意义。报告均值 ± 标准差或中位数 + 四分位距。不要只报告"最好的一次"。

90.10.6 典型失败模式的具体案例¶

从 UMI-on-Legs 论文和后续工作中总结的典型失败：

案例 1：相机遮挡导致策略混乱

症状：机器人在抓取物体的过程中突然停止或做出无关动作。

根因：操作过程中手臂挡住了腕部相机的视野。Diffusion Policy 收到的图像中操作物体消失了，生成的动作序列不再朝向目标。

解决：(a) 调整相机安装角度减少自遮挡；(b) 在训练数据中包含部分遮挡的样本；(c) 添加遮挡检测逻辑——当图像中关键物体消失时，冻结当前 chunk 而非生成新的。

案例 2：里程计漂移导致任务帧偏移

症状：操作初期正常，但随时间推移末端逐渐偏离目标。

根因：$T_{world}^{body}$ 由视觉-惯性里程计提供。行走过程中里程计累积漂移，导致计算的 $T_{body}^{ee}$ 偏移。

解决：(a) 使用外部定位（如 AprilTag 或 Vicon）定期校正里程计；(b) 在任务帧下工作时减少对世界帧的依赖；(c) 任务时间控制在里程计漂移可接受的范围内。

案例 3：夹爪力度不匹配

症状：物体被成功抓到但在行走中掉落。

根因：UMI 的手持夹爪和机器人上的夹爪有不同的力特性。训练时示教数据反映的是手持夹爪的力度，但部署时机器人夹爪可能力度不同。

解决：(a) 在部署时增加夹爪力度的安全裕度；(b) 在训练数据预处理中增广夹爪开合度。

练习 90.10a (⭐⭐): 设计一个失败归因的决策树：给定一次失败的实验日志，通过哪些检查步骤可以确定失败发生在哪一层？

练习 90.10b (⭐⭐⭐): 在零样本迁移中，如果新平台的 EE 工作空间比原平台小 20%，有哪些方法可以缓解这个问题？（提示：考虑运行时的轨迹裁剪、任务帧重新注册、底层速度放大等。）

90.11 与 VBC 的对比：目标接口的设计差异 ⭐⭐¶

90.11.1 位置接口 vs 速度接口¶

UMI-on-Legs 使用位置目标接口（输出 $T_{task}^{ee}$），VBC (Velocity-Based Control) 使用速度指令接口（输出 $\mathbf{v}_{ee}^{des}$）。两种接口方案导致了截然不同的系统行为。

维度	位置接口 (UMI-on-Legs)	速度接口 (VBC)
上层输出	"去这个位置" $T^{ee}$	"以这个速度移动" $\dot{T}^{ee}$
下层行为	位置跟踪控制	速度跟踪控制
延迟影响	过时目标导致跳变	过时速度导致漂移
误差特性	有界（目标位置固定）	无界（速度积分漂移）
示教方式	记录位置序列	记录速度序列

本质洞察：位置接口和速度接口的根本差异是**误差的性质**。位置接口的误差是有界的——无论中间怎么偏，最终都会收敛到目标位置。速度接口的误差会积分——任何持续的偏差都会导致位置漂移，且永远不会自动修复。这就是为什么 UMI-on-Legs 选择位置接口：操作任务的终态精度是最重要的。

90.11.2 何时选择哪种接口？¶

两种接口各有适用场景：

选择决策树:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
任务需要精确终态？
  ├── 是 → 位置接口
  │   ├── 示教数据有完整轨迹？ → UMI 方案
  │   └── 只有终态目标？       → 运动规划 + IK
  │
  └── 否 → 速度接口
      ├── 需要连续交互（如擦桌子）？ → VBC
      └── 需要力控（如打磨）？       → 阻抗控制
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

练习 90.11 (⭐⭐): 分析以下三个任务各适合哪种接口：(1) 从架子上取书；(2) 用抹布擦桌面；(3) 打开水龙头。给出理由。

90.11.2 Diffusion Policy vs ACT 的架构对比¶

UMI-on-Legs 选择了 Diffusion Policy 作为操作策略。另一个强力竞争者是 ACT (Action Chunking with Transformers, Zhao et al., RSS 2023)。两者都生成动作 chunk，但生成机制不同：

维度	Diffusion Policy	ACT
生成方式	迭代去噪 (DDPM/DDIM)	单次前向 (CVAE + Transformer)
多模态	天然支持（不同噪声→不同模态）	通过 CVAE 的 latent 采样
推理速度	慢（10-25 步去噪）	快（单次前向）
训练稳定性	简单（MSE on noise）	需要 KL 平衡
长时序质量	chunk 内部高质量	chunk 边界可能不连续

**Diffusion Policy 的独特优势**在于处理多模态性。在 UMI 的数据采集中，不同操作员完成同一个任务的方式不同（左手抓 vs 右手抓、快速完成 vs 慢速完成），数据天然是多模态的。Diffusion Policy 可以通过不同的初始噪声采样到不同模态，每次生成的动作序列都是某个模态的完整、一致的轨迹。ACT 依赖 CVAE 的 latent 变量来捕捉多模态，但 CVAE 的模态坍缩(mode collapse)问题更容易发生。

多模态处理对比:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
Diffusion Policy:
  噪声 ε₁ ──→ [去噪 10步] ──→ 左手抓 轨迹
  噪声 ε₂ ──→ [去噪 10步] ──→ 右手抓 轨迹
  噪声 ε₃ ──→ [去噪 10步] ──→ 左手抓 轨迹 (不同随机种子但同模态)
  → 自然地在模态之间切换

ACT (CVAE):
  z₁ ∈ latent ──→ [Transformer] ──→ 左手抓 轨迹
  z₂ ∈ latent ──→ [Transformer] ──→ 左手抓 轨迹 (模态坍缩!)
  → KL 权重太大→只学到一个模态；太小→latent 无意义
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

反事实推理：如果 UMI-on-Legs 使用 ACT 替代 Diffusion Policy，在单模态任务（如拧螺丝——只有一种正确方式）上可能更好（推理更快），但在多模态任务（如抓取杯子——可以从多个角度抓）上可能更差（模态坍缩导致平均化）。这说明策略架构的选择应该根据任务的多模态性程度来决定。

90.11.3 Score Matching 与噪声预测的等价性¶

Diffusion Policy 训练网络预测噪声 $\boldsymbol{\epsilon}_\theta$。这与**score matching**（学习对数概率密度的梯度 $\nabla_{\mathbf{a}} \log p(\mathbf{a})$）是等价的。

Score function 的定义：

\[\mathbf{s}(\mathbf{a}_t, t) = \nabla_{\mathbf{a}_t} \log q(\mathbf{a}_t)\]

由前向过程 $q(\mathbf{a}_t | \mathbf{a}_0) = \mathcal{N}(\sqrt{\bar{\alpha}_t}\,\mathbf{a}_0,\ (1-\bar{\alpha}_t)\mathbf{I})$，可以计算精确的 score：

\[\nabla_{\mathbf{a}_t} \log q(\mathbf{a}_t | \mathbf{a}_0) = -\frac{\mathbf{a}_t - \sqrt{\bar{\alpha}_t}\,\mathbf{a}_0}{1-\bar{\alpha}_t} = -\frac{\boldsymbol{\epsilon}}{\sqrt{1-\bar{\alpha}_t}}\]

因此噪声预测 $\boldsymbol{\epsilon}_\theta$ 和 score 预测 $\mathbf{s}_\theta$ 之间的关系为：

\[\boxed{\mathbf{s}_\theta(\mathbf{a}_t, t) = -\frac{\boldsymbol{\epsilon}_\theta(\mathbf{a}_t, t)}{\sqrt{1-\bar{\alpha}_t}}}\]

这个等价性意味着：训练 Diffusion Policy 预测噪声，本质上是在学习数据分布的 score function。在采样时，沿着 score 的方向迭代（Langevin 动力学），就能从噪声分布逐步移动到数据分布。

为什么这个理论理解重要？ 因为它解释了 Diffusion Policy 为什么能捕捉多模态：score function 在多模态分布的每个模态附近都有"吸引区域"——从不同初始噪声出发，Langevin 动力学会把样本引导到不同的模态。这不是 heuristic，而是有严格数学保证的。

⚠️ Pitfall: score matching 在低密度区域（模态之间）的 score 估计不准确——训练数据在那里很少。这意味着如果去噪步数太少（如 DDIM-3），样本可能停留在模态之间的"无人区"，产生不合理的动作序列。DDIM-10 通常是实用的下限。

90.11.4 Noise Schedule 的设计¶

噪声调度表 $\{\beta_t\}_{t=1}^T$ 的选择对 Diffusion Policy 的质量有显著影响：

调度类型	公式	特点
线性 (DDPM 原始)	$\beta_t = \beta_1 + \frac{t-1}{T-1}(\beta_T - \beta_1)$	简单，后期噪声增长太快
余弦 (improved DDPM)	$\bar{\alpha}_t = \frac{f(t)}{f(0)}$, $f(t)=\cos(\frac{t/T+s}{1+s}\cdot\frac{\pi}{2})^2$	前期信号保留更多
Squaredcos_cap_v2	裁剪后的余弦平方	Diffusion Policy 默认

余弦调度的关键优势是在 $t$ 较小时保留更多信号。对于机器人动作序列，细微的差异（如 1cm 的位移差异）在小 $t$ 时需要被准确重建——线性调度在 $t$ 小时加的噪声太多，模糊了这些细节。

练习 90.11c (⭐⭐⭐): 实现线性和余弦两种 noise schedule，绘制 $\bar{\alpha}_t$ 随 $t$ 的变化曲线。在一个 1D 双模态分布 $p(a) = 0.5\mathcal{N}(-1, 0.1) + 0.5\mathcal{N}(1, 0.1)$ 上训练 Diffusion Model，比较两种 schedule 的采样质量。

90.11.5 WBC 训练的 Privileged Learning 细节¶

UMI-on-Legs 的 WBC 使用 Privileged Learning 框架训练。这个框架分两个阶段：

Privileged Learning 两阶段训练:
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
阶段 1: 教师策略 (Teacher)
  观测: 完整信息 (包括真实地面摩擦系数、精确质心位置、
        外部扰动力、接触法向量等)
  训练: PPO, ~2000 轮
  目标: 学到"如果我知道一切，最好怎么做"

阶段 2: 学生策略 (Student)
  观测: 受限信息 (只有关节编码器、IMU、EE 目标)
  训练: 行为克隆 (模仿教师的动作)
  目标: 从受限观测中推断出教师能看到的信息

关键: 学生不直接模仿教师的动作，而是学习
      从受限观测中重构教师的 "privileged context"
      然后用这个重构的 context 做决策
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

教师策略的观测中包含的"特权信息"(privileged information)：

特权信息	维度	真机上不可用的原因
地面摩擦系数 $\mu$	1	无法直接测量
精确的外部力 $F_{ext}$	3	需要力传感器（通常没有）
精确的地形高度	若干	需要高精度深度传感器
精确的基座速度	3	IMU 积分有漂移
真实的质心位置	3	负载变化时不准确

学生策略通过一个编码器(estimator)从历史观测序列中隐式估计这些特权信息。这与 DreamWaQ 的 "implicit terrain imagination" 思想一致——不显式估计地形，而是学习一个隐式表征，其中编码了足以做出好决策的信息。

跨章回顾：回顾复合/180 Deep-WBC：我们讨论了 Privileged Learning 的理论基础——信息瓶颈(Information Bottleneck)。教师的特权观测是高维的（包含一切），学生的观测是低维的（只有传感器数据）。学生的编码器学到的是一个压缩表征——保留了与决策相关的信息，丢弃了无关的信息。这个压缩过程的最优解恰好是信息瓶颈的解。

90.11.6 UMI 数据采集的实操要点¶

虽然 UMI 的概念简单（手持夹爪采集数据），但实际操作中有很多细节决定数据质量：

要点	说明	常见错误
采集速度	慢速、自然、流畅	太快导致运动模糊、SLAM 失败
视角多样性	从不同角度重复同一任务	只从一个角度采集导致策略过拟合
失败示教	也要记录失败的尝试	只保留成功数据导致分布偏差
光照条件	在多种光照下采集	只在特定光照下采集导致部署失败
标定板可见性	确保参考标记物始终可见	SLAM 丢失导致轨迹断裂
夹爪状态一致	训练和部署的夹爪尺寸一致	不同夹爪的抓取策略不同

数据量的经验值：

任务复杂度	所需示教次数	训练轮次
简单抓放	50-100 次	3000 epoch
工具使用	100-200 次	5000 epoch
多阶段操作	200-500 次	8000+ epoch

💡 Insight: UMI 的数据效率比传统的 teleoperation（遥操作）高得多。因为 UMI 用的是**手持夹爪**——人可以自然地完成操作，不需要学习复杂的遥操作界面（如 VR 手柄映射到机器人关节）。自然的示教速度约 5-10 分钟/任务，而遥操作可能需要 20-30 分钟来适应界面。

90.11.7 Diffusion Policy 的训练超参数¶

对于机器人操作任务，Diffusion Policy 的关键训练超参数：

超参数	推荐值	影响
去噪步数 $T$ (训练)	100	增大→质量更好但训练慢
去噪步数 (DDIM推理)	10-16	增大→质量更好但推理慢
Chunk 长度 $K$	16 (观测2+预测16)	增大→更长的规划视野
观测窗口 $T_{obs}$	2	太大→特征模糊
学习率	1e-4	太大→不稳定
Batch size	256	太小→梯度噪声大
预测类型	epsilon (噪声预测)	比 sample 预测更稳定
网络架构	U-Net 或 Transformer	U-Net 更快，Transformer 更灵活

U-Net vs Transformer 架构对比：

U-Net 架构:
  优势: 推理快 (~30ms on RTX 3090)
        参数少 (~30M)
        适合固定维度的动作空间
  劣势: 难以处理可变长度的条件
        难以融合多种模态的条件

Transformer 架构:
  优势: 可以自然处理可变长度序列
        多模态条件融合更灵活
        支持 cross-attention
  劣势: 推理慢 (~100ms)
        参数多 (~100M)
        需要更多训练数据

UMI-on-Legs 使用 U-Net 架构——因为腕部相机只有一个，条件结构固定，推理速度更重要。

练习 90.11d (⭐⭐): 比较 DDIM-10 和 DDIM-16 在你的最小 1D Diffusion Model 上的采样质量差异。在什么条件下两者差异最大？

本章小结¶

知识点	核心要点	难度
移动操作动机	四足+臂解决移动+操作同时进行的需求	⭐⭐
三层架构	感知(10Hz) → 规划(2-5Hz) → 执行(50-100Hz)	⭐⭐
Diffusion Policy	DDPM/DDIM 条件去噪生成动作序列	⭐⭐⭐
Action Chunking	一次输出多步动作，桥接频率差	⭐⭐⭐
任务帧注册	将示教数据绑定到物体坐标系实现跨平台复用	⭐⭐⭐
腕部相机管线	视觉编码 + 时间同步	⭐⭐⭐
RL WBC	manipulation-centric 奖励设计	⭐⭐⭐
延迟处理	延迟随机化训练 + chunk 时间戳补偿	⭐⭐⭐
部署验证	分层失败分析 + 日志回放	⭐⭐⭐

累积项目：本章新增模块¶

在累积的移动操作项目中，本章新增以下模块：

模块 1: 最小 1D Diffusion Policy 实现（90.3 练习）
模块 2: Action Chunk 执行器，含延迟补偿（90.4, 90.9 练习）
模块 3: 坐标变换链，含任务帧注册（90.5 练习）
模块 4: 失败归因决策树（90.10 练习）

UMI-on-Legs 的开源代码结构¶

UMI-on-Legs 的代码开源在 GitHub (real-stanford/umi-on-legs)。理解代码结构有助于复现和改进：

umi-on-legs/
├── diffusion_policy/          # Diffusion Policy 训练和推理
│   ├── config/                # 训练配置 (YAML)
│   ├── model/                 # 网络架构 (U-Net / Transformer)
│   ├── dataset/               # UMI 数据加载
│   └── workspace/             # 训练主循环
├── whole_body_controller/     # RL WBC
│   ├── envs/                  # Isaac Gym 环境定义
│   ├── algorithms/            # PPO 训练
│   ├── configs/               # 奖励/观测/DR 配置
│   └── deployment/            # 真机部署
├── calibration/               # 标定工具
│   ├── hand_eye.py            # AX=XB 求解
│   └── camera_intrinsics.py   # 内参标定
├── deployment/                # 部署管线
│   ├── action_executor.py     # Chunk 执行器
│   ├── safety_monitor.py      # 安全监控
│   └── logger.py              # 日志记录
└── evaluation/                # 评估工具
    ├── success_checker.py     # 成功判定
    └── analysis.py            # 失败分析

代码阅读建议：

从 diffusion_policy/model/ 开始，理解去噪网络的输入/输出维度
然后读 whole_body_controller/envs/，理解 WBC 的观测/动作/奖励定义
最后读 deployment/action_executor.py，理解 chunk 执行和延迟补偿的实现

⚠️ Pitfall: 开源代码中的超参数通常是针对特定硬件（如 Unitree Go2 + ARX5 臂）调优的。直接迁移到其他硬件时，至少需要重新调整以下参数：PD 增益、动作缩放(action scale)、奖励权重、Domain Randomization 范围。

延伸阅读¶

资源	难度	内容
Chi et al., "Diffusion Policy" (RSS 2024)	⭐⭐⭐	Diffusion Policy 原始论文
Ho et al., "DDPM" (NeurIPS 2020)	⭐⭐⭐⭐	DDPM 数学基础
Song et al., "DDIM" (ICLR 2021)	⭐⭐⭐⭐	DDIM 加速采样
Chi et al., "UMI" (RSS 2024)	⭐⭐⭐	UMI 手持夹爪数据采集
He et al., "UMI-on-Legs" (CoRL 2024)	⭐⭐⭐⭐	本章主题论文
Zhou et al., "On the Continuity of Rotation Representations" (CVPR 2019)	⭐⭐⭐	6D 旋转表示

跨章综合题¶

综合练习 1 (⭐⭐⭐): 结合本章 (UMI-on-Legs) 和复合/210 (RAMBO) 的知识，设计一个系统：上层使用 Diffusion Policy 生成 EE 轨迹，底层使用 RAMBO 的 MPC+RL 混合控制器跟踪。画出完整的信号流图，标注每个模块的输入/输出维度、坐标系和频率。与 UMI-on-Legs 原始的纯 RL WBC 相比，这种混合方案有什么优势和劣势？

综合练习 2 (⭐⭐⭐⭐): 设计一个从头到尾的四足操作实验流程：从 UMI 数据采集 → Diffusion Policy 训练 → WBC 训练 → 标定 → 部署 → 评估。列出每个步骤的输入/输出、预计时间、可能出错的环节和验证方法。估算整个流程从零到部署需要多少人-天。

故障排查手册¶

症状	可能原因	排查步骤	相关章节
末端持续偏移 3-5cm	手眼标定偏差	1. 重新标定 2. 检查标定板检测 3. 验证重投影误差	90.8
动作片段到达后末端跳变	chunk 融合参数不当	1. 打印新旧 chunk 差异 2. 增大混合衰减系数 3. 检查时间戳	90.4
行走时末端抖动	WBC 末端跟踪权重太低	1. 打印 EE 跟踪误差 2. 增大 $w_{ee}$ 3. 检查基座扰动幅度	90.7
Diffusion 推理超时	GPU 负载过高或 DDIM 步数太多	1. 检查 GPU 占用 2. 减少 DDIM 步数到 10 3. 降低图像分辨率	90.3
零样本迁移成功率骤降	EE 工作空间不匹配	1. 可视化两个平台的工作空间 2. 检查夹爪尺寸差异 3. 调整任务帧位置	90.10

变换	来源	更新频率	典型误差源
\(T_{\text{world}}^{\text{body}}\)	IMU + 腿部里程计	100Hz+	漂移、冲击跳变
\(T_{\text{world}}^{\text{task}}\)	视觉定位 / 初始注册	任务开始时一次	标定误差、遮挡
\(T_{\text{task}}^{ee}\)	Diffusion Policy 输出	2-5Hz	模型泛化误差

参数	符号	典型值	影响
Chunk 长度	\(K\)	16-32	过短→频繁等待；过长→预测不准
执行步数	\(K_{exec}\)	8-16	过少→浪费计算；过多→使用过时预测
观测窗口	\(T_{obs}\)	2-4	过短→缺乏上下文；过长→特征模糊

误差类型	末端误差	量级估计 (L=0.3m)
位置 \(\delta p\)	\(\delta p\)	直接传递
旋转 \(\delta \theta\)	\(L \sin(\delta\theta) \approx L \cdot \delta\theta\)	\(\delta\theta = 2° → 0.3 \times 0.035 = 1.05\)cm
位置+旋转耦合	\(\delta p + L\delta\theta\)	叠加

奖励分项	公式	权重	含义
EE 位置跟踪	\(-\\|p_{ee} - p_{ee}^{des}\\|^2\)	高 (10.0)	末端位置误差越小越好
EE 朝向跟踪	\(-\\|R_{ee}^T R_{ee}^{des} - I\\|_F^2\)	高 (5.0)	末端旋转误差
基座高度	$-	h_{base} - h_{nom}	^2$
基座倾斜	\(-\\|\theta_{roll,pitch}\\|^2\)	中 (1.0)	身体保持水平
动作变化率	\(-\\|a_t - a_{t-1}\\|^2\)	中 (1.0)	关节命令平滑
关节力矩	\(-\\|\tau\\|^2\)	低 (0.1)	能量效率
接触奖励	\(+1\) if 足底接触正常	中 (1.0)	鼓励正确步态

维度	位置接口 (UMI-on-Legs)	速度接口 (VBC)
上层输出	"去这个位置" \(T^{ee}\)	"以这个速度移动" \(\dot{T}^{ee}\)
下层行为	位置跟踪控制	速度跟踪控制
延迟影响	过时目标导致跳变	过时速度导致漂移
误差特性	有界（目标位置固定）	无界（速度积分漂移）
示教方式	记录位置序列	记录速度序列

调度类型	公式	特点
线性 (DDPM 原始)	\(\beta_t = \beta_1 + \frac{t-1}{T-1}(\beta_T - \beta_1)\)	简单，后期噪声增长太快
余弦 (improved DDPM)	\(\bar{\alpha}_t = \frac{f(t)}{f(0)}\), \(f(t)=\cos(\frac{t/T+s}{1+s}\cdot\frac{\pi}{2})^2\)	前期信号保留更多
Squaredcos_cap_v2	裁剪后的余弦平方	Diffusion Policy 默认

特权信息	维度	真机上不可用的原因
地面摩擦系数 \(\mu\)	1	无法直接测量
精确的外部力 \(F_{ext}\)	3	需要力传感器（通常没有）
精确的地形高度	若干	需要高精度深度传感器
精确的基座速度	3	IMU 积分有漂移
真实的质心位置	3	负载变化时不准确

超参数	推荐值	影响
去噪步数 \(T\) (训练)	100	增大→质量更好但训练慢
去噪步数 (DDIM推理)	10-16	增大→质量更好但推理慢
Chunk 长度 \(K\)	16 (观测2+预测16)	增大→更长的规划视野
观测窗口 \(T_{obs}\)	2	太大→特征模糊
学习率	1e-4	太大→不稳定
Batch size	256	太小→梯度噪声大
预测类型	epsilon (噪声预测)	比 sample 预测更稳定
网络架构	U-Net 或 Transformer	U-Net 更快，Transformer 更灵活