CN109483596A - 一种机器人工具端负载的测试方法 - Google Patents

Abstract

本发明提供一种机器人工具端负载的测试方法，通过控制机器人末端三个关节转动一定角度实现对负载重心和转动惯量的测量，转动第六关节使其轴线与负载重心处于同一平面，待机器人稳定，此时负载对第六关节产生最大重力矩，读出电机输出力矩，计算出力臂长度即为负载一个方向上的相对第六关节坐标原点的位置绝对值，同理，可通过转动末端三关节实现负载其他两个方向上重心的求解，或在将其中一个或两个关节转动一定角度，并控制其他关节转动使其轴线与负载重心处于同水平面上来求解重心位置，在此基础上，控制机器人末端三关节匀加速转动，求解出负载三个方向上的转动惯量。本发明的有益效果：准确实现负载重心和转动惯量检测，具有通用性。

Description

一种机器人工具端负载的测试方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种机器人工具端负载的测试方法。

背景技术

机器人现已广泛在3C、汽车零部件、金属加工、食品、医药、物流等行业得到了应用。对每一种产品或工况，在机器人末端都有安装与之相对应的工具。作为负载又作为机器人本体的一部分，其质量、重心、转动惯量对机器人本体性能，如关节力矩输出有极大影响。因此在安装负载前，需对其质量，相对于工具端重心位置和转动惯量进行检测。负载的质量可直接通过电子秤称量获得，但负载由不同的零部件组成，组成后的外形不规则，质量分布不均匀。因此负载的重心和转动惯量的检测都较为困难。

现重心的主要测量方法有实验法和对称法。实验法又分为悬挂实验法和称重实验法。悬挂法主要适用于薄板类物件，称重法主要适用于质量较大或直杆类物件。而对于外形较为规则的工具或组成工具零部件外形较为规则的工具及较为规则的工件，可通过对称法计算各个零部件及工件的重心后，再综合计算得出整体负载重心位置。此方法计算量大，计算过程复杂。对于不规则工具和工件，则难以实现其负载重心的检测。而负载的转动惯量，一般需要通过专门仪器测量，且测量过程一般需要二次装卡，这也造成测量不精准，且无形之中也增加了设备成本。

发明内容

有鉴于此，本发明的实施例提供了一种机器人工具端负载的测试方法。

本发明的实施例提供一种机器人工具端负载的测试方法，所述机器人包括依次连接的六关节，分别为第一关节～第六关节，每一关节均内设驱动其转动的驱动电机，包括以下步骤：

S1对负载称重，记为M,并将负载固定于第六关节，将机器人恢复到零位，零位状态时，第四关节和第六关节处于水平状态，第五关节处于竖直状态；

S2在第六关节处建立坐标系O₆-X₆Y₆Z₆，使O₆Z₆轴与第六关节转动轴重合且坐标系O₆-X₆Y₆Z₆跟随所述第六关节同步转动；

S3控制第六关节匀速转动，当其驱动电机输出力矩最大时停止转动，建立力矩平衡方程Mg*d_xy+G₆₁＝T₆₁，并求解出d_xy，其中，d_xy为此时负载重心到第六关节的相对距离，G₆₁为此时第六关节重力矩，T₆₁为此时第六关节驱动电机输出力矩；

S4继续控制第六关节匀速转动90°，建立力矩平衡方程Mg*d_yx+G₆₂＝T₆₂，并求解出d_yx，其中，d_yx为此时负载重心到第六关节的相对距离，G₆₂为此时第六关节重力矩，T₆₂为此时第六关节驱动电机输出力矩；

S5将机器人恢复到零位，控制第五关节和第六关节匀速转动90°后，控制第四关节匀速转动，且在第四关节驱动电机输出力矩最大时使其停止转动，此时负载重心和第四关节轴线处于同一平面内，建立力矩平衡方程Mg*d_yz+G₄＝T₄，并求解出d_yz，其中，d_yz为此时负载重心到第四关节的相对距离，G₄为此时第四关节、第五关节和第六关节整体的重力矩，T₄为此时第四关节驱动电机输出力矩；

S6根据坐标变化法则将d_yz转化为负载重心相对于第六关节的距离d_z6，从而确定负载相对于第六关节的坐标(d_xy，d_yx，d_z6)。

进一步地，包括：

S7将所述机器人恢复到零位，控制第六关节匀加速转动，此时第六关节驱动电机输出力矩为T₆，建立力矩平衡方程T₆＝M₆*a+G₆，其中G₆为负载和第六关节的重力矩之和，M₆*a为负载和第六关节匀加速运动时产生力矩之和，即M₆*a＝M_6z*a+M₆₆*a，其中M₆₆为第六关节自身转动惯量，计算出M_6z，即为负载绕坐标系O₆-X₆Y₆Z₆的O₆Z₆轴转动的转动惯量。

进一步地，包括：

S8将所述机器人恢复到零位，在第五关节处建立坐标系O₅-X₅Y₅Z₅，使O₅X₅轴与O₆X₆轴平行，O₅Y₅轴与O₆Z₆轴平行，O₅Z₅轴与O₆Y₆轴平行,且坐标系O₅-X₅Y₅Z₅跟随所述第五关节同步转动，控制第五关节匀加速转动，此时第五关节驱动电机输出力矩为T_5y，建立力矩平衡方程T_5y＝M₁*a+G_5y，其中G_5y为负载、第五关节和第六关节的重力矩之和，M₁*a为负载、第五关节和第六关节匀加速运动时产生力矩之和，即M₁*a＝M_5y*a+M₆₅*a+M₅₅*a，其中M₆₅和M₅₅分别表示第六关节和第五关节绕第五关节转动的转动惯量，M_5y为负载相对于坐标系O₅-X₅Y₅Z₅系O₅Y₅轴转动的转动惯量，计算出M_5y并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆Y₆轴转动的转动惯量M_6y。

进一步地，包括：

S9将所述机器人恢复到零位，在第五关节处建立坐标系O₅-X₅Y₅Z₅，使O₅X₅轴与O₆X₆轴平行，O₅Y₅轴与O₆Z₆轴平行，O₅Z₅轴与O₆Y₆轴平行,且坐标系O₅-X₅Y₅Z₅跟随所述第五关节同步转动，控制第六关节转动90°，第五关节匀加速转动，此时第五关节驱动电机输出力矩为T_5x，建立力矩平衡方程T_5x＝M₂*a+G_5x，其中G_5x为负载、第五关节和第六关节的重力矩之和，M₂*a为负载、第五关节和第六关节匀加速运动时产生力矩之和，即M₂*a＝M_5x*a+M′₆₅*a+M′₅₅*a，其中M′₆₅和M′₅₅分别表示第六关节和第五关节绕第五关节转动的转动惯量，M_5x为负载相对于坐标系O₅-X₅Y₅Z₅的O₅X₅轴转动的转动惯量，计算出M_5x并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆X₆轴转动的转动惯量M_6x。

进一步地，所述机器人零位时，所述坐标系O₆-X₆Y₆Z₆的O₆Y₆轴竖直设置。

进一步地，所述机器人第一关节竖直固定于不动体，所述机器人零位时，所述第二关节处于水平状态，所述第三关节处于水平状态。

本发明的实施例提供的技术方案带来的有益效果是：本发明一种机器人工具端负载的测试方法，通过控制机器人第四关节、第五关节和第六关节转动到特定角度，以及这三关节的自身性质，来检测机器人第六关节(工具端)负载的重心和转动惯量，对机器人工具端负载重心的检测具有通用性，对于外形较为规则的负载，避免单个计算工具和工件重心后再综合计算整体负载重心的方法所造成的推导计算困难问题，且对于不规则外形的负载，在无其他仪器设备的协助下也可实现其重心的检测，提高了转动惯量的测量精度，也避免了需要再买其他仪器设备来测量转动惯量，节省成本。

附图说明

图1是本发明一种机器人工具端负载重心测试方法流程图；

图2是机器人零位时各关节的状态示意图；

图3是负载重心与第六关节转动轴同水平面时的示意图；

图4是机器人由图3状态第六关节转动90°后的示意图；

图5是负载重心与第四关节转动轴同水平面时的示意图。

其中：θ₁-第一关节、θ₂-第二关节、θ₃-第三关节、θ₄-第四关节、θ₅-第五关节、θ₆-第六关节。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。

请参考图1和图2，本发明的实施例提供了一种机器人工具端负载的测试方法，所述机器人包括依次连接的六关节，分别为第一关节θ₁～第六关节θ₆，每一关节均内设驱动其转动的驱动电机，第一关节θ₁竖直固定于不动体基座，第一关节θ₁、第二关节θ₂、第三关节θ₃不转动，所述机器人初始状态处于零位状态，零位状态时，第二关节θ₂、第三关节θ₃、第四关节θ₄和第六关节θ₆均处于水平状态，第五关节θ₅处于竖直状态。

所述测试方法具体包括以下步骤：

S1：对负载称重，记为M,并将负载固定于第六关节θ₆，第六关节θ₆即为安装负责的工具端，将机器人恢复到零位；

S2：在第六关节θ₆处建立坐标系O₆-X₆Y₆Z₆，使O₆Z₆轴与第六关节θ₆转动轴重合且坐标系O₆-X₆Y₆Z₆跟随所述第六关节θ₆同步转动，所述坐标系O₆-X₆Y₆Z₆的O₆Y₆轴竖直设置，为了便于观察所述负载重心跟随所述第六关节θ₆转动变化，建立坐标系O_t-X_tY_tZ_t，使O_tX_t轴与O₆X₆轴平行，O_tY_t轴与O₆Y₆轴平行，O_tZ_t轴与O₆Z₆轴平行，假设坐标系O_t-X_tY_tZ_t原点O_t与负载重心重合，且坐标系O_t-X_tY_tZ_t跟随第六关节转动；

S3：请参考图3，控制第六关节θ₆匀速转动，并观察其驱动电机输出力矩大小，当其驱动电机输出力矩最大时停止转动，此时负载重心和第六关节θ₆的转动轴处于同一平面，建立力矩平衡方程

Mg*d_xy+G₆₁＝T₆₁

求解出此时负载重心到第六关节θ₆的相对距离d_xy，其中G₆₁为此时第六关节θ₆重力矩，T₆₁为此时第六关节θ₆驱动电机输出力矩，Mg是负载的重力，d_xy即为负载在坐标系O₆-X₆Y₆Z₆的O₆X₆轴上的分量；

S4：请参考图4，在步骤S3的基础上，继续控制第六关节θ₆匀速继续转动90°，建立力矩平衡方程

Mg*d_yx+G₆₂＝T₆₂

并求解出此时负载重心到第六关节θ₆的相对距离d_yx，其中G₆₂为此时第六关节θ₆重力矩，T₆₂为此时第六关节θ₆驱动电机输出力矩，d_yx即为负载在坐标系O₆-X₆Y₆Z₆的O₆Y₆轴上的分量；；

S5：请参考图5，将机器人恢复到零位，控制第五关节θ₅和第六关节θ₆匀速转动90°后，控制第四关节θ₄匀速转动、观察第四关节θ₄电机输出力矩，在第四关节θ₄输出力矩最大时使其停止转动，此时负载重心和第四关节轴线处于同一平面内，建立力矩平衡方程

Mg*d_yz+G₄＝T₄

求解出此时负载重心到第四关节θ₄的相对距离d_yz，其中G₄为此时第四关节θ₄、第五关节θ₅和第六关节θ₆整体的重力矩，T₄为此时第四关节θ₄驱动电机输出力矩；

S6：由于计算出的d_yz为载重心到第四关节θ₄的相对距离d_yz，因此需要对其进行坐标转换，根据坐标变化法则将d_yz转化为负载重心相对于第六关节的距离d_z6，即为负载重心在O₆-X₆Y₆Z₆到O₆Z₆轴上的距离d_z6，从而确定负载重心相对于第六关节θ₆的坐标(d_xy，d_yx，d_z6)。

力矩等于转动惯量与角加速度的乘积，当刚体匀加速转动时，力矩为恒值，根据此原理即可计算负载的转动惯量，因此所述测试方法测量出负载重心后，可进一步测量出负载绕第六关节θ₆坐标O₆-X₆Y₆Z₆三轴的转动惯量，具体方法如下：

S7：将所述机器人恢复到零位，控制第六关节θ₆匀加速转动，此时第六关节θ₆驱动电机输出力矩为T₆，建立力矩平衡方程

T₆＝M₆*a+G₆

其中G₆为负载和第六关节θ₆的重力矩之和，负载重心已知，G₆可通过计算获得，M₆*a为负载和第六关节θ₆匀加速运动时产生力矩之和，因此

M₆*a＝M_6z*a+M₆₆*a

其中M₆₆为第六关节θ₆自身转动惯量，可通过计算获得，a为第六关节转动加速度，因此可计算出M_6z，即为负载绕坐标系O₆-X₆Y₆Z₆的O₆Z₆轴转动的转动惯量；

S8：将所述机器人恢复到零位，在第五关节θ₅处建立坐标系O₅-X₅Y₅Z₅，使O₅X₅轴与O₆X₆轴平行，O₅Y₅轴与O₆Z₆轴平行，O₅Z₅轴与O₆Y₆轴平行,且坐标系O₅-X₅Y₅Z₅跟随所述第五关节θ₅同步转动，控制第五关节θ₅匀加速转动，此时第五关节θ₅驱动电机输出力矩为T_5y，建立力矩平衡方程

T_5y＝M₁*a+G_5y

其中G_5y为负载、第五关节θ₅和第六关节θ₆的重力矩之和，负载重心已知，G_5y可通过计算获得，M₁*a为负载、第五关节θ₅和第六关节θ₆匀加速运动时产生力矩之和，因此

M₁*a＝M_5y*a+M₆₅*a+M₅₅*a

其中M₆₅和M₅₅分别表示第六关节θ₆和第五关节θ₅绕第五关节转动的转动惯量，可通过计算获得，M_5y为负载相对于坐标系O₅-X₅Y₅Z₅系O₅Y₅轴转动的转动惯量，计算出M_5y并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆Y₆轴转动的转动惯量M_6y。

S9将所述机器人恢复到零位，控制第六关节θ₆转动90°，第五关节θ₅匀加速转动，此时第五关节θ₅驱动电机输出力矩为T_5x，建立力矩平衡方程

T_5x＝M₂*a+G_5x

其中G_5x为负载、第五关节θ₅和第六关节θ₆的重力矩之和，负载重心已知，G_5x可通过计算获得，M₂*a为负载、第五关节θ₅和第六关节θ₆匀加速运动时产生力矩之和，因此

M₂*a＝M_5x*a+M′₆₅*a+M′₅₅*a

其中M′₆₅和M′₅₅分别表示第六关节θ₆和第五关节θ₅绕第五关节θ₅转动的转动惯量，可计算获得，M_5x为负载相对于坐标系O₅-X₅Y₅Z₅的O₅X₅轴转动的转动惯量，计算出M_5x并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆X₆轴转动的转动惯量M_6x。

为了提高测量效率，上述测试方法中将第四关节θ₄、第五关节θ₅和第六关节θ₆均按照同一个转向转动，转向为顺时针或逆时针中的一个。

在本文中，所涉及的前、后、上、下等方位词是以附图中零部件位于图中以及零部件相互之间的位置来定义的，只是为了表达技术方案的清楚及方便。应当理解，所述方位词的使用不应限制本申请请求保护的范围。

在不冲突的情况下，本文中上述实施例及实施例中的特征可以相互结合。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种机器人工具端负载的测试方法，所述机器人包括依次连接的六关节，分别为第一关节～第六关节，每一关节均内设驱动其转动的驱动电机，其特征在于，包括以下步骤：

S1对负载称重，记为M,并将负载固定于第六关节，将机器人恢复到零位，零位状态时，第四关节和第六关节处于水平状态，第五关节处于竖直状态；

S2在第六关节处建立坐标系O₆-X₆Y₆Z₆，使O₆Z₆轴与第六关节转动轴重合且坐标系O₆-X₆Y₆Z₆跟随所述第六关节同步转动；

S3控制第六关节匀速转动，当其驱动电机输出力矩最大时停止转动，建立力矩平衡方程Mg*d_xy+G₆₁＝T₆₁，并求解出d_xy，其中，d_xy为此时负载重心到第六关节的相对距离，G₆₁为此时第六关节重力矩，T₆₁为此时第六关节驱动电机输出力矩；

S4继续控制第六关节转动90°，建立力矩平衡方程Mg*d_yx+G₆₂＝T₆₂，并求解出d_yx，其中，d_yx为此时负载重心到第六关节的相对距离，G₆₂为此时第六关节重力矩，T₆₂为此时第六关节驱动电机输出力矩；

S5将机器人恢复到零位，控制第五关节和第六关节转动90°后，控制第四关节匀速转动，且在第四关节驱动电机输出力矩最大时使其停止转动，此时负载重心和第四关节轴线处于同一平面内，建立力矩平衡方程Mg*d_yz+G₄＝T₄，并求解出d_yz，其中，d_yz为此时负载重心到第四关节的相对距离，G₄为此时第四关节、第五关节和第六关节整体的重力矩，T₄为此时第四关节驱动电机输出力矩；

S6根据坐标变化法则将d_yz转化为负载重心相对于第六关节的距离d_z6，从而确定负载重心相对于第六关节的坐标(d_xy，d_yx，d_z6)。

2.如权利要求1所述的一种机器人工具端负载的测试方法，其特征在于，包括：

S7将所述机器人恢复到零位，控制第六关节匀加速转动，此时第六关节驱动电机输出力矩为T₆，建立力矩平衡方程T₆＝M₆*a+G₆，其中G₆为负载和第六关节的重力矩之和，M₆*a为负载和第六关节匀加速运动时产生力矩之和，即M₆*a＝M_6z*a+M₆₆*a，其中M₆₆为第六关节自身转动惯量，计算出M_6z，即为负载绕坐标系O₆-X₆Y₆Z₆的O₆Z₆轴转动的转动惯量。

3.如权利要求1所述的一种机器人工具端负载的测试方法，其特征在于，包括：

S8将所述机器人恢复到零位，在第五关节处建立坐标系O₅-X₅Y₅Z₅，使O₅X₅轴与O₆X₆轴平行，O₅Y₅轴与O₆Z₆轴平行，O₅Z₅轴与O₆Y₆轴平行,且坐标系O₅-X₅Y₅Z₅跟随所述第五关节同步转动，控制第五关节匀加速转动，此时第五关节驱动电机输出力矩为T_5y，建立力矩平衡方程T_5y＝M₁*a+G_5y，其中G_5y为负载、第五关节和第六关节的重力矩之和，M₁*a为负载、第五关节和第六关节匀加速运动时产生力矩之和，即M₁*a＝M_5y*a+M₆₅*a+M₅₅*a，其中M₆₅和M₅₅分别表示第六关节和第五关节绕第五关节转动的转动惯量，M_5y为负载相对于坐标系O₅-X₅Y₅Z₅系O₅Y₅轴转动的转动惯量，计算出M_5y并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆Y₆轴转动的转动惯量M_6y。

4.如权利要求1所述的一种机器人工具端负载的测试方法，其特征在于，包括：

S9将所述机器人恢复到零位，在第五关节处建立坐标系O₅-X₅Y₅Z₅，使O₅X₅轴与O₆X₆轴平行，O₅Y₅轴与O₆Z₆轴平行，O₅Z₅轴与O₆Y₆轴平行,且坐标系O₅-X₅Y₅Z₅跟随所述第五关节同步转动，控制第六关节转动90°，第五关节匀加速转动，此时第五关节驱动电机输出力矩为T_5x，建立力矩平衡方程T_5x＝M₂*a+G_5x，其中G_5x为负载、第五关节和第六关节的重力矩之和，M₂*a为负载、第五关节和第六关节匀加速运动时产生力矩之和，即M₂*a＝M_5x*a+M′₆₅*a+M′₅₅*a，其中M′₆₅和M′₅₅分别表示第六关节和第五关节绕第五关节转动的转动惯量，M_5x为负载相对于坐标系O₅-X₅Y₅Z₅的O₅X₅轴转动的转动惯量，计算出M_5x并通过坐标系变换，得到负载绕坐标系O₆-X₆Y₆Z₆的O₆X₆轴转动的转动惯量M_6x。

5.如权利要求1所述的一种机器人工具端负载的测试方法，其特征在于：所述机器人零位时，所述坐标系O₆-X₆Y₆Z₆的O₆Y₆轴竖直设置。

6.如权利要求1所述的一种机器人工具端负载的测试方法，其特征在于，包括：所述机器人第一关节竖直固定于不动体，所述机器人零位时，所述第二关节处于水平状态，所述第三关节处于水平状态。