本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
D08 运动映射与遥操作数据采集——Retargeting / ALOHA / GELLO / UMI¶
本章定位：本章是双臂协调与遥操作系列（D01-D10，24 周）Part 2 的收尾章。从"人类运动如何映射到机器人"出发，系统讲解主从运动映射的数学基础、工作空间缩放与离合机制、手部运动重定向、各类遥操作硬件系统的设计哲学，最终落地到数据采集 pipeline 和数据质量评估。本章建立了从遥操作控制到模仿学习数据的完整桥梁——D04（双臂学习）需要的训练数据正是从本章的 pipeline 中产出。
适用范围：运动映射算法对单臂遥操作、双臂遥操作、VR 遥操作、外骨骼遥操作均适用。手部 retargeting 对灵巧手操作（M16）同样是核心前置。
前置依赖：D05-D07（遥操作理论/无源通信/TDPA）——理解为什么有些系统不做力反馈；M03（IK 求解器）——数值 IK 概念；D04（双臂学习/ACT）——理解数据采集的应用场景
下游章节：D09（双臂 MoveIt2 集成）、D10（综合实战 Mini-DualArm）、D04（ACT/Diffusion Policy 训练数据接口）
建议用时：2 周（15-20 小时）
前置自测 ⭐¶
📋 答不出 >= 2 题 → 先回前置章节复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 无源性概念：什么是二端口网络的无源性条件？为什么遥操作系统需要满足无源性？写出散逸不等式 \(\dot{V} \leq y^T u\)。 | D05-D06 无源通信理论 |
| 2 | 正运动学：给定关节角 \(q \in \mathbb{R}^n\)，如何计算末端位姿 \(T_{ee} \in SE(3)\)？用齐次变换矩阵连乘表达。 | M01 Pinocchio 正运动学 |
| 3 | 逆运动学：解析 IK 和数值 IK 的区别是什么？为什么 7-DOF 臂需要数值 IK？什么是冗余自由度？ | M03 IK 求解器深度 |
| 4 | TDPA 机制：时域无源性方法的核心思想是什么？它通过什么方式在线监控和修正能量？ | D07 TDPA 与工程实现 |
| 5 | 模仿学习基础：ACT（Action Chunking with Transformers）的输入输出是什么？为什么需要 action chunking 而不是逐帧输出？ | D04 双臂学习 |
本章目标¶
学完本章后，你应该能够：
- **推导**主从运动缩放的数学模型，理解位置缩放因子 \(\lambda_p\) 和力缩放因子 \(\lambda_f\) 的能量一致条件；在本文端口定义下为 \(\lambda_f = \lambda_p\)
- **区分**关节空间映射和笛卡尔空间映射的适用场景，能根据主从构型差异正确选型
- **实现**优化式 retargeting（从人手关键点到机器人灵巧手），理解 DexPilot 的 pinch prior 设计
- 分析 ALOHA、GELLO、UMI、OpenTeleVision、ACE 五大遥操作系统的架构差异和工程取舍
- **搭建**完整的数据采集 pipeline（ROS2 录制 → HDF5/LeRobot 格式 → 数据清洗 → 训练数据生成）
- **评估**采集数据的质量，掌握数据过滤指标和质量增强方法
D8.1 运动缩放与能量一致性 ⭐⭐¶
动机——为什么需要运动缩放？¶
考虑一个外科医生操作 da Vinci 手术机器人的场景。人手的自然运动范围约为 \(\pm 15 \text{cm}\)（腕关节到指尖），但手术部位可能只有 \(1 \text{cm} \times 1 \text{cm}\) 的操作空间（如眼科手术）。如果 1:1 映射人手运动到手术器械，医生需要控制到 \(0.1 \text{mm}\) 级精度——这远超人手的运动分辨率（约 \(1 \text{mm}\)）。
反过来，一个建筑拆除遥操作场景中，操作者坐在控制室操纵一台 5m 臂展的液压机械臂。如果 1:1 映射，操作者需要在控制台上移动 5m——这显然不可能。
**运动缩放（Motion Scaling）**就是为了解决人体工作空间与机器人工作空间不匹配的根本矛盾。
如果不做缩放会怎样¶
不做缩放，1:1 直映的后果按场景分为两类：
微操作场景（手术/微装配）：人手的 \(1 \text{mm}\) 自然抖动直接传递到器械端。对于眼科手术而言，\(1 \text{mm}\) 的误差足以刺穿视网膜。同时，操作者需要在极小范围内精确控制，手部肌肉持续紧张，30 分钟后疲劳导致抖动加剧——形成恶性循环。
宏操作场景（建筑/核退役）：操作者的工作空间不足以覆盖机器人工作空间。操作者被迫频繁进行 clutching（离合操作），工作效率下降 3-5 倍。
历史——运动缩放的起源¶
运动缩放的概念最早来自 Goertz（1952）在美国阿贡国家实验室开发的核废料处理主从机械臂。当时的驱动方式是纯机械连杆传动，缩放比通过齿轮比物理实现。1985 年，MIT 的 Salisbury 首次将电动伺服与电子缩放结合，实现了可编程的缩放比——这为 1999 年 Intuitive Surgical 的 da Vinci 系统奠定了基础。da Vinci 的默认缩放比 3:1（手动 3cm → 器械 1cm）至今仍是手术遥操作的黄金标准。
数学模型¶
位置缩放：设主端（master）末端位置为 \(x_m \in \mathbb{R}^3\)，从端（slave）末端期望位置为 \(x_s^{des} \in \mathbb{R}^3\)，缩放关系为：
其中 \(\lambda_p\) 是位置缩放因子，\(x_m^{ref}\) 和 \(x_s^{ref}\) 分别是主端和从端的参考原点。
- \(\lambda_p < 1\)：动作缩小（微操作），如 da Vinci 手术 \(\lambda_p \in [0.2, 0.4]\)
- \(\lambda_p = 1\)：1:1 映射
- \(\lambda_p > 1\)：动作放大（宏操作），如核退役 \(\lambda_p \in [2, 5]\)
速度缩放：对位置缩放求时间导数：
速度缩放因子与位置缩放因子相同——这不是设计选择，而是数学必然。
力缩放：设从端接触力为 \(f_s\)，反馈到主端的力为 \(f_m\)：
其中 \(\lambda_f\) 是**无源缩放器内部**的力缩放因子。若系统还希望“动作缩小但触觉更明显”，那是额外的主动触觉整形/力觉显示增益，建议另记为 \(g_h\)，不要和功率守恒缩放器的 \(\lambda_f\) 混在一起。
能量一致性条件¶
为什么 \(\lambda_p\) 和 \(\lambda_f\) 不能独立选择？
考虑主端输入功率和从端输出功率：
两者的比值：
如果 \(\lambda_f / \lambda_p \neq 1\)，系统要么产生能量（\(\lambda_f / \lambda_p > 1\)），要么消耗能量（\(\lambda_f / \lambda_p < 1\)）。产生能量意味着系统不再无源——可能导致不稳定。
回顾 D05-D06：在无源通信理论中，我们花了两章讨论遥操作系统的无源性条件。其核心是散逸不等式 \(\dot{V} \leq y^T u\)——系统储存的能量增长率不超过外部输入功率。运动缩放引入了额外的功率变换，如果缩放不满足能量一致，等价于在系统中插入了一个能量源。
因此，在本节定义的端口变量下，功率守恒条件要求：
本质洞察：运动缩放不是简单的坐标变换——它是一个**功率变换器**。在 \(x_s=\lambda_p x_m, f_m=\lambda_f f_s\) 这一组定义下，\(\lambda_f=\lambda_p\) 的本质是要求这个变换器**功率守恒**，否则系统会从缩放环节产生或消耗能量，破坏透明度，严重时也会破坏无源性。
跨领域类比——变压器要谨慎使用：理想变压器中，一侧电压放大 \(n\) 倍时，同侧电流会缩小 \(1/n\) 倍，这是因为端口方向和"哪一侧相对哪一侧"的定义已经固定。遥操作文献里也常见 \(\lambda_f=1/\lambda_p\) 的写法，但那通常对应的是相反方向的力映射或不同的端口变量定义。本文当前定义的是"从端力 \(f_s\) 映射到主端反馈力 \(f_m\)"，因此必须沿着上面的功率推导使用 \(\lambda_f=\lambda_p\)。如果项目希望采用机械优势式的倒数关系，需要先重新定义端口方向和功率符号，再重新推导。
主动放大要单独记账：很多系统的实际设计是
其中 \(\lambda_p\) 保证缩放环节功率守恒，\(g_h>1\) 是主动触觉放大。\(g_h\) 不是免费的机械优势，而是主动系统向操作者端输出额外能量；工程上必须配合力饱和、能量罐、TDPA 或严格的人机交互稳定性分析。没有力反馈的系统（ALOHA、UMI、OpenTeleVision 常见配置）则根本没有 \(\lambda_f\)，只有运动映射和视觉反馈。
典型系统的缩放参数¶
| 系统 | 应用场景 | \(\lambda_p\) | 无源 \(\lambda_f\) | 主动触觉增益 \(g_h\) | 设计考量 |
|---|---|---|---|---|---|
| da Vinci/dVRK | 腹腔镜手术 | 0.2-0.4 | 若做被动力反馈则同为 0.2-0.4 | 另行设计或无直接力反馈 | 动作缩小 2.5-5 倍，触觉通常不是简单线性反力 |
| 显微手术原型 | 眼科/耳科 | 0.01-0.1 | 0.01-0.1 | 可能 >1，需能量安全层 | 极端微操作，常需触觉可感知性增强 |
| ALOHA | 桌面操作 | 1.0 | N/A（无力反馈） | N/A | 同构 1:1 映射 |
| KONTUR-2 | 空间遥操作 | 1.0 | 1.0 | 由能量罐约束 | ISS → 地面，时延补偿优先 |
| 核退役系统 | 放射性拆除 | 2-5 | 2-5（按本文端口定义） | 通常限幅而非放大 | 动作放大，处理大型部件 |
姿态缩放的特殊性¶
位置缩放是线性的，但姿态缩放面临根本困难：旋转不是线性空间。
错误做法：将四元数分量线性缩放 \(q_s = \lambda_R \cdot q_m\)——结果不再是单位四元数。
正确做法：在 \(SO(3)\) 的切空间（李代数 \(\mathfrak{so}(3)\)）上进行缩放：
其中 \(\text{Log}: SO(3) \to \mathfrak{so}(3)\) 是矩阵对数映射，\(\text{Exp}: \mathfrak{so}(3) \to SO(3)\) 是矩阵指数映射。这样确保缩放结果始终是有效的旋转矩阵。
回顾 M01（Pinocchio 深度精读）：李群 \(SO(3)\) 上的运算必须通过 Exp/Log 映射在切空间中进行线性操作。在那里我们用 Exp/Log 处理旋转插值，现在用同样的工具处理旋转缩放——本质上都是"在弯曲空间中做线性运算"。
典型姿态缩放：da Vinci 手术中姿态通常不缩放（\(\lambda_R = 1\)），因为医生需要直观的手腕转动映射。但在显微手术中，细微的手腕抖动会被放大——此时 \(\lambda_R = 0.5\)（手转 60 度 → 器械转 30 度）可以增强稳定性。
反事实推理——如果违反能量一致性¶
如果设置 \(\lambda_p = 0.2\)（缩小 5 倍）但 \(\lambda_f = 1\)（力不缩放），而非本文端口定义下的功率守恒取值 \(\lambda_f = 0.2\)，会发生什么？
\(\lambda_f / \lambda_p = 5 \neq 1\)，系统在缩放环节向主端反馈的功率比从端端口功率大。操作者感受到的力 \(f_m = f_s\)（未缩小），但实际从端运动缩小了 5 倍；在硬接触和高增益主端控制下，这个不匹配会放大能量回注，容易引起振荡。
反之，如果希望显微手术中"动作缩小但触觉放大"，那已经不是本文这组端口变量下的无损缩放器，而是带额外反馈整形的人机交互设计。必须配合力饱和、能量罐或 TDPA，把超出的能量显式耗散掉，否则从端接触力被放大反馈到主端，主端又驱动从端继续挤压，可能形成**正反馈振荡**。这正是 D05 中 Llewellyn 稳定性准则所提醒的风险。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：缩放参考点未更新
错误做法：clutching 后不更新 x_m^ref 和 x_s^ref
现象：释放离合后从端突然跳到错误位置
根本原因：位置缩放公式 x_s = λ_p(x_m - x_m^ref) + x_s^ref 中的参考点
在 clutch 切换时必须同步更新
正确做法：clutch 释放瞬间 x_m^ref = x_m_current, x_s^ref = x_s_current
💡 概念误区：认为"缩小动作就是提高精度"
新手想法："λ_p = 0.1 比 λ_p = 0.5 更精确"
实际上：缩放比不是越小越好。传感器分辨率和量化噪声不会被缩放改变——
如果主端编码器分辨率是 0.01mm，λ_p = 0.1 时从端分辨率为 0.001mm，
但编码器噪声也被缩小到 0.001mm 量级，此时信噪比不变。
如果为了可感知性再叠加主动触觉增益 g_h，力传感器噪声也会被放大。
正确思考：选择 λ_p 需要在精度增益、操作者感觉阈值和主动增益稳定性之间权衡。
🧠 思维陷阱：认为"能量一致就保证稳定"
新手想法："只要力/位缩放满足功率守恒，系统就稳定了"
实际上：能量一致只是无源性的必要条件之一。通信延迟、采样周期、控制器非理想性
都会额外注入能量。D07 的 TDPA 正是为了在线检测和消除这些额外能量。
能量一致 + TDPA + 正确的控制器设计，三者缺一不可。
练习¶
- [手推] 推导：当主端和从端都是 1-DOF 线性阻抗系统（\(Z_m(s) = M_m s + B_m + K_m/s\)，\(Z_e(s) = M_e s + B_e + K_e/s\)），加入本文定义的缩放 \(x_s=\lambda_p x_m, f_m=\lambda_f f_s\) 后，从主端看到的等效环境阻抗 \(Z_e'(s)\) 是什么？再令 \(\lambda_f=\lambda_p\)，验证端口功率守恒。
- [编程] 用 Python 实现 \(SO(3)\) 上的姿态缩放函数。输入：主端旋转矩阵 \(R_m\)、参考旋转 \(R_m^{ref}\)、\(R_s^{ref}\)、缩放因子 \(\lambda_R\)。输出：从端目标旋转 \(R_s\)。用
scipy.spatial.transform.Rotation验证结果始终是有效旋转矩阵（\(\det(R_s) = 1\)，\(R_s^T R_s = I\)）。 - [思考题] 显微手术中常希望 \(\lambda_p = 0.05\)（动作缩小 20 倍）但主端触觉不要按 0.05 倍缩小，甚至希望适度放大。这个设计会偏离本文端口定义下的无损缩放条件。工程上如何在力可感知性、操作者最大承受力和无源性之间折中？提示：力饱和 + 能量补偿 + TDPA。
D8.2 笛卡尔映射 vs 关节映射 ⭐⭐¶
动机——两种运动空间的选择¶
上一节讨论了运动缩放的数学，但还没回答一个更基本的问题：映射发生在哪个空间？
当你设计一个遥操作系统时，主端传感器给你的是什么信号？可能是关节角度（如 ALOHA 的 Dynamixel 编码器），也可能是末端位姿（如 VR 手柄的 6-DOF tracking）。从端接受的是什么指令？可能是关节位置（如 joint position controller），也可能是笛卡尔位置（需要 IK 转换）。
这两种选择——关节空间映射和笛卡尔空间映射——不只是工程便利性的区别，而是**反映了完全不同的设计哲学和适用场景**。
反面——如果对异构系统强行用关节映射¶
假设你有一个 6-DOF UR5 作为主端，要操控一个 7-DOF Franka Panda 从端。如果强行做关节映射 \(q_s = q_m + \text{offset}\)：
- 自由度不匹配（6 vs 7）：第 7 个关节怎么办？固定？随机？
- 运动学不同构：UR5 的第 1 关节绕竖直轴旋转，Franka 的第 1 关节也是——但后续关节的 DH 参数完全不同。\(q_{m,3} = 0.5 \text{rad}\) 在 UR5 上是肘关节弯曲，在 Franka 上可能是完全不同的构型
- 结果：操作者看到主端做了一个"伸手去拿"的动作，但从端可能做出一个完全不相关的扭曲运动——失去直觉操控性
关节空间映射（Joint-space Mapping）¶
数学形式：
其中 \(q_{\text{offset}}\) 是标定时确定的固定偏置，补偿主端和从端的零位差异。
适用条件：主端和从端运动学相同或高度相似（同构臂）。
典型系统：
| 系统 | 主端 | 从端 | 关节数 | 映射细节 |
|---|---|---|---|---|
| ALOHA（Zhao 2023） | Dynamixel leader arm（Widowx-250 变体） | Dynamixel follower arm（相同构型） | 6+1（含夹爪） | 1:1 关节镜像，50 Hz |
| GELLO（Wu 2024） | 3D 打印同构 leader | 多种从端（Franka/UR5/xArm7） | 7+1 | 1:1 关节镜像，100 Hz |
| Mobile ALOHA（Fu 2024） | 同上 ALOHA | 同上 + 移动底盘 | 6+1 per arm + 2 base | 关节镜像 + 底盘手推 |
优势与局限：
| 优势 | 说明 |
|---|---|
| 无 IK 计算 | 直接关节角复制，延迟极低（<1 ms 映射延迟） |
| 无奇异问题 | 不经过笛卡尔空间，不存在雅可比奇异 |
| 关节限位自然对应 | 主端碰到限位 → 从端也在限位附近 |
| 实现简单 | 核心代码不超过 10 行 |
| 局限 | 说明 |
|---|---|
| 要求同构 | 主端必须与从端运动学一致 |
| 不能缩放 | 1:1 映射，无法做微/宏操作缩放 |
| 感知耦合 | 操作者必须在"关节空间"中思考——如果主端的朝向和从端不同，直觉操控性下降 |
ALOHA 的关节映射实现（精读 aloha_scripts/robot_utils.py）：
# ALOHA 关节映射核心代码（简化版）
# 来源：tonyzhaozh/aloha
def get_action(master_bot_left, master_bot_right):
"""读取两个 leader arm 的关节位置，作为两个 follower 的目标"""
action = np.zeros(14) # 7 per arm (6 joints + 1 gripper)
# 左臂：直接读取 leader 关节位置
action[:6] = master_bot_left.dxl_io.get_joint_positions()[:6]
action[6] = master_bot_left.dxl_io.get_single_position('gripper')
# 右臂：同样
action[7:13] = master_bot_right.dxl_io.get_joint_positions()[:6]
action[13] = master_bot_right.dxl_io.get_single_position('gripper')
return action
# 发送到 follower 也是直接写入关节位置
def set_follower_position(follower_bot, target_joints):
"""关节镜像——零映射逻辑"""
follower_bot.dxl_io.set_joint_positions(target_joints)
为什么 ALOHA 能用如此简单的映射？ 因为 leader 和 follower 物理上是同一型号的 Dynamixel 机械臂。Zhao 等人的设计哲学是：与其在软件上解决映射问题，不如在硬件上消除映射需求。这是一个深刻的工程洞察——复杂的映射算法不如简单的同构硬件可靠。
笛卡尔空间映射（Cartesian-space Mapping）¶
数学形式：
其中 \(T_m \in SE(3)\) 是主端末端位姿（通过正运动学或 VR tracking 获得），\(T_{\text{offset}}\) 是主从坐标系之间的变换，\(T_{\text{calib}}\) 是标定偏置。
适用条件：主端和从端运动学不同（异构系统），或需要运动缩放。
完整映射流程：
主端传感器
│
▼
┌──────────────┐
│ 正运动学 FK │ ← 如果主端给关节角
│ (或直接读取) │ ← 如果主端给位姿（VR/tracking）
└──────┬───────┘
│ T_m ∈ SE(3)
▼
┌──────────────┐
│ 坐标变换 │ T_offset, 缩放 λ_p, λ_R
│ + 运动缩放 │
└──────┬───────┘
│ T_s^des ∈ SE(3)
▼
┌──────────────┐
│ 逆运动学 IK │ ← 求解 q_s = IK(T_s^des)
│ (数值求解) │
└──────┬───────┘
│ q_s ∈ ℝ^n
▼
从端关节控制器
IK 失败处理：笛卡尔映射的最大风险是 IK 求解失败。在 M03 中我们详细讨论了 IK 失败的原因：目标位姿在工作空间之外、接近奇异构型、多解选择不一致。在遥操作中，IK 失败意味着从端"卡住"——操作者移动主端但从端不跟随，体验极差。
OpenTeleVision 的笛卡尔映射实现（精读 teleop/television.py）：
# OpenTeleVision: VR → IK → joint
# 来源：OpenTeleVision/TeleVision
class TeleVisionAgent:
def __init__(self):
# Pinocchio 模型用于 CLIK (Closed-Loop IK)
self.robot = pin.buildModelFromUrdf("unitree_h1.urdf")
self.data = self.robot.createData()
def step(self, vr_pose_left, vr_pose_right):
"""VR 6-DOF pose → IK → joint position"""
# 1. VR 坐标系 → 机器人坐标系变换
T_left = self.vr_to_robot_frame(vr_pose_left)
T_right = self.vr_to_robot_frame(vr_pose_right)
# 2. Pinocchio CLIK (迭代 IK)
q_left = self.solve_ik(T_left, arm="left", q_init=self.q_prev_left)
q_right = self.solve_ik(T_right, arm="right", q_init=self.q_prev_right)
# 3. 平滑滤波（防止 IK 解跳变）
q_left = self.alpha * q_left + (1 - self.alpha) * self.q_prev_left
q_right = self.alpha * q_right + (1 - self.alpha) * self.q_prev_right
self.q_prev_left = q_left
self.q_prev_right = q_right
return np.concatenate([q_left, q_right])
def solve_ik(self, T_des, arm, q_init, max_iter=100, eps=1e-4):
"""Pinocchio CLIK: 雅可比迭代法"""
q = q_init.copy()
frame_id = self.robot.getFrameId(f"{arm}_hand")
for i in range(max_iter):
pin.forwardKinematics(self.robot, self.data, q)
pin.updateFramePlacements(self.robot, self.data)
T_current = self.data.oMf[frame_id]
# 采用 LOCAL 约定：误差 log(T_current^{-1}T_des) 与 LOCAL Jacobian
# 都表达在当前末端局部坐标系，避免和 LOCAL_WORLD_ALIGNED 混用。
error = pin.log6(T_current.inverse() * T_des).vector
if np.linalg.norm(error) < eps:
break
J = pin.computeFrameJacobian(self.robot, self.data, q,
frame_id, pin.LOCAL)
dq = np.linalg.lstsq(J, error, rcond=None)[0]
q = pin.integrate(self.robot, q, dq * 0.5) # 步长 0.5 增加收敛性
return q
两种映射的选型决策¶
主端与从端运动学是否同构？
│
┌────────────┴────────────┐
│ 是 │ 否
▼ ▼
┌──────────┐ 需要运动缩放吗？
│ 关节映射 │ │
│ (推荐) │ ┌─────────┴─────────┐
└──────────┘ │ 是 │ 否
▼ ▼
┌──────────┐ ┌──────────┐
│ 笛卡尔映射│ │ 笛卡尔映射│
│ + 缩放 │ │ (基础) │
└──────────┘ └──────────┘
本质洞察：关节映射和笛卡尔映射不是"好坏之分"，而是**复杂性在硬件和软件之间的分配**。关节映射把复杂性推给硬件（要求同构设计）但换来了软件的极简和可靠；笛卡尔映射接受硬件异构，但把复杂性（IK、奇异处理、多解选择）留给了软件。ALOHA 选择了前者，da Vinci 选择了后者——两者都是在各自约束下的最优解。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：笛卡尔映射中 IK 初始值不连续
错误做法：每帧用固定初始值（如零构型）调用 IK
现象：连续两帧的 IK 解可能跳到不同分支（如肘关节 up vs down），
从端剧烈跳动
根本原因：数值 IK 是局部搜索，初始值决定收敛到哪个解
正确做法：用上一帧的解 q_prev 作为下一帧的初始值
自检方法：连续 100 帧中最大关节角变化 < 0.1 rad/frame
💡 概念误区：认为"关节映射不需要标定"
新手想法："直接 q_s = q_m 就行了"
实际上：即使同构臂，两台的零位也不完全相同。Dynamixel 的出厂零位偏差
可达 ±5 度。不标定 q_offset 会导致从端静止时也有偏差。
ALOHA 的标定流程：将两臂摆到已知构型，记录 q_offset = q_m_calib - q_s_calib。
GELLO 进一步需要因为 3D 打印公差做单关节逐一标定。
练习¶
- [代码精读] 精读
aloha_scripts/robot_utils.py的完整数据流。画出时序图，标注每步延迟来源：USB ↔ RS-485（~0.5 ms）、sync_read（~1 ms）、Python 处理（~1 ms）、sync_write（~1 ms）。验证总延迟 ~4 ms + sleep(16 ms) = 20 ms（50 Hz）。 - [编程] 实现笛卡尔映射函数：输入主端 6-DOF 位姿（来自 VR 手柄模拟），输出从端关节角。使用 Pinocchio 的 CLIK 算法。测试：让主端画圆，观察从端是否流畅跟踪。测量 IK 求解时间，确认每帧 < 1 ms。
- [思考题] GELLO 用同构 leader 实现关节映射，但 GELLO 的 leader 是 3D 打印的——关节有摩擦但没有电机。操作者"反向驱动"（backdrive）GELLO 关节来设置目标位置。这种设计与 ALOHA 的"leader 有电机"设计相比，对数据质量有什么影响？提示：考虑重力补偿和操作者疲劳。
D8.3 Retargeting——人手到机器人的运动重定向 ⭐⭐⭐¶
动机——人手与机器手的鸿沟¶
人手有 20+ 自由度（4 per finger × 5 fingers + thumb abduction/adduction），27 块骨骼，17 个关节。而机器人灵巧手的自由度从 1（简单夹爪）到 24（Shadow Hand）不等，运动学结构通常与人手差异巨大。
考虑一个具体场景：你戴着 MediaPipe 手部追踪设备（25 个关键点），要控制一个 Allegro Hand（16 DOF，4 finger × 4 joint each）。你的手指弯曲时，如何计算 Allegro 的 16 个关节角？
这就是 Retargeting（运动重定向） 要解决的问题：将人体运动学空间的运动映射到机器人运动学空间，在自由度数目和运动学结构都不同的条件下，尽可能保持操控意图。
如果不做 retargeting 直接用关节映射¶
人手食指有 4 个关节（MCP flexion/extension, MCP abduction/adduction, PIP, DIP），Allegro 的食指也有 4 个关节。看起来可以 1:1 映射？
但人手 MCP 关节的运动范围是 0-90 度（flexion），Allegro 的对应关节范围是 0-1.6 rad（约 92 度）——数值上接近但不完全匹配。更关键的是，人手指节的长度比例（近节:中节:远节 ≈ 2.5:1.5:1）与 Allegro 的不同（Allegro 近节更长）。
结果：人做了一个"捏取"动作（拇指和食指指尖接触），但 Allegro 的指尖无法接触——因为虽然关节角度对了，但指节长度不同导致指尖位置偏差 1-2cm。对于精密操作这是不可接受的。
三种 Retargeting 方法¶
方法一：指尖位置映射（Fingertip Position Mapping）¶
思想：只关心指尖在哪里，忽略中间关节构型。
其中 \(p_i^{\text{robot}}(q)\) 是机器人第 \(i\) 个指尖的正运动学位置，\(p_i^{\text{human}}\) 是人手第 \(i\) 个指尖的检测位置，\(w_i\) 是权重，\(N\) 是指尖数目（通常 5）。
优点：最简单，直接对齐指尖位置。
缺点： - 对人手大小敏感——同样的"捏取"动作，小手和大手的指尖距离不同，导致机器人行为不一致 - 不考虑手指姿态——两根手指指尖在同一位置但伸展方向不同，映射结果相同
方法二：向量映射（Vector Mapping, AnyTeleop, Qin 2023）¶
思想：不关心绝对位置，只关心指尖之间的相对关系（方向和比例）。
其中 \(v_{ij} = p_j - p_i\) 是从第 \(i\) 个关键点到第 \(j\) 个关键点的向量，\(s\) 是全局缩放因子（补偿手掌大小差异）。
为什么向量映射优于位置映射？
因为向量映射对平移不变——无论人手在空间中的绝对位置如何，只要手指的**相对构型**相同，映射结果就相同。同时，缩放因子 \(s\) 自动补偿了不同操作者的手掌大小差异。
AnyTeleop 的实现细节（Qin, RSS 2023）：
# AnyTeleop 的向量映射优化
# 来源：dexsuite/dex-retargeting (VectorOptimizer)
class VectorRetargeting:
def __init__(self, robot_model, finger_tips, joint_pairs):
"""
finger_tips: 机器人指尖 frame names
joint_pairs: 人手关键点配对 [(thumb_tip, index_tip), ...]
"""
self.model = robot_model
self.data = robot_model.createData()
self.pairs = joint_pairs # 关键点对
def retarget(self, human_keypoints, q_prev, scale=1.0, smooth_weight=0.05):
"""
human_keypoints: (25, 3) 人手 25 个关键点的 3D 位置
q_prev: 上一帧关节角（平滑约束）
"""
def objective(q):
# 正运动学
pin.forwardKinematics(self.model, self.data, q)
pin.updateFramePlacements(self.model, self.data)
cost = 0.0
for (i, j), (frame_i, frame_j) in zip(self.pairs, self.frame_pairs):
# 机器人向量
v_robot = self.data.oMf[frame_j].translation - \
self.data.oMf[frame_i].translation
# 人手向量（缩放）
v_human = scale * (human_keypoints[j] - human_keypoints[i])
cost += np.sum((v_robot - v_human) ** 2)
# 时间平滑正则化
cost += smooth_weight * np.sum((q - q_prev) ** 2)
return cost
result = scipy.optimize.minimize(
objective, q_prev,
method='L-BFGS-B',
bounds=self.joint_limits,
options={'maxiter': 50}
)
return result.x
方法三：DexPilot Pinch Prior（Handa, ICRA 2020）¶
思想：在向量映射的基础上，加入**捏取先验**——当检测到人手两指尖距离小于阈值（如 2cm），强制机器人指尖接触。
为什么需要 pinch prior？
MediaPipe 的手部关键点检测精度约 5-10mm。当人手做捏取动作时，两指尖的检测距离可能在 0-15mm 之间波动（噪声）。如果直接映射，机器人指尖会在"接触/不接触"之间高频振荡——物体在指尖间反复滑动。
Pinch prior 通过**二值化接触状态**解决这个问题：一旦检测到"可能在捏"（距离 < 阈值），就强制完全闭合。这是一种**迟滞（hysteresis）**设计——与 D03 中力控的接触/脱离切换用迟滞避免抖动完全相同的工程思想。
跨领域类比——施密特触发器：DexPilot 的 pinch prior 与电子学中的施密特触发器完全同构。施密特触发器用上下两个阈值（\(V_{TH}\), \(V_{TL}\)）消除噪声引起的输出振荡。DexPilot 也可以加上下双阈值：距离 < 1.5cm 时进入 pinch 模式，距离 > 2.5cm 时退出——但原论文简化为单阈值。
dex-retargeting 库架构¶
# dex-retargeting 库（Qin, ~300 GitHub Stars）的核心架构
# src/dex_retargeting/
#
# optimizer.py:
# - PositionOptimizer → 方法一：指尖位置映射
# - VectorOptimizer → 方法二：向量映射
# - DexPilotOptimizer → 方法三：向量映射 + pinch prior
#
# robot_wrapper.py:
# - 后端：Pinocchio (运动学/雅可比)
# - 支持：Allegro, Shadow, Inspire, LEAP 灵巧手
#
# hand_detector.py:
# - 输入源：MediaPipe / Leap Motion / OptiTrack
# - 输出：25 个关键点的 3D 坐标
三种方法对比¶
| 方面 | 位置映射 | 向量映射 | DexPilot |
|---|---|---|---|
| 手掌大小鲁棒性 | 敏感 | 缩放不变 | 缩放不变 |
| 捏取精度 | 噪声敏感 | 噪声敏感 | 二值化稳定 |
| 计算量 | 最低 | 中等 | 中等 |
| 实现复杂度 | 最低 | 中等 | 较高 |
| 适用场景 | 粗操作 | 通用 | 精密捏取 |
| 代表系统 | 早期 teleoperation | AnyTeleop | DexPilot, DART |
⚠️ 常见陷阱¶
⚠️ 编程陷阱：retargeting 优化不加关节限位约束
错误做法：optimize(q, bounds=None)
现象：优化器输出超出关节极限的 q，发送到机器人后触发硬件保护停机
根本原因：L-BFGS-B 在无界时可能收敛到物理不可达的解
正确做法：always 设置 bounds=[(q_min_i, q_max_i) for i in range(nq)]
💡 概念误区：认为"retargeting 精度越高越好"
新手想法："优化到残差 < 1e-6 才发送"
实际上：MediaPipe 的输入精度只有 5-10mm，优化残差小于输入噪声是过拟合噪声。
同时高精度要求更多迭代 → 延迟增加 → 操控体验下降。
正确做法：设置 max_iter=50，残差阈值 1e-3 足够。速度比精度重要。
🧠 思维陷阱：认为"retargeting 只需要一次标定"
新手想法："标定了缩放因子 s 就一劳永逸"
实际上：不同操作者手掌大小不同（s 不同），同一操作者不同手部状态
（冷手 vs 暖手、戴手套 vs 不戴）也会导致关键点偏差。
工业方案：每次使用前做 10 秒在线标定（张开-握拳-捏取）自动估计 s。
练习¶
- [编程] 用 dex-retargeting 库，从 MediaPipe 手关键点输入，retarget 到 MuJoCo Allegro Hand。对比 PositionOptimizer 和 VectorOptimizer 的跟踪精度：让操作者做 10 次捏取-释放循环，统计两种方法的指尖距离误差均值和方差。
- [手推] 推导向量映射的梯度。设目标函数为 \(f(q) = \sum_{(i,j)} \|v_{ij}(q) - s v_{ij}^h\|^2\)，其中 \(v_{ij}(q) = p_j(q) - p_i(q)\)。求 \(\frac{\partial f}{\partial q}\)，表达式中应出现雅可比矩阵 \(J_i = \frac{\partial p_i}{\partial q}\)。
- [跨章综合题] 结合 M03（IK 求解器）和 D08（retargeting）：将 retargeting 问题看作一个特殊的 IK 问题——多个末端（5 个指尖）同时跟踪多个目标。与单末端 IK 相比，多末端 IK 在奇异性处理、冗余解选择上有什么新的困难？用 Pinocchio 实现一个多目标 CLIK（同时跟踪 5 个指尖），测试其收敛性。
D8.4 Clutching（离合）——工作空间边界处理 ⭐⭐¶
动机——有限工作空间的根本矛盾¶
每个遥操作主端设备都有有限的工作空间。Force Dimension Omega.7 的工作空间是一个直径 120mm 的球形区域。Franka Panda 作为从端的有效工作范围约 855mm。如果用 1:1 笛卡尔映射，操作者最多能让 Franka 移动 120mm——不到工作范围的 14%。
即使加了缩放（如 \(\lambda_p = 3\)），也只能覆盖 360mm——仍不到一半。更普遍地，任何有限工作空间的主端都无法通过纯缩放覆盖任意大的从端工作空间。
反面——如果不做 clutching¶
操作者推主端到工作空间边界。此时主端碰到物理硬限位（硬件挡块），无法继续移动。但从端还需要继续移动。操作者用力推 → 力被挡块吸收 → 从端不动 → 操作者沮丧。
或者更危险的情况：有些力反馈设备的工作空间由软件限位保护。达到软限位时产生大回复力，操作者被"弹回" → 从端也被弹回 → 操作者再推 → 再弹回 → 振荡。
Clutching 机制详解¶
Clutching（离合）的灵感来自打字机时代的"回车"操作：打字到行末，推回滑架到行首，继续打字。在遥操作中：
基本流程：
- 操作者按下 clutch 按钮（脚踏/手柄按钮/VR 手势）
- 从端冻结：保持当前位姿 \(T_s^{\text{frozen}} = T_s^{\text{current}}\)
- **操作者移动主端**到工作空间中心区域（不影响从端）
- 释放 clutch：更新参考偏置
不同系统的 clutch 实现：
| 系统 | Clutch 触发方式 | 安全机制 | 说明 |
|---|---|---|---|
| da Vinci | 脚踏 + 头部红外传感器 | 双校验：头部离开视窗 → 自动 clutch + freeze | 防止医生转头时无意移动器械 |
| Sigma.7 | 按钮 | 释放时渐进恢复（200ms ramp） | 防止偏置跳变导致的从端跳动 |
| VR (Quest3) | 控制器 grip 按钮 | 无（依赖视觉反馈） | 最简单实现 |
| ALOHA | 无需 clutch | N/A | 同构臂，工作空间自然匹配 |
| UMI | 无需 clutch（手持式） | N/A | 人直接在工作空间中操作 |
Clutch 切换的能量问题¶
回顾 D07（TDPA 与工程实现）：TDPA 通过在线监控端口的输入/输出能量来保证无源性。Clutch 切换引入了一个新的能量问题。
问题：clutch 释放瞬间，主端位置 \(x_m\) 和"虚拟参考" \(x_m^{\text{new\_center}}\) 之间可能有偏差（操作者没有精确回到中心）。这个偏差在释放瞬间突然生效，等价于一个**阶跃位移输入**——阶跃输入包含无穷大的瞬时功率。
Lee-Huang（2010）的解决方案——被动集位修正（Passive Set-Position Modulation, PSPM）：
- Clutch 释放瞬间，计算偏置跳变 \(\Delta x = x_m - x_m^{\text{new\_center}}\)
- 将 \(\Delta x\) 产生的能量记入 TDPA 的能量监控器
- 如果累积能量超过预算，TDPA 会自动消散多余能量（通过注入阻尼）
- 效果：clutch 切换过程中系统保持无源性
跨领域类比——汽车离合器：遥操作的 clutching 与汽车离合器有相似的设计考量。汽车换挡时，离合器逐渐接合（半联动），而非瞬间切换——瞬间切换会导致传动系统冲击。遥操作的 clutch 也需要"软切换"：释放后用 200ms 的线性 ramp 从冻结状态过渡到跟踪状态，而非瞬间跳变。
Indexing（索引）——Clutching 的泛化¶
Clutching 只处理平移空间的边界问题。对于旋转空间，类似的机制称为 Indexing：
旋转 Indexing：
1. 操作者将主端旋转到工作范围边界（如手腕旋转 ±90 度）
2. 触发 index：从端冻结旋转
3. 操作者将手腕转回中立位置
4. 释放 index：更新旋转偏置
R_s^des = R_s^frozen · Exp(λ_R · Log(R_m^new_center^{-1} · R_m))
双模式切换的用户体验：
经验研究（MacKenzie 2001）表明，频繁 clutching 显著降低操作效率——每次 clutch 切换约消耗 1-2 秒，且打断操作者的运动连续性。da Vinci 系统的设计指南建议：每分钟 clutch 次数不超过 4 次。超过这个频率，说明工作空间匹配或缩放比设置不当，需要重新标定。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：clutch 释放时不做偏置更新
错误做法：clutch 期间只冻结从端，释放后直接恢复原映射
现象：释放瞬间从端突然跳到一个意外位置
（因为操作者在 clutch 期间移动了主端）
根本原因：主端位置变了但 x_m_ref 没更新
正确做法：释放时 x_m_ref = x_m_current, x_s_ref = x_s_frozen
💡 概念误区：认为"clutching 是工作空间小的妥协"
新手想法："如果主端工作空间足够大就不需要 clutch"
实际上：即使工作空间足够大，clutch 还有另一个重要用途——
**暂停操作**。手术中医生需要休息、讨论、查看影像。
clutch 提供了一个安全的"暂停按钮"，从端保持静止，
操作者可以离开控制台。这在 ALOHA 系统中用 leader-follower
的物理断开实现，但在双边遥操作中必须通过 clutch 实现。
练习¶
- [编程] 实现 clutch 机制：用 Python + MuJoCo 仿真一个 1-DOF 主从系统。添加键盘触发的 clutch 功能。测试：主端从左极限 clutch-移动-释放-到达右极限，从端应能从 \(x = 0\) 到 \(x = 1m\)。比较有无 clutch 时的可达空间。
- [思考题] UMI 系统不需要 clutch，因为操作者直接在工作空间中手持夹爪。这种"手持式遥操作"的工作空间等于操作者手臂的可达范围（约 1.5m 直径球）。如果任务要求操作者在 2m × 2m 的桌面上工作，UMI 的操作者需要走动。与 da Vinci 的 clutch 相比，"走动"是更好还是更差的"clutching"？从操作效率、数据质量、操作者疲劳三个维度分析。
D8.5 各遥操作系统底层架构对比 ⭐⭐¶
动机——为什么需要系统级理解？¶
前几节讲了运动映射的数学基础。但实际构建遥操作系统时，映射只是其中一环——你还需要考虑硬件选型、通信架构、控制频率、数据格式、成本等系统级因素。本节对 8 个代表性遥操作系统做深入架构分析，帮助你在实际项目中做出正确的设计决策。
科研发展脉络¶
| 年份 | 论文/项目 | Venue | 引用/Stars | 核心贡献 |
|---|---|---|---|---|
| 2014 | Kazanzides et al., "dVRK Open-Source Research Kit" | ICRA 2014 | ~400 | da Vinci 开源套件；笛卡尔缩放 + 震颤滤波 + clutch |
| 2020 | Handa et al., "DexPilot" | ICRA 2020 | ~350 | 多相机 + DART 追踪 23-DOF Allegro；pinch prior 重定向 |
| 2023 | Qin et al., "AnyTeleop" | RSS 2023 | ~200 | 通用 vision-based 遥操作；优化式向量重定向 |
| 2023 | Zhao et al., "ACT/ALOHA" | RSS 2023 | ~1200 Stars | 低成本同构 leader-follower；50 Hz 关节镜像 |
| 2024 | Fu et al., "Mobile ALOHA" | CoRL 2024 | ~4400 Stars | ALOHA + 移动底盘；16D action；co-training |
| 2024 | Wu et al., "GELLO" | IROS 2024 | ~300 Stars | 3D 打印同构 leader，$300 BOM；100 Hz |
| 2024 | Chi et al., "UMI" | RSS 2024 | ~2000 Stars | 手持夹爪 + GoPro + ORB-SLAM3；无需机器人即可采集 |
| 2024 | Cheng et al., "OpenTeleVision" | CoRL 2024 | ~1100 Stars | VR → IK → joint；WebXR；stereo 视频回传 |
| 2024 | Shaw et al., "ACE" | CoRL 2024 | ~130 Stars | 被动外骨骼；末端位姿映射；跨 embodiment |
系统详细对比¶
| 系统 | master 类型 | 映射方式 | slave 底层 | 力反馈 | 控制频率 | BOM | 数据格式 |
|---|---|---|---|---|---|---|---|
| ALOHA | 同构 Dynamixel leader | 关节 1:1 | 位控 PID 1 kHz | 无 | 50 Hz Python | ~$20k | HDF5 |
| GELLO | 3D 打印同构 leader | 关节 1:1 | 阻抗/位控 1 kHz | 无 | 100 Hz | $300+robot | HDF5 |
| UMI | 手持夹爪+GoPro | SLAM轨迹 | 阻抗(Franka)/位控(UR) | 无 | 10 Hz 策略 | ~$500 | zarr |
| OpenTeleVision | VR 头显(Quest3/VP) | 笛卡尔 IK | 位控(Unitree) | 视觉 | 60 Hz | ~$500 VR | HDF5 |
| ACE | 被动外骨骼 | 笛卡尔 IK | 位控 | 无 | 30-60 Hz | ~$200 | HDF5 |
| da Vinci/dVRK | MTM 7-DOF cable | 笛卡尔缩放 | 力矩 3-10 kHz | 有 | 200-500 Hz | ~$200k+ | rosbag |
| Sigma.7 | 7-DOF haptic | 笛卡尔 | 阻抗 1 kHz | 有(20N) | 4-8 kHz | ~$50k | 自定义 |
| Bi-ACT | 4-CH bilateral | 关节/笛卡尔 | 力矩 1 kHz | 有(DOB) | 1 kHz | ~$30k | HDF5 |
六大系统深度剖析¶
ALOHA——极简主义的胜利¶
设计哲学：消除一切不必要的复杂性。不做力反馈、不做笛卡尔映射、不做缩放——只做关节镜像。
系统架构：
Leader Arm (Widowx-250 6S) Follower Arm (同型号)
│ Dynamixel USB │ Dynamixel USB
└───────┐ ┌───────────────┘
▼ ▼
Python Control Loop (50 Hz)
┌──────────────────────────┐
│ 1. sync_read(leader) │ ~1 ms
│ 2. q_follower = q_leader │ ~0 ms (直接复制!)
│ 3. sync_write(follower) │ ~1 ms
│ 4. record(q, images) │ 异步
│ 5. sleep(16ms) │ 凑 50 Hz
└──────────────────────────┘
成本构成（双臂系统）：
| 组件 | 单价 | 数量 | 小计 |
|---|---|---|---|
| Widowx-250 6S | $2,500 | 4 (2 leader + 2 follower) | $10,000 |
| Intel RealSense D405 | $300 | 4 | $1,200 |
| USB Hub + 线缆 | $50 | 2 | $100 |
| 铝型材底座 | $200 | 1 | $200 |
| 总计 | ~$11,500 |
为什么 ALOHA 能以如此简单的系统取得如此好的效果？ 三个关键因素：
- 同构硬件消除映射误差：关节 1:1 映射没有 IK 误差、没有奇异问题
- ACT 策略补偿硬件缺陷：50 Hz 控制频率和无力反馈的缺陷被 Transformer 策略通过视觉闭环学习补偿
- 操作者直觉：同构 leader 给操作者直接的体感反馈（通过 leader 臂的重量和惯性）
UMI——无机器人数据采集的范式突破¶
设计哲学：数据采集不需要机器人。
UMI 的最大突破是分离了"数据采集"和"机器人控制"两个阶段。操作者只需手持一个带 GoPro 的夹爪在桌面上操作，ORB-SLAM3 从 GoPro 视频中恢复 6-DOF 轨迹。后续在机器人上部署时，策略将视觉观测映射到末端位姿，再通过 IK 转为关节指令。
数据采集硬件（BOM ~$500）：
┌─────────────────────────────────┐
│ 手持夹爪单元 │
│ ├─ GoPro Hero 12 (~$350) │ → 第一人称视频
│ ├─ 3D 打印夹爪壳体 (~$30) │ → 刚性连接
│ ├─ 鱼眼镜头标定板 (~$20) │ → 内参标定
│ └─ ArUco marker (~$5) │ → 外参标定
└─────────────────────────────────┘
│ SD card / WiFi
▼
┌─────────────────────────────────┐
│ 后处理 Pipeline │
│ ├─ ORB-SLAM3 → 6-DOF 轨迹 │ → 手持夹爪在世界坐标系的位姿序列
│ ├─ GoPro 鱼眼去畸变 │
│ ├─ 夹爪开合检测（手指触发器） │
│ └─ 数据存储 → zarr 格式 │
└─────────────────────────────────┘
本质洞察：UMI 的核心洞察不是技术性的，而是概念性的——遥操作的本质是采集人类操作意图，而不是实时控制机器人。如果最终目标是训练策略，那么"人操控机器人"和"人直接操作"采集的数据在**意图**层面是等价的。差别只在于坐标系转换和执行器接口。UMI 消除了"通过机器人采集"的中间环节，把复杂性从硬件搬到了后处理软件中。
OpenTeleVision——VR 沉浸式遥操作¶
设计哲学：利用 VR 的沉浸感和 6-DOF 追踪能力，实现低成本但高质量的遥操作。
Meta Quest 3 / Apple Vision Pro
│ WebXR API (60 Hz)
│ ├─ 头部 6-DOF pose
│ ├─ 左手 6-DOF pose + 按钮
│ └─ 右手 6-DOF pose + 按钮
│
▼ WebSocket (LAN)
┌─────────────────────────────────┐
│ 控制服务器 (Python) │
│ ├─ VR pose → robot frame 变换 │
│ ├─ Pinocchio CLIK (IK) │ → 每帧 < 1 ms
│ ├─ 关节角平滑滤波 │
│ └─ 发送 joint command → 机器人 │
└─────────────────────────────────┘
│ 同时
▼
┌─────────────────────────────────┐
│ 视频回传 │
│ ├─ 双目相机（机器人头部） │
│ ├─ stereo → VR 头显 │ → 立体视觉沉浸感
│ └─ WebRTC 低延迟流 │ → ~50 ms 延迟
└─────────────────────────────────┘
VR 遥操作的完整映射代码：
# VR 手柄 → 机器人末端位姿映射（OpenTeleVision 风格）
import numpy as np
import pinocchio as pin
from scipy.spatial.transform import Rotation
class VRTeleopMapper:
"""VR 6-DOF 追踪 → 机器人关节指令映射器"""
def __init__(self, robot_model, frame_name="panda_hand_tcp"):
self.model = robot_model
self.data = robot_model.createData()
self.frame_id = robot_model.getFrameId(frame_name)
# VR → 机器人坐标系变换（需在标定时测量）
# VR: y-up, z-forward; Robot: z-up, x-forward
self.T_vr_to_robot = np.eye(4)
self.T_vr_to_robot[:3, :3] = Rotation.from_euler(
'xyz', [90, 0, -90], degrees=True).as_matrix()
# 运动缩放因子
self.position_scale = 0.5 # VR 动作缩小到 50%
self.orientation_scale = 1.0 # 姿态 1:1
# clutch 状态。True 表示离合按下：从端冻结，VR 手柄可自由重定位。
self.clutched = False
self.vr_ref = None # 最近一次释放 clutch 时的 VR 位姿
self.robot_ref = None # 最近一次释放 clutch 时的机器人位姿
# IK 参数
self.ik_dt = 1e-1
self.ik_damp = 1e-6
self.q_current = pin.neutral(robot_model)
def calibrate(self, T_vr_current, T_robot_current):
"""标定 VR 与机器人之间的偏置"""
self.vr_ref = T_vr_current.copy()
self.robot_ref = T_robot_current.copy()
def engage_clutch(self, T_robot):
"""按下 clutch：冻结机器人当前位姿"""
self.clutched = True
self.robot_ref = T_robot.copy()
def release_clutch(self, T_vr_current, T_robot_current=None):
"""释放 clutch：用释放瞬间的 VR 位姿重置参考点"""
self.vr_ref = T_vr_current.copy()
if T_robot_current is not None:
self.robot_ref = T_robot_current.copy()
self.clutched = False
def map(self, T_vr_current):
"""
将 VR 手柄位姿映射到机器人末端目标位姿
T_vr_current: (4,4) VR 手柄在 VR 坐标系下的位姿
返回: q_target (nq,) 目标关节角
"""
if self.clutched or self.vr_ref is None or self.robot_ref is None:
return self.q_current # clutch 按下期间冻结从端
# Step 1: 计算 VR 增量（相对于最近一次释放 clutch 的参考位姿）
delta_vr = np.linalg.inv(self.vr_ref) @ T_vr_current
# Step 2: 坐标系变换 + 缩放
delta_robot = self.T_vr_to_robot @ delta_vr @ np.linalg.inv(self.T_vr_to_robot)
delta_robot[:3, 3] *= self.position_scale
# 姿态缩放必须在 SO(3) 的切空间中做：
# Log(R_delta) -> rotvec，缩放 rotvec，再 Exp 回 SO(3)。
R_delta = Rotation.from_matrix(delta_robot[:3, :3])
rotvec = R_delta.as_rotvec()
delta_robot[:3, :3] = Rotation.from_rotvec(
self.orientation_scale * rotvec
).as_matrix()
# Step 3: 应用到机器人参考位姿
T_target = self.robot_ref @ delta_robot
# Step 4: 数值 IK（CLIK 迭代）
T_target_se3 = pin.SE3(T_target[:3, :3], T_target[:3, 3])
q_sol = self._solve_ik(T_target_se3)
if q_sol is not None:
self.q_current = q_sol
return self.q_current
def _solve_ik(self, T_des, max_iter=50, eps=1e-4):
"""CLIK 数值 IK 求解"""
q = self.q_current.copy()
for _ in range(max_iter):
pin.forwardKinematics(self.model, self.data, q)
pin.updateFramePlacements(self.model, self.data)
T_cur = self.data.oMf[self.frame_id]
err = pin.log6(T_cur.inverse() * T_des).vector
if np.linalg.norm(err) < eps:
return q
J = pin.computeFrameJacobian(
self.model, self.data, q, self.frame_id, pin.LOCAL)
# 阻尼最小二乘
JtJ = J.T @ J + self.ik_damp * np.eye(self.model.nv)
dq = np.linalg.solve(JtJ, J.T @ err)
q = pin.integrate(self.model, q, self.ik_dt * dq)
return q # 未收敛也返回最近解
OpenTeleVision vs ALOHA 的取舍：
| 维度 | OpenTeleVision | ALOHA |
|---|---|---|
| 映射方式 | 笛卡尔 IK（灵活但可能失败） | 关节 1:1（简单但要求同构） |
| 操作者视角 | 第一人称立体（沉浸） | 第三人称（直接看机器人） |
| 适用从端 | 任意臂（只要有 IK） | 仅同构臂 |
| 成本 | VR $500 + 从端 | leader + follower 对 |
| 数据多样性 | 高（任何人可操作） | 中（需要专用 leader） |
| 控制精度 | 依赖 IK 质量 | 高（直接关节） |
力反馈对数据质量的影响——实验证据¶
这是一个有争议的问题：力反馈到底值不值？
支持力反馈的证据¶
-
Bi-ACT（Buamanee 2024）：在豆腐/塑料杯等软物操作任务中，有力反馈的遥操作数据训练的策略成功率比无力反馈高 2-3 倍。原因：位控策略不知道接触力大小，容易过压破坏软物。
-
KONTUR-2（Panzirsch 2017）：ISS 微重力环境下宇航员遥操作，有力反馈使力控精度提升 40%。
-
手术机器人文献（多项 meta-analysis）：有力反馈减少组织损伤 30-50%。
反对力反馈（或说"不值得"）的证据¶
-
ALOHA/Mobile ALOHA（Zhao/Fu 2023-2024）：无力反馈 → 50 demos 即可 80-90% 成功率。ACT 策略从视觉闭环学习补偿力。
-
成本对比：Sigma.7 $50k vs GELLO $300 → 170 倍价差。同样预算做 170 个 GELLO 采集 170 倍数据。
-
Foundation Model 扩展性：RT-X/DROID/Open-X 的 95%+ 数据来自无力反馈遥操作 → Foundation Model 不能从力反馈中受益。
教学结论¶
力反馈在**接触丰富 + 力敏感**任务（手术、软物操作、精密装配）中不可替代；在**位置主导**任务（pick-and-place、清洁）中边际收益低。
本质洞察：力反馈的价值不在于"让遥操作更舒适"——它的本质价值是**让操作者能感知和控制无法从视觉推断的物理量**（接触力、刚度、滑动）。如果任务中所有关键信息都可以从视觉获取（物体位置、朝向、是否接触），力反馈的边际价值趋近于零。当前 Foundation Model 生态由于硬件成本限制未能大规模采纳力反馈数据——这是未来 3-5 年最具研究价值的空白。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：VR 遥操作时坐标系不匹配
错误做法：直接将 Quest3 的 VR 坐标系位姿发送给机器人 IK
现象：操作者向前伸手，机器人向上或向侧方移动
根本原因：VR 坐标系 (y-up, z-forward) 与机器人坐标系 (z-up, x-forward) 不同
正确做法：在初始化时标定 T_vr_to_robot 变换矩阵
自检方法：操作者做 ±X, ±Y, ±Z 方向的平移，确认从端运动方向一致
🧠 思维陷阱：认为"更贵的遥操作系统 = 更好的数据"
新手想法："用 $50k 的 Sigma.7 采集的数据肯定比 $300 的 GELLO 好"
实际上：数据质量由任务决定，不由硬件价格决定。
对于 pick-and-place，GELLO 的 100 Hz 关节镜像产生的数据质量
可能高于 Sigma.7 的笛卡尔映射（后者经过 IK 引入噪声）。
正确思考：先分析任务需求（是否需要力感知/精密控制/缩放），
再选择最适合的系统——不是最贵的系统。
练习¶
- [代码精读] 精读
mtsTeleOperationPSM.cpp（dVRK）的遥操作循环。标注：orientation lock/follow 切换逻辑、scale factor 参数、clutch 状态机、force feedback（如有）。画出完整的状态机图。 - [设计题] 你要设计一个成本 < $5000 的力反馈双臂遥操作系统。方案：GELLO 每臂 $300 × 2 = $600，加 Dynamixel 电流模式力估计（通过电机电流反推扭矩），再加一个简单的波变量双边控制。分析：(a) Dynamixel 电流估扭矩精度（±0.5 Nm）是否足够？(b) 100 Hz 带宽是否满足稳定性要求？(c) 与 D07 的 TDPA 结合是否可行？
- [跨章综合题] 结合 D06（波变量理论）和 D08（系统架构）：如果在 OpenTeleVision（Quest3 → Unitree 双臂）的基础上添加力反馈（通过 VR 手柄的振动马达），(a) 振动马达的带宽（~200 Hz）是否足够表达接触力？(b) WiFi 通信延迟（~5-20 ms）是否需要波变量补偿？(c) 单向力反馈（只有振动大小，没有方向）对操作质量有多大帮助？
D8.6 数据采集 Pipeline ⭐⭐¶
动机——从遥操作到训练数据¶
前五节讲了如何控制机器人（映射、缩放、clutch、系统架构）。但对于模仿学习（D04）来说，遥操作只是手段——目的是采集高质量的训练数据。
回顾 D04（双臂学习）：ACT 策略的输入是图像 + 关节状态，输出是 action chunk（未来 k 步的关节目标）。训练数据的每一帧需要包含：观测（图像 + 关节角 + 夹爪状态）和动作（下一步关节目标）。
数据采集 pipeline 的核心挑战：
- 多模态同步：图像（30-60 Hz）和关节状态（50-1000 Hz）的时间对齐
- 存储效率：一次 30 秒演示 × 4 相机 × 640×480 × 30 fps ≈ 2.5 GB 原始数据
- 格式标准化：不同系统产出不同格式，需要统一到训练框架能读取的格式
- 数据质量：如何过滤掉失败的、不完整的、质量差的演示
完整 Pipeline 架构¶
┌─────────────────────────────────────────────────────────┐
│ 实时采集层 │
│ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ 相机 ×N │ │关节状态 │ │ 夹爪状态 │ ← 可选：力/触觉│
│ │ 30-60 Hz │ │ 50-1kHz │ │ 50-1kHz │ │
│ └────┬─────┘ └────┬─────┘ └────┬─────┘ │
│ └──────────────┴──────────────┘ │
│ │ 时间戳对齐 │
│ ▼ │
│ ┌────────────────────┐ │
│ │ ROS2 rosbag 录制 │ ← 或 Python 直接录制 │
│ │ (mcap format) │ │
│ └────────┬───────────┘ │
└────────────────────┼────────────────────────────────────┘
│ 离线
▼
┌─────────────────────────────────────────────────────────┐
│ 后处理层 │
│ ┌──────────────────┐ ┌──────────────────┐ │
│ │ 时间重采样 │ │ 图像预处理 │ │
│ │ (统一到 50 Hz) │ │ (resize/crop/ │ │
│ │ 关节角插值 │ │ undistort) │ │
│ └────────┬─────────┘ └────────┬─────────┘ │
│ └──────────────────────┘ │
│ │ │
│ ▼ │
│ ┌────────────────────┐ │
│ │ 格式转换 │ │
│ │ → HDF5 (ALOHA) │ │
│ │ → LeRobot parquet │ │
│ │ → zarr (UMI) │ │
│ └────────┬───────────┘ │
└────────────────────┼────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────────────┐
│ 质量评估层 │
│ ┌──────────────────┐ ┌──────────────────┐ │
│ │ 成功/失败标注 │ │ 统计分析 │ │
│ │ (人工/自动) │ │ (分布/异常值) │ │
│ └────────┬─────────┘ └────────┬─────────┘ │
│ └──────────────────────┘ │
│ │ 过滤后 │
│ ▼ │
│ ┌────────────────────┐ │
│ │ 训练数据集 │ → D04 ACT/DP 训练 │
│ │ (清洗后的 HDF5) │ │
│ └────────────────────┘ │
└─────────────────────────────────────────────────────────┘
时间同步——最容易被低估的难题¶
问题：相机帧率 30 fps（33.3 ms），关节状态 1 kHz（1 ms）。一帧图像对应哪个关节状态？
三种同步策略：
| 策略 | 方法 | 延迟/精度 | 适用 |
|---|---|---|---|
| 软件同步 | 用 ROS2 message_filters ApproximateTimeSynchronizer | ±5-15 ms | 大多数场景够用 |
| 硬件同步 | 相机外触发 + 关节读取绑定同一时钟 | ±0.1 ms | 高精度需求（如手术） |
| 后处理插值 | 录制时不同步，后处理按图像时间戳插值关节状态 | 取决于插值方法 | UMI 等离线系统 |
ROS2 软件同步实现：
# ROS2 多模态同步录制
import rclpy
import numpy as np
from rclpy.node import Node
from message_filters import ApproximateTimeSynchronizer, Subscriber
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, JointState
from std_msgs.msg import Float64
class DataCollector(Node):
def __init__(self):
super().__init__('data_collector')
self.bridge = CvBridge()
# 订阅多个话题
self.img_sub = Subscriber(self, Image, '/camera/color/image_raw')
self.joint_sub = Subscriber(self, JointState, '/joint_states')
self.gripper_sub = Subscriber(self, Float64, '/gripper_position')
# 近似时间同步器（容忍 10ms 偏差）
# Float64 没有 Header，需要 allow_headerless=True；它会使用消息到达时间。
# 高精度采集时应改成带 header 的自定义 gripper 状态消息。
self.sync = ApproximateTimeSynchronizer(
[self.img_sub, self.joint_sub, self.gripper_sub],
queue_size=10,
slop=0.01, # 10ms 容忍窗口
allow_headerless=True
)
self.sync.registerCallback(self.synced_callback)
self.episode_data = []
def synced_callback(self, img_msg, joint_msg, gripper_msg):
"""三模态同步回调"""
frame = {
'timestamp': img_msg.header.stamp.sec +
img_msg.header.stamp.nanosec * 1e-9,
'image': self.bridge.imgmsg_to_cv2(img_msg),
'joint_positions': np.array(joint_msg.position),
'joint_velocities': np.array(joint_msg.velocity),
'gripper_position': gripper_msg.data,
}
self.episode_data.append(frame)
数据格式标准¶
Canonical schema（本课程统一约定）¶
后续 D04/D08/D10 统一使用以下语义，避免 actions/action、observations/qpos/observation.state 混用：
| 语义 | Canonical key | 形状 | ALOHA HDF5 legacy | LeRobot |
|---|---|---|---|---|
| 关节/夹爪状态 | observation.state |
(T, state_dim) |
observations/qpos |
observation.state |
| 关节速度 | observation.velocity |
(T, state_dim) |
observations/qvel |
observation.velocity（可选） |
| 动作目标 | action |
(T, action_dim) |
action（单数，不用 actions/） |
action |
| 图像 | observation.images.<cam> |
(T,H,W,3) 或 mp4 |
observations/images/<cam> |
observation.images.<cam> |
| 时间戳 | timestamp |
(T,) |
timestamps 或 attrs |
timestamp |
| 任务名/参数 | task.name, task.params |
episode 级 | attrs / JSON sidecar | tasks.jsonl + episode metadata |
ALOHA-HDF5 → LeRobot adapter 的核心就是字段重命名和视频编码：
def aloha_hdf5_frame_to_lerobot(h5, t, cam="cam_high"):
frame = {
f"observation.images.{cam}": h5[f"observations/images/{cam}"][t],
"observation.state": h5["observations/qpos"][t].astype("float32"),
"action": h5["action"][t].astype("float32"),
"timestamp": h5["timestamps"][t] if "timestamps" in h5 else t / 50.0,
}
if "observations/qvel" in h5:
frame["observation.velocity"] = h5["observations/qvel"][t].astype("float32")
return frame
反向适配 LeRobot → ALOHA-HDF5 时，只需把 observation.state 写回 observations/qpos，把 observation.images.<cam> 解码成 observations/images/<cam>，并保持 action 单数命名。
HDF5 格式（ALOHA 标准）¶
# ALOHA HDF5 数据结构
import h5py
# 写入
with h5py.File('episode_0.hdf5', 'w') as f:
# 观测
f.create_dataset('observations/images/cam_high',
data=cam_high_array, # (T, H, W, 3) uint8
chunks=(1, H, W, 3), # 按帧分块
compression='gzip')
f.create_dataset('observations/images/cam_left_wrist',
data=cam_lw_array)
f.create_dataset('observations/qpos',
data=joint_positions) # (T, 14) float64
f.create_dataset('observations/qvel',
data=joint_velocities)
# 动作（= 下一时刻的 qpos，或 leader 的 qpos）
f.create_dataset('action',
data=action_array) # (T, 14) float64
# 元数据
f.attrs['sim'] = False
f.attrs['compress'] = True
f.attrs['num_timesteps'] = T
LeRobot 格式（Hugging Face 标准，v3 API）¶
LeRobot（Hugging Face 2024）正在成为机器人学习数据的事实标准。相比 HDF5，它使用 parquet（表格数据）+ mp4（视频）的组合：
# LeRobot 数据结构
# dataset/
# ├── meta/
# │ ├── info.json # 数据集元信息
# │ ├── episodes.jsonl # episode 列表
# │ └── tasks.jsonl # 任务描述
# ├── meta/
# │ └── episodes/ # episode 边界、offset、任务索引等元数据
# ├── data/
# │ └── chunk-000/
# │ ├── file-000.parquet # 多个 episode 聚合到 shard
# │ └── ...
# └── videos/
# └── chunk-000/
# ├── observation.images.cam_high/
# │ ├── file-000.mp4 # 视频同样按 shard 存储
# │ └── ...
# └── observation.images.cam_wrist/
# 写入 LeRobot 格式
from lerobot.datasets.lerobot_dataset import LeRobotDataset
dataset = LeRobotDataset.create(
repo_id="user/my_aloha_dataset",
fps=50,
robot_type="aloha",
features={
"observation.images.cam_high": {
"dtype": "video",
"shape": (480, 640, 3),
},
"observation.state": {
"dtype": "float32",
"shape": (14,), # 双臂 14 关节
},
"action": {
"dtype": "float32",
"shape": (14,),
},
},
)
# 添加每帧数据
for frame in episode_frames:
dataset.add_frame({
"observation.images.cam_high": frame["image"],
"observation.state": frame["joint_positions"],
"action": frame["action"],
})
dataset.save_episode(task="pick_and_place")
dataset.finalize()
LeRobot vs HDF5 对比：
| 维度 | HDF5 (ALOHA) | LeRobot (parquet+mp4) |
|---|---|---|
| 视频存储 | 原始帧逐帧存储（巨大） | mp4 压缩（10-50x 小） |
| 元数据 | HDF5 attrs（非标准） | JSON（标准化、可搜索） |
| 社区共享 | 手动上传下载 | Hugging Face Hub 集成 |
| 跨框架兼容 | ALOHA 专用 | LeRobot/ACT/DP 通用 |
| 版本控制 | 文件级 | 数据集级 |
反事实推理——如果不做时间同步¶
如果图像和关节状态不同步，会发生什么？
假设图像延迟了 50ms（2-3 帧）。训练时模型看到的是 \(t\) 时刻的图像但 \(t+50\text{ms}\) 时刻的关节状态和动作。模型学到的是一个"从过去的视觉预测未来的动作"的策略——这在部署时不成立（部署时图像和关节是同步的）。
更微妙的情况：如果延迟不固定（有时 10ms，有时 50ms），模型学到的映射变得不确定——同样的图像对应不同的动作。这直接导致策略的多模态混淆（multimodal confusion），表现为：机器人在操作时"犹豫不决"、在两个动作之间来回切换。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：HDF5 不使用 chunked storage
错误做法：f.create_dataset('images', data=all_images) # 无 chunks
现象：读取单帧时需要加载整个数据集到内存，100 episodes × 1000 帧 → OOM
根本原因：HDF5 默认连续存储，不分块则无法随机访问
正确做法：f.create_dataset('images', data=..., chunks=(1, H, W, 3))
自检方法：h5py.File('data.hdf5')['images'].chunks 应返回非 None
💡 概念误区：认为"录更多数据总是更好"
新手想法："收集 1000 个 episode 肯定比 100 个好"
实际上：数据质量 > 数据数量。100 个高质量 episode（无失误、流畅、
成功完成任务）可能优于 1000 个参差不齐的 episode。
Mobile ALOHA 的 co-training 论文表明：50 个高质量 human demo +
大量 autonomy data 的效果优于 500 个低质量 human demo。
正确做法：先采集 50 个高质量 demo，训练策略，用策略 roll-out
生成额外数据（DAgger/co-training），再用少量人工修正。
🧠 思维陷阱：认为"数据格式不重要，后续可以转换"
新手想法："先随便存，需要时再转格式"
实际上：格式转换中最容易丢失的是时间戳信息和元数据。
如果原始录制没有精确时间戳，后续无法重建同步关系。
一旦存储，无法恢复丢失的时间信息。
正确做法：从录制第一帧开始就用标准化格式（LeRobot），
确保时间戳、帧率、数据类型在录制时就正确。
练习¶
- [编程] 实现一个完整的 ROS2 数据采集节点：订阅 2 个相机话题和 1 个关节状态话题，用 ApproximateTimeSynchronizer 同步，存为 LeRobot parquet+mp4 格式。测量同步精度（统计图像时间戳和最近关节状态时间戳的差值分布）。
- [编程] 写一个数据转换脚本：将 ALOHA HDF5 格式转为 LeRobot 格式。注意：HDF5 中图像是原始帧（uint8 数组），LeRobot 需要 mp4 编码。使用
ffmpeg进行视频编码。 - [思考题] UMI 的数据采集不使用 ROS2——它用 GoPro 的 MP4 视频和 ORB-SLAM3 的轨迹。如果要将 UMI 数据和 ALOHA 数据混合训练，关键的挑战是什么？提示：坐标系不同（UMI 是世界坐标系末端位姿，ALOHA 是关节空间），动作空间不同（笛卡尔 vs 关节），相机视角不同（第一人称 vs 第三人称）。
D8.7 数据质量评估与过滤 ⭐⭐⭐¶
动机——垃圾进垃圾出¶
模仿学习的一个核心假设是：训练数据代表了"专家行为"。如果数据中混入了失败的演示、不流畅的操作、或标注错误的 episode，策略会学到这些"坏习惯"。
ALOHA 原文（Zhao 2023）报告：用 50 个高质量 demo 训练的 ACT 策略成功率 80-90%，但如果混入 10 个失败 demo（20% 噪声），成功率下降到 50-60%。数据质量对模仿学习的影响远大于数据数量。
质量指标体系¶
指标一：任务成功率¶
最基本的指标——episode 是否成功完成了目标任务。
# 自动成功判定（基于物体最终位置）
def check_success(final_state, task_name, goal_params):
"""检查 episode 是否成功。
task_name 是字符串，goal_params 是结构化参数字典。
不要把 task_goal 一会儿当字符串、一会儿当 dict。
"""
if task_name == "pick_and_place":
obj_pos = final_state['object_position']
target_pos = goal_params['target_position']
return np.linalg.norm(obj_pos - target_pos) < 0.03 # 3cm 阈值
elif task_name == "handover":
# 物体从左手转移到右手
left_gripper = final_state['left_gripper_force']
right_gripper = final_state['right_gripper_force']
return left_gripper < 0.1 and right_gripper > 1.0
else:
raise ValueError(f"Unknown task_name: {task_name}")
指标二：操作流畅度¶
高质量的演示应该是流畅的——关节速度连续，无突变。
RMS Jerk 越低，操作越流畅。经验阈值（ALOHA 双臂）：
- 优秀：RMS Jerk < 50 rad/s^3
- 可接受：50-200 rad/s^3
- 差（考虑过滤）：> 200 rad/s^3
def compute_rms_jerk(joint_positions, dt=0.02):
"""计算 RMS Jerk"""
# 三阶差分近似三阶导数
vel = np.diff(joint_positions, axis=0) / dt
acc = np.diff(vel, axis=0) / dt
jerk = np.diff(acc, axis=0) / dt
rms_jerk = np.sqrt(np.mean(np.sum(jerk**2, axis=1)))
return rms_jerk
指标三：时间效率¶
同一任务，不同 episode 的完成时间差异反映操作效率。过长的 episode 可能包含犹豫、错误修正，不适合作为"专家演示"。
其中 \(T_{\text{optimal}}\) 是该任务的理想最短时间（由最快的 5% episodes 估算）。Efficiency < 0.3 的 episode 应标记为低质量。
指标四：动作空间覆盖度¶
训练数据应覆盖任务的多种变体（初始物体位置、朝向、大小的多样性）。
def compute_coverage(episodes, task_space_dim=6):
"""
计算数据的任务空间覆盖度
用初始物体位姿的凸包体积表示
"""
from scipy.spatial import ConvexHull
initial_configs = []
for ep in episodes:
obj_pose = ep['initial_object_pose'] # 6D
initial_configs.append(obj_pose)
initial_configs = np.array(initial_configs)
# PCA 降维后计算凸包
from sklearn.decomposition import PCA
pca = PCA(n_components=min(3, task_space_dim))
reduced = pca.fit_transform(initial_configs)
try:
hull = ConvexHull(reduced)
return hull.volume
except:
return 0.0 # 数据太少无法计算凸包
数据过滤流水线¶
原始 episodes (N 个)
│
▼ Step 1: 成功率过滤
┌───────────────────┐
│ 自动判断或人工标注 │ → 过滤掉失败 episodes
│ success/fail │
└────────┬──────────┘
│ 通过: ~80% (假设操作者熟练)
▼ Step 2: 流畅度过滤
┌───────────────────┐
│ RMS Jerk < 阈值 │ → 过滤掉抖动/犹豫的 episodes
│ 无关节限位碰撞 │
└────────┬──────────┘
│ 通过: ~90% (假设有经验)
▼ Step 3: 效率过滤
┌───────────────────┐
│ Efficiency > 0.3 │ → 过滤掉过慢的 episodes
│ 长度在 μ±2σ 内 │
└────────┬──────────┘
│ 通过: ~85%
▼ Step 4: 覆盖度检查
┌───────────────────┐
│ 初始条件是否均匀? │ → 如果覆盖不足，指导采集更多
│ 有无盲区? │
└────────┬──────────┘
│
▼
清洁数据集 (~0.8 × 0.9 × 0.85 × N ≈ 0.61N)
数据增强技术¶
当高质量数据不足时，可以通过数据增强扩充数据集：
| 增强方法 | 操作 | 适用场景 | 注意事项 |
|---|---|---|---|
| 图像增强 | 颜色抖动、随机裁剪、高斯噪声 | 视觉策略 | 不改变动作标签 |
| 时间拉伸 | 以 0.8x-1.2x 速度回放轨迹 | 时序策略 | 需要重采样到固定帧率 |
| 镜像 | 左右翻转图像 + 镜像关节角 | 对称任务 | 不适用于非对称任务 |
| action noise | 给动作加小噪声训练 | 增强鲁棒性 | 噪声过大则策略学不稳 |
| DAgger | 策略 roll-out + 人工修正 | 分布外 | 需要人在环 |
与 D04 学习章的数据接口¶
回顾 D04（双臂学习）：ACT 策略的训练需要特定格式的数据。数据接口规范：
# D04 ACT 训练期望的数据格式
# 每个 episode 是一个 dict:
{
'observations': {
'images': {
'cam_high': np.ndarray, # (T, 480, 640, 3) uint8
'cam_left_wrist': np.ndarray, # (T, 480, 640, 3) uint8
'cam_right_wrist': np.ndarray, # (T, 480, 640, 3) uint8
},
'qpos': np.ndarray, # (T, 14) float64 — 双臂 14 关节
'qvel': np.ndarray, # (T, 14) float64
},
'action': np.ndarray, # (T, 14) float64
# action = leader 的 qpos（ALOHA 风格）
# 或 = 下一帧的 qpos（通用风格）
}
关键约定：
- action 定义：ALOHA 风格中，action 是 leader 的关节位置（不是 follower 的）——因为 leader 直接反映人类意图，follower 可能因延迟略有偏差
- 帧率：统一到 50 Hz（ALOHA 原始帧率）。如果原始录制帧率不同，需要重采样
- 归一化：关节角度归一化到 [-1, 1] 区间（除以关节限位），图像归一化到 [0, 1]
⚠️ 常见陷阱¶
⚠️ 编程陷阱：action 和 observation 的时间偏移
错误做法：action[t] = qpos[t]（同一时刻）
现象：策略学到的是"保持当前位置不动"——因为 action 就是当前状态
根本原因：action 应该是"下一时刻的目标位置"，即 action[t] = qpos[t+1]
或者在 ALOHA 中 action[t] = leader_qpos[t]（leader 领先 follower）
正确做法：确认 action 定义与训练代码一致
自检方法：计算 ||action[t] - qpos[t]|| 的均值，应显著大于 0
💡 概念误区：认为"数据采集只是记录"
新手想法："开始录制→操作→停止录制，完事"
实际上：数据采集是一个需要精心设计的工程系统。
时间同步、格式规范、质量控制、元数据管理、版本控制——
每一环都会影响最终策略的质量。
工业实践：大型 Foundation Model 项目（如 RT-X）有专门的
"data engineering team"，人数与 ML 研究团队相当。
练习¶
- [编程] 为你的数据集实现一个自动质量评分系统：对每个 episode 计算任务完成率、轨迹平滑度（jerk 积分）、操作时长，生成质量报告并标记低质量 episode。
- [编程] 实现一个 A/B 测试框架：用完整数据集和过滤后数据集（去除最差 20%）分别训练 ACT 策略，统计成功率差异并用 Fisher 精确检验判断显著性。
- [思考题] 如果操作者水平参差不齐（新手 vs 专家），你会如何设计数据采集协议？讨论：是否需要对操作者标注技能等级？是否可以用学习曲线自动分级？
D8.8 前沿工作与开放问题 ⭐⭐⭐⭐¶
GELLO——低成本遥操作的新范式¶
GELLO（Wu, IROS 2024）用 $300 的 3D 打印 leader 替代了 ALOHA 的 $2500 Dynamixel leader。核心创新：
- 被动关节：GELLO 的 leader 没有电机——靠操作者手动反向驱动。优势是成本低，劣势是操作者需要对抗重力（没有重力补偿），长时间操作疲劳
- 通用适配：通过可拆卸末端和可调节连杆长度，一个 GELLO 可以适配 Franka、UR5、xArm7 等多种从端
- 100 Hz 采集：比 ALOHA 的 50 Hz 高一倍，提供更精细的运动轨迹
开放问题：如何低成本地给 GELLO 加力反馈？Dynamixel 电流模式可以估计扭矩（精度 ±0.5 Nm），但作为 leader 的 3D 打印关节摩擦大（±0.3 Nm）——力估计的信噪比很低。
Haptic-VLA——力触觉学习的未来¶
当前 VLA（Vision-Language-Action）模型完全依赖视觉和语言，不使用力/触觉信息。但许多操作任务（如判断水果成熟度、拧螺丝到位、组装卡扣）的关键信息在力和触觉通道。
Haptic-VLA（2025+ 方向）是将力反馈数据纳入 Foundation Model 训练的前沿探索。关键挑战：
- 数据稀缺：力反馈数据在 Open-X 数据集中占比 < 5%
- 模态对齐：如何将力时间序列（1 kHz）与视觉（30 Hz）和语言对齐
- 跨平台：不同力传感器的量程和精度差异巨大
跨 Embodiment Retargeting¶
当前 retargeting 主要是"人→机器人"。但未来需要"机器人 A → 机器人 B"的跨 embodiment transfer——在一个平台上采集的数据，如何在另一个完全不同的平台上使用？
MimicGen（Mandlekar, CoRL 2023） 提出了一种方法：从少量人类 demo 中提取任务级意图（grasp-approach-lift-transport-place），然后在不同机器人上重新生成运动学可行的轨迹。
项目精读清单¶
| 项目 | 精读目标文件 | 学习要点 |
|---|---|---|
| tonyzhaozh/aloha | aloha_scripts/robot_utils.py |
leader-follower 映射的精确实现 |
| wuphilipp/gello_software | gello/agents/gello_agent.py |
GELLO 的关节镜像 + offset 标定 |
| real-stanford/universal_manipulation_interface | umi/.../franka_interpolation_controller.py |
UMI 的 Franka 底层控制 |
| OpenTeleVision/TeleVision | teleop/television.py |
VR → IK 管线 |
| dexsuite/dex-retargeting | dex_retargeting/optimizer.py |
三种 retargeting 优化器 |
| jhu-dvrk/sawIntuitiveResearchKit | mtsTeleOperationPSM.cpp |
da Vinci 的遥操作控制 |
本章小结¶
| 知识点 | 核心内容 | 难度 | 关联章节 |
|---|---|---|---|
| 运动缩放 | 先定义端口方向；在本文 \(x_s=\lambda_p x_m, f_m=\lambda_f f_s\) 下，\(\lambda_f=\lambda_p\) 才功率守恒 | ⭐⭐ | D05-D06 无源性 |
| 关节映射 vs 笛卡尔映射 | 同构→关节，异构→笛卡尔 IK | ⭐⭐ | M03 IK |
| Retargeting | 位置/向量/DexPilot 三方法，优化式重定向 | ⭐⭐⭐ | M16 灵巧手 |
| Clutching | 工作空间边界处理，PSPM 能量安全 | ⭐⭐ | D07 TDPA |
| 系统架构 | ALOHA/GELLO/UMI/OTV/ACE 对比 | ⭐⭐ | — |
| 数据 Pipeline | ROS2 同步 → HDF5/LeRobot → 过滤 | ⭐⭐ | D04 ACT 训练 |
| 数据质量 | Jerk/效率/覆盖度指标，过滤流水线 | ⭐⭐⭐ | D04 数据接口 |
累积项目：本章新增模块¶
Mini-DualArm 项目进度：
D01-D04: 双臂运动学/规划/力控/学习理论 ✓
D05-D07: 遥操作理论/无源通信/TDPA ✓
D08 新增:
├─ 遥操作数据采集模块
│ ├─ GELLO 式关节映射 (100 Hz)
│ ├─ ROS2 多模态同步录制
│ ├─ LeRobot 格式输出
│ └─ 数据质量评估脚本
└─ [下一步] D09: MoveIt2 双臂集成
附录 D8.A ALOHA 硬件 BOM 表（Bill of Materials）¶
构建一套完整 ALOHA 双臂遥操作系统的硬件清单（基于 Zhao et al. 2023 原始设计，2024 年社区价格）：
| 类别 | 型号 | 数量 | 单价 (USD) | 小计 (USD) | 说明 |
|---|---|---|---|---|---|
| Follower 臂 | ViperX 300 6DOF | 2 | $2,650 | $5,300 | Trossen Robotics，含夹爪 |
| Leader 臂 | WidowX 250 6DOF | 2 | $1,250 | $2,500 | 操作者端，力矩反馈 |
| USB Hub | Powered 4-port USB3 | 1 | $30 | $30 | 独立供电避免掉电 |
| 相机（上方） | Logitech C920 | 1 | $70 | $70 | 1080p 30fps |
| 相机（腕部） | Logitech C922 | 2 | $80 | $160 | 左右腕各一，第一人称视角 |
| 桌面支架 | 80/20 铝型材 + 3D 打印 | — | $200 | $200 | 固定两对臂 |
| 电源 | 12V 10A × 4 | 4 | $25 | $100 | 每臂独立供电 |
| 计算平台 | RTX 3090 工作站 | 1 | $2,500 | $2,500 | 训练 + 推理 |
| 线材/配件 | USB/电源线/紧固件 | — | $50 | $50 | — |
| GELLO 替代 | 3D 打印关节 + 编码器 | 2 | $150 | $300 | 可选：替换 WidowX leader |
| 总计 | $11,210 | GELLO 方案可降至 $8,410 |
降本路线：
| 方案 | 变更 | 总成本 | 降幅 |
|---|---|---|---|
| 原版 ALOHA | WidowX leader | $11,210 | — |
| GELLO leader | 3D 打印 + AS5600 编码器 | $8,410 | -25% |
| 仿真版 (MuJoCo) | 无物理硬件 | $2,500 | -78%（仅 GPU 工作站） |
| ALOHA 2 (Google) | 定制 follower + ARX5 | ~$20,000 | +78%（更高精度） |
延伸阅读¶
| 资源 | 难度 | 说明 |
|---|---|---|
| Zhao et al. (2023) "Learning Fine-Grained Bimanual Manipulation with Low-Cost Hardware" (ALOHA/ACT) | ⭐⭐ | 遥操作系统设计 + 模仿学习的完美结合 |
| Wu et al. (2024) "GELLO: A General, Low-Cost, and Intuitive Teleoperation Framework" | ⭐⭐ | 低成本遥操作的系统设计 |
| Chi et al. (2024) "Universal Manipulation Interface" | ⭐⭐⭐ | 无机器人数据采集的范式突破 |
| Qin et al. (2023) "AnyTeleop: A General Vision-Based Dexterous Robot Teleop System" | ⭐⭐⭐ | 向量映射 retargeting 的理论 |
| Handa et al. (2020) "DexPilot: Vision-Based Teleoperation of Dexterous Robotic Hand-Arm System" | ⭐⭐⭐ | Pinch prior 和多指重定向 |
| Siciliano & Khatib (2016) "Springer Handbook of Robotics", Ch. 43 Telerobotics | ⭐⭐⭐⭐ | 遥操作的理论全景 |
| Niemeyer et al. (2016) "Telerobotics" in Springer Handbook | ⭐⭐⭐⭐ | 遥操作的系统视角 |
🔧 故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 从端不跟随主端 | USB 断连 / clutch 卡住 | 1.检查 USB 连接 2.检查 clutch 状态 3.重启控制循环 | D8.4 |
| 从端运动方向反向 | 坐标系不匹配 | 1.检查 T_offset 变换 2.逐轴测试 X/Y/Z 方向 3.修正旋转矩阵 | D8.2 |
| IK 求解失败 | 目标超出工作空间 | 1.打印 T_des 确认合理 2.检查缩放因子 3.增加 max_iter | D8.2, M03 |
| 录制数据时间不同步 | sync slop 设置过小 | 1.打印时间戳差值 2.增大 slop 到 20ms 3.检查话题发布频率 | D8.6 |
| 训练策略成功率低 | 数据质量问题 | 1.检查 action 定义 2.计算 RMS Jerk 3.过滤低质量 episodes 4.检查归一化 | D8.7, D04 |