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

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

Excavator Arm Motion Control Method, System and Device

The invention provides an arm motion control method, system and device, which relates to the technical field of automatic control, including acquiring the position information of the current target excavation point, the angle information between the excavation surface and the excavator base coordinate system, and the angle information between the excavation bucket section and the excavation surface; obtaining the desired position and position state of the excavator arm by solving the preset inverse kinematics model; and determining the excavator arm. The difference between the current position and the expected position is generated, and the difference signal is sent to the controller so that the controller can control the excavator arm to the desired position and attitude. The invention can realize the motion control of the excavator arm, realize the stable and efficient operation of the excavator arm, and reduce the labor cost.

【技术实现步骤摘要】
挖掘机臂运动控制方法、系统及装置
本专利技术涉及自动控制
，尤其是涉及一种挖掘机臂运动控制方法、系统及装置。
技术介绍
挖掘机是用挖斗挖掘高于或低于承机面的物料，并装入运输车辆或卸至堆料场的土方机械。挖掘机挖掘的物料主要是土壤、煤、泥沙以及经过预松后的土壤和岩石。挖掘机已广泛应用于矿山、建筑拆除、隧道挖掘等场景，由于大多数挖掘机臂都处于手动控制阶段，在一些高危作业场景，存在人员伤亡隐患，而目前，尚未提出更好的挖掘机臂的自主运动控制方案。
技术实现思路
有鉴于此，本专利技术的目的在于提供一种挖掘机臂运动控制方法、系统及装置，以控制挖掘机臂自主运动，减少人员成本。第一方面，本专利技术实施例提供了一种挖掘机臂运动控制方法，该方法包括：获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。结合第一方面，本专利技术实施例提供了第一方面的第一种可能的实施方式，该方法还包括：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。结合第一方面或其第一种可能的实施方式，本专利技术实施例提供了第一方面的第二种可能的实施方式，其中，通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态的步骤，包括：根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；该运动学模型为：其中，θ1表示动臂与基座标系横轴方向的夹角，θ2表示动臂与小臂的夹角，θ3表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l1表示挖掘机的动臂长度，l2表示挖掘机的小臂长度，l3表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据运动学模型确定逆运动学模型；通过逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：结合第一方面或其第一种可能的实施方式，本专利技术实施例提供了第一方面的第三种可能的实施方式，其中，将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态的步骤，包括：将位姿差值信号发送给控制器，以使控制器生成挖掘机臂的控制输入信号；通过控制输入信号控制比例电磁阀以使挖掘机臂运动到期望位姿状态；比例电磁阀设置在挖掘机臂的各个关节。结合第一方面或其第一种可能的实施方式，本专利技术实施例提供了第一方面的第四种可能的实施方式，其中，获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤，包括：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。第二方面，本专利技术实施例还提供一种挖掘机臂运动控制系统，该系统包括：期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块；期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态；期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，根据位姿差值生成挖掘机臂的位姿差值信号，并将位姿差值信号发送给控制器模块；控制器模块用于接收位姿差值信号，并将位姿差值信号转换为挖掘机臂的控制输入信号发送给信号转换模块；信号转换模块用于将所述控制输入信号转换为脉冲信号，通过脉冲信号控制比例电磁阀的开合状态以控制所述挖掘机臂运动到所述期望位姿状态；传感器模块用于实时获取挖掘机臂的运动位姿状态，并发送给误差计算模块；误差计算模块用于根据运动位姿状态和期望位姿状态获取位姿差值，并将位姿差值信号作为控制器输入信号发送给控制器模块，以控制挖掘机臂运动到期望位姿状态。第三方面，本专利技术实施例还提供一种挖掘机臂运动控制装置，该装置包括：信息获取模块，用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；解算模块，用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；差值模块，用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；发送模块，用于将位姿差值信号发送给控制器，以使控制器控制挖掘机臂运动到期望位姿状态。结合第三方面，本专利技术实施例提供了第三方面的第一种可能的实施方式，其中，发送模块，还用于：通过传感器模块监测挖掘机臂是否运动至期望位姿状态；如果是，向控制器发送停止运动指令；如果否，重新确定当前位姿状态与期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。结合第三方面或其第一种可能的实施方式，本专利技术实施例提供了第三方面的第二种可能的实施方式，其中，解算模块，还用于：根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；运动学模型为：其中，c1＝cosθ1,c12＝cos(θ1+θ2),c123＝cos(θ1+θ2+θ3)，s1＝sinθ1,s12＝sin(θ1+θ2),s123＝sin(θ1+θ2+θ3)，θ1表示动臂与基座标系横轴方向的夹角，θ2表示动臂与小臂的夹角，θ3表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l1表示挖掘机的动臂长度，l2表示挖掘机的小臂长度，l3表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据运动学模型确定逆运动学模型；通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态；逆运动学模型为：结合第三方面或其第一种可能的实施方式，本专利技术实施例提供了第三方面的第二种可能的实施方式，其中，信息获取模块，还用于：建立基座标系，基坐标系的横轴与挖掘机的履带共面，基坐标系的纵轴垂直于挖掘机的履带所在平面；根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。本专利技术实施例带来了以下有益效果：本专利技术实施例提供了一种挖掘机臂运动控制方法、系统及装置，该方法通过当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息及预设的运动学模型解算得到挖掘机臂的期望位姿状态，根据挖掘机臂当前位姿状态与期望位姿状态的位姿差值确定位姿差值信号，通过位姿差值信号控制挖掘机臂运动到期望位姿状态。本专利技术实施例可以实现对挖掘机臂的运动控制，以实现挖掘机臂的稳定、高效作业，降低人力成本。本公开的其他特征和优点将在随后的说明书中阐述，或者，部分特征和优点可以从说明书推知或毫无疑义地确定，或者通过实施本公开的上述技术即可得知。为使本公开的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如本文档来自技高网...

【技术保护点】
1.一种挖掘机臂运动控制方法，其特征在于，包括：获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态。

【技术特征摘要】
1.一种挖掘机臂运动控制方法，其特征在于，包括：获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息；通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态；确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值，并生成位姿差值信号；将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态。2.根据权利要求1所述的挖掘机臂运动控制方法，其特征在于，还包括：通过传感器模块监测所述挖掘机臂是否运动至所述期望位姿状态；如果是，向所述控制器发送停止运动指令；如果否，重新确定当前位姿状态与所述期望位姿状态的位姿差值，生成位姿差值信号，并将位姿差值信号作为控制器输入信号发送给所述控制器。3.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，所述通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态的步骤，包括：根据所述当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型；所述运动学模型为：其中，c1＝cosθ1,c12＝cos(θ1+θ2),c123＝cos(θ1+θ2+θ3)，s1＝sinθ1,s12＝sin(θ1+θ2),s123＝sin(θ1+θ2+θ3)，θ1表示动臂与基座标系横轴方向的夹角，θ2表示动臂与小臂的夹角，θ3表示小臂与挖斗的夹角，α表示挖斗齿尖处的角度值，β表示挖斗切面与挖掘面的夹角，γ表示挖掘面与挖掘机基座标系的夹角，l1表示挖掘机的动臂长度，l2表示挖掘机的小臂长度，l3表示挖掘机的挖斗长度，(x，y，z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标；根据所述运动学模型确定逆运动学模型；通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态；所述逆运动学模型为：4.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，将所述位姿差值信号发送给控制器，以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态的步骤，包括：将所述位姿差值信号发送给控制器，以使所述控制器生成所述挖掘机臂的控制输入信号；通过所述控制输入信号控制比例电磁阀以使所述挖掘机臂运动到所述期望位姿状态；所述比例电磁阀设置在所述挖掘机臂的各个关节。5.根据权利要求1或2所述的挖掘机臂运动控制方法，其特征在于，所述获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤，包括：建立基座标系，所述基坐标系的横轴与所述挖掘机的履带共面，所述基坐标系的纵轴垂直于所述挖掘机的履带所在平面；根据所述基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。6.一种挖掘机臂运动控制系统，其特征在于，包括：期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块；所述期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状...

【专利技术属性】
技术研发人员：卢兴阳，隋少龙，张越，
申请(专利权)人：北京拓疆者智能科技有限公司，
类型：发明
国别省市：北京,11