CN115906466A - 挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 - Google Patents
挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 Download PDFInfo
- Publication number
- CN115906466A CN115906466A CN202211434362.2A CN202211434362A CN115906466A CN 115906466 A CN115906466 A CN 115906466A CN 202211434362 A CN202211434362 A CN 202211434362A CN 115906466 A CN115906466 A CN 115906466A
- Authority
- CN
- China
- Prior art keywords
- excavator
- bucket
- joint
- tooth
- tooth tip
- 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 161
- 238000000034 method Methods 0.000 claims abstract description 70
- 230000006870 function Effects 0.000 claims description 91
- 230000001133 acceleration Effects 0.000 claims description 18
- 230000033228 biological regulation Effects 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 11
- 238000009412 basement excavation Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 11
- 238000004590 computer program Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 4
- 238000006073 displacement reaction Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000009966 trimming Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000003750 conditioning effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 238000001824 photoionisation detection Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Operation Control Of Excavators (AREA)
Abstract
本公开涉及一种挖掘机及其铲斗齿尖运动规划系统和装置、存储介质。该方法包括:建立挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束;构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系;在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。本公开能够提高铲斗齿尖运动规划的速度,提高规划程序的执行效率,且可以实现挖掘机臂铲齿齿尖在笛卡尔空间进行直线运动。
Description
技术领域
本公开涉及运动规划技术领域,特别涉及一种挖掘机及其铲斗齿尖运动规划系统和装置、存储介质。
背景技术
挖掘机作为一种移动物料的机械,在采矿和建筑领域有广泛的应用。由于其应用的广泛性,挖掘机操作员必须掌握多种多样的操作技术以适应不同的工况和需求。这使得培养合格的操作员成为耗时耗力的工作。此外,由于操作的复杂性和恶劣的工作环境,操作员难以长时间保持高效的工作状态,这使得挖掘机自动化的需求愈加高涨。
发明内容
鉴于以上技术问题中的至少一项,本公开提供了一种挖掘机及其铲斗齿尖运动规划系统和装置、存储介质,能够提高铲斗齿尖运动规划的速度,提高规划程序的执行效率,且可以实现挖掘机臂铲齿齿尖在笛卡尔空间进行直线运动。
根据本公开的一个方面,提供一种挖掘机铲斗齿尖运动规划方法,包括:
建立挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束;
构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系;
在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划方法还包括:
根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束;
依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
在本公开的一些实施例中,所述依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置包括:
根据挖机臂正运动学模型设定k时刻各关节初始速度;
在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值,其中,所述k+1时刻为k时刻的下一时刻;
计算k+1时刻各个关节的目标角度;
调用挖机臂正运动学模型计算k+1时刻计算出铲斗齿尖的位置;
判断k+1时刻铲斗齿尖是否运动到目标位置;
在k+1时刻铲斗齿尖运动到目标位置的情况下,控制各个关节k+1时刻停止运动。
在本公开的一些实施例中,所述依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置还包括:
在k+1时刻铲斗齿尖未运动到目标位置的情况下,令k等于k+1,之后执行在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值的步骤。
在本公开的一些实施例中,所述关节包括第一关节、第二关节、第三关节和第四关节,其中:
第一关节为挖掘机转台与挖掘机行走平台间的转动连接部位;
第二关节为挖掘机动臂与挖掘机转台间的铰接部位;
第三关节为挖掘机斗杆与挖掘机动臂间的铰接部位;
第四关节为挖掘机斗杆与挖掘机铲斗间的铰接部位。
在本公开的一些实施例中,所述构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系包括:
构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖线速度之间的关系,其中,所述各关节角速度包括第二关节角速度、第三关节角速度和第四关节角速度。
在本公开的一些实施例中,所述物理约束包括角度约束、角速度约束和角加速度约束中的至少一项。
在本公开的一些实施例中,所述硬性约束函数为第一比值、第二比值和第三比值均相等,其中,第一比值为初始位置x轴坐标和终点位置x轴坐标的差值与第一线速度的比值,第二比值为初始位置y轴坐标和终点位置y轴坐标的差值与第二线速度的比值,第三比值为初始位置z轴坐标和终点位置z轴坐标的差值与第三线速度的比值,第一线速度、第二线速度和第三线速度分别是挖掘机铲斗铲齿齿尖部位在笛卡尔空间内三个两两垂直方向的线速度。
在本公开的一些实施例中,平整面任务包括平整面挖掘和平整面修整两部分,其中,平整面挖掘包括平面挖掘和斜面挖掘,平整面修整包括平面修整和斜面修整。
在本公开的一些实施例中,平整面挖掘过程中铲斗姿态逐渐收敛到铲斗铲面和挖掘面平行,铲斗铲齿齿尖为直线运动。
在本公开的一些实施例中,平整面修整过程中铲斗姿态保持不变,铲斗铲齿齿尖为直线运动。
在本公开的一些实施例中,所述确定k+1时刻的各个关节速度值包括:
选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
在本公开的一些实施例中,所述选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0包括:
设定预设速度范围,其中,所述预设速度范围为速度最小值到速度最大值的速度范围;
根据预定速度模型选取各个关节速度值;
构建第一比例积分微分PID结构,确定第k个时刻输入所述第一PID结构的误差,其中,所述第一PID结构的误差为当前铲斗铲齿齿尖的位置与最终位置的归一化距离;
第一PID结构输出第k个时刻的速度调节系数到预定速度模型;
根据速度调节系数与归一化距离、比例调节系数、积分调节系数和微分调节系数的对应关系,调节归一化距离、比例调节系数、积分调节系数和微分调节系数,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
在本公开的一些实施例中,所述根据预定速度模型选取各个关节速度值包括:
根据速度最小值、速度最大值和速度调节系数,选取各个关节速度值。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划方法还包括:
构建第二PID结构,其中,所述第二PID结构,用于在所述各关节速度均分别为0时启动,并判断挖掘机铲齿齿尖是否到达所述终点位置。
在本公开的一些实施例中,所述第二PID结构的数据输入为所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置与实际给定的终点位置的归一化向量。
在本公开的一些实施例中,所述第二PID结构的数据输出为所述第一PID改变后的终点位置,其中,所述改变后的终点位置根据归一化向量、比例调节系数、积分调节系数和微分调节系数确定。
在本公开的一些实施例中,所述改变后的终点位置不能超出遇预定工作空间,当所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置到达实际给定的终点位置的情况下,所述第二PID结构停止。
根据本公开的另一方面,提供一种挖掘机铲斗齿尖运动规划装置,包括:
模型建立模块,被配置为挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束;
启发函数构建模块,被配置为构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系;
运行路线规划模块,被配置为在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划装置还包括:
硬件约束模块,被配置为根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束;
最优运动确定模块,被配置为依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划装置用于执行实现如上述任一实施例所述的挖掘机铲斗齿尖运动规划方法的操作。
根据本公开的另一方面,提供一种挖掘机铲斗齿尖运动规划装置,包括:
存储器,被配置为存储指令;
处理器,被配置为执行所述指令,使得所述挖掘机铲斗齿尖运动规划装置执行实现如上述任一实施例所述挖掘机铲斗齿尖运动规划方法的操作。
根据本公开的另一方面,提供一种挖掘机,包括如上述任一实施例所述挖掘机铲斗齿尖运动规划装置。
根据本公开的另一方面,提供一种计算机可读存储介质,其中,所述计算机可读存储介质存储有计算机指令,所述指令被处理器执行时实现如上述任一实施例所述挖掘机铲斗齿尖运动规划方法。
本公开能够提高铲斗齿尖运动规划的速度,提高规划程序的执行效率,且可以实现挖掘机臂铲齿齿尖在笛卡尔空间进行直线运动。
附图说明
为了更清楚地说明本公开实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本公开的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本公开挖掘机铲斗齿尖运动规划方法一些实施例的示意图。
图2为本公开一些实施例中挖机建模的示意图。
图3为本公开一些实施例中挖掘机物理约束的示意图。
图4为本公开一些实施例中挖掘机平面挖掘示意图。
图5为本公开一些实施例中挖掘机斜面挖掘示意图。
图6为本公开一些实施例中挖掘机平面修整示意图。
图7为本公开一些实施例中挖掘机斜面修整示意图。
图8为本公开挖掘机铲斗齿尖运动规划方法另一些实施例的示意图。
图9为本公开一些实施例中挖掘机铲斗铲齿齿尖笛卡尔空间启发式直线运动规划的流程图。
图10为本公开另一些实施例中挖掘机铲斗铲齿齿尖笛卡尔空间启发式直线运动规划的流程图。
图11为本公开一些实施例中双层PID结构规划的结构示意图。
图12为本公开挖掘机铲斗齿尖运动规划装置一些实施例的示意图。
图13为本公开挖掘机铲斗齿尖运动规划装置另一些实施例的结构示意图。
具体实施方式
下面将结合本公开实施例中的附图,对本公开实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本公开一部分实施例,而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的,决不作为对本公开及其应用或使用的任何限制。基于本公开中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本公开保护的范围。
除非另外具体说明,否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。
同时,应当明白,为了便于描述,附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论,但在适当情况下,所述技术、方法和设备应当被视为授权说明书的一部分。
在这里示出和讨论的所有示例中,任何具体值应被解释为仅仅是示例性的,而不是作为限制。因此,示例性实施例的其它示例可以具有不同的值。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步讨论。
发明人通过研究发现:相关技术的挖掘机系统主要由人来操作,完成诸如直线沟壑挖掘、土方移动运输、平整操作面等高层任务;且相关技术关于挖掘机的技术方案大多集中在机构设计以及电控实体装置上,没有涉及自动化挖掘机运动规划的算法描述与研究。
相关技术挖掘机操作员在平整坡面或者挖掘沟面时,不能有效控制坡面精度。
鉴于以上技术问题中的至少一项,本公开提供了一种挖掘机及其铲斗齿尖运动规划系统和装置、存储介质,下面通过具体实施例对本公开进行说明。
图1为本公开挖掘机铲斗齿尖运动规划方法一些实施例的示意图。优选的,本实施例可由本公开挖掘机铲斗齿尖运动规划装置或本公开挖掘机铲斗齿尖运动规划系统执行。该方法包括步骤1至步骤3中的至少一个步骤,其中:
步骤1,建立挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束。
在本公开的一些实施例中,所述物理约束包括角度约束、角速度约束和角加速度约束中的至少一项。
如下表1所示,基于MDH(Modified D-H,改进的Modified D-H)方法建立挖掘机臂正运动学模型;其中,相关运动学参数参见表1。
表1
j | αj-1/(°) | aj-1/(m) | dj/(m) | θj/(°) |
1 | 0 | 0 | d1 | θ1 |
2 | 90 | a1 | d2 | θ2 |
3 | 0 | a2 | 0 | θ3 |
4 | 0 | a3 | 0 | θ4 |
5 | 0 | a4 | 0 | 0 |
表1中,以回转中心中点为原点建立挖机臂基坐标系,参数1(关节1)表示固结于回转中心的坐标系中心,参数2(关节2)表示固结于动臂旋转轴心的坐标系中心,参数3(关节3)表示固结于斗杆旋转轴心的坐标系中心,参数4(关节4)表示固结于铲斗旋转轴心的坐标系中心,参数5(关节5)表示固结于铲齿的坐标系中心,该坐标系随铲齿而动。αj-1为连杆扭转角,aj-1为连杆长度,dj为连杆偏距,θj为连杆长度。aj,j={1,2,3,4}以及dj,j={1,2}是常数参数,随挖机型号的不同而不同。θj,j={1,2,3,4}在挖机完成任务的过程中是可变的变量,本套运动规划算法直接的输出是带有时间戳的这四个角的角度序列。
表2给出了αj以及dj的一组典型的示例参数。
表2
名称 | 数值/(cm) |
a1 | 12.0 |
a2 | 460.0 |
a3 | 210.9 |
a4 | 123.5 |
d1 | 57.9 |
d2 | 13.7 |
图2为本公开一些实施例中挖机建模的示意图。如图2、图4至图7所示,本公开挖机(挖掘机)的关节包括第一关节、第二关节、第三关节和第四关节,其中:
第一关节为挖掘机转台与挖掘机行走平台间的转动连接部位。
第二关节为挖掘机动臂与挖掘机转台间的铰接部位。
第三关节为挖掘机斗杆与挖掘机动臂间的铰接部位。
第四关节为挖掘机斗杆与挖掘机铲斗间的铰接部位。
在本公开的一些实施例中,挖机臂关节i相对于关节i-1的齐次变换矩阵如公式(1)所示。
图3为本公开一些实施例中挖掘机物理约束的示意图。如图3所示,挖机的物理约束指各关节在运动过程中应该满足角度约束、速度约束、加速度约束。
在本公开的一些实施例中,所述角度约束包括:关于第一关节:挖掘机转台相对于挖掘机行走平台转动时产生的带时间戳的角度序列为θ1;关于第二关节:挖掘机动臂与挖掘机转台间形成的带时间戳的线面角度序列为θ2;关于第三关节:挖掘机斗杆与挖掘机动臂间形成的带时间戳的角度序列为θ3和关于第四关节:挖掘机斗杆与挖掘机铲斗侧立面间形成的带时间戳的角度序列为θ4。
在本公开的一些实施例中,上述物理约束的实例参数如表3所示。
表3
图4为本公开一些实施例中挖掘机平面挖掘示意图。图5为本公开一些实施例中挖掘机斜面挖掘示意图。图6为本公开一些实施例中挖掘机平面修整示意图。图7为本公开一些实施例中挖掘机斜面修整示意图。
在本公开的一些实施例中,平整面任务包括平整面挖掘和平整面修整两部分,其中,平整面挖掘包括平面挖掘(如图4所示)和斜面挖掘(如图5所示);平整面修整包括平面修整(如图7所示)和斜面修整(如图8所示)。
在本公开的一些实施例中,平整面挖掘过程中铲斗姿态逐渐收敛到铲斗铲面和挖掘面平行,以达到平整挖掘目的。铲斗铲齿齿尖为直线运动。
在本公开的一些实施例中,平整面修整过程中铲斗姿态保持不变,但是铲斗铲齿齿尖为直线运动,可以对平面进行精细修整,达到更好的平整效果。
本公开上述两种挖掘过程中,铲斗铲齿齿尖均为直线运动,基于此,本公开需要进行满足各关节物理约束的铲齿齿尖直线路径规划算法研究,得到能够完成任务的带时间戳的角度序列。需要指出的是,在平整面挖掘任务中,θ1始终保持不变。
步骤2,构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系。
在本公开的一些实施例中,步骤2可以包括:通过铲斗铲齿齿尖启发式直线运动规划算法,构建启发式函数,构建出所述挖掘机各关节中,各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系。
规划算法中θ1的值始终为一定值,因此规划算法专注于对θ2,θ3,θ4的规划,即专注于对铲齿齿尖的直线路径规划。
发明人通过研究发现:相关技术其他规划算法在规划直线路径时一般采用先加速后匀速最后减速,二分法得到临界的最大速度以满足各关节的物理约束,但这样的方法只能以最大速度作为二分处理的变量,而对于加速时间、匀速时间定义较为机械、固定。
本公开上述实施例提出了一种更加智能的算法,借助速度的雅可比矩阵,启发式的进行规划,最终得到满足各关节物理约束的θ2,θ3,θ4的带时间戳的角度序列,效果是铲斗的铲齿齿尖直线运动。同时,为了进一步简化,并考虑到实际任务需求,令θ1角度在整个任务中保持不变,即θ1=0。
在本公开的一些实施例中,步骤2可以包括:构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖线速度之间的关系,其中,所述各关节角速度包括第二关节角速度、第三关节角速度和第四关节角速度。
在本公开的一些实施例中,步骤2可以包括:应用雅可比矩阵构建出了三个关节角速度与铲齿齿尖线速度之间的关系。
雅可比矩阵提供了在某一时刻关节空间速度到笛卡尔空间速度的线性映射,这个线性映射是时变的,仅对某一时刻认为是常矩阵。本规划算法注重的是铲斗铲齿齿尖在笛卡尔空间下的直线规划。
在本公开的一些实施例中,有如公式(3)所示,为某一时刻与铲斗铲齿齿尖在笛卡尔空间下的运动参量(vx,vy,vz,Ωx,Ωy,Ωz)T(参量前三列为铲齿齿尖运动线速度,后三列为铲齿齿尖运动角速度)的映射关系。
其中第i关节相对于第i-1关节的旋转矩阵可表示为公式(4)。
公式(4)中,表示第k关节在挖机臂基坐标系下的位移,为铲斗铲齿齿尖在挖机臂基坐标系下的位移;vx,vy,vz,分别是挖掘机铲斗铲齿齿尖部位在所述笛卡尔空间内三个两两垂直方向的线速度。考虑到本公开规划算法中仅考虑铲齿齿尖在笛卡尔坐标系下的位移速度,因此,在某一时刻θ2,θ3,θ4与铲齿齿尖在笛卡尔空间下的位移速度有如公式(5)映射关系。
公式(5)中第一个矩阵(映射矩阵)可以由公式(6)确定。
由此可以得到如公式(7)所示的方程组。
求解方程组(7),可以得到如公式(8)所示的启发式函数。
本公开可以通过如公式(8)所示的启发式函数搜索到全局最优解。至此,应用雅可比矩阵构建出了三个关节角速度与铲齿齿尖线速度之间的关系。
步骤3,在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
图8为本公开挖掘机铲斗齿尖运动规划方法另一些实施例的示意图。优选的,本实施例可由本公开挖掘机铲斗齿尖运动规划装置或本公开挖掘机铲斗齿尖运动规划系统执行。图8实施例方法包括步骤1至步骤5中的至少一个步骤,其中,步骤1至步骤3与图1实施例的步骤1至步骤3相同或类似,其中:
步骤1,建立挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束。
步骤2,构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系。
步骤3,在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
步骤4,根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束。
在本公开的一些实施例中,所述硬性约束函数为第一比值、第二比值和第三比值均相等,其中,第一比值为初始位置x轴坐标和终点位置x轴坐标的差值与第一线速度的比值,第二比值为初始位置y轴坐标和终点位置y轴坐标的差值与第二线速度的比值,第三比值为初始位置z轴坐标和终点位置z轴坐标的差值与第三线速度的比值,第一线速度、第二线速度和第三线速度分别是挖掘机铲斗铲齿齿尖部位在笛卡尔空间内三个两两垂直方向的线速度。
在本公开的一些实施例中,步骤4可以包括步骤41至步骤42中的至少一个步骤,其中:
步骤41,定义铲斗铲齿齿尖的初始位置如公式(9)所示,终点位置如公式(10)所示。
p0=(x0,y0,z0) (9)
p1=(x1,y1,z1) (10)
在本公开的一些实施例中,步骤42可以包括:为了使得铲斗铲齿齿尖能够沿着初末位置方向进行直线移动,需要满足如公式(11)的硬性约束函数,根据公式(11)的硬性约束函数,可以构建出铲齿齿尖始末位置与铲齿齿尖线速度之间的关系。
本公开上述实施例可以根据铲齿齿尖始末位置与铲齿齿尖线速度之间的关系,以及,三个关节角速度与铲齿齿尖线速度的关系,确定铲齿齿尖始末位置与三个关节角速度的关系,从而通过控制各个关节的角速度实现对铲齿齿尖启发式直线运动的规划。
步骤5,依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
在本公开的一些实施例中,步骤5可以包括:通过构建铲齿齿尖直线运动规划的硬性约束函数确定铲齿齿尖始末位置与铲齿齿尖线速度之间的关系;同时通过构建铲齿齿尖直线运动规划启发式函数确定个关节角速度与铲齿齿尖线速度之间的关系,将铲齿齿尖直线运动规划的硬性约束函数和启发式函数结合构建出铲斗铲齿齿尖启发式直线运动规划算法,实现铲齿齿尖在始末位置所在直线的直线运动规划。
本公开上述实施例可以通过构建铲齿齿尖直线运动规划的硬性约束函数确定铲齿齿尖始末位置与铲齿齿尖线速度之间的关系;同时通过构建铲齿齿尖直线运动规划启发式函数确定个关节角速度与铲齿齿尖线速度之间的关系,将铲齿齿尖直线运动规划的硬性约束函数和启发式函数结合构建出铲斗铲齿齿尖启发式直线运动规划算法,实现铲齿齿尖在始末位置所在直线的直线运动规划。本公开上述实施例启发式运动规划的方法,相对于相关技术的规划方法,在满足各个关节物理约束下,启发式运动规划方法规划速度快,在启发式函数的作用下能快速搜索到全局最优路径,能提高规划效率,进而提高控制系统的性能。
图9为本公开一些实施例中挖掘机铲斗铲齿齿尖笛卡尔空间启发式直线运动规划的流程图。如图9所示,本公开启发式直线运动规划方法(即图8实施例的步骤5)可以包括步骤51至步骤57中的至少一个步骤,其中:
步骤51,根据挖机臂正运动学模型设定k时刻各关节初始速度。
步骤52,在角加速度约束、所述硬性约束函数(如公式(11))及启发式函数(如公式(8))的约束下确定k+1时刻的各个关节速度值,其中,所述k+1时刻为k时刻的下一时刻。
在本公开的一些实施例中,步骤52中,所述确定k+1时刻的各个关节速度值的步骤可以包括:选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
步骤53,计算k+1时刻各个关节的目标角度。
步骤54,调用挖机臂正运动学模型计算k+1时刻计算出铲斗齿尖的位置。
步骤55,判断k+1时刻铲斗齿尖是否运动到目标位置。在k+1时刻铲斗齿尖运动到目标位置的情况下,执行步骤56;否则,在k+1时刻铲斗齿尖未运动到目标位置的情况下,执行步骤57。
步骤56,控制各个关节k+1时刻停止运动。
步骤57,令k等于k+1,之后返回步骤52重复执行步骤52至步骤55,直到铲齿齿尖到达所述终点位置。
图10为本公开另一些实施例中挖掘机铲斗铲齿齿尖笛卡尔空间启发式直线运动规划的流程图。如图10所示,本公开启发式直线运动规划方法(即图8实施例的步骤5)可以包括步骤1至步骤57中的至少一个步骤,其中,图10实施例的步骤1与图1或图8实施例的步骤1相同或类似,图10实施例的步骤51、步骤53至步骤57与图9实施例的步骤51、步骤53至步骤57相同或类似。图9实施例的步骤52可以包括图10实施例的步骤521至步骤523。
图9和图10实施例给给出了迭代更新算法流程,其中,k表示第k个离散时刻;a2max表示关节2的角加速度物理约束;a3max表示关节3的角加速度物理约束;a4max表示关节4的角加速度物理约束;dt表示离散时刻的间隔;表示第k个离散时刻关节i的角速度;v2max表示关节2的角速度物理约束;v3max表示关节3的角速度物理约束;v4max表示关节4的角速度物理约束。
步骤1,构建挖机臂正运动学模型。步骤1的详细描述请参见上文,在此不再赘述。
步骤51,按照公式(12)设定k时刻各关节初始速度。
步骤521,按照公式(13)设计角加速度约束条件。
步骤522,设计启发式直线运动规划函数组。
在本公开的一些实施例中,启发式直线运动规划函数组包括启发式函数(参见公式(8))和直线运动规划硬性约束函数(参见公式(11))。
步骤523,在步骤51和步骤521的基础上选择k+1刻的各关节速度值,如公式(14)所示。
步骤53:计算k+1时刻各个关节的目标角度,如公式(15)所示。
步骤54、步骤55,判断铲齿齿尖是否到达目标位置。如果是,执行步骤56;如果否,执行步骤57。
步骤57,令k=k+1,并返回步骤521,直到铲齿齿尖到达所述终点位置。
本公开的技术方案提出了一种挖掘机铲斗铲齿齿尖直线运动的规划方法,该技术方案通过构建铲齿齿尖直线运动规划的硬性约束函数确定铲齿齿尖始末位置与铲齿齿尖线速度之间的关系;同时通过构建铲齿齿尖直线运动规划启发式函数确定个关节角速度与铲齿齿尖线速度之间的关系,将铲齿齿尖直线运动规划的硬性约束函数和启发式函数结合构建出铲斗铲齿齿尖启发式直线运动规划算法,实现铲齿齿尖在始末位置所在直线的直线运动规划。
本公开的启发式运动规划的方法,相对于相关技术的规划方法,在满足各个关节物理约束下,启发式运动规划方法规划速度快,在启发式函数的作用下能快速搜索到全局最优路径,能提高规划效率,进而提高控制系统的性能。另外,本公开挖掘机铲斗铲齿齿尖部位做直线规划是基于笛卡尔空间启发式直线运动规划的方法,可以实现挖掘机臂铲齿齿尖在笛卡尔空间进行直线或规划的运动,在应用到实际情况中时,既可以帮助挖掘机操作员进行挖沟修坡作业,还能帮助挖掘机操作员进行平整路面作业,能解决挖掘机操作员在进行上述作业时不能有效控制坡面精度的问题。本公开填补了相关技术的空白,具有较好的工程实用化价值;因此,将本公开上述方法部署在挖掘机上应用后,能够极大的解放人力,提高生产效率。
在本公开的一些实施例中,本公开挖掘机铲斗齿尖运动规划方法还可以包括构建PID结构,PID控制器由比例单元(P)、积分单元(I)和微分单元(D)组成。基于反馈概念,包括三个基本要素:测量、比较和执行。测量关心的是变量,并与期望值相比较,以此误差来纠正和控制系统的响应。
图11为本公开一些实施例中双层PID结构规划的结构示意图。本公开双层规划方法可以包括步骤(1)和步骤(2),其中:
步骤(1)构建第一PID结构。
值得注意的是:如图8至图10实施例所示,步骤5中并未给出如何在给定范围内选取具体的下一时刻的各关节速度;同时,在算法到达目标位置后,由于惯性的存在,各关节速度可能不为0,会出现“超调”,即不能正好停在p1处、“收敛速度慢”等问题。因此,需要对算法进行改进,改进的措施是研究在步骤5中在给定范围内选取下一时刻各关节速度的方法,使得能够在到达目标位置时正好各关节速度为0。
接下来重点描述在图9实施例的步骤5中如何在给定范围内具体选取各关节速度值。
在本公开的一些实施例中,步骤(1)可以包括:构建第一PID结构,所述第一PID结构通过各关节速度的选取方法搭建而成。
在本公开的一些实施例中,图9实施例的步骤52中,所述选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0的步骤可以包括步骤521至步骤524中的至少一个步骤,其中:
步骤521,设定预设速度范围[vlower,vupper],其中,所述预设速度范围[vlower,vupper]为速度最小值vlower到速度最大值vupper的速度范围。
步骤522,根据预定速度模型选取各个关节速度值。
在本公开的一些实施例中,步骤522可以包括:根据速度最小值、速度最大值和速度调节系数,选取各个关节速度值。
在本公开的一些实施例中,步骤522可以包括:根据如公式(16)的预定速度模型选取各个关节速度值。公式(16)中,m为速度调节系数,若m在整个过程中均取1,则所述各关节的速度不断增加;若m在前一阶段取1,后一阶段取0,则所述各关节速度先增加后减小。公式(16)对均适用。
vselect=vlower+m(vupper-vlower),m∈[0,1] (16)
步骤523,构建第一比例积分微分PID结构,确定第k个时刻输入所述第一PID结构的误差,其中,所述第一PID结构的误差为当前铲斗铲齿齿尖的位置与最终位置的归一化距离第一PID结构输出第k个时刻的速度调节系数mk到预定速度模型。
在本公开的一些实施例中,步骤523可以包括:利用公式(17)确定第k个时刻的速度调节系数mk。
步骤524,根据速度调节系数与归一化距离、比例调节系数、积分调节系数和微分调节系数的对应关系,调节归一化距离、比例调节系数、积分调节系数和微分调节系数,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
(2)构建第二PID结构。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划方法还可以包括:考虑再加一层PID结构(即第二PID结构),该层PID处于如图11描述的PID的第二PID结构,即需要等第一PID结构所指代的过程完全结束后进行作用,第一PID结构结束的条件是各关节速度为0时结束。
在本公开的一些实施例中,所述第二PID结构,用于在所述各关节速度均分别为0时启动,并判断挖掘机铲齿齿尖是否到达所述终点位置,如未到达,则执行图8至图10任一实施例中步骤5对挖掘机铲齿齿尖位置进行补偿。
在本公开的一些实施例中,如图11所示,所述第二PID结构的数据输出为所述第一PID改变后的终点位置p′1,其中,所述改变后的终点位置根据归一化向量、比例调节系数、积分调节系数和微分调节系数确定。
在本公开的一些实施例中,第二PID结构的数据输出,即终点位置p′1满足公式(18)。
公式(18)中,分别代表比例调节系数、积分调节系数、微分调节系数。所述改变后的终点位置p′1不能超出遇预定工作空间,当所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置pend到达一开始给定的终点位置P1的情况下,所述第二PID结构停止。
表4
针对启发式运动规划算法精度不高的情况下,本公开进一步提出的一种双层PID结构,来改进启发式运动规划算法,优化铲斗铲齿齿尖启发式直线运动规划;具体地,通过构建双层PID结构,并通过设置第一PID和第二PID不同的结束条件,实现在满足关节物理约束的条件下铲斗铲齿齿尖从给定的初始位置p0直线运动到p1并保证到达p1时铲斗铲齿齿尖速度为0,也就是,当控制铲齿齿尖达到目标位置时,确保各个关节速度为零,避免出现“超调”、不能正好停在指定位置、“收敛速度慢”等问题,由此进一步提升了控制精度。
图12为本公开挖掘机铲斗齿尖运动规划装置一些实施例的示意图。如图12所示,本公开挖掘机铲斗齿尖运动规划装置可以包括模型建立模块121、启发函数构建模块122和运行路线规划模块123,其中:
模型建立模块121,被配置为挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束。
在本公开的一些实施例中,所述关节包括第一关节、第二关节、第三关节和第四关节,其中:第一关节为挖掘机转台与挖掘机行走平台间的转动连接部位;第二关节为挖掘机动臂与挖掘机转台间的铰接部位;第三关节为挖掘机斗杆与挖掘机动臂间的铰接部位;第四关节为挖掘机斗杆与挖掘机铲斗间的铰接部位。
在本公开的一些实施例中,所述物理约束包括角度约束、角速度约束和角加速度约束中的至少一项。
启发函数构建模块122,被配置为构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系。
在本公开的一些实施例中,启发函数构建模块122,被配置为构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖线速度之间的关系,其中,所述各关节角速度包括第二关节角速度、第三关节角速度和第四关节角速度。
运行路线规划模块123,被配置为在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
在本公开的一些实施例中,如图12所示,所述挖掘机铲斗齿尖运动规划装置还可以包括硬件约束模块124和最优运动确定模块125,其中:
硬件约束模块124,被配置为根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束。
在本公开的一些实施例中,所述硬性约束函数为第一比值、第二比值和第三比值均相等,其中,第一比值为初始位置x轴坐标和终点位置x轴坐标的差值与第一线速度的比值,第二比值为初始位置y轴坐标和终点位置y轴坐标的差值与第二线速度的比值,第三比值为初始位置z轴坐标和终点位置z轴坐标的差值与第三线速度的比值,第一线速度、第二线速度和第三线速度分别是挖掘机铲斗铲齿齿尖部位在笛卡尔空间内三个两两垂直方向的线速度。
最优运动确定模块125,被配置为依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
在本公开的一些实施例中,最优运动确定模块125,被配置为根据挖机臂正运动学模型设定k时刻各关节初始速度;在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值,其中,所述k+1时刻为k时刻的下一时刻;计算k+1时刻各个关节的目标角度;调用挖机臂正运动学模型计算k+1时刻计算出铲斗齿尖的位置;判断k+1时刻铲斗齿尖是否运动到目标位置;在k+1时刻铲斗齿尖运动到目标位置的情况下,控制各个关节k+1时刻停止运动。
在本公开的一些实施例中,最优运动确定模块125,还可以被配置为在k+1时刻铲斗齿尖未运动到目标位置的情况下,令k等于k+1,之后执行在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值的操作。
在本公开的一些实施例中,平整面任务包括平整面挖掘和平整面修整两部分,其中,平整面挖掘包括平面挖掘和斜面挖掘,平整面修整包括平面修整和斜面修整。
在本公开的一些实施例中,平整面挖掘过程中铲斗姿态逐渐收敛到铲斗铲面和挖掘面平行,铲斗铲齿齿尖为直线运动。
在本公开的一些实施例中,平整面修整过程中铲斗姿态保持不变,铲斗铲齿齿尖为直线运动。
在本公开的一些实施例中,最优运动确定模块125,在确定k+1时刻的各个关节速度值的情况下,可以被配置为选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
在本公开的一些实施例中,最优运动确定模块125,在选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0的情况下,可以被配置为设定预设速度范围,其中,所述预设速度范围为速度最小值到速度最大值的速度范围;根据预定速度模型选取各个关节速度值;构建第一比例积分微分PID结构,确定第k个时刻输入所述第一PID结构的误差,其中,所述第一PID结构的误差为当前铲斗铲齿齿尖的位置与最终位置的归一化距离;第一PID结构输出第k个时刻的速度调节系数到预定速度模型;根据速度调节系数与归一化距离、比例调节系数、积分调节系数和微分调节系数的对应关系,调节归一化距离、比例调节系数、积分调节系数和微分调节系数,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
在本公开的一些实施例中,最优运动确定模块125,在根据预定速度模型选取各个关节速度值的情况下,可以被配置为根据速度最小值、速度最大值和速度调节系数,选取各个关节速度值。
在本公开的一些实施例中,最优运动确定模块125,还可以被配置为构建第二PID结构,其中,所述第二PID结构,用于在所述各关节速度均分别为0时启动,并判断挖掘机铲齿齿尖是否到达所述终点位置。
在本公开的一些实施例中,所述第二PID结构的数据输入为所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置与实际给定的终点位置的归一化向量。
在本公开的一些实施例中,所述第二PID结构的数据输出为所述第一PID改变后的终点位置,其中,所述改变后的终点位置根据归一化向量、比例调节系数、积分调节系数和微分调节系数确定。
在本公开的一些实施例中,所述改变后的终点位置不能超出遇预定工作空间,当所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置到达实际给定的终点位置的情况下,所述第二PID结构停止。
在本公开的一些实施例中,所述挖掘机铲斗齿尖运动规划装置用于执行实现如上述任一实施例(例如图1-图11任一实施例)所述的挖掘机铲斗齿尖运动规划方法的操作。
本公开上述实施例可以规划挖掘机铲斗齿尖直线运动,并有效控制坡面精度。
图13为本公开挖掘机铲斗齿尖运动规划装置另一些实施例的结构示意图。如图13所示,本公开挖掘机铲斗齿尖运动规划装置可以包括存储器131和处理器132。
存储器131用于存储指令,处理器132耦合到存储器131,处理器132被配置为基于存储器存储的指令执行实现如上述任一实施例(例如图1至图11任一实施例)所述挖掘机铲斗齿尖运动规划方法。
如图13所示,该挖掘机铲斗齿尖运动规划装置还包括通信接口133,用于与其它设备进行信息交互。同时,该挖掘机铲斗齿尖运动规划装置还包括总线134,处理器132、通信接口133、以及存储器131通过总线134完成相互间的通信。
存储器131可以包含高速RAM存储器,也可还包括非易失性存储器(Non-volatileMemory),例如至少一个磁盘存储器。存储器131也可以是存储器阵列。存储器131还可能被分块,并且块可按一定的规则组合成虚拟卷。
此外,处理器132可以是一个中央处理器CPU,或者可以是专用集成电路ASIC,或是被配置成实施本公开实施例的一个或多个集成电路。
本公开上述实施例在平整坡面或者挖掘沟面时,能有效控制坡面精度。
根据本公开的另一方面,提供一种挖掘机,包括如上述任一实施例(例如图12或图13实施例)所述挖掘机铲斗齿尖运动规划装置。
根据本公开的另一方面,提供一种计算机可读存储介质,其中,所述计算机可读存储介质存储有计算机指令,所述指令被处理器执行时实现如上述任一实施例(例如图1-图11任一实施例)所述挖掘机铲斗齿尖运动规划方法。
在本公开的一些实施例中,所述计算机可读存储介质可以为非瞬时性计算机可读存储介质。
本领域内的技术人员应明白,本公开的实施例可提供为方法、装置、或计算机程序产品。因此,本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(系统)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在上面所描述的挖掘机铲斗齿尖运动规划装置、模型建立模块、启发函数构建模块、运行路线规划模块、硬件约束模块和最优运动确定模块可以实现为用于执行本申请所描述功能的通用处理器、可编程逻辑控制器(PLC)、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件或者其任意适当组合。
至此,已经详细描述了本公开。为了避免遮蔽本公开的构思,没有描述本领域所公知的一些细节。本领域技术人员根据上面的描述,完全可以明白如何实施这里公开的技术方案。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成,也可以通过程序来指示相关的硬件完成,所述的程序可以存储于一种非瞬时性计算机可读存储介质中,上述提到的存储介质可以是只读存储器,磁盘或光盘等。
本公开的描述是为了示例和描述起见而给出的,而并不是无遗漏的或者将本公开限于所公开的形式。很多修改和变化对于本领域的普通技术人员而言是显然的。选择和描述实施例是为了更好说明本公开的原理和实际应用,并且使本领域的普通技术人员能够理解本公开从而设计适于特定用途的带有各种修改的各种实施例。
Claims (20)
1.一种挖掘机铲斗齿尖运动规划方法,包括:
建立挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束;
构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系;
在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
2.根据权利要求1所述的挖掘机铲斗齿尖运动规划方法,还包括:
根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束;
依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
3.根据权利要求2所述的挖掘机铲斗齿尖运动规划方法,其中,所述依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置包括:
根据挖机臂正运动学模型设定k时刻各关节初始速度;
在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值,其中,所述k+1时刻为k时刻的下一时刻;
计算k+1时刻各个关节的目标角度;
调用挖机臂正运动学模型计算k+1时刻计算出铲斗齿尖的位置;
判断k+1时刻铲斗齿尖是否运动到目标位置;
在k+1时刻铲斗齿尖运动到目标位置的情况下,控制各个关节k+1时刻停止运动。
4.根据权利要求3所述的挖掘机铲斗齿尖运动规划方法,其中,所述依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置还包括:
在k+1时刻铲斗齿尖未运动到目标位置的情况下,令k等于k+1,之后执行在角加速度约束、所述硬性约束函数及启发式函数的约束下确定k+1时刻的各个关节速度值的步骤。
5.根据权利要求1-4中任一项所述的挖掘机铲斗齿尖运动规划方法,其中,所述关节包括第一关节、第二关节、第三关节和第四关节,其中:
第一关节为挖掘机转台与挖掘机行走平台间的转动连接部位;
第二关节为挖掘机动臂与挖掘机转台间的铰接部位;
第三关节为挖掘机斗杆与挖掘机动臂间的铰接部位;
第四关节为挖掘机斗杆与挖掘机铲斗间的铰接部位。
6.根据权利要求5所述的挖掘机铲斗齿尖运动规划方法,其中,所述构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系包括:
构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖线速度之间的关系,其中,所述各关节角速度包括第二关节角速度、第三关节角速度和第四关节角速度。
7.根据权利要求1-4中任一项所述的挖掘机铲斗齿尖运动规划方法,其中,所述物理约束包括角度约束、角速度约束和角加速度约束中的至少一项。
8.根据权利要求2-4中任一项所述的挖掘机铲斗齿尖运动规划方法,其中:
所述硬性约束函数为第一比值、第二比值和第三比值均相等,其中,第一比值为初始位置x轴坐标和终点位置x轴坐标的差值与第一线速度的比值,第二比值为初始位置y轴坐标和终点位置y轴坐标的差值与第二线速度的比值,第三比值为初始位置z轴坐标和终点位置z轴坐标的差值与第三线速度的比值,第一线速度、第二线速度和第三线速度分别是挖掘机铲斗铲齿齿尖部位在笛卡尔空间内三个两两垂直方向的线速度。
9.根据权利要求1-4中任一项所述的挖掘机铲斗齿尖运动规划方法,其中:
平整面任务包括平整面挖掘和平整面修整两部分,其中,平整面挖掘包括平面挖掘和斜面挖掘,平整面修整包括平面修整和斜面修整;
平整面挖掘过程中铲斗姿态逐渐收敛到铲斗铲面和挖掘面平行,铲斗铲齿齿尖为直线运动;
平整面修整过程中铲斗姿态保持不变,铲斗铲齿齿尖为直线运动。
10.根据权利要求3或4所述的挖掘机铲斗齿尖运动规划方法,其中,所述确定k+1时刻的各个关节速度值包括:
选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
11.根据权利要求10所述的挖掘机铲斗齿尖运动规划方法,其中,所述选取在k+1时刻的各个关节速度值,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0包括:
设定预设速度范围,其中,所述预设速度范围为速度最小值到速度最大值的速度范围;
根据预定速度模型选取各个关节速度值;
构建第一比例积分微分PID结构,确定第k个时刻输入所述第一PID结构的误差,其中,所述第一PID结构的误差为当前铲斗铲齿齿尖的位置与最终位置的归一化距离;
第一PID结构输出第k个时刻的速度调节系数到预定速度模型;
根据速度调节系数与归一化距离、比例调节系数、积分调节系数和微分调节系数的对应关系,调节归一化距离、比例调节系数、积分调节系数和微分调节系数,使得挖掘机铲齿齿尖到达所述终点位置时各关节的速度均为0。
12.根据权利要求11所述的挖掘机铲斗齿尖运动规划方法,其中,所述根据预定速度模型选取各个关节速度值包括:
根据速度最小值、速度最大值和速度调节系数,选取各个关节速度值。
13.根据权利要求11所述的挖掘机铲斗齿尖运动规划方法,还包括:
构建第二PID结构,其中,所述第二PID结构,用于在所述各关节速度均分别为0时启动,并判断挖掘机铲齿齿尖是否到达所述终点位置。
14.根据权利要求13所述的挖掘机铲斗齿尖运动规划方法,其中:
所述第二PID结构的数据输入为所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置与实际给定的终点位置的归一化向量;
所述第二PID结构的数据输出为所述第一PID改变后的终点位置,其中,所述改变后的终点位置根据归一化向量、比例调节系数、积分调节系数和微分调节系数确定;
所述改变后的终点位置不能超出遇预定工作空间,当所述第一PID结构运行停止后的挖掘机铲斗铲齿齿尖的位置到达实际给定的终点位置的情况下,所述第二PID结构停止。
15.一种挖掘机铲斗齿尖运动规划装置,包括:
模型建立模块,被配置为挖掘机臂正运动学模型,基于所述挖掘机臂正运动学模型为挖掘机各关节建立物理约束;
启发函数构建模块,被配置为构建启发式函数,确定所述挖掘机各关节角速度与挖掘机铲斗铲齿齿尖运动之间的关系;
运行路线规划模块,被配置为在所述正运动学模型中规划出挖掘机铲斗铲齿齿尖部位的矢量运动路线。
16.根据权利要求15所述的挖掘机铲斗齿尖运动规划装置,还包括:
硬件约束模块,被配置为根据所述矢量运动路线,构建硬性约束函数对挖掘机铲斗铲齿齿尖部位的位置进行约束;
最优运动确定模块,被配置为依据所述挖掘机臂正运动学模型、物理约束、硬性约束函数和启发式函数求解出各个关节的最优运动,使得铲斗铲齿齿尖从初始位置直线运动到终点位置。
17.根据权利要求15或16所述的挖掘机铲斗齿尖运动规划装置,其中,所述挖掘机铲斗齿尖运动规划装置用于执行实现如权利要求1-14中任一项所述的挖掘机铲斗齿尖运动规划方法的操作。
18.一种挖掘机铲斗齿尖运动规划装置,包括:
存储器,被配置为存储指令;
处理器,被配置为执行所述指令,使得所述挖掘机铲斗齿尖运动规划装置执行实现如权利要求1-14中任一项所述挖掘机铲斗齿尖运动规划方法的操作。
19.一种挖掘机,包括如权利要求15-18中任一项所述挖掘机铲斗齿尖运动规划装置。
20.一种计算机可读存储介质,其中,所述计算机可读存储介质存储有计算机指令,所述指令被处理器执行时实现如权利要求1-14中任一项所述挖掘机铲斗齿尖运动规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211434362.2A CN115906466A (zh) | 2022-11-16 | 2022-11-16 | 挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211434362.2A CN115906466A (zh) | 2022-11-16 | 2022-11-16 | 挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115906466A true CN115906466A (zh) | 2023-04-04 |
Family
ID=86476025
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211434362.2A Pending CN115906466A (zh) | 2022-11-16 | 2022-11-16 | 挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115906466A (zh) |
-
2022
- 2022-11-16 CN CN202211434362.2A patent/CN115906466A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR101941147B1 (ko) | 암형 로봇의 장애물 자동 회피방법 및 제어장치 | |
JP6497953B2 (ja) | オフライン教示装置、オフライン教示方法及びロボットシステム | |
Kim et al. | A practical approach for minimum‐time trajectory planning for industrial robots | |
JP4137909B2 (ja) | ロボットプログラム補正装置 | |
US11707843B2 (en) | Initial reference generation for robot optimization motion planning | |
US7571027B2 (en) | Kinematic singular point compensation systems and methods | |
US20220198085A1 (en) | Method and system for designing block layouts for use in block placement during construction | |
CN101842755A (zh) | 逆运动学 | |
CN110722576A (zh) | 工业机器人铣削加工路径全局光顺方法及系统 | |
Dao et al. | High accuracy contouring control of an excavator for surface flattening tasks based on extended state observer and task coordinate frame approach | |
Makkonen et al. | A 3D model based control of an excavator | |
CN114411840B (zh) | 平地控制方法、装置和挖掘机 | |
JP6825026B2 (ja) | 情報処理装置、情報処理方法及びロボットシステム | |
JP2021059012A (ja) | 情報処理装置、情報処理方法及びロボットシステム | |
CN114055467B (zh) | 基于五自由度机器人的空间位姿在线仿真系统 | |
CN115906466A (zh) | 挖掘机及其铲斗齿尖运动规划系统和装置、存储介质 | |
JP7475841B2 (ja) | 情報処理方法、ロボットシステム、物品の製造方法、および情報処理装置 | |
Papp et al. | Navigation of differential drive mobile robot on predefined, software designed path | |
CN114800523B (zh) | 机械臂轨迹修正方法、系统、计算机及可读存储介质 | |
Yang et al. | On-line Cartesian trajectory control of mechanisms along complex curves | |
JP2015016536A (ja) | ロボット制御システム、ロボット、経路作成装置、経路作成方法及び経路作成プログラム | |
JP2001229407A (ja) | 数値解析用モデル作成装置、数値解析用モデル作成方法および記憶媒体 | |
JP7175433B1 (ja) | 加工面品位シミュレーション装置および加工面品位表示方法 | |
CN117128907A (zh) | 铲斗齿尖坐标估计方法、装置和系统、挖掘机和存储介质 | |
CN113867260B (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 |