CN113510698A - 一种机械臂控制方法、装置、机械臂和可读存储介质 - Google Patents

Abstract

本发明公开了一种机械臂控制方法、装置、机械臂和可读存储介质，该方法包括：利用机械臂末端的传感器获取所述机械臂处于n个不同位姿时所述机械臂末端的操作器在末端坐标系下的n个重力矩阵数据，n≥3；利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；只需要获取机械臂在n个不同位姿时对应的n个重力矩阵数据和n个旋转变换矩阵，即可计算所述操作器的质心坐标和质量。本申请的技术方案充分考虑了力传感器的上电误差，不仅计算量小，而且可以实现在线进行数据处理，确定操作器的质心坐标和质量，无需离线求解后再修改模型参数，有效提高确定操作器相关参数的速度。

Description

一种机械臂控制方法、装置、机械臂和可读存储介质

技术领域

本发明涉及机械臂应用技术领域，尤其涉及一种机械臂控制方法、装置、机械臂和可读存储介质。

背景技术

随着机械臂在服务机器人领域的推广和普及，通常会用机械臂替代机器人手臂来执行不同的任务，而针对不同的任务，机械臂的末端会安装不同的操作器以抓取或夹持负载，比如抓取箱子时可用吸盘式操作器，抓取不规则物品时可采用多指抓手，写字时可安装相应的笔夹等。不同的质量属性的操作器将对机械臂的位置和力控制产生影响。

现有技术中，可以通过操作器的CAD模型获取相关的质量和质心参数。但是，由于加工、装配、电器元件等与理论不同，导致实际质量参数与所述CAD模型不同，并且很多机械臂的末端安装的操作器是无法利用CAD模型获取其理论模型参数的。导致机械臂的控制端无法精准获取操作器的质量和质心，从而影响机械臂的控制端无法对机械臂进行精确的位置移动和力控制。

发明内容

鉴于上述问题，本发明提出一种机械臂控制方法、装置、机械臂和可读存储介质。

本申请提出一种机械臂控制方法，所述方法包括：

利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3；

确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；

利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量；

将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂。

本申请所述的机械臂控制方法，所述利用安装于机械臂末端的传感器获取所述机械臂末端的操作器的n个重力矩阵数据，包括：

控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；

在所述机械臂保持第j个位姿时，利用所述安装于机械臂末端的传感器获取所述操作器在所述末端坐标系下的第j个重力矩阵数据，j≤n。

本申请所述的机械臂控制方法，所述利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵，包括：

控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；

在所述机械臂保持第j个位姿时，j≤n：

获取机械臂的各个连杆关节角；

利用所述各个连杆关节角和预设的各个连杆扭曲度确定各个相邻连杆坐标系之间的相邻变换矩阵；

利用各个相邻变换矩阵确定第j个位姿对应的第j个旋转变换矩阵。

本申请所述的机械臂控制方法，利用以下公式确定所述操作器的质心坐标和质量：

或3，m表示所述操作器的质量，r_c表示所述操作器的质心坐标，f_FTbias表示所述安装于机械臂末端的传感器采集力的静态误差，τ_FTbias表示所述安装于机械臂末端的传感器采集力矩的静态误差，Y_j表示第j个位姿对应的参数矩阵，

表示第j个旋转变换矩阵对应的转置矩阵，g表示重力加速度向量，

表示求

的反对称矩阵，0_1×3表示1行3列的零矩阵，I_3×3表示3行3列的单位矩阵，0_3×3表示3行3列的零矩阵，W_j表示第j个位姿对应的重力矩阵数据，f_FTj表示第j个位姿对应的重力矩阵数据的力向量，τ_FTi表示第j个位姿对应的重力矩阵数据的力矩向量。

本申请所述的机械臂控制方法，所述控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿，包括：

控制所述机械臂的末端依次移动至预设的n个末端位置，并在每一末端位置停留预设时间，所述预设的n个末端位置在所述运动范围内；

或，利用随机函数在所述运动范围内随机选择n个末端位置，控制所述机械臂的末端依次移动至n个末端位置，并在每一末端位置停留预设时间；

或，在保持所述机械臂的末端位置不变的情况下，控制所述机械臂的各个连杆在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

本申请所述的机械臂控制方法，所述预设时间的取值范围为1s～2s。

本申请所述的机械臂控制方法，还包括：将所述安装于机械臂末端的传感器静态误差反馈至所述机械臂的控制端。

本申请提出一种机械臂控制装置，所述装置包括：

获取模块，用于利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3；

确定模块，用于利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；

计算模块，用于利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量；

控制模块，用于将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂。

本申请提出一种机械臂，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行本申请所述的机械臂控制方法。

本申请提出一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本申请所述的机械臂控制方法。

本申请的技术方案利用机械臂末端的传感器获取所述机械臂处于n个不同位姿时所述机械臂末端的操作器在末端坐标系下的n个重力矩阵数据，n≥3；确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；只需要利用机械臂在n个不同位姿时对应的n个重力矩阵数据和n个旋转变换矩阵，即可计算所述操作器的质心坐标和质量。本申请公开的计算所述操作器的质心坐标和质量的技术方案不仅计算量小，而且可以实现在线进行数据处理，确定操作器的质心坐标和质量，无需离线求解后再修改模型参数，进而有效提高确定操作器相关参数的速度；并且，本申请公开的机械臂控制方法还将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂，使得机械臂的控制端可以对机械臂进行准确的力控制，从而控制机械臂进行精确的位置移动。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本申请实施例提出的一种机械臂控制方法的流程示意图；

图2示出了本申请实施例提出的一种确定n个重力矩阵数据方法的流程示意图；

图3示出了本申请实施例提出的一种确定旋转变换矩阵方法的流程示意图；

图4示出了本申请实施例提出的一种机械臂相关参数的示意图；

图5示出了本申请实施例提出的另一种机械臂控制方法的流程示意图；

图6示出了本申请实施例提出的一种机械臂控制装置的结构示意图；

图7示出了本申请实施例提出的一种机械臂的结构示意图。

主要元件符号说明：

10-机械臂控制装置；11-获取模块；12-确定模块；13-计算模块；14-控制模块；200-机械臂；210-存储器；220-处理器。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

考虑到当机械臂运动时，安装于机械臂末端的操作器的位置和姿态会发生改变，在不考虑外力对操作器的存在干扰的前提下，末端的操作器仅受重力影响，由于重力场的方向是不会变的，因此，重力大小在机械臂的基坐标系中是不变，但是，操作器的重力相对于机械臂末端的末端坐标系的值会随着机械臂运动发生变化，因此，可以根据上述特性对操作器的质量属性进行自动辨识。

进一步的，本申请的技术方案可用在任意构型的机械臂上，在机械臂的末端安装新的操作器后，利用本申请公开的操作器参数自动辨识方法，对机械臂进行上电初始化，只需要采集三个随机位姿对应的相关信息，即可计算操作器的质心坐标和质量。不仅计算量小，而且可以实现在线数据处理，无需离线求解再修改模型参数，实时更新操作器的相关参数信息。并且，本申请公开的操作器参数自动辨识方法在计算操作器的质心坐标和质量时，充分考虑了力传感器的上电误差，使得计算确定的操作器的质心坐标和质量更接近真实值。

实施例1

本申请的一个实施例，提出一种机械臂控制方法，如图1所示，机械臂控制方法包括以下步骤：

S100：利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3。

由于机械臂和工作面的接触常常是未知的复杂曲面，因而，在确定机械臂末端的操作器在末端坐标系下的操作器重力时，需要使用六维力传感器获取机械臂处于n个不同位姿时机械臂末端的操作器在末端坐标系下的n个重力矩阵数据。六维力传感器(6-Dimension Force Torque Sensor)是一种力传感器，能同时测量多维力/力矩，并将多维力/力矩信号转换为电信号，可用于监测方向和大小不断变化的力与力矩，可以测量加速度或惯性力以及检测接触力的大小和作用点。可以理解的，六维力传感器可以测量空间X、Y、Z三个方向上的力和扭矩。

进一步的，参见图2，确定n个重力矩阵数据的方法包括以下步骤：

S110：控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

可以理解，控制所述机械臂在运动范围内任意变换n次的方法包括以下三种方法：

第一种，可以预先在机械臂的运动范围内设置n个末端位置，然后控制所述机械臂的末端依次移动至预设的n个末端位置，并在每一末端位置停留预设时间。

第二种，可以利用随机函数在机械臂的运动范围内随机选择n个末端位置，然后控制所述机械臂的末端依次移动至n个随机选择的末端位置，并在每一末端位置停留预设时间。

第三种，可以在保持所述机械臂的末端位置不变的情况下，控制所述机械臂的各个连杆在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

可选的，当前位姿保持的预设时间的取值范围可以为1s～2s，设置的预设时间的原则是：应当保证安装于机械臂末端的传感器可以在预设时间内获取到稳定的重力矩阵数据。优选的，预设时间可以是1s，即可以保证安装于机械臂末端的传感器可以在预设时间内获取到稳定的重力矩阵数据，又可以避免整个参数自动辨识过程花费较长时间。

S120：在所述机械臂保持第j个位姿时，利用所述安装于机械臂末端的传感器获取所述操作器在所述末端坐标系下的第j个重力矩阵数据，j≤n。

在机械臂保持每一个位姿时，利用所述机械臂末端安装的六维力传感器获取操作器在末端坐标系下的对应重力矩阵数据。

可以理解的，在不考虑外力对操作器的存在干扰的前提下，末端的操作器仅受重力影响，由于重力场的方向是不会变的，因此，重力大小在机械臂的基坐标系中是不变，但是，重力相对于机械臂的末端坐标系的值会随着机械臂运动发生变化，因此，可以利用获得的多个重力矩阵数据对操作器的质量属性进行辨识。

S200：确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵。

可以利用旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵，旋转变换矩阵确定方法包括标准的DH(Denavit-Hartenberg)法、改进的DH法和S-U(Sheth and Uicker)法中的至少一种。示范性的，可以利用DH法对机器人进行建模，在机械臂处于不同位姿时，获取不同位姿下机械臂的每个连杆的连杆关节角，然后，通过机械臂的各个连杆的连杆关节角以及预设的各个连杆扭曲度、各个连杆偏置可以确定旋转变换矩阵。其中，各个连杆扭曲度和各个连杆偏置与机械臂的构型有关，在机械臂的构型一定时，确定机械臂构型对应的各个连杆扭曲度和各个连杆偏置，将各个连杆扭曲度和各个连杆偏置作为预设值。

示范性的，参见图3，n个旋转变换矩阵利用以下步骤确定：

S210：控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

可以理解的，控制所述机械臂在运动范围内任意变换n次的方法包括：可以预先在机械臂的运动范围内设置n个末端位置，然后控制所述机械臂的末端依次移动至预设的n个末端位置，并在每一末端位置停留预设时间；可以利用随机函数在机械臂的运动范围内随机选择n个末端位置，然后控制所述机械臂的末端依次移动至n个随机选择的末端位置，并在每一末端位置停留预设时间；还可以在保持所述机械臂的末端位置不变的情况下，控制所述机械臂的各个连杆在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

可选的，当前位姿保持的预设时间的取值范围可以为1s～2s，设置的预设时间的原则是：应当保证利用预设的旋转变换矩阵确定方法可以在预设时间内确定对应的旋转变换矩阵。优选的，预设时间可以是1s，即可以保证预设的旋转变换矩阵确定方法可以在预设时间内确定对应的旋转变换矩阵，又可以避免整个参数自动辨识过程花费较长时间。

进一步的，在机械臂保持第j个位姿时，j≤n，执行以下步骤S220～S240，直至j＝n时确定了第n个旋转变换矩阵为止。

S220：获取机械臂的各个连杆关节角。

可以利用机械臂各个关节对应位置的角位移传感器或位置编码器等采集对应关节的连杆关节角。

S230：利用所述各个连杆关节角和预设的各个连杆扭曲度确定各个相邻连杆坐标系之间的相邻变换矩阵。

S240：利用各个相邻变换矩阵确定第j个位姿对应的第j个旋转变换矩阵。

可以理解的，采用DH法对机器人进行建模，在机械臂处于不同位姿时，通过机械臂各个关节的位置编码器或角位移传感器可以获取机械臂的第i个连杆的连杆关节角q_i，利用第i个连杆的连杆关节角q_i和预设的第i个连杆的连杆扭曲度α_i、第i个连杆的连杆偏置d_i可以计算相邻两个坐标系的齐次变换矩阵T_i ^i-1为：

示范性的，如图4所示，一种机械臂包括多个连杆和多个连杆关节，“link i-1”表示第i-1个连杆，“link i”表示第i个连杆，“Joint 1”表示第1个连杆关节，“Joint i-1”表示第i-1个连杆关节，“Joint i”表示第i个连杆关节，“Joint i+1”表示第i+1个连杆关节，q_i表示第i个连杆的连杆关节角，α_i表示预设的第i个连杆的连杆扭曲度，d_i表示第i个连杆的连杆偏置。

进一步的，齐次变换矩阵T_i ^i-1的前三行可以表示相邻变换矩阵

其中，相邻变换矩阵

可以表示第i个连杆对应的坐标系与第i-1个连杆对应的坐标系之间的i个连杆对应的坐标系。

在确定每两个相邻连杆之间的相邻变换矩阵后，根据

可以确定第j个位姿对应的机械臂的基坐标系到末端坐标系的旋转变换矩阵R_j。其中，L为连杆的总数。

S300：利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量。

可以利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量。

S400：将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂。

将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂，使得机械臂的控制端可以对机械臂进行准确的力控制，从而控制机械臂进行精确的位置移动。

可以理解，上述步骤S100和步骤S200可以同时执行，也可依次执行，在依次执行时，步骤S100和步骤S200之间无先后顺序，但是，在依次执行时应该保证第j个旋转变换矩阵和第j个重力矩阵数据对应第j个位姿相同。优选的，同时执行步骤S100和步骤S200，在第j个位姿，同时确定第j个旋转变换矩阵和第j个重力矩阵数据，同时执行步骤S100和步骤S200可以加快操作器的质心坐标和质量的识别速度。

本实施例的技术方案利用机械臂末端的传感器获取所述机械臂处于n个不同位姿时所述机械臂末端的操作器在末端坐标系下的n个重力矩阵数据，n≥3；利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；只需要获取机械臂在n个不同位姿时对应的n个重力矩阵数据和n个旋转变换矩阵，即可计算所述操作器的质心坐标和质量。本申请公开的计算所述操作器的质心坐标和质量的技术方案不仅计算量小，而且可以实现在线进行数据处理，确定操作器的质心坐标和质量，无需离线求解后再修改模型参数，进而有效提高确定操作器相关参数的速度；并且，本申请公开的将所述质心坐标、所述质量反馈至所述机械臂的控制端的技术方案，可以使所述机械臂的控制端可以根据所述质心坐标和所述质量控制所述机械臂，使得机械臂的控制端可以对机械臂进行准确的力控制，从而控制机械臂进行精确的位置移动。

实施例2

进一步的，利用以下公式确定所述操作器的质心坐标和质量：

其中，

j＝1、2或3，m表示所述操作器的质量，r_c表示所述操作器的质心坐标，f_FTbias表示所述机械臂末端的传感器采集力的静态误差，τ_FTbias表示所述机械臂末端的传感器采集力矩的静态误差，Y_j表示第j个位姿对应的参数矩阵，

表示第j个旋转变换矩阵对应的转置矩阵，g表示重力加速度向量，

表示求

的反对称矩阵，0_1×3表示1行3列的零矩阵，I_3×3表示3行3列的单位矩阵，0_3×3表示3行3列的零矩阵，W_j表示第j个位姿对应的重力矩阵数据，f_FTj表示第j个位姿对应的重力矩阵数据的力向量，τ_FTi表示第j个位姿对应的重力矩阵数据的力矩向量。

应当理解，六维力传感器，在每次上电使用时，六维力传感器获取的采集值都存在静态误差，并且，静态误差每次都不一样。一般，在使用前需要先将六维力传感器静置一段时间后，测量出六维力传感器的静态误差后再用于测量力。但是每次都需要将六维力传感器和末端的操作器都拆下来，测量完静态误差后再使用，非常的耗时且不方便。若不将六维力和末端的操作器卸下来测量，静态误差将受末端的操作器影响，导致六维力测量结果误差较大。因此，本申请公开的用于确定操作器的质心坐标和质量的公式中考虑到六维力传感器每次上电后的静态误差的影响，用f_FTbias表示所述机械臂末端的传感器获取的力的静态误差，τ_FTbias表示所述机械臂末端的传感器获取的力矩的静态误差。

进一步的，假设Σ_base为机械臂的基坐标系，Σ_FT为安装在机械臂末端六维力传感器的末端坐标系，Σ_load为位于末端操作器质心处的坐标系。在任意时刻，末端操作器质心受到的重力在基坐标系中的大小不变，可以表示为

其中m为末端的操作器的质量，g∈R³为重力加速度。

进一步的，安装在机械臂末端的六维力传感器采集到的操作器的重力将受机械臂的运动影响，且采集值中还包含了六维力传感器上电后的静态误差。因此，可以得到六维力传感器采集的重力矩阵数据为

并且，

为末端的操作器受到的重力在六维力传感器所在的末端坐标系中的值，

为六维力传感器的静态误差。由于，

和

只是末端的操作器质心受到的重力在不同坐标系下的表示，因此，可以根据机械臂的构型和坐标转换得到二者之间的转换关系。r_c＝[r_cx,r_cy,r_cz]为末端的操作器质心坐标，S为计算r_c向量的反对称矩阵，

为基坐标系到六维力传感器的旋转变换矩阵。

即为从机械臂的基座到六维力传感器的中相邻变换矩阵依次相乘。

进一步的，将需要辨识的质量、质心坐标、传感器静态误差提出来，可以得到如下表达式：

其中，需要辨识的参数

总共有10个，而

为6*10的矩阵，仅一个点的数据无法求得所需参数，因此，对Y矩阵进行判断，以找出最少需要采集的机器人的信息数。假设需要n个点的信息，则n个点的六维力信息和辨识矩阵组合起来如下所示：

要想求解，则Y矩阵的行秩应该要大于辨识参数的个数，即rank(Y)≥10，通过分析可得，当n＝1时，rank(Y)＝5，当m＝2时rank(Y)＝9，当n≥3时，rank(Y)＝10，因此，对于任意关节数目的机械臂，最少仅需要3个不同的机械臂位姿就可求解出末端的操作器的参数和六维力传感器静态误差，并且无需分步求解，只要采集三个位姿对应的三个重力矩阵数据和三个旋转变换矩阵数据，即可求解出末端的操作器的质心坐标r_c和质量m。

实施例3

进一步的，参见图5，机械臂控制方法还包括以下步骤：

S500：将所述安装于机械臂末端的传感器静态误差反馈至所述机械臂的控制端。

将安装于机械臂末端的传感器静态误差反馈至所述机械臂的控制端，以使控制端可以根据操作器的质心坐标、质量以及所述机械臂末端的传感器静态误差对重力向和静态误差进行补偿，进而下发准确的控制指令，提高控制精度，提高机械臂的动态和稳态性能。

实施例4

本申请的一个实施例，如图6所示，提出一种机械臂控制装置10包括获取模块11、确定模块12、计算模块13和控制模块14。

获取模块11，用于利用机械臂末端的传感器获取所述机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3；确定模块12，用于利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；计算模块13，用于利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量；控制模块14，用于将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂；还用于将所述安装于机械臂末端的传感器静态误差反馈至所述机械臂的控制端。

进一步的，所述利用机械臂末端的传感器获取所述机械臂处于n个不同位姿时所述机械臂末端的操作器在末端坐标系下的n个重力矩阵数据，包括：控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；在所述机械臂保持第j个位姿时，利用所述机械臂末端的传感器获取所述操作器在所述末端坐标系下的第j个重力矩阵数据，j≤n。

进一步的，所述利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵，包括：控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；在所述机械臂保持第j个位姿时，j≤n：

利用所述机械臂各个关节位置的编码器获取机械臂的各个连杆关节角；利用所述各个连杆关节角和预设的各个连杆扭曲度确定各个相邻连杆坐标系之间的相邻变换矩阵；利用各个相邻变换矩阵确定第j个位姿对应的第j个旋转变换矩阵。

进一步的，利用以下公式确定所述操作器的质心坐标和质量：

j＝1、2或3，m表示所述操作器的质量，r_c表示所述操作器的质心坐标，f_FTbias表示所述机械臂末端的传感器获取的力的静态误差，τ_FTbias表示所述机械臂末端的传感器获取的力矩的静态误差，Y_j表示第j个位姿对应的参数矩阵，

表示第j个旋转变换矩阵对应的转置矩阵，g表示重力加速度向量，

表示求

的反对称矩阵，0_1×3表示1行3列的零矩阵，I_3×3表示3行3列的单位矩阵，0_3×3表示3行3列的零矩阵，W_j表示第j个位姿对应的重力矩阵数据，f_FTj表示第j个位姿对应的重力矩阵数据对应的力的向量，τ_FTi表示第j个位姿对应的重力矩阵数据对应的力矩的向量。

进一步的，所述控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿，包括：

控制所述机械臂的末端依次移动至预设的n个末端位置，并在每一末端位置停留预设时间，所述预设的n个末端位置在所述运动范围内；或，利用随机函数在所述运动范围内随机选择n个末端位置，控制所述机械臂的末端依次移动至n个末端位置，并在每一末端位置停留预设时间；或，在保持所述机械臂的末端位置不变的情况下，控制所述机械臂的各个连杆在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

进一步的，所述预设时间的取值范围为1s～2s。

本实施例公开的机械臂控制装置10通过获取模块11、确定模块12、计算模块13和控制模块14的配合使用，用于执行上述实施例所述的机械臂控制方法，上述实施例所涉及的实施方案以及有益效果在本实施例中同样适用，在此不再赘述。

本申请的一些实施例，如图7所示，提出一种机械臂200，包括存储器210和处理器220，所述存储器210存储有计算机程序，所述计算机程序在所述处理器220上运行时执行本申请所述的机械臂控制方法。

本申请还提出一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本申请所述的机械臂控制方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种机械臂控制方法，其特征在于，所述方法包括：

利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3；

确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；

利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量；

将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂。

2.根据权利要求1所述的机械臂控制方法，其特征在于，所述利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，包括：

控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；

在所述机械臂保持第j个位姿时，利用所述安装于机械臂末端的传感器获取所述操作器在所述末端坐标系下的第j个重力矩阵数据，j≤n。

3.根据权利要求1所述的机械臂控制方法，其特征在于，所述利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵，包括：

控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿；

在所述机械臂保持第j个位姿时，j≤n：

获取机械臂的各个连杆关节角；

利用所述各个连杆关节角和预设的各个连杆扭曲度确定各个相邻连杆坐标系之间的相邻变换矩阵；

利用各个相邻变换矩阵确定第j个位姿对应的第j个旋转变换矩阵。

4.根据权利要求1所述的机械臂控制方法，其特征在于，利用以下公式确定所述操作器的质心坐标和质量：

或3，m表示所述操作器的质量，r_c表示所述操作器的质心坐标，f_FTbias表示所述安装于机械臂末端的传感器采集力的静态误差，τ_FTbias表示所述安装于机械臂末端的传感器采集力矩的静态误差，Y_j表示第j个位姿对应的参数矩阵，

表示第j个旋转变换矩阵对应的转置矩阵，g表示重力加速度向量，

表示求

的反对称矩阵，0_1×3表示1行3列的零矩阵，I_3×3表示3行3列的单位矩阵，0_3×3表示3行3列的零矩阵，W_j表示第j个位姿对应的重力矩阵数据，f_FTj表示第j个位姿对应的重力矩阵数据的力向量，τ_FTi表示第j个位姿对应的重力矩阵数据的力矩向量。

5.根据权利要求2或3所述的机械臂控制方法，其特征在于，所述控制所述机械臂在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿，包括：

控制所述机械臂的末端依次移动至预设的n个末端位置，并在每一末端位置停留预设时间，所述预设的n个末端位置在所述运动范围内；

或，利用随机函数在所述运动范围内随机选择n个末端位置，控制所述机械臂的末端依次移动至n个末端位置，并在每一末端位置停留预设时间；

或，在保持所述机械臂的末端位置不变的情况下，控制所述机械臂的各个连杆在运动范围内任意变换n次，并且每一次变换后在预设时间内保持当前位姿。

6.根据权利要求2或3所述的机械臂控制方法，其特征在于，所述预设时间的取值范围为1s～2s。

7.根据权利要求4所述的机械臂控制方法，其特征在于，还包括：将所述安装于机械臂末端的传感器静态误差反馈至所述机械臂的控制端。

8.一种机械臂控制装置，其特征在于，所述装置包括：

获取模块，用于利用安装于机械臂末端的传感器获取所述安装于机械臂末端的操作器的n个重力矩阵数据，所述n个重力矩阵数据为所述机械臂处于n个不同位姿时所述操作器在末端坐标系下的n个重力矩阵数据，n≥3；

确定模块，用于利用预设的旋转变换矩阵确定方法确定所述机械臂处于n个不同位姿时所述机械臂的基坐标系到所述末端坐标系的n个旋转变换矩阵；

计算模块，用于利用所述n个重力矩阵数据和所述n个旋转变换矩阵计算所述操作器的质心坐标和质量；

控制模块，用于将所述质心坐标、所述质量反馈至所述机械臂的控制端，以使所述机械臂的控制端根据所述质心坐标和所述质量控制所述机械臂。

9.一种机械臂，其特征在于，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行权利要求1至7任一项所述的机械臂控制方法。

10.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时执行权利要求1至7任一项所述的机械臂控制方法。

Images