CN111673716A - 灵巧手定位方法及控制系统 - Google Patents

Abstract

本发明提供一种灵巧手定位方法及控制系统，用于三连杆机械手，三连杆机械手包括依次连接的基座、第一连杆、第二连杆和第三连杆；通过按照特定的原则对每个关节建立坐标系，然后计算相邻关节的坐标转换矩阵，从而实现对机械手末端的位置进行精确定位控制。

Description

灵巧手定位方法及控制系统

技术领域

本发明属于机械手的定位技术领域，具体涉及一种灵巧手定位方法及控制系统。

背景技术

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

发明内容

本发明要解决的技术问题是：提供一种灵巧手定位方法及控制系统，实现三连杆机械手末端位置精确定位。

本发明为解决上述技术问题所采取的技术方案为：一种灵巧手定位方法，用于三连杆机械手，三连杆机械手包括依次连接的基座、第一连杆、第二连杆和第三连杆；对三连杆机械手的关节做如下设置：

定义三连杆机械手的关节编号i为1、2、3、4；其中第一关节定义为基座所在处，以第一关节为坐标原点建立空间三维坐标系，三个轴为x₁轴、y₁轴、z₁轴；第二关节定义为第一连杆与第二连杆连接处，以第二关节为坐标原点建立空间三维坐标系，三个轴为x₂轴、y₂轴、z₂轴；第三关节定义为第二连杆与第三连杆连接处，以第三关节为坐标原点建立空间三维坐标系，三个轴为x₃轴、y₃轴、z₃轴；第四关节定义为第三连杆的末端，即机械手末端，以机械手末端为坐标原点建立空间三维坐标系，三个轴为x₄轴、y₄轴、z₄轴；以第一关节为坐标原点的空间三维坐标系中，机械手末端坐标(P_x，P_y，P_z)；

定义出关节所在处的坐标系相应于下一个相邻坐标系的参数如下：a_i是顺x_i-1坐标轴从z_i-1坐标轴到z_i坐标轴的空间距离，单位为m；α_i是绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度，单位为rad；d_i是顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离，单位为m；Θ_i是绕z_i坐标轴从x_i-1坐标轴到x_i坐标轴的角度，单位为rad；i＝1,2,3,4；

A₁为第一关节的旋转轴线与第二关节的旋转轴线之间的距离，单位为m；A₂为第二关节的旋转轴线与第三关节的旋转轴线之间的距离，单位为m；A₃为第三关节的旋转轴线与机械手末端之间的距离，单位为m；D₃为第三关节的x₃轴与机械手末端的x₄轴之间的距离，单位为m；

为第二关节的x₂轴与第一关节的x₁轴之间的角度，单位为rad；

为第三关节的x3轴与第二关节的x2轴之间的角度，单位为rad；θ₁为第一关节的沿z₁轴旋转角度，单位为rad；θ₂为第二关节的沿z₂轴旋转角度，单位为rad；θ₃为第三关节的沿z₃轴旋转角度，单位为rad；

因绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度α_i，实现相邻关节的转换用下列矩阵表示：

因顺x_i-1坐标轴从z_i-1坐标轴到z坐标轴的空间距离a_i，实现相邻关节的转换用下列矩阵表示：

因绕z_i坐标轴从x_i-1坐标轴到x坐标轴的角度Θ_i，实现相邻关节的转换用下列矩阵表示：

顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离d_i，实现相邻关节的转换用下列矩阵表示：

将式(5)、式(6)、式(7)、式(8)相乘，得出相邻关节四个参数转换的矩阵如下：

^i-1T_i＝Rot(x_i-1,α_i)Trans(x_i-1,a_i)Rot(z_i,Θ_i)Trans(z_i,d_i)

将式(1)、式(2)、式(3)、式(4)的数值带入式(9)，然后矩阵相乘，得到

其中

又因三连杆机械手末端用一个4x4矩阵进行如下表述，即

其中(P_x，P_y，P_z)则为机械手末端坐标，且

A＝T₁ 式(16)

两边均左乘⁰T₁的逆，得到

^oT₁ ^-1A＝^oT₁ ^-1T₁ 式(17)

其中

其中P_x1＝-P_xsinΘ₁-P_ycosΘ₁ 式(20)

P_y1＝-P_xcosΘ₁-P_ysinΘ₁ 式(21)

P_z1＝P_z 式(22)

接着，计算^oT₁ ^-1T₁：

从中得到，^oT₁ ^-1A与⁰T₁ ^-1T₁的第二行、第四列的元素相等且等于0，得到

由^oT₁ ^-1A与⁰T₁ ^-1T₁的第1行、第四列元素相等，以及第三行、第四列元素相等，得到，

其中

由式(24)、式(25)、式(27)得到机械手末端坐标与θ₁，θ₂，θ₃的关系；

当已知机械手末端的目标位置时，控制第一关节、第二关节和第三关节的旋转角度，使得θ₁，θ₂，θ₃到达对应的角度即可。

一种灵巧手定位控制系统，本系统包括角度传感器、控制器、存储器和执行机构；其中，角度传感器用于采集三连杆机械手各个关节的旋转角度，存储器中存有计算机程序供控制器调用，以完成所述的灵巧手定位方法，并由执行机构执行。

本发明的有益效果为：通过采用本发明方法和系统，能够实现三连杆机械手末端的准确定位控制。

附图说明

图1为本发明的三连杆机械手结构图。

图2为本发明一实施例的硬件框图。

图3为本发明一实施例的控制流程图。

图中：第一关节1、第一连杆2、第二关节3、第二连杆4、第三关节5，第三连杆6、机械手末端7

具体实施方式

下面结合具体实例和附图对本发明做进一步说明。

本发明提供一种灵巧手定位方法，用于三连杆机械手，如图1所示，三连杆机械手包括依次连接的基座、第一连杆2、第二连杆4和第三连杆6；对三连杆机械手的关节做如下设置：

定义三连杆机械手的关节编号i为1、2、3、4；其中第一关节1定义为基座所在处，以第一关节1为坐标原点建立空间三维坐标系，三个轴为x₁轴、y₁轴、z₁轴；第二关节3定义为第一连杆2与第二连杆4连接处，以第二关节3为坐标原点建立空间三维坐标系，三个轴为x₂轴、y₂轴、z₂轴；第三关节5定义为第二连杆4与第三连杆6连接处，以第三关节5为坐标原点建立空间三维坐标系，三个轴为x₃轴、y₃轴、z₃轴；第四关节定义为第三连杆6的末端，即机械手末端7，以机械手末端7为坐标原点建立空间三维坐标系，三个轴为x₄轴、y₄轴、z₄轴；以第一关节1为坐标原点的空间三维坐标系中，机械手末端7的坐标(P_x，P_y，P_z)。

关节坐标轴的设置原则如下：①经过设置各个关节所在坐标系的零点和各个坐标轴，使得上一个关节所在坐标系的x坐标轴与相邻的下一个坐标系的z坐标轴垂直并且相交；②关节轴所在位置与关节所在坐标系的z轴一致。关节轴即为关节旋转轴。

定义出关节所在处的坐标系相应于下一个相邻坐标系的参数如下：a_i是顺x_i-1坐标轴从z_i-₁坐标轴到z_i坐标轴的空间距离，单位为m；α_i是绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度，单位为rad；d_i是顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离，单位为m；Θ_i是绕z_i坐标轴从x_i-1坐标轴到x_i坐标轴的角度，单位为rad；i＝1,2,3,4；

A₁为第一关节的旋转轴线与第二关节的旋转轴线之间的距离，单位为m；A₂为第二关节的旋转轴线与第三关节的旋转轴线之间的距离，单位为m；A₃为第三关节的旋转轴线与机械手末端之间的距离，单位为m；D₃为第三关节的x₃轴与机械手末端的x₄轴之间的距离，单位为m；

为第二关节的x₂轴与第一关节的x₁轴之间的角度，单位为rad；

为第三关节的x3轴与第二关节的x2轴之间的角度，单位为rad；θ₁为第一关节的沿z₁轴旋转角度，单位为rad；θ₂为第二关节的沿z₂轴旋转角度，单位为rad；θ₃为第三关节的沿z₃轴旋转角度，单位为rad；

因绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度α_i，实现相邻关节的转换用下列矩阵表示：

因顺x_i-1坐标轴从z_i-1坐标轴到z坐标轴的空间距离a_i，实现相邻关节的转换用下列矩阵表示：

因绕z_i坐标轴从x_i-1坐标轴到x坐标轴的角度Θ_i，实现相邻关节的转换用下列矩阵表示：

顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离d_i，实现相邻关节的转换用下列矩阵表示：

将式(5)、式(6)、式(7)、式(8)相乘，得出相邻关节四个参数转换的矩阵如下：

^i-1T_i＝Rot(x_i-1,α_i)Trans(x_i-1,a_i)Rot(z_i,Θ_i)Trans(z_i,d_i)

将式(1)、式(2)、式(3)、式(4)的数值带入式(9)，然后矩阵相乘，得到

其中

又因三连杆机械手末端用一个4x4矩阵进行如下表述，即

其中(P_x，P_y，P_z)则为机械手末端坐标，且

A＝T₁ 式(16)

两边均左乘⁰T₁的逆，得到

^oT₁ ^-1A＝^oT₁ ^-1T₁ 式(17)

其中

其中P_x1＝-P_xsinΘ₁-P_ycosΘ₁ 式(20)

P_y1＝-P_xcosΘ₁-P_ysinΘ₁ 式(21)

P_z1＝P_z 式(22)

接着，计算^oT₁ ^-1T₁：

从中得到，^oT₁ ^-1A与⁰T₁ ^-1T₁的第二行、第四列的元素相等且等于0，得到

由^oT₁ ^-1A与⁰T₁ ^-1T₁的第1行、第四列元素相等，以及第三行、第四列元素相等，得到，

其中

由式(24)、式(25)、式(27)得到机械手末端坐标与θ₁，θ₂，θ₃的关系；

当已知机械手末端的目标位置时，控制机械手使得θ₁，θ₂，θ₃到达对应的角度即可。

如图2和图3所示，本发明还提供一种灵巧手定位控制系统，本系统包括角度传感器、控制器、存储器和执行机构；其中，角度传感器用于采集三连杆机械手各个关节的旋转角度θ₁、θ₂、θ₃，存储器中存有计算机程序供控制器调用，以完成所述的灵巧手定位方法，并由执行机构执行。本实施例中，执行机构为旋转电机，其中第一电机M1控制θ₁，第二电机M2控制θ₂，第三电机M3控制θ₃；控制器为PLC控制器，通过控制第一电机、第二电机和第三电机的变频器而控制第一电机、第二电机和第三电机。优选的，PLC控制器还连接有监控中心计算机，通过在监控中心计算机中设置程序，发送指令给PLC控制器。

本发明中，首先对系统进行设置：对A₁，A₂，A₃，D₃，

机械手的末端坐标(P_x，P_y，P_z)序列存储在PLC记忆数据库中。然后，依据系统设置参数，由式(24)、式(25)、式(27)可以得到机械手末端坐标与θ₁，θ₂，θ₃的关系，PLC输出控制1#变频器，1#变频器控制第一电机M1的θ₁；PLC输出控制2#变频器，2#变频器控制第二电机M2的θ₂；PLC输出控制3#变频器，3#变频器控制第三电机M3的θ₃；从而使机械手末端达到系统设置的位置。

本发明提供一种三连杆机械手智能控制系统，从而实现三连杆机械手末端位置精确控制的目的。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (5)

1.一种灵巧手定位方法，用于三连杆机械手，其特征在于：三连杆机械手包括依次连接的基座、第一连杆、第二连杆和第三连杆；对三连杆机械手的关节做如下设置：

定义三连杆机械手的关节编号i为1、2、3、4；其中第一关节定义为基座所在处，以第一关节为坐标原点建立空间三维坐标系，三个轴为x₁轴、y₁轴、z₁轴；第二关节定义为第一连杆与第二连杆连接处，以第二关节为坐标原点建立空间三维坐标系，三个轴为x₂轴、y₂轴、z₂轴；第三关节定义为第二连杆与第三连杆连接处，以第三关节为坐标原点建立空间三维坐标系，三个轴为x₃轴、y₃轴、z₃轴；第四关节定义为第三连杆的末端，即机械手末端，以机械手末端为坐标原点建立空间三维坐标系，三个轴为x₄轴、y₄轴、z₄轴；以第一关节为坐标原点的空间三维坐标系中，机械手末端坐标(P_x，P_y，P_z)；

关节坐标轴的设置原则如下：经过设置各个关节所在坐标系的零点和各个坐标轴，使得上一个关节所在坐标系的x坐标轴与相邻的下一个坐标系的z坐标轴垂直并且相交；关节轴所在位置与关节所在坐标系的z轴一致；关节轴即为关节的旋转轴；

定义出关节所在处的坐标系相应于下一个相邻坐标系的参数如下：a_i是顺x_i-1坐标轴从z_i-1坐标轴到z_i坐标轴的空间距离，单位为m；α_i是绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度，单位为rad；d_i是顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离，单位为m；Θ_i是绕z_i坐标轴从x_i-1坐标轴到x_i坐标轴的角度，单位为rad；i＝1,2,3,4；

a₁＝0；α₁＝0；d₁＝0；

式(1)

a₂＝A₁；

d₂＝0；

式(2)

a₃＝A₂；α₃＝0；d₃＝0；

式(3)

a₄＝A₃；

d₃＝D₃；Θ₄＝0；式(4)

A₁为第一关节的旋转轴线与第二关节的旋转轴线之间的距离，单位为m；A₂为第二关节的旋转轴线与第三关节的旋转轴线之间的距离，单位为m；A₃为第三关节的旋转轴线与机械手末端之间的距离，单位为m；D₃为第三关节的x₃轴与机械手末端的x₄轴之间的距离，单位为m；

为第二关节的x₂轴与第一关节的x₁轴之间的角度，单位为rad；

为第三关节的x3轴与第二关节的x2轴之间的角度，单位为rad；θ₁为第一关节的沿z₁轴旋转角度，单位为rad；θ₂为第二关节的沿z₂轴旋转角度，单位为rad；θ₃为第三关节的沿z₃轴旋转角度，单位为rad；

因绕x_i-1坐标轴从z_i-1坐标轴到z坐标轴的角度α_i，实现相邻关节的转换用下列矩阵表示：

因顺x_i-1坐标轴从z_i-1坐标轴到z坐标轴的空间距离a_i，实现相邻关节的转换用下列矩阵表示：

因绕z_i坐标轴从x_i-1坐标轴到x坐标轴的角度Θ_i，实现相邻关节的转换用下列矩阵表示：

顺z_i坐标轴从x_i-1坐标轴到x_i坐标轴的空间距离d_i，实现相邻关节的转换用下列矩阵表示：

将式(5)、式(6)、式(7)、式(8)相乘，得出相邻关节四个参数转换的矩阵如下：

将式(1)、式(2)、式(3)、式(4)的数值带入式(9)，然后矩阵相乘，得到

其中

又因三连杆机械手末端用一个4x4矩阵进行如下表述，即

其中(P_x，P_y，P_z)则为机械手末端坐标，且

A＝T₁ 式(16)

两边均左乘⁰T₁的逆，得到

^oT₁ ^-1A＝^oT₁ ^-1T₁ 式(17)

其中

其中P_x1＝-P_xsinΘ₁-P_ycosΘ₁ 式(20)

P_y1＝-P_xcosΘ₁-P_ysinΘ₁ 式(21)

P_z1＝P_z 式(22)

接着，计算^oT₁ ^-1T₁：

从中得到，^oT₁ ^-1A与⁰T₁ ^-1T₁的第二行、第四列的元素相等且等于0，得到

由^oT₁ ^-1A与⁰T₁ ^-1T₁的第1行、第四列元素相等，以及第三行、第四列元素相等，得到，

其中

由式(24)、式(25)、式(27)得到机械手末端坐标与θ₁，θ₂，θ₃的关系；

当已知机械手末端的目标位置时，控制第一关节、第二关节和第三关节的旋转角度，使得θ₁，θ₂，θ₃到达对应的角度即可。

2.一种灵巧手定位控制系统，其特征在于：本系统包括角度传感器、控制器、存储器和执行机构；其中，角度传感器用于采集三连杆机械手各个关节的旋转角度，存储器中存有计算机程序供控制器调用，以完成权利要求1所述的灵巧手定位方法，并由执行机构执行。

3.根据权利要求2所述的系统，其特征在于：所述的执行机构包括控制第一关节旋转角度的第一电机、控制第二关节旋转角度的第二电机、以及控制第三关节旋转角度的第三电机；所述的控制器通过控制第一、第二、第三变频器从而控制对应的第一、第二、第三电机。

4.根据权利要求2所述的系统，其特征在于：所述的控制器为PLC控制器。

5.根据权利要求2所述的系统，其特征在于：所述的控制器还连接有监控中心计算机。

Images