CN104339349B - 机器人装置及机器人控制方法 - Google Patents

Abstract

本发明提供一种机器人装置及机器人控制方法。机器人装置(1)包括多关节机器人(2)以及基于输入的运动命令对多关节机器人(2)进行驱动控制的控制器(3)。控制器(3)包括计算用于基于所述运动命令驱动多关节机器人(2)的各关节角命令的关节角计算单元(32)、通过基于由所述关节角计算单元(32)计算出的所述关节角命令对各旋转关节进行旋转驱动来移动多关节机器人(2)的伺服控制装置(30)、计算多关节机器人(2)和多关节机器人(2)的奇异点之间的距离的奇异点计算单元(51)、以及在所述奇异点距离变为小于预定值的情况下限制基于奇异点类型预先指定的旋转关节的最大旋转速度的最大关节角偏差调整单元(52)。

Description

机器人装置及机器人控制方法

技术领域

本发明涉及一种包括多关节机器人的机器人装置和多关节机器人的控制方法。

背景技术

近年来，使多关节机器人进行诸如装配工件的工作的机器人装置日益发展。这种机器人装置的最终目的在于使多关节机器人像人手一样移动，并且使多关节机器人高速进行诸如装配复杂工件的工作。

令人遗憾地，多关节机器人具有多个姿态(即所谓的奇异点(singular point))，在所述多个姿态中在三维空间中指定的一个位置和姿态无法被确定为关节空间中的一个位置和姿态，而这些奇异点阻碍了多关节机器人以高速进行复杂的移动。因此，在机器人装置的开发中对这些奇异点进行适当的处理是重要的。

在图13中示出了对多关节机器人进行控制的一般控制装置的框图。在图13中，当前位置C_i表示在三维空间中代表的机械手的手端梢的位置和姿态，偏差命令V_i表示手端梢的位置和姿态针对预先决定的各控制周期(例如2ms)的偏差。因为控制周期是恒定的，所以偏差命令V_i代表速度，并且使用示教器等针对各控制周期生成偏差命令V_i。

在这种配置中，位置命令计算单元150将偏差命令V_i与当前位置C_i相加，以获得下一位置C_i+1，并且逆向运动学计算单元155对下一位置C_i+1进行逆向运动学计算，以获得下一关节角命令q_i+1。然后，将其发送给对多关节机器人的关节进行控制的伺服控制装置130，由此对多关节机器人进行驱动控制。然而，如果多关节机器人位于奇异点附近，则关节角命令q_i+1从前一关节角命令q_i显著改变，因而多关节机器人可能会急剧移动。

为了应对上述问题，能够想到限制关节角命令速度的方法。然而，如果关节角命令速度受限，则多关节机器人可能从手端梢的原始目标位置显著偏离，并且一旦多关节机器人从其偏离，返回到原始目标位置就变得很难。根据公开的应对该问题的技术，如果关节角速度过大，则针对减速进行速度限制，由此防止多关节机器人在奇异点附近急剧移动(日本特开2003-300183号公报)。

令人遗憾地，在日本特开2003-300183号中公开的技术中，总是限制关节角速度，因此多关节机器人的运动在奇异点之外的位置和姿态也变慢。

发明内容

鉴于上述情况，本发明的目的是提供能够对奇异点进行适当处理并且能够提高多关节机器人的运动速度的机器人装置及机器人控制方法。

根据实施例的一个方面，提供一种机器人装置，所述机器人装置包括：多关节机器人，其包括多个旋转关节；以及控制装置，其基于输入的运动命令，对所述多关节机器人进行驱动控制，其中，所述控制装置包括：关节角计算单元，其计算所述多个旋转关节中的各个的关节角命令，所述关节角命令用于基于所述运动命令驱动所述多关节机器人；关节驱动控制单元，其通过基于由所述关节角计算单元计算出的所述关节角命令对所述多个旋转关节中的各个进行旋转驱动，来移动所述多关节机器人；奇异点计算单元，其针对所述多关节机器人的奇异点，计算所述多个旋转关节中的各个的奇异点角度和奇异点距离中的一个；以及关节速度限制单元，其在由所述奇异点计算单元计算出的所述奇异点角度和所述奇异点距离中的一个变为小于预定值的情况下，限制基于所述奇异点的类型预先指定的旋转关节的旋转速度。

根据实施例的另一方面，提供一种机器人控制方法，所述机器人控制方法用于基于输入的运动命令对包括多个旋转关节的多关节机器人进行驱动控制，所述机器人控制方法包括：控制单元计算所述多个旋转关节中的各个的关节角命令，所述关节角命令用于基于所述运动命令驱动所述多关节机器人；所述控制单元通过基于在计算所述关节角命令中计算出的所述关节角命令对所述多个旋转关节中的各个进行旋转驱动，来移动所述多关节机器人；所述控制单元针对所述多关节机器人的奇异点，计算所述多个旋转关节中的各个的奇异点角度和奇异点距离中的一个；以及在计算所述奇异点角度和所述奇异点距离中的一个中计算出的所述奇异点角度和所计算出的奇异点距离中的一个变为小于预定值的情况下，所述控制单元限制基于所述奇异点的类型预先指定的旋转关节的旋转速度。

本发明能够提供一种机器人装置和机器人控制方法，其能够对奇异点进行适当处理，并且能够提高多关节机器人的运动速度。

通过以下参照附图对示例性实施例的描述，本发明的其他特征将变得清楚。

附图说明

图1是示意性地示出根据本发明的第一实施例的机器人装置的总体结构的立体图。

图2是用于描述根据第一实施例的机器人臂的第一奇异点的说明图。

图3是用于描述根据第一实施例的机器人臂的第二奇异点的说明图。

图4是用于描述根据第一实施例的机器人臂的第三奇异点的说明图。

图5是示出根据第一实施例的关节控制装置的配置的框图。

图6是用于描述计算最大关节角偏差的函数的说明图。

图7是用于描述根据各个奇异点类型要进行速度限制的关节轴的说明图。

图8是示出根据第二实施例的关节控制装置的配置的框图。

图9是示出根据第三实施例的关节控制装置的配置的框图。

图10是示出根据第四实施例的关节控制装置的配置的框图。

图11是示出根据第五实施例的关节控制装置的配置的框图。

图12是示出根据第六实施例的关节控制装置的配置的框图。

图13是根据传统技术控制多关节机器人的控制装置的框图。

具体实施方式

现在，将根据附图详细描述本发明的优选实施例。

<第一实施例>

下文中，参照图1至图7描述根据本发明的第一实施例的机器人装置。首先，参照图1描述根据第一实施例的整个机器人装置1的示意性配置。图1是示意性地示出根据本发明的第一实施例的机器人装置1的总体结构的图。

如图1所示，机器人装置1包括：进行工件装配的6轴垂直多关节机器人(下文中称为“多关节机器人”)2、对多关节机器人2进行控制的控制器3、以及能够用于操作多关节机器人2的示教器4。

多关节机器人(多关节机械手)2包括6轴多关节机器人臂20和连接到机器人臂20的远端的末端执行器21。机器人臂20包括6个旋转关节(J1至J6)以及分别关于关节轴(J1轴至J6轴)旋转驱动的6个致动器(在本实施例中为伺服机构)。机器人臂20选择性地驱动各关节的伺服机构，由此将末端执行器21移动到期望的三维位置。末端执行器21可拆卸地附装到机器人臂20的远端，并且基于工作的内容是可替换的。在本实施例中使用的末端执行器是能够用3个手指抓握工件的所谓的手。

控制器(控制装置)3包括：对机器人臂20的各关节的伺服机构进行驱动控制的伺服控制装置(参见稍后要描述的图5)30、以及对机器人臂20的关节进行控制的关节控制装置(参见稍后要描述的图5)31。此外，控制器3包括对末端执行器21进行控制的末端执行器控制装置、存储单元、记录介质读取装置以及通信装置。

关节控制装置31对各旋转关节进行控制，使得当机器人臂20接近奇异点时，机器人臂20在避免在特定旋转关节处急剧移动的同时移动。注意，稍后详细描述关节控制装置31。伺服控制装置(关节驱动控制单元)30基于由关节控制装置31计算出的关节角命令(稍后要描述)，对各旋转关节的伺服机构进行旋转驱动，由此移动机器人臂20。

存储单元在其中存储用于执行机器人控制方法(稍后要描述)的程序(机器人控制程序)以及诸如机器人臂20的奇异点和用户预先设置的初始教示点的数据。记录介质读取装置用于读取在其中记录诸如机器人控制程序的各种程序的计算机可读记录介质的内容，并且将记录在记录介质中的程序和数据存储到存储单元中。通信装置例如用于在不使用上述记录介质的情况下，通过通信装置下载经由因特网分发的更新程序。

示教器(操作单元)4是针对用户的人机接口，并且由用户用来操作多关节机器人2。此外，示教器4具有称为点动操作的功能。在点动操作中，例如，能够在用户按下按钮的同时，在三维坐标中线性地移动多关节机器人2的手端梢位置。因此，多关节机器人2可以在点动操作期间接近奇异点，并且如果多关节机器人2接近奇异点，则特定关节可能急剧移动。在本实施例中，即使在多关节机器人2接近奇异点的情况下，关节控制装置31也使得能够在避免特定关节的急剧移动的同时，驱动机器人臂20。

接下来，参照图2至图7具体描述关节控制装置31。首先，参照图2至图4描述机器人臂20的奇异点和奇异点距离。图2是用于描述根据第一实施例的机器人臂20的第一奇异点的说明图。图3是用于描述根据第一实施例的机器人臂20的第二奇异点的说明图。图4是用于描述根据第一实施例的机器人臂20的第三奇异点的说明图。

通常，多关节机器人(多关节机械手)具有针对特定手端梢位置无法计算关节角的大量奇异点。例如，已知6轴多关节机器人2具有3个奇异点(第一奇异点至第三奇异点)，并且多关节机器人2可以同时接近这3个奇异点。下文中，描述多关节机器人2的这3个奇异点。

注意，下面，为了便于描述，将J1轴方向称为垂直方向，并且将与J1轴方向正交的方向称为水平方向。此外，一般的工业机器人被设计为使得J1轴方向与J2轴方向正交，并且J2轴方向与J3轴方向平行，因此基于这种条件给出描述。也可以采用J2轴相对于J1轴沿水平方向偏移的配置，但是可以以类似的方式对这种配置进行讨论，因此这里省略其描述。此外，通常，在许多情况下，J4轴、J5轴和J6轴被设计为在一个点(图2至图4所示的点P)处彼此相交，以便于进行逆向运动学(inverse kinematics)计算，因此基于这种条件给出描述。

如图2所示，第一奇异点对应于J4轴、J5轴和J6轴彼此相交的点(即点P位于J1轴上的情况)。在第一奇异点处，J1轴、J4轴、J5轴和J6轴在一个点(即点P)处彼此相交，因此4个旋转关节J1以及J4至J6的旋转角度不确定。这是因为旋转角度由三维空间中的3种类型的方向(变量)确定，而在第一奇异点处，变量的数量是4，其比3大1。在第一奇异点附近，手端梢的位置和姿态的微小改变导致旋转关节J1以及J4至J6的角度的显著改变。

此外，假设将点P定位到J1轴上所需的旋转关节J2的旋转角度是δ1，旋转角度δ1是示出到第一奇异点的距离的代表指标。因此，在本实施例中，特定旋转关节的旋转角度(下文中称为“奇异点角度”)被用作奇异点距离。根据经验，如果旋转关节J2的旋转角度(奇异点角度)δ1落到15度以下，则手端梢的位置和姿态的微小改变使旋转关节J1以及J4至J6的旋转角度显著改变，这需要关注。

如图3所示，第二奇异点对应于J4轴和J6轴位于同一轴上的情况。在第二奇异点处，J4轴和J6轴彼此一致，因此旋转关节J4和J6的旋转角度无法被确定为一个。在第二奇异点附近，手端梢的位置和姿态的微小改变导致旋转关节J4和J6的角度的显著改变。此外，假设将J4轴和J6轴定位到同一轴上所需的旋转关节J5的旋转角度是δ2，根据经验，如果旋转关节J2的旋转角度(奇异点角度)δ2落到15度以下，则手端梢的位置和姿态的微小改变使旋转关节J4和J6的旋转角度显著改变，这需要关注。

如图4所示，第三奇异点对应于J2轴和J3轴位于J4轴上的情况。在第三奇异点处，因为J2轴和J3轴位于J4轴上，因此无法确定旋转关节J2和J3的旋转角度。在第三奇异点附近，手端梢的位置和姿态的微小改变导致旋转关节J2和J3的角度的显著改变。此外，假设将J2轴和J3轴定位到J4轴上所需的旋转关节J3的旋转角度是δ3，根据经验，如果旋转关节J2的旋转角度(奇异点角度)δ3落到15度以下，则手端梢的位置和姿态的微小改变使旋转关节J2和J3的旋转角度显著改变，这需要关注。

接下来，参照图5，描述当机器人臂20接近这3个奇异点时，能够在抑制机器人臂20的急剧移动的同时，对机器人臂20进行驱动控制的关节控制装置31。图5是示出根据第一实施例的关节控制装置31的配置的框图。

首先，描述关节控制装置31的配置。如图5所示，关节控制装置31包括：基于运动命令计算用于驱动机器人臂20的各旋转关节的关节角命令的关节角计算单元32、以及计算各旋转关节对各奇异点的奇异点角度的奇异点计算单元51。关节控制装置31还包括：进行根据奇异点角度使针对各旋转关节预先给定的最大关节角偏差(最大旋转速度)的极限值更小的处理的最大关节角偏差调整单元52。

关节角计算单元32包括位置命令计算单元50、校正水平调整单元53、位置命令校正单元54、逆向运动学计算单元55、关节角校正单元56、正向运动学(forward kinematics)计算单元57以及校正量计算单元58。

位置命令计算单元50将机器人臂20的手端梢的偏差命令与由三维坐标表示的其手端梢的当前位置相加，由此计算位置命令。校正水平调整单元53进行根据奇异点角度来进行计算校正水平(其是校正的比率，并且下文中将其称为“校正比率”)的处理。具体来说，校正水平调整单元53进行随着奇异点角度越小使校正比率越小的处理。位置命令校正单元54将前一校正量和校正比率的积，与由位置命令计算单元50计算出的位置命令相加，由此计算校正后的位置命令。逆向运动学计算单元55将由位置命令校正单元54校正后的位置命令变换为临时(tentative)关节角命令。关节角校正单元56根据由逆向运动学计算单元55计算出的临时关节角命令和前一关节角命令，来计算关节角偏差，并且使临时关节角命令更小，使得关节角偏差不超过由最大关节角偏差调整单元52计算出的最大关节角偏差，由此计算下一关节角命令。正向运动学计算单元57根据修正后的关节角命令，计算下一关节角命令和下一位置。校正量计算单元58将校正量与由正向运动学计算单元57计算出的下一位置和位置命令之差相加，由此计算下一校正量。

如果基于奇异点类型预先指定的旋转关节的奇异点角度变为小于预定值，则最大关节角偏差调整单元(关节速度限制单元)52使最大关节角偏差(最大旋转速度)的极限值小于预先设置的最大关节角偏差的极限值。

接下来，除了图5，还参照图6和图7，描述如上所述构成的关节控制装置31进行的对机器人臂20的驱动控制(机器人控制方法)。注意，下面给出的各标记的下标i代表计算的次数，并且每次一系列计算结束时将下标i递增1。此外，针对例如2ms的各控制周期进行该计算。图6是用于描述计算最大关节角偏差的函数的说明图。图7是用于描述根据各奇异点类型要进行速度限制的关节轴的说明图。

如果向示教器4输入运动命令，则基于输入的运动命令，从示教器4向关节控制装置31输入偏差命令V_i、当前位置C_i、前一校正量E_i和前一关节角命令q_i。因此，关节控制装置31基于输入的数据，计算下一关节角命令q_i+1、下一位置C_i+1和下一校正量E_i+1，并且向伺服控制装置30输出计算结果。伺服控制装置30基于输入的数据对旋转关节进行旋转驱动，由此移动机器人臂20。下文中，具体描述关节控制装置31计算下一关节角命令q_i+1、下一位置C_i+1和下一校正量E_i+1的方法。

如果从示教器4输入了偏差命令V_i、当前位置C_i、前一校正量E_i和前一关节角命令q_i，则首先，关节控制装置31将手端梢的偏差命令V_i和手端梢的当前位置C_i相加，由此计算位置命令D_i(位置命令计算步骤)。这里，当前位置数据和偏差命令数据是具有由X、Y和Z方向上的运动和旋转构成的6个分量的矢量，并代表位置和姿态。代表该位置和姿态的其他方法涉及使用4×4齐次坐标变换矩阵。在这种方法中，通过矩阵乘法表示矢量的总和。这仅在如何表示位置和姿态中不同，而与本发明的要旨无关。因此，在本实施例中，使用容易直观地理解的矢量表达式用于表示位置和姿态。

随后，关节控制装置31计算奇异点角度，并且确定机器人臂20是否在奇异点附近(奇异点计算步骤)。在本实施例中，关节控制装置31根据三种类型的奇异点，来计算相应旋转关节的奇异点角度。注意，可以使用当前关节角作为相应旋转关节的奇异点角度的代替。这是因为根据本实施例的多关节机器人2的控制周期为非常短时间的2ms，并且关节角的改变也小。

在计算奇异点角度之后，关节控制装置31然后进行根据奇异点角度，使针对各旋转关节预先给出的最大关节角偏差(最大旋转速度)的极限值更小的处理(关节速度调整步骤)。这里，基于电机产生的转矩和编码器能够处置的最大旋转速度的限制，针对各旋转关节确定最大旋转速度(即最大关节角偏差的极限值)。在这种情况下，根据奇异点角度，使其极限值更小。

例如，如图6所示，假设针对各旋转关节给出的最大旋转角度偏差(最大旋转速度)是V_max，奇异点角度为零时的旋转角度偏差是V_min，并且确定奇异点附近的角度是θs。根据下面的表达式计算针对奇异点角度θ的减速的最大旋转角度偏差V_lim。

[表达式1]

$V_{\lim }=(V_{\max }-V_{\min })(-2{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^3+3{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^2)+V_{\min },\mathrm{\&theta;}\&le;{\mathrm{\&theta;}}_s$

V_lim＝V_max θ＞θ_s

在将确定奇异点附近的角度θs设置为例如15度的情况下，如果奇异点角度落到15度以下，则限制特定旋转关节的旋转速度。此外，针对基于奇异点类型预先指定的各旋转关节，进行使极限值更小的处理。例如，如图7所示，在第一奇异点的情况下，使旋转关节J1、J4、J5和J6的最大关节角偏差(最大旋转速度)的极限值更小。注意，将这时的各极限值的最小值设置为零。此外，在第二奇异点的情况下，使旋转关节J4和J6的最大关节角偏差(最大旋转速度)的极限值更小。注意，将这时的各极限值的最小值设置为零。此外，在第三奇异点的情况下，使旋转关节J2和J3的最大关节角偏差(最大旋转速度)的极限值更小。注意，将这时的各极限值的最小值不设置为零，而设置为例如诸如每秒一度的低速。这是因为如果将该速度设置为零，则无法从第三奇异点脱离。

此外，机器人臂20可以同时接近第一奇异点至第三奇异点。例如，在机器人臂20同时接近第一奇异点和第二奇异点的情况下，关节控制装置31计算旋转关节的最大关节角偏差(最大旋转速度)，并且将最小的偏差定义为极限值。

随后，关节控制装置31根据奇异点角度计算校正比率(校正水平调整步骤)。如上所述，存在3个奇异点，并且存在与这3个奇异点相对应的奇异点角度。这里，将奇异点角度的最小值视为奇异点角度。此外，校正比率是从0到1的任意数字。在奇异点附近，校正比率为零，而在远离奇异点的位置，校正比率为1。例如，假设确定奇异点附近的奇异点角度为θs，并且三种类型的奇异点的奇异点角度中的最小的一个为θ，则可以用下面的表达式来代表校正比率r。

[表达式2]

$r=-2{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^3+3{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^2,\mathrm{\&theta;}\&le;{\mathrm{\&theta;}}_s$

r＝1 θ＞θ_s

注意，该函数的形式与图6中的V_max＝1和V_min＝0的情况相同，因此省略其描述。此外，将奇异点角度θs设置为例如15度。

随后，关节控制装置31将前一校正量E_i和校正比率r的积与在位置命令计算步骤中计算出的位置命令D_i相加，由此计算校正后的位置命令F_i(位置命令校正步骤)。

[表达式3]

F_i＝D_i+rE_i

作为校正水平调整步骤的结果，校正比率在奇异点附近变得更小。因此，如果将前一校正量乘以校正比率，则校正量在奇异点附近变得更小。因此，根据传统技术，如果校正量在奇异点附近变得更大，则关节的运动可能变得不稳定。相比来说，在本实施例中，校正量在奇异点附近变得更小，因此能够使关节的运动稳定。

随后，关节控制装置31将在位置命令校正步骤中校正后的位置命令F_i变换为临时关节角命令

[表达式4]

(逆向运动学计算步骤)。随后，关节控制装置31根据临时关节角命令

[表达式5]

和前一关节角命令q_i来计算关节角偏差，并且使临时关节角命令更小，使得关节角偏差不超过在最大关节角偏差调整步骤中计算出的最大旋转角度偏差V_lim，由此计算下一关节角命令q_i+1(关节角校正步骤)。这里，需要根据关节角偏差不超过其上限的条件，使下面的表达式成立。

[表达式6]

q_i-V_lim≤q_i+1≤q_i+V_lim

关节控制装置31根据下面的表达式计算下一关节角命令q_i+1，使得满足表达式6。

[表达式7]

$q_{i+1}={\hat{q}}_i$ 在 $q_i-V_{\lim }\&le;{\hat{q}}_i\&le;q_i+V_{\lim }$ 的情况下

q_i+1＝q_i-V_lim在的情况下

q_i+1＝q_i+V_lim在的情况下

在计算下一关节角命令q_i+1之后，关节控制装置31根据下一关节角命令q_i+1，计算下一位置C_i+1(正向运动学计算步骤)。随后，关节控制装置31将校正量E_i与在正向运动学计算步骤中计算出的下一位置C_i+1和位置命令D_i+1之差相加，由此计算下一校正量E_i+1(校正量计算步骤)。

[表达式8]

E_i+1＝D_i+E_i-C_i+1

这里，如果机器人臂20不在奇异点附近，则校正比率为1，因此由下面的表达式代表校正后的位置命令。

[表达式9]

F_i＝D_i+E_i

此外，因为在关节角校正步骤中不进行减速，因此其通过逆向运动学计算步骤和正向运动学计算步骤返回到前述的值C_i+1＝D_i+E_i。如果将该C_i+1代入表达式8，则E_i+1变为零。也就是说，如果机器人臂20远离奇异点，则校正量变为零。

如上所述，根据第一实施例的机器人装置1计算奇异点角度(距离)，根据奇异点角度调整最大关节角偏差，并且对关节角偏差进行校正，使得关节角偏差等于或小于其最大值。因此，例如，如果将最大关节角偏差在奇异点附近设置为小，而在其他区域中设置为大，则能够解决与奇异点相比，多关节机器人的运动在其他区域中变得更慢的问题。其结果是，能够提高多关节机器人的运动速度。此外，在奇异点附近，针对基于奇异点类型指定的各旋转关节限制最大关节角偏差(即关节角速度)，因此能够防止机器人臂急剧移动。

此外，如果对关节角偏差进行修正，则手端梢的位置从原始目标位置偏离，从而需要对手端梢的位置进行校正。在传统技术中，即使在奇异点附近，也对手端梢位置进行校正，因此发生关节速度的显著改变。相比来说，在本实施例中，计算奇异点角度，相应地调整校正比率，并且考虑到校正比率对位置命令进行校正。例如，将校正比率在奇异点附近设置为小，而在其他区域中设置为大。其结果是，校正量在奇异点附近变得更小，因此能够防止关节速度增大。此外，能够防止多关节机器人2停止，以避免关节速度增大，因此能够提高生产率。此外，如果机器人臂20远离奇异点，则校正水平变得更大，因此手端梢的位置能够跟随原始目标位置。

此外，根据第一实施例的机器人装置1针对各奇异点类型计算奇异点角度，因此能够针对各奇异点类型限制不同的关节角偏差(即不同旋转关节的旋转速度)。因此，即使在奇异点附近，在使用不减速的旋转关节的情况下，也能够从奇异点迅速脱离。

此外，根据第一实施例的机器人装置1能够根据奇异点角度中的最小的一个，使校正量更小。因此，即使在奇异点附近，计算也是稳定的，并且手端梢速度不需要减速，从而运动速度提高。

<第二实施例>

接下来，参照图8描述根据第二实施例的机器人装置。第二实施例与第一实施例的不同之处在于，用雅克比(Jacobian)矩阵计算单元59和雅克比矩阵相乘单元60替换关节控制装置31的正向运动学计算单元57。因此，在第二实施例中，描述雅克比矩阵计算单元59和雅克比矩阵相乘单元60。图8是示出根据第二实施例的关节控制装置31A的配置的框图。

在正向运动学计算中，从基座处的关节开始依次计算位置和姿态，由此计算手端梢的位置和姿态。然后，已知通过使用雅克比矩阵的乘法，能够近似计算手端梢的位置和姿态。雅克比矩阵计算单元59计算该雅克比矩阵。在6轴多关节机器人臂20的情况下，雅克比矩阵是6×6矩阵，如果将该矩阵乘以关节角的偏差矢量(关节速度)，则获得手端梢位置的偏差矢量(手端梢速度)。

雅克比矩阵相乘单元60将雅克比矩阵乘以在关节角校正步骤中计算出的关节角命令与前一关节角命令之差，也就是说，将雅克比矩阵乘以关节角的偏差矢量，并且将当前位置与获得的积相加，由此计算下一位置。例如，可以根据下面的表达式计算下一位置。

[表达式10]

C_i+1＝C_i+J(q_i+1-q_i)

根据第二实施例，因为正向运动学计算是包括三角函数的复杂的坐标变换，因此使用雅克比矩阵计算单元59和雅克比矩阵相乘单元60作为正向运动学计算单元57的代替，由此能够减少计算量。

<第三实施例>

接下来，参照图9描述根据第三实施例的机器人装置。第三实施例与第一实施例的不同之处在于，增加了校正量修正单元61以使校正量更小。因此，在第三实施例中，描述校正量修正单元61。图9是示出根据第三实施例的关节控制装置31B的配置的框图。

这里，假设操作员按下示教器4的按钮并移动多关节机器人2的手端梢位置(末端执行器21)的场景。在按下示教器4的按钮的同时，偏差命令继续输出零之外的数字。如果机器人臂20继续移动并且接近奇异点，则手端梢速度降低旋转关节的速度限制，并且在校正量中累积与这时的受限运动相对应的量。然后，如果机器人臂20远离奇异点，则继而减小校正量，因此手端梢速度增加。例如，如果校正量返回到零，则手端梢速度也返回到其原始速度。

考虑上述一系列运动，在机器人臂20远离奇异点附近的时刻，机器人臂20的速度突然增加，优选地，不在操作员期望的运动方向上累积校正量。在第三实施例中，配设去除偏差命令的方向分量的校正量修正单元61，并且校正量修正单元61进行下面的矢量计算。首先，校正量修正单元61计算偏差命令V_i的方向矢量v_i。

[表达式11]

$v_i=\frac{V_i}{\left|V_i\right|}$

随后，校正量修正单元61根据未修正的校正量

[表达式12]

和方向矢量v_i，计算修正后的校正量E_i+1。

[表达式13]

$E_{i+1}={\hat{E}}_{i+1}-({\hat{E}}_{i+1}\&CenterDot;v_i)v_i$

表达式13是指为从校正量中去除了偏差命令的方向分量。根据第三实施例，能够使校正量更小，因此能够抑制在机器人臂20远离奇异点附近的时刻，速度不必要地增加。特别从示教器的操作员的观点来看，能够抑制操作员期望的方向(即偏差命令的方向)上的速度增加。此外，因为校正量变得更小，因此奇异点附近的校正计算变得更稳定。此外，因为校正量变得更小，所以能够缩短返回到原始目标位置所需的时间。

<第四实施例>

接下来，参照图10描述根据第四实施例的机器人装置。第四实施例与第一实施例的不同之处在于，配设了除最大关节角偏差的限制之外还限制偏差改变的最大关节角偏差改变调整单元62。因此，在第四实施例中，描述最大关节角偏差改变调整单元62。图10是示出根据第四实施例的关节控制装置31C的配置的框图。

最大关节角偏差改变调整单元62根据奇异点角度，使针对各旋转关节预先给出的最大关节角偏差改变(最大旋转速度改变)更小。因为控制周期是恒定的，因此关节角偏差改变是加速度，并且基于各旋转关节的电机可产生的转矩等的条件，针对各旋转关节确定其最大值。这里，根据奇异点角度，使关节角偏差改变的极限值进一步更小，以进行减速。具体的减速方法与在偏差(旋转速度)的情况下参照图6描述的方法相同。

假设针对各旋转关节给出的最大旋转角度偏差改变为A_max，横轴的奇异点角度θ为零时的关节角偏差改变为A_min，并且确定奇异点附近的奇异点角度为θs。根据下面的表达式，计算针对奇异点角度θ的减速的最大旋转角度偏差改变A_lim。

[表达式14]

$A_{\lim }=(A_{\max }-A_{\min })(-2{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^3+3{\left(\frac{\mathrm{\&theta;}}{{\mathrm{\&theta;}}_s}\right)}^2)+A_{\min },\mathrm{\&theta;}\&le;{\mathrm{\&theta;}}_s$

A_lim＝A_maxθ＞θ_s

例如，在将奇异点角度θs设置为15度的情况下，如果奇异点角度落到15度以下，则旋转关节的旋转速度变高。在第四实施例中，除了最大关节角偏差之外，还限制最大关节角偏差改变。根据下面的表达式，根据下一关节角命令q_i+1、前一关节角命令q_i和前一关节角命令之前的关节角命令q_i-1，计算偏差改变。

[表达式15]

q_i+1+q_i-1-2q_i

在表达式15不超过由最大关节角偏差改变调整单元62计算出的极限值的条件下，根据表达式17计算下一关节角命令q_i+1，以满足表达式16。

[表达式16]

2q_i-q_i-1-A_lim≤q_i+1≤2q_i-q_i-1+A_lim

[表达式17]

q_i+1＝2q_i-q_i-1+A_lim在的情况下

q_i+1＝2q_i-q_i-1-A_lim在的情况下

$q_{i+1}={\hat{q}}_i$ 在 ${2q}_i-q_{i-1}-A_{\lim }\&le;{\hat{q}}_i\&le;{2q}_i-q_{i-1}+A_{\lim }$ 的情况下

进一步对最大关节角偏差进行限制。这与第一实施例中的关节角校正步骤相同，因此省略其描述。注意，对于上述两种类型的限制的优先顺序，在第四实施例中，在偏差改变的限制之后，计算对最大关节角偏差的限制，因此对最大关节角偏差的限制设置更高的优先级。根据第四实施例，除了最大关节角偏差之外，还限制最大关节角偏差改变，因此能够进行更平滑的速度控制。

<第五实施例>

接下来，参照图11描述根据第五实施例的机器人装置。第五实施例与第一实施例的不同之处在于，在奇异点计算步骤之前，进行位置命令校正步骤和逆向运动学计算步骤。因此，在第五实施例中，描述在奇异点计算步骤之前，进行位置命令校正步骤和逆向运动学计算步骤的情况。图11是示出根据第五实施例的关节控制装置31D的配置的框图。

通过逆向运动学计算，还能够获得奇异点角度。因此，如果在该计算之后计算奇异点角度，则能够使用最新关节角计算奇异点角度。在第五实施例中，在位置命令校正步骤中，使用前一校正比率r_i计算校正后的位置命令F_i。

[表达式18]

F_i＝D_i+r_iE_i

注意，因为控制周期为2ms，这是非常短的时间，因此在使用前一校正比率时不存在问题。原因如下：因为2ms内的关节角的改变小，因此奇异点角度的改变也小，并且校正比率的改变也小。第五实施例能够产生与第一实施例的效果类似的效果。

<第六实施例>

接下来，参照图12描述根据第六实施例的机器人装置。第六实施例与第一实施例的不同之处在于，在校正量计算步骤之后，提供衰减计算步骤。因此，在第六实施例中，描述进行衰减计算步骤的衰减计算单元63。图12是示出根据第六实施例的关节控制装置31E的配置的框图。

在第一实施例中，在奇异点附近，校正比率小，并且随着机器人臂20远离奇异点，校正比率变得更大。这里，讨论机器人臂20在长时间内保持在奇异点附近，然后远离奇异点的情况。在这种情况下，对这种长时间保持之前的校正量施加校正，并且机器人臂20移动。例如，在多关节机器人2的操作员给出偏差命令的情况下，机器人臂20在经过长时间之后移动不利于操作员。在这种情况下，校正量自然随着时间衰减。

鉴于上述内容，在第六实施例中，衰减计算单元63以预定比率进行校正量的衰减计算。具体来说，衰减计算单元63将nE_i+1代入修正后的校正量E_i+1。在校正量通过n次衰减减半的情况下，比率是n＝0.5^(1/n)。例如，在2ms的控制周期下，在4秒内校正量通过衰减减半的情况下，次数n为4/0.002＝2,000次，并且比率是n＝0.999653。

在上文中，对本发明的实施例进行了描述，本发明不限于上述实施例。此外，在本发明的实施例中描述的效果仅是本发明产生的示例性效果，本发明的效果不限于在本发明的实施例中描述的效果。

例如，在第五实施例中，作为使用前一控制周期计算的C_i，描述了当前位置。作为另选方案，伺服控制装置可以计算实际旋转角度，并且可以通过正向运动学计算获得当前位置。即使在这种情况下，也维持本发明的要旨，并且获得相同的效果。

其他实施例

本发明的实施例还能够由读出并执行记录在存储介质(例如非易失性计算机可读存储介质)上的计算机可执行指令以执行本发明的一个或多个上述实施例中的功能的系统或装置的计算机，以及通过由系统或装置的计算机通过例如从存储介质中读出并执行计算机可执行指令以执行一个或多个上述实施例的功能的方法来实现。计算机可以包括中央处理单元(CPU)、微处理单元(MPU)或其他电路中的一个或多个，并且可以包括单独的计算机或单独的计算机处理器的网络。例如可以从网络或存储介质向计算机提供计算机可执行指令。存储介质例如可以包括硬盘、随机存取存储器(RAM)、只读存储器(ROM)、分布式计算系统的存储设备、光盘(例如紧凑盘(CD)、数字通用盘(DVD)或蓝光盘(BD)^TM)、闪存设备、存储卡等中的一个或多个。

虽然参照示例性实施例对本发明进行了描述，但是应当理解，本发明不限于所公开的示例性实施例。应当对所附权利要求的范围给予最宽的解释，以使其涵盖所有这种变型以及等同结构和功能。

Claims (5)

1.一种机器人装置，所述机器人装置包括：

多关节机器人，其包括多个旋转关节；以及

控制装置，其基于输入的运动命令，对所述多关节机器人进行驱动控制，

其中，所述控制装置包括：

位置命令计算单元，其基于所述运动命令计算位置命令；

奇异点计算单元，其针对所述多关节机器人的奇异点计算所述多个旋转关节中的各个的奇异点角度和奇异点距离中的一个；

校正水平调整单元，其计算校正比率，所述校正比率是根据由所述奇异点计算单元计算出的所述奇异点角度和所述奇异点距离中的所述一个来进行校正的比率；

位置命令校正单元，其通过将前一校正量和由所述校正水平调整单元计算出的所述校正比率的乘积与所述位置命令相加，来对所述位置命令进行校正；

逆向运动学计算单元，其将由所述位置命令校正单元校正的所述位置命令变换为临时关节角命令；

关节角校正单元，其通过根据所述临时关节角命令和前一关节角命令计算关节角偏差，并且以所述关节角偏差不超过基于所述奇异点的类型预先指定的旋转关节的旋转速度的方式使所述临时关节角命令更小，来计算下一关节角命令；

正向运动学计算单元，其根据由所述关节角校正单元计算出的所述下一关节角命令，来计算下一位置；以及

校正量计算单元，其通过将所述前一校正量与所述下一位置和所述位置命令之差相加，来计算下一校正量。

2.一种机器人控制方法，所述机器人控制方法用于基于输入的运动命令对包括多个旋转关节的多关节机器人进行驱动控制，所述机器人控制方法包括以下步骤：

基于所述运动命令计算位置命令；

针对所述多关节机器人的奇异点计算所述多个旋转关节中的各个的奇异点角度和奇异点距离中的一个；

计算校正比率，所述校正比率是根据在计算所述奇异点角度和所述奇异点距离中的一个中计算出的所述奇异点角度和所述奇异点距离中的一个来进行校正的比率；

通过将前一校正量和在计算所述校正比率中计算出的所述校正比率的乘积与所述位置命令相加，来对所述位置命令进行校正；

通过将在校正所述位置命令中校正的所述位置命令变换为临时关节角命令，来进行逆向运动学计算；

通过根据所述临时关节角命令和前一关节角命令计算关节角偏差，并且以所述关节角偏差不超过基于所述奇异点的类型预先指定的旋转关节的旋转速度的方式使所述临时关节角命令更小，来计算下一关节角命令；

通过根据在计算所述下一关节角命令中计算出的所述下一关节角命令计算下一位置，来进行正向运动学计算；以及

通过将所述前一校正量与所述下一位置和所述位置命令之差相加，来计算下一校正量。

3.根据权利要求2所述的机器人控制方法，其中，进行所述正向运动学计算包括通过如下步骤来计算所述下一位置：获得雅克比矩阵；将所述雅克比矩阵乘以所述前一关节角命令和在计算所述下一关节角命令中计算出的所述下一关节角命令之差；以及将当前位置与获得的积相加。

4.根据权利要求2所述的机器人控制方法，所述机器人控制方法还包括：

从计算所述下一校正量的计算结果中去除偏差命令的方向分量。

5.根据权利要求2所述的机器人控制方法，所述机器人控制方法还包括：

以预定比率对计算所述下一校正量的计算结果进行衰减计算。