一种三连杆机械手智能控制方法、芯片和系统与流程

本发明涉及三连杆机械手智能技术领域，尤其涉及一种三连杆机械手智能控制方法、芯片和系统。

背景技术：

随着自动化技术的不断发展和进步，物流自动化领域对三连杆机械手的应用日渐广泛，要求也不断提高，三连杆工业机械手相关技术的研究也得以深化和展开。三连杆机械手能够准确地完成任务，它的运动过程中的准度、精度和稳定性不可忽视，特别是三连杆机械手末端位置的精确控制。但三连杆机械手是一种复杂的序列关节结构，位置研究情况较为复杂，因此如何实现对三连杆机械手的精确控制是业内亟待解决的技术问题。

技术实现要素：

本发明所要解决的技术问题是针对现有技术的不足，提供了一种三连杆机械手智能控制方法、芯片和系统。

本发明的一种三连杆机械手智能控制方法的技术方案如下：

s1、根据预设末端坐标(px，py，pz)计算出第一关节需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3；

s2、控制所述第一关节绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3；

其中，三连杆机械手包括依次连接的第一连杆、第二连杆和第三连杆，所述第一关节为所述第一连杆的首端，

所述第二关节为所述第一关节的末端与所述第二连杆的首端的连接处，

所述第三关节为所述第二连杆的的末端与所述第三连杆的首端的连接处；

以所述第一关节的位置作为原点建立所述第一坐标系，且以所述第一关节作为所述三连杆机械手的基座；

以所述第二关节的位置作为原点建立所述第二坐标系；

以所述第三关节的位置作为原点建立所述第三坐标系；

所述预设末端坐标(px，py，pz)为所述第三连杆的末端在所述第一坐标系上的坐标。

本发明的一种三连杆机械手智能控制方法的有益效果如下：以三连杆机械手的第一关节、第二关节和第三关节为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3，然后控制所述第一关节绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制，由此实现了一种能对三连杆机械手进行精确控制的三连杆机械手智能控制方法。

在上述方案的基础上，本发明的一种三连杆机械手智能控制方法还可以做如下改进。

进一步，还包括：以所述第四关节的位置作为原点建立第四坐标系；

将所述第一坐标系、所述第二坐标系、所述第三坐标系和所述第四坐标系中任意相邻的两个坐标系中，前一个坐标系的x轴与下一个坐标系的z轴垂直且相交。

采用上述进一步方案的有益效果是：通过设置第一坐标系、所述第二坐标系、所述第三坐标系和第四坐标系之间的关系，便于对第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3进行计算。

进一步，s1具体包括如下步骤：

s10、设置第一公式为：a1＝0，α1＝0，d1＝0，

第二公式为：a2＝a1，d2＝0，

第三公式为：a3＝a2，α3＝0，d3＝0，

第四公式为：a4＝a3，d3＝d3，θ4＝0；

s11、根据所述第一公式至所述第四公式推出第十六公式、第十七公式和第十八公式；

根据所述第十六公式：计算出所述第一旋转角度θ1；

根据所述第十七公式：

计算出所述第二旋转角度θ2；

根据所述第十八公式：计算出所述第二旋转角度θ3；

其中，

i＝1,2,3,4，ai表示第i关节顺xi-1轴从zi-1到zi轴的空间距离；

αi表示第i关节绕xi-1轴从zi-1到zi轴的角度；

di表示第i关节顺zi轴从xi-1到xi轴的空间距离；

θi表示第i关节绕zi轴从xi-1到xi轴的空间距离；

xi轴表示所述第一坐标系至所述第四坐标中的第i坐标系的x轴；

zi轴表示所述第一坐标系至所述第四坐标中的第i坐标系的z轴；

表示所述第二坐标系的x轴与所述第一坐标系的x轴之间的角度；

表示所述第三坐标系的x轴与所述第二坐标系的x轴之间的角度。

进一步，s11具体包括如下步骤：

s110、将αi用矩阵进行表示，得到第五公式：

将ai用矩阵进行表示，得到第六公式：

将θi用矩阵进行表示，得到第七公式：

将di用矩阵进行表示，得到第八公式：

将所述第五公式、所述第六公式、所述第七公式和所述第八公式相乘，得到第九公式：

s111、将所述第一公式、所述第二公式、所述第三公式和所述第四公式带入所述第九公式，然后矩阵相乘得到第十公式：

其中，

s112、对所述第三连杆的末端用矩阵进行表示，得到第十一公式：

且a＝t1，标记为第十二公式；

s113、在所述第十二公式的两边均乘以⁰t1的逆矩阵得到第十三公式：^ot1^-1a＝^ot1^-1t1，其中，

s114、根据第十四公式：

计算出

其中，px1＝-pxsinθ1-pycosθ1；py1＝-pxcosθ1-pysinθ1；pz1＝pz；

根据第十五公式：计算出^ot1^-1t1；

s115、根据所述第十四公式和所述第十五公式推出所述第十六公式、所述第十七公式和所述第十八公式。

本发明的一种芯片的技术方案是：用于执行上述任一项所述的一种三连杆机械手智能控制方法。

本发明的一种芯片的有益效果是：芯片以三连杆机械手的第一关节、第二关节和第三关节为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第一坐标系的z轴旋转第三旋转角度θ3，然后芯片控制所述第一关节绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制。

本发明的一种三连杆机械手智能控制系统的技术方案如下：

采用上述的一种芯片，还包括三个电机，且与所述的三个电机与所述第一关节、所述第二关节和所述第三关节一一对应，所述芯片为plc控制器，所述plc控制器通过控制相应的电机进而控制所述第一关节绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3。

本发明的一种三连杆机械手智能控制系统的有益效果如下：

以三连杆机械手的第一关节、第二关节和第三关节为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第一坐标系的z轴旋转第三旋转角度θ3，然后芯片控制所述第一关节绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制。

其中，芯片可采用plc控制器，通过plc控制器控制电机进而控制相应的电机进而控制所述第一关节绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节需绕第三坐标系的z轴旋转第三旋转角度θ3，结构简单。

附图说明

图1为本发明实施例的一种三连杆机械手智能控制方法的流程示意图；

图2为本发明实施例的一种三连杆机械手智能控制方法的计算模型的示意图；

图3为本发明实施例的一种三连杆机械手智能控制系统的结构示意图。

图4为本发明实施例的一种三连杆机械手智能控制系统的控制逻辑示意图。

具体实施方式

如图1所示，本发明实施例的一种三连杆机械手智能控制方法，包括如下步骤：

s1、根据预设末端坐标(px，py，pz)计算出第一关节1需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3；

s2、控制所述第一关节1绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节3绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3；

其中，三连杆机械手包括依次连接的第一连杆2、第二连杆4和第三连杆6，

所述第一关节1为所述第一连杆2的首端，

所述第二关节3为所述第一关节1的末端与所述第二连杆4的首端的连接处，

所述第三关节5为所述第二连杆4的的末端与所述第三连杆6的首端的连接处；

以所述第一关节1的位置作为原点建立所述第一坐标系，且以所述第一关节1作为所述三连杆机械手的基座；

以所述第二关节3的位置作为原点建立所述第二坐标系；

以所述第三关节5的位置作为原点建立所述第三坐标系；

所述预设末端坐标(px，py，pz)为所述第三连杆6的末端在所述第一坐标系上的坐标。

以三连杆机械手的第一关节1、第二关节3和第三关节5为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节1需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，然后控制所述第一关节1绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆6的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制，由此实现了一种能对三连杆机械手进行精确控制的三连杆机械手智能控制方法。

较优地，在上述技术方案中，还包括：以所述第四关节7的位置作为原点建立第四坐标系；将所述第一坐标系、所述第二坐标系、所述第三坐标系和所述第四坐标系中任意相邻的两个坐标系中，前一个坐标系的x轴与下一个坐标系的z轴垂直且相交。通过设置第一坐标系、所述第二坐标系、所述第三坐标系和第四坐标系之间的关系，便于对第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3进行计算。

较优地，在上述技术方案中，s1具体包括如下步骤：

s10、设置第一公式为：a1＝0，α1＝0，d1＝0，

第二公式为：a2＝a1，d2＝0，

第三公式为：a3＝a2，α3＝0，d3＝0，

第四公式为：a4＝a3，d3＝d3，θ4＝0；

s11、根据所述第一公式至所述第四公式推出第十六公式、第十七公式和第十八公式；

根据所述第十六公式：计算出所述第一旋转角度θ1；

根据所述第十七公式：

计算出所述第二旋转角度θ2；

根据所述第十八公式：计算出所述第二旋转角度θ3；

其中，

i＝1,2,3,4，ai表示第i关节顺xi-1轴从zi-1到zi轴的空间距离；

αi表示第i关节绕xi-1轴从zi-1到zi轴的角度；

di表示第i关节顺zi轴从xi-1到xi轴的空间距离；

θi表示第i关节绕zi轴从xi-1到xi轴的空间距离；

xi轴表示所述第一坐标系至所述第四坐标中的第i坐标系的x轴；

zi轴表示所述第一坐标系至所述第四坐标中的第i坐标系的z轴；

表示所述第二坐标系的x轴与所述第一坐标系的x轴之间的角度；

表示所述第三坐标系的x轴与所述第二坐标系的x轴之间的角度。

较优地，在上述技术方案中，s11具体包括如下步骤：

s110、将αi用矩阵进行表示，得到第五公式：

将ai用矩阵进行表示，得到第六公式：

将θi用矩阵进行表示，得到第七公式：

将di用矩阵进行表示，得到第八公式：

将所述第五公式、所述第六公式、所述第七公式和所述第八公式相乘，得到第九公式：

s111、将所述第一公式、所述第二公式、所述第三公式和所述第四公式带入所述第九公式，然后矩阵相乘得到第十公式：

其中，

s112、对所述第三连杆6的末端用矩阵进行表示，得到第十一公式：

且a＝t1，标记为第十二公式；

s113、在所述第十二公式的两边均乘以⁰t1的逆矩阵得到第十三公式：^ot1^-1a＝^ot1^-1t1，其中，

s114、根据第十四公式：

计算出

其中，px1＝-pxsinθ1-pycosθ1；py1＝-pxcosθ1-pysinθ1；pz1＝pz；

根据第十五公式：计算出^ot1^-1t1；

s115、根据所述第十四公式和所述第十五公式推出所述第十六公式、所述第十七公式和所述第十八公式。

下面结合图2对本申请中的一种三连杆机械手智能控制方法进行更详细地阐述：

图2中，第一坐标系的x轴、y轴和z轴分别标记为x1、y1和z1；第二坐标系的x轴、y轴和z轴分别标记为x2、y2和z2；第三坐标系的x轴、y轴和z轴分别标记为x1、y3和z3；第四坐标系的x轴、y轴和z轴分别标记为x4、y4和z4；在三连杆机械手中，驱动关节就会驱动相应地连杆做相应的动作，此为常规技术，在此不做赘述。

按照上文内容建立第一坐标系、第二坐标系、第三坐标系和第四坐标系，设置第一公式为：a1＝0，α1＝0，d1＝0，

第二公式为：a2＝a1，d2＝0，

第三公式为：a3＝a2，α3＝0，d3＝0，

第四公式为：a4＝a3，d3＝d3，θ4＝0；

将αi用矩阵进行表示，得到第五公式：

将ai用矩阵进行表示，得到第六公式：

将θi用矩阵进行表示，得到第七公式：

将di用矩阵进行表示，得到第八公式：

将所述第五公式、所述第六公式、所述第七公式和所述第八公式相乘，得到第九公式：

将所述第一公式、所述第二公式、所述第三公式和所述第四公式带入所述第九公式，然后矩阵相乘得到第十公式：

其中，

对所述第三连杆6的末端用矩阵进行表示，得到第十一公式：

且a＝t1，标记为第十二公式；

s113、在所述第十二公式的两边均乘以⁰t1的逆矩阵得到第十三公式：^ot1^-1a＝^ot1^-1t1，其中，

根据第十四公式：

计算出

其中，px1＝-pxsinθ1-pycosθ1；py1＝-pxcosθ1-pysinθ1；pz1＝pz；

根据第十五公式：计算出^ot1^-1t1；

在第十四公式和第十五公式中，即^ot1^-1a与⁰t1^-1t1的第二行、第四列的元素相等且等于0，可得到第十六公式：

在第十四公式和第十五公式中，即^ot1^-1a与⁰t1^-1t1的第一行、第四列的元素相等，以及第三行、第四列的元素相等，可得到第十七公式：

和第十八公式：

其中，

根据第十六公式、第十七公式和第十八公式得到了三连杆机械手的预设末端坐标(px，py，pz)与第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3之间的关系，从而控制所述第一关节1绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节3绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3实现对三连杆机械手的精确控制。

其中，可设置上述的距离单位均为m，上述的角度单位均为rad，便于进行计算，且c2、s2、c23和s23均为无量纲的中间变量。

本发明实施例的一种芯片，用于执行上述任一实施例所述的一种三连杆机械手智能控制方法。

芯片以三连杆机械手的第一关节1、第二关节3和第三关节5为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节1需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，然后控制所述第一关节1绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆6的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制，其中，芯片可为plc控制器或cpu等。

如图3和图4所示，本发明实施例的一种三连杆机械手智能控制系统，采用上述实施例中一种芯片，还包括三个电机，且与所述的三个电机与所述第一关节1、所述第二关节3和所述第三关节5一一对应，所述芯片为plc控制器，所述plc控制器通过控制相应的电机进而控制所述第一关节1绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节3绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3。

芯片以三连杆机械手的第一关节1、第二关节3和第三关节5为原点分别建立第一坐标系、第二坐标系和第三坐标系，根据预设末端坐标计算出第一关节1需绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3需绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，然后控制所述第一关节1绕第一坐标系的z轴旋转第一旋转角度θ1、第二关节3绕第二坐标系的z轴旋转第二旋转角度θ2和第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，使第三连杆6的末端的坐标准确的到达预设末端坐标的位置，且只需控制三个参数：第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3就能实现对三连杆机械手的精确控制。

当芯片为plc控制器时，plc控制器控制电机进而控制相应的电机进而控制所述第一关节1绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节3绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，结构简单。

详细地，将与第一关节1对应的电机标记为m1，与第二关节3对应的电机标记为m2，与第三关节5对应的电机标记为m3；与m1相应的变频器标记为1#变频器，与m2相应的变频器标记为2#变频器，与m3相应的变频器标记为3#变频器；

通过监测中心计算机对plc控制器进行系统设置，也就是将a1、a2、a3、d3、以及预设末端坐标(px，py，pz)序列存储在plc控制器的记忆数据库中，根据第十六公式、第十七公式和第十八公式得到三连杆机械手的预设末端坐标(px，py，pz)与第一旋转角度θ1、第二旋转角度θ2和第三旋转角度θ3；然后对plc控制器向1#变频器、2#变频器和3#变频器分别发出指令，进而分别控制m1、m2和m3驱动第一关节1绕所述第一坐标系的z轴旋转第一旋转角度θ1、所述第二关节3绕所述第二坐标系的z轴旋转第二旋转角度θ2和所述第三关节5需绕第三坐标系的z轴旋转第三旋转角度θ3，从而实现对三连杆机械手的精确控制。

在本发明中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。