CN114109246A - 岩土工程机械及其工作臂控制方法 - Google Patents

岩土工程机械及其工作臂控制方法 Download PDF

Info

Publication number
CN114109246A
CN114109246A CN202111432857.7A CN202111432857A CN114109246A CN 114109246 A CN114109246 A CN 114109246A CN 202111432857 A CN202111432857 A CN 202111432857A CN 114109246 A CN114109246 A CN 114109246A
Authority
CN
China
Prior art keywords
actual value
arm
vehicle body
pose
real
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.)
Granted
Application number
CN202111432857.7A
Other languages
English (en)
Other versions
CN114109246B (zh
Inventor
刘浩
黄建华
侯志强
周忠尚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu XCMG Construction Machinery Institute Co Ltd
Original Assignee
Jiangsu XCMG Construction Machinery Institute Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Jiangsu XCMG Construction Machinery Institute Co Ltd filed Critical Jiangsu XCMG Construction Machinery Institute Co Ltd
Priority to CN202111432857.7A priority Critical patent/CN114109246B/zh
Publication of CN114109246A publication Critical patent/CN114109246A/zh
Priority to SE2251370A priority patent/SE2251370A1/en
Application granted granted Critical
Publication of CN114109246B publication Critical patent/CN114109246B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B15/00—Supports for the drilling machine, e.g. derricks or masts
    • E21B15/04—Supports for the drilling machine, e.g. derricks or masts specially adapted for directional drilling, e.g. slant hole rigs
    • E21B15/045—Hydraulic, pneumatic or electric circuits for their positioning
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B7/00—Special methods or apparatus for drilling
    • E21B7/02—Drilling rigs characterised by means for land transport with their own drive, e.g. skid mounting or wheel mounting
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B19/00—Handling rods, casings, tubes or the like outside the borehole, e.g. in the derrick; Apparatus for feeding the rods or cables
    • E21B19/08—Apparatus for feeding the rods or cables; Apparatus for increasing or decreasing the pressure on the drilling tool; Apparatus for counterbalancing the weight of the rods
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B44/00—Automatic control systems specially adapted for drilling operations, i.e. self-operating systems which function to carry out or modify a drilling operation without intervention of a human operator, e.g. computer-controlled drilling systems; Systems specially adapted for monitoring a plurality of drilling variables or conditions
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B7/00—Special methods or apparatus for drilling
    • E21B7/04—Directional drilling
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21D—SHAFTS; TUNNELS; GALLERIES; LARGE UNDERGROUND CHAMBERS
    • E21D11/00—Lining tunnels, galleries or other underground cavities, e.g. large underground chambers; Linings therefor; Making such linings in situ, e.g. by assembling
    • E21D11/04—Lining with building materials
    • E21D11/10—Lining with building materials with concrete cast in situ; Shuttering also lost shutterings, e.g. made of blocks, of metal plates or other equipment adapted therefor
    • E21D11/105—Transport or application of concrete specially adapted for the lining of tunnels or galleries ; Backfilling the space between main building element and the surrounding rock, e.g. with concrete
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21D—SHAFTS; TUNNELS; GALLERIES; LARGE UNDERGROUND CHAMBERS
    • E21D20/00—Setting anchoring-bolts
    • E21D20/003—Machines for drilling anchor holes and setting anchor bolts
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/40—Control within particular dimensions
    • G05D1/49—Control of attitude, i.e. control of roll, pitch or yaw

Landscapes

  • Engineering & Computer Science (AREA)
  • Mining & Mineral Resources (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Physics & Mathematics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Fluid Mechanics (AREA)
  • Structural Engineering (AREA)
  • Mechanical Engineering (AREA)
  • Architecture (AREA)
  • Civil Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Operation Control Of Excavators (AREA)
  • Forklifts And Lifting Vehicles (AREA)
  • Working Measures On Existing Buildindgs (AREA)

Abstract

本公开提供一种岩土工程机械及其工作臂控制方法。岩土工程机械包括：车体；工作臂，相对于车体具有多个运动自由度；车体位姿检测系统，被配置为获取用于反映车体在岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息；工作臂位姿检测系统，被配置为获取用于反映工作臂相对于车体的实时位置和姿态的工作臂实时位姿信息；和位姿调整系统，包括控制装置和驱动装置，控制装置被配置为根据用于反映工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、车体实时位姿信息和工作臂实时位姿信息，向驱动装置发送用于调节工作臂的位置和姿态的控制信号，驱动装置被配置为根据控制信号驱动工作臂运动以使工作臂达到目标位姿。

Description

岩土工程机械及其工作臂控制方法
技术领域
本公开涉及工程机械领域，特别涉及一种岩土工程机械及其工作臂控制方法。
背景技术
凿岩台车是钻孔爆破岩石的主要设备之一，具有转场灵活、作业高效的优点，在铁路隧道和公路隧道钻爆法施工中被广泛应用。钻爆法施工时，为了获得最佳的爆破效果，需要对隧道掌子面的炮孔进行定位。根据发明人已知的相关技术，工程施工时，主要采用手动方式调整机械臂的位置和姿态，这种调整方式高度依赖操作人员的经验，难以保证对钻孔位置的高精度控制，由于机械臂的自由度较多，钻孔前每次定位需要操作人员多次调整机械臂，效率低下；并且由于一个断面上炮孔数量众多，多个孔的位置误差累积将导致钻孔结果与设计值有较大的偏差，最终导致隧道爆破效果较差，经常需要对爆破断面进行修补作业，大幅度地提高了施工成本。
发明内容
本公开的目的在于提供一种岩土工程机械及其工作臂控制方法，以提升岩土工程机械的施工精度和作业效率。
本公开第一方面提供一种岩土工程机械，包括：
车体；
工作臂，连接于所述车体且相对于所述车体具有多个运动自由度；
车体位姿检测系统，设置于所述车体上，被配置为获取用于反映所述车体在所述岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息；
工作臂位姿检测系统，设置于所述工作臂上，被配置为获取用于反映所述工作臂相对于所述车体的实时位置和姿态的工作臂实时位姿信息；和
位姿调整系统，包括控制装置和驱动装置，所述控制装置与所述车体位姿检测系统、所述工作臂位姿检测系统和所述驱动装置信号连接，所述控制装置被配置为根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、所述车体实时位姿信息和所述工作臂实时位姿信息，向所述驱动装置发送用于调节所述工作臂的位置和姿态的控制信号，所述驱动装置与所述工作臂驱动连接，所述驱动装置被配置为根据所述控制信号驱动所述工作臂运动以使所述工作臂达到目标位姿。
根据本公开的一些实施例，
所述车体位姿检测系统被配置为获取以下所述车体实时位姿信息至少之一：所述车体相对于所述作业空间的偏向角的第一实际值α1、所述车体相对于所述作业空间的俯仰角的第二实际值β1和所述车体相对于所述作业空间的滚转角的第三实际值γ1；和/或
所述工作臂包括第一臂节和推进梁，所述第一臂节的第一端连接于所述车体且相对于所述车体具有多个运动自由度，所述推进梁连接于所述第一臂节的第二端且相对于所述第一臂节具有多个运动自由度，所述工作臂位姿检测系统包括第一臂节姿态角检测装置和推进梁姿态角检测装置，所述第一臂节姿态角检测装置被配置为获取以下所述工作臂实时位姿信息至少之一：所述第一臂节相对于所述车体的偏向角的第四实际值α2、所述第一臂节对于所述车体的俯仰角的第五实际值β2和所述第一臂节相对于所述车体的滚转角的第六实际值γ2，所述推进梁姿态角检测装置被配置为获取以下所述工作臂实时位姿信息至少之一：所述推进梁相对于所述第一臂节的偏向角的第七实际值α3、所述推进梁相对于所述第一臂节的俯仰角的第八实际值β3和所述推进梁相对于所述第一臂节的滚转角的第九实际值γ3。
根据本公开的一些实施例，所述车体位姿检测系统包括：
棱镜组，包括设置于所述车体上的不同位置的多个棱镜，被配置为检测所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1；和/或
双轴倾角传感器，被配置为检测所述第二实际值β1和所述第三实际值γ1。
根据本公开的一些实施例，
所述第一臂节姿态角检测装置包括第一角度传感器、第二角度传感器和第三角度传感器，所述第一角度传感器被配置为检测所述第四实际值α2，所述第二角度传感器被配置为检测所述第五实际值β2，所述第三角度传感器被配置为检测所述第六实际值γ2；和/或
所述推进梁姿态角检测装置包括第四角度传感器、第五角度传感器和第六角度传感器，所述第四角度传感器被配置为检测所述第七实际值α3，所述第五角度传感器被配置为检测所述第八实际值β3，所述第六角度传感器被配置为检测所述第九实际值γ3。
根据本公开的一些实施例，所述控制装置被进一步配置为：根据所述第一实际值α1、所述第二实际值β1、所述第三实际值γ1、所述第四实际值α2、所述第五实际值β2、所述第六实际值γ2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3中至少之一获取所述推进梁相对于所述作业空间的俯仰角的第十实际值H和所述推进梁相对于所述作业空间的偏向角的第十一实际值V。
根据本公开的一些实施例，所述目标位姿信息包括所述推进梁相对于所述作业空间的俯仰角的第一目标值H0和所述推进梁相对于所述作业空间的偏向角的第二目标值V0，所述控制装置被进一步配置为：根据所述第十实际值H和所述第一目标值H0获取所述俯仰角的第一偏差ΔH，根据所述第十一实际值V和所述第二目标值V0获取所述偏向角的第二偏差ΔV，并根据所述第一偏差ΔH和所述第二偏差ΔV向所述驱动装置发送所述控制信号。
根据本公开的一些实施例，
所述第一臂节沿自身长度方向可伸缩地设置，所述工作臂位姿检测系统还包括第一臂节位移传感器，所述第一臂节位移传感器被配置为检测所述第一臂节的长度方向的第一端相对于长度方向的第二端的位移；和/或
所述推进梁沿自身长度方向可伸缩地设置，所述工作臂位姿检测系统还包括推进梁位移传感器，所述推进梁位移传感器被配置为检测所述推进梁的长度方向的第一端相对于长度方向的第二端的位移；和/或
所述工作臂还包括沿所述推进梁的长度方向可移动地设置于所述推进梁上的钻杆，所述工作臂位姿检测系统还包括钻杆位移传感器，所述钻杆位移传感器被配置为检测所述钻杆相对于所述推进梁沿所述推进梁的长度方向的位移。
根据本公开的一些实施例，所述驱动装置包括与所述推进梁驱动连接的第一驱动装置、第二驱动装置和第三驱动装置，所述第一驱动装置被配置为驱动所述推进梁相对于所述第一臂节绕第一轴线旋转，所述第二驱动装置被配置为驱动所述推进梁相对于所述第一臂节绕第二轴线旋转，所述第三驱动装置被配置为驱动所述推进梁相对于所述第一臂节绕第三轴线旋转，其中，所述第一轴线沿所述推进梁的高度方向延伸，所述第二轴线沿所述第一臂节的宽度方向延伸，所述第三轴线沿所述第一臂节的长度方向延伸。
根据本公开的一些实施例，所述驱动装置为液压驱动装置，所述位姿调整系统还包括与所述控制装置信号连接且与所述液压驱动装置通过液压管路连接的控制阀，所述控制阀被配置为根据所述控制装置发送的所述控制信号调节所述液压驱动装置中液压油的压力和/或流量以驱动所述工作臂运动。
根据本公开的一些实施例，所述岩土工程机械包括凿岩台车、锚杆台车或湿喷台车。
本公开第二方面提供一种岩土工程机械的工作臂控制方法，包括：
获取用于反映所述岩土工程机械的车体在所述岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息和用于反映所述岩土工程机械的工作臂在所述作业空间内的实时位置和姿态的工作臂实时位姿信息；和
根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、所述车体实时位姿信息和所述工作臂实时位姿信息，调节所述工作臂的位置和姿态以使所述工作臂达到目标位姿。
根据本公开的一些实施例，
获取所述车体实时位姿信息包括：获取所述车体在所述作业空间内的偏向角的第一实际值α1、所述车体在所述作业空间内的俯仰角的第二实际值β1和所述车体在所述作业空间内的滚转角的第三实际值γ1中至少之一；和/或
所述工作臂包括第一臂节和推进梁，所述第一臂节的第一端连接于所述车体且相对于所述车体具有多个运动自由度，所述推进梁连接于所述第一臂节的第二端且相对于所述第一臂节具有多个运动自由度，获取所述工作臂实时位姿信息包括：获取所述第一臂节相对于所述车体的偏向角的第四实际值α2、所述第一臂节相对于所述车体的俯仰角的第五实际值β2和所述第一臂节相对于所述车体的滚转角的第六实际值γ2、所述推进梁相对于所述第一臂节的偏向角的第七实际值α3、所述推进梁相对于所述第一臂节的俯仰角的第八实际值β3和所述推进梁相对于所述第一臂节的滚转角的第九实际值γ3中至少之一。
根据本公开的一些实施例，获取所述车体实时位姿信息包括：
提供棱镜组坐标信息，所述棱镜组坐标信息包括设置于所述车体上的不同位置的多个棱镜的坐标；
根据所述多个棱镜的坐标获取所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1。
根据本公开的一些实施例，
提供棱镜组坐标信息包括提供第一棱镜坐标(x1，y1，z1)和第二棱镜坐标(x2，y2，z2)；
根据以下对应关系获取所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1：
α1＝arctan((y2-y1)/(x2-x1))
β1＝arctan((z2-z1)/(x2-x1))
γ1＝arctan((z2-z1)/(y2-y1))。
根据本公开的一些实施例，获取所述工作臂实时位姿信息包括：根据所述第一实际值α1、所述第二实际值β1、所述第三实际值γ1、所述第四实际值α2、所述第五实际值β2、所述第六实际值γ2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3中至少之一获取所述推进梁相对于所述作业空间的俯仰角的第十实际值H和所述推进梁相对于所述作业空间的偏向角的第十一实际值V。
根据本公开的一些实施例，
根据所述第一实际值α1、所述第三实际值γ1、所述第四实际值α2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3和以下对应关系获取所述第十实际值H：
H＝-(α1+γ1+α2+α3×cosγ3+β3×sinγ3)；
根据所述第二实际值β1、所述第五实际值β2、所述第七实际值α3、所述第八实际值β3和所述第九实际值γ3和以下对应关系获取所述第十一实际值V：
V＝-(β1+β2+β3×cosγ3-α3×sinγ3)。
根据本公开的一些实施例，根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息与所述车体实时位姿信息和所述工作臂实时位姿信息的偏差，调节所述工作臂的位置和姿态以使所述工作臂达到所述目标位姿信息包括：
提供所述目标位姿信息，包括提供所述推进梁相对于所述作业空间的俯仰角的第一目标值H0和所述推进梁相对于所述作业空间的偏向角的第二目标值V0；
根据所述第十实际值H和所述第一目标值H0获取所述俯仰角的第一偏差ΔH，根据所述第十一实际值V相对于所述第二目标值V0获取所述偏向角的第二偏差ΔV；
根据所述第一偏差ΔH和所述第二偏差ΔV，驱动所述推进梁绕第一轴线、第二轴线和第三轴线旋转以调节所述推进梁的位置和姿态，直至所述第一偏差ΔH和所述第二偏差ΔV小于允许的范围，其中，所述第一轴线沿所述推进梁的高度方向延伸，所述第二轴线沿所述第一臂节的宽度方向延伸，所述第三轴线沿所述第一臂节的长度方向延伸。
本公开实施例的岩土工程机械及其工作臂控制方法中，可以通过车体位姿检测系统获取的车体实时位姿信息、工作臂位姿检测系统获取的工作臂实时位姿信息以及根据施工要求确定的工作臂的目标位姿信息，自动调整工作臂的实时位姿直至达到目标位姿，可以实现工作臂的随动控制，无需人工操作，利于提升岩土工程机械的施工精度和作业效率。
通过以下参照附图对本公开的示例性实施例的详细描述，本公开的其它特征及其优点将会变得清楚。
附图说明
此处所说明的附图用来提供对本公开的进一步理解，构成本申请的一部分，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
图1为本公开一些实施例的岩土工程机械的结构示意图。
图2为本公开一些实施例的车体位姿检测系统的结构示意图。
图3为本公开一些实施例的工作臂位姿检测系统的结构示意图。
图4和图5为本公开一些实施例的工作臂控制装置的工作原理示意图。
图6为本公开一些实施例的工作臂控制方法的流程示意图。
具体实施方式
下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本公开及其应用或使用的任何限制。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，这些技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中反映类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
在本公开的描述中，需要理解的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本公开保护范围的限制。
在本公开的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、“横向、竖向、垂直、水平”和“顶、底”等所指示的方位或位置关系通常是基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，在未作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有特定的方位或者以特定的方位构造和操作，因此不能理解为对本公开保护范围的限制；方位词“内、外”是指相对于各部件本身的轮廓的内外。
如图1至图6所示，本公开的实施例提供一种岩土工程机械及其工作臂控制方法。
岩土工程机械包括车体、工作臂、车体位姿检测系统、工作臂位姿检测系统和位姿调整系统，其中车体位姿检测系统、工作臂位姿检测系统和位姿调整系统组成工作臂控制装置。
工作臂连接于车体且相对于车体具有多个运动自由度。岩土工程机械可以包括一个或多个功能相同或不同的工作臂。
车体位姿检测系统设置于车体上，被配置为获取用于反映车体在岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息。
工作臂位姿检测系统设置于工作臂上，被配置为获取用于反映工作臂相对于车体的实时位置和姿态的工作臂实时位姿信息。
位姿调整系统包括控制装置和驱动装置。控制装置与车体位姿检测系统、工作臂位姿检测系统和驱动装置信号连接，控制装置被配置为根据用于反映工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、车体实时位姿信息和工作臂实时位姿信息，向驱动装置发送用于调节工作臂的位置和姿态的控制信号，驱动装置与工作臂驱动连接，驱动装置被配置为根据控制信号驱动工作臂运动以使工作臂达到目标位姿。
本公开中，岩土工程机械可以是凿岩台车、锚杆台车或湿喷台车等。例如，图1所示的实施例中，岩土工程机械为凿岩台车，车体包括底盘1、设置于行走底盘1上的驾驶室2、多个工作臂和连接于行走底盘1的支腿4。每个工作臂包括用于执行施工作业的作业装置。多个工作臂包括多个凿岩臂3A和一个平台臂3B，凿岩臂3A包括用于在施工作业面上钻孔的钻孔装置，通过调整工作臂的位姿达到目标位姿，可以使钻孔装置的位姿也相应地满足钻孔施工的要求。在一些未图示的实施例中，工作臂还可以湿喷台车的湿喷机械手等。
本公开实施例的岩土工程机械中，位姿调整系统可以通过车体位姿检测系统获取的车体实时位姿信息、工作臂位姿检测系统获取的工作臂实时位姿信息以及根据施工要求确定的工作臂的目标位姿信息，自动调整工作臂的实时位姿直至达到目标位姿，可以实现工作臂的随动控制，无需人工操作，利于提升岩土工程机械的施工精度和作业效率。
定义作业空间为隧道内的空间，定义以隧道的宽度方向为x轴、掘进方向为y轴、高度方向为z轴的作业空间坐标系Oxyz；定义以车体的宽度方向为xa轴、长度方向为ya轴、高度方向为za轴的车体坐标系Oxayaza；定义以第一臂节的宽度方向为xb轴、长度方向为yb轴、高度方向为zb轴的第一臂节坐标系Oxbybzb；定义以推进梁的宽度方向为xc轴、长度方向为yc轴、高度方向为zc轴的推进梁坐标系Oxcyczc。
基于上述定义，在以下描述中：
“车体相对于作业空间的偏向角”指的是车体坐标系Oxayaza的ya轴在作业空间坐标系Oxyz的xOy平面内的投影与作业空间坐标系Oxyz的y轴的夹角；
“车体相对于作业空间的俯仰角”指的是车体坐标系Oxayaza的ya轴与作业空间坐标系Oxyz的xOy平面的夹角；
“车体相对于作业空间的滚转角”指的是车体坐标系Oxayaza的za轴与垂直于作业空间坐标系Oxyz的xOy平面且包含车体坐标系Oxayaza的ya轴的平面的夹角；
“第一臂节311相对于车体的偏向角”指的是第一臂节坐标系Oxbybzb的yb轴在车体坐标系Oxayaza的xaOya平面内的投影与车体坐标系Oxayaza的ya轴的夹角；
“第一臂节311相对于车体的俯仰角”指的是第一臂节坐标系Oxbybzb的yb轴与车体坐标系Oxayaza的xaOya平面的夹角；
“第一臂节311相对于车体的滚转角”指的是第一臂节坐标系Oxbybzb的zb轴与垂直于车体坐标系Oxayaza的xaOya平面且包含第一臂节坐标系Oxbybzb的yb轴的平面的夹角；
“推进梁331相对于第一臂节311的偏向角”指的是推进梁坐标系Oxcyczc的yc轴在第一臂节坐标系Oxbybzb的xbOyb平面内的投影与第一臂节坐标系Oxbybzb的yb轴的夹角；
“推进梁331相对于第一臂节311的俯仰角”指的是推进梁坐标系Oxcyczc的yc轴与第一臂节坐标系Oxbybzb的xbOyb平面的夹角；
“推进梁331相对于第一臂节311的滚转角”指的是推进梁坐标系Oxcyczc的zc轴与垂直于第一臂节坐标系Oxbybzb的xbOyb平面且包含推进梁坐标系Oxcyczc的yc轴的平面的夹角。
“推进梁331相对于作业空间的俯仰角”指的是推进梁坐标系Oxcyczc的yc轴与作业空间坐标系Oxyz的xOy平面的夹角。
“推进梁331相对于作业空间的偏向角”指的是推进梁坐标系Oxcyczc的yc轴在作业空间坐标系Oxyz的xOy平面内的投影与作业空间坐标系Oxyz的y轴的夹角。
在一些实施例中，车体位姿检测系统被配置为获取以下车体实时位姿信息至少之一：车体相对于作业空间的偏向角的第一实际值α1、车体相对于作业空间的俯仰角的第二实际值β1和车体相对于作业空间的滚转角的第三实际值γ1。
在一些实施例中，工作臂包括第一臂节311和推进梁331，第一臂节311的第一端连接于车体且相对于车体具有多个运动自由度，推进梁331连接于第一臂节311的第二端且相对于第一臂节311具有多个运动自由度。例如，图3所示的实施例中，工作臂包括第一臂节311、旋转座312、第二臂节321、第一臂节基座322、推进梁331、推进梁基座332。第一臂节311与第二臂节321通过第一臂节基座322可动地连接，第一臂节311与推进梁通过旋转座312和推进梁基座332可动地连接。第一臂节311相对于底盘1分别具有绕沿底盘1的长度方向延伸的轴线、绕沿底盘1的宽度方向延伸的轴线和绕沿底盘1的高度方向延伸的轴线的转动自由度，推进梁331相对于第一臂节311分别具有绕沿第一臂节311的长度方向延伸的轴线、绕沿第一臂节311的宽度方向延伸的轴线和绕沿第一臂节311的高度方向延伸的轴线的的转动自由度。
以上实施例中，工作臂位姿检测系统包括第一臂节姿态角检测装置和推进梁姿态角检测装置。第一臂节姿态角检测装置被配置为获取以下工作臂实时位姿信息至少之一：第一臂节311相对于车体的偏向角的第四实际值α2、第一臂节311对于车体的俯仰角的第五实际值β2和第一臂节311相对于车体的滚转角的第六实际值γ2，推进梁姿态角检测装置被配置为获取以下工作臂实时位姿信息至少之一：推进梁331相对于第一臂节311的偏向角的第七实际值α3、推进梁331相对于第一臂节311的俯仰角的第八实际值β3和推进梁331相对于第一臂节311的滚转角的第九实际值γ3。
在一些实施例中，车体位姿检测系统包括棱镜组。棱镜组包括设置于车体上的不同位置的多个棱镜，被配置为检测第一实际值α1、第二实际值β1和第三实际值γ1。多个棱镜可以是双棱镜或三棱镜。例如，图2所示的实施例中，棱镜组包括沿底盘1的其中一条对角线间隔设置的第一棱镜51和第二棱镜52。
在一些未图示的实施例中，车体位姿检测系统也可以包括双轴倾角传感器。双轴倾角传感器被配置为检测第二实际值β1和第三实际值γ1。
在一些实施例中，如图3所示，第一臂节姿态角检测装置包括第一角度传感器61、第二角度传感器62和第三角度传感器63。第一角度传感器61被配置为检测第四实际值α2，第二角度传感器62被配置为检测第五实际值β2，第三角度传感器63被配置为检测第六实际值γ2。
在一些实施例中，如图3所示，推进梁姿态角检测装置包括第四角度传感器64、第五角度传感器65和第六角度传感器66。第四角度传感器64被配置为检测第七实际值α3，第五角度传感器65被配置为检测第八实际值β3，第六角度传感器66被配置为检测第九实际值γ3。
以凿岩台车为例，凿岩台车的凿岩臂在钻孔之前，需要根据孔的轴线的方向确定推进梁331在施工作业面上执行作业所需的位置和姿态，即相对于作业空间的位置和姿态。因此，可以通过车体位姿检测系统和工作臂位姿检测系统的各角度传感器检测的角度，获取推进梁331相对于作业空间的俯仰角的实际值和推进梁331相对于作业空间的偏向角的实际值，再结合推进梁331相对于作业空间的俯仰角的目标值和推进梁331相对于作业空间的偏向角的目标值，调节推进梁331的位姿，使推进梁331上钻进部件的轴线与孔的轴线对正，从而满足施工要求。
在一些实施例中，控制装置被进一步配置为：根据第一实际值α1、第二实际值β1、第三实际值γ1、第四实际值α2、第五实际值β2、第六实际值γ2、第七实际值α3、第八实际值β3、第九实际值γ3中至少之一获取推进梁331相对于作业空间的俯仰角的第十实际值H和推进梁331相对于作业空间的偏向角的第十一实际值V。
在一些实施例中，目标位姿信息包括推进梁331相对于作业空间的俯仰角的第一目标值H0和推进梁331相对于作业空间的偏向角的第二目标值V0，控制装置被进一步配置为：根据第十实际值H和第一目标值H0获取俯仰角的第一偏差ΔH，根据第十一实际值V和第二目标值V0获取偏向角的第二偏差ΔV，并根据第一偏差ΔH和第二偏差ΔV向驱动装置发送控制信号。
在施工作业面上钻孔时，上述实施例的岩土工程机械根据车体位姿检测系统和工作臂位姿检测系统的检测结果即可通过调整推进梁331的位姿直接满足施工要求，无须调节车体和工作臂的其他部分的位姿，利于进一步提升作业效率。
在上述实施例采用角度信息作为车体实时位姿信息和工作臂实时位姿信息的基础上，为了更好地满足施工精度和作业类型的要求，还可以进一步获取位移信息等其它信息作为车体实时位姿信息和工作臂实时位姿信息。
在一些实施例中，如图3所示，第一臂节311沿自身长度方向可伸缩地设置，工作臂位姿检测系统还包括第一臂节位移传感器71，第一臂节位移传感器71被配置为检测第一臂节311的长度方向的第一端相对于长度方向的第二端的位移。
在一些实施例中，如图3所示，推进梁331沿自身长度方向可伸缩地设置，工作臂位姿检测系统还包括推进梁位移传感器72，推进梁位移传感器72被配置为检测推进梁331的长度方向的第一端相对于长度方向的第二端的位移。
在一些实施例中，如图3所示，工作臂还包括沿推进梁331的长度方向可移动地设置于推进梁331上的钻杆34，工作臂位姿检测系统还包括钻杆位移传感器73，钻杆位移传感器73被配置为检测钻杆34相对于推进梁331沿推进梁331的长度方向的位移。
在一些实施例中，驱动装置包括与推进梁331驱动连接的第一驱动装置、第二驱动装置和第三驱动装置。第一驱动装置被配置为驱动推进梁331相对于第一臂节311绕第一轴线旋转，第二驱动装置被配置为驱动推进梁331相对于第一臂节311绕第二轴线旋转，第三驱动装置被配置为驱动推进梁331相对于第一臂节311绕第三轴线旋转，其中，第一轴线沿推进梁331的高度方向延伸，第二轴线沿第一臂节311的宽度方向延伸，第三轴线沿第一臂节311的长度方向延伸。上述实施例中，第一驱动装置、第二驱动装置和第三驱动装置既可以单独驱动推进梁331运动，也可以同时驱动推进梁331运动，实现推进梁331姿态的调整。
在一些实施例中，驱动装置为液压驱动装置，位姿调整系统还包括与控制装置信号连接且与液压驱动装置通过液压管路连接的控制阀，控制阀被配置为根据控制装置发送的控制信号调节液压驱动装置中液压油的压力和/或流量以驱动工作臂运动。
图4和图5示出了本公开一些实施例的工作臂控制装置的工作原理。
图4和图5所示的实施例中，液压驱动装置包括作为第一驱动装置的偏向驱动油缸、作为第二驱动装置的俯仰驱动油缸和作为第三驱动装置的滚转驱动油缸。相应地，控制阀包括与偏向驱动油缸通过液压管路连接的第一控制阀、与俯仰驱动油缸通过液压管路连接的第二控制阀和与滚转驱动油缸通过液压管路连接的第三控制阀。第一控制阀、第二控制阀和第三控制阀可以是比例阀。
图4和图5所示的实施例中，控制装置包括运动控制模块和信号调理转化模块，运动控制模块与信号调理转化模块信号连接，目标位姿信息可以预先存储于运动控制模块中。其中，信号调理转化模块将车体位姿检测系统采集的车体实时位姿信息和工作臂位姿检测系统采集的工作臂实时位姿信息转化为运动控制模块可以识别的信号，运动控制模块获取推进梁的实际位姿与目标位姿的偏差，信号调理转化模块再将运动控制模块获取的偏差转化为控制第一控制阀、第二控制阀和第三控制阀所需的比例信号，从而通过偏向驱动油缸、俯仰驱动油缸和滚转驱动油缸驱动推进梁运动。
在一些实施例中，在上面所描述的控制装置可以实现为用于执行本公开所描述功能的通用处理器、可编程逻辑控制器(Programmable Logic Controller，简称：PLC)、数字信号处理器(Digital Signal Processor，简称：DSP)、专用集成电路(ApplicationSpecific Integrated Circuit，简称：ASIC)、现场可编程门阵列(Field-ProgrammableGate Array，简称：FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件或者其任意适当组合。
本公开的一些实施例还提供一种岩土工程机械的工作臂控制方法，包括：获取用于反映岩土工程机械的车体在岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息和用于反映岩土工程机械的工作臂在作业空间内的实时位置和姿态的工作臂实时位姿信息；和根据用于反映工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、车体实时位姿信息和工作臂实时位姿信息，调节工作臂的位置和姿态以使工作臂达到目标位姿。
本公开实施例提供的工作臂控制方法可以基于本公开实施例提供的岩土工程机械实现。
本公开实施例提供的工作臂控制方法可以通过车体位姿检测系统获取的车体实时位姿信息、工作臂位姿检测系统获取的工作臂实时位姿信息以及根据施工要求确定的工作臂的目标位姿信息，自动调整工作臂的实时位姿直至达到目标位姿，可以实现工作臂的随动控制，无需人工操作，利于提升岩土工程机械的施工精度和作业效率。
在一些实施例中，获取车体实时位姿信息包括：获取车体在作业空间内的偏向角的第一实际值α1、车体在作业空间内的俯仰角的第二实际值β1和车体在作业空间内的滚转角的第三实际值γ1中至少之一。
在一些实施例中，工作臂包括第一臂节311和推进梁331，第一臂节311的第一端连接于车体且相对于车体具有多个运动自由度，推进梁331连接于第一臂节311的第二端且相对于第一臂节311具有多个运动自由度，获取工作臂实时位姿信息包括：获取第一臂节311相对于车体的偏向角的第四实际值α2、第一臂节311相对于车体的俯仰角的第五实际值β2和第一臂节311相对于车体的滚转角的第六实际值γ2、推进梁331相对于第一臂节311的偏向角的第七实际值α3、推进梁331相对于第一臂节311的俯仰角的第八实际值β3和推进梁331相对于第一臂节311的滚转角的第九实际值γ3中至少之一。
在一些实施例中，获取车体实时位姿信息包括：提供棱镜组坐标信息，棱镜组坐标信息包括设置于车体上的不同位置的多个棱镜的坐标；根据多个棱镜的坐标获取第一实际值α1、第二实际值β1和第三实际值γ1。
在一些实施例中，如图2所示，提供棱镜组坐标信息包括提供第一棱镜坐标(x1，y1，z1)和第二棱镜坐标(x2，y2，z2)；根据以下对应关系获取第一实际值α1、第二实际值β1和第三实际值γ1：
α1＝arctan((y2-y1)/(x2-x1))
β1＝arctan((z2-z1)/(x2-x1))
γ1＝arctan((z2-z1)/(y2-y1))
在一些实施例中，获取工作臂实时位姿信息包括：根据第一实际值α1、第二实际值β1、第三实际值γ1、第四实际值α2、第五实际值β2、第六实际值γ2、第七实际值α3、第八实际值β3、第九实际值γ3中至少之一获取推进梁331相对于作业空间的俯仰角的第十实际值H和推进梁331相对于作业空间的偏向角的第十一实际值V。
在一些实施例中，根据第一实际值α1、第三实际值γ1、第四实际值α2、第七实际值α3、第八实际值β3、第九实际值γ3和以下对应关系获取第十实际值H：
H＝-(α1+γ1+α2+α3×cosγ3+β3×sinγ3)；
根据第二实际值β1、第五实际值β2、第七实际值α3、第八实际值β3和第九实际值γ3和以下对应关系获取第十一实际值V：
V＝-(β1+β2+β3×cosγ3-α3×sinγ3)。
第十实际值H由多个环节的分量直接线性叠加得到，其中，α1为车体相对于作业空间的偏向角所贡献的分量，γ1为车体相对于作业空间的滚转角所贡献的分量，α2为第一臂节相对于车体的偏向角所贡献的分量，α3×cosγ3+β3×sinγ3为推进梁相对于第一臂节的偏向角、俯仰角和滚转角共同作用下所贡献的分量。
第十一实际值V由多个环节的分量直接线性叠加得到，其中，β1为车体相对于作业空间的俯仰角所贡献的分量，β2为第一臂节相对于车体的俯仰角所贡献的分量，β3×cosγ3-α3×sinγ3为推进梁相对于第一臂节的偏向角、俯仰角和滚转角共同作用下所贡献的分量。
在一些实施例中，根据用于反映工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息与车体实时位姿信息和工作臂实时位姿信息的偏差，调节工作臂的位置和姿态以使工作臂达到目标位姿信息包括：提供目标位姿信息，包括提供推进梁331相对于作业空间的俯仰角的第一目标值H0和推进梁331相对于作业空间的偏向角的第二目标值V0；根据第十实际值H和第一目标值H0获取俯仰角的第一偏差ΔH，根据第十一实际值V相对于第二目标值V0获取偏向角的第二偏差ΔV；根据第一偏差ΔH和第二偏差ΔV，驱动推进梁331绕第一轴线、第二轴线和第三轴线旋转以调节推进梁331的位置和姿态，直至第一偏差ΔH和第二偏差ΔV小于允许的范围，其中，第一轴线沿推进梁331的高度方向延伸，第二轴线沿第一臂节311的宽度方向延伸，第三轴线沿第一臂节311的长度方向延伸。
上述工作臂控制方法中各步骤的作用可参考前面岩土工程机械的工作臂工作装置的相关描述。
下面结合图6进一步说明本公开一些实施例的工作臂控制方法。图6所示的实施例中，工作臂为凿岩台车的凿岩臂，执行隧道掌子面上的炮孔的钻孔作业。
1、设定目标位姿信息。根据炮孔的轴线的方向确定推进梁331在施工作业面上执行作业所需的位置和姿态，并设定钻孔作业所需的目标位姿，包括推进梁331相对于作业空间的俯仰角的第一目标值H0和推进梁331相对于作业空间的偏向角的第二目标值V0。
2、获取车体实时位姿信息。通过棱镜组中第一棱镜坐标(x1，y1，z1)和第二棱镜坐标(x2，y2，z2)，获取第一实际值α1、第二实际值β1和第三实际值γ1：
α1＝arctan((y2-y1)/(x2-x1))
β1＝arctan((z2-z1)/(x2-x1))
γ1＝arctan((z2-z1)/(y2-y1))。
3、获取工作臂实时位姿信息。通过第一角度传感器61检测第四实际值α2，通过第二角度传感器62检测第五实际值β2，通过第三角度传感器63检测第六实际值γ2，通过第四角度传感器64检测第七实际值α3，通过第五角度传感器65检测第八实际值β3，通过第九角度传感器66检测第九实际值γ3，并根据第一实际值α1、第三实际值γ1、第四实际值α2、第七实际值α3、第八实际值β3、第九实际值γ3获取第十实际值H，根据第二实际值β1、第五实际值β2、第七实际值α3、第八实际值β3和第九实际值γ3获取第十一实际值V，其中：
H＝-(α1+γ1+α2+α3×cosγ3+β3×sinγ3)
V＝-(β1+β2+β3×cosγ3-α3×sinγ3)。
4、计算推进梁的实际位姿与目标位姿的偏差。根据第十实际值H和第一目标值H0获取俯仰角的第一偏差ΔH＝H0-H，根据第十一实际值V相对于第二目标值V0获取偏向角的第二偏差ΔV＝V0-V。
5、调节推进梁的位姿。根据第一偏差ΔH和第二偏差ΔV，求解推进梁331的偏向角的变化量Δα3、俯仰角的变化量Δβ3和滚转角的变化量Δγ3，求解Δα3、Δβ3和Δγ3时可以将其中一个作为定值，例如令Δα3＝0。驱动推进梁331绕第一轴线旋转Δα3、绕第二轴线旋转Δβ3、绕第三轴线旋转Δγ3，以调节推进梁331的位置和姿态，直至第一偏差ΔH和第二偏差ΔV小于允许的范围，钻杆34的轴线与孔的轴线对正。
上述实施例的工作臂控制方法可实现凿岩台车的凿岩臂的随动控制，利于提高钻孔作业的钻孔精度与钻孔效率。
最后应当说明的是：以上实施例仅用以说明本公开的技术方案而非对其限制；尽管参照较佳实施例对本公开进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本公开的具体实施方式进行修改或者对部分技术特征进行等同替换，其均应涵盖在本公开请求保护的技术方案范围当中。

Claims (17)

1.一种岩土工程机械，其特征在于，包括：
车体；
工作臂，连接于所述车体且相对于所述车体具有多个运动自由度；
车体位姿检测系统，设置于所述车体上，被配置为获取用于反映所述车体在所述岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息；
工作臂位姿检测系统，设置于所述工作臂上，被配置为获取用于反映所述工作臂相对于所述车体的实时位置和姿态的工作臂实时位姿信息；和
位姿调整系统，包括控制装置和驱动装置，所述控制装置与所述车体位姿检测系统、所述工作臂位姿检测系统和所述驱动装置信号连接，所述控制装置被配置为根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、所述车体实时位姿信息和所述工作臂实时位姿信息，向所述驱动装置发送用于调节所述工作臂的位置和姿态的控制信号，所述驱动装置与所述工作臂驱动连接，所述驱动装置被配置为根据所述控制信号驱动所述工作臂运动以使所述工作臂达到目标位姿。
2.根据权利要求1所述的岩土工程机械，其特征在于，
所述车体位姿检测系统被配置为获取以下所述车体实时位姿信息至少之一：所述车体相对于所述作业空间的偏向角的第一实际值α1、所述车体相对于所述作业空间的俯仰角的第二实际值β1和所述车体相对于所述作业空间的滚转角的第三实际值γ1；和/或
所述工作臂包括第一臂节(311)和推进梁(331)，所述第一臂节(311)的第一端连接于所述车体且相对于所述车体具有多个运动自由度，所述推进梁(331)连接于所述第一臂节(311)的第二端且相对于所述第一臂节(311)具有多个运动自由度，所述工作臂位姿检测系统包括第一臂节姿态角检测装置和推进梁姿态角检测装置，所述第一臂节姿态角检测装置被配置为获取以下所述工作臂实时位姿信息至少之一：所述第一臂节(311)相对于所述车体的偏向角的第四实际值α2、所述第一臂节(311)对于所述车体的俯仰角的第五实际值β2和所述第一臂节(311)相对于所述车体的滚转角的第六实际值γ2，所述推进梁姿态角检测装置被配置为获取以下所述工作臂实时位姿信息至少之一：所述推进梁(331)相对于所述第一臂节(311)的偏向角的第七实际值α3、所述推进梁(331)相对于所述第一臂节(311)的俯仰角的第八实际值β3和所述推进梁(331)相对于所述第一臂节(311)的滚转角的第九实际值γ3。
3.根据权利要求2所述的岩土工程机械，其特征在于，所述车体位姿检测系统包括：
棱镜组，包括设置于所述车体上的不同位置的多个棱镜，被配置为检测所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1；和/或
双轴倾角传感器，被配置为检测所述第二实际值β1和所述第三实际值γ1。
4.根据权利要求2所述的岩土工程机械，其特征在于，
所述第一臂节姿态角检测装置包括第一角度传感器(61)、第二角度传感器(62)和第三角度传感器(63)，所述第一角度传感器(61)被配置为检测所述第四实际值α2，所述第二角度传感器(62)被配置为检测所述第五实际值β2，所述第三角度传感器(63)被配置为检测所述第六实际值γ2；和/或
所述推进梁姿态角检测装置包括第四角度传感器(64)、第五角度传感器(65)和第六角度传感器(66)，所述第四角度传感器(64)被配置为检测所述第七实际值α3，所述第五角度传感器(65)被配置为检测所述第八实际值β3，所述第六角度传感器(66)被配置为检测所述第九实际值γ3。
5.根据权利要求2所述的岩土工程机械，其特征在于，所述控制装置被进一步配置为：根据所述第一实际值α1、所述第二实际值β1、所述第三实际值γ1、所述第四实际值α2、所述第五实际值β2、所述第六实际值γ2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3中至少之一获取所述推进梁(331)相对于所述作业空间的俯仰角的第十实际值H和所述推进梁(331)相对于所述作业空间的偏向角的第十一实际值V。
6.根据权利要求5所述的岩土工程机械，其特征在于，所述目标位姿信息包括所述推进梁(331)相对于所述作业空间的俯仰角的第一目标值H0和所述推进梁(331)相对于所述作业空间的偏向角的第二目标值V0，所述控制装置被进一步配置为：根据所述第十实际值H和所述第一目标值H0获取所述俯仰角的第一偏差ΔH，根据所述第十一实际值V和所述第二目标值V0获取所述偏向角的第二偏差ΔV，并根据所述第一偏差ΔH和所述第二偏差ΔV向所述驱动装置发送所述控制信号。
7.根据权利要求2所述的岩土工程机械，其特征在于，
所述第一臂节(311)沿自身长度方向可伸缩地设置，所述工作臂位姿检测系统还包括第一臂节位移传感器(71)，所述第一臂节位移传感器(71)被配置为检测所述第一臂节(311)的长度方向的第一端相对于长度方向的第二端的位移；和/或
所述推进梁(331)沿自身长度方向可伸缩地设置，所述工作臂位姿检测系统还包括推进梁位移传感器(72)，所述推进梁位移传感器(72)被配置为检测所述推进梁(331)的长度方向的第一端相对于长度方向的第二端的位移；和/或
所述工作臂还包括沿所述推进梁(331)的长度方向可移动地设置于所述推进梁(331)上的钻杆(34)，所述工作臂位姿检测系统还包括钻杆位移传感器(73)，所述钻杆位移传感器(73)被配置为检测所述钻杆(34)相对于所述推进梁(331)沿所述推进梁(331)的长度方向的位移。
8.根据权利要求2所述的岩土工程机械，其特征在于，所述驱动装置包括与所述推进梁(331)驱动连接的第一驱动装置、第二驱动装置和第三驱动装置，所述第一驱动装置被配置为驱动所述推进梁(331)相对于所述第一臂节(311)绕第一轴线旋转，所述第二驱动装置被配置为驱动所述推进梁(331)相对于所述第一臂节(311)绕第二轴线旋转，所述第三驱动装置被配置为驱动所述推进梁(331)相对于所述第一臂节(311)绕第三轴线旋转，其中，所述第一轴线沿所述推进梁(331)的高度方向延伸，所述第二轴线沿所述第一臂节(311)的宽度方向延伸，所述第三轴线沿所述第一臂节(311)的长度方向延伸。
9.根据权利要求1至8中任一项所述的岩土工程机械，其特征在于，所述驱动装置为液压驱动装置，所述位姿调整系统还包括与所述控制装置信号连接且与所述液压驱动装置通过液压管路连接的控制阀，所述控制阀被配置为根据所述控制装置发送的所述控制信号调节所述液压驱动装置中液压油的压力和/或流量以驱动所述工作臂运动。
10.根据权利要求1至8中任一项所述的岩土工程机械，其特征在于，所述岩土工程机械包括凿岩台车、锚杆台车或湿喷台车。
11.一种岩土工程机械的工作臂控制方法，包括：
获取用于反映所述岩土工程机械的车体在所述岩土工程机械的作业空间内的实时位置和姿态的车体实时位姿信息和用于反映所述岩土工程机械的工作臂在所述作业空间内的实时位置和姿态的工作臂实时位姿信息；和
根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息、所述车体实时位姿信息和所述工作臂实时位姿信息，调节所述工作臂的位置和姿态以使所述工作臂达到目标位姿。
12.根据权利要求11所述的工作臂控制方法，其特征在于，
获取所述车体实时位姿信息包括：获取所述车体在所述作业空间内的偏向角的第一实际值α1、所述车体在所述作业空间内的俯仰角的第二实际值β1和所述车体在所述作业空间内的滚转角的第三实际值γ1中至少之一；和/或
所述工作臂包括第一臂节(311)和推进梁(331)，所述第一臂节(311)的第一端连接于所述车体且相对于所述车体具有多个运动自由度，所述推进梁(331)连接于所述第一臂节(311)的第二端且相对于所述第一臂节(311)具有多个运动自由度，获取所述工作臂实时位姿信息包括：获取所述第一臂节(311)相对于所述车体的偏向角的第四实际值α2、所述第一臂节(311)相对于所述车体的俯仰角的第五实际值β2和所述第一臂节(311)相对于所述车体的滚转角的第六实际值γ2、所述推进梁(331)相对于所述第一臂节(311)的偏向角的第七实际值α3、所述推进梁(331)相对于所述第一臂节(311)的俯仰角的第八实际值β3和所述推进梁(331)相对于所述第一臂节(311)的滚转角的第九实际值γ3中至少之一。
13.根据权利要求12所述的工作臂控制方法，其特征在于，获取所述车体实时位姿信息包括：
提供棱镜组坐标信息，所述棱镜组坐标信息包括设置于所述车体上的不同位置的多个棱镜的坐标；
根据所述多个棱镜的坐标获取所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1。
14.根据权利要求13所述的工作臂控制方法，其特征在于，
提供棱镜组坐标信息包括提供第一棱镜坐标(x1，y1，z1)和第二棱镜坐标(x2，y2，z2)；
根据以下对应关系获取所述第一实际值α1、所述第二实际值β1和所述第三实际值γ1：
α1＝arctan((y2-y1)/(x2-x1))
β1＝arctan((z2-z1)/(x2-x1))
γ1＝arctan((z2-z1)/(y2-y1))。
15.根据权利要求12所述的工作臂控制方法，其特征在于，获取所述工作臂实时位姿信息包括：根据所述第一实际值α1、所述第二实际值β1、所述第三实际值γ1、所述第四实际值α2、所述第五实际值β2、所述第六实际值γ2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3中至少之一获取所述推进梁(331)相对于所述作业空间的俯仰角的第十实际值H和所述推进梁(331)相对于所述作业空间的偏向角的第十一实际值V。
16.根据权利要求15所述的工作臂控制方法，其特征在于，
根据所述第一实际值α1、所述第三实际值γ1、所述第四实际值α2、所述第七实际值α3、所述第八实际值β3、所述第九实际值γ3和以下对应关系获取所述第十实际值H：
H＝-(α1+γ1+α2+α3×cosγ3+β3×sinγ3)；
根据所述第二实际值β1、所述第五实际值β2、所述第七实际值α3、所述第八实际值β3和所述第九实际值γ3和以下对应关系获取所述第十一实际值V：
V＝-(β1+β2+β3×cosγ3-α3×sinγ3)。
17.根据权利要求15所述的工作臂控制方法，其特征在于，根据用于反映所述工作臂在施工作业面上执行作业所需的位置和姿态的目标位姿信息与所述车体实时位姿信息和所述工作臂实时位姿信息的偏差，调节所述工作臂的位置和姿态以使所述工作臂达到所述目标位姿信息，包括：
提供所述目标位姿信息，包括提供所述推进梁(331)相对于所述作业空间的俯仰角的第一目标值H0和所述推进梁(331)相对于所述作业空间的偏向角的第二目标值V0；
根据所述第十实际值H和所述第一目标值H0获取所述俯仰角的第一偏差ΔH，根据所述第十一实际值V相对于所述第二目标值V0获取所述偏向角的第二偏差ΔV；
根据所述第一偏差ΔH和所述第二偏差ΔV，驱动所述推进梁(331)绕第一轴线、第二轴线和第三轴线旋转以调节所述推进梁(331)的位置和姿态，直至所述第一偏差ΔH和所述第二偏差ΔV小于允许的范围，其中，所述第一轴线沿所述推进梁(331)的高度方向延伸，所述第二轴线沿所述第一臂节(311)的宽度方向延伸，所述第三轴线沿所述第一臂节(311)的长度方向延伸。
CN202111432857.7A 2021-11-29 2021-11-29 岩土工程机械及其工作臂控制方法 Active CN114109246B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202111432857.7A CN114109246B (zh) 2021-11-29 2021-11-29 岩土工程机械及其工作臂控制方法
SE2251370A SE2251370A1 (en) 2021-11-29 2022-11-23 Geotechnical engineering machine, and method of controlling a working arm thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111432857.7A CN114109246B (zh) 2021-11-29 2021-11-29 岩土工程机械及其工作臂控制方法

Publications (2)

Publication Number Publication Date
CN114109246A true CN114109246A (zh) 2022-03-01
CN114109246B CN114109246B (zh) 2024-02-02

Family

ID=80371324

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111432857.7A Active CN114109246B (zh) 2021-11-29 2021-11-29 岩土工程机械及其工作臂控制方法

Country Status (2)

Country Link
CN (1) CN114109246B (zh)
SE (1) SE2251370A1 (zh)

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10121507A (ja) * 1996-10-17 1998-05-12 Hitachi Constr Mach Co Ltd 建設機械の軌跡制御装置
CN105298387A (zh) * 2015-11-03 2016-02-03 中国铁建重工集团有限公司 凿岩台车
CN105745381A (zh) * 2015-10-30 2016-07-06 株式会社小松制作所 作业机械、以及作业机械的工作装置参数修正方法
US20160298316A1 (en) * 2015-03-27 2016-10-13 Komatsu Ltd. Calibration Device for Work Machine and Calibration Method of Working Equipment Parameter for Work Machine
CN108301766A (zh) * 2018-02-08 2018-07-20 北京科技大学 一种钻孔自动定位凿岩台车的双钻臂车体定位方法
CN109025815A (zh) * 2018-09-14 2018-12-18 北京奥腾岩石科技有限公司 用于凿岩台车的控制系统和控制方法
CN109732590A (zh) * 2018-12-20 2019-05-10 中国铁建重工集团有限公司 一种工程机械臂的在线标定方法
WO2020187947A1 (en) * 2019-03-19 2020-09-24 Sandvik Mining And Construction Oy Mine vehicle boom positioning control
CN112282781A (zh) * 2020-10-27 2021-01-29 上海米度测量技术有限公司 一种凿岩设备姿态测量系统和方法
CN112627799A (zh) * 2020-12-13 2021-04-09 江西鑫通机械制造有限公司 一种用于不平整工作面的自动钻孔的施工方法
CN113048972A (zh) * 2021-03-12 2021-06-29 衢州海西电子科技有限公司 矿山工程机械姿态位置确定方法及系统
CN113445907A (zh) * 2021-06-28 2021-09-28 江苏徐工工程机械研究院有限公司 凿岩台车钻孔方法、钻孔质量评价方法及系统、凿岩台车
CN113639688A (zh) * 2021-07-19 2021-11-12 江苏徐工工程机械研究院有限公司 一种凿岩臂架、凿岩台车及凿岩臂架传感器校准方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
PT2725184T (pt) * 2012-10-24 2019-06-12 Sandvik Mining & Construction Oy Aparelho de perfuração de rocha e método para controlar a orientação da viga de alimentação
SE542711C2 (en) * 2016-06-09 2020-06-30 Husqvarna Ab Improved arrangement and method for operating a hydraulically operated boom carrying a tool
CN107575161A (zh) * 2017-10-16 2018-01-12 湖南五新隧道智能装备股份有限公司 一种智能凿岩台车
CN113216842B (zh) * 2021-05-28 2022-05-17 中联重科股份有限公司 潜孔钻机控制方法及潜孔钻机控制装置

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10121507A (ja) * 1996-10-17 1998-05-12 Hitachi Constr Mach Co Ltd 建設機械の軌跡制御装置
US20160298316A1 (en) * 2015-03-27 2016-10-13 Komatsu Ltd. Calibration Device for Work Machine and Calibration Method of Working Equipment Parameter for Work Machine
CN105745381A (zh) * 2015-10-30 2016-07-06 株式会社小松制作所 作业机械、以及作业机械的工作装置参数修正方法
CN105298387A (zh) * 2015-11-03 2016-02-03 中国铁建重工集团有限公司 凿岩台车
CN108301766A (zh) * 2018-02-08 2018-07-20 北京科技大学 一种钻孔自动定位凿岩台车的双钻臂车体定位方法
CN109025815A (zh) * 2018-09-14 2018-12-18 北京奥腾岩石科技有限公司 用于凿岩台车的控制系统和控制方法
CN109732590A (zh) * 2018-12-20 2019-05-10 中国铁建重工集团有限公司 一种工程机械臂的在线标定方法
WO2020187947A1 (en) * 2019-03-19 2020-09-24 Sandvik Mining And Construction Oy Mine vehicle boom positioning control
CN112282781A (zh) * 2020-10-27 2021-01-29 上海米度测量技术有限公司 一种凿岩设备姿态测量系统和方法
CN112627799A (zh) * 2020-12-13 2021-04-09 江西鑫通机械制造有限公司 一种用于不平整工作面的自动钻孔的施工方法
CN113048972A (zh) * 2021-03-12 2021-06-29 衢州海西电子科技有限公司 矿山工程机械姿态位置确定方法及系统
CN113445907A (zh) * 2021-06-28 2021-09-28 江苏徐工工程机械研究院有限公司 凿岩台车钻孔方法、钻孔质量评价方法及系统、凿岩台车
CN113639688A (zh) * 2021-07-19 2021-11-12 江苏徐工工程机械研究院有限公司 一种凿岩臂架、凿岩台车及凿岩臂架传感器校准方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
何清华, 曾桂英: "凿岩机器人的车体定位", 中南工业大学学报(自然科学版), no. 03, pages 299 - 302 *
刘泽鑫;管会生;: "凿岩台车钻臂工作空间求解", 机械, no. 05, pages 9 - 11 *
吴昊骏;龚敏;赵振振;: "凿岩台车关节间隙影响定位精度的补偿设计与应用", 煤炭学报, vol. 44, no. 1, pages 378 - 386 *
杨洋;: "自动定位钻孔凿岩台车钻臂控制系统设计与试验", 煤矿机械, vol. 36, no. 10, pages 164 - 166 *

Also Published As

Publication number Publication date
SE2251370A1 (en) 2023-05-30
CN114109246B (zh) 2024-02-02

Similar Documents

Publication Publication Date Title
JP7014789B2 (ja) 可撓性アーム上に取り付けられたロボットアームの動的補償
US9663917B2 (en) Work vehicle, bucket device, and method for obtaining tilt angle
US9500079B2 (en) Method and control system for a mining vehicle and a mining vehicle
CN105298387A (zh) 凿岩台车
AU2019304101A1 (en) Active damping system
CN105736007A (zh) 融合地层信息的盾构机定位及纠偏系统及方法
US11040445B2 (en) Cartesian control of a boom tip of a large manipulator, in particular a concrete pump
CN109025815A (zh) 用于凿岩台车的控制系统和控制方法
JP6239356B2 (ja) トンネル掘削装置およびその制御方法
CN110455290B (zh) 一种智能液压挖掘机的最优轨迹规划方法
CN105239992A (zh) 钻孔方法
JPH0138957B2 (zh)
US20200232181A1 (en) Work machine and method for controlling the same
CN114109246B (zh) 岩土工程机械及其工作臂控制方法
JP7000132B2 (ja) 穿孔支援装置
CN108081270A (zh) 一种用于危险物品处理系统及控制方法
CN114135224B (zh) 岩土工程机械及其工作臂挠度补偿方法
RU2800704C1 (ru) Машина для инженерно-геологических работ и способ компенсации отклонения манипулятора указанной машины
CN109667539A (zh) 一种臂架装置和包含其的凿岩台车
JP2991603B2 (ja) 埋設管推進型掘進機及びその方向制御方法
JP2019100111A (ja) 建設機械の制御システム
CN115030750A (zh) 一种喷浆机械臂、控制系统及控制方法
RU2022130645A (ru) Машина для инженерно-геологических работ и способ управления ее манипулятором
CN113894790A (zh) 一种基于末端姿态约束的凿岩机器人钻臂运动控制方法
CN114991236A (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