CN114396091B - 一种基于gnss的推土机推土铲自动控制方法及装置 - Google Patents
一种基于gnss的推土机推土铲自动控制方法及装置 Download PDFInfo
- Publication number
- CN114396091B CN114396091B CN202210157046.9A CN202210157046A CN114396091B CN 114396091 B CN114396091 B CN 114396091B CN 202210157046 A CN202210157046 A CN 202210157046A CN 114396091 B CN114396091 B CN 114396091B
- Authority
- CN
- China
- Prior art keywords
- point
- blade
- hydraulic cylinder
- bulldozer
- current
- 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.)
- Active
Links
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/76—Graders, bulldozers, or the like with scraper plates or ploughshare-like elements; Levelling scarifying devices
- E02F3/80—Component parts
- E02F3/84—Drives or control devices therefor, e.g. hydraulic drive systems
- E02F3/844—Drives or control devices therefor, e.g. hydraulic drive systems for positioning the blade, e.g. hydraulically
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/76—Graders, bulldozers, or the like with scraper plates or ploughshare-like elements; Levelling scarifying devices
- E02F3/80—Component parts
- E02F3/84—Drives or control devices therefor, e.g. hydraulic drive systems
- E02F3/841—Devices for controlling and guiding the whole machine, e.g. by feeler elements and reference lines placed exteriorly of the machine
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Operation Control Of Excavators (AREA)
Abstract
本发明涉及数字化施工技术领域,公开了一种基于GNSS的推土机推土铲自动控制方法及装置,可以在推土机的机体顶端固定安装GNSS接收器以及在机体和推土铲分别固定安装惯性测量单元后,根据施工设计面的三维坐标信息以及获取的GNSS定位信息、机体姿态信息和推土铲姿态信息,推算得到左右两侧铲刀点的实时三维坐标以及倾斜液压缸和抬升液压缸的实时目标伸缩量,最后控制所述倾斜液压缸和所述抬升液压缸完成对应的目标伸缩量,使得铲刀位置控制方案能够趋向线性化,清晰地明确各个控制目标,进而可降低控制参数的整定难度,实现快、准和稳的控制目的,便于实际应用和推广。
Description
技术领域
本发明属于数字化施工技术领域,具体地涉及一种基于GNSS的推土机推土铲自动控制方法及装置。
背景技术
推土机是一种能够进行挖掘、运输和排弃岩土等施工作业的土方工程机械,可用于建设排土场、平整汽车排土场、堆集分散的矿岩、平整工作平盘和建筑场地等。为了提高土方整平效率,实现精准且昼夜施工,一键精平目标,又出现了基于推土铲自动控制技术的自动推土机,可利用全球定位***(Global Positioning System,GPS)和安装在工程机械上的传感器,实时掌握工程机械自身的位置、用于整平地面的铲刀及机械臂的状态和地面情况等数据,并在将作业指示数据传送到工程机械配备的控制盒后,通过导入测量/测绘信息,可达成辅助或无人施工的过程,实现数字化施工作业目的。
在现有用于推土平地的自动推土机产品中,大多数是使用激光传感器进行土方整平,存在激光传感器架设麻烦且复杂,平整精度随机械与激光发射器的距离增大而降低,无法对大面积土方进行整平作业以及机动性能差等缺陷。为此,虽然目前还有一些自动推土机产品可以采用GNSS(Global Navigation Satellite System,全球导航卫星***,是一种能在地球表面或近地空间的任何地点为用户提供全天候的3维坐标和速度以及时间信息的导航定位***)技术和控制技术实现土方整平,填补前述缺陷,但是由于是将GNSS接收器安装于推土铲上,无法量化推土铲的高程值与液压缸(即推土机的抬升液压缸和倾斜液压缸)的伸缩长度值的换算关系,使得在铲刀位置控制上存在非线性因素,导致控制目标不够清晰分明,增加了控制参数的整定难度。
发明内容
为了解决现有用于推土平地的自动推土机产品在铲刀位置控制上存在非线性因素及控制目标不够清晰分明的问题,本发明目的在于提供一种基于GNSS的推土机推土铲自动控制方法、装置、计算机设备及计算机可读存储介质,可使得铲刀位置控制方案能够趋向线性化,清晰地明确各个控制目标,进而可降低控制参数的整定难度,实现快、准和稳的控制目的。
第一方面,本发明提供了一种基于GNSS的推土机推土铲自动控制方法,包括:
获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端,所述定位信息包含有机体顶端安装点的三维坐标以及偏航角,所述第一惯性测量单元固定安装在所述推土机的机体上,所述第一姿态信息包含有所述机体的横滚姿态角以及俯仰姿态角,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含有所述推土铲的横滚姿态角以及俯仰姿态角;
根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点;
根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角;
根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标;
根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量;
根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
基于上述发明内容,可以在推土机的机体顶端固定安装GNSS接收器以及在机体和推土铲上分别固定安装惯性测量单元后,根据施工设计面的三维坐标信息以及获取的GNSS定位信息、机体姿态信息和推土铲姿态信息,推算得到左右两侧铲刀点的实时三维坐标以及倾斜液压缸和抬升液压缸的实时目标伸缩量,最后控制所述倾斜液压缸和所述抬升液压缸完成对应的目标伸缩量,使得铲刀位置控制方案能够趋向线性化,清晰地明确各个控制目标,进而可降低控制参数的整定难度,实现快、准和稳的控制目的,便于实际应用和推广。
在一个可能的设计中,根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标,包括:
根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角和顶推梁长度尺寸信息,推算出第二顶推梁链接点的三维坐标,其中,所述顶推梁长度尺寸信息包含有所述左侧顶推梁的长度和所述右侧顶推梁的长度,所述第二顶推梁链接点包括有所述左侧顶推梁与所述推土铲的铰接点和所述右侧顶推梁与所述推土铲的铰接点;
根据所述第二顶推梁链接点的三维坐标和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标,其中,所述铲体尺寸信息包含有所述第二顶推梁链接点与所述左侧铲刀点及所述右侧铲刀点的相对位置关系。
在一个可能的设计中,根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,包括:
根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到铲刀底边的当前相对水平面倾角值,并结合所述推土机的当前机体倾角值,计算得到所述推土铲相对机体的当前运动倾角值;
根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到所述左侧铲刀点及所述右侧铲刀点在所述施工设计面上的当前投影点坐标,然后根据两个所述当前投影点坐标计算得到所述铲刀底边的当前投影长度,最后结合所述左侧铲刀点及所述右侧铲刀点的当前高程,计算得到所述推土铲相对机体的目标运动倾角值;
将所述当前运动倾角值输入第一转换关系中,得到倾斜液压缸的当前伸缩长度值,以及将所述目标运动倾角值输入所述第一转换关系中,得到所述倾斜液压缸的目标伸缩长度值,其中,所述第一转换关系为预先确定的且关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系;
将所述倾斜液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述倾斜液压缸的倾斜目标伸缩量。
在一个可能的设计中,在根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量之前,所述方法还包括:
在倾斜液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且铲刀底边的相对水平面倾角值和所述推土机相对水平面的机体倾角值;
针对所述各个不同伸缩长度值,将对应的所述相对水平面倾角值与所述机体倾角值的差值,作为对应的且所述推土铲相对机体的运动倾角值;
根据所述各个不同伸缩长度值及对应的所述运动倾角值,采用仿真建模的形式获得第一转换关系,其中,所述第一转换关系是指关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系。
在一个可能的设计中,控制所述倾斜液压缸完成所述倾斜目标伸缩量,包括:
根据所述倾斜液压缸的开环传递函数,应用比例-积分-微分PID控制算法生成连续控制信号;
将所述连续控制信号传送至所述倾斜液压缸,以便所述倾斜液压缸根据所述连续控制信号进行伸缩调整,直到完成所述倾斜目标伸缩量。
在一个可能的设计中,根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,包括:
根据所述第一顶推梁链接点的当前三维坐标,计算得到第一关键点的当前高程,以及根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到第二关键点的当前高程,其中,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点;
根据所述第一关键点的当前高程、所述第二关键点的当前高程和关键点线段的长度,计算得到所述关键点线段相对水平面的当前夹角值,并结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的当前俯仰角度值,其中,所述关键点线段为连接所述第一关键点和所述第二关键点的线段;
根据所述施工设计面的三维坐标信息,获取所述施工设计面的设计高程,并结合所述第一关键点的当前高程和所述关键点线段的长度,计算得到所述关键点线段相对水平面的目标夹角值,再结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的目标俯仰角度值;
将所述当前俯仰角度值输入第二转换关系中,得到抬升液压缸的当前伸缩长度值,以及将所述目标俯仰角度值输入所述第二转换关系中,得到所述抬升液压缸的目标伸缩长度值,其中,所述第二转换关系为预先确定的且关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的模型运动关系,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸;
将所述抬升液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述抬升液压缸的抬升目标伸缩量。
在一个可能的设计中,在根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量之前,所述方法还包括:
在抬升液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,其中,所述抬升液压缸为所述推土机的左侧抬升液压缸或右侧抬升液压缸,所述关键点线段为连接第一关键点和第二关键点的线段,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点;
针对所述各个不同伸缩长度值,根据对应的且所述关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,计算得到对应的且所述推土铲相对机体的俯仰角度值;
根据所述各个不同伸缩长度值及对应的所述俯仰角度值,采用仿真建模的形式获得第二转换关系,其中,所述第二转换关系是指关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的模型运动关系。
第二方面,本发明提供了一种基于GNSS的推土机推土铲自动控制装置,包括有信息采集处理模块、三维坐标解算模块、相对姿态计算模块、三维坐标推算模块、推土铲倾斜控制模块和推土铲抬升控制模块;
所述信息采集处理模块,用于获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端,所述定位信息包含有机体顶端安装点的三维坐标以及偏航角,所述第一惯性测量单元固定安装在所述推土机的机体上,所述第一姿态信息包含有所述机体的横滚姿态角以及俯仰姿态角,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含有所述推土铲的横滚姿态角以及俯仰姿态角;
所述三维坐标解算模块,通信连接所述信息采集处理模块,用于根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点;
所述相对姿态计算模块,通信连接所述信息采集处理模块,用于根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角;
所述三维坐标推算模块,分别通信连接所述三维坐标解算模块和所述相对姿态计算模块,用于根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标;
所述推土铲倾斜控制模块,通信连接所述三维坐标推算模块,用于根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量;
所述推土铲抬升控制模块,通信连接所述三维坐标推算模块,用于根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
第三方面,本发明提供了一种计算机设备,包括有依次通信连接的存储器、处理器和收发器,其中,所述存储器用于存储计算机程序和配置相关文件,所述收发器用于收发信息,所述处理器用于读取所述计算机程序,执行如第一方面或第一方面中任意可能设计所述的推土机推土铲自动控制方法。
第四方面,本发明提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有指令,当所述指令在计算机上运行时,执行如上第一方面或第一方面中任意可能设计所述的推土机推土铲自动控制方法。
第五方面,本发明提供了一种包含指令的计算机程序产品,当所述指令在计算机上运行时,使所述计算机执行如上第一方面或第一方面中任意可能设计所述的推土机推土铲自动控制方法。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明提供的推土机推土铲自动控制方法的流程示意图。
图2是本发明提供的推土机推土铲自动控制***的结构示意图。
图3是本发明提供的推土机的立体结构示意图。
图4是本发明提供的推土机的侧视结构示意图。
图5是本发明提供的推土机与施工设计面的位置关系示意图。
图6是本发明提供的连续控制信号生成方法的流程示意图。
图7是本发明提供的基于PID控制算法的闭环传递***结构示意图。
图8是本发明提供的良好稳定性区在复平面中的示意图。
图9是本发明提供的基于前馈增益补偿的PID控制算法的控制回路示意图。
图10是本发明提供的在推土机三维运动仿真模型中抬升运动的关键点线段标识示意图。
图11是本发明提供的推土机推土铲自动控制装置的结构示意图。
图12是本发明提供的计算机设备的结构示意图。
上述附图中:1-机体;101-驾驶室;2-推土铲;31-左侧顶推梁;32-右侧顶推梁;4-倾斜液压缸;51-左侧抬升液压缸;52-右侧抬升液压缸。
具体实施方式
下面结合附图及具体实施例来对本发明作进一步阐述。在此需要说明的是,对于这些实施例方式的说明虽然是用于帮助理解本发明,但并不构成对本发明的限定。本文公开的特定结构和功能细节仅用于描述本发明示例的实施例。然而,可用很多备选的形式来体现本发明,并且不应当理解为本发明限制在本文阐述的实施例中。
应当理解,尽管本文可能使用术语第一和第二等等来描述各种对象,但是这些对象不应当受到这些术语的限制。这些术语仅用于区分一个对象和另一个对象。例如可以将第一对象称作第二对象,并且类似地可以将第二对象称作第一对象,同时不脱离本发明的示例实施例的范围。
应当理解,对于本文中可能出现的术语“和/或”,其仅仅是一种描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A、单独存在B或者同时存在A和B等三种情况;对于本文中可能出现的术语“/和”,其是描述另一种关联对象关系,表示可以存在两种关系,例如,A/和B,可以表示:单独存在A或者同时存在A和B等两种情况;另外,对于本文中可能出现的字符“/”,一般表示前后关联对象是一种“或”关系。
如图1~10所示,本实施例第一方面提供的所述基于GNSS的推土机推土铲自动控制方法,涉及三维坐标推算和液压控制等技术,可以但不限于由具有一定计算资源的计算机设备执行,例如由推土机控制盒、个人计算机(Personal Computer,PC,指一种大小、价格和性能适用于个人使用的多用途计算机;台式机、笔记本电脑到小型笔记本电脑和平板电脑以及超级本等都属于个人计算机)、智能手机、个人数字助理(Personal digitalassistant,PAD)或可穿戴设备等电子设备执行,以便在推土机的机体顶端固定安装GNSS接收器以及在机体和推土铲分别固定安装惯性测量单元后,根据施工设计面的三维坐标信息以及获取的GNSS定位信息、机体姿态信息和推土铲姿态信息,推算得到左右两侧铲刀点的实时三维坐标以及倾斜液压缸和抬升液压缸的实时目标伸缩量,最后控制所述倾斜液压缸和所述抬升液压缸完成对应的目标伸缩量,使得铲刀位置控制方案能够趋向线性化,清晰地明确各个控制目标,进而可降低控制参数的整定难度,实现快、准和稳的控制目的。如图1所示,所述推土机推土铲自动控制方法,可以但不限于包括有如下步骤S1~S6。
S1.获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端(例如驾驶室顶端),所述定位信息包含但不限于有机体顶端安装点的三维坐标以及偏航角等,所述第一惯性测量单元固定安装在所述推土机的机体(例如驾驶室)上,所述第一姿态信息包含但不限于有所述机体的横滚姿态角以及俯仰姿态角等,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含但不限于有所述推土铲的横滚姿态角以及俯仰姿态角等。
在所述步骤S1中,所述GNSS接收器为现有设备,可分别通过定位天线和定向天线来接收卫星信号,并根据卫星信号确定出所述机体顶端安装点在绝对坐标系(其为使所有坐标全部基于一个固定的坐标系原点的位置的描述的坐标***,其绝对坐标是一个固定的坐标位置,使用它输入的点坐标不会因参照物的不同而不同,后续所有的三维坐标均为在所述绝对坐标系下的绝对坐标)下的三维坐标(即平面坐标和高程值)以及偏航角(即机体轴在水平面上的投影与地理北向之间的夹角,可作为所述推土机的机体偏航角)。所述第一惯性测量单元和所述第二惯性测量单元均是一种测量物体三轴姿态角(或角速率)以及加速度的现有装置,一般配置有三个单轴的加速度计和三个单轴的陀螺,其中,加速度计检测物体在载体坐标***独立三轴的加速度信号,而陀螺检测载体相对于导航坐标系的角速度信号,测量物体在三维空间中的角速度和加速度,并以此解算出物体的姿态,因此可以采集获取对应载体(即所述机体或所述推土铲)的横滚姿态角以及俯仰姿态角。此外,所述机体是指在所述推土机中相对于所述推土铲等部件不动的车身部分,包括但不限于有驾驶室等部分;以及所述计算机设备可通过常规通信连接方式实时获取到所述定位信息、所述第一姿态信息和所述第二姿态信息;举例的,在如图2所示的推土机推土铲自动控制***中,由于推土机控制盒(即作为一种具体的计算机设备)是分别通信连接所述GNSS接收器、所述第一惯性测量单元和所述第二惯性测量单元的,因此可以实时获取到所述定位信息、所述第一姿态信息和所述第二姿态信息,以便对通信连接的倾斜液压缸和抬升液压缸进行实时控制。
S2.根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括但不限于有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点。
在所述步骤S2中,示例的,所述机体顶端安装点即为如图3和图4所示的S点,所述右侧顶推梁与所述机体的铰接点即为如图3和图4所示的C点,所述左侧顶推梁与所述机体的铰接点即为如图10所示的D点。所述机体顶端安装点与所述第一顶推梁链接点的相对位置关系为已知的机体尺寸信息,因此可以通过常规几何知识推算得到所述第一顶推梁链接点在所述绝对坐标系下的三维坐标。
S3.根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角。
在所述步骤S3中,同样可通过常规几何知识推算得到所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角。
S4.根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标。
在所述步骤S4中,示例的,所述左侧铲刀点即为如图3~5所示的L点,所述右侧铲刀点即为如图3~5所示的R点。具体的,可先根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角和顶推梁长度尺寸信息,推算出第二顶推梁链接点的三维坐标,其中,所述顶推梁长度尺寸信息包含但不限于有所述左侧顶推梁的长度和所述右侧顶推梁的长度,所述第二顶推梁链接点包括但不限于有所述左侧顶推梁与所述推土铲的铰接点和所述右侧顶推梁与所述推土铲的铰接点;然后根据所述第二顶推梁链接点的三维坐标和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标,其中,所述铲体尺寸信息包含但不限于有所述第二顶推梁链接点与所述左侧铲刀点及所述右侧铲刀点的相对位置关系。示例的,所述右侧顶推梁与所述推土铲的铰接点即为如图4所示的A点,所述左侧顶推梁与所述推土铲的铰接点即为如图10所示的B点,可通过常规几何知识推算得到所述第二顶推梁链接点的三维坐标,以及由于所述第二顶推梁链接点与所述左侧铲刀点及所述右侧铲刀点的相对位置关系为已知的铲体尺寸信息,因此可以结合所述第二姿态信息,通过常规几何知识推算得到所述左侧铲刀点及所述右侧铲刀点的三维坐标。
如图3和图4所示,考虑在推土铲倾斜运动过程中,驱动件为倾斜液压缸,该倾斜液压缸可通过调节伸缩长度值(即伸长或缩短的长度值),改变图4中HGA三角形(其中,H点表示所述右侧倾斜液压缸与所述推土铲的铰接点,G点表示所述右侧倾斜液压缸与所述右侧顶推梁的铰接点)的角度值,使得所述推土铲随着所述HGA三角形的角度变化发生左右倾斜,并在倾斜过程中,可配合抬升液压缸(其包括所述推土机的左侧抬升液压缸和右侧抬升液压缸)的长度变化,带动所述推土铲产生相对水平面不同的侧倾运动(由于抬升液压缸本身内部液压油恒定,因此倾斜变化左侧抬升液压缸和右侧抬升液压缸的伸缩长度等同,如左侧抬升液压缸做伸长5厘米的动作,则右侧抬升液压缸也做伸长5厘米的动作)。同时考虑在推土铲抬升运动过程中,驱动件为抬升液压缸,该抬升液压缸可通过调节伸缩长度值来改变横杆(即在所述机体前固定连接所述左侧顶推梁和所述右侧顶推梁的杆体)的抬升高度,进而实现所述推土铲的抬升运动。虽然抬升过程不受倾斜过程影响,但可以肯定的是,推土铲抬升均相对机体呈一定倾斜角度做抬升运动。由此基于前述运动分析可知,推土铲倾斜运动不会受到抬升控制运动的影响,而抬升控制运动会受到倾斜运动的影响,使得倾斜运动控制越精确,抬升运动控制的偏差量越小,所以在推土铲自动控制过程中,优选先计算并控制倾斜液压缸升缩至目标值,再计算并控制抬升液压缸升缩至目标值,即需先执行后续步骤S5,再执行后续步骤S6。
S5.根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量。
在所述步骤S5中,所述施工设计面可以是如图5所示的平面(此时所述施工设计面的三维坐标信息仅有高程值),也可以是斜坡面(此时所述施工设计面的三维坐标信息为斜面函数)。具体的,根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,包括但不限于有如下步骤S51~S54。
S51.根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到铲刀底边的当前相对水平面倾角值,并结合所述推土机的当前机体倾角值,计算得到所述推土铲相对机体的当前运动倾角值。
在所述步骤S51中,所述左侧铲刀点及所述右侧铲刀点的当前三维坐标即为根据前述步骤S1~S4而实时得到的对应三维坐标。如图5所示,所述铲刀底边即为连接所述左侧铲刀点L和所述右侧铲刀点R的线段LR,所述推土机的当前机体倾角值即为在实时而得的第一姿态信息中的机体横滚姿态角,因此可通过常规几何知识推算得到所述推土铲相对机体的当前运动倾角值。
S52.根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到所述左侧铲刀点及所述右侧铲刀点在所述施工设计面上的当前投影点坐标,然后根据两个所述当前投影点坐标计算得到所述铲刀底边的当前投影长度,最后结合所述左侧铲刀点及所述右侧铲刀点的当前高程,计算得到所述推土铲相对机体的目标运动倾角值。
在所述步骤S52中,所述推土铲相对机体的目标运动倾角值即为图5中的“γ”,可通过常规几何知识推算得到。
S53.将所述当前运动倾角值输入第一转换关系中,得到倾斜液压缸的当前伸缩长度值,以及将所述目标运动倾角值输入所述第一转换关系中,得到所述倾斜液压缸的目标伸缩长度值,其中,所述第一转换关系为预先确定的且关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系。
在所述步骤53中,所述第一转换关系需提前确定,即在根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量之前,所述方法还包括但不限于有如下步骤S501~S503。
S501.在倾斜液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且铲刀底边的相对水平面倾角值和所述推土机相对水平面的机体倾角值。
在所述步骤S501中,所述铲刀底边的相对水平面倾角值可参照前述步骤S51得到,所述推土机相对水平面的机体倾角值即为所述第一姿态信息中的机体横滚姿态角,于此不再赘述。
S502.针对所述各个不同伸缩长度值,将对应的所述相对水平面倾角值与所述机体倾角值的差值,作为对应的且所述推土铲相对机体的运动倾角值。
S503.根据所述各个不同伸缩长度值及对应的所述运动倾角值,采用仿真建模的形式获得第一转换关系,其中,所述第一转换关系是指关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系。
在所述步骤S503中,所述仿真建模的形式为现有的关系模型构建方式。
S54.将所述倾斜液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述倾斜液压缸的倾斜目标伸缩量。
在所述步骤S5中,具体的,控制所述倾斜液压缸完成所述倾斜目标伸缩量,包括但不限于有如下步骤S551~S552。
S551.根据所述倾斜液压缸的开环传递函数,应用比例-积分-微分PID控制算法生成连续控制信号。
在所述步骤S551中,所述倾斜液压缸可具体为采用与其匹配的比例阀进行控制的阀控液压缸,如此优选的,如图6所示,根据所述倾斜液压缸的开环传递函数,应用比例-积分-微分PID控制算法生成连续控制信号,包括但不限于有如下步骤S100~S500。
S100.针对所述倾斜液压缸的开环传递函数Ga(s),辨识得到对应的开环传递参数,其中,所述开环传递函数s表示复数变量,K表示开环增益,ζ表示阻尼比,ωn表示无阻尼自然频率,所述开环传递参数包含但不限于有所述开环增益K、所述阻尼比ζ和所述无阻尼自然频率ωn等。
在所述步骤S100中,考虑对于采用比例阀的阀控液压***,在选择与所述比例阀适应的控制信号频率后,为了便于后续能够精准确定出可保持闭环传递***稳定的闭环极点及PID参数,需要先分别辨识所述开环传递函数Ga(s)中的所述开环增益K、所述阻尼比ζ和所述无阻尼自然频率ωn等开环传递参数。具体的,针对所述倾斜液压缸的开环传递函数Ga(s),辨识得到对应的开环传递参数,包括但不限于有如下步骤S101~S104。
S102.采用预测误差最小化算法中的基于Levenberg-Marquardt非线性最小二乘算法,计算得到与所述倾斜液压缸的开环传递函数Ga(s)对应的开环传递参数预测值,其中,所述开环传递函数s表示复数变量,K表示开环增益,ζ表示阻尼比,ωn表示无阻尼自然频率,所述开环传递参数预测值包含但不限于有与所述开环增益K对应的预测值、与所述阻尼比ζ对应的预测值和与所述无阻尼自然频率ωn对应的预测值等。
在所述步骤S103中,s为复数变量,Ga(s)为开环传递函数,通过开环传递函数可得出其微分方程,根据控制***可计算出理论响应伸缩量,此部分为自动控制理论的基础知识,于此不再赘述。
S104.对所述理论响应伸缩量与所述实际响应伸缩量进行数据对比,若数据对比结果指示数据符合程度达到预设阈值,则将所述开环传递参数预测值作为与所述开环传递函数Ga(s)对应的开环传递参数的辨识结果,否则返回执行步骤S102,其中,所述开环传递参数包含但不限于有所述开环增益K、所述阻尼比ζ和所述无阻尼自然频率ωn等。
在所述步骤S104中,所述数据符合程度是指在采样时间间隔下理论计算的每个响应伸缩量与实际采集的对应响应伸缩量,相同的伸缩量个数与在采样时间下的总伸缩量个数之比;所述预设阈值可以具体为大于90%的小数,即只有在数据符合程度达到90%以上时,才可完成参数辨识。
S200.根据比例-积分-微分PID控制算法和所述开环传递函数Ga(s),推导得到所述倾斜液压缸的闭环传递函数。
在所述步骤S200中,所述PID(Proportion Integral Differential)控制算法是最经典的自动控制算法,即在PID控制***中,输出的控制信号必须根据控制对象的状态及时进行调整,以便维持控制对象状态的稳定,为此,必须及时掌握控制对象的当前控制误差、累计误差和下一刻变化趋势等信息,利用这三种信息,通过比例控制(P)、积分控制(I)和微分控制(D)来计算输出控制信号的大小。比例控制的数值越大,控制***的灵敏度越高,对误差的反应越强烈,使控制对象能够较快达到设定值,但也容易使***不稳定,反应过度;积分控制可以协同比例控制,加强控制效果,在控制对象的状态被稳定在设定值之后,误差为零,这时比例控制已经失效,但误差的积分值维持不变,凭借这点历史本钱,积分控制仍然可以产生一个稳定的输出控制信号,维持误差为零的控制效果;微分控制产生的控制输出专门用来对抗控制对象状态的激烈变化,产生超前的控制作用,有打预防针的功效。具体的,所述倾斜液压缸的闭环传递函数可基于如图7所示的闭环传递***结构进行常规推导得到,即为:
式中,Kp表示所述PID控制算法中的比例增益,TD表示所述PID控制算法中的微分时间常数。
S300.根据所述闭环传递函数中的分母多项式,得到闭环极点特征方程D(s)=0,其中,D(s)表示所述分母多项式。
S400.根据所述开环传递参数,对所述闭环极点特征方程D(s)=0进行求解,并整定得到所述PID控制算法的且使所有闭环极点的解位于复平面中负实轴上的PID参数,其中,所述PID参数包含但不限于有比例增益Kp和微分时间常数TD等。
在所述步骤S400中,根据经典控制理论可知,如果控制***的开环传递函数的极点有一个或多个分布在复平面的虚轴右侧,则该控制***的开环是不稳定的,既然***开环不稳定,那么,***闭环后是否稳定,这完全取决于闭环特征根的分布情况,一般建议将闭环极点放置在如图8所示的稳定性较好区域中,该区域的包围边界可基于如下分析来确定:(a)要***稳定,则全部闭环极点均应分布在复平面的虚轴左侧区域,而***稳定与否,与闭环零点的位置无关;(b)要***的快速性好,则闭环极点均应远离虚轴,以便使阶跃响应中的每个分量都衰减得更快;(c)要***的平稳性好,则共轭复数极点应位于β=±45°的等阻尼线上,其对应的阻尼系数(ξ=0.707)为最佳阻尼系数;(d)离虚轴最近的闭环极点对***的动态过程性能影响最大,起着决定性的主导作用,故称为主导极点。在工程上往往只用闭环主导极点去估算***的性能,即将***近似地看成是由共轭主导极点构成的二阶***或由实数主导极点组成的一阶***。因此如图8所示,阴影区域即是所述稳定性较好区域,应该将闭环极点放置在该区域中。为保证***的稳定性,闭环极点应位于所述稳定性较好区域的中心,这样一来,即使在负载发生变化时,极点位置会发生变化,但仍处于所述稳定性较好区域中,仍能保持***良好的稳定性。
针对所述闭环极点特征方程D(s)=0,如果难以将所有闭环极点(即所有闭环极点的解)保持在所述稳定性较好区域中,可优选将闭环极点保持接近负实轴,以实现出色的稳定性,即尝试将所有闭环极点放在-α处(α>0)(即构建如下方程:然后求解),如此一来,闭环极点的所有实部都将相等,并且以相同的速率衰减,保证了***的稳定性,进而根据之前辨识已得的所述开环传递参数,对所述闭环极点特征方程D(s)=0进行求解,并整定得到所述PID控制算法的且使所有闭环极点的解位于复平面中负实轴上的PID参数(即在根据所述开环传递参数确定出α后,进一步确定和),可保证闭环***的稳定性能。此外,若对闭环***的响应性能要求较高,还可以微调所述PID参数来达到目标控制效果。
在所述步骤S400中,还考虑单纯使用PID控制算法的局限性,在于比例环节需要一个误差来产生控制输出,而积分环节则需要误差和时间,用于控制比例阀的输出信号总是与负载的目标位置和实际位置的误差相关。在很多情况下,如果仅仅使用比例增益,误差需要足够大才能产生理想的控制信号,而增加一个积分环节会通过误差的积累使输出的控制信号增大,但是积分环节使输出信号逐渐增大需要时间。即便当积分环节积累了上升误差,在误差减小时也很可能会造成位置控制的超调或过冲。对此,所述PID控制算法优选采用基于前馈增益补偿的PID控制算法,如图9所示,前馈增益可用来满足增加的动态***响应,同时减小位置和速度误差,这会使得机器的控制性能变更高,同时由于使运行平顺稳定,机械寿命也得以增长。
详细的,前馈是使用S动作曲线来生成PID控制算法的信息。通常,高性能控制算法的控制原则是:控制算法生成目标运动曲线,之后由控制回路控制实际运动跟随目标运动曲线。目标运动曲线定时刷新,如每毫秒刷新一次。在每次PID控制刷新之前,目标生成算法会计算出预期的位置、速度和加速度等参数。因为控制算法事先就知道了目标速度和目标加速度,所以它不必等待PID对目标位置与实际位置误差的响应,就可以直接输出控制信号来满足对速度和加速度的要求。输出信号的强度由前馈增益决定,前馈增益是一个预测的参数。与PID不同的是,PID增益是与反馈误差相乘,前馈增益是预测的增益,其参数与目标速度和目标加速度分别相乘后再相加,以相加后的总和作为输出控制信号。前馈对输出信号的计算是基于如下公式进行的:前馈输出组成=Kv×目标速度+Ka×目标加速度,式中,Kv表示速度前馈增益,Ka表示加速度前馈增益。故而当所述PID控制算法采用基于前馈增益补偿的PID控制算法时,所述PID参数还需包含有速度前馈增益Kv和加速度前馈增益Ka。
此外,要确保***的稳定性,用于计算的反馈增益不能过大。通常,在不引起震荡和失稳的条件下,想单靠增大PID来把误差减小到可接受的水平是不可能的。从前馈中获得控制信号的好处在于,前馈不需要像PID控制那样依靠误差信号来生成控制信号。设计稳定和便于调试是***的关键,在于利用前馈来产生尽可能多的控制信号,同时尽量减小使用PID控制获得控制信号,这样结果的误差就会达到最小。
S500.在获取所述倾斜液压缸的倾斜目标伸缩量后,应用已导入所述PID参数的所述闭环传递函数,生成连续控制信号。
前述步骤S100~S400可在所述步骤S5之前完成,然后在将所述倾斜液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述倾斜液压缸的倾斜目标伸缩量后,直接应用已导入所述PID参数的所述闭环传递函数,生成所述连续控制信号,由此通过先针对倾斜液压缸的开环传递函数,辨识得到对应的开环传递参数,然后根据PID控制算法和所述开环传递函数,推导得到所述倾斜液压缸的闭环传递函数,再然后通过根据所述开环传递参数,对闭环极点特征方程进行求解,并整定得到所述PID控制算法的且使所有闭环极点的解位于复平面中负实轴上的PID参数,可保证闭环***的稳定性能,最终在得到目标伸缩量后,可应用已导入所述PID参数的所述闭环传递函数来生成连续控制信号,以便通过该连续控制信号来使所述倾斜液压缸快准稳地完成所述倾斜目标伸缩量,进一步降低控制参数的整定难度,实现在倾斜液压缸控制过程中达成快、准和稳的控制目的。
S552.将所述连续控制信号传送至所述倾斜液压缸,以便所述倾斜液压缸根据所述连续控制信号进行伸缩调整,直到完成所述倾斜目标伸缩量。
S6.根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
在所述步骤S6中,具体的,根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,包括但不限于有如下步骤S61~S65。
S61.根据所述第一顶推梁链接点的当前三维坐标,计算得到第一关键点的当前高程,以及根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到第二关键点的当前高程,其中,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点。
在所述步骤S61中,优选的,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的中间点,即为如图10所示的E点;所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的中间点,即为如图10所示的F点,可通过常规几何知识推算得到所述第一关键点及所述第二关键点的当前高程。
S62.根据所述第一关键点的当前高程、所述第二关键点的当前高程和关键点线段的长度,计算得到所述关键点线段相对水平面的当前夹角值,并结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的当前俯仰角度值,其中,所述关键点线段为连接所述第一关键点和所述第二关键点的线段。
在所述步骤S62中,优选的,所述关键点线段即为如图10所示的中点线段EF,可通过常规几何知识推算得到所述关键点线段相对水平面的当前夹角值。所述推土机的当前机体俯仰角即为在实时而得的第一姿态信息中的机体俯仰姿态角,因此可通过常规几何知识推算得到所述推土铲相对机体的当前俯仰角度值。
S63.根据所述施工设计面的三维坐标信息,获取所述施工设计面的设计高程,并结合所述第一关键点的当前高程和所述关键点线段的长度,计算得到所述关键点线段相对水平面的目标夹角值,再结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的目标俯仰角度值。
S64.将所述当前俯仰角度值输入第二转换关系中,得到抬升液压缸的当前伸缩长度值,以及将所述目标俯仰角度值输入所述第二转换关系中,得到所述抬升液压缸的目标伸缩长度值,其中,所述第二转换关系为预先确定的且关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的函数关系,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
在所述步骤S64中,所述第二转换关系也需提前确定,即在根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量之前,所述方法还包括但不限于有如下步骤S601~S603。
S601.在抬升液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,其中,所述抬升液压缸为所述推土机的左侧抬升液压缸或右侧抬升液压缸,所述关键点线段为连接第一关键点和第二关键点的线段,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点。
在所述步骤S601中,所述关键点线段相对水平面的夹角值可参照前述步骤S61~S62得到,于此不再赘述。所述推土机的机体俯仰角即为所述第一姿态信息中的机体俯仰姿态角。
S602.针对所述各个不同伸缩长度值,根据对应的且所述关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,计算得到对应的且所述推土铲相对机体的俯仰角度值。
S603.根据所述各个不同伸缩长度值及对应的所述俯仰角度值,采用仿真建模的形式获得第二转换关系,其中,所述第二转换关系是指关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的模型运动关系。
S65.将所述抬升液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述抬升液压缸的抬升目标伸缩量。
在所述步骤S6中,对于控制所述抬升液压缸完成所述抬升目标伸缩量,同样可以参照前述步骤S551~S552实现,于此不再赘述。
由此基于前述步骤S1~S6所描述的且基于GNSS的推土机推土铲自动控制方法,可以在推土机的机体顶端固定安装GNSS接收器以及在机体和推土铲上分别固定安装惯性测量单元后,根据施工设计面的三维坐标信息以及获取的GNSS定位信息、机体姿态信息和推土铲姿态信息,推算得到左右两侧铲刀点的实时三维坐标以及倾斜液压缸和抬升液压缸的实时目标伸缩量,最后控制所述倾斜液压缸和所述抬升液压缸完成对应的目标伸缩量,使得铲刀位置控制方案能够趋向线性化,清晰地明确各个控制目标,进而可降低控制参数的整定难度,实现快、准和稳的控制目的,便于实际应用和推广。
如图11所示,本实施例第二方面提供了一种实现第一方面所述的基于GNSS的推土机推土铲自动控制方法的虚拟装置,包括有信息采集处理模块、三维坐标解算模块、相对姿态计算模块、三维坐标推算模块、推土铲倾斜控制模块和推土铲抬升控制模块;
所述信息采集处理模块,用于获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端,所述定位信息包含有机体顶端安装点的三维坐标以及偏航角,所述第一惯性测量单元固定安装在所述推土机的机体上,所述第一姿态信息包含有所述机体的横滚姿态角以及俯仰姿态角,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含有所述推土铲的横滚姿态角以及俯仰姿态角;
所述三维坐标解算模块,通信连接所述信息采集处理模块,用于根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点;
所述相对姿态计算模块,通信连接所述信息采集处理模块,用于根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角;
所述三维坐标推算模块,分别通信连接所述三维坐标解算模块和所述相对姿态计算模块,用于根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标;
所述推土铲倾斜控制模块,通信连接所述三维坐标推算模块,用于根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量;
所述推土铲抬升控制模块,通信连接所述三维坐标推算模块,用于根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
本实施例第二方面提供的前述装置的工作过程、工作细节和技术效果,可以参见第一方面所述的推土机推土铲自动控制方法,于此不再赘述。
如图12所示,本实施例第三方面提供了一种执行第一方面所述基于GNSS的推土机推土铲自动控制方法的计算机设备,包括有依次通信连接的存储器、处理器和收发器,其中,所述存储器用于存储计算机程序和配置相关文件,所述收发器用于收发信息,所述处理器用于读取所述计算机程序,执行如第一方面所述基于GNSS的推土机推土铲自动控制方法。具体举例的,所述存储器可以但不限于包括随机存取存储器(Random-Access Memory,RAM)、只读存储器(Read-Only Memory,ROM)、闪存(Flash Memory)、先进先出存储器(FirstInput First Output,FIFO)和/或先进后出存储器(First Input Last Output,FILO)等等;所述处理器可以但不限于采用型号为STM32F105系列的微处理器。此外,所述计算机设备还可以但不限于包括有电源模块、显示屏和其它必要的部件。
本实施例第三方面提供的前述计算机设备的工作过程、工作细节和技术效果,可以参见第一方面所述的推土机推土铲自动控制方法,于此不再赘述。
本实施例第四方面提供了一种存储包含第一方面所述基于GNSS的推土机推土铲自动控制方法的指令的计算机可读存储介质,即所述计算机可读存储介质上存储有指令,当所述指令在计算机上运行时,执行如第一方面所述基于GNSS的推土机推土铲自动控制方法。其中,所述计算机可读存储介质是指存储数据的载体,可以但不限于包括软盘、光盘、硬盘、闪存、优盘和/或记忆棒(Memory Stick)等计算机可读存储介质,所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。
本实施例第四方面提供的前述计算机可读存储介质的工作过程、工作细节和技术效果,可以参见第一方面所述的推土机推土铲自动控制方法,于此不再赘述。
本实施例第五方面提供了一种包含指令的计算机程序产品,当所述指令在计算机上运行时,使所述计算机执行如第一方面所述基于GNSS的推土机推土铲自动控制方法。其中,所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。
最后应说明的是,本发明不局限于上述可选的实施方式,任何人在本发明的启示下都可得出其他各种形式的产品。上述具体实施方式不应理解成对本发明的保护范围的限制,本发明的保护范围应当以权利要求书中界定的为准,并且说明书可以用于解释权利要求书。
Claims (10)
1.一种基于GNSS的推土机推土铲自动控制方法,其特征在于,包括:
获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端,所述定位信息包含有机体顶端安装点的三维坐标以及偏航角,所述第一惯性测量单元固定安装在所述推土机的机体上,所述第一姿态信息包含有所述机体的横滚姿态角以及俯仰姿态角,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含有所述推土铲的横滚姿态角以及俯仰姿态角;
根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点;
根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角;
根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标;
根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量;
根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
2.如权利要求1所述的推土机推土铲自动控制方法,其特征在于,根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标,包括:
根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角和顶推梁长度尺寸信息,推算出第二顶推梁链接点的三维坐标,其中,所述顶推梁长度尺寸信息包含有所述左侧顶推梁的长度和所述右侧顶推梁的长度,所述第二顶推梁链接点包括有所述左侧顶推梁与所述推土铲的铰接点和所述右侧顶推梁与所述推土铲的铰接点;
根据所述第二顶推梁链接点的三维坐标和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标,其中,所述铲体尺寸信息包含有所述第二顶推梁链接点与所述左侧铲刀点及所述右侧铲刀点的相对位置关系。
3.如权利要求1所述的推土机推土铲自动控制方法,其特征在于,根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,包括:
根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到铲刀底边的当前相对水平面倾角值,并结合所述推土机的当前机体倾角值,计算得到所述推土铲相对机体的当前运动倾角值;
根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到所述左侧铲刀点及所述右侧铲刀点在所述施工设计面上的当前投影点坐标,然后根据两个所述当前投影点坐标计算得到所述铲刀底边的当前投影长度,最后结合所述左侧铲刀点及所述右侧铲刀点的当前高程,计算得到所述推土铲相对机体的目标运动倾角值;
将所述当前运动倾角值输入第一转换关系中,得到倾斜液压缸的当前伸缩长度值,以及将所述目标运动倾角值输入所述第一转换关系中,得到所述倾斜液压缸的目标伸缩长度值,其中,所述第一转换关系为预先确定的且关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系;
将所述倾斜液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述倾斜液压缸的倾斜目标伸缩量。
4.如权利要求3所述的推土机推土铲自动控制方法,其特征在于,在根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量之前,所述方法还包括:
在倾斜液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且铲刀底边的相对水平面倾角值和所述推土机相对水平面的机体倾角值;
针对所述各个不同伸缩长度值,将对应的所述相对水平面倾角值与所述机体倾角值的差值,作为对应的且所述推土铲相对机体的运动倾角值;
根据所述各个不同伸缩长度值及对应的所述运动倾角值,采用仿真建模的形式获得第一转换关系,其中,所述第一转换关系是指关于所述倾斜液压缸的伸缩长度值与所述推土铲相对机体的运动倾角值的模型运动关系。
5.如权利要求1所述的推土机推土铲自动控制方法,其特征在于,控制所述倾斜液压缸完成所述倾斜目标伸缩量,包括:
根据所述倾斜液压缸的开环传递函数,应用比例-积分-微分PID控制算法生成连续控制信号;
将所述连续控制信号传送至所述倾斜液压缸,以便所述倾斜液压缸根据所述连续控制信号进行伸缩调整,直到完成所述倾斜目标伸缩量。
6.如权利要求1所述的推土机推土铲自动控制方法,其特征在于,根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,包括:
根据所述第一顶推梁链接点的当前三维坐标,计算得到第一关键点的当前高程,以及根据所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到第二关键点的当前高程,其中,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点;
根据所述第一关键点的当前高程、所述第二关键点的当前高程和关键点线段的长度,计算得到所述关键点线段相对水平面的当前夹角值,并结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的当前俯仰角度值,其中,所述关键点线段为连接所述第一关键点和所述第二关键点的线段;
根据所述施工设计面的三维坐标信息,获取所述施工设计面的设计高程,并结合所述第一关键点的当前高程和所述关键点线段的长度,计算得到所述关键点线段相对水平面的目标夹角值,再结合所述推土机的当前机体俯仰角,计算得到所述推土铲相对机体的目标俯仰角度值;
将所述当前俯仰角度值输入第二转换关系中,得到抬升液压缸的当前伸缩长度值,以及将所述目标俯仰角度值输入所述第二转换关系中,得到所述抬升液压缸的目标伸缩长度值,其中,所述第二转换关系为预先确定的且关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的模型运动关系,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸;
将所述抬升液压缸的目标伸缩长度值与当前伸缩长度值的差值作为所述抬升液压缸的抬升目标伸缩量。
7.如权利要求6所述的推土机推土铲自动控制方法,其特征在于,在根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量之前,所述方法还包括:
在抬升液压缸处于各个不同伸缩长度值时,采集得到与所述各个不同伸缩长度值对应的且关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,其中,所述抬升液压缸为所述推土机的左侧抬升液压缸或右侧抬升液压缸,所述关键点线段为连接第一关键点和第二关键点的线段,所述第一关键点为位于所述第一顶推梁链接点中的左右两链接点之间的点,所述第二关键点为位于所述左侧铲刀点与所述右侧铲刀点之间的点;
针对所述各个不同伸缩长度值,根据对应的且所述关键点线段相对水平面的夹角值和所述推土机的机体俯仰角,计算得到对应的且所述推土铲相对机体的俯仰角度值;
根据所述各个不同伸缩长度值及对应的所述俯仰角度值,采用仿真建模的形式获得第二转换关系,其中,所述第二转换关系是指关于所述抬升液压缸的伸缩长度值与所述推土铲相对机体的俯仰角度值的模型运动关系。
8.一种基于GNSS的推土机推土铲自动控制装置,其特征在于,包括有信息采集处理模块、三维坐标解算模块、相对姿态计算模块、三维坐标推算模块、推土铲倾斜控制模块和推土铲抬升控制模块;
所述信息采集处理模块,用于获取来自全球导航卫星***GNSS接收器的定位信息、来自第一惯性测量单元的第一姿态信息和来自第二惯性测量单元的第二姿态信息,其中,所述GNSS接收器固定安装在推土机的机体顶端,所述定位信息包含有机体顶端安装点的三维坐标以及偏航角,所述第一惯性测量单元固定安装在所述推土机的机体上,所述第一姿态信息包含有所述机体的横滚姿态角以及俯仰姿态角,所述第二惯性测量单元固定安装在所述推土机的推土铲上,所述第二姿态信息包含有所述推土铲的横滚姿态角以及俯仰姿态角;
所述三维坐标解算模块,通信连接所述信息采集处理模块,用于根据所述定位信息、所述第一姿态信息和所述机体顶端安装点与第一顶推梁链接点的相对位置关系,解算出所述第一顶推梁链接点的三维坐标,其中,所述第一顶推梁链接点包括有所述推土机的左侧顶推梁与所述机体的铰接点和右侧顶推梁与所述机体的铰接点;
所述相对姿态计算模块,通信连接所述信息采集处理模块,用于根据所述第一姿态信息和所述第二姿态信息,计算出所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角;
所述三维坐标推算模块,分别通信连接所述三维坐标解算模块和所述相对姿态计算模块,用于根据所述第一顶推梁链接点的三维坐标、所述推土铲相对于所述机体的横滚姿态角以及俯仰姿态角、顶推梁长度尺寸信息和所述推土铲的铲体尺寸信息以及所述第二姿态信息,推算出所述推土铲的左侧铲刀点及右侧铲刀点的三维坐标;
所述推土铲倾斜控制模块,通信连接所述三维坐标推算模块,用于根据施工设计面的三维坐标信息和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到倾斜液压缸的倾斜目标伸缩量,并控制所述倾斜液压缸完成所述倾斜目标伸缩量;
所述推土铲抬升控制模块,通信连接所述三维坐标推算模块,用于根据所述施工设计面的三维坐标信息、所述第一顶推梁链接点的当前三维坐标和所述左侧铲刀点及所述右侧铲刀点的当前三维坐标,计算得到抬升液压缸的抬升目标伸缩量,并控制所述抬升液压缸完成所述抬升目标伸缩量,其中,所述抬升液压缸包括有所述推土机的左侧抬升液压缸和右侧抬升液压缸。
9.一种计算机设备,其特征在于,包括有依次通信连接的存储器、处理器和收发器,其中,所述存储器用于存储计算机程序和配置相关文件,所述收发器用于收发信息,所述处理器用于读取所述计算机程序,执行如权利要求1~7中任意一项所述的推土机推土铲自动控制方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有指令,当所述指令在计算机上运行时,执行如权利要求1~7中任意一项所述的推土机推土铲自动控制方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210157046.9A CN114396091B (zh) | 2022-02-21 | 2022-02-21 | 一种基于gnss的推土机推土铲自动控制方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210157046.9A CN114396091B (zh) | 2022-02-21 | 2022-02-21 | 一种基于gnss的推土机推土铲自动控制方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114396091A CN114396091A (zh) | 2022-04-26 |
CN114396091B true CN114396091B (zh) | 2022-12-06 |
Family
ID=81234380
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210157046.9A Active CN114396091B (zh) | 2022-02-21 | 2022-02-21 | 一种基于gnss的推土机推土铲自动控制方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114396091B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103119225A (zh) * | 2011-09-30 | 2013-05-22 | 株式会社小松制作所 | 推土铲控制***及建筑机械 |
CN107514022A (zh) * | 2017-07-07 | 2017-12-26 | 南京天辰礼达电子科技有限公司 | 一种推土机引导*** |
US10533301B1 (en) * | 2018-12-20 | 2020-01-14 | David Armas | GPS and laser grading control |
CN112031052A (zh) * | 2020-08-28 | 2020-12-04 | 山东四维卓识信息技术有限公司 | 推土机施工管控***及方法 |
CN112627261A (zh) * | 2020-11-19 | 2021-04-09 | 徐州徐工筑路机械有限公司 | 一种基于机器视觉的铲刀姿态控制***、方法及平地机 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8145391B2 (en) * | 2007-09-12 | 2012-03-27 | Topcon Positioning Systems, Inc. | Automatic blade control system with integrated global navigation satellite system and inertial sensors |
-
2022
- 2022-02-21 CN CN202210157046.9A patent/CN114396091B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103119225A (zh) * | 2011-09-30 | 2013-05-22 | 株式会社小松制作所 | 推土铲控制***及建筑机械 |
CN107514022A (zh) * | 2017-07-07 | 2017-12-26 | 南京天辰礼达电子科技有限公司 | 一种推土机引导*** |
US10533301B1 (en) * | 2018-12-20 | 2020-01-14 | David Armas | GPS and laser grading control |
CN112031052A (zh) * | 2020-08-28 | 2020-12-04 | 山东四维卓识信息技术有限公司 | 推土机施工管控***及方法 |
CN112627261A (zh) * | 2020-11-19 | 2021-04-09 | 徐州徐工筑路机械有限公司 | 一种基于机器视觉的铲刀姿态控制***、方法及平地机 |
Also Published As
Publication number | Publication date |
---|---|
CN114396091A (zh) | 2022-04-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3283841B1 (en) | Dynamic motion compensation of a working equiment, in particular a hydraulic excavator | |
US8571762B2 (en) | Real time method for determining the spatial pose of electronic mining shovels | |
US20220106773A1 (en) | Control system for construction machine, construction machine, and control method for construction machine | |
CN113107043A (zh) | 使用传感器融合控制机器的运动 | |
JP2009243262A (ja) | 地ならし機の動的安定化および制御 | |
US12000108B2 (en) | Control system for construction machine, construction machine, and control method for construction machine | |
CN110455290A (zh) | 一种智能液压挖掘机的最优轨迹规划方法 | |
CN114396091B (zh) | 一种基于gnss的推土机推土铲自动控制方法及装置 | |
CN111207742A (zh) | 一种附加外方位元素约束的采煤机定位定姿方法 | |
CN114541502B (zh) | 一种阀控液压缸控制方法及推土机推土铲自动控制*** | |
US20220106764A1 (en) | Control system for construction machine, construction machine, and control method for construction machine | |
JP2023083576A (ja) | 建設機械の制御システム、建設機械、及び建設機械の制御方法 | |
CN208384428U (zh) | 基于反向位移补偿的稳定转台*** | |
Wang et al. | Development of an agricultural vehicle levelling system based on rapid active levelling | |
CN112523286A (zh) | 一种装载机水平举升自动调节控制方法及*** | |
Li et al. | Finite-time event-triggered sliding mode predictive control of unmanned underwater vehicles without velocity measurements | |
US11898321B2 (en) | Input shaping for error detection and recovery in dynamically agile grading machines | |
CN109857146A (zh) | 一种基于前馈和权值分配的分层无人机跟踪控制方法 | |
CN114676901A (zh) | 平地机作业路径全局优化方法 | |
CN113670303A (zh) | 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法 | |
CN117739991B (zh) | 一种挖掘机最优作业轨迹规划方法、装置、设备及介质 | |
CN110779552B (zh) | 一种地球固联坐标系下的自适应对准方法 | |
JPH0313377B2 (zh) | ||
CN115718507A (zh) | 一种面向光电探测设备的目标跟踪控制方法 | |
CN118331318A (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |