CN101842755A - 逆运动学 - Google Patents

Abstract

一种用于控制系统的实时方法，所述系统包括：多个控制部件，每个具有至少一个可变参数(dq)；和受控元件，具有由所述控制部件控制的轨迹，其中所述轨迹通过可变矩阵与所述可变参数相关联，所述方法包括：定义将所述可变参数dq与所述轨迹dx相关联的控制转移矩阵(K)，并且使用反馈环，在反馈环中，计算依赖于作为可能具有被指定为任意维度(n)的希望轨迹(dxd)和当前轨迹(dx)之间的差异的误差(e)的反馈项。

Description

逆运动学

技术领域

本发明涉及雅可比(Jacobian)矩阵的逆。这也称为逆运动学问题(IK)。IK问题与下述有关：利用在用于游戏和动画的计算机图形中使用的任意数量的关节(joint)操纵计算机产生的关节角色(articularted character)、关键帧(keyframing)、控制机器人、用于控制控制力矩陀螺机制和许多其他应用的奇异回避方法。

背景技术

在使用控制力矩陀螺(Control Moment Gyro，CMG)等的诸如机器人技术、计算机动画、航天器控制的应用中，作为例如(q)或时间的可变参数的函数并因此随着时间改变的可变矩阵将输入空间连接到输出空间。

让(x)用来代表描述终端效果器(end effector)的位置和配置的笛卡儿坐标的矢量，例如图1中的14。对于三维笛卡儿空间，(x)将具有一个或多个元素，以代表位置和角度旋转。终端效果器空间的大小(即，维数)被表示为(m)。例如，如果终端效果器可仅执行三维位移，则(m＝3)。

让(q)用来描述关节坐标和它们的配置，例如，用于图1中的关节11、12和13。每个关节可具有一个或多个自由度。将(q)中的自由度的总数表示为(n)。例如，如果图1中的每个关节，即11、12和13，具有两个自由度，则自由度的总数为(n＝6)。

然后，可如下描述结构的模型、机器人或计算机动画对象(object)：

x＝f(q)           (1a)

在(等式1a)中，f(q)是关节变量(q)的非线性函数。

在许多应用中，需要对于希望终端效果器位移(x)的给定集合计算关节变量(q)。这要求转换(等式1a)

q＝[f(x)]^-1     (1b)

在(等式1b)中，([f(x)]^-1)表示从(q)到(x)的映射的反转。

解析(等式1b)是冗长的工作。用于求解(等式1a)的数字方法涉及从两侧对(等式1a)求微分。这给出了运动学关系：

dx＝J(q)dq                   (1c)

在(等式1c)中，(dq)定义输出空间，(dx)定义输入空间，而(J(q))是通过关于(q)对(f(q))求微分计算出的参数依赖型雅可比矩阵。

J(q)＝diff(f(q))/diff(q)     (1d)

(等式1d)表示(f(q))关于(q)的微分。

在关节图的机器人技术和动画中，(x)表示根据在笛卡尔空间中定义的角和线性位移构造的矢量。(q)用于描述关节坐标和它们的配置。(dx)描述在笛卡尔空间中定义的速度。(dq)描述关节速度。

在使用控制力矩陀螺机制的姿势控制中，如技术人员所知道的，(x)用于描述CMG集群(cluster)的总角动量(angular momentum)。由万向(gimbal)角来构成(q)，(dx)描述扭矩的分量。如技术人员所知道的，(dq)表示万向速率。

(x)和(dx)的维度(即，维数)为(m)。

(q)和(dq)的维度是(n)。

典型地，(m)小于或等于(n)。

对于备用配置，(m)小于(n)。

在本发明的一些实施例中，(m)和(n)可为任意大。

图1是例如具有3个关节11、12和13以及1个终端效果器14的机器人的结构的例子。关于终端效果器的目标轨迹被指定为如图1中的15，并且在三维笛卡尔空间中被定义。在图1中，11、12和13是可按角度或线性地改变以提供自由度(n)并允许14追随目标15的关节机制。关节11、12和13的每个可具有一个或多个自由度，并且关节可具有不同数目的自由度。

图2是在诸如计算机动画的领域中使用的图形结构。图2中的结构的具体示例具有5个终端效果器211、214、215、213和212和10个关节21、22、23、24、25、26、27、28、29、210。包括图2中的结构仅为了说明目的，并且在本发明中描述的IK算法可工作于下述结构上：所述结构对于终端效果器具有(m)个自由度，且对于关节具有(n)个自由度，其中(m)和(n)可为任意大。216、217、218、219和220是终端效果器必须追随的、在笛卡尔空间中定义的目标轨迹。

在机器人技术中，电动机或其他机制可用于提供线性和或角动量，以产生位移并改变(q)。

在计算机图形中，动画软件重新画图，以产生(q)中的位移。

由于(等式1c)中的雅可比依赖于(q)，所以传感器被用于测量(q)或数学模型被用于计算、导出或估计(q)。

在本发明的该说明书中，(dxd)被用于表示对于(dx)的希望值的矢量。假设已知(dxd)和(q)，并且通过数学模型或传感器来提供(dxd)和(q)。

对于希望轨迹(dxd)的给定集合，逆运动学问题(IK)是求解(等式1c)的逆的问题，这用于给定矢量(dxd)如何导出(dq)。

dq＝iJ(q)dxd            (2)

在等式2中，(iJ(q))表示(等式1)中的雅可比的逆。如果(m)严格地小于(n)，即，(m)不等于(n)，则伪逆方法被用于计算(iJ(q))。

在计算机图形和机器人中，(dxd)表示终端效果器的希望速度。

在使用CMG的姿势控制中，(dxd)表示扭矩的希望分量。

作为本发明的主题的逆运动学的问题，是如何选择关节变量，即在动画应用中如何操纵电动机或如何重画结构，以使得(dx)追随希望的目标轨迹(dxd)。

图3表示逆运动学块，即，处理系统，其确定产生目标轨迹(dxd)所需要的关节变量(dq)。该块的输入是(dxd)并且从该块的输出是(dq)。逆运动学块必须对给定的(dxd)给出(dq)，使得在每次迭代时(dx)与(dxd)相同。

当给定(dxd)并且(J(q))是时变参数依赖型雅可比矩阵时，对于(dq)求解(等式2)是计算机动画、机器人的控制、航天器的控制以及其他应用中的一个关键的元素或步骤。

实时求解(等式2)是冗长的并且计算密集的任务。

本发明一些实施例的目的是得到用于求解(等式2)中定义的逆运动学问题的计算有效方法。

由于(q)中的较高数量的自由度(n)，所以实时求解(等式2)中的IK问题是数字密集的任务。

例如，增加用关节连接的对象的动画中的细节和自由度导致动作的改进视觉表现。但是，由于需要对大量(n)实时计算(等式2)中的IK问题，所以导致高密集计算。

因为由于(等式1c)和(等式2)中的雅可比(J(q))依赖于参数(q)而是时变函数(在特定的配置中被称为奇异(singular)状态)，所以(J(q))变为秩亏，并且结果，(等式2)中的逆可导致对于给定轨迹(dxd)，(dq)任意大的值。这是与逆运动学问题的计算相关联的另一复杂因素。

导出(iJ(q))、即雅可比的逆的传统IK方法是基于矩阵的操纵，其使得处理高度计算密集，并且难以在并行处理器构架上运行。

阻尼最小二乘法(DLS)(也称为奇异鲁棒(SR)算法)传统上被用于求解(等式2)中的问题(Y.Nakamura和H.Hanafusa，“Inverse kinematic solutionwith singularity robustness for robot manipulator control”，Journal of Dynamicsystems，Measurements and Control，Vol.108，Sep.1986，C.W.Wampler and L.J.Leifer，“Application of damped least-squares methods to resolved-rate andresolved-acceleration control of manipulators”，Journal of Dynamic Systems，Measurement，and Control，110(1998)，pp.31-98)。

iJ(q)＝J(q)[J(q)Jt(q)+kI]^-1     (3a)

(Jt(q))是雅可比的转置。通过在(等式3a)中定义的用于计算(iJ(q))的形式，可从(等式2)导出对于给定矢量(dxd)的(dq)，即，

dq＝Jt(q)[J(q)Jt(q)+kI]^-1dxd    (3b)

在等式(3b)中，(Jt(q))是在(等式1d)中定义的雅可比的转置。(I)是单位矩阵，已知(k)为需要适应的阻尼因子，而(^-1)代表逆运算符。当(k＝0)时，(等式3)变化到伪逆方法。

iJ(q)＝Jt(q)[J(q)Jt(q)]^-1       (4a)

通过在(等式4a)中定义的用于计算(iJ(q))的形式，使用(等式2)可以导出对于给定矢量(dxd)的(dq)，即，

dq＝Jt(q)[J(q)Jt(q)]^-1dxd       (4b)

在奇异状态，雅可比变为秩亏，并且结果，(等式4a)中的逆不存在，并且其不能使用(等式4a)的算术公式生成。

此外，当(J(q))接近奇异状态时，基于(等式4b)的解导致对于(dq)的过分大的值。阻尼因子(k)因此被用作解的精确性和解的可行性之间的折中。当(k＝0)(等式3a)时变化到(等式4a)；当配置远离奇异时，(k)通常被设置为(k＝0)。接近奇异(k＞0)。因此，由于需要适应(k)，所以(等式3a)和(等式3b)中的IK方法更复杂。适应阻尼因子(k)要求额外的计算，并且因此需要额外的处理功率。

W.A.Wolovich和H.Elliott在“A computational technique for inversekinematics””中，Proceedings ofthe 23^rd Conference on Decision and Control，1984和A.Balestrino，G.De Maria和L.Sciavicco在“Robust control of roboticmanipulators”，9th IFAC World Congress，1984中，已经提到了包括仅使用用于机器人的控制的雅可比的转置的反馈环的算法。但是，与在本发明中提到的方法不同，这些算法不能避免或躲开奇异状态并且当系统处于奇异状态时不能给出解。

发明内容

本发明提供一种用于控制系统的实时方法，所述系统包括：多个控制部件，每个具有至少一个可变参数(dq)；和受控元件，具有由控制部件控制的轨迹，其中轨迹通过可变矩阵与可变参数相关联，所述方法包括：定义将可变参数dq与轨迹dx相关联的控制转移矩阵(K)，并且使用反馈环，在反馈环中，计算依赖于作为希望轨迹(dxd)和当前轨迹(dx)之间的差异的误差(e)的反馈项。

可通过多次循环重复计算反馈项，使得当前轨迹到达希望轨迹。

矩阵K的输出(dq)具有被指定为(n)的维度，希望轨迹dxd可具有维度(m)，并且(m)可以小于或等于(n)。

所述方法可包括选择具有维度为(m)乘以(n)的控制转移矩阵(K)，以及依赖于系统的属性确定(K)形式和数值。

对于给定(dxd)生成(dq)的数值算法可以以滤波器的形式。该算法可以被布置为仅需要乘法和累加类型的指令。

可在单个处理器或在并行平台上执行所述方法。

即使当(J(q))变为秩亏时，矩阵(K)也可以被布置为给出(dq)的解。

在奇异状态下，误差(e)在奇异方向上增长，并且(K)的全满结构可以被布置为生成(dq)的非零解，其产生促使轨迹远离奇异状态的动作。

所述系统可以是被布置为显示可移动对象的图像的显示器，控制部件包括可移动对象的关节，并且受控对象包括可移动对象的元素。

所述系统可为包括机器人和控制系统的机器人系统，受控元件是机器人的元件，并且控制部件包括机器人的关节。

控制部件可包含控制力矩陀螺系统的陀螺。

本发明还提供一种用于可移动系统的控制系统，所述控制系统被布置为根据本发明的方法而操作。

本发明还提供一种包含机器人和根据本发明的控制系统的机器人系统。

本发明还包括包含多个陀螺和根据本发明的控制系统的控制力矩陀螺系统。

本发明的一些实施例的目的是导出用于求解(等式2)中定义的逆运动学问题的计算有效的、实时的数字方法。

在本发明的一些附加实施例中，奇异鲁棒性意味着即使对于雅可比矩阵(J(q))为秩亏或奇异时的情形也存在解。

此外，在本发明的一些实施例中的算法不需要计算阻尼因子。

因此，本发明提供了一种用于数字计算可变矩阵的逆的实时方法，其中所述方法使用反馈环，在所述反馈环中，可能具有被指定为(m)的任意维度的希望轨迹(dxd)与当前轨迹(dx)相比较，以生成误差(e)。可在计算逆运动学问题的每个循环进行所述比较。所述误差(e)可用作对于控制转移矩阵(K)的输入，其中所述控制转移矩阵(K)生成所需要的输出(dq)。(dq)可具有被指定为(n)的任意维度，(m)小于或等于(n)。

矩阵可随时间改变。例如，其可为时间依赖型或依赖于随时间改变的另一参数。

所述方法可为计算机实现的方法。

逆运动学问题是计算时间可变和参数依赖型矩阵的逆的问题。其具有例如在机器人控制、利用任意数目的关节以及终端效果器控制和操作计算机生成的关节角色、关键帧应用、使用控制力矩陀螺机制等其他事物控制航天器中的应用。

所述方法可包括：用于选择反馈补偿(K)的方法，所述反馈补偿(K)具有维度(m)乘以(n)；以及用于依赖于针对其正在解决逆运动学问题的属性和结构，确定(K)的形式和数字值的方法。

所述方法可使用对于给定(dxd)生成(dq)的算法的数字实现方式，并且可作为只需要乘法和累加类型的指令的滤波器来执行，其中可在单个处理器或并行平台上运行所述指令。

在一些实施例中，即使当(J(q))变为秩亏时(K)也给出(dq)的解。在奇异状态下，误差(e)可在奇异方向增长，并且(K)的全满结构可对于(dq)生成非零解，该非零解产生轨迹从奇异状态远离的动作。结果，可在所提出的算法中实现奇异性的避免。

本发明因此可对于解决逆运动学问题提供计算有效和奇异鲁棒的方法。

本发明还提供用于控制系统的运动的方法，该方法包括本发明的方法。系统可为诸如机器人胳膊的接合系统、或者诸如航空器的另一个可移动系统。

本发明还包括用于可移动系统的控制系统，该控制系统被布置为根据本发明的方法操作。

本发明还提供生成可移动对象的图形图像的方法，所述方法包括在以上任意段落中定义的方法。

本发明还提供一种显示系统，包括：处理部件，被布置为执行在以上任意段落中定义的方法，从而生成图像数据；以及显示部件，被布置为显示图像。例如，所述系统可包含游戏机或其他计算机系统。

现在将仅以示例的方式通过参考附图来描述本发明的优选实施例。

附图说明

图1是例如具有3个关节和1个终端效果器(end effector)位置的机器人结构的示例。

图2是在诸如计算机动画的领域中使用的图形结构。

图3代表了逆运动学块。

图4代表了根据本发明的第一实施例的逆运动学解的反馈形式。

图5是先前在图4中描绘的反馈逆运动学定律的扩展版本。

图6包括实现在本发明中描述的逆运动学问题的算法的C代码的示例。

图7是根据本发明的实施例的机器人和控制系统的示意图。

图8是根据本发明的实施例的图形显示系统的示意图

具体实施方式

在本发明的一个实施例中：

1)存在乘法部件。

2)存在加法部件。

3)存在减法部件。

4)存在诸如可存储雅可比的具有维度(m)乘以(n)的存储块的雅可比存储部件。

5)存在提供(dxd)的部件。

6)存在诸如可提供(q)的数学模型或传感器的部件。

7)存在诸如可存储参数(z(t-1))的具有维度(m)的存储块的参数存储部件。

8)存在诸如可存储参数(dq(t-1))的具有维度(m)的存储块的参数存储部件。

8)存在诸如可存储参数(dx)的具有维度(m)的存储块的参数存储部件。

9)存在诸如可存储参数(tmp)的具有维度(m)的存储块的参数存储部件。

10)存在诸如可存储(A)的具有维度(m)的存储块的参数存储部件；如果A是乘以标量(scalar)的单位矩阵，则存储块需要维度(1)来仅存储标量。

11)存在诸如可存储(P)的具有维度(m)的存储块的参数存储部件。

通常通过例如个人计算机或微处理器或微控制器或数字信号处理器的通用处理器来提供以上部件，或者以上部件可为专用构建算法和存储块。

本发明的该实施例的目的是提供一种用于求解(等式2)的计算有效并且奇异鲁棒的实时算法，其不需要计算矩阵的逆和阻尼因子。用于求解的等式(2)中的雅可比可表现机器人的运动学、计算机动画角色，或者可与航空器的控制相关联或者可与其他应用相关联，其中需要被称为雅可比矩阵的矩阵的逆，所述雅可比矩阵依赖于可变参数，例如(q)。

首先，提出并如图4中所示构造反馈环。在每次循环时计算该反馈环并且因此其实时运行。所生成的(dx)45和所要求的(dxd)44矢量之间的误差47被用作控制定律(K)41的输入。该控制定律生成所需要的(dq)43，使得促使误差47为难以察觉的小的值，46是加法器，48表示正号而49表示负号。

图4中所示的反馈环提供了用于对于给定的雅可比(J(q))从(dxd)导出(dq)的机制。图4中的反馈环实质上替代图3中的IK块。使用图中的反馈环，可导出以下关系：

dq＝K[JK+I]^-1dxd         (5)

在(等式5)中，(K)是作为本发明的本实施例的一部分导出的控制转移(transfer)矩阵或者控制定律，(J)是从(等式1d)导出的雅可比矩阵而(^-1)代表逆运算符。在本发明的该实施例和其他实施例中，重要的元素是(K)的导出和(K)的形式的选择，以提供奇异避免属性。(K)也需要被适应于解决J(q)的可变属性。

(K)是转移函数的全转移矩阵，具有非零非对角元素，如技术人员所知，以便于在环中提供奇异避免属性。(K)具有(n)乘以(m)的维度。

(K)是(q)的函数，即，如技术人员所知，其被适应于解决雅可比的时变属性，并且其依赖于(q)。

当雅可比变为秩亏时，误差(e)将在一个方向上增长，这是因为(J(q))将在该特定的方向上传递0输出。(K)的非零元素将随后生成必要输出(dq)，(dq)将导致远离奇异方向的轨迹，结果是难以察觉的小误差(e)和目标的良好轨迹。

图4中的反馈环中的误差47描述希望变量(dxd)和根据(等式1c)计算的实际变量之间的差异。

e＝dxd-J(q)dq         (6)

通过适当地选择(K)，可使得(等式6)中的误差任意小。假设(K)已知，可根据(等式5)计算(dq)。

对于固定采样间隔的实时实现方式

如果在规律的时间间隔需要执行参数依赖型矩阵的雅可比(在此称为雅可比矩阵)的逆(典型的而不是仅有的应用是机器人控制和飞行器控制)，则可使用以下算法，其中如技术人员所知，所述规律的时间间隔也称为采样间隔。

在本发明中的该实施例中，以以下离散形式给出(K)的形式：

K：z(t)＝Az(t-1)+e    (7a)

dq＝Jt(q)Pz(t-1)      (7b)

(等式7a)和(等式7b)一起表示图4和(等式5)中的反馈环和块(K)。

(等式7a)和(等式7b)一起将(等式6)中的误差(e)与(dq)相连。

(等式7a)和(等式7b)在每个循环一起实时运行，在所述循环求解逆运动学问题，以找到dq的值，从而获得dxd的希望结果。如技术人员所知，两个连续采样之间的时间差，或IK算法的两个连续执行之间的时间为采样时间(dt)。

在(等式7a)中，(A)是具有维度(m)乘以(m)的负定对角矩阵。

在(等式7a)中，(P)是具有维度(m)乘以(m)的正定满矩阵。

在(等式7b)中，(Jt(q))是具有维度(n)乘以(m)的在(等式1d)中导出的雅可比的转置。

在(等式7b)中，在每次采样时使用(q)的当前值重新计算雅可比的转置(Jt(q))。通过传感器或数学模型来提供(q)。

在(等式7a)和(等式7b)中，(z(t))是根据维度(m)的状态变量的矢量。

在(等式7a)中，(z(t-1))是根据维度(m)的状态变量的矢量。

在(等式7a)和(等式7b)中，(z(t-1))被延迟等于(z(t))的(dt)值的一个采样周期。为了计算延迟值，需要根据维度(m)的存储元件。这里将等于(dt)的一个采样的延迟表示为D(dt)。

在(等式7a)和(等式7b)中，(z(t-1))表示在先前迭代时根据(等式7a)计算的(z(t))的值。

(等式7a)和(等式7b)一起表示对于给定误差(e)允许计算(dq)的滤波器。

(等式7a)和(等式7b)以及(等式6)一起是本发明的该实施例的结果，并且提供用于求解逆运动学问题的完整的算法。该算法总结如下。

e＝dxd-J(q)dq(t-1)      (9a)

z(t)＝Az(t-1)+e         (9b)

dq＝Jt(q)Pz(t-1)        (9c)

(等式9a)、(等式9b)和(等式9c)一起对于给定的希望轨迹(dxd)，允许计算(dq)。

在图5中以图形形式表现(等式9a)、(等式9b)和(等式9c)，图5为图4的扩展版本。图5反应了实时实现方式并示范了算法的滤波属性。D(dt)51和511是延迟块并且代表其中存储了对于(z(t))513的值和(dq)53的值的存储器。在(等式16)中定义了(A)512。在(等式20)中定义了510中的P。510中的(Jt(q))是使用当前采样的(q)的值计算的雅可比的转置。通过传感器或数学模型来提供(q)。根据(等式9b)计算(z(t))513，(J(q))52是使用当前采样的(q)的值计算的雅可比。通过传感器或数学模型来提供(q)。(e)是要求轨迹(dxd)54和实际轨迹(dx)55之间的误差。56和515代表加法器块，58、516和517代表正号，而59代表负号。(z(t-1))514是当在先前采样或迭代中计算的(z(t))513的值。(dq(t-1))518是当在先前的采样或迭代中计算的(dq)53的值。

(等式9a)、(等式9b)和(等式9c)一起代替图3中的IK块。

(等式9a)、(等式9b)和(等式9c)一起表示代替(等式2)的新算法。

(等式9a)、(等式9b)和(等式9c)一起表示用于求解逆运动学(IK)问题的算法。

在每个循环实时执行(等式9a)、(等式9b)和(等式9c)。

(等式9a)、(等式9b)和(等式9c)执行与滤波器的操作类似的操作，并且因此只需要累加(accumulate)和乘法指令。在图5中以滤波器形式示出了计算。

在(等式9a)中，(dq(t-1))是延迟了等于(dt)的一个采样周期的(dq(t))的值。

dq(t-1)＝D(dt)dq          (10)

对于可变采样间隔的实时实现方式

作为逆运动学的另一类应用，实时但不以固定的采样速率执行雅可比矩阵或参数依赖型矩阵的逆。例如，在关节角色动画的计算机图形和动作合成中，以预定速率(有时称为帧速率)定义端点(或如图1中的14和图2中的211、212、213、214、215的终端效果器)的新位置。在一帧(或时间间隔)中，顺序地若干次解决逆运动学任务，直到端点或终端效果器达到所需要的位置。有时在一帧中的这些顺序求解被称为迭代。在一帧中，执行逆运动学的若干次迭代，直到终端效果器或端点到达所需要的级别。通过计算机程序以无规律的间隔执行每次迭代，其中无规律的间隔是不固定的，并且依赖于软件的负载而改变。对于这些应用，可使用以下修改的算法在每次迭代时计算逆运动学或dq的值：

dq＝Jt(q)Pz(t-1)                         (9d)

z(t)＝z(t-1)+h(Az(t-1)-Jt(q)dq)+h dxd    (9e)

因此(等式9d)和(等式9e)表示根据本发明的另一实施例的K的格式和图4中的反馈环。

在本发明的该实施例中，除了之前实施例的组件之外：

12)存在诸如可存储h、具有维度(1)的存储块的参数存储部件。

13)存在诸如可存储参数bdq、具有维度(n)的存储块的参数存储部件。

14)存在诸如可存储dq0、具有维度(n)的存储块的参数存储部件。

15)存在诸如可存储dq full、具有维度(n)的存储块的参数存储部件。

15)存在诸如可存储V、具有维度(n)的存储块的参数存储部件。

等式8用于根据z(t)计算z(t-1)。h是需要对于应用而调整的常数；选择h使得该常数为正，并且小于或等于如技术人员所知的以下系统矩阵(A-J(q)Jt(q)P)所描述的设备(或系统)的最小时间常数的50％。h依赖于A、P和当前雅可比值J(q)的选择。也可固定h的值，例如h＝2e-2，然后选择A和P。在每次迭代可计算等式9d和9e，直到dq非常小或端点(或终端效果器)达到如当前帧所需要的希望位置。等式9d和9e是对于图4中的反馈逆运动学定律的第一阶积分求解。还可使用更高阶的积分。在等式9d中，dq可乘以增益(每个自由度的差)以调节增益中的差异，即，dq＝VJt(q)Pz(t-1)，其中V＝[v1，v2，...，vn]定义每个自由度n的增益。

接下来给出(P)和(A)的导数。

用于(等式9a-c)中(A)和(P)的导数的算法

(等式7a)和(等式9b)中的A是具有维度(m)乘以(m)的负定对角矩阵。(A)总共具有(m)个元素。

$A=\begin{array}{c}|-a1000...0|\\ |0-a200...0|\\ |0000...-\mathrm{am}|\end{array}---\left(11\right)$

(a1)、(a2)、(a3)、......、(am)为定义下限频率和(dx)和(dxd)之间的误差的正实标量。依赖于系统的动态表现、机器人或图形结构或控制力矩陀螺操控定律，(a1)到(am)必须被适当地调节。如果全部自由度(m)的表现被相等加权，则(a1)＝(a2)＝......＝(am)＝(a)可被设置为同一值(a)。对于具有快速万向节惯性的CMG控制系统，(a)可被设置为例如

a1＝a2＝......＝am＝a＝exp(0.05dt)      (12a)

(dt)是如上定义的采样周期，而(exp)代表指数运算符。

对于图形结构，可以使用以下示例值：

a1＝a2＝......＝am＝a＝exp(5dt)         (12b)

(等式7b)和(等式9c)中的(P)是如技术人员所知的正定对称矩阵。其可为满矩阵或对角矩阵。

为了找到(P)，首先计算被表示为(Pa)的矩阵，如技术人员所知，该矩阵是Riccati等式的解(Doyle，J.，Glover，K.，Khargonekar，P.，和Francis，B.，“State-space solutions to standard H2and H-infinity control problems”IEEETransactions on Automatic Control，Vol.34，1989，pp.831-847)。

Mt*Pa+Pa*M-Pa*(B2*(V)^-1*B2t-s^-2*B 1*B1t)*Pa+Ct*C＝0     (13)

对于(Pa)的导数，定义了6个矩阵，除了正的非零标量(s)以外，是(B1)、(B2)、(M)、(V)和(C)。

(B1)是具有维度(m)乘以(m)的单位对角矩阵。(B1t)是(B1)的转置。

(B2)是具有维度(m)乘以(n)的矩阵，并且等于(-1)乘以在一些非零配置(q)＝(q0)时计算的雅可比(J(q))。

B2＝-J(q0)            (14)

(q0)具有维度(n)并且可通过对于变量(q)的非零值的任意组合而构成。(B2t)是(B2)的转置。

重要的是，注意(J(q0))必须具有非零奇异值。

(C)具有维度(n+m+m)乘以(m)。(C)是如下正整数式(partition)

$C=\begin{array}{c}\left|0\right|\\ |w*I|\\ \left|0\right|\end{array}---\left(15\right)$

根据等于0的(n)乘以(m)个元素构成第一组。

具有维度(m)乘以(m)的第二组等于单位矩阵(I)乘以正标量(w)，所述单位矩阵(I)具有(m)乘以(m)个元素，所述正标量(w)定义图4中的循环的截止频率，例如w＝4rad/s。(w)需要依赖于应用而被适当地调节。

根据等于0的(m)乘以(m)个元素构成第三组。

(等式13)中的(M)是具有维度(m)乘以(m)的负定对角矩阵。M总共具有(m)个元素。

$M=\begin{array}{c}|-m1000...0|\\ |0-m200...0|\\ |0000.-\mathrm{mm}|\end{array}---\left(16\right)$

(m1)、(m2)、(m3)、......、(mm)为定义(dx)和(dxd)之间的误差的较低频率的衰减的正实标量。依赖于系统的动态表现、机器人或图形结构或控制力矩陀螺操控定律，(m1)到(mm)必须被适当地调节。如果全部自由度(m)的表现被相等加权，则(m1)＝(m2)＝......＝(mm)可被设置为同一值。对于具有快速万向节惯性的CMG控制系统，可被设置为例如：

m1＝m2＝......＝mm＝0.05     (17a)

对于图形结构，示例值是：

m1＝m2＝......＝mm＝5        (17b)

(Mt)是(M)的转置。

(等式13)中的(V)是具有维度(n)乘以(n)的正定对角矩阵。(V)总共具有(n)个元素。

$A=\begin{array}{c}|v1000...0|\\ |0v200...0|\\ |0000...\mathrm{vn}|\end{array}---\left(11\right)$

(v1)、(v2)、(v3)、......、(vm)为对于特定系统可实现的每个自由度(q)定义最大速率的平方(可能以rad/s或者m/s)的正实标量。依赖于系统的动态表现、机器人或图形结构或控制力矩陀螺操控定律，(v1)到(vn)必须被适当地调节。如果通过具有类似特征的机制构造全部自由度(n)，则(v1)＝(v2)＝......＝(vn)可被设置为同一值。对于具有快速万向节惯性的CMG控制系统，可被设置为例如：

v1＝v2＝......＝vn＝1.8*1.8 (19a)

对于图形结构，示例值为：

v1＝v2＝......＝vn＝1*1     (19b)

等式13中的(s)是标量。以下算法用于确定(s)。

步骤-1：首先将(s)设置为较大数，例如s＝100。

步骤-2：对于(s)的该值，通过求解(等式13)确定(Pa)。

步骤-3：如果解存在，并且(Pa)为正定矩阵，则减小(s)，例如(s)＝(s)/2，并且从步骤-2继续算法。

步骤-4：如果对于(s)的新值(等式13)未产生正定解，则使用最后的正定解(Pa)作为(等式13)中Riccati等式的解。

一旦根据(等式13)计算了(Pa)，则通过将(Pa)与(等式19a)或者(等式19b)中定义的一个自由度的最大速率相乘来计算(等式7b)和(等式9c)中的(P)，即：

P＝(v1)*Pa            (20)

在包括计算机动画的许多应用中，可将等式9d和等式9e中的P和A可以被选择为常数。这显著减少了计算需求。为了计算P和A，以下算法可被用作上述算法的变形：

步骤-1：选择A，典型地A为采用从0.0001到-1000的值的负变量。对于95个自由度的骨架的典型值为A＝-20。

步骤-2：取得在当前关节组合雅可比J(q)并计算

JJnorm＝sqrt(trace(J(q)Jt(q)))

其中trace(J(q)Jt(q))是矩阵的迹(trace)或者沿着(J(q)Jt(q))的主对角线的元素的和，而sqrt是平方根。

步骤-3：选择P使得

P＜＝(pi/h-A)/Jjnorm

A、h和P的适应：根据步骤-3明显的是，A和h是相互关连的。例如在典型的算法中，一旦运动学结构被定义(或者在计算机动画中的骨架帧被定义)，则：

步骤-1：A固定为例如：

A＝-20

步骤-1：h固定为例如：

h＝0.02

步骤-3：根据如下计算P：

P＜＝(pi/h-A)/Jjnorm

步骤-4：定义对于终端效果器的虚拟(dummy)轨迹并且利用以上状态执行。通过监视需要以由计算等式9d和等式9e的希望值处理的迭代数量，调节并增加P，以减少迭代的数目。如果响应不稳定(dp单调增长)，则减少h。

步骤-5：一旦达到稳定的响应，则调节A以改进迭代的数量。典型地增加A以减少迭代的数量。

仅对于给定的骨架执行一次以上适应性算法。

反馈逆运动学的实际实现方式(对于固定的采样间隔)

在(等式9a-9c)中定义的FIK算法作为滤波器工作，并且只需要乘法和累加指令。图5中示出算法的滤波器形式。

以下描述反馈逆运动学算法的实际实现方式。

步骤-0(初始化)：

将(z(k-1))设置为依从维度(m)的零矢量

将(dq(t-1))设置为依从维度(n)的零矢量

步骤-1：使用(q)的当前值计算雅可比(J(q))，通过来自传感器(相机、电动机光学编码器、电动机转速计或其他)的一些测量或者数学模型来提供(q)的当前值。雅可比依赖于对于其求解逆运动学问题的系统的结构。典型地，根据标量和诸如sin(q)和cos(q)的依赖于(q)的三角函数来构造雅可比。雅可比具有维度(m)乘以(n)。计算雅可比包括利用其数字形式代替(q)、求解(J(q))的每个元素并生成依从维度(m)乘以(n)的数字矩阵。

步骤-2：使用乘法部件、加法部件，并使用用于存储(dx)的部件将结果存储到(dx)，计算

dx＝J(q)dq(t-1)

这给出了具有维度(m)的矢量(dx)。

步骤-3：使用减法部件，通过从(dxd)中减去来自步骤-2的结果来计算(等式9a)中的误差，即，

e＝dxd-dx

使用存储部件来将结果存储到(e)。

步骤-4：使用存储部件(P)和存储部件(z(t-1))来计算(P)乘以(z(t-1))；将结果存储在(tmp)中。

tmp＝Pz(t-1)

该操作使用存储部件、乘法部件和加法部件。使用用于存储(tmp)的部件，来存储(tmp)，所述(tmp)是具有维度(m)的矢量。

步骤-5：将雅可比的转置(Jt(q))乘以如在步骤-4中计算的(tmp)来导出(等式9c)中的(dq)：

dq＝Jt(q)tmp

该操作使用乘法部件、加法部件和用于存储(dq)的部件。

步骤-6：使用(z(t-1))、(A)和在步骤-3中计算的(e)，根据(等式9b)计算z(t)，

z(t)＝Az(t-1)+e

该操作使用加法部件、乘法部件和用于存储(z(t))的部件

步骤-7：设置

z(t-1)＝z(t)

dq(t-1)＝dq

该操作使用用于存储(z(t-1))和(dq(t-1))的部件。这些值将被用于在(dt)秒后将发生的IK算法的下一迭代。

步骤-8：使用来自步骤-5的(dq)作为IK问题的解。在(dt)数量的秒后将发生的下一迭代从步骤-1继续。

图6中示出了使用实现对于一般逆运动学问题的以上算法C语音指令的计算机软件程序。运算的总数等于：

2*m*m+2*m-1+n*(2m-1)    (21)

如果(m＝6)，则需要(83+11*n)次运算来计算雅可比的逆并求解IK问题。

如果(m＝12)并且(n＝25)则需要(886)次运算来计算雅可比的逆并求解IK问题。

反馈逆运动学的实际实现方式(对于可变的采样间隔)

考虑使用可变采样间隔的应用，可使用以下算法。

步骤-0和步骤-1如上。

步骤-3：使用雅可比的转置(Jt(q))、P和z(t-1)，计算公式9d：

dq＝Jt(q)Pz(t-1)

步骤-4：使用雅可比J(q)、A、z(t-1)、h和dxd，计算等式9e：

z(t)＝z(t-1)+h(Az(t-1)-J(q)dq)+h dxd

实践中，z(t)是必须被用于步骤-3中的接下来的迭代的z(t-1)的值。

在本次迭代中，使用来自步骤-3的dp作为对于逆运动学问题的解。重复以上步骤，直到dq减少到阈值以下的小值或/和端点或终端效果器到达目标。

总共下来，使用每次迭代2*m*n+4*m次乘法和2*m*n-n+2*m次加法，来计算对于逆运动学问题的解。显然地，计算的复杂度与m和n成线性。

如Y.Nakamura和H.Hanafusa，“Inverse kinematic solution with singularityrobustness for robot manipulator control”，Journal of Dynamic systems，Measurements and Control，Vol.108，Sep.1986已经记载的，对于上述运算，将需要最少6000个指令、包括用于将消耗额外的乘法和累加指令的除法指令，来对于相同的维度(m＝12)和(n＝25)计算IK问题。因此，所提出的算法给出了通过以至少因子7对计算负担的减少。对于基于传统IK方法的计算负担的一些估算值也可在A.A.Maciejewski和J.M.Reagin，“Parallel Algorithm andArchitecturefor the control of kinematically redundant manipulators”，Roboticsand Automation，IEEE Transactions on，Volume 10，Issue 4，Aug 1994Page(s)：405-414中找到。再次，通过在本发明中提出的算法可实现与该论文中呈现的数目相比减少最少因子10的计算负担。

仅为了示范目的包括(等式21)中列出的运算总数，并且假设雅可比(J(q))具有非零元素的全满结构。这可以通过使用更有效的实现方式而被进一步优化，在所述更有效的实现方式中，在该算法中适当地解决(J(q))的零元素。

也可利用对(J(q0))的适当调节使得(P)对角。

通过利用2个或多个处理块，本实现方式还可通过采用并行、多处理方案改进。在本发明中提出的算法适于并行结构上的实现方式，而不需要额外的开销。这可使用若干方法来完成。例如：

考虑图6，在包括两个处理块的结构上，(e(t))和(z(t))的计算可与(tmp)和(dp)的计算并行运行。

例如考虑(J(q)dq(t-1))的计算，具有维度(m)乘以(n)的(J(q))可以分割为：

其中，每行具有(n)个元素。各行可被并行乘以(dq(t-1))。因此，如果系统具有(m)个可用处理器，则每个处理器可并行将(J(q))中的每行乘以(dq(t-1))，这导致比在单个处理器上运行环快(m)次。

同样的结论对于以上算法中的步骤-4和步骤-5中的计算成立。通过对于乘法的结果适当构造存储器指针，这可以零增量开销而实现。

对于定义关节约束、关节限制、轨迹和环境约束以及用于动作重新定标(retargeting)的空动作

为了引入关节约束、关节限制、环境约束以及用于动作重新定标，额外的同类项被添加到等式2中的主要解：

dq＝iJ(q)dxd+[I-iJ(q)J(q)]dq0

通过以等式4中的反馈形式代替iJ(q)，可通过以上提到的反馈形式来计算新的同类项[I-iJ(q)J(q)]dq0。

考虑到计算机动画和非固定采样间隔，完整解可被修改为：

bdq＝Jt(q)Pbz(t-1)                           (9f)

bz(t)＝bz(t-1)+h(Abz(t-1)-J(q)bdq)+hJ(q)dq0  (9j)

则最终的关节速度为：

dq_full＝dq-bdq+dq0

其中根据等式9D计算dq，根据等式9f计算bdq并且根据给定的约束来计算dq0，例如为了限制关节动作，例如在动作重新定标中，对于给定的关节数目(k)：

dq0(k)＝gain*(bq(k)-q(k))

其中bq(k)是对于关节的希望值(其可为常数或时变函数)，q(k)是关节变量(k)的当前值，而gain是要选择的正常数。

参考图7，本发明的一个实施例包括机器人，该机器人包括终端效果器714和3个关节711、712、713。因此，该机器人对应于图1中的机器人。每个关节具有3个自由度，并且包括3个致动器，每个控制1个自由度的动作。控制系统包括计算机720，该计算机720包括被布置为提供所需的全部处理部件的处理器722、以及被布置为包括所需的全部存储部件的存储器724。输入(在此情况下是使用者输入730的形式)被布置为生成到计算机720的、指示终端效果器714的希望动作的输入。传感器740被布置为测量关节711、712、713的位置和移动，并向指示其的计算机720提供信号。计算机被布置为使用上述方法来控制与关节711、712、713相关的致动器。

参考图8，本发明的另一实施例包括包含处理器822和存储器824的PC820。计算机被连接到显示器826和用户输入830。存储器824具有存储在其中的指令，以使得用户能够在计算机上玩游戏，其中图形图像832在显示器834上，以响应于来自用户输入830的输入受控移动。计算机被布置为使用以上描述的一个方法、响应于来自用户输入830的输入信号控制图像的移动，所述图像可包括与图2的图对应的图。

Claims (19)

1.一种用于控制系统的实时方法，所述系统包括：多个控制部件，每个具有至少一个可变参数(dq)；和受控元件，具有由所述控制部件控制的轨迹，其中所述轨迹通过可变矩阵与所述可变参数相关联，所述方法包括：定义将所述可变参数(dq)与所述轨迹(dx)相关联的控制转移矩阵(K)，并且使用反馈环，在反馈环中，计算依赖于作为希望轨迹(dxd)和当前轨迹(dx)之间的差异的误差(e)的反馈项。

2.如以上任意权利要求所述的方法，其中，所述矩阵是雅可比矩阵。

3.如权利要求1或2所述的方法，其中，通过多次循环重复计算所述反馈项，使得所述当前轨迹到达所述希望轨迹。

4.如以上任意权利要求所述的方法，其中，所述矩阵K的输出(dq)具有被指定为(n)的维度，所述希望轨迹dxd具有维度(m)，并且(m)小于或等于(n)。

5.如权利要求4所述的方法，包括：选择具有维度(m)乘以(n)的所述控制转移矩阵(K)，以及依赖于所述系统的属性确定(K)形式和数值。

6.如以上任意权利要求所述的方法，其中，对于给定的(dxd)生成(dq)的数值算法是滤波器的形式。

7.如权利要求6所述的方法，其中，所述算法仅需要乘法和累加类型的指令。

8.如以上任意权利要求所述的方法，被布置为在单个处理器或在并行平台上执行所述方法。

9.如以上任意权利要求所述的方法，其中，(K)被布置为即使(J(q))变为秩亏时也给出(dq)的解。

10.如以上任意权利要求所述的方法，其中，在奇异状态下，所述误差(e)在奇异方向上增长，并且(K)的全满结构生成(dq)的非零解，该非零解产生轨迹从奇异状态远离的动作。

11.如以上任意权利要求所述的方法，其使用下述形式的算法：

e＝dxd-J(q)dq(t-1)

z(t)＝Az(t-1)+e

dq＝Jt(q)Pz(t-1)

其中，

J为矩阵，

Jt为J的转置，

A为负定对角矩阵或者负常数，以及

P为正定全满矩阵或正对角矩阵或正常数。

12.如权利要求1-9中任意一个所述的方法，其使用下述形式的算法：

dq＝Jt(q)Pz(t-1)

z(t)＝z(t-1)+h(Az(t-1)-Jt(q)dq)+dxd

其中，

J为矩阵，

Jt为J的转置，

A为负定对角矩阵或者负常数，

P为正定全满矩阵或正对角矩阵或正常数，以及

h为正常数。

13.如以上任意权利要求所述的方法，其中，所述系统是被布置为显示可移动对象的图像的显示器，所述控制部件包括所述可移动对象的关节，并且所述受控对象包括所述可移动对象的元素。

14.如权利要求1-12中任意一个所述的方法，其中，所述系统是包括机器人和控制系统的机器人系统，所述受控元件是机器人的元件，并且所述控制部件包括机器人的关节。

15.如权利要求1-12中任意一个所述的方法，其中，所述控制部件包括控制力矩陀螺系统的陀螺。

16.一种用于可移动系统的控制系统，所述控制系统被布置为根据以上任意权利要求的方法而操作。

17.一种机器人系统，包括机器人和根据权利要求16的用于控制所述机器人的移动的控制系统。

18.一种控制力矩陀螺系统，包括多个陀螺和根据权利要求16的控制系统。

19.一种显示系统，包括：处理部件，被布置为执行权利要求1至11中任意一个所述方法从而生成图像数据、以及显示部件，被布置为显示图像。

Images