用于规划接触交互轨迹的设备和方法与流程

1.本公开总体上涉及机器人，更具体地，涉及一种在没有预定义的接触时间表的情况下对多接触轨迹进行广义规划的设备和方法。


背景技术：

2.在机器人系统中，运动规划用于给定当前配置确定机器人执行需要达到目标配置(机器人系统的状态)或末端执行器姿态的任务的轨迹。为了机器人的高效运动规划，各种轨迹优化技术用于寻找服从机器人的动力学和任务约束的可行轨迹。换言之，轨迹优化技术旨在确定使服从对机器人系统的状态和输入的一组约束的成本函数最小化的输入轨迹。
3.通常，轨迹优化方法旨在避免机器人与环境之间的接触，即，避免碰撞。然而，在各种机器人操纵和行动任务中必须利用与环境的接触。为此，在轨迹优化中需要考虑接触。由于接触的离散本质，向机器人的运动中引入接触导致非光滑动力学。例如，与环境进行接触或断开接触或者对于接触使用不同的摩擦模式(例如，粘着或滑动)改变了给定关节力确定系统运动的动力学约束。这种现象阻碍了使用轨迹优化来规划接触交互轨迹。
4.为了克服这一缺点，由用户预定义或通过高级启发式规划器生成接触时间表。这种方法适用于涉及少量接触的运动规划。然而，对于复杂运动规划，预定义接触时间表计算上变得不切实际。
5.在另选方法中，使用接触隐式轨迹优化(cito)，其在没有预定义接触时间表的情况下实现接触丰富的复杂运动的运动规划。cito仅给定高级目标(例如，系统期望的结束姿态)使用接触动力学的微分模型同时优化状态、输入和接触力轨迹。物理接触被建模为光滑函数的接触模型是实现基于梯度的优化以推理接触的要素。使用允许一定距离的贯穿和/或接触力的光滑接触模型方便优化的收敛。然而，必须调整接触模型和成本函数的一个或更多个参数以准确地近似真实接触动力学，同时寻找完成给定任务的运动。然而，这种调整是困难的。另外，由于使数值优化高效的松弛(一定距离的贯穿和/或接触力)，接触模型导致物理不准确。此外，当任务或机器人改变时，可能需要重新调整一个或更多个参数。即使对于微小的任务变化，不重新调整一个或更多个参数也可能导致所规划的运动的突变。
6.因此，需要一种给定系统模型和任务规范自动地确定可行接触交互轨迹的免调整接触隐式轨迹优化技术。


技术实现要素：

7.一些实施方式的目的是规划机器人的运动以使对象移动到目标姿态。一些实施方式的另一目的是规划机器人的运动以经由对象与机器人之间(例如，对象与机器人的抓取器之间)的物理接触使对象移动，而无需抓取对象。这种控制的挑战之一是缺乏使用各种优化技术来为机器人确定合适轨迹以实现与环境的这些接触交互的能力。对于机器人操纵，物理接触表现为冲击，因此在动力学中引入了非光滑性，继而阻碍了基于梯度的求解器的利用。为此，提出了多种方法以通过测试众多轨迹来确定轨迹。然而，轨迹的这种生成计算
上低效，并且可能得不到可行结果。
8.为此，一些实施方式的目的是引入表示机器人与环境之间的接触动力学的模型，同时允许在使用这种模型来规划接触交互轨迹时使用光滑优化技术。另外地或另选地，一些实施方式的另一目的是提供这种模型，该模型添加了最少数量的附加参数，具有允许高效规划计算的结构，得到物理上准确的轨迹，并且对运动规划的初始化不敏感。
9.在本公开中，这种模型被称为松弛接触模型，其利用了松弛参数的循环自动惩罚调节。具体地，除了作用于机器人和对象上的物理(实际)力，例如机器人根据轨迹的致动、对象响应于机器人触摸而接收的冲击、摩擦力以及重力之外，松弛接触模型还利用了虚拟力(不存在于现实中)作为建模的载具，并且利用机器人末端执行器与环境(例如，对象)之间的接触进行非抓握操纵。因此，虚拟力在未致动自由度(表示为自由体，例如待操纵的对象或人形机器人的躯干)的动力学与通过接触包括机器人和自由体的系统的配置之间提供了光滑关系。
10.在本公开中，机器人和环境的接触几何形状由用户考虑任务来定义。例如，机器人上的抓取器或连杆、待操纵对象的表面或在行动的情况下地板。此外，几何形状由用户配对，使得使用一个或更多个接触对，可完成给定任务。例如，机器人的抓取器和对象的所有表面可配对以用于非抓握操纵应用；或者机器人的脚和地板可配对以用于行动应用。此外，各个接触对被指派给自由体和标称虚拟力方向，其将基于系统的配置而旋转。例如，机器人的抓取器与对象的表面之间的接触对可生成在垂直于对象的接触面的方向上作用于对象的质心上的虚拟力。例如，使用包括对象的正面(即，面向机器人)的一对可在对象上生成向前推动的虚拟力，而使用右面可在对象的质心处生成向左推动的虚拟力。在行动的情况下，使用人形机器人的脚和地板的一对可在躯干上生成通过将接触点处垂直于地板的虚拟力投影到躯干的质心上而计算的虚拟力。
11.另外，在一些实施方式中，虚拟力的大小被表示为接触对中的接触几何形状之间的距离的函数。虚拟力在优化处理期间被惩罚并逐渐减小，使得在优化结束时，虚拟力不再存在。随着优化收敛，这产生仅使用物理接触来求解任务的机器人运动。以这种单独的松弛来发现接触(即，虚拟力)允许仅使虚拟力最小化，而没有考虑摩擦刚体接触作为决策变量。因此，这种表示允许松弛接触模型仅利用一个新的独立参数(松弛的惩罚)来注释作用在自由体上的物理力。
12.因此，一些实施方式在摩擦刚体接触的欠致动动力学中利用松弛接触模型来描述机器人操纵和行动运动，并且利用至少两个目标项(即，通过机器人和虚拟力移动的对象的姿态和虚拟力的大小)上的多目标优化来替换用于机器人控制的轨迹的一般确定。具体地，多目标优化使成本函数最小化，以生成惩罚对象的目标姿态与对象的最终姿态之间的差异的轨迹，该对象通过机器人沿着针对摩擦刚体接触力学的欠致动动力学和松弛接触模型估计的轨迹移动，同时惩罚虚拟力而放置。
13.一些实施方式通过适当地调整与目标姿态的姿态偏差和虚拟力的惩罚来执行优化，使得虚拟力和姿态偏差二者在优化结束时收敛到零，而无需改变惩罚值。这些实施方式需要针对各个任务重新调整。一些实施方式基于这样的认识：当从先前迭代得到的轨迹在调节惩罚的同时将当前迭代初始化时，可迭代地执行优化。另外，对于每次迭代，虚拟力的大小减小。以这种方式，在没有优化的情况下处理具有较大虚拟力的先前轨迹，以在每次迭
代中减小虚拟力并改进轨迹。在一些实现方式中，执行迭代，直至满足终止条件，例如虚拟力达到零或优化达到预定迭代次数。
14.因此，一个实施方式公开了一种被配置用于在环境中执行涉及使对象从对象的初始姿态移动到对象的目标姿态的任务的机器人，该机器人包括输入接口，其被配置为接受机器人与环境之间的接触交互。该机器人还包括存储器，其被配置为存储表示机器人和环境的几何性质、动力学性质和摩擦性质中的一个或更多个的动力学模型以及经由通过与机器人上的几何形状和对象上的几何形状关联的一个或更多个接触对生成的虚拟力表示机器人与对象之间的动力学交互的松弛接触模型，其中在各个接触对中以一定距离作用在对象上的虚拟力与虚拟力的刚度成比例。该机器人还包括处理器，其被配置为通过执行优化来迭代地确定用于控制机器人的关联控制命令、轨迹以及使对象根据轨迹移动的虚拟刚度值，直至满足终止条件，所述优化减小虚拟力的刚度并且减小所述对象的目标姿态与经由根据松弛接触模型生成的虚拟力通过根据控制命令控制的机器人从初始姿态移动的所述对象的最终姿态之间的差异。
15.为了执行至少一次迭代，处理器被配置为：通过求解优化问题来针对虚拟力刚度的当前惩罚值确定当前轨迹、当前控制命令和当前虚拟刚度值，该优化问题利用在先前迭代期间以虚拟力刚度的先前惩罚值确定的先前轨迹和先前控制命令初始化；更新当前轨迹和当前控制命令以减小各个接触对中的距离，以生成更新的轨迹和更新的控制命令以在下一迭代中将优化问题初始化；并且更新虚拟力刚度的当前值以用于下一迭代中的优化。该机器人还包括致动器，其被配置为使机器人的机器人臂根据轨迹和关联控制命令移动。
16.另一实施方式公开了一种由机器人执行涉及使对象从对象的初始姿态移动到对象的目标姿态的任务的方法，其中该方法使用与实现该方法的指令联接的处理器，其中这些指令被存储在存储器中。存储器存储表示机器人和环境的几何性质、动力学性质和摩擦性质中的一个或更多个的动力学模型以及经由通过与机器人上的几何形状和对象上的几何形状关联的一个或更多个接触对生成的虚拟力表示机器人与对象之间的动力学交互的松弛接触模型，其中在各个接触对中以一定距离作用在对象上的虚拟力与虚拟力的刚度成比例。这些指令在由处理器执行时执行该方法的步骤，所述方法包括：获得机器人与对象之间的交互的当前状态；以及通过执行优化来迭代地确定用于控制机器人的关联控制命令、轨迹以及使对象根据轨迹移动的虚拟刚度值，直至满足终止条件，所述优化减小虚拟力的刚度并且减小所述对象的目标姿态与经由根据松弛接触模型生成的虚拟力通过根据控制命令控制的机器人从初始姿态移动的所述对象的最终姿态之间的差异。
17.此外，为了执行至少一次迭代，该方法还包括：通过求解优化问题来针对虚拟力刚度的当前惩罚值确定当前轨迹、当前控制命令和当前虚拟刚度值，该优化问题利用在先前迭代期间以虚拟力刚度的先前惩罚值确定的先前轨迹和先前控制命令初始化；更新当前轨迹和当前控制命令以减小各个虚拟主动接触对中的距离，以生成更新的轨迹和更新的控制命令以在下一迭代中将优化问题初始化；更新虚拟力刚度的当前值以用于下一迭代中的优化；以及使机器人的机器人臂根据轨迹和关联控制命令移动。
18.作为本公开的示例性实施方式的非限制性示例，在随后的详细描述中参照所述多个附图进一步描述本公开，其中贯穿附图的多个视图，相似的标号表示相似的部分。所示的附图未必按比例，重点通常放在示出当前公开的实施方式的原理。
附图说明
19.[图1a]图1a示出根据本公开的实施方式的机器人执行涉及使对象从对象的初始姿态移动到对象的目标姿态的任务的环境。
[0020]
[图1b]图1b示出根据本公开的实施方式的机器人的框图。
[0021]
[图1c]图1c示出根据一些实施方式的作为牵引控制器的执行结果，距离的变化。
[0022]
[图2a]图2a示出根据本公开的实施方式的当当前轨迹不满足姿态约束时惩罚循环算法执行的步骤。
[0023]
[图2b]图2b示出根据本公开的实施方式的当当前轨迹满足姿态约束时惩罚算法执行的步骤。
[0024]
[图2c]图2c示出根据本公开的实施方式的当更新的轨迹不满足姿态约束时惩罚算法执行的步骤。
[0025]
[图2d]图2d示出根据本公开的实施方式的在后处理期间执行的步骤。
[0026]
[图2e]图2e示出根据本公开的实施方式的由机器人执行的用于执行涉及使对象从对象的初始姿态移动到对象的目标姿态的任务的方法的步骤。
[0027]
[图3a]图3a示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制1自由度(dof)推杆-滑块系统。
[0028]
[图3b]图3b示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制7-dof机器人。
[0029]
[图3c]图3c示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制具有圆柱形完整基座的移动机器人。
[0030]
[图3d]图3d示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制具有棱柱形躯干和圆柱形臂和腿的2-dof的类人机器人。
[0031]
尽管上述附图阐述了当前公开的实施方式，但是如讨论中指出的，也可以想到其它实施方式。本公开作为表示而非限制呈现了例示性实施方式。本领域技术人员可设计出落在当前公开的实施方式的原理的范围和精神内的众多其它修改和实施方式。
具体实施方式
[0032]
在以下描述中，为了说明，阐述了众多具体细节以便提供本公开的彻底理解。然而，对于本领域技术人员而言将显而易见的是，本公开可在没有这些具体细节的情况下实践。在其它情况下，设备和方法仅以框图形式示出，以避免使本公开模糊。
[0033]
如本说明书和权利要求中使用的，术语“例如”和“诸如”以及动词“包括”、“具有”、“包含”及其其它动词形式在结合一个或更多个组件或其它项目的列表使用时各自应被解释为开放式，意味着列表不应被视为排除其它附加组件或项目。术语“基于”意指至少部分地基于。此外，将理解，本文所采用的措辞和术语是为了描述目的，不应被视为限制。此描述内利用的任何标题仅是为了方便，不具有法律或限制作用。
[0034]
图1a示出根据本公开的实施方式的机器人101执行涉及使对象107从对象107的初始姿态移动到对象107的目标姿态113的任务的环境100。此外，图1b示出根据本公开的实施方式的机器人101的框图。参照图1a结合图1b详细说明机器人101所执行的任务的执行。如
图1a所示，机器人101包括机器人臂103，其用于执行非抓握任务，例如将对象107从对象的初始姿态推动到目标姿态113。目标姿态113可以是用户想要移动对象107的预期姿态。在一些实施方式中，机器人101可执行抓握任务，例如抓取对象107以执行诸如使对象107移动的任务。为此，机器人101包括机器人机构103上的末端执行器105，其被致动以沿着轨迹111移动以接触对象107的表面以在对象上施加虚拟力，以便使对象107从初始姿态移动到目标姿态113。对象107具有质心(com)109。此外，在这种情况下在机器人末端执行器105与环境100中的对象107的表面上的四个接触候选107a、107b、107c或107d中的至少一个之间存在四个接触对。各个接触对具有距离和与其关联的刚度(k)。
[0035]
此外，机器人101的移动性或机器人101的自由度(dof)的数量被定义为指定机器人101的所有连杆(例如机器人臂103、机器人末端执行器105)在空间中的位置所需的独立关节变量的数量。其等于为控制机器人101而致动的关节的最小数量。可图1a中观察到的，机器人101具有两个自由度(dof)，并且这两个dof均被致动。此外，如图1b中观察到的，机器人101包括输入接口115，其被配置为接受机器人101与对象107之间的接触交互。输入接口115可包括接近传感器等。输入接口115可通过总线121连接到机器人101的其它组件(例如处理器117、存储部119等)。处理器117被配置为执行存储在存储部119中的存储指令。处理器117可以是单核处理器、多核处理器、计算集群或任何数量的其它配置。存储部119可以是随机存取存储器(ram)、只读存储器(rom)、闪存或任何其它合适的存储器系统。处理器117可通过总线121连接到机器人101的其它组件。
[0036]
一些实施方式基于这样的认识：控制机器人101执行抓握或非抓握任务的挑战是缺乏使用各种优化技术来确定机器人末端执行器105的合适轨迹(例如，轨迹111)以实现期望控制的能力。对于机器人操纵，物理接触表现为冲击，因此引入了非光滑动力学，继而阻碍了基于梯度的求解器的利用。一些实施方式基于这样的认识：可通过测试众多轨迹来确定轨迹。然而，轨迹的这种生成计算上低效，并且可能得不到最优结果。
[0037]
为了避免这些结果，在一些实施方式中，机器人101的存储部119被配置为存储机器人101和环境100的动力学模型143。动力学模型143表示机器人101和环境100的几何性质、动力学性质和摩擦性质。存储部还被配置为存储经由与机器人末端执行器105和环境100中的表面(107a、107b、107c或107d)关联的一个或更多个接触对处生成的虚拟力，机器人101与对象107之间的交互的动力学的松弛接触模型123。虚拟力由各个接触对中在距离处作用在对象107上的虚拟力表示，其中虚拟力与刚度成比例。松弛接触模型123将机器人101以及由机器人101通过从一定距离(即，没有物理接触)作用的虚拟力来移动的对象的配置联系起来，同时允许在使用这种模型控制机器人101时使用优化技术。另外，松弛接触模型123添加最少数量的附加参数。松弛接触模型123具有允许有效控制计算的结构，得到准确的控制轨迹，此外该结构对机器人控制的初始化不敏感。
[0038]
此外，处理器117被配置为通过执行减小虚拟力刚度的优化来确定轨迹111和关联控制命令以用于控制机器人101使对象107根据轨迹111移动到目标姿态113。此外，该优化减小了对象107的目标姿态113与经由根据松弛接触模型123生成的虚拟力通过根据控制命令控制的机器人101从初始姿态移动的对象107的最终姿态之间的差异。对象107的最终姿态可以是对象107早前(例如，在先前迭代中)移动的姿态，其中最终姿态仍远离目标姿态113。机器人101被配置为使用优化来减小对象107的目标姿态113与最终姿态之间的这种差
异。
[0039]
为了减小虚拟力的刚度以及对象107的目标姿态113与通过机器人101从初始姿态移动的对象107的最终姿态之间的差异，处理器117可进一步被配置为执行成本函数的多目标优化。多目标优化旨在通过在至少两个参数上执行优化来实现多个竞争目标。根据实施方式，多目标优化/减小了刚度和对象107的目标姿态113与最终姿态之间的差异。此外，成本函数是确定通过机器人101移动的对象107的最终姿态相对于对象107的目标姿态113的定位误差的第一成本和确定虚拟力的累积刚度的第二成本的组合。
[0040]
此外，处理器117可被配置为迭代地确定轨迹111，直至满足终止条件139。为此，分析每一次迭代中的轨迹以检查轨迹是否满足姿态约束131和终止条件139。满足姿态约束131和终止条件139的轨迹可用作机器人101移动机器人臂103以使对象107移动的轨迹111。在迭代次数大于第一阈值或者虚拟力减小至零的情况下，可满足终止条件139。第一阈值可由机器人101基于使对象107从初始姿态移动到目标姿态113可能所需的可能迭代次数、距离等确定。在示例实施方式中，第一阈值可由用户手动定义。
[0041]
为了执行至少一次迭代，处理器117还被配置为通过求解利用先前轨迹和先前控制命令初始化的优化问题来针对虚拟力刚度的当前值确定当前轨迹和当前控制命令。该先前轨迹和先前控制命令在先前迭代期间以虚拟力刚度的先前值确定。优化问题聚焦于优化当前轨迹以确定最优轨迹(例如，轨迹111)，使得机器人101使对象107从初始姿态或初始姿态和目标姿态113之间的姿态(在对象107已从初始姿态移动，但是还未到达目标姿态113的情况下)移动到目标姿态113。在存在接触的情况下轨迹优化的概念可被表述为给定高级任务，寻找接触位置、定时和力和机器人控制输入。
[0042]
处理器117还被配置为更新当前轨迹和当前控制命令以减小各个接触对(即，机器人末端执行器105与对象107的表面107a、107b、107c或107d中的至少一个表面之间的接触)中的距离，以生成更新的轨迹和更新的控制命令以在下一迭代中将优化问题初始化，并且更新虚拟力刚度的当前值以用于下一迭代中的优化。
[0043]
机器人101包括致动器125，其被配置为使机器人101的机器人臂103根据轨迹111和关联控制命令移动。致动器125经由总线121与处理器117和机器人臂103通信。
[0044]
在实施方式中，根据松弛接触模型123生成的虚拟力对应于机器人末端执行器105与至少一个表面107a、107b、107c或107d之间的四个接触对中的至少一个接触对。虚拟力可基于虚拟刚度、与虚拟力关联的曲率以及与接触对关联的机器人末端执行器105与对象107的表面107a、107b、107c和107d之间的带符号距离中的一个或更多个。在交互期间的各个时刻，虚拟力指向对象107上的接触面法线在对象107的com上的投影。
[0045]
一些实施方式基于这样的认识：向轨迹优化问题中引入接触导致非光滑动力学，因此阻碍了在各种机器人操纵和行动任务中使用基于梯度的优化方法。为了解决这一问题，在一些实施方式中，本公开中的机器人101可使用由处理器117实现的松弛接触模型123来使用松弛接触模型123和轨迹优化模块127生成接触交互轨迹111。
[0046]
一些实施方式基于这样的认识：为了求解具有可靠收敛特性的轨迹优化问题，可使用连续凸化(scvx)算法，其是特殊类型的顺序二次规划。在此方法中，通过将关于先前轨迹的动力学约束线性化来获得原始优化问题的凸近似，并且在信任区域内求解凸子问题。基于凸近似与实际动力学的相似度来调节信任区域的半径。在一些实施方式中，机器人101
可使用轨迹优化模块127中的scvx算法高效地计算轨迹111。
[0047]
一些实施方式基于这样的认识：为了确定优化问题的解，可使用光滑接触模型。在光滑模型中，接触力是距离的函数，从而可规划机器人101的动力学运动。光滑模型方便确定最优轨迹111所需的迭代的收敛。然而，光滑模型导致物理上不准确并且很难调整。
[0048]
为了解决这一问题，在一些实施方式中，本公开中的机器人101可被配置为使用可变光滑接触模型(vscm)，其中利用作用在一定距离处的虚拟力来发现接触，而物理引擎用于对现有接触力学进行仿真。贯穿优化使虚拟力最小化。因此，在维持快速收敛的同时获得物理上准确的运动。在这些实施方式中，松弛接触模型123对应于vscm。连同scvx一起使用vscm通过将调整参数的数量减少为一(即，虚拟刚度的惩罚)而显著减轻了对轨迹初始猜测的敏感性和调整负担。然而，当任务或机器人改变时，使用vscm和scvx的机器人101仍可能需要重新调整松弛惩罚；在没有额外调整的情况下，即使微小的任务修改也会在规划的运动中发生突变。此外，由于接触模型的结构，所得接触通常是冲击的。
[0049]
为了解决这一问题，在一些实施方式中，机器人101可包括惩罚循环模块129，其针对与轨迹111的确定关联的至少一次迭代实现特定惩罚循环算法。在该特定惩罚循环算法中，基于姿态约束131迭代地改变松弛接触模型123所包括的松弛参数(例如，与虚拟力关联的虚拟刚度)的惩罚。
[0050]
为此，处理器117被配置为执行惩罚循环模块129以向与虚拟力关联的虚拟刚度指派第一惩罚值作为更新的惩罚值，其中如果满足姿态约束131，则所指派的惩罚值大于在先前迭代中指派的惩罚值。另一方面，如果不满足姿态约束131，则向与虚拟力关联的虚拟刚度指派第二惩罚值作为更新的惩罚值，其中所指派的惩罚值小于在先前迭代中指派的惩罚值。姿态约束131包括与轨迹111关联的关于位置误差和取向误差的信息。更具体地，如果位置误差的值低于阈值并且取向误差的值低于阈值(例如，归一化位置误差低于30％，并且取向误差低于1rad)，则满足姿态约束131。
[0051]
此外，处理器117确定满足姿态约束131的当前轨迹、关联控制命令和虚拟刚度值以及指示用于执行任务的物理力的位置、定时和大小的残余虚拟刚度。
[0052]
一些实施方式基于这样的认识：惩罚循环模块129所计算的与当前轨迹关联的平均刚度可通过接触对象107的机器人臂103在对象107上产生冲击接触力。对象107上的这种冲击接触力可能不可取地使对象107位移。为了解决这一问题，在一些实施方式中，机器人101使用后处理模块133。对当前轨迹执行后处理以使用牵引控制器135将机器人上的几何形状吸引到环境中的对应几何形状上，以方便物理接触。为此，处理器117被配置为利用与指示完成任务所需的力的位置、定时和大小的残余虚拟刚度变量关联的信息。当当前轨迹的虚拟刚度的平均值大于虚拟刚度阈值时，执行牵引控制器135。
[0053]
图1c示出根据一些实施方式的作为牵引控制器135的执行结果，距离的变化。处理器117使用与残余虚拟刚度变量关联的信息来在当前轨迹上执行牵引控制器135以确定牵引力牵引力将与非零虚拟刚度值关联的机器人101上的虚拟主动接触几何形状朝着对象107上的对应接触几何形状吸引。例如，末端执行器105和接触候选107a之间的距离改变为距离小于距离为此，牵引力由下式给出
[0054]
[0055]
其中是从机器人上的接触候选(例如，末端执行器105)的几何中心到环境中的接触候选(例如，接触候选107a)的几何中心的距离向量，并且k是当前轨迹中的虚拟刚度值。
[0056]
为此，在施加牵引力之后，距离变小。然而，虚拟刚度值k可能导致过大的虚拟力，因为法向虚拟力的大小与距离成反比，即，为了避免这些过大的虚拟力，通过爬山搜索减小刚度值，使得满足任务约束。为此，处理器117被配置为使用由爬山搜索(hcs)模块137实现的爬山搜索。只要定位误差减小，爬山搜索就逐个减小非零刚度值。在实施方式中，爬山搜索使用固定步长。在另选实施方式中，爬山搜索使用自适应步长。牵引控制器135和hcs模块137包括后处理模块133。因此，后处理包括执行牵引控制器和爬山搜索。根据实施方式，后处理输出满足姿态约束的关联控制命令和轨迹，以用于执行任务。因此，后处理减少了确定轨迹的迭代次数。换言之，后处理改进了收敛。此外，由后处理得到的轨迹好于当前轨迹，因为其方便了物理接触并且明确减小了虚拟刚度值，而不会使任务性能劣化。
[0057]
在一些实施方式中，机器人101还包括阻尼控制器141。处理器117被配置为执行阻尼控制器141以防止牵引控制器135针对大的刚度值生成突然运动。
[0058]
一些实施方式基于这样的认识：通过调整权重(或惩罚值)，随着优化收敛，虚拟力消失，从而仅使用物理接触产生解决任务的运动。在这些实施方式中，在通过调整惩罚值减小虚拟力的过程中可应用虚拟刚度的惩罚，其中由于剩余虚拟力，小的惩罚值可能导致物理上不一致的运动。此外，如果惩罚值太大，则可能找不到完成任务的运动。尽管这种惩罚的调整相当直接，但是其阻碍了该方法广义化以用于广泛的任务和机器人。为了解决这一问题，在本公开的一些实施方式中利用自动调节惩罚的惩罚循环算法。此外，在每次迭代之后通过上述后处理阶段改进所确定的轨迹、关联控制命令和虚拟刚度值。
[0059]
图2a示出根据本公开的实施方式的当当前轨迹不满足姿态约束131时通过惩罚循环算法执行的步骤。在一些实施方式中，处理器117可被配置为通过惩罚循环算法执行步骤。
[0060]
在步骤201，可获得表示机器人101的状态的初始状态向量、调整松弛接触模型123所包括的松弛参数(例如，虚拟刚度)的初始惩罚值以及要优化以获得以零虚拟力使对象107移动的最优轨迹111的初始控制轨迹。
[0061]
在步骤203，可基于初始状态向量、惩罚和控制轨迹的初始化值来执行轨迹优化模块127，以确定当前轨迹、当前控制命令和当前虚拟刚度值。连续凸化算法可显著减轻对初始猜测的敏感度，并且可变光滑接触模型可将调整参数的数量减少至一个，即，虚拟刚度惩罚。
[0062]
在步骤205，可评估性能参数，其中性能参数可包括位置误差、取向误差、最大刚度值k
max
和平均刚度值k
avg
。姿态约束131包括位置误差和取向误差。
[0063]
在步骤207，可检查当前轨迹是否满足姿态约束131。当所确定的轨迹满足姿态约束131时，控制转到步骤211。另一方面，当当前轨迹不满足姿态约束131时，控制转到步骤209。
[0064]
在步骤209，指派给松弛参数的惩罚值可减小先前改变的一半，并且这些值可被反馈给步骤203，其中利用这些值执行轨迹优化模块127以在步骤207确定下一迭代中满足姿
态约束131的新的优化轨迹。
[0065]
图2b示出根据本公开的实施方式的当当前轨迹满足姿态约束131时通过惩罚算法执行的步骤。
[0066]
在步骤211，当所确定的轨迹满足姿态约束131时，可检查平均刚度值k
avg
是否小于阈值刚度值k
threshold
。在确定平均刚度值k
avg
小于k
threshold
时，控制转到步骤213。否则，控制转到步骤223。
[0067]
在步骤213，基于平均刚度值k
avg
小于阈值刚度值k
threshold
，可对所确定的轨迹执行后处理。后处理步骤通过利用基于惩罚值暗示的接触信息调节先前迭代中的松弛参数(例如，与虚拟力关联的虚拟刚度)来改进当前轨迹和关联的当前控制命令。此外，后处理步骤使用牵引控制器135将与非零刚度值关联的机器人连杆(或机器人臂103的机器人末端执行器105)朝着环境100中的对应接触候选吸引。因此，执行后处理213产生更新的轨迹、更新的控制命令和更新的虚拟刚度值。
[0068]
在步骤215，可确定在后处理步骤213期间更新的轨迹、关联控制命令和虚拟刚度值是否满足姿态约束131。在满足姿态约束131的情况下，控制转到步骤217。否则，控制转到步骤223。
[0069]
在步骤217，可确定是否满足终止条件139。在不满足终止条件139的情况下，控制转到步骤219。否则，控制转到步骤221。在迭代次数大于第一阈值或者虚拟刚度值减小至零的情况下，可满足终止条件139。
[0070]
在步骤219，当不满足终止条件139时，惩罚值可增加固定步长。此外，控制转到步骤203。
[0071]
在步骤221，可根据当前轨迹和关联的当前控制命令以及当前虚拟刚度值来控制机器人101。在一些实施方式中，根据轨迹和关联控制命令来控制致动器125以使机器人臂103移动。在执行步骤219之后，惩罚算法的步骤的执行结束。
[0072]
图2c示出根据本公开的实施方式的当更新的轨迹不满足姿态约束131时通过惩罚算法执行的步骤。
[0073]
在后处理之后(在步骤215)获得的更新的轨迹不满足姿态约束131的情况下，控制转到步骤223。
[0074]
在步骤223，先前迭代轨迹和关联控制命令可用作最优解。此外，控制转到步骤217。
[0075]
在步骤217，可确定是否满足终止条件139。在不满足终止条件139的情况下，控制转到步骤219，否则，控制转到步骤221。
[0076]
在步骤219，当不满足终止条件139时，惩罚值可增加固定步长。此外，控制转到步骤203。
[0077]
在步骤221，可根据当前轨迹和关联的当前控制命令来控制机器人101。在一些实施方式中，根据轨迹和关联控制命令来控制致动器125以使机器人臂103移动。在执行步骤219之后，惩罚算法的步骤的执行结束。
[0078]
图2d示出根据本公开的实施方式的在后处理期间执行的步骤。当对于通过惩罚算法确定的当前轨迹，平均刚度值小于根据当前轨迹使机器人臂103移动所需的阈值刚度值时，执行后处理。在获得当前轨迹、关联的当前控制命令和当前虚拟刚度值之后，方法在步
骤225开始。
[0079]
在步骤225，可执行牵引控制器135以将虚拟主动机器人末端执行器105向环境100中的对象107上的候选107a、107b、107c和107d当中的对应接触候选吸引，以方便物理接触。基于当前轨迹、当前控制命令和当前虚拟刚度值执行牵引控制器135。此外，随着机器人末端执行器105与接触候选107a、107b、107c和107d之间的距离变小，为执行当前轨迹而考虑的刚度值可能导致过大的虚拟力。为了克服这种情况，控制转到步骤227。
[0080]
在步骤227，可执行爬山搜索(hcs)操作。只要非线性姿态误差减小，hcs操作通过归一化最终成本的变化除以先前变化而导致非零刚度值减小。减小非零刚度值导致明确地抑制虚拟力。因此，通过后处理改进当前轨迹、关联控制命令和虚拟刚度值以生成更新的轨迹、更新的控制命令和更新的虚拟刚度值。在惩罚循环算法中进一步分析更新的轨迹，以检查更新的轨迹是否满足姿态约束131。
[0081]
在一些实施方式中，后处理用于明确地通过利用松弛的利用所暗示的接触信息来改进通过惩罚循环算法确定的输出轨迹和关联控制命令。例如，对于接触对p和控制周期i，从距离向量d(p,i)∈r3和关联的虚拟刚度值k(p,i)计算牵引力f(p,i)∈r3，其中牵引力被给出为：
[0082]
f(p,i)＝k(p,i)d(p,i)，(1)
[0083]
其中，d是从机器人101上的接触候选(即，末端执行器105)的质心到环境100中相对于接触候选的中心偏移的点的向量。在示例实施方式中，该偏移可对于第一次惩罚迭代任意初始化为5cm，对于后续迭代通过将初始偏移除以成功惩罚迭代次数来获得。该偏移有助于到达环境100中的遮挡表面。在另一实施方式中，势场方法可与在零刚度值的情况下表面107a、107b、107c和107d上的斥力一起使用。
[0084]
对应广义关节力向量可通过下式计算：
[0085][0086]
其中是机器人101上的接触候选的质心的平移雅可比矩阵。
[0087]
为了防止牵引力对于大的刚度值生成突然运动，可应用阻尼控制器141以保持关节速度接近规划的运动，其中阻尼力可被给出为：
[0088][0089]
其中是正有限增益矩阵，是关节速度与规划的速度的偏差，是阻尼的广义关节力。在一些实施方式中，该计算可使用惯性矩阵的稀疏形式高效地进行。
[0090]
一些实施方式基于这样的认识：松弛接触模型123可用于实现接触隐式轨迹优化框架，该框架可使用不重要的初始猜测为不同的机器人架构和任务规划接触交互轨迹(例如，轨迹111)，而无需任何参数调整。服从通过松弛接触模型123生成的虚拟力和由于物理世界中的摩擦刚体接触力学而引起的外力二者的具有na致动dof和nu未致动dof的欠致动系统的动力学的数学表示可如下给出：
[0091]
[0092]
其中是配置向量；是质量矩阵；表示科里奥利、离心和重力项；是对致动dof的选择矩阵，是对未致动dof的选择矩阵；是广义关节力的向量；是nc接触点处的广义接触力的向量，是将关节速度映射到接触点处的笛卡尔速度的雅可比矩阵，是通过接触模型生成的未致动dof的广义接触力的向量。
[0093]
在另一实施方式中，对于特殊欧几里得(se)组(例如se(3))中的nf个自由体(例如，自由对象或人形躯干)，nu＝6nf。系统的状态由表示，其中n＝2(na nu)。此系统中存在两种类型的接触力学：(i)由于仿真世界中的实际接触(即，由物理引擎检测到的接触)而引起的接触力，其在所有dof上均有效；以及(ii)由于接触模型而引起并且仅施加在未致动dof上的虚拟力。
[0094]
在另一实施方式中，广义关节力被分解为其中和是和λc的估计；并且是与关节力关联的控制变量的向量。这有助于将优化问题集中在关节力方面，这意味着即使存在外部接触，控制项也与加速度线性相关。
[0095]
一些实施方式基于这样的认识：接触隐式操纵用于将操纵任务(抓握任务或非抓握任务)定义为优化问题，其中作为轨迹优化的结果寻找接触时间表和对应力。接触模型的选择至关重要。
[0096]
在本公开中，松弛接触模型123用于方便基于梯度的求解器的收敛。松弛接触模型123考虑机器人101上(例如，末端执行器连杆105)和环境100中(例如，对象107的表面107a、107b、107c和107d)的n
p
对预定义的接触候选。对于各个接触对，垂直于表面的虚拟力的大小γ∈r使用接触候选之间的带符号距离φ∈r、虚拟刚度k和曲率α由γ(q)＝ke-αφ(q)
计算。作用在与接触对关联的自由体上的对应广义虚拟力λv∈r6由λv(q)＝γ(q)[i
3-i]
t
n(q)计算，其中i3是3
×
3单位矩阵，l是从自由体的质心到机器人101上的接触候选上的最近点的向量，是执行叉积的i的斜对称矩阵形式，n∈r3是接触面法线。作用在自由体上的净虚拟力是与该体关联的接触候选所对应的虚拟力之和。结果，虚拟力在自由体的动力学与系统的配置之间提供光滑关系。
[0097]
此外，在vscm中，虚拟刚度值是优化的决策变量因此，控制变量的向量为其中m＝na n
p
。
[0098]
一些实施方式基于这样的认识：轨迹优化可用于为给定高级任务确定最优轨迹111。为此，机器人101基于轨迹优化方法来确定轨迹111，其在满足与所确定的轨迹111关联的姿态约束131的同时使虚拟力最小化。n个时间步的有限维轨迹优化问题可依据状态轨迹和控制轨迹最终成本项cf和综合成本项ci；以及下控制边界u
l
、上控制边界uu、下状态边界x
l
和上状态边界xu来编写：
[0099][0100]
服从：
[0101]
x
i 1
＝f(xi,ui)，对于i＝1,
…
,n(5b)
[0102]ul
≤u
1,
…
,n
≤uu,x
l
≤x
1,
…
,n 1
≤xu(5c)
[0103]
其中x
i 1
＝f(xi,ui)描述非线性动力学在控制周期i上的演变。
[0104]
一些实施方式基于期望的躯干/对象配置定义行动和非抓握操纵任务。为此，使用加权二次最终成本，其基于自由体的位置和取向与期望的姿态的偏差pe和θe：
[0105][0106]
其中w1和w2是权重。为了抑制所有虚拟力，在综合成本中虚拟刚度变量的l1范数被惩罚：
[0107]ci
＝ω‖ki‖1(7)
[0108]
此外，通过包括与惩罚循环算法的步骤对应的指令的惩罚循环模块129调节惩罚ω。
[0109]
一些实施方式基于这样的认识：非凸性(或与轨迹关联的动力学的非线性)可源自目标函数、状态或控制约束或者非线性动力学。第一种情况通常易于管理，通过变量的变化将非凸性从目标转移到约束。对于第二种情况，需要在保证最优解的同时将非凸约束(状态或控制约束、非线性动力学)转换为凸约束。连续凸化(scvx)是通过迭代地创建和求解凸问题序列来求解具有非凸约束或动力学的最优控制问题的算法。下面描述此算法。
[0110]
scvx算法基于依次重复三个主要步骤：(i)将关于来自先前顺序的轨迹的非凸约束(例如，非线性动力学)线性化；(ii)求解所得凸子问题，服从避免由于线性化而引起的人为无界性的信任区域约束；以及(iii)基于线性近似的保真度调节信任区域半径。
[0111]
凸子问题由下式给出：
[0112][0113]
服从：
[0114]
对于i＝1,
…
,n,(8b)
[0115]
对于i＝1,
…
,n 1(8c)
[0116]
对于i＝1,
…
,n,(8d)
[0117]
‖δx‖1 ‖δu‖1≤rs(8e)其中(xs,us)是来自顺序s的轨迹；)是来自顺序s的轨迹；r是信任区域半径。另外，可向该问题添加虚拟控制以防止由于线性化而引起的人为不可行性。
[0118]
凸子问题是联立问题，因此具有较大的大小但结构稀疏，其可由合适的求解器利用。在求解凸子问题之后，仅应用控制的变化，而非应用状态和控制二者的变化。然后，通过展开动力学来重新计算状态轨迹。这种修改防止了在原始方法中使用惩罚方法时可能发生的缺陷(即，f(xi,ui)-x
i 1
)的累积，并且通过在我们的实验中允许较大的信任区域而改进了收敛速度。结果，修改的方法组合了直接方法的数值效率和打靶法的准确性。
[0119]
图2e示出根据本公开的实施方式的由机器人101执行的用于执行涉及使对象107从初始姿态移动到目标姿态113的任务的方法的步骤。该方法由机器人101的处理器117执行。该方法从步骤229开始。
[0120]
在步骤229，初始状态向量、惩罚值和控制轨迹可被初始化以确定最优状态和控制
轨迹。初始值可对应于控制轨迹的起始值。在一些实施方式中，可以为机器人101预定义初始值。在一些其它实施方式中，初始值可由用户手动提供。此外，经由输入接口115获得机器人101与对象107之间的交互的当前状态，以确定完整轨迹。
[0121]
在步骤231，可执行scvx算法以按照数值高效方式求解由于非线性动力学而非凸的轨迹优化问题。
[0122]
在步骤233，可评估与轨迹关联的性能测量参数(例如，位置误差、取向误差、平均刚度值和最大刚度值)。姿态约束131包括位置误差和取向误差。性能测量参数可用于优化轨迹，以便获得优化的轨迹111。为此，控制转到步骤235。
[0123]
在步骤235，可执行惩罚循环算法。执行惩罚循环以通过执行优化来迭代地确定对象107的轨迹111以及用于控制机器人101使对象107根据轨迹111移动的关联控制命令，直至满足终止条件139，该优化减小(例如，最小化)虚拟力的刚度并且减小(例如，最小化)对象107的目标姿态113与通过机器人101从初始姿态移动的对象107的最终姿态之间的差异。在迭代次数大于第一阈值或者虚拟力减小至零的情况下，可满足终止条件139。经由根据松弛接触模型123生成的虚拟力基于控制命令来控制机器人101。
[0124]
为此，基于所计算的轨迹是否满足姿态约束131的确定，向诸如虚拟刚度的松弛参数指派不同的惩罚。通过基于姿态约束131动态地改变松弛参数上的惩罚，惩罚算法逐渐将虚拟力减小至零，从而仅使用物理力执行任务。此外，确定平均刚度值是否小于使机器人臂103根据通过惩罚循环算法确定的轨迹移动所需的阈值刚度值。此外，控制转到步骤237。
[0125]
在步骤237，可对当前轨迹执行后处理，以使用牵引控制器135将与非零刚度值关联的机器人连杆(或机器人臂103的机器人末端执行器105)朝着环境100中的对应接触候选吸引。为此，处理器117被配置为利用与残余虚拟刚度变量关联的信息，残余虚拟刚度变量指示完成使对象从对象的初始姿态移动到对象的目标姿态113的任务所需的力的位置、定时和大小。此外，控制转到步骤239。
[0126]
在步骤239，基于在步骤237中使用后处理更新的轨迹是否满足终止条件139的确定，可确定最优轨迹和关联控制命令，其中在迭代次数大于第一阈值或者虚拟力减小至零的情况下，可满足终止条件139。在一些实施方式中，最优轨迹满足姿态约束131和终止条件139二者。
[0127]
在一些实施方式中，基于在惩罚循环算法中确定的轨迹包括小于阈值刚度值的平均刚度值并且轨迹满足终止条件139的确定，可确定最优轨迹。此外，机器人101可使用控制命令来使机器人末端执行器105沿着最优轨迹移动，以使对象107移动到目标姿态113。
[0128]
因此，对象107根据优化的轨迹从初始姿态移动到目标姿态113。
[0129]
在示例实施方式中，在如图3a、图3b、图3c和图3d所示的四个不同的机器人应用中实现使用惩罚循环方法和后处理的轨迹优化。
[0130]
图3a示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制1自由度(dof)推动滑块系统301。系统301被配置为以1秒(sec)的单个控制时间步执行推动任务。执行推动任务的系统301包括接触对，其包括推杆311的尖端309和滑块303的正面313。系统301可包括松弛接触模型123以确定优化的轨迹和关联控制命令。
[0131]
此外，系统301基于松弛接触模型123所确定的优化的轨迹和关联控制命令在单个方向(例如，向前方向305)上推动滑块303(20cm)以达到滑块(例如，箱303)的目标姿态307。
[0132]
图3b示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制7-dof机器人315。在示例实施方式中，7-dof机器人315可以是sawyer机器人。除了向前推动箱319之外，7-dof机器人315可执行侧面和对角推动。7-dof机器人315在箱319的侧面321a和321b与圆柱形末端执行器凸缘311之间具有四个接触对。在示例实施方式中，7-dof机器人315执行三个向前推动任务以使箱319移动。使用松弛接触模型123确定优化的轨迹和关联控制命令，以进行轻微运动或冲击运动以使箱319移出7-dof机器人315的工作空间。
[0133]
图3c示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制具有圆柱形完整基座323的移动机器人。在示例实施方式中，移动机器人323可以是具有用于接触环境的圆柱形完整基座的人类支持机器人(hsr)。
[0134]
为了使用hsr 323的速度控制完整基座327执行推动箱325的任务，通过松弛接触模型123为hsr 323确定优化的轨迹和控制命令。如图3c所示，在箱325的侧面与hsr 323的圆柱形基座327之间存在四个接触对。由于平移速度和旋转速度的界限是
±
2m/s和
±
2rad/s，所以使用5秒的较长仿真时间和0.5秒的较大控制采样周期来执行不同的任务。由hsr 323执行使箱325移动50cm的向前推动任务和两个对角推动任务。观察到，当使用物理引擎的默认摩擦系数(μ＝1)时，hsr 323严重依赖于摩擦力进行对角推动，这看起来不切实际。为了避免这一问题，使用μ＝0.1重复该任务。
[0135]
图3d示出根据本公开的示例实施方式的基于优化的轨迹和关联控制命令来控制具有棱柱形躯干和圆柱形臂331a、331b和腿331c、331d的2-dof类人机器人329。
[0136]
针对行动应用根据最优轨迹和关联控制命令来控制平面类人机器人329，其中类人机器人329可同时进行多个接触和断开多个接触。包括类人机器人329的环境具有零重力，这避免了稳定性约束。依据类人机器人329利用环境中的四块静止砖可达到的躯干期望姿态来指定任务，如图3d所示。然而，由于运动是无摩擦的，所以类人机器人329可使用接触来减速或停止。如图3d所示，存在作为砖的正面和背面的8个接触候选以及类人机器人329上作为臂和腿的末端连杆的4个接触候选。这些候选基于侧面进行配对，从而总共存在16个接触候选。不需要腿接触来完成任务，然而包括腿接触作为接触候选，以表明额外或不必要的接触对并不妨碍所提出的方法的性能。
[0137]
本文中概述的各种方法或处理可被编码为可在采用各种操作系统或平台中的任一种的一个或更多个处理器上执行的软件。另外，这种软件可使用多种合适的编程语言和/或编程或脚本工具中的任一种来编写，并且也可被编译为在框架或虚拟机上执行的可执行机器语言代码或中间代码。通常，在各种实施方式中，程序模块的功能可根据需要组合或分布。
[0138]
另外，本公开的实施方式可被具体实现为一种方法，已提供其示例。作为该方法的一部分执行的动作可按照任何合适的方式排序。因此，可构造以与所示不同的次序执行动作的实施方式，其可包括同时执行一些动作，尽管在例示性实施方式中作为顺序动作示出。此外，在权利要求中使用诸如“第一”、“第二”的序数词修饰权利要求元素本身并不暗示一个权利要求元素相比于另一权利要求元素的任何优先或次序或者方法动作执行的时间次序，而是仅用作标签以将具有特定名称的一个权利要求元素与具有相同名称(但使用序数词)的另一元素相区分，以区分权利要求元素。
[0139]
尽管参考特定优选实施方式描述了本公开，但是将理解，在本公开的精神和范围
内可进行各种其它调整和修改。因此，所附权利要求的方面涵盖落在本公开的真实精神和范围内的所有这些变化和修改。