CN109296019A - 挖掘机臂运动控制方法、系统及装置 - Google Patents
挖掘机臂运动控制方法、系统及装置 Download PDFInfo
- Publication number
- CN109296019A CN109296019A CN201811328944.6A CN201811328944A CN109296019A CN 109296019 A CN109296019 A CN 109296019A CN 201811328944 A CN201811328944 A CN 201811328944A CN 109296019 A CN109296019 A CN 109296019A
- Authority
- CN
- China
- Prior art keywords
- excavator
- pose
- coordinate system
- arm
- difference value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 72
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000009412 basement excavation Methods 0.000 claims abstract description 91
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000012544 monitoring process Methods 0.000 claims description 6
- 210000000245 forearm Anatomy 0.000 claims description 5
- 230000011664 signaling Effects 0.000 claims description 3
- 230000005540 biological transmission Effects 0.000 claims description 2
- 230000006641 stabilisation Effects 0.000 abstract 1
- 238000011105 stabilization Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 239000000463 material Substances 0.000 description 4
- 238000005065 mining Methods 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 239000002689 soil Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003245 coal Substances 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000011435 rock Substances 0.000 description 1
- -1 silt Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/14—Booms only for booms with cable suspension arrangements; Cable suspensions
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/20—Drives; Control devices
- E02F9/2025—Particular purposes of control systems not otherwise provided for
- E02F9/2037—Coordinating the movements of the implement and of the frame
Landscapes
- Engineering & Computer Science (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Operation Control Of Excavators (AREA)
Abstract
本发明提供了一种机臂运动控制方法、系统及装置,涉及自动控制的技术领域,包括获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息;通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态;确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,并生成位姿差值信号;将位姿差值信号发送给控制器,以使控制器控制挖掘机臂运动到期望位姿状态。本发明可以实现对挖掘机臂的运动控制,以实现挖掘机臂的稳定、高效作业,降低人力成本。
Description
技术领域
本发明涉及自动控制技术领域,尤其是涉及一种挖掘机臂运动控制方法、系统及装置。
背景技术
挖掘机是用挖斗挖掘高于或低于承机面的物料,并装入运输车辆或卸至堆料场的土方机械。挖掘机挖掘的物料主要是土壤、煤、泥沙以及经过预松后的土壤和岩石。挖掘机已广泛应用于矿山、建筑拆除、隧道挖掘等场景,由于大多数挖掘机臂都处于手动控制阶段,在一些高危作业场景,存在人员伤亡隐患,而目前,尚未提出更好的挖掘机臂的自主运动控制方案。
发明内容
有鉴于此,本发明的目的在于提供一种挖掘机臂运动控制方法、系统及装置,以控制挖掘机臂自主运动,减少人员成本。
第一方面,本发明实施例提供了一种挖掘机臂运动控制方法,该方法包括:获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息;通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态;确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,并生成位姿差值信号;将位姿差值信号发送给控制器,以使控制器控制挖掘机臂运动到期望位姿状态。
结合第一方面,本发明实施例提供了第一方面的第一种可能的实施方式,该方法还包括:通过传感器模块监测挖掘机臂是否运动至期望位姿状态;如果是,向控制器发送停止运动指令;如果否,重新确定当前位姿状态与期望位姿状态的位姿差值,生成位姿差值信号,并将位姿差值信号作为控制器输入信号发送给所述控制器。
结合第一方面或其第一种可能的实施方式,本发明实施例提供了第一方面的第二种可能的实施方式,其中,通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态的步骤,包括:根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型;该运动学模型为:其中,θ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为本发明实施例提供的挖掘机臂运动控制方法流程图;
图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)根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型;该运动学模型为:其中,θ1表示动臂与基座标系横轴方向的夹角,θ2表示动臂与小臂的夹角,θ3表示小臂与挖斗的夹角,α表示挖斗齿尖处的角度值,β表示挖斗切面与挖掘面的夹角,γ表示挖掘面与挖掘机基座标系的夹角,l1表示挖掘机的动臂长度,l2表示挖掘机的小臂长度,l3表示挖掘机的挖斗长度,(x,y,z)为挖斗的齿尖在挖掘机基坐标系下的位置坐标。
挖掘机臂包括动臂1、小臂2和挖斗3三部分,为了更好的对挖掘机运动形式进行分析,首先对其进行了运动学建模。参见图3所示的挖掘机臂二维几何示意图,其中各坐标系z轴的方向与各连杆伸缩方向平行,垂直于(x,y)平面。
挖掘机工作装置运动学主要研究的是各关节变量与挖斗末端之间。首先分别求出各个连杆机构变换矩阵:
综上可得挖斗坐标系到基础坐标系的转换矩阵:
其中:c1=cosθ1,c12=cos(θ1+θ2),c123=cos(θ1+θ2+θ3),s1=sinθ1,s12=sin(θ1+θ2),s123=sin(θ1+θ2+θ3)。
挖斗齿尖在坐标系(x1,y1)下的位置坐标为(x,y,z),其中,α表示挖斗的角度值,β表示挖斗切面与被挖物体所在平面的角度,γ表示所挖平面与基座标系的夹角,则:
上式为挖掘机臂的正运动学模型,根据上式当得知每个连杆的偏转角度后,即可得到挖斗齿尖的位置。但是在实际的挖掘机臂运动控制中,往往知道目标物体的位置,即已知挖掘机挖斗齿尖的位置坐标及期望位姿(x,y,z,θ),需要逆解算出每个连杆的偏转角度,因此需要进一步的逆运动学分析。
(2)根据运动学模型确定逆运动学模型;通过逆运动学模型解算得到挖掘机臂的期望位姿状态;逆运动学模型为:
运动学逆问题指由位姿空间变量向关节空间变量的转换,相对于挖掘机臂而言,即给定挖斗相对于参考坐标系的期望位姿(x,y,z,θ),求挖掘机能否使其挖斗达到这个预期的位姿,并给出每一个连杆达到这一位姿的偏转角度。
由于cθ=c123,θ=θ1+θ2+θ3为已知量,所以s123,c123已知,由(5)式可得:
其中x′、y′为可求出的已知量。
θ2=arccosK (9)
由(9)式可以得到θ2的期望值,结合式(6)、(9)可得:
综上逆运动学模型为:
为了实现对挖掘机臂的柔性控制,需要对挖掘机臂的各部分进行控制,将位姿差值信号发送给控制器,以使控制器控制挖掘机臂运动到期望位姿状态的步骤,包括:将位姿差值信号发送给控制器,以使控制器生成挖掘机臂的控制输入信号;通过控制输入信号控制比例电磁阀以使挖掘机臂运动到期望位姿状态;比例电磁阀设置在挖掘机臂的各个关节。
可以将比例电磁阀安装在挖掘机臂的各关节处,位姿差值信号通过控制器可以转换为控制输入信号,控制输入信号通过信号转换模块可以转换为脉冲宽度调制(PulseWidthModulation,PWM)信号,PWM信号可以控制比例电磁阀,从而实现对挖掘机臂的各个关节的控制,以使挖掘机臂运动到期望位姿状态。
考虑到要对位姿信息进行准确的描述,可以通过建立坐标系实现。因此,获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息的步骤,包括:建立基座标系,基坐标系的横轴与挖掘机的履带共面,基坐标系的纵轴垂直于挖掘机的履带所在平面;根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。
参见图3所示的挖掘机臂运动控制方法中挖掘机臂二维几何示意图,可以以动臂和挖掘机车身的连接处为原点,以挖掘机履带所在平面为横轴,与履带垂直的方向建立纵轴,建立关于x1,y1,z的基座标系,基于基座标系及动臂1、小臂2、挖斗3来表示当前目标挖掘点的位置信息,基于目标挖掘点及挖斗的角度来表示挖掘面4与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面4的夹角信息。
本发明实施例提供了一种挖掘机臂运动控制方法、系统及装置,建立了挖掘机臂的运动学模型,适用于多种不同型号的挖掘机臂,可以根据挖掘面与挖掘机基坐标系的夹角,确定挖斗的挖掘角度;通过PWM信号转换传输及传感器的信号的实时反馈,让挖掘机在具备原有液压系统的前提下实现挖掘机臂的复合柔顺控制,提高挖掘机的工作效率。
实施例2
本发明实施例2提供了一种挖掘机臂运动控制系统,参见图4所示的挖掘机臂运动控制系统的结构框图,该系统包括:
期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块;期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态;期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,根据位姿差值生成挖掘机臂的位姿差值信号,并将位姿差值信号发送给控制器模块;控制器模块用于接收位姿差值信号,并将位姿差值信号转换为挖掘机臂的控制输入信号发送给信号转换模块;信号转换模块用于将所述控制输入信号转换为脉冲信号,通过脉冲信号控制比例电磁阀的开合状态以控制所述挖掘机臂运动到所述期望位姿状态;传感器模块用于实时获取挖掘机臂的运动位姿状态,并发送给误差计算模块;误差计算模块用于根据运动位姿状态和期望位姿状态获取位姿差值,并将位姿差值信号作为控制器输入信号发送给控制器模块,以控制挖掘机臂运动到期望位姿状态;
实施例3
本发明实施例3提供了一种挖掘机臂运动控制装置,参见图5所示的挖掘机臂运动控制装置结构框图,该装置包括:
信息获取模块51,用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息;解算模块52,用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态;差值模块53,用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,并生成位姿差值信号;发送模块54,用于将位姿差值信号发送给控制器,以使控制器控制挖掘机臂运动到期望位姿状态。
其中,发送模块54,还用于:通过传感器模块监测挖掘机臂是否运动至期望位姿状态;如果是,向控制器发送停止运动指令;如果否,重新确定当前位姿状态与所述期望位姿状态的位姿差值,生成位姿差值信号,并将位姿差值信号作为控制器输入信号发送给所述控制器。
其中,解算模块52,还用于:根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型;运动学模型为:其中,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)为挖斗的齿尖在挖掘机基坐标系下的位置坐标;根据运动学模型确定逆运动学模型;通过逆运动学模型解算得到挖掘机臂的期望位姿状态;逆运动学模型为:
其中,信息获取模块51,还用于:建立基座标系,基坐标系的横轴与挖掘机的履带共面,基坐标系的纵轴垂直于挖掘机的履带所在平面;根据基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。
本发明实施例所提供的挖掘机臂运动控制装置,其实现原理及产生的技术效果和前述挖掘机臂运动控制方法实施例相同,为简要描述,装置实施例部分未提及之处,可参考前述方法实施例中相应内容。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的挖掘机臂运动控制系统和挖掘机臂运动控制装置的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
最后应说明的是:以上所述实施例,仅为本发明的具体实施方式,用以说明本发明的技术方案,而非对其限制,本发明的保护范围并不局限于此,尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化,或者对其中部分技术特征进行等同替换;而这些修改、变化或者替换,并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应所述以权利要求的保护范围为准。
Claims (10)
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.一种挖掘机臂运动控制系统,其特征在于,包括:期望信号获取模块、控制器模块、信号转换模块、误差计算模块和传感器模块;
所述期望信号获取模块用于根据当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息生成挖掘机臂的期望位姿状态;
所述期望信号获取模块还用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,根据所述位姿差值生成所述挖掘机臂的位姿差值信号,并将所述位姿差值信号发送给所述控制器模块;
所述控制器模块用于接收所述位姿差值信号,并将所述位姿差值信号转换为挖掘机臂的控制输入信号发送给所述信号转换模块;
所述信号转换模块用于将所述控制输入信号转换为脉冲信号,通过脉冲信号控制比例电磁阀的开合状态,以控制所述挖掘机臂运动到所述期望位姿状态;
所述传感器模块用于实时获取所述挖掘机臂的运动位姿状态,并发送给所述误差计算模块;
所述误差计算模块用于根据所述运动位姿状态和所述期望位姿状态获取位姿差值,并将位姿差值信号作为控制器输入信号发送给控制器模块,以控制所述挖掘机臂运动到所述期望位姿状态。
7.一种挖掘机臂运动控制装置,其特征在于,包括:
信息获取模块,用于获取当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息;
解算模块,用于通过预设的逆运动学模型解算得到挖掘机臂的期望位姿状态;
差值模块,用于确定挖掘机臂当前位姿状态与期望位姿状态的位姿差值,并生成位姿差值信号;
发送模块,用于将所述位姿差值信号发送给控制器,以使所述控制器控制所述挖掘机臂运动到所述期望位姿状态。
8.根据权利要求7所述的挖掘机臂运动控制装置,其特征在于,其中,所述发送模块,还用于:
通过传感器模块监测所述挖掘机臂是否运动至所述期望位姿状态;
如果是,向所述控制器发送停止运动指令;
如果否,重新确定当前位姿状态与所述期望位姿状态的位姿差值,生成位姿差值信号,并将位姿差值信号作为控制器输入信号发送给所述控制器。
9.根据权利要求7或8所述的挖掘机臂运动控制装置,其特征在于,所述解算模块,还用于:
根据所述当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息建立运动学模型;所述运动学模型为:
其中,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)为挖斗的齿尖在挖掘机基坐标系下的位置坐标;
根据所述运动学模型确定逆运动学模型;通过所述逆运动学模型解算得到挖掘机臂的期望位姿状态;所述逆运动学模型为:
10.根据权利要求7或8所述的挖掘机臂运动控制装置,其特征在于,所述信息获取模块,还用于:
建立基座标系,所述基坐标系的横轴与所述挖掘机的履带共面,所述基坐标系的纵轴垂直于所述挖掘机的履带所在平面;
根据所述基座标系确定当前目标挖掘点的位置信息、挖掘面与挖掘机基坐标系的夹角信息及挖斗切面与挖掘面的夹角信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811328944.6A CN109296019A (zh) | 2018-11-08 | 2018-11-08 | 挖掘机臂运动控制方法、系统及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811328944.6A CN109296019A (zh) | 2018-11-08 | 2018-11-08 | 挖掘机臂运动控制方法、系统及装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109296019A true CN109296019A (zh) | 2019-02-01 |
Family
ID=65145871
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811328944.6A Pending CN109296019A (zh) | 2018-11-08 | 2018-11-08 | 挖掘机臂运动控制方法、系统及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109296019A (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109972688A (zh) * | 2019-04-10 | 2019-07-05 | 北京拓疆者智能科技有限公司 | 挖掘控制方法、装置及挖掘机控制器 |
CN110905032A (zh) * | 2019-12-03 | 2020-03-24 | 深知智能科技(金华)有限公司 | 一种装载机工作装置作业姿态自动调整控制方法及系统 |
CN111042244A (zh) * | 2019-12-26 | 2020-04-21 | 三一重机有限公司 | 挖掘机工作轨迹控制方法和系统 |
CN115030247A (zh) * | 2022-05-27 | 2022-09-09 | 三一重机有限公司 | 一种臂架位姿信息的校正方法、装置及挖掘机 |
CN115126020A (zh) * | 2022-04-14 | 2022-09-30 | 网易(杭州)网络有限公司 | 一种挖掘机控制方法以及装置 |
CN115302509A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制工程设备臂架的方法、处理器、装置及工程设备 |
CN115674262A (zh) * | 2022-11-14 | 2023-02-03 | 网易(杭州)网络有限公司 | 挖掘机控制方法、装置、设备及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20100025604A (ko) * | 2008-08-28 | 2010-03-10 | 전자부품연구원 | 굴삭기의 굴삭위치 제어방법 |
CN102995679A (zh) * | 2012-12-17 | 2013-03-27 | 潍柴动力股份有限公司 | 一种挖掘机动作控制方法、装置及系统 |
CN107553496A (zh) * | 2017-09-29 | 2018-01-09 | 南京阿凡达机器人科技有限公司 | 机械臂逆运动学求解方法误差的确定及校正方法和装置 |
CN107882103A (zh) * | 2017-10-26 | 2018-04-06 | 南京工业大学 | 一种挖掘机三维姿态显示及远程自动控制系统 |
CN108705532A (zh) * | 2018-04-25 | 2018-10-26 | 中国地质大学(武汉) | 一种机械臂避障路径规划方法、设备及存储设备 |
-
2018
- 2018-11-08 CN CN201811328944.6A patent/CN109296019A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20100025604A (ko) * | 2008-08-28 | 2010-03-10 | 전자부품연구원 | 굴삭기의 굴삭위치 제어방법 |
CN102995679A (zh) * | 2012-12-17 | 2013-03-27 | 潍柴动力股份有限公司 | 一种挖掘机动作控制方法、装置及系统 |
CN107553496A (zh) * | 2017-09-29 | 2018-01-09 | 南京阿凡达机器人科技有限公司 | 机械臂逆运动学求解方法误差的确定及校正方法和装置 |
CN107882103A (zh) * | 2017-10-26 | 2018-04-06 | 南京工业大学 | 一种挖掘机三维姿态显示及远程自动控制系统 |
CN108705532A (zh) * | 2018-04-25 | 2018-10-26 | 中国地质大学(武汉) | 一种机械臂避障路径规划方法、设备及存储设备 |
Non-Patent Citations (1)
Title |
---|
叶贤成: "自动挖掘机的轨迹规划算法研究", 《华侨大学硕士专业学位论文》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109972688A (zh) * | 2019-04-10 | 2019-07-05 | 北京拓疆者智能科技有限公司 | 挖掘控制方法、装置及挖掘机控制器 |
CN109972688B (zh) * | 2019-04-10 | 2021-06-18 | 北京拓疆者智能科技有限公司 | 挖掘控制方法、装置及挖掘机控制器 |
CN110905032A (zh) * | 2019-12-03 | 2020-03-24 | 深知智能科技(金华)有限公司 | 一种装载机工作装置作业姿态自动调整控制方法及系统 |
CN111042244A (zh) * | 2019-12-26 | 2020-04-21 | 三一重机有限公司 | 挖掘机工作轨迹控制方法和系统 |
CN115126020A (zh) * | 2022-04-14 | 2022-09-30 | 网易(杭州)网络有限公司 | 一种挖掘机控制方法以及装置 |
CN115030247A (zh) * | 2022-05-27 | 2022-09-09 | 三一重机有限公司 | 一种臂架位姿信息的校正方法、装置及挖掘机 |
CN115030247B (zh) * | 2022-05-27 | 2023-10-31 | 三一重机有限公司 | 一种臂架位姿信息的校正方法、装置及挖掘机 |
CN115302509A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制工程设备臂架的方法、处理器、装置及工程设备 |
CN115302509B (zh) * | 2022-08-16 | 2023-09-05 | 湖南中联重科智能高空作业机械有限公司 | 用于控制工程设备臂架的方法、处理器、装置及工程设备 |
CN115674262A (zh) * | 2022-11-14 | 2023-02-03 | 网易(杭州)网络有限公司 | 挖掘机控制方法、装置、设备及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109296019A (zh) | 挖掘机臂运动控制方法、系统及装置 | |
Singh | State of the art in automation of earthmoving | |
CN109115213B (zh) | 用于利用传感器融合来确定机器状态的系统和方法 | |
US9969084B2 (en) | Coordinated joint motion control system | |
US11332910B2 (en) | Excavating earth from a dig site using an excavation vehicle | |
Sakaida et al. | The analysis of excavator operation by skillful operator-extraction of common skills | |
US7007415B2 (en) | Method and system of controlling a work tool | |
JP4444884B2 (ja) | 建設機械および建設機械に用いられる制御装置 | |
US20220282451A1 (en) | Autonomous operation by earth-moving vehicle based on triggering conditions | |
CN211113856U (zh) | 一种挖掘机的电控系统及正流量系统挖掘机 | |
Allain et al. | Blended shared control of a hydraulic excavator | |
EP4303369A1 (en) | Work machine | |
Maeda | Learning and reacting with inaccurate prediction: applications to autonomous excavation | |
Zhang et al. | Toward autonomous mining: design and development of an unmanned electric shovel via point cloud-based optimal trajectory planning | |
JP7014930B1 (ja) | アースバケット認識システム及びプログラム | |
US9752336B2 (en) | Systems and methods for controlling an implement of a machine utilizing an orientation leveling system | |
CN116897235A (zh) | 用于对土方机械的循环操作进行自动控制的方法 | |
Yang et al. | Motion control for earth excavation robot based on force pre-load and cross-coupling compensation | |
Lee et al. | Development of a machine control technology and productivity evaluation for excavator | |
CN115544768A (zh) | 一种自主挖掘作业轨迹生成方法及系统 | |
Park et al. | Implementation of a virtual autonomous excavator | |
JP2916957B2 (ja) | 掘削作業機の自動制御方法 | |
Lertcomepeesin et al. | Study of Autnomous Pond Excavation in Real Environment | |
US20230392346A1 (en) | System for mitigation of unintentional working machine movement | |
CN111622297B (zh) | 一种挖掘机的在线作业纠偏系统和方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190201 |