CN101633168B - 一种大型工程机械手的控制方法及控制系统 - Google Patents

Abstract

本发明涉及一种大型工程机械手的控制方法，所述机械手的各节臂架上均设有驱动机构，所述驱动机构可驱动臂架在设定角度范围内转动，该方法包括，采集所述驱动机构的当前位移量，并获取控制指令，依据所述当前位移量和所述控制指令计算驱动机构的所需位移量，按所需位移量控制所述驱动机构，驱动所述机械手动作。本发明还提供一种大型工程机械手的控制系统。本发明通过采集驱动机构的当前位移量，并依据该当前位移量、机械手的结构特征、及控制指令实现对机械手的精确控制。

Description

一种大型工程机械手的控制方法及控制系统

技术领域

本发明涉及大型工程机械领域，特别是涉及一种大型工程机械手的控制方法及控制系统。

背景技术

许多大型工程机械，特别是具有多节臂架或折弯结构机械臂的工程机械，例如混凝土泵车、起重机、挖掘机等，需要对机械手进行精确控制，满足作业要求。参见图1，示出现有的大型工程机械手的控制系统，包括指令给定单元11、控制单元12、及测量单元13。

指令给定单元11获取控制者的控制指令，将控制指令传送给控制单元12。该控制指令可为该大型机械的机械手需转动的角度，需到达位置等。

控制单元12接收控制指令，通过一定的控制算法，得出控制量，传送到驱动机构，驱动机构根据该控制量驱动机械手动作。

测量单元13是对机械手的相关物理量进行采集，并作为反馈量提供给控制单元12参与控制算法，以提高控制精度。

通常大型工程机械手的驱动机构为液压油缸，需控制的直接物理量多为液压油缸的位移量。测量单元13需先检测安装液压油缸的臂架的转动角度，传送给控制单元12。控制单元12依据臂架的转动角度计算执行机构的位移量，再依据执行机构的位移量和控制指令，计算相关控制量。

见图2，示出一大型工程机械手的臂架和液压油缸的结构关系，在臂架21和臂架22的夹角处设置旋转编码器，采集臂架21和臂架22之间夹角234的转动角度，再依据转动角度计算液压油缸23的位移量。由图2可知，液压油缸23的位移量为点1到点5之间的距离。

计算算式为：

d24＝sqrt(d23*d23+d34*d34-2*d23*d34*cos(ang234))；    式1

ang324＝acos((d24*d24+d23*d23-d34*d34)/(2*d24*d23))； 式2

ang325＝acos((d24*d24+d25*d25-d54*d54)/(2*d24*d25))+ang324； 式3

ang125＝ang123-ang325；                             式4

d15＝sqrt(d12*d12+d25*d25-2*d12*d25*cos(ang125))；  式5

其中，d表示两点间的距离，例如d24表示点2到点4之间的距离，例如d34表示点3到点4之间的距离；ang表示角度，例如ang324表示点3到点2的线段与点2到点4的线段之间的夹角；例如ang125表示点1到点2的线段与点2到点5的线段之间的夹角。Sqrt表示开根号。

该方式虽然可以得到液压油缸23的位移量，但是，旋转编码器的检测精度受温度影响较大，当该驱动机构的温度变化较大时，旋转编码器检测的夹角234有较大的误差，使计算的液压油缸的位移量误差较大，导致控制机构很难对机械手进行精确控制。

发明内容

本发明所要解决的技术问题是提供一种大型工程机械手的控制方法，该控制方法可实现对机械手的精确控制。

本发明的另一目的是提供一种大型工程机械手的控制系统，该控制系统可实现对机械手的精确控制。

本发明涉及一种大型工程机械手的控制方法，所述机械手的各节臂架上均设有驱动机构，所述驱动机构可驱动臂架在设定角度范围内转动，其特征在于，该方法包括：采集所述驱动机构的当前位移量，并获取控制指令；依据所述当前位移量和所述控制指令计算驱动机构的所需位移量；按所需位移量控制所述驱动机构，驱动所述机械手动作。

优选的，所述驱动机构为液压油缸，在所述液压油缸内部设置位移传感器，位移传感器采集所述液压油缸的当前位移量。

优选的，依据所述当前位移量和所述控制指令计算驱动机构的所需位移量步骤为：将所述当前位移量转换为机械臂的当前转动角度；在所述控制指令中提取机械臂要到达位置的坐标数据，结合当前转动角度，计算该坐标数据对应的机械臂的所需转动角度；将机械臂的所需转动角度转换为驱动机构的所需位移量。

本发明还公开一种大型工程机械手的控制系统，所述机械手的各节臂架上均设有驱动机构，所述驱动机构可驱动臂架在设定角度范围内转动，其特征在于，包括位移量采集单元、控制指令获取单元、控制量计算单元、及控制驱动单元：所述位移量采集单元，用于采集驱动机构的当前位移量；所述控制指令获取单元，用于获取控制指令；所述控制量计算单元，用于依据所述当前位移量和所述控制指令计算驱动机构的所需位移量；所述控制驱动单元，用于按所需位移量控制所述驱动机构，驱动所述机械手动作。

优选的，所述控制量计算单元包括当前转动角度计算子单元、所需转动角度计算子单元、及所需位移量计算子单元：所述当前转动角度计算子单元，用于将所述当前位移量转换为机械臂的当前转动角度；所述所需转动角度计算子单元，用于在所述控制指令中提取机械臂要到达位置的坐标数据，结合当前转动角度，计算该坐标数据对应的机械臂的所需转动角度；所述所需位移量计算子单元，用于将机械臂的所需转动角度转换为驱动机构的所需位移量。

优选的，所述驱动机构为液压油缸，在所述液压油缸内部设置位移传感器，位移传感器采集所述液压油缸的当前位移量。

与现有技术相比，本发明具有以下优点：

本发明通过采集驱动机构的当前位移量，并依据该当前位移量、机械手的结构特征、及控制指令实现对机械手的精确控制。相对于编码传感器，位移传感器受温度影响较小，当外界温度变化较大时，对位移传感器的检测精度影响较少，检测的当前位移量误差较小，因此可实现对机械手的精确控制。本发明位移传感器设置液压油缸内部，不但可提高系统的可靠性及集成化程度，而且实现了对液压油缸的闭环控制，提高系统控制性能。

附图说明

图1为现有的大型工程机械手的控制系统示意图；

图2为现有大型工程机械手的臂架和液压油缸的结构示意图；

图3为本发明大型工程机械手的控制方法第一实施例流程图；

图4为本发明二轴机械臂的机械手结构示意图；

图5为本发明大型工程机械手的控制系统第一实施例流程图；

图6为本发明大型工程机械手的控制系统第二实施例流程图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明采集机械手驱动机构的当前位移量，利用当前位移量，结合控制指令中的机械手要到达的位置信息，计算机械手需转动的角度，再根据机械手需转动的角度计算机械手驱动机构的需调整位移量，依据需调整位移量控制驱动机构，驱动机械手动作，到达指定位置，实现对机械手的精确控制。

参见图3，本发明大型工程机械手的控制方法第一实施例，具体步骤如下。

步骤S301、采集机械手驱动机构的当前位移量。大型工程机械手的驱动机构多为液压油缸，本发明在液压油缸内部设置位移传感器，采集驱动机构的当前位移量。

步骤S302、获取控制指令。获取操控者输入的控制指令，控制指令可为机械手要到达的空间位置等。例如从A(X，Y)点移动到A’(X’，Y’)。

步骤S303、依据当前位移量和控制指令计算驱动机构的所需位移量。通过当前位移量，结合机械手的机械结构计算机械手的当前转动角度；再根据控制指令中的机械手要到达的位置信息，结合机械手的机械机构，计算机械手的驱动机构的所需转动角度，再将所需转动角度转换为所需位移量。

步骤S304、按所需位移量控制驱动机构，驱动机械手动作，到达制定位置，实现对机械手的精确控制。

本发明通过采集驱动机构的当前位移量，并依据该当前位移量、机械手的结构特征、及控制指令实现对机械手的精确控制。相对于旋转编码器，位移传感器受温度影响较小，检测精度较高，检测的误差较小，可实现对机械手的精确控制。

本发明位移传感器设置液压油缸内部，不但可提高系统的可靠性及集成化程度，而且实现了对液压油缸的闭环控制，提高系统控制性能。

本发明机械手可以是多节臂架的机械手，也可以是二轴机械臂或单轴机械手，现以二轴机械臂为例，说明通过采集机械臂驱动机构的位移量，实现对机械臂的精确控制。

参见图4，示出二轴机械臂的机械手示意图，以对二轴机械臂的控制为例，进行说明。

二轴机械臂可分别绕关节O₁和O₂旋转，已知O₁到O₂长度为L1，O₂到A点长度为L2，设关节O₁和O₂的转角分别为Q1、Q2，端点为A(x、y)，由几何分析法，可建立机械臂的运动方程：

x＝l₁cosθ₁+l₂cos(θ₁+θ₂)    式6

y＝l₁sinθ₁+l₂sin(θ₁+θ₂)    式7

本发明通过位移传感器分别采集二轴机械臂的两个机械臂驱动机构的当前位移量，驱动机构与两个机械臂的结构关系如图2所示，利用驱动机构的当前位移量，计算两个机械臂的当前转动角度，计算算式为：

ang125＝acos((d12^2+d25^2-d15^2)/(2*d12*d25))；   式8

ang325＝ang123-ang125；                           式9

d35＝sqrt(d23*d23+d25*d25-2*d23*d25*cos(ang325))；式10

ang235＝acos((d23^2+d35^2-d25^2)/(2*d23*d35))；   式11

ang435＝acos((d35^2+d34^2-d45^2)/(2*d35*d34))；   式12

ang234＝(ang235+ang435)*180/pi；                  式13

其中，d表示两点间的距离，例如d24表示点2到点4之间的距离，例如d34表示点3到点4之间的距离；ang表示角度，例如ang324表示点3到点2的线段与点2到点4的线段之间的夹角；例如ang125表示点1到点2的线段与点2到点5的线段之间的夹角；pi表示π。当d15为L1时，ang234即为图4中的Q1；当d15为L2时，ang234即为图4中的Q2。

在控制指令中，获取二轴机械臂要到达位置A’(x’、y’)，设二轴机械臂的所需转动角度分别为Q’1、Q’2，将式6和式7变形，得到：

$x^{\′2}+y^{\′2}=l_1^2+l_2^2+2l_1{l}_2[\cos {{\mathrm{\θ}}^{\′}}_1\cos ({{\mathrm{\θ}}^{\′}}_1+{{\mathrm{\θ}}^{\′}}_2)+\sin {{\mathrm{\θ}}^{\′}}_1\sin ({{\mathrm{\θ}}^{\′}}_1+{{\mathrm{\θ}}^{\′}}_2)]$

$=l_1^2+l_2^2+2l_1{l}_2\cos {\mathrm{\θ}}_2$

${{\mathrm{\θ}}^{\′}}_1=\arctan \left(\frac{y^{\′}}{x^{\′}}\right)-\arccos \left(\frac{x^{\′2}+y^{\′2}+l_1^2-l_2^2}{2l_1\sqrt{x^{\′2}+y^{\′2}}}\right)$

${{\mathrm{\θ}}^{\′}}_2=\arccos \left(\frac{x^{\′2}+y^{\′2}-l_1^2-l_2^2}{2l_1{l}_2}\right)$

利用上述算式1到算式5，计算Q’1-Q1，Q’2-Q2的值，并转换为驱动机构的所需位移量D1、D2，分别控制两个臂架的驱动机构执行所需的位移量，两个驱动机构分别带动二轴机械臂运动，将机械臂从A(X，Y)点到A’(X’，Y’)的运动。驱动机构在执行过程中，不断采集驱动机构的当前位移量，重复上述计算过程，实现对驱动机构的闭环控制，提高机械手的控制精度。

上述实施例仅以机械手在二维空间运动为例介绍本发明的控制原理，同理，本发明在控制指令中可给出机械手在三维空间的运动位置，例如A’(x’、y’、z’)，x’、y’、z’表示在机械手在三维空间要到达位置坐标，分别建立x’、y’、z’与机械臂转动角度的对应关系式，即可通过上述控制原理对控制机械臂在三维空间内任意运动。

如果机械臂为多节臂架，则需对每节臂架的驱动机构都进行相应的控制，使多节臂架协调工作，最终实现对终端臂架移动的精确控制。

基于上述大型工程机械手的控制方法，本发明还提供一种大型工程机械手的控制系统。参见图5，示出大型工程机械手的控制系统第一实施例，机械手的各节臂架上均设有驱动机构，驱动机构可驱动臂架在设定角度范围内转动。该系统包括位移量采集单元51、控制指令获取单元52、控制量计算单元53、及控制驱动单元54。

位移量采集单元51采集机械手驱动机构的当前位移量，并将当前位移量发送到控制量计算单元53。驱动机构为液压油缸时，位移量采集单元51为设置在液压油缸内部的位移传感器。

控制指令获取单元52获取控制指令，并将控制指令发送到控制量计算单元53。

控制量计算单元53依据当前位移量和控制指令计算驱动机构的所需位移量，并将所需位移量发送到控制驱动单元54。

控制驱动单元54按所需位移量控制驱动机构，驱动机械手动作。

参见图6，示出大型工程机械手的控制系统第二实施例，该系统包括位移量采集单元51、控制指令获取单元52、控制量计算单元53、及控制驱动单元54，其中，控制量计算单元53包括当前转动角度计算子单元531、所需转动角度计算子单元532、及所需位移量计算子单元533。

当前转动角度计算子单元531将当前位移量转换为机械臂的当前转动角度，并将当前转动角度发送到所需转动角度计算子单元532。

所需转动角度计算子单元532在控制指令中提取机械臂要到达位置的坐标数据，结合当前转动角度，计算该坐标数据对应的机械臂的所需转动角度，并将该所需转动角度发送到所需位移量计算子单元533。

所需位移量计算子单元533将机械臂的所需转动角度转换为驱动机构的所需位移量。

位移量采集单元51、控制指令获取单元52、及控制驱动单元54在该实施例中的功能和作用与图5所示实施例相同，不再赘述。

以上对本发明所提供的一种大型工程机械手的控制方法及控制系统，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (2)

1.一种大型工程机械手的控制方法，所述机械手的各节臂架上均设有驱动机构，所述驱动机构可驱动臂架在设定角度范围内转动，其特征在于，该方法包括：

采集所述驱动机构的当前位移量，并获取控制指令；

将所述当前位移量转换为机械臂的当前转动角度；

在所述控制指令中提取机械臂要到达位置的坐标数据，结合当前转动角度，计算该坐标数据对应的机械臂的所需转动角度；

将机械臂的所需转动角度转换为驱动机构的所需位移量；

按所需位移量控制所述驱动机构，驱动所述机械手动作；

所述驱动机构为液压油缸，在所述液压油缸内部设置位移传感器，位移传感器采集所述液压油缸的当前位移量。

2.一种大型工程机械手的控制系统，所述机械手的各节臂架上均设有驱动机构，所述驱动机构可驱动臂架在设定角度范围内转动，其特征在于，包括位移量采集单元、控制指令获取单元、控制量计算单元、及控制驱动单元：

所述位移量采集单元，用于采集驱动机构的当前位移量；

所述控制指令获取单元，用于获取控制指令；

所述控制量计算单元，用于依据所述当前位移量和所述控制指令计算驱动机构的所需位移量；其中，所述控制量计算单元包括：当前转动角度计算子单元，用于将所述当前位移量转换为机械臂的当前转动角度；所需转动角度计算子单元，用于在所述控制指令中提取机械臂要到达位置的坐标数据，结合当前转动角度，计算该坐标数据对应的机械臂的所需转动角度；所需位移量计算子单元，用于将机械臂的所需转动角度转换为驱动机构的所需位移量；

所述控制驱动单元，用于按所需位移量控制所述驱动机构，驱动所述机械手动作；

所述驱动机构为液压油缸，在所述液压油缸内部设置位移传感器，位移传感器采集所述液压油缸的当前位移量。

Images