分级的机器人控制系统和用多个操纵器对选择的物体自由度进行控制的方法

摘要

本发明涉及分级机器人控制系统和用多个操纵器对选择物体自由度进行控制的方法。具体地，提供了一种机器人系统，其包括控制器和具有操纵器的机器人，操纵器在主要任务期间使用多种抓握类型中的一种来抓握物体。控制器在主要任务期间使用多任务控制分级结构来控制操纵器，并且响应于输入信号自动将用于每种抓握类型的系统的内力参数化。使用诸如闭链变换将主要任务限定在控制的物体级，使得仅指定物体的选定自由度。用于机器人系统的控制系统具有主机和使用上述分级结构控制操纵器的算法。用于控制该系统的方法包括使用主机接收并处理输入信号，包括例如使用闭链限定将主要任务限定在控制的物体级，并将用于每种抓握类型的内力参数化。

说明书

关于联邦政府资助的研究或开发的声明

本发明是根据SAA-AT-07-003号NASA太空行动协议在政府资助下完成。政府可在本发明中享有一定权利。

相关申请的交叉引用

本申请要求2009年4月30日提交的美国临时专利申请No.61/174316的权益和优先权。

技术领域

本发明涉及用于控制具有多个关节和多自由度的一个或多个类人机器人的系统和方法。

背景技术

机器人是能够使用操纵器(例如，手、手指、拇指等)和一系列经由机器人关节相互连接的联接物来操纵物体的自动装置。典型机器人中的每个关节都代表至少一个独立的控制变量，即自由度(DOF)。末端执行器或操纵器用于执行手边的特定任务，例如，抓握工作工具或其他物体。因此，可通过任务说明的等级来组织机器人的精确运动控制：物体级控制(其描述了机器人在单次或协同抓握中对被抓握或保持的物体的行为进行控制的能力)、末端执行器控制和关节级控制。各种控制级共同实现了所要求的机器人运动性、灵活性和与工作任务相关的功能性。

类人机器人是一种特定类型的机器人，其无论整个身体、躯干和/或四肢都具有近似于人的结构或外观，类人机器人的结构复杂性很大程度上取决于所要执行的工作任务的性质。在需要与为人类使用而特制的设备或系统直接相互作用的情况下，可优选使用类人机器人。在需要与人相互作用的情况下，也可优选使用类人机器人，因为可以对运动进行编程以便接近人的运动，使得协同操作的人类伙伴能理解任务队列。

由于期待由类人机器人完成的工作任务范围很广，所以可能同时需要不同的控制模式。例如，在上述的不同控制空间内以及在对给定的电动机驱动式关节、关节运动和/或各种抓握类型所应用的转矩或力的控制上，都必须应用精确控制。在组装工作线中部署类人机器人需要能够与未组织的环境互动并且能够实施多样化的应用的能力。

发明内容

因此，本文提供了一种机器人控制系统和方法，用于通过下面将进行描述的控制框架来控制一个或多个机器人。对机器人的复杂控制，例如对具有多个DOF(诸如在一个具体实施例中超过42个DOF)的类人机器人的控制，可针对多个可独立运动的和可相互依赖运动的机器人关节以及物体末端执行器或操纵器来提供，或者关于同时对物体应用了协同抓握的多于一个的机器人的操纵器来提供。本文公开的框架基于多优先级任务，从而实质上是分级的。主要任务限定在控制的物体级，例如，使用“闭链(closed chain)”Jacobian变换和/或“闭链”抓握矩阵，下面会有详细说明。这提供了这样的任务，即只指令选择的物体自由度(DOF)，而允许其他DOF保持自由或不受约束。这又产生了一体的零空间，该零空间不仅包括每个独立的机器人操纵器(例如手、多个手指/拇指等)的冗余DOF，还包括物体在各种操纵器上共享的自由DOF。另一方面，次级任务可限定在控制的关节级，即，在关节空间中。这种多优先级的控制框架提供了用于协同组装应用的极大的功能性，特别是使用了本文所描述类型的高复杂度类人机器人时。

在本发明的范围内，控制器在多种机器人抓握类型期间提供了内力的自动参数化。作为示例，这种抓握类型可包括对物体的协同性双手抓握和协同性三手指抓握。这两者的可行性都将在本文中以数学的方式详细描述。

具体地，本文提供一种机器人系统，其包括控制器和一个或多个操纵器，该一个或多个操纵器可以是单个机器人的也可是多个机器人的，并且共同适于在主要任务执行期间使用多种抓握类型中的一种来抓握物体。控制器电连接至(一个或多个)机器人，并且在主要任务执行期间使用多任务控制分级结构来控制(一个或多个)操纵器。控制器响应于输入信号自动地将机器人系统的内力参数化，以便用于每种抓握类型，其中，主要任务被限定于物体级控制，例如，在一个实施例中使用闭链运动变换的方式来实施。

还提供了一种用于上述机器人系统的控制器。控制器包括电连接至(一个或多个)机器人的主机和由主机执行的算法。当被执行时，该算法适于使用多任务控制分级结构来控制多个操纵器。算法的执行自动地将机器人系统的内力参数化，以便用于(一个或多个)机器人的多种抓握类型中的每一种。

一种用于控制上述机器人系统的方法，包括通过主机接收输入信号，并使用主机通过多任务控制分级结构处理输入信号，从而在主要任务执行期间控制多个操纵器。处理输入信号包括：将主要任务限定在控制的物体级，以及响应于该输入信号自动地将机器人系统的内力参数化，以便用于多种抓握类型中的每一种。

本发明还提供了以下方案：

方案1.一种机器人系统，包括：

具有多个操纵器的机器人，所述多个操纵器共同适于在主要任务执行期间使用多种抓握类型中的一种来抓握物体；和

电连接至所述机器人的控制器，所述控制器适于在所述主要任务执行期间使用多任务控制分级结构来控制所述多个操纵器；

其中，所述控制器响应于输入信号自动地将所述机器人系统的内力参数化，以便用于所述多种抓握类型中的每一种，所述主要任务被限定在控制的物体级，具有仅选择所述物体所有可用自由度的子组的能力。

方案2.如方案1所述的机器人系统，其中，所述机器人是至少具有42个自由度的类人机器人。

方案3.如方案1所述的机器人系统，其中，所述主要任务在所述控制的物体级的限定包括使用“闭链”Jacobian变换和“闭链”抓握矩阵中的至少一个。

方案4.如方案1所述的机器人系统，其中，所述多任务控制分级结构利用了在所述控制的物体级的零空间中操作的阻抗关系。

方案5.如方案1所述的机器人系统，其中，所述控制器适于在所述机器人的协同抓握中使用所述多个操纵器中的至少一些来仅控制所述物体所有可用自由度的子组。

方案6.如方案5所述的机器人系统，其中，所述控制器还适于在所述控制的物体级执行所述零空间中的次级任务，所述零空间包括所述物体的至少一个自由的自由度。

方案7.一种用于机器人系统的控制器，所述机器人系统包括至少一个机器人，每个机器人均具有适于在主要任务执行期间抓握物体的至少一个操纵器，所述控制器包括：

电连接至所述至少一个机器人的主机；和

能够由所述主机执行的算法，所述算法适于使用多任务控制分级结构来控制所述至少一个机器人的所述至少一个操纵器；

其中，所述算法的执行响应于输入信号自动地将所述机器人系统的内力参数化，以便用于所述至少一个机器人的多种抓握类型中的每一种，所述主要任务被限定在物体级，具有仅选择所述物体所有可用自由度的子组的能力。

方案8.如方案7所述的控制器，其中，所述至少一个机器人包括具有至少42个自由度的类人机器人。

方案9.如方案7所述的控制器，其中，所述控制器适于在所述至少一个机器人的协同抓握中使用所述多个操纵器中的至少一些来仅控制所述物体所有自由度的子组，同时在所述控制的物体级执行所述零空间中的次级任务，所述零空间包括所述物体的至少一个自由的自由度。

方案10.如方案7所述的控制器，其中，所述主要任务在所述控制的物体级的限定使用“闭链”Jacobian变换和“闭链”抓握矩阵中的至少一个。

方案11.一种用于控制机器人系统的方法，所述机器人系统具有包含多个操纵器的机器人和电连接至所述机器人的控制器，所述多个操纵器共同适于在主要任务执行期间使用多种抓握类型中的一种来抓握物体，所述控制器适于在所述主要任务的执行期间控制所述多个操纵器，所述方法包括：

经由所述控制器的主机接收输入信号；

使用所述主机并经由多任务控制分级结构处理所述输入信号，从而在所述主要任务执行期间控制所述多个操纵器；

其中，处理所述输入信号包括：

将所述主要任务限定在所述控制的物体级；和

响应于所述输入信号自动将所述机器人系统的内力参数化，以便用于所述多种抓握类型中的每一种。

方案12.如方案11所述的方法，其中，所述多种抓握类型包括协同抓握类型。

方案13.如方案11所述的方法，其中，限定所述主要任务包括使用“闭链”Jacobian变换和“闭链”抓握矩阵中的至少一个。

方案14.如方案11所述的方法，其中，所述零空间包括所述机器人在所述控制的物体级的多个未被指定的自由度。

通过下面对实施本发明的最优模式的详细描述并结合附图，本发明的上述特征和优点以及其他特征和优点将变得相当明显。

附图说明

图1是根据本发明的具有机器人的机器人系统的示意图，通过使用分级的、多任务控制框架，该机器人是可控制的；和

图2是与可被诸如图1所示类型的机器人抓握的物体相关的多个力和坐标的示意图。

具体实施方式

参照附图，在所有几幅附图中，相似的附图标记指示相同或相似的部件，从图1开始，示出了一种机器人系统11，机器人系统11具有例如灵巧的类人机器人这样的机器人10，机器人10是通过控制系统或控制器(C)22可控的。虽然仅示出了一个机器人10，但系统11可如下面将描述的那样包括多于一个的机器人。控制器22电连接至机器人10，并且适于如下所述那样使用适于执行多任务控制分级结构的(一个或多个)算法100来控制机器人10的各种末端执行器或者物体操纵器。在这种控制分级结构中，阻抗(impedance)关系在某些实施例中可操作在位于控制的物体级的零空间中，但是该分级结构并不限于阻抗控制。响应于输入到控制器22的输入信号(箭头i_C)和/或由控制器产生或来自控制器外部的信号，控制器22使系统11的内力自动参数化，以便用于机器人10的多抓握类型。在一个实施例中，同样如下所述，闭链Jacobian运动变换或任务定义可用于将机器人10的主要任务限定在控制的物体级。

机器人10适于执行一个或多个具有多自由度(DOF)的自动化任务，并且适于执行其他交互任务或控制其它集成系统部件，例如，夹紧装置、照明设备、继电器等。根据一个实施例，机器人10设置为所示的类人机器人，在一个实施例中其可能具有超过42个DOF。机器人10具有多个可独立运动并且可相互依赖运动的操纵器，例如，手18、手指19、拇指21等，并且还包括多个机器人关节。关节可包括但是不必限于，肩关节(其位置大体上由箭头A指示)、肘关节(箭头B)、腕关节(箭头C)、颈关节(箭头D)和腰关节(箭头E)，以及位于每个机器人手指指骨之间的指关节(箭头F)。

每个机器人关节可具有一个或多个DOF。例如，某些柔顺关节(例如，肩关节(箭头A)和肘关节(箭头B))可具有至少两个俯仰和滚转形式的DOF。相同地，颈关节(箭头D)可具有至少三个DOF，并且腰部和腕部(分别为箭头E和C)可具有一个或多个DOF。如上所述，基于任务的复杂性，机器人10可以以超过42个DOF进行运动。每个机器人关节可包含一个或多个致动器并且可由该一个或多个致动器内部驱动，这些致动器例如是关节电动机、线性致动器、旋转致动器等等。

机器人10可包括类似于人的部件(诸如头部12、躯干14、腰15和手臂16)，以及某些操纵器(即，手18、手指19和拇指21)，其中上述的各种关节被置于这些部件内或置于这些部件之间。根据机器人的特定应用或期望用途，机器人10还可包括适于任务的固定装置或底座(未示出)，诸如腿、支撑面(tread)、或另一种可运动或固定的底座。电源13可一体地安装到机器人10以向各种关节提供充足的电能用于其运动，电源例如是携带或穿在躯干14后背上的可再充电电池组或其它适合的电源；或者电源可通过栓系电缆(tethering cable)来远程地附接。

控制器22提供对机器人10的精确运动控制，包括对通过上述操纵器操作物体20所需要的精细运动和粗略运动的控制。也就是说，物体20可被一个或多个手18的手指19和拇指21抓握。控制器22能够以与其它关节和系统部件隔离的方式对每个机器人关节和其它集成的系统部件独立地进行控制，还能相互依赖地控制数个关节以在执行相对复杂的工作任务时全面协调多个关节的动作。

仍然参照图1，控制器22可包括多个数字计算机或数据处理装置，每个数字计算机或数据处理装置都具有一个或多个微处理器或中央处理单元(CPU)、只读存储器(ROM)、随机访问存储器(RAM)，电可擦除可编程只读存储器(EEPROM)、高速时钟、模数转换(A/D)电路、数模转换(D/A)电路、和任何所需的输入/输出(I/O)电路和装置，以及信号调整和缓冲电子设备。由此，驻存在控制器22内的或可易于访问的单独控制算法可存储在ROM中并自动地在一个或多个不同的控制级上被执行，以提供相应的控制功能。

控制器22可包括服务器或主机17，服务器或主机17构造为分布的或中央的控制模块，并且具有以期望方式执行机器人10所有要求的控制功能所必需的控制模块和能力。另外，控制器22可被构造为通用数字计算机，其一般包括微处理器或中央处理单元、只读存储器(ROM)、随机访问存储器(RAM)、电可擦除可编程只读存储器(EEPROM)、高速时钟、模数转换(A/D)电路、数模转换(D/A)电路、和输入/输出电路和装置(I/O)，以及合适的信号调整和缓冲电路。由此，驻存在控制器22中的或可访问的任何算法(包括如下所述的用于执行分级的、基于阻抗的控制框架的算法100在内)可存储在ROM中并被执行以提供相应的功能。

控制器22可电连接至图形用户界面(GUI)24，GUI 24提供到控制器的直观访问。GUI 24能够向操作者或程序员提供范围广泛的主要和次级工作任务的控制访问，即，能够对机器人10的物体级、末端执行器级和/或关节空间级中的一个或多个中的运动进行控制的能力。GUI 24可以是简化且直观的，允许使用者利用简单图形或图标驱动的输入，从而通过输入输入信号(箭头i_C)来控制机器人10，例如，通过一个或多个前述的操纵器施加到物体20上的期望力或转矩，或者机器人的期望动作。

为了利用机器人10或多个机器人来执行多种操作任务，需要对(一个或多个)机器人进行范围宽广的功能控制。该功能包括混合的力/位置控制、带有多种协同抓握类型的物体级控制、末端执行器的笛卡尔空间控制(即，在XYZ笛卡尔坐标空间内的控制)、和关节空间操纵器控制，以及对多个控制任务的分级优先化。本发明提供了内力的参数化空间以控制这种协同抓握。在一个实施例中还提供了操作在物体20的零空间中的次级关节空间阻抗关系，正如下面数学地详述的那样。

阻抗规则：本文所述的控制框架的第一步是，描绘物体20的动力学行为特性，该物体仅受到机器人10的作用，或者由抓握相同的物体的两个或多个机器人作用。本节采用本文下面将要描述的被动动力学给出了闭环动力学。期望的闭环行为可由下面阻抗关系限定，即，方程式(1)：

$M_O\stackrel{··}{y}+B_O\stackrel{·}{y}+K_O\mathrm{\Delta y}=F-F^{*}$

$\stackrel{·}{y}\stackrel{·}{=}\left(\begin{array}{c}v\\ \omega \end{array}\right)$

在这个公式中，M_O、B_O和K_O分别是指定的惯性矩阵、阻尼矩阵和刚度矩阵，其中它们都∈R^6×6。v是物体20质心的线速度，并且ω是物体的角速度。两者都是相对于地面参照系来测量的。F和F^*表示物体上的实际外部力旋量(wrench)和期望外部力旋量的净量(net)。Δy是位置误差(y-y^*)。不失一般性，y的方向分量通过角-轴线表示法来表达，这将在下面的方程式(12)中示出。处于平衡状态时，该阻抗关系指定了内力F应为额定力F^*和弹簧弹力K_OΔy的和。如果某些方向上期望单纯的力控制，则可通过在K_O中将那些方向上的刚度设置为零来实现。将某些方向设置为单纯的力控制并且将F^*的互补分量设置为零，则在正交方向上得到力和运动控制的“混合”策略。

操纵器的冗余允许次级任务在物体阻抗的零空间中起作用。处于对所述次级任务的考虑，关节空间阻抗规则定义为下面的方程式(2)：

$M_j\stackrel{··}{q}+B_j\stackrel{·}{q}+K_j\mathrm{\Delta q}={\tau }_f$

在上述的方程式(2)中，M_j、B_j和K_j分别是用于关节空间的指定的惯性矩阵、阻尼矩阵和刚度矩阵。q是系统中用于所有操纵器的关节角度的列矩阵。Δq是关节位置误差。τ_f表示由作用在操纵器上的力所产生的关节转矩的列矩阵。这两个阻抗规则推导出下面用于控制器的任务目标：

${\stackrel{··}{y}}^{*}\stackrel{·}{=}{M}_O^{-1}(F-F^{*}-B_O\stackrel{·}{y}-K_O\mathrm{\Delta y})$

${\stackrel{··}{q}}_{\mathrm{ns}}^{*}\stackrel{·}{=}{M}_j^{-1}({\tau }_f-B_j\stackrel{·}{q}-K_j\mathrm{\Delta q})$

即方程式(3)，其中是期望物体加速度，是关于零空间(ns)的期望关节加速度。

开链运动学：参照图2，示出了物体20的受力图25和坐标系，其中N和B分别表示地参照系和主体参照系。r_i是从质心到接触点i的位置矢量，其中，i＝1，...，n。f_i和t_i分别表示来自点i的接触力和力矩。这种标准的运动学关系可用于定义刚体加速度，如下所示：

${\stackrel{·}{v}}_i=\stackrel{·}{v}+\stackrel{·}{\omega }\times r_i+\omega \times (\omega \times r_i)+2\omega \times v_{\mathrm{reli}}+a_{\mathrm{reli}}$

${\stackrel{·}{\omega }}_i=\stackrel{·}{\omega }+{\alpha }_{\mathrm{reli}}$

即，方程式(4)。v_reli和a_reli分别定义为物体框架中r_i的一阶导数和二阶导数，如方程式(5)所示：

$v_{\mathrm{reli}}\stackrel{·}{=}\frac{{}^{B}d}{\mathrm{dt}}{r}_i,$$a_{\mathrm{reli}}\stackrel{·}{=}\frac{{}^{B}d}{\mathrm{dt}}{v}_{\mathrm{reli}}$

这些关系能够以矩阵形式表示为熟知的抓握映射(mapping)。让x^.表示由接触所约束的末端执行器速度的列矩阵；其确切形式将稍后在下面描述。在这种定义下，加速度的映射遵循方程式(6)：

$\stackrel{··}{x}=G\stackrel{··}{y}+h$

G已知为抓握矩阵，提供了接触信息的映射。h是向心加速度、科氏(coriolus)加速度、和相对加速度的列矩阵。G和h的形式取决于抓握类型，下面将对其进行讨论。为将向下映射到操纵器空间，引入以下Jacobian矩阵。在下面的方程式(7)中分别定义了线性和旋转Jacobian矩阵J_vi和J_ωi：

$v_i=J_{\mathrm{vi}}\stackrel{·}{q},$${\omega }_i=J_{\mathrm{\omega i}}\stackrel{·}{q}$

将这些子矩阵堆叠到复合Jacobian矩阵J中，在时，方程式(6)中的抓握映射可表示为下面的在关节和物体加速度之间的变换，方程式(8)：

$J\stackrel{··}{q}+\stackrel{·}{J}\stackrel{·}{q}=G\stackrel{··}{y}+h$

抓握类型：在该变换中，J、G和h的结构取决于抓握类型。为了便于描述，我们将考虑两种抓握类型：双手抓握和三手指抓握。手抓握表示能够传递任意力和力矩的刚性接触。因此，手抓握约束了末端执行器的线性运动和角运动。手指接触表示只能传递力的无滑动的点接触。因此，手指接触只约束末端执行器的线性运动。据此，每种类型的矩阵分别表示如下：

双手抓握：$\stackrel{·}{x}=\left(\begin{array}{c}{v}_1\\ {\omega }_1\\ v_2\\ {\omega }_2\end{array}\right),$$J=\left(\begin{array}{c}{J}_{v1}\\ J_{\omega 1}\\ J_{v2}\\ J_{\omega 2}\end{array}\right),$$G=\left(\begin{array}{cc}{I}_3& -r_1^{\times }\\ 0& I_3\\ I_3& -r_2^{\times }\\ 0& I_3\end{array}\right),$$h=\left(\begin{array}{c}{\lambda }_1\\ {\alpha }_{\mathrm{rel}1}\\ {\lambda }_2\\ {\alpha }_{\mathrm{rel}2}\end{array}\right)---\left(9\right)$

三手指抓握：$\stackrel{·}{x}=\left(\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right),$$J=\left(\begin{array}{c}{J}_{v1}\\ J_{v2}\\ J_{v3}\end{array}\right),$$G=\left(\begin{array}{cc}{I}_3& -r_1^{\times }\\ I_3& -r_2^{\times }\\ I_3& -r_3^{\times }\end{array}\right),$$h=\left(\begin{array}{c}{\lambda }_1\\ {\lambda }_2\\ {\lambda }_3\end{array}\right)---\left(10\right)$

在这些方程式中，实践中，相对速度被认为是可忽略的，并且相对加速度将包括闭环伺服以调整内力。I_k表示k×k的单位矩阵，以及表示等同于r_i的叉积的斜对称矩阵，或者：

$r_i^{\times }\stackrel{·}{=}\left(\begin{array}{ccc}0& -r_{i_3}& r_{i_2}\\ r_{i_3}& 0& -r_{i_1}\\ -r_{i_2}& r_{i_1}& 0\end{array}\right)$

闭链运动学：本控制框架的下一步是将端点DOF向下映射到操纵器空间。为此目的，我们引入闭链Jacobian矩阵。这种变换限定了仅指定选择的物体DOF的任务。未被指定的DOF被并入主要任务的零空间中。这允许次级任务在空间中被优化，该空间不仅包括机器人10每个独立操纵器的冗余DOF，而且也包括物体在操纵器上共享的自由DOF。这也允许主要任务在扩展的工作区内操作。现在，由于物体20被限制在了多个工作区的联合部，所以它可提供相当大的控制优势。

为推导出该闭链Jacobian矩阵，考虑在末端执行器和物体20之间的运动约束。这些运动或完整约束提供了物体DOF和操纵器DOF之间的联接。在点接触中，这些约束仅施加到位置上，类似于球关节。在刚性接触中，假定没有发生滑动，则相同的约束施加到末端执行器的所有六个DOF上。在给定了完整的运动约束组(motion constraint)的情况下，那么可明确地消除物体20未被指定的DOF，以求出减少的且独立组的运动约束。这种技术产生了相对简单的结果，其不需要额外的实时运算以进行推导。

使表示物体的将被主要任务指定的pDOF。为此，可引入恒定的p×6矩阵S，其挑选出控制方向。完整的DOF组和减少的DOF组之间的关系及其逆满足：

$\stackrel{··}{z}=S\stackrel{··}{y}---\left(11\right)$

$\stackrel{··}{y}=S^{+}\stackrel{··}{z}+S^{\perp }\mu ---\left(12\right)$

此处，S⁺是S的伪逆，S^⊥是跨越S的零空间的6×(6-p)矩阵，并且μ∈R^6-p是任意的。方程式(8)中的变换表示物体和末端执行器或操纵器之间的完整的运动约束组，并且这些约束包括自由参数。为将所述组减少到最小的约束组，自由参数μ可被消除，以将自由参数变至任务的零空间，在那里，自由参数对于机器人10的次级任务变得可用。

将方程式(12)代入等式(8)推导出方程式(13)：

$J\stackrel{··}{q}+\stackrel{·}{J}\stackrel{·}{q}=G(S^{+}\stackrel{··}{z}+S^{\perp }\mu )+h---\left(13\right)$

为除去μ，找到满秩矩阵E使得EGS^⊥＝0，即，等式(14)，其中，E∈R^(6n+p-6)×6n。

将方程式(13)乘以E，得出减少的组：

$\mathrm{EJ}\stackrel{··}{q}+E\stackrel{·}{J}\stackrel{·}{q}={\mathrm{EGS}}^{+}\stackrel{··}{z}+\mathrm{Eh}$

$={\mathrm{EGS}}^{+}S\stackrel{··}{y}+\mathrm{Eh}---\left(15\right)$

矩阵EJ在闭链运动学中起到与Jacobian矩阵在开链运动学中通常起到的类似的作用。因此，可推导出下面的矩阵：

$\hat{J}\stackrel{·}{=}\mathrm{EJ},$$\widehat{\stackrel{·}{J}}\stackrel{·}{=}E\stackrel{·}{J},$$\hat{G}\stackrel{·}{=}{\mathrm{EGS}}^{+}S,$$\hat{h}\stackrel{·}{=}\mathrm{Eh}.---\left(16\right)$

这允许定义最终的闭链变换：

$\hat{J}\stackrel{··}{q}+\widehat{\stackrel{·}{J}}\stackrel{·}{q}=\hat{G}\stackrel{··}{y}+\hat{h}---\left(17\right)$

和分别定义为“闭链”Jacobian矩阵和抓握矩阵。

考虑三个任务类型：

1.全位姿控制，其中：S＝I₆，S⁺＝I₆，S^⊥＝0；

2.仅定向控制，其中：S＝[0 I₃]，$S^{+}=\left(\begin{array}{c}0\\ I_3\end{array}\right),$$S^{\perp }=\left(\begin{array}{c}{I}_3\\ 0\end{array}\right);$

3.仅位置控制，其中：S＝[I₃ 0]，$S^{+}=\left(\begin{array}{c}{I}_3\\ 0\end{array}\right),$$S^{\perp }=\left(\begin{array}{c}0\\ I_3\end{array}\right).$

双手抓握：

全位姿：由于该方案中涉及到没有减少DOF，所以闭链的表达式保持不变，并且：

$\hat{J}=J,$$\hat{G}=G,$$\hat{h}=h---\left(18\right)$

仅定向：下面的矩阵是该方案的有效零化子：

$E=\left(\begin{array}{cccc}{I}_3& 0& -I_3& 0\\ 0& I_3& 0& 0\\ 0& 0& 0& I_3\end{array}\right)$

根据给出的所述E，由等式(16)的闭链限定得到用于双手抓握的仅定向控制的下述矩阵：

$\hat{J}=\left(\begin{array}{c}{J}_{v1}-J_{v2}\\ J_{\omega 1}\\ J_{\omega 2}\end{array}\right),$$\hat{G}=\left(\begin{array}{cc}0& r_2^{\times }-r_1^{\times }\\ 0& I_3\\ 0& I_3\end{array}\right),$$\hat{h}=\left(\begin{array}{c}{\lambda }_1-{\lambda }_2\\ {\alpha }_{{\mathrm{rel}}_1}\\ {\alpha }_{{\mathrm{rel}}_2}\end{array}\right)---\left(19\right)$

在所有这些方案中，的形式直接遵循其中，Jacobian子矩阵可简单地替换为它们的导数。

仅位置：下面的矩阵是该方案的有效零化子：

$E=\left(\begin{array}{cccc}{I}_3& r_1^{\times }& 0& 0\\ 0& 0& I_3& r_2^{\times }\\ 0& I_3& 0& -I_3\end{array}\right)$

根据给出的所述E，由方程式(16)的闭链限定得到用于双手抓握的仅位置控制的下述矩阵：

$\hat{J}=\left(\begin{array}{c}{J}_{v1}+r_1^{\times }{J}_{\omega 1}\\ J_{v2}+r_2^{\times }{J}_{\omega 2}\\ J_{\omega 1}-J_{\omega 2}\end{array}\right),$$\hat{G}=\left(\begin{array}{cc}{I}_3& 0\\ I_3& 0\\ 0& 0\end{array}\right),$$\hat{h}=\left(\begin{array}{c}{\lambda }_1+r_1^{\times }{\alpha }_{{\mathrm{rel}}_1}\\ {\lambda }_2+r_2^{\times }{\alpha }_{{\mathrm{rel}}_2}\\ {\alpha }_{{\mathrm{rel}}_1}-{\alpha }_{{\mathrm{rel}}_2}\end{array}\right)---\left(20\right)$

三手指抓握：在三手指抓握方案中，对点接触进行处理，并且运动约束仅施加至端点的位置。

全位姿：由于该方案涉及到没有减少DOF，所以闭链的表达式保持不变，并且：

$\hat{J}=J,$$\hat{G}=G,$$\hat{h}=h---\left(21\right)$

仅定向：下面的矩阵是该方案的有效零化子：

$E=\left(\begin{array}{ccc}{I}_3& -I_3& 0\\ I_3& 0& -I_3\end{array}\right)$

根据给出的所述E，等式(16)的闭链限定导致了用于三手指抓握的仅定向控制的下述矩阵：

$\hat{J}=\left(\begin{array}{c}{J}_{v1}-J_{v2}\\ J_{v1}-J_{v3}\end{array}\right),$$\hat{G}=\left(\begin{array}{cc}0& r_2^{\times }-r_1^{\times }\\ 0& r_3^{\times }-r_1^{\times }\end{array}\right),$$\hat{h}=\left(\begin{array}{c}{\lambda }_1-{\lambda }_2\\ {\lambda }_1-{\lambda }_3\end{array}\right)---\left(22\right)$

仅位置：由于难以从运动约束组明确消除自由变量所以该方案更加有挑战性。对于该方案来说：

${\mathrm{GS}}^{\perp }=\left(\begin{array}{c}-r_1^{\times }\\ -r_2^{\times }\\ -r_3^{\times }\end{array}\right)$

r₃＝αr₁+βr₂+γr₁×r₂       (23)

其中，α，β和γ是方程式(23)中的待求解标量。

然后，E可被推导为：

$E=\left(\begin{array}{ccc}{r}_1^T& 0& 0\\ 0& r_2^T& 0\\ r_2^T& r_1^T& 0\\ \alpha I_3-\gamma r_2^{\times }& \beta I_3+\gamma r_1^{\times }& -I_3\end{array}\right)---\left(24\right)$

运动的方程式：再次考虑图2的受力图，f_i和t_i分别表示来自接触i的接触力和力矩。运动的方程式可表示为：

$F_{\mathrm{ma}}=F+G^{T}f+m\hat{g}---\left(25\right)$

$F_{\mathrm{ma}}\stackrel{·}{=}\left(\begin{array}{c}{\mathrm{ma}}_G\\ I_G\stackrel{·}{\omega }+\omega \times I_G\omega +r_G\times {\mathrm{ma}}_G\end{array}\right),$$\hat{g}\stackrel{·}{=}\left(\begin{array}{c}g\\ r_G\times g\end{array}\right)$

F_ma表示内力，其中，m是物质20的质量，I_G是绕质心G的惯性矩。a_G是G的加速度，并且r_G是从参考点到G的位置矢量。f是接触的力旋量的列矩阵；其形式与上述方程式(9)和(10)中所示的的形式成镜像。g是重力矢量。

内力：从该运动的方程式可见，接触力通过抓握矩阵的转置从而映射到物体空间。据此，物体20上的内力由G^T的零空间所限定。为了应用内力控制，需要两个特性。首先，应当利用物理相关的参数将零空间参数化。其次，参数应处于两种抓握类型的零空间中。这些要求通过相互作用力的概念而得到满足。在两个接触点之间划一直线，如本领域所已知的那样，相互作用力是两个接触力沿这条直线的投影之间的差。因此，可利用各种相互作用分量来参数化系统10的内力。

如更早之前所描述的那样，可使用相对加速度项来控制内力。为确保这些相对加速度仅影响内力而不影响外部动力学，它们也必须位于G^T的零空间中。如果是相对加速度的列矩阵，那么当时条件得到满足。据此，我们使用相对加速度来闭合相互作用力的伺服环路。将u_ij定义为从接触i指向接触j的单位矢量，两个接触之间的相互作用力f_ij的大小为：

$f_{\mathrm{ij}}\stackrel{·}{=}(f_i-f_j)·u_{\mathrm{ij}}---\left(26\right)$

$u_{\mathrm{ij}}\stackrel{·}{=}\frac{r_j-r_i}{\left|\right|r_j-r_i\left|\right|}$

我们将引入相互作用加速度a_ij，作为这些力的PI调整器，其中，k_P和k_I是恒定增益。

$a_{\mathrm{ij}}\stackrel{·}{=}{k}_P(f_{\mathrm{ij}}-{f^{*}}_{\mathrm{ij}})-k_I\int (f_{\mathrm{ij}}-{f^{*}}_{\mathrm{ij}})\mathrm{dt}---\left(27\right)$

注意到u_ij＝-u_ji以及a_ij＝a_ji，所以三个接触的内加速度可概括成下面的形式。对于两个接触的情况，仅需设置a_i3＝0。

a_rel1＝a₁₂u₁₂+a₁₃u₁₃

a_rel2＝-a₁₂u₁₂+a₂₃u₂₃                    (28)

a_rel3＝-a₁₃u₁₃-a₂₃u₂₃

由于我们选择不控制任何旋转的分量，对于所有的i来说，a_reli＝0。

控制规则：可使用这些阻抗任务、运动变换和内力给出控制规则。首先，开始建立用于操纵器整个系统的运动方程式模型：

$M\stackrel{··}{q}+c-{\tau }_f=\tau ---\left(29\right)$

M是关节空间惯性矩阵。c是科氏力、向心力和重力等广义力的列矩阵，并且τ是关节转矩的列矩阵。假设力仅作用在操纵器的末端执行器上，

τ_f＝-J^Tf                    (30)

在控制规则的准备中，对物体20的某些未感测的量进行估计。首先，从物体20上的其他力来估计外部力旋量(F)。参照方程式(25)，可使用力的准静态近似。

$F=-G^{T}f-m\hat{g}---\left(31\right)$

虽然此处包括了物体重量，但是物体重量在大多数情况下也可被忽略。此外，物体速度可利用下面作为刚体的系统的最小平方误差估值来估算：

$\stackrel{·}{y}=G^{+}J\stackrel{·}{q}---\left(32\right)$

其中，上标(+)表示相应矩阵的伪逆。

最后，我们基于下面的逆动力学公式[12]给出控制规则。

$\tau =M{\stackrel{··}{q}}^{*}+c-{\tau }_f---\left(33\right)$

该表达式中的是指定的关节加速度。其可根据方程式(17)从指定的物体加速度来推导出。

${\stackrel{··}{q}}^{*}={\hat{J}}^{+}(\hat{G}{\stackrel{··}{y}}^{*}+\hat{h}-\widehat{\stackrel{·}{J}}\hat{q})+N_{\hat{J}}{\stackrel{··}{q}}_{\mathrm{ns}}^{*}---\left(34\right)$

$N_{\hat{J}}\stackrel{·}{=}I-{\hat{J}}^{+}\hat{J}$

是指用于的零空间的正交投影算子，并且是投影到所述零空间的加速度矢量。使用这种闭链Jacobian矩阵，第二任务因而可在包括物体的自由DOF的空间中得到优化。从方程式(3)的阻抗任务中获得两个指定的加速度和

明确的控制规则可从方程式(33)、(34)和(3)中完全导出。在方程式(30)和(31)中引入力的估算，最终控制规则遵循下述的方程式(35)：

$\tau =-M{\hat{J}}^{+}\hat{G}{M}_o^{-1}(F^{*}+B_o\stackrel{·}{y}+K_o\mathrm{\Delta y}+G^{T}f+m\hat{g})+M{\hat{J}}^{+}(\hat{h}-\widehat{\stackrel{·}{J}}\stackrel{·}{q})$

$-{\mathrm{MN}}_{\hat{J}}{M}_j^{-1}(B_j\stackrel{·}{q}+K_j\mathrm{\Delta q}+J^{T}f)+c+J^{T}f$

为理解系统的真实行为，考虑下述的闭环分析。通过注意到和我们获得用于系统的范围空间和零空间两者的下述独立的闭环动力学。

第一关系揭示了施加至由S选择的DOF的期望的物体阻抗任务。如果阻抗矩阵是对角的，则任务空间将保持解耦。该关系的右手侧表示来自物体加速度的由F的准静态估算得到的扰动。该扰动不会影响到内力。第二关系示出了期望的第二阻抗任务以投影到零空间的最小误差投影而被执行。

该控制规则能够通过两个特征来消除对物体动力学的需求。首先，该控制规则在末端执行器的力上引入反馈。其次，该控制规则使用加速度而不是力来实现从物体空间到末端执行器空间的转换。相对于其它依赖于物体惯性和加速度的估算的控制规则，该方法会使内力保持更好的完整性。虽然外部动力学会证明前述的扰动，但按照我们的观点，在协同操纵中，内力是关键因素。

零力反馈：不幸地是，力感测不会在每个末端执行器上一直可用。因此，本节将引入对力反馈的需求进行估算的控制规则形式。不过，这里给出的解决方案不具有适于全部范围的能力。它仅可适用于应用到双手抓握的具有全位姿控制的方案。控制规则(35)中的力反馈项可通过对主动惯性(active inertia)M_o和M_j的适当选择来消除。当f的系数总和为零时反馈被消除：

$J^T-M{\hat{J}}^{+}\hat{G}{M}_o^{-1}{G}^T-{\mathrm{MN}}_{\hat{J}}{M}_j^{-1}{J}^T=0---\left(38\right)$

求解该关系从而得到下面两个条件：

$M_o^{-1}={\hat{G}}^{\#}\left(\hat{J}{M}^{-1}{J}^T\right)G^{T\#}---\left(39\right)$

M_j＝M                    (40)

上标(#)表示满足G^#G＝I的相应矩阵的广义逆，诸如，上述加权伪逆类。第一个条件需要具有完整列秩。因此，这种解决方法仅可适用于全位姿控制情况。给定全位姿控制，可利用和的事实。可引入A作为末端执行器空间惯性，其中，这些结果可被解释为与被动惯性匹配的主动惯性。换而言之，保持系统的自然惯性(natural inertia)消除了对力反馈的需求。

结果是，这两个条件并不说明物体上的内力分量。因此，引入第三个条件以将内力设置为零。对于内部空间，可使用由A^-1加权的G^T的伪逆。该加权的伪逆及其对应的零空间投影矩阵限定如下：

$G_{A^{-1}}^{T^{+}}\stackrel{·}{=}\mathrm{AG}{\left(G^T\mathrm{AG}\right)}^{-1}$

$N_{G^T}\stackrel{·}{=}I-G_{A^{-1}}^{T^{+}}{G}^T---\left(41\right)$

该加权的伪逆使物体运动不干扰内部空间。第三个条件因此变成：由于该条件，这种控制规则仅可适用于刚性抓握，这是因为刚性抓握不需要内力来维持抓握。据此，我们在方程式(39)中设置

将这三个条件应用到方程式(35)，可推导出零力反馈控制规则：

${\tau }_{\mathrm{zff}}=-{\mathrm{MJ}}^{+}{A}^{-1}{G}_{A^{-1}}^{T^{+}}(F^{*}+B_o\stackrel{·}{y}+K_o\mathrm{\Delta y}+m\hat{g})$

$+{\mathrm{MJ}}^{+}(\hat{h}-\stackrel{·}{J}\stackrel{·}{q})-{\mathrm{MN}}_J{M}^{-1}(B_j\stackrel{·}{q}+K_j\mathrm{\Delta q})+c---\left(42\right)$

通过注意到该表达式得到了简化。

该控制规则的闭环分析揭示出物体的两个独立的动力学关系，第一个在外部空间内，第二个在内部空间内。

$\left(G^T\mathrm{AG}\right)\stackrel{··}{y}+B_o\stackrel{·}{y}+K_o\mathrm{\Delta y}=\mathrm{\Delta F}---\left(43\right)$

$N_{G^T}\left(\mathrm{AG}\right)\stackrel{··}{y}=N_{G^T}f---\left(44\right)$

根据与被动惯性相匹配的惯性，第一种关系揭示出方程式(1)中的期望物体阻抗。对于第二种关系，由于加权的伪逆，可见N_GT(AG)＝0。因此，加权的伪逆从内部空间过滤掉物体加速度，并且从而在物体20上产生零内力。

虽然具体描述了实施本发明的最佳模式，但本领域技术人员将意识到在所附权利要求范围内的实现本发明的各种替代性设计和实施例。