CN109296019A - 挖掘机臂运动控制方法、系统及装置 - Google Patents

Abstract

本发明提供了一种机臂运动控制方法、系统及装置，涉及自动控制的技术领域，包括获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。本发明可以实现对挖掘机臂的运动控制，以实现挖掘机臂的稳定、高效作业，降低人力成本。

Description

挖掘机臂运动控制方法、系统及装置

技术领域

本发明涉及自动控制技术领域，尤其是涉及一种挖掘机臂运动控制方法、系统及装置。

背景技术

挖掘机是用挖斗挖掘高于或低于承机面的物料，并装入运输车辆或卸至堆料场的土方机械。挖掘机挖掘的物料主要是土壤、煤、泥沙以及经过预松后的土壤和岩石。挖掘机已广泛应用于矿山、建筑拆除、隧道挖掘等场景，由于大多数挖掘机臂都处于手动控制阶段，在一些高危作业场景，存在人员伤亡隐患，而目前，尚未提出更好的挖掘机臂的自主运动控制方案。

发明内容

有鉴于此，本发明的目的在于提供一种挖掘机臂运动控制方法、系统及装置，以控制挖掘机臂自主运动，减少人员成本。

第一方面，本发明实施例提供了一种挖掘机臂运动控制方法，该方法包括：获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，该方法还包括：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

结合第一方面或其第一种可能的实施方式，本发明实施例提供了第一方面的第二种可能的实施方式，其中，通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态的步骤，包括：根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；该运动学模型为：其中，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据运动学模型确定逆运动学模型；通过逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：

结合第一方面或其第一种可能的实施方式，本发明实施例提供了第一方面的第三种可能的实施方式，其中，将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态的步骤，包括：将位姿差值信号发送给控制器，以使控制器生成挖掘机臂的控制输入信号；通过控制输入信号控制比例电磁阀以使挖掘机臂运动到期望位姿状态；比例电磁阀设置在挖掘机臂的各个关节。

结合第一方面或其第一种可能的实施方式，本发明实施例提供了第一方面的第四种可能的实施方式，其中，获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤，包括：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。

第二方面，本发明实施例还提供一种挖掘机臂运动控制系统，该系统包括：期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块；期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态；期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，根据位姿差值生成挖掘机臂的位姿差值信号，并将位姿差值信号发送给控制器模块；控制器模块用于接收位姿差值信号，并将位姿差值信号转换为挖掘机臂的控制输入信号发送给信号转换模块；信号转换模块用于将所述控制输入信号转换为脉冲信号，通过脉冲信号控制比例电磁阀的开合状态以控制所述挖掘机臂运动到所述期望位姿状态；传感器模块用于实时获取挖掘机臂的运动位姿状态，并发送给误差计算模块；误差计算模块用于根据运动位姿状态和期望位姿状态获取位姿差值，并将位姿差值信号作为控制器输入信号发送给控制器模块，以控制挖掘机臂运动到期望位姿状态。

第三方面，本发明实施例还提供一种挖掘机臂运动控制装置，该装置包括：信息获取模块，用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；解算模块，用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；差值模块，用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；发送模块，用于将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。

结合第三方面，本发明实施例提供了第三方面的第一种可能的实施方式，其中，发送模块，还用于：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

结合第三方面或其第一种可能的实施方式，本发明实施例提供了第三方面的第二种可能的实施方式，其中，解算模块，还用于：根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；运动学模型为：其中，c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据运动学模型确定逆运动学模型；通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：

结合第三方面或其第一种可能的实施方式，本发明实施例提供了第三方面的第二种可能的实施方式，其中，信息获取模块，还用于：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。

本发明实施例带来了以下有益效果：本发明实施例提供了一种挖掘机臂运动控制方法、系统及装置，该方法通过当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息及预设的运动学模型解算得到挖掘机臂的期望位姿状态，根据挖掘机臂当前位姿状态与期望位姿状态的位姿差值确定位姿差值信号，通过位姿差值信号控制挖掘机臂运动到期望位姿状态。本发明实施例可以实现对挖掘机臂的运动控制，以实现挖掘机臂的稳定、高效作业，降低人力成本。

本公开的其他特征和优点将在随后的说明书中阐述，或者，部分特征和优点可以从说明书推知或毫无疑义地确定，或者通过实施本公开的上述技术即可得知。

为使本公开的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的挖掘机臂运动控制方法流程图；

图2为本发明实施例提供的挖掘机臂运动控制方法工作流程示意图；

图3为本发明实施例提供的挖掘机臂运动控制方法中挖掘机臂二维几何示意图；

图4为本发明实施例提供的挖掘机臂运动控制系统的结构框图；

图5为本发明实施例提供的挖掘机臂运动控制装置结构框图。

图标：

1－动臂；2－小臂；3－挖斗；4－挖掘面；51－信息获取模块；52－解算模块；53－差值模块；54－发送模块。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

目前，随着机器人技术的发展，各式机器人已应用于制造业、公共服务等行业，在矿业、农业等工程机械领域也亟待利用前沿科技来改变现状。挖掘机已广泛应用于矿山、建筑拆除、隧道挖掘等场景，实现挖掘机的自动驾驶可降低人力成本，提高工程收益，在高危作业场景可以减少人员伤亡。挖掘机臂的自主运动控制是挖掘机无人驾驶非常关键的技术之一，挖掘机臂的自主运动控制技术尚不成熟，目前大部分挖掘机还是通过手动控制实现相应操作。由于挖掘机型号各异，需要挖掘的物体多种多样，且被挖掘面的倾斜角度也会因挖掘机作业场景不同而不同。

基于此，本发明实施例提供的一种挖掘机臂运动控制方法、系统以及装置，可以实现对挖掘机臂的运动控制，以实现挖掘机臂的稳定、高效作业，降低人力成本。

为便于对本实施例进行理解，首先对本发明实施例所公开的一种挖掘机臂运动控制方法进行详细介绍。

实施例1

本发明实施例1提供了一种挖掘机臂运动控制方法，参见图1所示的挖掘机臂运动控制方法流程图，该方法包括以下步骤：

步骤S102，获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；

当前目标挖掘点是指挖掘机将要挖掘的点，即挖掘机的挖斗齿尖与待挖掘面的接触点。基坐标系是基于挖掘机所处于的平面建立的坐标系。当前目标挖掘点的位置信息是指当前目标挖掘点在基座标系中的坐标信息。挖掘面是指挖掘机将挖掘的面，可以为斜坡的上表面。挖斗切面是指与挖斗的齿尖连接的面的切面。

步骤S104，通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；

逆运动学模型是一种数学模型，可以结合已知量求得未知量。期望位姿状态包括挖掘机要完成一项挖掘任务对应的挖掘机的动臂、小臂及挖斗所要执行的挖掘角度。通过当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息等已知量，根据逆运动学模型可以解算出挖掘机的动臂、小臂及挖斗所要执行的挖掘角度。

步骤S106，确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；

挖掘机臂当前位姿状态是指挖掘机要执行任务之前的位姿状态，包括动臂、小臂及挖斗所对应的弯曲角度。位姿差值包括动臂、小臂及挖斗所对应当前位姿状态与期望位姿状态之间的弯曲角度差值，将这个角度差值转换为位姿差值信号，位姿差值信号可以为电信号。

步骤S108，将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。

控制器可以采用比例－积分－微分控制器(Proportion IntegrationDifferentiation，PID)，可利用其他控制算法如自抗扰控制、滑模控制、智能控制等算法作为替代方案。控制器可以将位姿差值信号转换为控制信号以控制挖掘机臂的运动，使其达到期望位姿状态，最终完成对挖掘机臂运动的控制。

本发明实施例提供了一种挖掘机臂运动控制方法，该方法通过当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息及预设的运动学模型解算得到挖掘机臂的期望位姿状态，根据挖掘机臂当前位姿状态与期望位姿状态的位姿差值确定位姿差值信号，通过位姿差值信号控制挖掘机臂运动到期望位姿状态。本发明实施例可以实现对挖掘机臂的运动控制，以实现挖掘机臂的稳定、高效作业，降低人力成本。

考虑到控制器控制挖掘机臂运动过程，并不是瞬间就达到期望状态，因此，该方法还包括以下步骤：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

参见图2所示的挖掘机臂运动控制方法工作流程示意图，在初始化挖掘机臂运动学模型参数、获取目标点位姿信息并结合挖掘机臂期望位姿状态解算逆运动学模型，得到期望信号(期望信号即为挖掘机臂的期望位姿状态)以根据期望位姿状态与当前位姿状态的差值生成位姿差值信号控制挖掘机臂运动之后，还包括误差解算及判断是否达到期望位姿状态的过程。

该过程包括通过传感器模块监测挖掘机臂的运动状态，并根据传感器模块获取到的最新的运动状态信息与期望位姿状态进行误差解算，如果已经达到期望位姿状态，则向控制器发送停止运动指令；如果没有达到期望位姿状态，则根据挖掘机臂最新的运动状态与期望位姿状态的位姿差值确定控制器输入信号，通过控制器控制挖掘机臂继续运动，直至到达期望位姿状态。其中，控制器可以用PID控制器，传感器可以使用拉绳传感器或角度传感器等。

考虑到在实际的挖掘机臂运动控制中，往往知道目标物体的位置，即已知挖掘机挖斗齿尖的位置坐标及期望位姿，需要解算出每个连杆的偏转角度，即每个连杆的期望位姿状态。可以通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态。具体的步骤，包括：

(1)根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；该运动学模型为：其中，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标。

挖掘机臂包括动臂1、小臂2和挖斗3三部分，为了更好的对挖掘机运动形式进行分析，首先对其进行了运动学建模。参见图3所示的挖掘机臂二维几何示意图，其中各坐标系z轴的方向与各连杆伸缩方向平行，垂直于(x，y)平面。

挖掘机工作装置运动学主要研究的是各关节变量与挖斗末端之间。首先分别求出各个连杆机构变换矩阵：

综上可得挖斗坐标系到基础坐标系的转换矩阵：

其中：c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)。

挖斗齿尖在坐标系(x₁,y₁)下的位置坐标为(x,y,z)，其中，α表示挖斗的角度值，β表示挖斗切面与被挖物体所在平面的角度，γ表示所挖平面与基座标系的夹角，则：

上式为挖掘机臂的正运动学模型，根据上式当得知每个连杆的偏转角度后，即可得到挖斗齿尖的位置。但是在实际的挖掘机臂运动控制中，往往知道目标物体的位置，即已知挖掘机挖斗齿尖的位置坐标及期望位姿(x,y,z,θ)，需要逆解算出每个连杆的偏转角度，因此需要进一步的逆运动学分析。

(2)根据运动学模型确定逆运动学模型；通过逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：

运动学逆问题指由位姿空间变量向关节空间变量的转换，相对于挖掘机臂而言，即给定挖斗相对于参考坐标系的期望位姿(x,y,z,θ)，求挖掘机能否使其挖斗达到这个预期的位姿，并给出每一个连杆达到这一位姿的偏转角度。

由于c_θ＝c₁₂₃，θ＝θ₁+θ₂+θ₃为已知量，所以s₁₂₃,c₁₂₃已知，由(5)式可得：

其中x′、y′为可求出的已知量。

θ₂＝arccosK (9)

由(9)式可以得到θ₂的期望值，结合式(6)、(9)可得：

综上逆运动学模型为：

为了实现对挖掘机臂的柔性控制，需要对挖掘机臂的各部分进行控制，将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态的步骤，包括：将位姿差值信号发送给控制器，以使控制器生成挖掘机臂的控制输入信号；通过控制输入信号控制比例电磁阀以使挖掘机臂运动到期望位姿状态；比例电磁阀设置在挖掘机臂的各个关节。

可以将比例电磁阀安装在挖掘机臂的各关节处，位姿差值信号通过控制器可以转换为控制输入信号，控制输入信号通过信号转换模块可以转换为脉冲宽度调制(PulseWidthModulation，PWM)信号，PWM信号可以控制比例电磁阀，从而实现对挖掘机臂的各个关节的控制，以使挖掘机臂运动到期望位姿状态。

考虑到要对位姿信息进行准确的描述，可以通过建立坐标系实现。因此，获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤，包括：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。

参见图3所示的挖掘机臂运动控制方法中挖掘机臂二维几何示意图，可以以动臂和挖掘机车身的连接处为原点，以挖掘机履带所在平面为横轴，与履带垂直的方向建立纵轴，建立关于x₁，y₁，z的基座标系，基于基座标系及动臂1、小臂2、挖斗3来表示当前目标挖掘点的位置信息，基于目标挖掘点及挖斗的角度来表示挖掘面4与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面4的夹角信息。

本发明实施例提供了一种挖掘机臂运动控制方法、系统及装置，建立了挖掘机臂的运动学模型，适用于多种不同型号的挖掘机臂，可以根据挖掘面与挖掘机基坐标系的夹角，确定挖斗的挖掘角度；通过PWM信号转换传输及传感器的信号的实时反馈，让挖掘机在具备原有液压系统的前提下实现挖掘机臂的复合柔顺控制，提高挖掘机的工作效率。

实施例2

本发明实施例2提供了一种挖掘机臂运动控制系统，参见图4所示的挖掘机臂运动控制系统的结构框图，该系统包括：

期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块；期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态；期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，根据位姿差值生成挖掘机臂的位姿差值信号，并将位姿差值信号发送给控制器模块；控制器模块用于接收位姿差值信号，并将位姿差值信号转换为挖掘机臂的控制输入信号发送给信号转换模块；信号转换模块用于将所述控制输入信号转换为脉冲信号，通过脉冲信号控制比例电磁阀的开合状态以控制所述挖掘机臂运动到所述期望位姿状态；传感器模块用于实时获取挖掘机臂的运动位姿状态，并发送给误差计算模块；误差计算模块用于根据运动位姿状态和期望位姿状态获取位姿差值，并将位姿差值信号作为控制器输入信号发送给控制器模块，以控制挖掘机臂运动到期望位姿状态；

实施例3

本发明实施例3提供了一种挖掘机臂运动控制装置，参见图5所示的挖掘机臂运动控制装置结构框图，该装置包括：

信息获取模块51，用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；解算模块52，用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；差值模块53，用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；发送模块54，用于将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。

其中，发送模块54，还用于：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与所述期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

其中，解算模块52，还用于：根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；运动学模型为：其中，c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据运动学模型确定逆运动学模型；通过逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：

其中，信息获取模块51，还用于：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。

本发明实施例所提供的挖掘机臂运动控制装置，其实现原理及产生的技术效果和前述挖掘机臂运动控制方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的挖掘机臂运动控制系统和挖掘机臂运动控制装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种挖掘机臂运动控制方法，其特征在于，包括：

获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；

通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；

确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；

将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态。

2.根据权利要求1所述的挖掘机臂运动控制方法，其特征在于，还包括：

通过传感器模块监测所述挖掘机臂是否运动至所述期望位姿状态；

如果是，向所述控制器发送停止运动指令；

如果否，重新确定当前位姿状态与所述期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

3.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，所述通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态的步骤，包括：

根据所述当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；所述运动学模型为：

其中，c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；

根据所述运动学模型确定逆运动学模型；通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态；所述逆运动学模型为：

4.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态的步骤，包括：

将所述位姿差值信号发送给控制器，以使所述控制器生成所述挖掘机臂的控制输入信号；

通过所述控制输入信号控制比例电磁阀以使所述挖掘机臂运动到所述期望位姿状态；所述比例电磁阀设置在所述挖掘机臂的各个关节。

5.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，所述获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤，包括：

建立基座标系，所述基坐标系的横轴与所述挖掘机的履带共面，所述基坐标系的纵轴垂直于所述挖掘机的履带所在平面；

根据所述基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。

6.一种挖掘机臂运动控制系统，其特征在于，包括：期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块；

所述期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态；

所述期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，根据所述位姿差值生成所述挖掘机臂的位姿差值信号，并将所述位姿差值信号发送给所述控制器模块；

所述控制器模块用于接收所述位姿差值信号，并将所述位姿差值信号转换为挖掘机臂的控制输入信号发送给所述信号转换模块；

所述信号转换模块用于将所述控制输入信号转换为脉冲信号，通过脉冲信号控制比例电磁阀的开合状态，以控制所述挖掘机臂运动到所述期望位姿状态；

所述传感器模块用于实时获取所述挖掘机臂的运动位姿状态，并发送给所述误差计算模块；

所述误差计算模块用于根据所述运动位姿状态和所述期望位姿状态获取位姿差值，并将位姿差值信号作为控制器输入信号发送给控制器模块，以控制所述挖掘机臂运动到所述期望位姿状态。

7.一种挖掘机臂运动控制装置，其特征在于，包括：

信息获取模块，用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；

解算模块，用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；

差值模块，用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；

发送模块，用于将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态。

8.根据权利要求7所述的挖掘机臂运动控制装置，其特征在于，其中，所述发送模块，还用于：

通过传感器模块监测所述挖掘机臂是否运动至所述期望位姿状态；

如果是，向所述控制器发送停止运动指令；

如果否，重新确定当前位姿状态与所述期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。

9.根据权利要求7或8所述的挖掘机臂运动控制装置，其特征在于，所述解算模块，还用于：

根据所述当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；所述运动学模型为：

其中，c₁＝cosθ₁,c₁₂＝cos(θ₁+θ₂),c₁₂₃＝cos(θ₁+θ₂+θ₃)，s₁＝sinθ₁,s₁₂＝sin(θ₁+θ₂),s₁₂₃＝sin(θ₁+θ₂+θ₃)，θ₁表示动臂与基座标系横轴方向的夹角，θ₂表示动臂与小臂的夹角，θ₃表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l₁表示挖掘机的动臂长度，l₂表示挖掘机的小臂长度，l₃表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；

根据所述运动学模型确定逆运动学模型；通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态；所述逆运动学模型为：

10.根据权利要求7或8所述的挖掘机臂运动控制装置，其特征在于，所述信息获取模块，还用于：

建立基座标系，所述基坐标系的横轴与所述挖掘机的履带共面，所述基坐标系的纵轴垂直于所述挖掘机的履带所在平面；

根据所述基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。