论文笔记（一百零八）Physics-Driven Data Generation for Contact-Rich Manipulation via Trajectory Optimization

本文提出了一种低成本的数据生成方法，通过结合物理仿真、人类示范和轨迹优化，为接触丰富的机器人操作任务生成大规模高质量数据集。该方法从少量VR环境中采集的人类示范出发，利用运动学重定向和轨迹优化技术适配不同机器人形态和物理参数，生成多样化且物理一致的数据。实验表明，基于该数据集训练的扩散策略可成功应用于多种机器人平台（如浮动Allegro手和双臂机械臂），并在实体机器人上实现零样本部署，仅需极少人工

墨绿色的摆渡人

832人浏览 · 2025-12-11 16:25:11

墨绿色的摆渡人 · 2025-12-11 16:25:11 发布

Physics-Driven Data Generation for Contact-Rich Manipulation via Trajectory Optimization

文章概括
ABSTRACT
I. INTRODUCTION
II. 相关工作
III. DATA COLLECTION
IV. 自动化数据生成
- A. Kinematic Motion Retargeting 运动学运动重定向
- B. Demonstration-Guided Trajectory Optimization 基于示范引导的轨迹优化
V. 轨迹优化实验
VI. 行为克隆实验
- A. 仿真中的策略评估
- B. 硬件上的策略评估
VII. 局限性与未来工作
VIII. 结论
IX. 附录
- A. CEM 实现细节
- B. 策略实现细节

文章概括

引用：

@article{yang2025physics,
  title={Physics-driven data generation for contact-rich manipulation via trajectory optimization},
  author={Yang, Lujie and Suh, HJ and Zhao, Tong and Graesdal, Bernhard Paus and Kelestemur, Tarik and Wang, Jiuguang and Pang, Tao and Tedrake, Russ},
  journal={arXiv preprint arXiv:2502.20382},
  year={2025}
}

Yang, L., Suh, H.J., Zhao, T., Graesdal, B.P., Kelestemur, T., Wang, J., Pang, T. and Tedrake, R., 2025. Physics-driven data generation for contact-rich manipulation via trajectory optimization. arXiv preprint arXiv:2502.20382.

主页： https://lujieyang.github.io/physicsgen/
原文： https://arxiv.org/pdf/2502.20382
代码、数据和视频：

系列文章：
请在 $《$ 文章 $》$ 专栏中查找

宇宙声明！

引用解析部分属于自我理解补充，如有错误可以评论讨论然后改正！

ABSTRACT

我们提出了一条低成本的数据生成流水线，将基于物理的仿真、人类示范以及基于模型的规划整合在一起，从而高效生成面向接触丰富型机器人操作任务的大规模高质量数据集。该流水线首先从虚拟现实仿真环境中采集少量具身形态灵活的人类示范出发，随后通过基于优化的运动学重定向和轨迹优化对这些示范进行改进，使其能够适配多种机器人具身形态和物理参数。这一过程产生了一个多样化、物理一致且接触丰富的数据集，既支持跨具身形态的数据迁移，也为复用在不同硬件配置或物理参数下采集的遗留数据集提供了可能。我们通过在生成的数据集上训练扩散策略，并将其应用于多个机器人具身形态上的具有挑战性的长时域接触丰富操作任务（包括浮动式 Allegro 手和双臂机械臂），来验证该流水线的有效性。训练得到的策略被零样本部署到双臂 iiwa 实体机械臂上，在仅需极少人工参与的情况下仍然取得了较高的任务成功率。项目主页：https://lujieyang.github.io/physicsgen/。

I. INTRODUCTION

基础模型（foundation models）的兴起已经深刻改变了自然语言处理和计算机视觉等领域，在这些领域中，基于海量互联网级数据集训练的模型在多样化的推理任务上展现出显著的泛化能力[1,2,3,4,5]。受这一成功的启发，机器人领域目前正致力于构建面向通用机器人策略的基础模型，使其能够在广泛任务范围内进行灵活且鲁棒的决策[6,7,8]，并由此推动了业界在大规模机器人学习方面进行大量投入[9]。然而，对通用机器人策略的追求仍然受到高质量数据集有限性的制约，尤其是在接触丰富的机器人操作任务方面。现有的数据集[7,10,11,12]的规模，比其他领域（例如大型语言模型，Large Language Models, LLMs）中用于训练基础模型的数据集要小上几个数量级。多样化且高保真操作数据的匮乏，限制了策略在不同具身形态、任务情境与物理条件之间的泛化能力。

为缓解数据稀缺问题，机器人学习研究者往往依赖一系列在成本、质量和可迁移性上各不相同的数据来源。信息量最高的数据通常是针对特定任务、环境和具身形态的高质量示范[7,10]，但这类数据的采集代价高且耗时，因为它需要依赖人类通过特定硬件进行遥操作。在这一谱系的另一端，则存在大量质量较低的数据，例如互联网上展示人类或机器人执行操作任务的视频[13,14,15,16]。然而，显著的具身形态差异以及有限的动作标注，使得这类数据难以高效地迁移为机器人的策略数据。仿真数据则位于中间地带，它有潜力以相对较低的成本生成大规模、多样化且高质量的数据集[17,18,19]。在实践中，通过在这一谱系不同位置来源的数据混合上进行联合训练，可以实现有效的策略学习，从而在降低数据采集成本的同时提升策略的泛化能力[20]。

本文的一个关键洞见是：在人机生成高质量机器人数据的过程中，人类示范与基于模型的规划器在若干关键方面具有互补性。虽然人类示范的采集成本较高，但它们为求解复杂任务提供了非常有价值的全局信息。然而，通过遥操作来采集真实世界中接触丰富的操作数据非常具有挑战性，因为这类任务需要精确的多点接触交互，而在现实中受限于硬件延迟、人机具身差异以及对细粒度控制的需求，很难实现这一点[21]。相比之下，轨迹优化已经在为接触丰富任务生成局部最优轨迹方面展现出良好效果[22,23,24]，但通常需要依赖以良好初始猜测形式提供的全局引导。

在本工作中，我们提出了一种数据生成框架，结合了这两类方法的优势：由人类示范提供全局引导，再通过轨迹优化对这些示范进行局部细化，以确保其在动力学上的可行性。我们的方法从在虚拟现实（VR）环境中采集的少量人类示范出发，利用基于模型的轨迹优化，在仿真中生成大规模的、在动力学上可行且接触丰富的轨迹数据集。这些示范为规划器在复杂搜索空间中的求解提供引导，而规划器则保证在不同物理参数与机器人具身形态下的物理一致性与鲁棒性。如图1所示，我们的流水线支持高效的跨具身形态数据迁移，即由一种机器人配置采集的示范可以适配到另一种配置上，并且支持通过域随机化来进一步提升泛化能力与鲁棒性。此外，该流水线还为“重启”和适配在不同硬件或配置下采集的遗留数据集提供了可能，使得旧数据集在新的机器人系统中重新具有利用价值。

在这里插入图片描述图1：基于物理驱动的数据生成概览。得益于轨迹优化，我们的框架仅依靠24条人类示范，就能够在不同的具身形态和物理参数配置下，自动生成数千条在动力学上可行的、接触丰富的轨迹。使用该生成数据集通过模仿学习训练得到的策略，表现出更高的鲁棒性和性能。

我们的主要贡献包括：

1）我们提出了一种基于虚拟现实和物理仿真的、直观且具身形态灵活的示范接口，使得灵巧、接触丰富的操作任务数据能够被快速采集。

2）我们提出了一个可扩展的框架，利用轨迹优化将少量人类示范扩展为大规模、物理一致的数据集，从而实现跨具身形态、初始条件与物理参数的泛化。

3）我们通过在生成的数据集上训练策略，并将其应用于多个机器人平台上的具有挑战性的接触丰富操作任务（包括双臂机械臂和带浮动基座的 Allegro 手），来验证我们方法的有效性。

4）我们在双臂 iiwa 实体机械臂上实现了高成功率的零样本部署，凸显了经扩增数据集在真实场景中的实用价值。

II. 相关工作

在本节中，我们回顾生成多样化接触丰富机器人数据的最相关方法。我们将这些方法划分为数据采集、数据增强、基于模型的规划、示范引导的强化学习以及跨具身形态迁移五类。

A. Data Collection for Imitation Learning 用于模仿学习的数据采集

行为克隆（Behavior Cloning）[25]通过训练机器人策略去模仿专家行为，并已在广泛的灵巧操作任务中展现出令人瞩目的实验效果[26]。采集高质量的机器人数据一直是模仿学习（Imitation Learning, IL）的关键组成部分。这类方法中有相当一部分依赖人类专家通过遥操作来驱动机器人完成特定任务。研究者使用了诸如三维空间鼠标（3D spacemouse）[26,27]以及“提线木偶式”（puppeteering）平台[28,29]等接口，用于末端执行器控制[7]以及全身控制[30,31]。

近年来，虚拟现实和增强现实（VR/AR）接口逐渐成为机器人数据采集的有效替代方案[32,33]；与传统的“动力示教”（kinesthetic teaching）或三维鼠标控制等技术相比，它们能够降低操作者的认知负担、身体负担以及挫败感[34]。这些技术为复杂任务，尤其是灵巧操作，提供了一种更加直观的数据采集范式。 AR2-D2[35]通过将虚拟机器人投射到物理工作空间中，从而无需实体机器人即可完成数据采集，但其缺乏精确控制所必需的实时反馈。 DART[36]支持在纯仿真环境中进行数据采集，并通过VR头显进行可视化，但在将策略部署到真实机器人时面临缩小仿真到现实（sim-to-real）差距的挑战。 ARCap[37]集成了实时AR反馈，但除AR头显外，还需要RGBD相机、动捕手套以及VR控制器等专用硬件。 ARMADA[38]通过实时虚拟机器人反馈，使操作者能够徒手采集真实世界的操作数据，并在将这些数据回放到实体机器人硬件上时获得了较高的成功率。与上述系统相比，我们的工作重点在于：利用轨迹优化，从少量人类示范中进行可扩展的数据生成，从而促进在不同机器人具身形态、初始条件和物理参数之间的泛化。

B. Data Augmentation 数据增强

尽管已有大量研究工作，但采集大规模数据集依然耗时且昂贵，需要投入大量人力和资源。为应对这些挑战，研究者投入了大量精力，通过数据增强技术来自动化数据生成过程。现有方法已经利用最先进的生成模型来进行视觉增强[39,40,41]和语义增强[42,43,44]。 MimicGen[45]及其双臂扩展版本 DexMimicGen[46]可以从少量人类示范中自动合成大规模数据集。这些工作将长时域任务分解为以物体为中心的子任务，并在仿真中以开环方式回放经过变换的示范。 SkillMimicGen[47]在这一范式基础上进一步发展，将任务划分为“运动”和“技能”两个组成部分，通过 MimicGen 式回放来增强局部操作技能，并使用运动规划将这些技能片段连接起来。 RoboCasa[48]利用生成模型构造多样化的厨房场景及丰富的三维资产，并结合 MimicGen 进行自动化轨迹生成。尽管这些方法在自动化数据生成方面取得了成功，但它们主要依赖对示范的运动学回放，这往往不足以应对接触丰富的操作任务。我们的工作可以被视为对 MimicGen 系列方法的重要扩展，使其支持在动力学上可行的接触丰富数据生成；这类数据生成需要机器人进行细粒度控制，并持续推理与环境之间的接触建立与断开过程。

C. Trajectory Optimization for Contact-Rich Tasks 面向接触丰富任务的轨迹优化

对于基于学习和基于模型的方法而言，通过接触进行规划与控制仍然是一个重大挑战，其原因在于接触模式数量的爆炸式增长以及接触动力学固有的非光滑特性。为应对这些挑战，研究者提出了多种针对多重接触交互的轨迹优化形式。

接触隐式轨迹优化（Contact-Implicit Trajectory Optimization） 方面，现有基于CITO的方法[23,22]尝试通过使用互补约束，将本质上具有组合性质的问题转化为平滑优化问题。 CITO 已被应用于多个领域，包括平面操作[49,50]、动态推运[51]以及各类运动任务[52,53,54]。最近的工作将CITO扩展为用于实时应用的模型预测控制（Model Predictive Control, MPC）[55,56]，并借助专门设计的求解器成功部署在四足机器人硬件上[57,58]。 Aydinoglu 等人[59]利用交替方向乘子法（Alternating Direction Method of Multipliers, ADMM）并行求解线性互补问题，并在多接触操作任务的硬件实验中验证了该方法。尽管CITO在处理接触模式方面展现出良好的可扩展性，但其全局探索能力有限，并且高度依赖较好的初始猜测[60]。新近的一些工作尝试通过高效的全局优化[61]来缓解这些问题，但尚未能够扩展到我们本文所考虑的任务规模。

采样式规划（Sampling-Based Planning）方面，采样式方法在求解接触丰富任务的轨迹优化问题上同样展现出巨大潜力。 Hamäläinen 等人[62]采用基于采样的置信传播方法来解决类人机器人平衡、杂耍和行走任务。 Carius 等人[63]将路径积分（path integral）形式扩展到含有状态–输入约束的情形，并在四足机器人稳定控制的硬件实验中验证了该方法。最新的工作中，Pezzato 等人[64]将采样式预测控制（Sampling-based Predictive Control, SPC）应用于相对简单的接触任务，如推运；而 Howell 等人[65]和 Li 等人[66]则将SPC扩展到更复杂、接触更丰富的任务，例如手内立方体重定向。 Pang 等人[67]采用平滑化的接触动力学结合全局采样，在不到一分钟的时间内生成接触丰富的运动规划，其性能可与强化学习相媲美。 Cheng 等人提出了 HiDex[68]，这是一种将蒙特卡洛树搜索与集成接触投影相结合的分层规划器，在灵巧操作任务中实现了快速规划。有趣的是，直接使用采样式规划器为行为克隆生成接触丰富的数据可能会带来问题，因为其生成轨迹的高熵往往会降低下游策略的性能[69,70]。

在本工作中，我们利用低熵的人类示范来引导多接触交互的全局规划，并通过轨迹优化在局部针对特定物理参数和机器人具身形态对轨迹进行细化。基于少量示范，基于模型的规划器即可高效生成大量高质量的接触丰富数据，用于训练鲁棒的机器人策略。

D. Demonstration-Guided Reinforcement Learning 示范引导的强化学习

尽管模仿学习通常需要大量专家示范才能获得鲁棒且高性能的策略，强化学习（Reinforcement Learning, RL）则试图通过基于奖励的自主探索来完成任务。然而，纯粹的强化学习往往存在探索效率低下以及需要大量奖励塑形的问题，特别是在复杂操作任务中更为突出[71,72]。为应对这些挑战，研究者尝试使用示范来引导强化学习，从而同时提升样本效率和探索质量。

示范可以通过多种方式集成到强化学习流程中，例如将其直接加入重放缓冲区[73,74]，使用行为克隆对策略进行预训练[75,76,77]，以及利用从示范中提取的信息对任务奖励进行增强[78,79,80]等。 Sleiman 等人[81]使用由基于模型的轨迹优化器生成的示范来引导多接触行走–操作任务中的强化学习，并在四足移动操作机器人硬件上验证了其方法。虽然这些方法主要在神经网络策略参数空间中进行搜索，并有望优化更全局的目标函数，但我们则将轨迹优化作为一种互补工具，在局部对示范轨迹进行细化与扩展。这种做法既能高效生成接触丰富的数据，又能避免强化学习在高维搜索空间中带来的计算开销、近似误差以及不必要的探索。

E. Cross-Embodiment Generalization 跨具身形态泛化

在不同具身形态之间复用数据集和策略，为大规模机器人学习释放了巨大潜力。一类工作从人类与环境交互的视频中学习潜在计划（latent plans），并将这些知识迁移到机器人操作中[82,83]。另一类方法使用便携式数据采集工具，例如手持夹爪[21,84]，在自然环境中采集人类示范。虽然这些方法可以将策略部署到多个机器人平台上，但通常被限制在与数据采集时相同末端执行器的机器人上，从而限制了跨平台泛化能力。另一方面，为了利用大规模数据集，近期工作从多种异构机器人（从导航到操作任务）中汇集数据，并训练能够完成多样任务的机器人基础模型[85,86]。我们提出的框架能够让同一组易于采集的示范在多种机器人上复用，从而避免为接触丰富任务单独采集具身形态特定的数据。

III. DATA COLLECTION

我们提出了一条基于虚拟现实（Virtual Reality, VR）的数据采集流水线，用于在多种机器人具身形态上直观且高效地采集人类示范。该流水线强调系统的简洁性和跨具身形态的泛化能力，同时最大限度地降低对实体机器人硬件的依赖。尽管我们将这条数据采集流水线视为本文的一项贡献，但我们特别强调，下一节将介绍的基于仿真的大规模数据生成方法并不依赖于这种特定的数据采集方式。

我们的数据采集流水线（见图2）是一个基于VR的人手示范接口。我们使用 Apple Vision Pro 来追踪人类示范者双手的位姿，并将这些位姿流式传输至 Drake 物理仿真器[87]，由其模拟物体与手之间的接触交互。随后，更新后的物体位姿会被发送回 Apple Vision Pro，并通过 Vuer[88] 在VR环境中进行实时可视化。

在这里插入图片描述

我们的示范接口具备速度快、成本低的特点。由于系统完全在仿真中运行，它消除了对机器人硬件的依赖，从而显著降低了数据采集的成本与复杂度。在实际使用中，对于每一种所考虑的系统，采集24条长时域示范大约只需7分钟。该设置同样易于上手，因为人类示范者无需在心理上弥合自身身体与特定机器人之间的具身差异。

我们在两类不同的机器人具身形态上展示了这条流水线的使用：一是灵巧机械手，二是双臂操作系统。

Floating Allegro Hand 浮动式 Allegro 手 对于灵巧机械手场景，我们考虑的是一只具有22个自由度的自由浮动 Allegro 手在桌面上操作一个立方体，如图3所示。由于 Allegro 手只有四根手指，我们将基于VR的示范限制为使用人类右手的四根手指在仿真中与物体交互。

在这里插入图片描述

Bimanual Robot Arms 双臂机械臂 对于双臂操作设置，我们考虑两类不同的定基双臂操作机械臂：一对具有7个自由度的 Kuka LBR iiwa 手臂，以及一对 Franka Emika Panda 手臂。每一对手臂协同操作一个大型箱体（见图3）。在VR示范过程中，人类示范者使用双手的食指在VR中操作一个小立方体，并限制手腕运动，以模拟定基机械臂的约束。在运动学运动重定向过程中（详见第IV-A节），该小立方体和手指会被按比例缩放，以匹配大型箱体和机器人操作臂的尺寸。

这一设计促进了两种形式的跨具身形态泛化。首先，它利用易于采集的人类手指示范来为更困难、更高维度的任务（例如双臂机械臂）提供规划引导。其次，它支持在多种机器人平台之间复用同一组示范，因为 iiwa 手臂和 Panda 手臂都可以利用同一份数据完成操作任务，从而无需为每种具身形态单独采集示范。

IV. 自动化数据生成

在本节中，我们介绍一种方法，该方法仅依托少量示范，就能够在多种物体、初始条件和具身形态下，自动生成大量在物理上可行的、用于接触丰富操作任务的轨迹。所提出的方法还具有一个潜力，即可以将使用过时时配置采集得到的遗留数据集适配到新的机器人配置上，从而避免在新机器人系统上从零开始采集海量数据，显著降低数据采集成本。

我们的方法首先将最初在VR中采集到的、具身形态灵活的人类示范中的运动学轨迹重定向到仿真中的特定机器人具身形态上，从而生成在运动学上可行的轨迹。随后，我们通过局部轨迹优化对这些轨迹进行细化与扩增，以在一系列物理参数范围内获得在动力学上可行的轨迹。接下来的各个小节将对该流水线中的每一个步骤进行详细说明。

目标：
手上只有少量人类演示（在VR里录的人动作），但我想要一个又大又干净、物理上真的能跑的机器人轨迹数据集，包括：

不同物体

不同初始状态

不同机器人形态（双臂、手机机械臂、以后换新机器人……）

做法就是两步：

“照猫画虎”：把人类的动作姿势转换成机器人能做到的“样子”（运动学重定向） 这一步只管“每一帧长得像不像”，不管动力学是否真的能跑。

“物理补作业”：在这个“样子”附近做轨迹优化，让它既像示范，又满足动力学+接触约束（CITO那类优化），而且在仿真里疯狂改物理参数、起始状态，多次重算，从一条示范“裂变”出一堆动力学可行的轨迹。

一些术语先搞清楚

运动学一致（kinematically consistent）：
机器人每个时刻的姿态“长得像”人类的动作，比如手的位置、方向差不多，但不保证动力学上真能做到（可能速度太快、力不够等）。

动力学可行（dynamically feasible）：
轨迹满足动力学方程 $x_{t+1}=f(x_t,u_t)$ ，不穿模、不超关节力矩、不飞起来这些物理约束都满足，仿真里真的能跑出来。

具身差异（embodiment gap）：
人体和机器人长得不一样：

人有手指、手腕；

机器人可能是两段机械臂+抓手；
所以同一个“抓杯子”的任务，它们的关节数、手的位置等都不一样。

接触丰富任务（contact-rich manipulation）：
不只是“抓一下搬一下”，而是有一堆接触、摩擦、推、顶、滑等操作。

A. Kinematic Motion Retargeting 运动学运动重定向

给定一段时域长度为 $T$ 的示范序列 $x^{\text{demo}}_{0:T}$ ，我们的目标是找到一组机器人配置 $q^{\text{retarget}}_{0:T}$ ，使其既能够匹配示范者的动作位置，又避免发生穿透并满足关节限制。在每一个时间步，我们需要求解如下非凸优化问题：

$q^{\text{retarget}^\star}_t=\argmin_{q^{\text{retarget}}_t}\sum_{i=0}^{N} w_i\|\psi_i(q^{\text{retarget}}_t)-\tilde{\psi}_i(x^{\text{demo}}_t)\|^2 \tag{1a}$

$\text{s.t.}\ \ \ \phi_j(q^{\text{retarget}}_t)\ge 0,\ \forall j \tag{1b}$

$q_{\min} \le q^{\text{retarget}}_t \le q_{\max}, \tag{1c}$

其中， $w_i>0$ 为权重参数， $\psi_i$ 与 $\tilde{\psi}_i$ 分别表示从机器人配置和示范者状态映射到具身形态对应关键点的第 $i$ 个映射函数。对于每一对机器人与示范者，其对应的关键点都是人工定义的。例如，在双臂机械臂系统中， $\psi_0$ 表示从机器人关节角度到左臂末端执行器位置的正向运动学映射，而 $\tilde{\psi}_0$ 表示从人手姿态映射到左手食指指尖位置的函数。我们发现，由轨迹优化生成的结果对于关键点对应方式及权重参数的选择具有较强的鲁棒性。其中 $\phi_j$ 表示第 $j$ 个碰撞对之间的符号距离函数，而约束（1b）用于强制避免穿透。 $q_{\min}$ 和 $q_{\max}$ 分别表示关节角度的下界和上界。注意， $q^{\text{retarget}}$ 与 $x^{\text{demo}}$ 的维度可以不同，只要 $\psi_i$ 和 $\tilde{\psi}_i$ 能够将它们映射到相同空间的向量即可（例如，Apple Vision Pro 仅捕捉食指的5个关键点，而双臂系统中的每条机器人手臂具有7个自由度）。我们使用一种序列二次规划（Sequential Quadratic Programming, SQP）式的算法来求解式（1）：在每次迭代中，我们对非穿透约束（1b）进行线性化处理，并对匹配目标（1a）在上一次迭代的解附近做二次近似。为了加速收敛并保持时间上的一致性，我们将前一时刻的最优解 $q^{\text{retarget}^\star }_{t-1}$ 作为当前时刻 $t$ 非线性规划的初始解（warm-start）。

A.1 我们手里有什么？

一段人类示范： $x^{\text{demo}}_{0:T}$

这是在 VR 里录到的人体/手的状态序列。

每个时刻 $t$ 的状态 $x^{\text{demo}}_t$ 包含：手的位置、手指关键点、身体姿态等等。

我希望得到：机器人在同一时间段上的关节轨迹 $q^{\text{retarget}}_{0:T}$

$q_t$ 是机器人每个关节角度的向量（比如双臂各 7 自由度 → 14 维）。

A.2 我们到底在优化啥？

对每个时刻 $t$ ，要解一个优化问题（式 $(1)$ ）：

核心目标是：

$q^{\text{retarget}^\star}_t=\argmin_{q^{\text{retarget}}_t}\sum_{i=0}^{N} w_i\|\psi_i(q^{\text{retarget}}_t)-\tilde{\psi}_i(x^{\text{demo}}_t)\|^2 \tag{1a}$

你可以把它理解成：

“把机器人身上的一堆关键点（手上、手指、手腕……）尽量对齐到人类示范里对应的那些关键点。”

$\psi_i(\cdot)$ ：把机器人关节角度 $q$ 映射到第 $i$ 个“关键点”的位置。比如：

$\psi_0(q)$ = 左手末端执行器的位置（通过正向运动学 FK 算出来）

$\psi_1(q)$ = 右手末端执行器的位置

……

$\tilde{\psi}_i(\cdot)$ ：把人类状态 $x^{\text{demo}}_t$ 映射到对应的“人类关键点”——比如左手食指指尖的位置。

$w_i>0$ ：每个关键点的重要性权重。手指尖可能 $w$ 大一点，手肘小一点等。

所以整项 $||\psi_i(q^{\text{retarget}}_t)-\tilde{\psi}_i(x^{\text{demo}}_t)||^2$ 就是“机器人关键点位置”和“人类关键点位置”的距离平方。全体求和+带权：

“机器人整体姿态尽量在空间里贴近人类示范的姿态。”

A.3 为啥有一堆约束？

后面还有两个约束 $(1 b)$ 、 $(1 c)$ ：

不穿模约束：

$\phi_j(q^{\text{retarget}}_t)\ge 0,\ \forall j \tag{1b}$

$\phi_j$ 是第 $j$ 对可能碰撞物体之间的符号距离函数：

$> 0$ ：两个物体之间有间隙

$= 0$ ：刚好接触

$< 0$ ：已经穿透

$\phi_j\ge0$ 就是强制“别穿透”。

关节范围约束：

$q_{\min} \le q^{\text{retarget}}_t \le q_{\max}, \tag{1c}$

机器人每个关节都有最小角度和最大角度，不能超出。

所以这个优化问题就是：“让机器人长得像人干的动作，但不能把胳膊掰断，也不能穿墙。”

A.4 维度可以不一样这件事

他们特别强调：

$q^{\text{retarget}}$ 和 $x^{\text{demo}}$ 的维度可以完全不同，只要 $\psi_i$ 和 $\tilde{\psi}_i$ 把它们都投到同一个“关键点空间”里就行。

例子：

Apple Vision Pro 只给你5个手指关键点的位置；

机器人每条手臂却有 7 个关节。

没关系，只要你定义：

$\psi_i(q)$ ：机器人手某块位置；

$\tilde{\psi}_i(x)$ ：人手对应那块位置；

就可以做到“姿态对齐”。

A.5 这个优化是怎么解的？SQP

这是个非凸优化（因为：

正向运动学 $\psi_i$ 里有三角函数；

碰撞距离函数 $\phi_j$ 都是非线性的；

他们用的是序列二次规划 SQP：

思路：原问题太难 → 在当前解附近“线性+二次近似” → 变成一个好解的二次规划 QP → 解出来 → 更新解 → 继续迭代。

具体：

在每次迭代中：

把不穿模约束 $\phi_j(q)\ge0$ 线性化；

把目标函数 $(1 a)$ 在上一次解附近做二次近似；

这样每一步都是一个标准 QP，可以用成熟算法很快求解。

同时，为了让每个时刻 $t$ 的优化都比较容易收敛，他们：

把上一时刻 $t - 1$ 的最优解 $q^{\text{retarget}^\star }_{t-1}$ 当成当前时刻 $t$ 的初始点（warm-start）。

直观理解：

人的动作是连续的；

机器人这帧的姿态一般跟上一帧差不多；

所以用前一帧当初始解，迭代次数就少，时间上也比较平滑。

1. 先想象一下：我们在解什么问题？

对每一个时刻 $t$ ，你要解的是：

找一个 $q_t^{\text{retarget}}$ ，让机器人“长得像人类这一帧的动作”，同时满足“不穿透”“关节限位”这堆约束。

数学上就是式(1a)(1b)(1c)那套东西。
关键点：这是一个“非线性 + 带约束”的优化问题，直接一下子很难解。

目标 $\sum_i w_i|\psi_i(q_t)-\tilde\psi_i(x_t^{\text{demo}})|^2$ 里有正运动学 $\psi_i$ ，是非线性的（有好多 $\sin,\cos$ ）。

碰撞约束 $\phi_j(q_t)\ge0$ 也是非线性的（距离函数很弯）。

非线性 + 约束 → 这类问题的典型“重武器”之一：SQP（序列二次规划）。

2. SQP 是什么？先用一个特别笨的比喻

你可以把 SQP 想成：

原问题是“一座很奇怪的山（非线性目标+约束）”。你站在山上的某个点，不知道往哪里走能最快下山。于是你在脚下画一个小小的“局部地图”：

把山在附近用一个 光滑的小碗（quadratic，二次函数） 来代替；

把障碍物（约束）用 直线（linear） 来代替。

在这个小地图上，你就可以很容易找出“最好的方向”。然后你走到那个新点，再画一张新的小地图。如此反复，直到走到山谷。

“局部地图” = 一个带线性约束的二次优化问题（Quadratic Program，QP）。所以 SQP ≈「反复解二次规划」：

用当前点的邻域，把原问题近似成一个“二次目标 + 线性约束”的问题；

解这个简单的问题，得到一个更新方向；

走过去，再近似，再解；

不断重复，逐渐接近原问题的最优解。

3. “线性化约束、二次近似目标”是什么意思？

3.1 线性化约束（拿 $\phi_j(q_t)\ge0$ 举例）

碰撞约束是：

$\phi_j(q_t)\ge0$

$\phi_j$ 是一个很弯的函数（比如两块几何体的距离），不好直接处理。

线性化就是：

在当前解 $q^{(k)}$ 附近，用一条切线来近似这条弯弯曲线。

一维情况下你可以这么想：

原函数： $y=\phi(q)$

在当前点 $q^{(k)}$ ，作一条切线： $\phi(q)\approx \phi(q^{(k)})+\phi'(q^{(k)})(q-q^{(k)})$

高维情况就是梯度 $\nabla\phi$ ，写成：

$\phi_j(q)\approx\phi_j(q^{(k)})+\nabla\phi_j(q^{(k)})^\top (q-q^{(k)})$

然后把原本的 $\phi_j(q)\ge0$ 替换成这个线性的近似不等式：

$\phi_j(q^{(k)})+\nabla\phi_j(q^{(k)})^\top (q-q^{(k)})\ge0$

这就好像：原来“障碍物边界”是一条弯弯的曲线，现在在你脚下这附近，用一条直线来代替它。

好处是：线性约束非常适合放进二次规划（QP）里求解。

3.2 二次近似目标（拿式(1a)举例）

目标是：

$F(q)=\sum_i w_i|\psi_i(q)-\tilde{\psi}_i|^2$

这个 $F (q)$ 也是弯弯的（非线性），不好直接解。二次近似就是使用二阶泰勒展开，把它在当前点 $q^{(k)}$ 附近用一个“小碗形”的函数代替：

$F(q)\approx F(q^{(k)}) + \nabla F(q^{(k)})^\top (q-q^{(k)}) + \frac12 (q-q^{(k)})^\top H^{(k)} (q-q^{(k)})$

$\nabla F(q^{(k)})$ ：在当前点的梯度（斜率）；

$H^{(k)}$ ：当前点的 Hessian（或其近似），决定“小碗”的弯曲程度。

这个近似函数是一个标准的二次函数，长得就像一个（高维的）“碗”。最终，我们在第 $k$ 次迭代解的“局部问题”大概长这样：

目标： 最小化一个二次函数 $Q^{(k)}(q)$ （是 $F$ 在 $q^{(k)}$ 的二次近似）

约束： 一堆线性不等式（是 $\phi_j(q)\ge0$ 在 $q^{(k)}$ 的线性近似）再加上 $q_{\min}\le q\le q_{\max}$ 这种本来就是线性的约束。

这就是一个典型的 Quadratic Program (QP)，有很多成熟、高效的算法可以解。你可以把这一整步简称成：

“在当前解附近，把原来又弯又麻烦的问题，改成了一个‘二次目标 + 线性约束’的小问题（QP），先把这个小问题解了再说。”

这就是论文里那句话：

“对非穿透约束（1b）进行线性化处理，并对匹配目标（1a）在上一次迭代的解附近做二次近似。”

4. 怎么“迭代”？

用伪代码帮你过一遍，一个时间步 $t$ 的 SQP 大致是这样：

选一个初始猜测 $q^{(0)}_t$ （稍后说 warm-start 就是选这个）。

对于 $k = 0, 1, 2, ...$ 重复：

在 $q^{(k)}_t$ 附近：

把目标 $F(q_t)$ 做二次近似 → 得到 $Q^{(k)}(q_t)$ ；

把约束 $\phi_j(q_t)\ge0$ 做线性化；

解这个 QP，得到一个更新步长 $\Delta q^{(k)}$ ；

更新：
$q^{(k+1)}_t = q^{(k)}_t + \Delta q^{(k)}$

如果变化已经很小（或者迭代次数够多）就停。

最终得到 $q^{\star}_t \approx$ 原问题在时刻 $t$ 的最优解。

一句话：

SQP = “原问题太难 → 不断在当前点附近搭建一个小 QP → 解这个小 QP → 往前挪一步 → 再搭一个新的 QP → ……”

SQP例子

B. Demonstration-Guided Trajectory Optimization 基于示范引导的轨迹优化

运动学这步只是让“每一帧的姿态看起来像”，但是：

可能末端移动太快；

可能需要的力矩超过电机极限；

多接触情况下可能黏在一起、弹开等复杂现象都没考虑。

所以要上第二步：动力学轨迹优化。

由于具身差异和物理参数的不同，运动学一致的机器人轨迹 $q^{\text{retarget}^\star}_{0:T}$ 通常在动力学上并不可行。但这些轨迹能够为生成具有复杂多接触交互的动力学可行轨迹提供良好的指导。特别是，人类示范提供了关于何时、何处与物体接触的全局性信息，而基于模型的规划方法可以在此基础上进行局部细化。我们将重定向后的系统状态 $x^{\text{retarget}}_t$ 定义为同时包含物体状态 $x^{\text{object}}_t$ （它是示范状态 $x^{\text{demo}}_t$ 的一个子集）以及由 $q^{\text{retarget}^\star}_t$ 确定的机器人状态。随后，通过求解以下非凸优化问题对轨迹 $x^{\text{retarget}}_{0:T}$ 进行局部细化：

$x_t^*, u_t^*=\argmin_{x_t,u_t} ||x_T - x^{\text{retarget}}_T||^2_{Q_T} +\\ \sum_{t=0}^{T-1} \left(||x_t - x^{\text{retarget}}_t||^2_{Q_t} +||u_t||^2_{R_t}\right) \tag{2a}$
$\text{s.t.}\ \ x_{t+1}=f(x_t,u_t) \tag{2b}$

$\phi_j(x_t)\ge 0,\ \forall j \tag{2c}$

$x_{\min}\le x_t\le x_{\max} \tag{2d}$

$u_{\min}\le u_t\le u_{\max} \tag{2e}$

其中， $f$ 是由动力学引擎通过时间步进得到的动力学函数； $x_{\min}$ / $x_{\max}$ （ $u_{\min}$ / $u_{\max}$ ）为状态（控制输入）的上下界； $Q_t$ 与 $R_t$ 分别是状态与输入的代价矩阵； $Q_T$ 为终端状态代价矩阵。为了增强物体轨迹的精确跟踪，我们在 $Q_t$ 中对对应于 $x^{\text{object}}_t$ 的分量赋予更高权重。具体参数可参见附录 IX-A。

B.1 状态 $x$ 里有什么？

他们定义重定向后的系统状态 $x^{\text{retarget}}_t$ 包含两部分：

物体状态 $x^{\text{object}}_t$

比如物体的位置、姿态、线速度、角速度等；

这是 $x^{\text{demo}}_t$ 的一个子集——意思是人类演示里也有物体的状态信息。

机器人状态（由 $q^{\text{retarget}^\star}_t$ 决定）

也可能包括关节速度等。

这样 $x^{\text{retarget}}_{0:T}$ 就是一条完整的“名义轨迹”：既有机器人状态，又有物体状态，只不过不一定满足动力学。
B.2 要解的优化问题 $(2)$

他们现在要解的是：

$x_t^*, u_t^*=\argmin_{x_t,u_t} ||x_T - x^{\text{retarget}}_T||^2_{Q_T} +\\ \sum_{t=0}^{T-1} \left(||x_t - x^{\text{retarget}}_t||^2_{Q_t} +||u_t||^2_{R_t}\right) \tag{2a}$

约束条件：

动力学方程： $x_{t+1}=f(x_t,u_t)$

不穿模： $\phi_j(x_t)\ge 0,\ \forall j$

状态上下界： $x_{\min}\le x_t\le x_{\max}$

控制输入上下界： $u_{\min}\le u_t\le u_{\max}$

逐项解释一下目标函数：

$||x_T - x^{\text{retarget}}_T||^2_{Q_T}$ → 最终时刻 $T$ 的状态要尽量接近重定向轨迹给定的终点。

$\sum_{t=0}^{T-1}||x_t - x^{\text{retarget}}_t||^2_{Q_t}$ → 在整个轨迹上，当前状态 $x_t$ 要尽量跟重定向后的名义轨迹在一起，即不要偏太多。这保证 “看起来像示范” 。

$\sum_{t=0}^{T-1}||u_t||^2_{R_t}$ → 控制输入 $u_t$ （如关节力矩）不要太大，避免乱蹦乱跳、能量消耗太高。

矩阵 $Q_t$ 、 $Q_T$ 、 $R_t$ 控制：

不同状态分量的重要性；

不同时间段的权重。

他们提到：为了更精确地跟踪物体轨迹，把 $Q_t$ 里对应 $x^{\text{object}}_t$ 的那部分权重加大。意思就是“物体要走对路，机器人的姿态可以稍微妥协”。
B.3 动力学函数 $f$ 是啥？

$f$ 是：由仿真器（动力学引擎）通过时间步进算出来的状态更新： $x_{t+1}=f(x_t,u_t)$

包括：

刚体动力学（牛顿–欧拉方程）；

接触力、摩擦力；

关节约束；

……

只要把当前状态+控制送进仿真器一步，就得到下一个状态。这里把这个过程简写成一个函数 $f$ 。
B.4 为啥说没有示范很难？

他们说：

没有示范的时候，用 CITO 这类基于模型的规划，很难直接从随机初值找到长时域、多接触的好轨迹。

原因直观：

接触什么时候发生？

哪个时间点开始抓？什么时候松？

不知道“接触时刻表 contact schedule”的情况，优化容易：

不是物体根本不动；

就是乱碰几下卡在某个局部最优。

人类示范的作用就是：

把“何时、何处接触”的全局结构告诉你。

而重定向后的 $x^{\text{retarget}}_{0:T}$ 正好可以作为CITO 的初始轨迹/参考轨迹，让优化只在附近“微调”，而不是从零瞎摸。

通常情况下，若没有示范，基于模型的规划器难以找到高质量的长时域、多接触轨迹。 CITO（接触隐式轨迹优化）需要良好的初始猜测，否则很容易陷入局部最优而无法取得进展。人类示范能够提供有价值的全局引导，从而帮助克服上述挑战，而重定向后的轨迹 $x^{\text{retarget}}_{0:T}$ 可自然作为 CITO 类方法的初始猜测，使随后进行的局部调整能够满足动力学约束（2b）。

由于在仿真中可以直接调用系统动力学 $f$ ，我们可以在示范附近对物理参数以及机器人与物体的状态进行局部扰动。借助单一示范，我们可以针对具有不同动力学 $f(x_t,u_t,\theta_t)$ 的任务分布求解式（2），其中 $\theta_t\sim\rho$ 表示各种扰动。我们假设运动学重定向后的轨迹 $x^{\text{retarget}}_{0:T}$ 能够在名义示范附近的任务空间内持续提供良好的指导。因此，我们能够从一条人类示范生成大量在物理上自洽、且具有不同物理属性和初始条件的轨迹。我们在算法 1 中总结了整个数据生成流水线。

利用仿真做大规模扩增（物理参数扰动）

前面两步看上去只得到了一条轨迹：

给定一条示范 $x^{\text{demo}}_{0:T}$ ；

得到一个重定向轨迹 $x^{\text{retarget}}_{0:T}$ ；

再跑一次 $(2)$ 得到一条动力学可行轨迹 $x^*,u^*)$ 。

这还不够“数据量巨大”。关键在这里：

在仿真里，我们可以随便改物理参数和初始条件。

他们做的事情是：

在仿真中引入了一个参数向量 $\theta_t$ ，比如：

物体质量

摩擦系数

接触刚度

甚至一点点传感/执行误差
……

用一个概率分布 $\rho$ 来表示这些参数的分布：

比如质量在某个区间内均匀采样，

摩擦系数在某个范围内高斯采样等。

那动力学就变成： $f(x_t,u_t,\theta_t)$

然后：

在同一条示范附近，反复采样不同的 $\theta_t\sim\rho$ ，对每个采样都解一次优化问题 $(2)$ 。

直觉就是：

“这回杯子重一点” → 再算一条轨迹；

“这回桌子更滑一点” → 再算一条轨迹；

“这回机器人初始位置偏左一点” → 再算一条轨迹；

因为重定向后的轨迹 $x^{\text{retarget}}_{0:T}$ 本身在 “示范附近的任务空间” 里都还是有指导作用，所以：

从一条人类示范，能裂变出许多不同物理属性、不同初始条件下的“物理自洽”的轨迹。

这就是他们所说的：

“借助单一示范，我们可以针对具有不同动力学 $f(x_t,u_t,\theta_t)$ 的任务分布求解式（2）。”

在这里插入图片描述

算法 1：把整个流程写成伪代码

标题

Algorithm 1: Automated Data Generation
算法 1：自动化数据生成

输入（Input）

概率分布 $\rho$ ：

就是物理参数（质量、摩擦等）或者扰动的分布；

扩增数量 $N$ ：

想要从这一条示范生成多少条新轨迹；

示范轨迹 $x^{\text{demo}}_{0:T}$ ：

VR 里录的人类演示。

输出（Output）

$N$ 条在目标机器人具身上的、动力学自洽的轨迹 $\{(x^\star_{0:T},u^\star_{0:T-1})\}$ 。

第 3 行：重定向（对应式(1)）

$q^{\text{retarget}^\star}_{0:T}\leftarrow\text{Solve (1) for }x^{\text{demo}}*{0:T}$

意思是：

用 步骤 A 的重定向优化 $(1)$ ；

把整条示范 $x^{\text{demo}}_{0:T}$ 转到机器人上，得到 $q^{\text{retarget}^\star}_{0:T}$ ；

然后你就能构造 $x^{\text{retarget}}_{0:T}$ （把物体状态+机器人状态拼起来）。

第 4 行：开始扩增循环

for $n=1,\dots,N$ do

我们准备生成第 $n$ 条轨迹。

第 5 行：采样物理参数

Sample $\theta_{0:T}\sim\rho$

从给定分布 $\rho$ 中，采一组物理参数（可能是整条时间上的一串，也可能是固定不变的，这取决于实现）。

第 6 行：解轨迹优化（对应式(2)）

$(x^\star_{0:T},u^\star_{0:T-1})\leftarrow\text{Solve (2) with }x^{\text{retarget}}_{0:T},\theta_{0:T}\text{ and }x_{t+1}=f(x_t,u_t,\theta_t)$

翻译成白话：

给定：

名义轨迹 $x^{\text{retarget}}_{0:T}$ ；

当前采样的物理参数序列 $\theta_{0:T}$ ；

动力学函数 $f(x_t,u_t,\theta_t)$ ；

解优化问题 $(2)$ ，得到一条：

状态轨迹 $x^\star_{0:T}$ ；

控制序列 $u^\star_{0:T-1}$ ；

这条轨迹是：

尽量贴近人类示范（通过跟 $x^{\text{retarget}}$ 的距离惩罚）；

在动力学、接触约束、状态/控制上下界下真正可行的。

整个循环做完 $N$ 次，你就拿到 $N$ 条这样的轨迹，构成一个“又大又真实”的训练数据集。

V. 轨迹优化实验

尽管对示范进行运动学重定向可能足以为抓取与放置等简单操作任务生成数据，但对于那些需要频繁接触模式切换和细粒度动作的、更具挑战性的接触丰富任务而言，这种方法往往显得不足。在本节中，我们展示了轨迹优化对于在三个高维灵巧操作系统——浮动式 Allegro 手、双臂 iiwa 机械臂以及双臂 Panda 机械臂——上生成多样化且在动力学上可行的接触丰富轨迹具有关键作用。

我们的数据生成框架对具体选用哪种轨迹优化器是“无关”的。在实现上，我们采用交叉熵方法（Cross-Entropy Method, CEM）[89]，按照表 I 中给定的物理参数与初始条件分布来求解式（2）。

在这里插入图片描述

任务任务目标是在桌面上将物体操作至给定目标位姿（见图 6）。物体最初随机放置在桌面上，并以任意一面朝上。对于 Allegro 手，当物体到达目标位姿附近 3 cm 以内且姿态误差不超过 $0.2$ rad 时视为任务成功；而对于双臂机械臂，当物体位姿误差在 10 cm 和 $0.2$ rad 以内时则判定为成功。该任务需要对机器人与物体之间复杂的多接触交互进行长时域的推理。其中频繁的接触模式切换以及高维的动作空间，对传统的基于模型的规划器构成了巨大挑战，而精确的接触交互又要求具备细粒度的控制动作。

在这里插入图片描述

动力学可行性 尽管运动学运动重定向能够生成在视觉上看起来合理的机器人与物体轨迹，但由于人类示范者与目标机器人在物理参数和具身形态上存在差异，这些轨迹往往缺乏动力学一致性。为了说明这一点，我们回放了最初 24 条人类示范的运动学重定向轨迹，并在表 II 中记录了各个系统对应的成功率。此外，我们还依据表 I 随机采样物体尺寸和初始位姿扰动，并在这些条件下执行名义的运动学重定向轨迹。得益于“围笼式抓取”（caging grasps）或其他在示范过程中促进鲁棒性的策略，在某些扰动条件下仍有部分轨迹可以成功完成任务。对于所有系统而言，这些成功的 rollout 轨迹都相对较短：通常在 1–2 次旋转以内就将物体操作到目标位姿。

在这里插入图片描述

纯运动学重定向轨迹的低成功率凸显出：为了针对特定具身形态和物理参数对示范进行局部细化，轨迹优化是至关重要的。在轨迹优化之前，浮动式 Allegro 手只是轻触立方体，并且在顺时针旋转时很容易失去接触（如图 4a 所示）。完成轨迹优化后，机械手会主动增大接触面积，从而建立起更稳定的抓持来进行旋转。在图 4b 中，我们可以在双臂 iiwa 机械臂上观察到类似的“促进接触”的行为：示范轨迹试图仅用单臂顺时针旋转箱体，而轨迹优化则鼓励另一条手臂参与支撑，从而更稳定地重新定向箱体。这种鼓励增加接触的细化策略在物体更重或更小、或者摩擦系数低于预期时尤为有用。此外，当物体姿态相对于示范有轻微偏移时，简单回放运动学重定向轨迹往往会失败，将物体“推”到超出机器人可及范围之外（如图 4c 所示）。相比之下，轨迹优化会显式考虑系统的真实动力学，并据此调整机器人的动作。在随机物理参数与物体初始条件扰动下，各个系统使用轨迹优化得到的成功率也记录在表 II 中。

在这里插入图片描述

跨具身形态泛化 我们展示了：同一组人类示范可以被有效“再利用”，在不同任务时域长度的多种机器人具身形态上生成动力学一致且接触丰富的轨迹。具体而言，最初是人类使用双手食指操作一个小立方体的示范，经过重定向后，被映射为定基双臂 Kuka LBR iiwa 和 Franka Emika Panda 机械臂操作一个更大箱体的轨迹（如图 3 所示）。这种方式解决了接触丰富任务数据采集中的若干关键难题：若直接遥操作两条真实机械臂去翻转一个大箱子，将在体力负担和成本方面都极其高昂，并且会受到硬件延迟、有限反馈以及“具身差异”（即人类手臂和机器人手臂在运动学结构、自由度以及工作空间上的差异）等因素的严重制约。相比之下，在小尺度下用人类手指完成同一任务更加直观，体力负担更小，同时还能更快速、更一致地采集示范数据。

在这里插入图片描述

iiwa 与 Panda 机械臂在接触几何、速度上限和关节约束等方面均存在差异，而这些因素都被显式纳入式（2）所描述的轨迹优化框架中进行建模。为了在实际硬件部署中保证安全，我们在 iiwa 手臂上施加了较为保守的速度上限；而在仿真环境中，我们仅对 Panda 手臂使用柔性的速度正则项，以允许其执行更“激进”的运动。

数据多样性 轨迹优化可以高效地将单条示范扩展为在局部物理参数和初始条件扰动下的一整分布轨迹集，如图 5 所示。所生成数据集中所包含的多样化状态覆盖了更广的训练分布，并促使最终学到的策略更加平滑，这一点将在下一节中进一步讨论。

在这里插入图片描述

VI. 行为克隆实验

我们通过实验展示本框架能够在多种机器人平台上高效生成多样且高质量的接触丰富数据集，用于训练行为克隆策略，这些平台包括仿真中的浮动式 Allegro 手和双臂 Panda 机械臂，以及真实硬件上的双臂 iiwa 机械臂。我们表明，相比仅基于原始示范数据训练得到的策略，利用生成数据训练的策略能够在更广泛的物理参数与初始条件分布上实现良好的泛化，并在鲁棒性与性能上大幅提升。

A. 仿真中的策略评估

从仅有的 24 条人类示范出发，我们的数据生成流水线通过轨迹优化即可高效生成数千条在动力学上可行的接触丰富轨迹。我们分别在 24 条原始示范轨迹、以及包含 500 条和 1000 条生成轨迹的数据集上训练基于状态的扩散策略（state-based diffusion policies）[26]。虽然我们的方法可以与任意行为克隆算法兼容，但在实验中我们选用扩散策略，是因为其在近期的接触丰富任务中已展现出良好表现[21,70,90]。图 6 展示了策略执行过程中的 rollout 结果可视化。我们在仿真中为每一种具身形态各执行 48 次策略 rollout，并在图 8 中记录其成功率来评估性能。成功判定标准与轨迹优化实验中的设定相同。

在这里插入图片描述

1）浮动式 Allegro 手：在人类示范中，操作者在虚拟现实环境下平均约 5 秒即可完成任务；然而，为了在具有速度上限约束的浮动式 Allegro 手上实现更平滑且动力学可行的运动，我们将示范轨迹在时间上按 2.5 倍进行缩放。我们将任务时域长度设定为 25 秒，以便在执行过程中为策略提供足够的时间，从漏接触或其他错误中恢复。任务的复杂性主要来自 Allegro 手 22 维的动作空间以及任务的长时域特性：要将立方体重新调整至竖直姿态，需要一系列协同的滚转、俯仰和偏航动作。在缺乏额外引导的情况下，这些因素共同对传统的基于模型的规划器构成了巨大挑战。

仅在 24 条原始示范数据上训练得到的基线行为克隆策略，其成功率为 $10/48=21\%$ ，并且在遇到分布外状态时会出现明显的抖动行为。由于工作空间中物体的姿态与位置分布多样，空间范围足够大，以至于策略在 rollout 过程中出现的微小偏差就可能将轨迹推离示范所覆盖的分布。典型的失败模式包括：Allegro 手在尝试重新定向立方体时反复错失接触，或者卡在物体表面（如图 7a 所示），从而导致物体停留在某个中间姿态无法完成任务。相比之下，使用我们流水线生成的扩展数据集训练得到的策略，在初次接触失败后更有可能重新建立与物体的接触，从而使成功率显著提高，可达到 $39/48=81\%$ 。

在这里插入图片描述

2）双臂机械臂：在双臂 iiwa 系统上，仅利用 24 条人类示范训练得到的基线策略成功率为 $27/48=56\%$ 。我们推测，较为严格的速度限制促使策略倾向于更接近准静态的行为，从而产生更长的轨迹，并在训练数据中形成更高密度的状态–动作样本。相比之下，在双臂 Panda 系统上，基线策略的成功率为 $14/48=29\%$ ，我们认为这与 Panda 系统较宽松的速度约束所导致的更具动态性的行为有关。两种基线策略都表现出明显的抖动运动，经常在重新定向箱体过程中把箱子“踢”出可及范围、失去接触，或者直接撞上并卡在箱体表面（如图 7b 和图 7c 所示）。与之相对，基于扩增数据集训练得到的策略可以生成明显更为平滑的轨迹，并且在初次错失接触后能够重新与物体建立接触，使双臂 iiwa 的成功率最高达到 $44/48 = 92$ ，双臂 Panda 的成功率则达到 $42/48 = 87.5$ 。此外，这些策略还能复现原始人类示范中出现的多模态行为，例如在相似物体姿态下选择顺时针或逆时针旋转箱体等不同操作方式。

B. 硬件上的策略评估

我们在双臂 iiwa 机械臂真实硬件上零样本（zero-shot）部署已训练的策略，用于在桌面上翻转一个边长为 30 cm 的立方体箱子（见图 9）。我们使用 OptiTrack 动作捕捉系统来估计物体的位姿。基线行为克隆策略在该硬件任务上的成功率仅为 $6/23=26\%$ ，且其成功的 rollout 多为相对短时域，仅涉及 1–2 次旋转。基线策略的典型失败模式包括：1）偏离示范轨迹，导致机械臂与箱体表面发生碰撞（图 10a）；2）在滚动过程中箱体出现明显滑动，使得策略进入分布外状态并难以恢复（图 10b）。相比之下，如图 8b 所示，在 500 条生成轨迹上训练的策略成功率达到 $17/23=74\%$ ，而在 1000 条生成轨迹上训练的策略成功率为 $16/23=70\%$ 。尽管在滚动过程中偶尔仍会出现箱体滑动，这些策略已经呈现出更好的稳定能力：它们会利用一条手臂更用力地固定住箱体的另一侧，以抑制进一步滑动（图 10d）。不过，如图 10c 所示，基于扩增数据集训练的两种策略仍然表现出一种由 iiwa 手臂未建模的碰撞几何引发的失败模式：在执行俯仰动作时会导致箱体出现明显且不期望的偏航运动。

在这里插入图片描述

VII. 局限性与未来工作

尽管我们的方法可以高效生成大量接触丰富的轨迹，但仍然存在一些局限。首先，尽管我们的人手示范框架在效率和直观性方面具有优势，但它未必能充分发挥目标机器人的运动学能力，例如关节的连续旋转或特定的灵巧操作动作。未来工作可以探索将我们的自动化数据生成框架应用于具身感知（embodiment-aware）的遗留数据集，从而更好地捕捉不同机器人系统所特有的运动能力。

其次，尽管得益于轨迹优化，我们的方法在示范附近区域表现出较强性能，但在远离示范区域的状态（例如由灾难性失败导致的状态）下，所学习到的策略仍然难以实现有效恢复。未来工作可以考虑引入更先进的规划技术，以迭代方式提升在尚未访问状态空间区域中的策略鲁棒性。

再者，我们目前主要在训练鲁棒的基于状态的策略方面验证了该流水线的有效性。若能通过引入高质量的仿真合成渲染，将该框架扩展至训练视觉–运动（visuomotor）策略，那么策略向真实场景的可迁移性有望进一步提升。

VIII. 结论

在本研究中，我们提出了一条新颖且具成本效益的流水线，将基于物理的仿真、人类示范以及基于模型的规划相结合，以缓解接触丰富机器人操作任务中的数据匮乏问题。我们方法的一个关键洞见在于：即便人类示范是基于与目标机器人形态不同的具身形态采集的，它仍能提供重要的全局任务信息；而在高维搜索空间和复杂接触动力学的双重影响下，基于模型的规划器往往难以独立发现这些信息。通过将这些示范视为全局先验，我们的方法利用运动学重定向与轨迹优化对其进行细化与扩增，从而在多种物理参数、初始条件和具身形态下生成大规模的动力学可行轨迹数据集。该框架大幅降低了对昂贵且依赖特定硬件配置的数据采集过程的依赖，同时为复用在过时硬件或配置下采集的遗留数据集提供了可能。我们在多种仿真机器人系统上验证了该框架的有效性，并将基于扩增数据集训练得到的策略零样本部署至双臂 iiwa 硬件平台上，取得了成功。

IX. 附录

在本附录中，我们给出 CEM 的实现细节以及策略训练的具体实现。

A. CEM 实现细节

我们在表 III 中给出了 CEM 实现所用的详细参数。我们在动作控制点 $u_{0:T-1}$ 上进行优化，并对这些控制点进行线性插值，从而生成发送给 Drake 的动作指令。 Drake 以 200 Hz 的频率仿真接触动力学函数 $f$ 。状态代价矩阵设为
$Q_t = {diag}(q_o\cdot \mathbf{1}_{n_o}, q_r\cdot \mathbf{1}_{n_r}),$
其中， $n_o$ 与 $n_r$ 分别表示物体与机器人的状态维度， $\mathbf{1}$ 为全 1 向量。终端状态代价矩阵为
$Q_T = 10\cdot Q_t.$

控制输入代价矩阵为
$R_t = {diag}(r_u\cdot \mathbf{1}_{n_u}),$
其中 $n_u$ 表示控制输入维度。在所有系统中，我们使用 50 个采样、5 个精英样本，并将动作采样的初始标准差设为
$\sigma = 0.05\cdot \mathbf{1}_{n_u}。$