阮思朴 RUAN Sipu

PRIMP：示教学习引导机器人在全新场景中操纵常用物品

面向协作机器人在未知居家场景中执行复杂操作任务的需求，本项目提出了一种基于概率模型的示教学习方法PRIMP（全称：PRobabilistically-Informed Motion Primitives）与基于工作空间密度的机械臂轨迹规划算法Workspace-STOMP。进而，通过机器人想象，学习物品可供性，在虚拟环境中模拟倒水、舀水等动作，得到机器人完成特定任务所需的关键末端六维位姿，从而生成全新操作轨迹，引导机器人操纵物品。所提出的算法提升了类人操作轨迹生成效率、新环境中避障成功率及任务完成度。该成果发表于机器人领域的顶级刊物IEEE Transactions on Robotics (T-RO)上。

项目网站

论文

开源代码

研究背景

教会机器人如何操纵物品与工具以协助人们的日常生活一直是领域内研究的热点。机器人需要从知识中学习到相应的技能，并将其泛化到未知的场景中。在非结构化的居家环境中，仅对机器人的操作轨迹进行人工编程将会十分耗时耗力，且难以应对环境的复杂变化。示教学习为机器人的技能迁移提供了思路，在近期也涌现了众多针对不同场景的算法。本项目聚焦家用常见物品的操作技能学习与运动规划，提出了基于李群概率模型的示教学习方法PRIMP，并结合轨迹规划算法及物品可供性学习方法，在物理仿真的辅助下，帮助机器人学习倒水、舀水、开关抽屉等日常操作的技能并迁移至包含障碍物的全新环境中。

primp-teaser

图1 技能学习示意图

整体思路框图

所提方法的整体思路如下：

primp-workflow

图2 机器人系统框图

基于李群的示教学习方法：PRIMP

所提出的示教学习方法主要解决示教轨迹的时序归一化、概率学建模、在新场景中的轨迹变形与迁移等问题。通过拖动示教的方式采集5-10组机械臂末端六维位姿的轨迹，构成示教数据集。由于每组的操作运动速度不同，会导致经过关键位姿点的时间有所区别，而无法正确进行概率建模。因此，需要进行时间重参数化，将完成某一动作的所有轨迹在时序上进行归一化处理。采用了全局最优的时序重参数化算法GORA（Globally Optimal Reparameterization Algorithm），通过变分法构建欧拉-拉格朗日方程求解极值，并通过柯西-施瓦茨不等式证明其为全局最优解，进而将所有轨迹在时序上进行重参数化操作，得到了新的轨迹数据集。将已归一化的轨迹进行概率学建模，对于每一离散时间点上的位姿在李群SE(3)中进行均值、协方差计算（其中均值基于绝对位姿，而协方差基于前后时间点的相对位姿）。进而，通过条件概率及链式法则，计算得到轨迹数据的联合概率分布，所得到的均值为6n维向量，协方差为6n x 6n维矩阵（n为离散的步数）。

在得到示教数据的概率模型后，可针对新情景进行轨迹变形，从而使其（1）经过新的关键途径点；（2）适应不同观测视角的转换；（3）在不同机器人间进行技能迁移。在新场景中，要求机械臂末端在特定时间途径特定的关键位姿点，且该位姿具有一定的不确定性。将新的途径点信息表征为观测模型，运用条件概率的计算方法得到新的后验概率，从而使轨迹经过该位姿而保持原有的形状。当观测视角发生变化时，通过对轨迹中的六维位姿进行共轭计算，可推导出变换后的概率分布，证明了所提方法在应对此变换的等变性。由于轨迹的描述与建模均针对末端位姿，与机器人本体的运动学模型无关，因此可通过较为简单的操作将相关模型在不同机器人之间进行迁移。将不同机器人的工作空间密度函数表达为高斯分布，构建为观测模型，进而运用条件概率方法计算出后验概率，使新的轨迹更接近机器人工作空间中概率较高的区域，又能保持形状的相似程度，从而达到轨迹迁移的目的。

primp-novel-situation

图3 PRIMP方法应对新的场景。(a) 从示教轨迹中学习李群中的概率模型，粉色虚线为示教轨迹，蓝色视线为均值轨迹；(b) 轨迹变形后到达新的终点位姿；(c) 轨迹变形后途径新的中间位姿并到达新的终点位姿。

primp-robot-agnostic

图4 运用工作空间密度进行不同机器人间的技能迁移。(a) 在Franka Panda机械臂中建立概率学模型并到达新的终点位姿；(b) 将轨迹迁移至Kinova Gen3机械臂中。

基于工作空间轨迹分布的运动规划算法：Workspace-STOMP

对于物品及工具操作的示教环境中通常不包含周围其他无关物体，而在现实居家场景中，随着环境中障碍物的加入，仅通过示教学习生成的轨迹并不能满足安全操作运动的要求，需要通过运动规划算法帮助机器人躲避障碍。本工作提出了基于机器人末端六维位姿轨迹概率分布的运动规划算法Workspace-STOMP，以基于优化的STOMP（Stochastic Trajectory Optimization for Motion Planning）算法为基础，通过从PRIMP生成的轨迹分布中随机采样，作为参考轨迹，并将每条参考轨迹与STOMP算法每步更新的轨迹进行比较，定义了基于李群中测地线距离的损失函数。最终得到了无碰撞的运动轨迹，并尽可能地保留了PRIMP生成的末端位姿轨迹形状。

primp-wstomp

图5 基于工作空间密度的运动规划方法Workspace-STOMP。绿色物体为障碍物，粉色曲线表示从PRIMP中生成的参考轨迹，蓝色曲线表示最终规划所得轨迹。

通过机器人想象学习物品的可供性及操作功能

PRIMP方法中重要的部分之一在于新场景中途径点位姿的定义。对于一些特定任务（如倒水、舀水），需要具体分析物品与工具的相关性质，从而得到可成功完成相关操作的关键位姿。本工作采用了物理仿真，通过人为制定规则来模拟特定动作，学习物品或工具的可供性，获取关键位姿信息（如舀水任务需要机械臂在中间时刻到达舀的位姿，使勺子口向上并尽可能到达碗底）。所得到位姿随即传递至PRIMP方法，进行机器人技能的迁移及泛化。

primp-affordance

图6 通过Gazebo物理仿真得到的舀水动作关键位姿。

方法对比

与现有基于几何基元的示教学习及基于优化的运动规划方法进行了对比实验与消融实验。

PRIMP与基于运动基元的示教学习方法ProMP、KMP进行对比

  1. LASA手写笔迹数据集 primp-benchmark-lasa
图7 PRIMP在LASA数据集中的对比实验，显示了对于26组数据的算法表现排名。
  1. 针对倒、舀、传递、开抽屉、开门5类动作的真实数据集 primp-experiment-task
图8 不同任务的拖动示教数据采集过程。

primp-benchmark-real-d-demo

图9 PRIMP在真实数据集中的对比实验：生成轨迹与示教轨迹的距离。

primp-benchmark-real-d-via

图10 PRIMP在真实数据集中的对比实验：生成轨迹与途径点的距离。

PRIMP方法的消融实验

  1. 对于时序归一化方法GORA的消融实验 primp-ablation-gora
图11 PRIMP将GORA作为轨迹数据前处理方法的消融实验。
  1. 对于不同机器人间进行技能迁移的消融实验 primp-ablation-wd
图12 PRIMP通过结合工作空间密度进行不同机器人间技能迁移的消融实验。

Workspace-STOMP与STOMP、Cartesian-STOMP方法的对比实验

  1. 不同仿真环境中的规划时间对比 primp-benchmark-wstomp-time
图13 不同运动规划方法的解算时间对比。
  1. 不同仿真环境中的任务完成度对比 primp-benchmark-wstomp-sr
图14 不同方法进行运动规划后的任务完成度对比。

其他实验

还进行了多项针对PRIMP方法性质的对比实验，详见论文。

实物实验

针对上述倒、舀、传递、开抽屉、开门5类动作进行了实验。收集了5-10组示教演示数据，通过三维场景重构、学习、想象及规划，Franka Panda机器人在全新场景中完成了相应的任务。

primp-experiments

图15 真实实验过程。

报告展示