CN109129525A - 机器人的负载重心位置推定装置及负载重心位置推定方法 - Google Patents

Abstract

在顶端有质量已知的工件的状态下，对机器人臂从第一初始位置使第二轴动作并到达第一推定用位置及从第一推定用位置再次到达第一初始位置为止的第一动作期间的加速区域及减速区域取得第二轴的加速度、第四轴的加速转矩及减速转矩，算出绕第四轴的惯量。从第二初始位置使第二轴动作进行第二动作，取得第二动作期间的第二轴的加速度、第四轴的加速转矩及减速转矩，算出绕第四轴的惯量。从第三初始位置使第二轴动作进行第三动作，取得第三动作期间的第二轴的加速度、第四轴的加速转矩及减速转矩，算出绕第四轴的惯量。基于第四轴至工件的重心距离、工件重心方向、上述各绕第四轴的惯量及第四轴位置算出工件重心位置。

Description

机器人的负载重心位置推定装置及负载重心位置推定方法

技术领域

本发明涉及在水平四轴结构的机器人臂的顶端存在负载的状态下推定所述负载的重心位置的装置以及方法。

背景技术

例如专利文献1等公开了以往在垂直六轴型的机器人臂抓持作为负载的工件的状态下推定该工件的重心位置的技术。

在先技术文献

专利文献

专利文献1：日本专利第5327722号公报

发明内容

发明所要解决的技术问题

在垂直六轴型的机器人臂中，由于能够通过第五轴的动作来使重力作用于工件的状态发生变化，因此能够推定工件的重心位置。专利文献1等中公开的技术以这样的结构为前提，因此，无法将上述技术应用于如水平四轴结构那样不能使重力作用于工件的状态发生变化的机器人臂。

本发明是鉴于上述技术问题而作出的，其目的在于提供一种机器人的负载重心位置推定装置以及机器人的负载重心位置推定方法，其对于由水平四轴结构的机器人臂处理的负载，也能够推定重心位置。

用于解决技术问题的方案

根据第一项技术方案记载的机器人的负载重心位置推定装置，第一加速度取得部在水平四轴结构的机器人臂的顶端存在质量m已知的负载的状态，对第一动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第一动作期间为，机器人臂从将第四轴位置设为θ_a的第一初始位置使第二轴动作从而到达第一推定用位置为止、以及从第一推定用位置再次到达第一初始位置为止的期间。

第一转矩取得部取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩。第一惯量计算部基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量(inertia)τ_I4a。

第二加速度取得部对第二动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第二动作期间为，从对于所述第一初始位置将第四轴位置变为θ_b的第二初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第二推定用位置为止、以及从第二推定用位置再次到达第二初始位置为止的期间。第二转矩取得部对于第二动作期间也与第一转矩取得部同样地取得第四轴的加速转矩、减速转矩。第二惯量计算部对于第二动作期间也与第一惯量计算部同样地计算出围绕第四轴的惯量τ_I4b。

第三加速度取得部对第三动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第三动作期间为，从对于所述第一初始位置将第四轴位置变为θ_c的第三初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第三推定用位置为止、以及从第三推定用位置再次到达第三初始位置为止的期间。第三转矩取得部对于第三动作期间同样地取得第四轴的加速转矩、减速转矩。第三惯量计算部对于第三动作期间也同样地计算出围绕第四轴的惯量τ_I4c。而且，重心位置计算部基于从第四轴到负载的重心距离l_h、所述负载的重心方向θ_h、惯量τ_I4a、τ_I4b及τ_I4c、以及第四轴位置θ_a、θ_b及θ_c，计算出负载的重心位置x_cog、y_cog。

即，对于彼此不同的三个第四轴位置，基于使负载在水平方向上同样地移动而施加到第四轴的加速度，获得各个惯量τ_I4a、τ_I4b及τ_I4c。而且，基于上述各个惯量、第四轴位置θ_a、θ_b及θ_c，能够通过下式求出重心位置x_cog、y_cog。

x_cog＝l_hcosθ_h

y_cog＝l_hsinθ_h

因此，对于具备水平四轴结构的臂的机器人，也能够推定负载的水平面内重心位置x_cog、y_cog。

根据第二项技术方案记载的机器人的负载重心位置推定装置，第一至第三加速度取得部分别将某一个第四轴位置θ_a、θ_b及θ_c设定为0°、90°、-90°。由此，能够立即求出重心位置x_cog、y_cog，计算处理变得简单。

根据第三项技术方案记载的机器人的负载重心位置推定装置，第一至第三加速度取得部将第三轴固定于上端位置来进行动作。由此，在使臂动作到第一至第三推定用位置时，能够通过三轴的动作来抑制负载上产生的振动，因此，能够以更高的精度推定重心位置。

附图说明

图1是示出一个实施方式的重心位置推定处理的内容的流程图(其一)。

图2是示出重心位置推定处理的内容的流程图(其二)。

图3是用四轴臂模型示出第一动作期间的动作的图。

图4是示出第二轴的速度波形的图。

图5是示出第二轴的加速度波形的图。

图6是示出第四轴的转矩波形的图。

图7是用四轴臂模型示出第二动作期间的动作的图。

图8是用四轴臂模型示出第三动作期间的动作的图。

图9是示出工业用机器人的系统结构的图。

图10是示出四轴臂的模型的图。

附图标记说明

2：机器人

3：控制器

11：工件

具体实施方式

下面，参照附图，对一个实施方式进行说明。图9示出了一般的工业用机器人的系统结构。机器人系统1包括机器人2、控制机器人2的控制器3以及与控制器3连接的示教盒4。控制器3相当于控制装置、负载重心位置推定装置。

机器人2例如构成为四轴的水平多关节型机器人。机器人2包括：基座5，固定于设置面，并设定了基座坐标XYZ；第一臂6，以能够以第一轴J1为中心旋转的方式连接在基座5上，该第一轴J1具有作为垂直轴的Z轴方向的轴心；第二臂7，以能够以第二轴J2为中心旋转的方式连接在第一轴6的顶端部上，该第二轴J2具有Z轴方向的轴心；以及轴体8，以能够上下移动且能够旋转的方式设置于第二臂7的顶端部。使轴体8上下移动时的轴为第三轴J3，使轴体8旋转时的轴为第四轴J4。凸缘9被定位在轴体8的下端部，并以可拆装的方式被安装在轴体8的下端部。对第四轴设定指尖坐标xy(或称为凸缘坐标)。

基座5、第一臂6、第二臂7、轴体8以及凸缘9发挥作为机器人2的臂的功能。在作为臂顶端的凸缘9上，作为工具例如安装有气动卡盘10。在图7中示出了通过气动卡盘10吸附了相当于负载的工件11的状态。工件11的形状实际上无定形，密度也不均匀，因此知晓其重心位置在控制上是非常重要的。设置于机器人2的多个轴J1至J4通过分别对应设置的未图示的电机来驱动。在各电机的附近，设置有用于检测各个旋转轴的旋转角度、旋转位置的未图示的位置检测器。

通常，工业用机器人按照预先通过实施示教等作成的规定的动作程序进行动作。控制器3基于该动作程序对电机的驱动进行反馈控制，进行机器人2的臂的动作控制。为此，虽未图示，但控制器3采用了包含承担该动作控制的运算装置(CPU)3A和存储装置3B的计算机的结构，该存储装置3B预先存储有控制程序以及将在后面进行说明的重心位置推定处理的程序，并且例如以半导体元件为存储器要素。因此，运算装置3A在其启动的同时，从存储装置3B中将各种程序读取到工作区域中，并依次执行由程序规定的步骤，以实现各种功能单元。

示教盒4例如以使用者能够携带或者能够手持操作的程度的大小，例如形成为薄型的大致矩形箱状。在示教盒4上设置有各种按键开关，使用者通过这些按键开关来进行各种输入操作。示教盒4经由电缆连接到控制器3，并经由通信接口在其与控制器3之间执行高速的数据传送，通过操作按键开关输入的操作信号等的信息从示教盒4发送到控制器3。

接下来，对推定包括工具在内的工件11的重心位置的原理进行说明。图10示出了机器人2的四轴臂模型。在本实施方式中，在使臂的第二轴动作时，基于作用于第四轴的加速度来推定重心位置。假设包括工具在内的工件11的质量m是已知的。

在机器人2动作时，施加到电机的转矩τ_M为式(1)。

[数学式1]

τ_I：惯性转矩 M_I：电机的惯性力矩

τ_f：离心转矩 M_vis：电机的粘性力矩 Gear：齿轮比

τ_G：重力转矩 M_Frc：电机的摩擦力矩 ω：电机的角速度

Frc：摩擦转矩 ω(点)＝dω/dt：电机的角加速度

此外，在下面的说明中，有时将角加速度简称为“加速度”。在此，将使电机以图2所示的角速度模式动作时在加速区间中作用于电机的转矩设为τ_Macc，将在减速区间中作用于电机的转矩设为τ_Mdcc，求出τ_Macc与τ_Mdcc之差。于是，依存于速度的项τ_f、Frc、M_vis、M_Frc消去，剩下依存于加速度的项τ_I、M_I。

[数学式2]

如果使第二轴动作而不使第四轴动作，则第四轴的角加速度为零。于是，第四轴的惯性转矩τ_I4变为式(3)。

[数学式3]

将第四轴的角度设定为彼此不同的角度θ_a、θ_b、θ_c，在使机器人臂进行相同动作时，将各个动作中的第四轴惯量设为I_a、I_b、I_c，则变为如下数学式。

[数学式4]

I_a-I_b＝ml_hL₂{cos(θ_a+θ_h)-cos(θ_b+θ_h)}

＝ml_hL₂{(cosθ_a-cosθ_b)cosθ_h-(sinθ_a-sinθ_b)sinθ_h}...(4)

I_a-I_c＝ml_hL₂{(cosθ_a-cosθ_c)cosθ_h-(sinθ_a-sinθ_c)sinθ_h}...(5)

希望求得的工件11的重心位置x_cog、y_cog如下。

x_cog＝I_hcosθ_h

y_cog＝I_hsinθ_h ...(6)

这些能够根据式(4)、式(5)，如式(7)、式(8)所示那样计算。

[数学式5]

特别是，当设定为θ_a＝0、θ_b＝π/2、θ_c＝-π/2时，变为如下数学式，能够简单地导出。

[数学式6]

接下来，参照图1至图8，对基于以上原理的本实施方式的作用进行说明。图1以及图2是示出由控制器3执行的重心位置推定处理的内容的流程图。首先，使机器人2的臂移动到第一初始位置(S1)。第一初始位置例如如图3所示，以θ值表示时(在以下的说明中也一样)，设为(J1，J2，J3，J4)＝J(0，-40，220，0)。此外，J3＝220对应于使第三轴即轴体8移动到其高度Z轴方向的上端位置(例如允许最大高度位置Zmax，参照图9)后的状态。另外，将此时的第四轴角度设为θ_a。

接下来，开始进行使臂到达第一推定用位置之后再次返回到第一初始位置的第一动作(S2)。第一推定用位置例如设为J(0，40，220，0)。第四轴维持初始位置不变，设为J4＝θ_a＝0。而且，在<1>臂移动到第一推定用位置为止、以及<2>从该第一推定用位置再次到达第一初始位置为止的第一动作期间内，对第二轴的加速度波形以及第四轴的转矩波形进行取样(S3)。这些分别例如为图4、图5所示的波形。此时，通过取得第一动作期间的上述动作<1>与上述动作<2>的波形的平均，获得去除了摩擦力的波形。

然后，对于取得的加速度波形，通过从波形进行的逆变换运算，取得加速区域、减速区域中的第二轴加速度ω(点)_2aa、ω(点)_2ad(S4)。步骤S2相当于第一加速度取得部。另外，对于取得的转矩波形，取得加速区域、减速区域中的第四轴加速转矩τ_4aa、τ_4ad(S5)。步骤S5相当于第一转矩取得部。之后，将在步骤S4、S5中取得的第二轴加速度以及第四轴转矩代入式(11)，计算出四轴惯量τ_I4a(S6)。

[数学式7]

接下来，使臂移动到第二初始位置(S7)。第二初始位置是从第一初始位置起仅使第四轴位置从θ_a变化到θ_b＝90°的位置，J(0，-40，220，90)。之后，使臂执行第二动作(S8)。如图7所示，第二推定用位置也是从第一推定用位置仅使第四轴位置变化到θ_b＝90°的位置，J(0，40，220，90)。与第一动作期间同样地，第二动作期间为从第二推定用位置再次返回到第二初始位置为止的期间。

之后的步骤S9至S12为对第二动作期间进行与步骤S3至S6同样的处理的步骤。只是，在步骤S10中获得的第二轴加速度变为ω(点)_2ba、ω(点)_2bd，在步骤S11中获得的第四轴转矩变为τ_4ba、τ_4bd。步骤S10相当于第二加速度取得部，步骤S11相当于第二转矩取得部。然后，将在步骤S10、S11中取得的第二轴加速度以及第四轴转矩代入式(12)，计算出四轴惯量τ_I4b(S12)。

[数学式8]

接下来，使臂移动到第三初始位置(S13)。第三初始位置也是从第一初始位置仅使第四轴位置从θ_a变化到θ_c＝-90°的位置，J(0，-40，220，-90)。之后，使臂执行第三动作(S14)。如图8所示，第三推定用位置也是从第一推定用位置仅使第四轴位置变化到θ_c＝-90°的位置，J(0，40，220，-90)。与第一动作期间同样地，第三动作期间为从第三推定用位置再次返回到第三初始位置为止的期间。

之后的步骤S15至S18为对第三动作期间进行与步骤S3至S6同样的处理的步骤。只是，在步骤S16中获得的第二轴加速度变为ω(点)_2ca、ω(点)_2cd，在步骤S17中获得的第四轴转矩变为τ_4ca、τ_4cd。步骤S16相当于第三加速度取得部，步骤S17相当于第三转矩取得部。然后，将在步骤S16、S17中取得的第二轴加速度以及第四轴转矩代入式(13)，计算出四轴惯量τ_I4c(S18)。

[数学式9]

在此，如本实施方式那样，若将第四轴角度θ_a、θ_b、θ_c分别选为0°、90°、-90°(S19：是)，则使用式(9)、(10)求出l_hcosθ_h、l_hsinθ_h(S20)，通过式(6)获得工件11的重心位置x_cog、y_cog(S21)。另一方面，若未将第四轴角度θ_a、θ_b、θ_c分别选为0°、90°、-90°(S19：否)，则使用式(7)、(8)求出l_hcosθ_h、l_hsinθ_h(S22)。进一步，与上述同样地，通过式(6)获得工件11的指尖坐标xy中的重心位置x_cog、y_cog(S22)。此外，在使用式(9)、式(10)、或者式(7)、式(8)时，将这些计算式中所需的四轴惯量I_a、I_b、I_c替换为通过式(11)、式(12)以及式(13)计算出的四轴惯量τ_I4a、τ_I4b、τ_I4c来进行计算。步骤S19至S22相当于重心位置计算部。

此外，在工件11的实际的重心位置与通过本实施方式获得的重心位置x_cog、y_cog之间存在误差时，只要使用与误差值相应的修正系数进行修正即可。

如上所述，根据本实施方式，控制器3在顶端存在质量m已知的工件11的状态下，对第一动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第一动作期间为，水平四轴机器人2的臂从将第四轴位置设为θ_a的第一初始位置使第二轴动作从而到达第一推定用位置为止、以及从第一推定用位置再次到达第一初始位置为止的期间。

另外，当控制器3取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩时，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4a。

控制器3从对于第一初始位置将第四轴位置变为θ_b的第二初始位置起使第二轴动作从而进行第二动作，对第二动作期间中的加速区域以及减速区域分别取得第二轴的加速度。另外，对于第二动作期间，也与第一转矩取得部同样地，当取得第四轴的加速转矩、减速转矩时，计算出围绕第四轴的惯量τ_I4b。

另外，控制器3从对于第一初始位置将第四轴位置变为θ_c的第三初始位置起使第二轴动作从而进行第三动作，对第三动作期间中的加速区域以及减速区域分别取得第二轴的加速度。另外，对于第三动作期间，当取得第四轴的加速转矩、减速转矩时，计算出围绕第四轴的惯量τ_I4c。然后，基于从第四轴到负载的重心距离l_h、所述负载的重心方向θ_h、惯量τ_I4a、τ_I4b及τ_I4c、第四轴位置θ_a、θ_b及θ_c，计算出工件11的重心位置x_cog、y_cog。

即，对于彼此不同的三个第四轴位置，基于使负载在水平方向上同样地移动而施加到第四轴的加速度，获得各个惯量τ_I4a、τ_I4b及τ_I4c。而且，基于上述各个惯量以及第四轴位置θ_a、θ_b及θ_c，求出工件11的重心位置x_cog、y_cog。因此，对于具备水平四轴结构的臂的机器人，也能够推定工件11的水平面内、即指尖坐标xy上的重心位置。因此，即使在四轴水平多关节型机器人中，也能够正确地推定工件的重心位置，由此能够执行更高精度的臂控制。

另外，通过将第四轴角度θ_a、θ_b、θ_c分别设定为0°、90°、-90°，能够由式(9)、(10)立即求出重心位置x_cog、y_cog，计算处理变得简单。进一步，由于将第三轴固定于上端位置来进行动作，因此，在使臂动作到第一、第二推定用位置时，能够通过第三轴来抑制工件11上产生的振动，能够以更高的精度推定重心位置。

本发明并不仅限于上述的、或者附图中记载的实施方式，可以进行如下的变形或者扩展。

可以改变θ_a、θ_b、θ_c与角度0°、90°、-90°之间的对应关系。另外，θ_a、θ_b、θ_c的各设定无需限定于上述的角度。

可以对初始位置以及推定用位置进行适当变更来实施。

只要振动的影响不会产生问题，就无需一定将第三轴设置在上端位置，也可以设置在Z轴方向上比最大高度位置Zmax低的高度位置。

可以对第二、第三步骤的执行顺序进行对调，也可以对第五、第六步骤的执行顺序进行对调。另外，也可以对第八、第九步骤的执行顺序进行对调。

Claims (6)

1.一种机器人的负载重心位置推定装置，具备：

第一加速度取得部，在水平四轴结构的机器人臂的顶端存在质量(m)已知的负载的状态下，对第一动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第一动作期间为，所述机器人臂从将第四轴位置设为θ_a的第一初始位置起使第二轴动作从而到达第一推定用位置为止、以及从所述第一推定用位置再次到达所述第一初始位置为止的期间；

第一转矩取得部，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第一惯量计算部，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4a；

第二加速度取得部，对第二动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第二动作期间为，所述机器人臂从对于所述第一初始位置将第四轴位置变为θ_b的第二初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第二推定用位置为止、以及从所述第二推定用位置再次到达所述第二初始位置为止的期间，其中θ_b不等于θ_a；

第二转矩取得部，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第二惯量计算部，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4b；

第三加速度取得部，对第三动作期间分别取得加速区域以及减速区域中的第二轴的加速度，所述第三动作期间为，所述机器人臂从对于所述第一初始位置将第四轴位置变为θ_c的第三初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第三推定用位置为止、以及从所述第三推定用位置再次到达所述第三初始位置为止的期间，其中θ_c不等于θ_a、θ_b；

第三转矩取得部，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第三惯量计算部，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4c；以及

重心位置计算部，基于从第四轴到所述负载的重心距离(l_h)、所述负载的重心方向(θ_h)、所述惯量τ_I4a、τ_I4b及τ_I4c、所述第四轴位置θ_a、θ_b及θ_c，计算出所述负载的重心位置(x_cog、y_cog)。

2.根据权利要求1所述的机器人的负载重心位置推定装置，其中，

所述第一加速度取得部至所述第三加速度取得部分别将所述第四轴位置θ_a、θ_b及θ_c的任意一个设定为0°、90°、-90°。

3.根据权利要求1或2所述的机器人的负载重心位置推定装置，其中，

所述第一加速度取得部至所述第三加速度取得部将第三轴固定于上端位置来进行动作。

4.一种机器人的负载重心位置推定方法，包括：

第一步骤，在水平四轴结构的机器人臂的顶端存在质量(m)已知的负载的状态下，对第一动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第一动作期间为，所述机器人臂从将第四轴位置设为θ_a的第一初始位置起使第二轴动作从而到达第一推定用位置为止、以及从所述第一推定用位置再次到达所述第一初始位置为止的期间；

第二步骤，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第三步骤，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4a；

第四步骤，对第二动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第二动作期间为，所述机器人臂从对于所述第一初始位置将第四轴位置变为θ_b的第二初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第二推定用位置为止、以及从所述第二推定用位置再次到达所述第二初始位置为止的期间，其中θ_b不等于θ_a；

第五步骤，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第六步骤，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4b；

第七步骤，对第三动作期间中的加速区域以及减速区域分别取得第二轴的加速度，所述第三动作期间为，所述机器人臂从对于所述第一初始位置将第四轴位置变为θ_c的第三初始位置起使第二轴动作从而使第一轴至第三轴到达与第一推定用位置相等的第三推定用位置为止、以及从所述第三推定用位置再次到达所述第三初始位置为止的期间，其中θ_c不等于θ_a、θ_b；

第八步骤，取得所述加速区域中的第四轴的加速转矩、以及所述减速区域中的第四轴的减速转矩；

第九步骤，基于所述加速区域以及减速区域的加速度、所述加速转矩和所述减速转矩，计算出围绕第四轴的惯量τ_I4c；以及

第十步骤，基于从第四轴到所述负载的重心距离(l_h)、所述负载的重心方向(θ_h)、所述惯量τ_I4a、τ_I4b及τ_I4c、所述第四轴位置θ_a、θ_b及θ_c，计算出所述负载的重心位置(x_cog、y_cog)。

5.根据权利要求4所述的机器人的负载重心位置推定方法，其中，

在所述第一步骤、所述第四步骤以及所述第七步骤中，分别将所述第四轴位置θ_a、θ_b及θ_c的任意一个设定为0°、90°、-90°。

6.根据权利要求4或5所述的机器人的负载重心位置推定方法，其中，

在所述第一步骤、所述第四步骤以及所述第七步骤中，将第三轴固定于上端位置来进行动作。