本文档属于 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 双臂学习 |
本章知识导航¶
D8.1 运动映射基础 ⭐
│ └─ Master-Slave 尺度映射 / 工作空间对齐
│
▼
D8.2 关节空间映射 vs 任务空间映射 ⭐⭐
│ ├─ 关节角直接映射（同构型）
│ └─ 末端位姿映射（异构型）
│
▼
D8.3 工作空间缩放与偏移 ⭐⭐
│ └─ 尺度因子 / 索引控制 / 重定向
│
▼
D8.4 遥操作数据采集管线 ⭐⭐
│ ├─ 硬件搭建
│ ├─ 数据格式（HDF5/rosbag2）
│ └─ 质量控制
│
├─→ D8.5 ALOHA 数据采集实战 ⭐⭐
├─→ D8.6 VR/手套映射 ⭐⭐⭐
└─→ D8.7 前沿展望 ⭐⭐⭐⭐
前置知识桥接¶
回顾 D05-D07（遥操作理论）：D05 建立了二端口分析框架，D06-D07 解决了延迟下的稳定性问题。本章聚焦遥操作系统的另一半——运动映射：如何将操作者的手部运动映射到远端机器人的末端运动？这不仅是遥操作的核心实现问题，也是**学习型双臂控制（D04 的 ACT/Diffusion Policy）数据采集的关键管线**。
回顾 D04（双臂学习）：ACT 和 Diffusion Policy 需要大量高质量的双臂操作示范数据。遥操作是采集这些数据的最主要方式。数据质量直接决定学习策略的性能——本章建立数据采集的工程最佳实践。
如果跳过本章会怎样¶
- 运动映射失败：不理解工作空间缩放和对齐，你的遥操作系统可能出现严重的位置偏差或方向错误。
- 数据质量差：不理解数据采集管线的最佳实践，采集的训练数据可能包含大量噪声和不一致——训练出的策略性能低下。
本章目标¶
学完本章后，你应该能够：
- **推导**主从运动缩放的数学模型，理解位置缩放因子 \(\lambda_p\) 和力缩放因子 \(\lambda_f\) 的能量一致条件；在本文端口定义下为 \(\lambda_f = \lambda_p\)
- **区分**关节空间映射和笛卡尔空间映射的适用场景，能根据主从构型差异正确选型
- **实现**优化式 retargeting（从人手关键点到机器人灵巧手），理解 DexPilot 的 pinch prior 设计
- 分析 ALOHA、GELLO、UMI、OpenTeleVision、ACE 五大遥操作系统的架构差异和工程取舍
- **搭建**完整的数据采集 pipeline（ROS2 录制 → HDF5/LeRobot 格式 → 数据清洗 → 训练数据生成）
- **评估**采集数据的质量，掌握数据过滤指标和质量增强方法
预计阅读时间¶
| 模式 | 时间 | 建议 |
|---|---|---|
| 精读（含推导和实践） | 10-14 小时 | 完整阅读，手推关键公式，运行代码示例 |
| 速读（抓核心概念） | 3-5 小时 | 重点读核心理论节，跳过实现细节 |
| 速查 | 15-30 分钟 | 利用知识导航和术语速查表定位目标 |
| 复习 | 1-2 小时 | 读本章小结和常见误解，做自测题 |
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 \(\times\) 5 fingers + thumb abduction/adduction），27 块骨骼，17 个关节。而机器人灵巧手的自由度从 1（简单夹爪）到 24（Shadow Hand）不等，运动学结构通常与人手差异巨大。
考虑一个具体场景：你戴着 MediaPipe 手部追踪设备（25 个关键点），要控制一个 Allegro Hand（16 DOF，4 finger \(\times\) 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 度）——数值上接近但不完全匹配。更关键的是，人手指节的长度比例（近节:中节:远节 \(\approx\) 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 \(\times\) 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 \(\times\) 2 = \(600，加 Dynamixel 电流模式力估计（通过电机电流反推扭矩），再加一个简单的波变量双边控制。分析：(a) Dynamixel 电流估扭矩精度（\)\pm$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 秒演示 \(\times\) 4 相机 \(\times\) 640\(\times\)480 \(\times\) 30 fps \(\approx\) 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 | \(\pm\)5-15 ms | 大多数场景够用 |
| 硬件同步 | 相机外触发 + 关节读取绑定同一时钟 | \(\pm\)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 电流模式可以估计扭矩（精度 \(\pm\)0.5 Nm），但作为 leader 的 3D 打印关节摩擦大（\(\pm\)0.3 Nm）——力估计的信噪比很低。
Haptic-VLA——力触觉学习的未来¶
当前 VLA（Vision-Language-Action）模型完全依赖视觉和语言，不使用力/触觉信息。但许多操作任务（如判断水果成熟度、拧螺丝到位、组装卡扣）的关键信息在力和触觉通道。
Haptic-VLA（2025+ 方向）是将力反馈数据纳入 Foundation Model 训练的前沿探索。关键挑战：
- 数据稀缺：力反馈数据在 Open-X 数据集中占比 < 5%
- 模态对齐：如何将力时间序列（1 kHz）与视觉（30 Hz）和语言对齐
- 跨平台：不同力传感器的量程和精度差异巨大
VR 遥操作数据采集 Pipeline——Meta Quest / Apple Vision Pro ⭐⭐⭐¶
VR 头显（Meta Quest 3, Apple Vision Pro）正在成为低成本遥操作数据采集的新范式，与 ALOHA 的"同构臂 leader"方案互补。核心差异在于：ALOHA 用物理 leader 臂提供运动学约束和本体感觉，VR 用手部追踪和视觉沉浸提供直觉操作体验。
OpenTeleVision (Cheng et al., IROS 2024) 建立了从 VR 到机器人的完整数据采集管线：
VR 遥操作数据采集管线
══════════════════════════════════════════
Meta Quest 3 工作站 机器人
┌──────────┐ ┌──────────┐ ┌──────────┐
│ 手部追踪 │──WiFi6──→│ Retarget │──ROS2──→│ 双臂执行 │
│ 52关节/手 │ 90 Hz │ IK求解 │ 500Hz │ Franka×2 │
│ 头部姿态 │ │ 碰撞检测 │ │ │
└──────────┘ │ 数据录制 │ └──────────┘
↑ └──────────┘ │
│ │ │
立体视频流 ←── WebRTC ── 双目相机 ←── 安装在机器人头部
(低延迟 <50ms)
Apple Vision Pro 的独特优势：手部追踪精度（<1mm，苹果官方数据）远超 Quest 3（~3mm），且支持眼动追踪——操作者的注视方向可以编码为任务意图，用于 D04 的 VLA 训练数据中。主要限制是成本（$3500 vs $500）和开发者生态（visionOS 相比 Android 更封闭）。
数据格式接口：VR 采集的原始数据是手部 52 关节角度序列（MANO 模型），需要经过 D8.3 的 retargeting 转换为机器人 action space。从 VR 到 D04 ACT 训练的数据流为：VR 手部关节 → retargeting → 机器人关节位置 → canonical schema → ACT dataloader。
Whole-body Teleoperation——从双臂到全身 ⭐⭐⭐⭐¶
当机器人从"桌面双臂"扩展到"移动双臂"甚至"人形机器人"时，遥操作的维度从 14D（双臂关节）跃升到 30-50D（全身关节）。传统的 leader 臂方案无法覆盖腰部、腿部、头部的控制——需要全身运动捕捉。
Cheng et al. (CoRL 2024, "Expressive Humanoid Whole-Body Control") 提出的方案将人体运动（IMU 套装 / RGB 相机 pose estimation）映射到人形机器人全身：上半身用 D8.3 的 retargeting 方法，下半身用步态控制器（D05 足式方向）自主行走，腰部旋转通过 IK 插值。
跨领域类比：Whole-body Teleoperation 与动作捕捉电影制作有相似之处——演员穿戴 mocap 套装，动作实时映射到 CG 角色。区别在于 CG 角色不受物理约束（可以穿过墙壁），而机器人必须满足动力学、碰撞和关节极限约束。因此 D8.2 的 retargeting 优化中必须加入物理可行性约束——这是 CG 动作捕捉不需要考虑的。
跨 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 的遥操作控制 |
本章常见误解汇总¶
| 误解 | 正确理解 |
|---|---|
| 运动映射只是坐标变换 | 运动映射需要处理尺度差异、工作空间不匹配、奇异构型避免等复杂问题 |
| 关节空间映射总是可行的 | 只有 master 和 slave 同构型时才可行；异构型必须用任务空间映射 |
| 索引控制（clutching）无副作用 | clutch 释放瞬间的位置跳变可能导致急动（jerk），需要平滑过渡 |
| 数据量越大学习效果越好 | 数据质量比数量更重要——100 个高质量 episode 可能优于 1000 个噪声 episode |
| ALOHA 的数据格式可以随意设计 | 统一的数据格式（HDF5 + 标准化 key）对下游训练至关重要 |
| VR 手套映射比物理 master 更好 | VR 手套缺乏力反馈，精细操作的数据质量通常不如物理 master |
| 遥操作采集无需标定 | master-slave 之间的运动学标定直接影响映射精度和数据质量 |
| 采样率越高越好 | 高于控制频率的采样只增加存储成本，不增加信息量 |
本章常见误解汇总¶
| 误解 | 正确理解 |
|---|---|
| 理论模型可以完美描述实际系统 | 实际系统存在建模误差、传感器噪声、通信延迟等非理想因素 |
| 参数越大/越小越好 | 参数设计是多目标权衡，需要在性能指标之间寻找平衡 |
| 仿真验证通过即可部署 | 仿真与实物存在 Sim2Real gap，需要在实物上再次验证和调参 |
| 经典方法已被学习方法取代 | 经典方法提供安全性和稳定性保证，学习方法提供自适应能力，两者互补 |
| 高频控制总是更好 | 高频控制增加计算负担，且传感器噪声在高频被放大 |
| 线性分析工具可以完全预测非线性行为 | 线性分析提供局部近似和设计指导，但非线性效应需要额外验证 |
本章小结¶
| 知识点 | 核心内容 | 难度 | 关联章节 |
|---|---|---|---|
| 运动缩放 | 先定义端口方向；在本文 \(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 数据接口 |
数据采集的质量控制 ⭐⭐¶
高质量的遥操作数据是学习型双臂控制成功的基础。以下是数据质量控制的系统化方法：
采集前检查清单：
□ Master-Slave 运动学标定验证（误差 < 1mm）
□ 通信延迟测试（延迟 < 20ms 且 jitter < 5ms）
□ 力传感器标定（如有）
□ 相机内外参标定
□ 工作空间对齐验证（操作者视角 vs 机器人坐标系）
□ 紧急停止功能测试
□ 数据格式和存储路径确认
□ 操作者培训（至少 30 分钟练习）
采集中质量指标：
| 指标 | 合格阈值 | 超标处理 |
|---|---|---|
| 位置跟踪误差 RMS | < 5mm | 检查映射参数，重新标定 |
| 力反馈延迟 | < 50ms | 检查通信链路，降低采样率 |
| 数据丢帧率 | < 0.1% | 检查带宽，使用 QoS 可靠传输 |
| 操作者主观评分 | \(\ge\) 3/5 | 休息后重试，或调整映射参数 |
| 任务完成率 | \(\ge\) 80% | 简化任务，增加练习时间 |
| 单 episode 时间方差 | CV < 30% | 排除异常值，确保操作一致性 |
采集后数据清洗：
# 数据清洗管线示例
import h5py
import numpy as np
def validate_episode(filepath):
with h5py.File(filepath, 'r') as f:
# 1. 检查数据完整性
required_keys = ['observations/qpos', 'observations/qvel',
'action', 'observations/images/cam_high']
for key in required_keys:
assert key in f, f"Missing key: {key}"
# 2. 检查时间戳单调递增
timestamps = f['timestamps'][:]
assert np.all(np.diff(timestamps) > 0), "Timestamps not monotonic"
# 3. 检查关节角在合理范围内
qpos = f['observations/qpos'][:]
assert np.all(np.abs(qpos) < 2*np.pi), "Joint angles out of range"
# 4. 检查动作维度一致
action = f['action'][:]
assert action.shape[1] == 14, f"Action dim {action.shape[1]} != 14"
# 5. 检查图像完整性
images = f['observations/images/cam_high'][:]
assert images.shape[0] == qpos.shape[0], "Image count mismatch"
# 6. 计算质量分数
smoothness = np.mean(np.abs(np.diff(qpos, axis=0)))
duration = timestamps[-1] - timestamps[0]
return {
'valid': True,
'duration': duration,
'smoothness': smoothness,
'n_frames': len(timestamps)
}
ALOHA 数据采集的最佳实践 ⭐⭐¶
ALOHA 平台因其低成本和开源特性成为双臂学习研究的标准数据采集平台。以下是经过验证的最佳实践：
硬件配置：
Master 端（2x ViperX 250 leader）：
- 舵机设为"电流模式"（可被操作者移动）
- 关节编码器 12-bit（分辨率 0.088°）
- 采样率 50 Hz
Slave 端（2x ViperX 250 follower）：
- 舵机设为"位置模式"
- 跟踪 Master 的关节角（直接映射）
- 控制频率 50 Hz
相机配置：
- 高位俯视相机：640x480 @ 30 Hz
- 手腕相机（可选）：320x240 @ 30 Hz
- 侧视相机（可选）：640x480 @ 30 Hz
数据格式标准（ACT/Diffusion Policy 兼容）：
episode_0.hdf5
├── action # (T, 14) float32，14 个关节目标角
├── observations/
│ ├── qpos # (T, 14) float32，当前关节角
│ ├── qvel # (T, 14) float32，当前关节速度
│ └── images/
│ ├── cam_high # (T, 480, 640, 3) uint8
│ ├── cam_left_wrist # (T, 240, 320, 3) uint8（可选）
│ └── cam_right_wrist # (T, 240, 320, 3) uint8（可选）
└── timestamps # (T,) float64，Unix 时间戳
关键注意事项：
- 同步：所有传感器数据必须时间同步——ALOHA 的 50 Hz 采样是瓶颈，相机帧需要对齐到关节采样时刻
- 数据量：ACT 论文使用 50-100 个 episode 训练简单任务，复杂任务需要 200-500 个
- 操作者一致性：同一任务应由同一操作者完成，避免不同操作风格混合
- 变异性：每个 episode 的初始物体位姿应适度随机化，增加策略泛化能力
- 负样本：不要只保留成功的 episode——适量的失败 episode 有助于策略学习避免失败
反事实推理：如果数据采集不做质量控制，训练出的策略会怎样？实际经验表明：（1）丢帧率 > 1% 时，ACT 的行为克隆性能下降约 15-20%；（2）位置跟踪误差 > 10mm 时，策略学到的是"错误的"示范——执行时偏差被放大；（3）操作者不一致（不同人采集同一任务）时，策略可能学到"平均行为"而非任何一个操作者的行为——导致模棱两可的动作。
累积项目：本章新增模块¶
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 \(\times\) 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%（更高精度） |
本章与后续章节的关系¶
| 后续章节 | 与 D08 的关系 | 从 D08 带走的关键知识 |
|---|---|---|
| D04 双臂学习 | D08 的数据管线 \(\to\) D04 的训练数据 | HDF5 格式、数据质量指标 |
| D09 系统集成 | D08 的映射算法 \(\to\) D09 的 ROS2 节点 | 映射参数配置、话题设计 |
延伸阅读¶
| 资源 | 难度 | 说明 |
|---|---|---|
| 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 | ⭐⭐⭐⭐ | 遥操作的系统视角 |
运动映射的工作空间对齐算法 ⭐⭐¶
Master 和 Slave 的工作空间通常不同——需要对齐算法使操作直觉化。
方法 1：固定偏移对齐
最简单的方法：master 的 home 位置映射到 slave 的 home 位置，运动通过尺度因子 \(\alpha\) 缩放。
方法 2：自适应工作空间映射
当 master 工作空间 < slave 工作空间时，使用非线性映射在 master 中心区域提供 1:1 映射（高精度），在边缘区域压缩映射（扩大可达范围）：
方法 3：视角对齐
最符合直觉的映射是让操作者的运动方向与显示器上看到的方向一致。如果 slave 相对于相机旋转了角度 \(\theta\)，映射需要包含旋转补偿：
工程教训：视角对齐是操作者体验最关键的因素。当操作者向右推 master 但看到 slave 向左移动时（因为相机角度），操作者会本能地"反向补偿"——导致混乱和低效。这种方向不匹配是遥操作中最常见的"可用性 bug"。
遥操作数据与模仿学习的接口 ⭐⭐¶
遥操作采集的数据直接输入到 D04 的模仿学习管线。关键接口要求：
| 模仿学习方法 | 数据格式要求 | 关键字段 | 典型数据量 |
|---|---|---|---|
| ACT (Zhao 2023) | HDF5，50 Hz | qpos, action, images | 50-200 episodes |
| Diffusion Policy (Chi 2023) | HDF5/zarr，50 Hz | obs, action, images | 100-500 episodes |
| RT-2 / Octo | RLDS (TFRecord) | observation, action | 1000+ episodes |
| DROID (Khazatsky 2024) | HDF5，15 Hz | 多模态融合 | 76000+ episodes |
研究实践建议¶
| 层次 | 建议 | 适用读者 |
|---|---|---|
| 入门实践 | 搭建 ALOHA 遥操作系统，采集 20 个简单任务（拾取放置）episode | 硕一新生 |
| 中级实践 | 实现完整的数据采集管线（标定+映射+采集+质量检查），采集复杂任务数据 | 硕士研究生 |
| 高级实践 | 对比不同映射方法（关节/任务空间/VR）对下游学习策略性能的影响 | 博士研究生 |
遥操作数据采集的常见问题与解决方案 ⭐¶
| 问题 | 症状 | 原因 | 解决方案 |
|---|---|---|---|
| Master-Slave 方向相反 | 操作者向右推但 slave 向左 | 坐标系不匹配 | 检查映射矩阵的旋转部分 |
| 运动范围不匹配 | Slave 到达极限但 master 还有余量 | 尺度因子错误 | 重新测量两侧工作空间 |
| 数据时间戳不连续 | HDF5 中 timestamps 有跳变 | 通信丢包 | 检查 QoS，降低采样率 |
| 图像与关节数据不同步 | 图像帧与动作不对应 | 异步采集 | 使用硬件同步或软件时间戳对齐 |
| Episode 长度差异大 | 同一任务有的 3s 有的 30s | 操作者策略不一致 | 增加练习，设置时间窗口 |
| 夹爪状态不稳定 | 夹爪抖动开合 | 编码器噪声 | 加死区滤波（dead zone） |
| 采集疲劳 | 后期 episode 质量下降 | 操作者疲劳 | 每 20 个 episode 休息 5 分钟 |
| 入门级 | 复现本章核心算法的最简版本，在仿真中验证正确性 | 本科高年级/硕一新生 | | 进阶级 | 在真实平台或高保真仿真器（MuJoCo/Isaac）中实现完整系统 | 硕士研究生 | | 研究级 | 针对本章理论的局限性提出改进方案，发表会议论文 | 博士研究生 | | 前沿级 | 将本章方法与学习/基础模型结合，构建下一代系统 | 博士高年级/博后 |
🔧 故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 从端不跟随主端 | 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 |
术语速查表¶
本章核心术语的中英对照，按首次出现顺序排列。详细定义见正文对应小节。
VR 和手套映射的工程考量 ⭐⭐¶
除了物理 master（如 ALOHA 的 leader 臂），VR 头显和数据手套也是双臂遥操作的输入设备。每种设备有不同的映射挑战：
VR 控制器（Quest 3, Vive）：
输入：6D 手部位姿（位置+姿态）@ 72-120 Hz
输出：双臂末端位姿目标
映射挑战：
- 工作空间尺度不同（VR 空间 vs 机器人空间）
- 无力反馈（操作者不知道何时接触物体）
- 手部追踪精度有限（~2mm RMS）
- 遮挡导致追踪丢失
解决方案：
- 尺度因子 α = 机器人工作空间 / VR 工作空间
- 视觉力反馈替代（颜色变化/振动提示）
- 追踪丢失时锁定最后有效位姿
- 索引控制（clutch）处理工作空间不匹配
数据手套（Manus, HaptX）：
输入：手指关节角 @ 90-120 Hz
输出：夹爪开合度 + 手臂姿态
映射挑战：
- 人手 DOF（~27）远多于机械夹爪 DOF（1-2）
- 需要从高维手部信号提取低维抓取意图
- 不同人的手部尺寸差异大
解决方案：
- 手指弯曲度 → 夹爪开合度（简单映射）
- 手掌姿态 → 末端姿态（任务空间映射）
- 个性化标定（每位操作者首次使用时标定手部模型）
Apple Vision Pro 双手追踪：
2024 年以来，Apple Vision Pro 提供了无需外部传感器的高精度双手追踪（手部关键点检测 @ 30 Hz），为低成本双臂遥操作开辟了新路径。核心优势是零额外硬件成本（只需 Vision Pro 本身），劣势是追踪精度和帧率不如专用设备。
跨章综合练习¶
-
[综合 D04+D08] 使用 ALOHA 采集 100 个"叠衣服"的双臂 episode，按本章数据质量控制标准检查数据。然后用 D04 的 ACT 训练策略，评估策略性能。分析数据量和数据质量对策略性能的影响。
-
[综合 D05+D08] 在 D08 的遥操作映射中加入 D05 的 PF 架构力反馈。对比有/无力反馈时采集数据的质量差异——特别是接触任务（如插装）中的力控制精度。
-
[调研] 调查 2024-2025 年的双臂数据采集平台和数据集。列出至少 3 个公开的双臂操作数据集，比较它们的规模、任务类型、数据格式和采集设备。
数据采集平台选型指南¶
| 平台 | 映射方式 | 力反馈 | 成本 | 数据质量 | 适用场景 |
|---|---|---|---|---|---|
| ALOHA (双 ViperX) | 关节直接映射 | 无 | ~$20K | 中等 | 学习型控制数据采集 |
| 双 Franka + Omega | 任务空间映射 | 有 | ~$150K | 高 | 力控任务数据采集 |
| VR (Quest 3) + 双臂 | 手部位姿映射 | 无 | ~$30K | 低-中 | 大范围运动任务 |
| 外骨骼 + 双臂 | 关节直接映射 | 有 | ~$200K+ | 高 | 人体运动学研究 |
| 数据手套 + 灵巧手 | 手指关节映射 | 部分 | ~$50K | 中等 | 灵巧操作数据 |
选择决策： - 预算有限 + 学习研究 → ALOHA（ACT/Diffusion Policy 生态完善） - 需要力控数据 → 双 Franka + 触觉设备（可获取力/力矩信息） - 大规模数据采集 → VR 映射（操作者不需要培训，可并行多套） - 灵巧操作 → 数据手套 + 灵巧手（捕捉手指精细动作）
工程建议：不要低估操作者培训的成本。ALOHA 的关节直接映射对操作者最友好（运动直觉一致），VR 映射需要 30-60 分钟的适应期，任务空间映射在工作空间边界可能不直觉。选择映射方式时，操作者的学习曲线是重要的考量因素。
版本信息速查¶
| 库 / 工具 | 推荐版本 | 备注 |
|---|---|---|
| Pinocchio | \(\ge\) 2.6.x | 运动学和动力学计算 |
| Eigen | \(\ge\) 3.4 | 矩阵运算和 SVD |
| MuJoCo | \(\ge\) 2.3.x | 物理仿真验证 |
| ROS2 | Humble+ | 通信框架 |
| h5py | >= 3.8 | HDF5 数据读写（Python） | | OpenCV | >= 4.8 | 图像处理和相机标定 | | Dynamixel SDK | >= 3.7.x | ALOHA ViperX 舵机通信 |
| zarr | >= 2.14 | Diffusion Policy 的替代数据格式 | | rosbag2 | Humble+ | ROS2 话题录制和回放 |
D08 到 D04 的数据流：本章建立的数据采集管线直接为 D04 的学习型方法提供训练数据。数据格式的标准化（HDF5 + 统一 key 命名）使得 ACT、Diffusion Policy 等方法可以无缝接入。数据质量控制的每一步（标定、同步、清洗）都直接影响学习策略的最终性能——"garbage in, garbage out" 在模仿学习中尤为突出。
D08 全景回顾：本章覆盖了从运动映射基础到数据采集最佳实践的完整管线。关键要点：（1）映射方式取决于 master/slave 的构型关系；（2）数据质量比数量更重要；（3）标准化的数据格式是下游学习方法的基础。