CN113216842A - 潜孔钻机控制方法及潜孔钻机控制装置 - Google Patents

潜孔钻机控制方法及潜孔钻机控制装置 Download PDF

Info

Publication number
CN113216842A
CN113216842A CN202110595249.1A CN202110595249A CN113216842A CN 113216842 A CN113216842 A CN 113216842A CN 202110595249 A CN202110595249 A CN 202110595249A CN 113216842 A CN113216842 A CN 113216842A
Authority
CN
China
Prior art keywords
actual
expected
posture
vehicle body
hole drill
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
CN202110595249.1A
Other languages
English (en)
Other versions
CN113216842B (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.)
Zoomlion Heavy Industry Science and Technology Co Ltd
Original Assignee
Zoomlion Heavy Industry Science and Technology 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 Zoomlion Heavy Industry Science and Technology Co Ltd filed Critical Zoomlion Heavy Industry Science and Technology Co Ltd
Priority to CN202110595249.1A priority Critical patent/CN113216842B/zh
Publication of CN113216842A publication Critical patent/CN113216842A/zh
Application granted granted Critical
Publication of CN113216842B publication Critical patent/CN113216842B/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
    • E21B7/00—Special methods or apparatus for drilling
    • E21B7/04—Directional drilling
    • 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
    • E21B19/086—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 with a fluid-actuated cylinder
    • 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
    • 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
    • E21B44/02—Automatic control of the tool feed
    • E21B44/06—Automatic control of the tool feed in response to the flow or pressure of the motive fluid of the drive
    • 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
    • E21B7/022—Control of the drilling operation; Hydraulic or pneumatic means for activation or operation
    • 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
    • E21B7/024—Drilling rigs characterised by means for land transport with their own drive, e.g. skid mounting or wheel mounting having means for adapting to inclined terrain; having means for stabilizing the vehicle while drilling

Landscapes

  • Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Mining & Mineral Resources (AREA)
  • Physics & Mathematics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Fluid Mechanics (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Mechanical Engineering (AREA)
  • Earth Drilling (AREA)

Abstract

本发明公开一种潜孔钻机控制方法及潜孔钻机控制装置，该潜孔钻机控制方法包括：获取潜孔钻机的工作模式；获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的实际姿态和期望姿态是否一致，如果一致则结束，如果不一致则根据潜孔钻机的实际姿态和期望姿态差异计算执行机构调节参数值；根据调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态，并返回比较潜孔钻机的实际姿态和期望姿态是否一致的步骤。本潜孔钻机控制方法及潜孔钻机控制装置中，能根据实时地形、实时工作模式等情况自动调整潜孔钻机的姿态，大大提高了潜孔钻机的安全性，且对驾驶员要求较低。

Description

潜孔钻机控制方法及潜孔钻机控制装置
技术领域
本发明涉及地下施工设备技术领域，特别是涉及一种潜孔钻机控制方法及潜孔钻机控制装置。
背景技术
潜孔钻机是钻凿矿物和岩石的一种工程机械，是由冲击器潜入孔内直接冲击钻头而回转机构在孔外带动钻杆旋转向岩石钻进的设备，主要用于露天矿山开采、建筑基础、水利、电站、建材、交通及国防建设等多种工程中的凿岩钻孔；是凿岩机械的一种，目前钻凿炮孔作业广为使用的凿岩机械之一。常见的凿岩机，有钻孔深、钻孔直径大、钻孔效率高、适用范围广等特点，冲击、推进、回转和排渣是潜孔钻机凿岩工作的四个基本环节，其过程为由推进机构推进钻具依次包括钻杆、冲击器和钻头使钻头与孔底岩石紧密地接触由冲击器间歇冲击钻头，同时，回转机构使钻具连续回转。
潜孔钻机的工作环境复杂，安全性能要求较高，在行走和打孔掘进根据不同的地形规定钻机要保持相应的姿态。例如行走时推进梁要水平放置，车身要与水平面平行，特殊情况如斜面行走，需将大臂系统竖直定位，并将其尽可能靠近整机的中心位置来保持重心平衡；打孔掘进后需将推进梁抬升到安全距地位置。目前市面上潜孔钻机的姿态调整基本由驾驶员根据经验来实现，存在一定的安全隐患。
发明内容
本发明的目的是提供一种能根据实时地形实现自动调整姿态，提高潜孔钻机使用安全性的潜孔钻机控制方法及潜孔钻机控制装置。
本发明提供一种潜孔钻机控制方法，包括：
获取潜孔钻机的工作模式；
获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的所述实际姿态和所述期望姿态是否一致，如果一致则结束，如果不一致则根据潜孔钻机的所述实际姿态和所述期望姿态差异计算执行机构调节参数值；
根据所述调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态，并返回获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的所述实际姿态和所述期望姿态是否一致的步骤。
在其中一实施例中，所述潜孔钻机的工作模式包括行走模式和掘进模式，其中所述行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，所述水平面行走模式、所述上坡行走模式和所述下坡行走模式为常规行走模式；
当工作模式为所述常规行走模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，接收推进梁的期望姿态、车身期望姿态和推进梁期望距地距离，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述车身实际姿态和所述车身期望姿态是否一致，所述推进梁实际距地距离和所述推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致；当工作模式为所述横越斜坡行走模式时，获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，接收推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对实际姿态，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述推进梁实际距地距离和所述推进梁期望距地距离是否一致，所述推进梁与车身相对期望姿态与所述推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致；当工作模式为所述掘进模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态，接收推进梁的期望姿态、车身期望姿态，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述车身实际姿态和所述车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致。
在其中一实施例中，所述推进梁的期望姿态包括推进梁期望俯仰角pte和推进梁期望滚转角rte，所述推进梁期望距地距离包括推进梁期望距地值Le，所述车身期望姿态包括车身期望俯仰角pce，所述推进梁和车身相对期望姿态包括推进梁期望偏航角yte和车身期望偏航角yce的相对期望关系，潜孔钻机的所述推进梁的实际姿态包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，所述推进梁实际距地距离包括推进梁实际距地值Lr，所述车身实际姿态包括车身实际俯仰角pcr，所述推进梁和车身相对实际姿态包括推进梁实际偏航角ytr和车身实际偏航角ycr的相对实际关系；当工作模式为所述水平面行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述上坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述下坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述横越斜坡行走模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，yte＝ce，Le>0；当工作模式为所述掘进工作模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，pce＝0°。
在其中一实施例中，当工作模式为所述行走模式时，计算执行机构调节参数值的步骤具体包括计算控制推进梁姿态的调节参数值、计算控制推进梁距地距离的调节参数值和计算控制车身姿态的调节参数值；当工作模式为所述掘进模式时，计算执行机构调节参数值的步骤具体包括计算控制推进梁姿态的调节参数值和计算控制车身姿态的调节参数值。
在其中一实施例中，所述计算控制推进梁姿态的调节参数值具体包括：
接收推进梁期望角度，即推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，并获取推进梁实际角度，即推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并据此计算推进梁误差；
根据所述推进梁姿态误差计算推进梁期望角速度，即推进梁俯仰角速度ωtpr、推进梁滚转角速度ωtre、推进梁偏航角ωtye；
根据所述推进梁期望角速度和推进梁实际角速度计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1、推进梁摆动油缸驱动量Qtr、大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty；或者，
所述计算控制推进梁距地距离的调节参数值具体包括：
接收推进梁期望距地距离Le和推进梁实际距地距离Lr，并据此计算推进梁距地误差；
根据所述推进梁距离误差计算推进梁期望移动速度值ve；
根据所述推进梁期望移动速度值ve和推进梁实际移动速度值vr计算推进梁补偿液压油缸的驱动量QL；或者，
所述计算控制车身姿态的调节参数值具体包括：
接收车身期望角度，即车身期望俯仰角pce，并获取车身实际角度，即车身实际俯仰角pcr，并据此计算车身姿态误差；
根据所述车身姿态误差计算车身期望角速度ωcpe；
根据所述车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp。
在其中一实施例中，所述计算推进梁姿态误差为：
Figure BDA0003090685900000041
计算推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye具体为：
Figure BDA0003090685900000051
计算推进梁俯仰油缸驱动量Qtp，大臂俯仰油缸驱动量Qtp2，推进梁摆动油缸驱动量Qtr，大臂摆动油缸驱动量Qty具体为：
Figure BDA0003090685900000052
计算推进梁距地误差为：E(L)＝Le-Lr；
计算推进梁期望移动速度值ve具体为：
Figure BDA0003090685900000053
计算推进梁补偿液压油缸的驱动量QL具体为：
Figure BDA0003090685900000054
计算车身姿态误差为：
E(pc)＝pce-pcr；
计算车身期望角速度ωcpe具体为：
Figure BDA0003090685900000055
计算车架调平油缸驱动量Qcp具体为：
Figure BDA0003090685900000056
其中，kp1、kp2……kp11、ki1、ki2......ki11、kd1、kd2……kd11均为常数。
在其中一实施例中，所述执行元件包括推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸、推进梁补偿液压油缸、车架液压调平油缸；
所述根据调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态的步骤具体包括：根据所述推进梁俯仰油缸驱动量Qtp1控制所述推进梁俯仰液压油缸，根据所述推进梁摆动油缸驱动量Qtr控制所述推进梁摆动液压油缸，根据所述大臂俯仰油缸驱动量Qtp2控制所述大臂俯仰液压油缸，根据所述大臂俯仰油缸驱动量Qtp2控制所述大臂俯仰液压油缸，根据所述大臂摆动油缸驱动量Qty控制所述大臂摆动液压油缸，根据所述推进梁补偿液压油缸的驱动量QL控制所述推进梁补充液压油缸，根据所述车架调平油缸驱动量Qcp控制所述车架液压调平油缸。
本发明还提供一种潜孔钻机控制装置，包括潜孔钻机姿态获取装置和控制器；所述潜孔钻机姿态获取装置用于获取潜孔钻机的实际姿态；所述控制器内设有潜孔钻机期望姿态，所述控制器用于接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的实际姿态和期望姿态是否一致，如果不一致则根据潜孔钻机的实际姿态和期望姿态差异计算执行元件调节参数值，并将调节参数值发送给执行元件以调整潜孔钻机的姿态直到潜孔钻机的实际姿态和期望姿态一致。
在其中一实施例中，潜孔钻机的工作模式包括行走模式和掘进模式，其中所述行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，所述水平面行走模式、所述上坡行走模式和所述下坡行走模式为常规行走模式；
当工作模式为所述常规行走模式时，所述潜孔钻机姿态获取装置用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，所述控制器用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述车身实际姿态和车身期望姿态是否一致，所述推进梁实际距地距离和推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为所述横越斜坡行走模式时，所述潜孔钻机姿态获取装置用于获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，所述控制器用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述推进梁实际距地距离和推进梁期望距地距离是否一致，所述推进梁与车身相对期望姿态与推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为所述掘进模式时，所述潜孔钻机姿态获取装置用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态，所述控制器用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述车身实际姿态和车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致，所述推进梁的期望姿态、所述车身期望姿态、所述推进梁期望距地距离和所述推进梁与车身相对期望姿态预设在所述控制器内。
在其中一实施例中，所述潜孔钻机姿态获取装置包括推进梁倾角检测装置、推进梁角速度检测装置、测距装置、车身倾角检测装置和车身角速度检测装置；
所述控制器用于根据推进梁期望角度和推进梁实际角度计算推进梁误差，所述推进梁期望角度包括推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，所述推进梁实际角度包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并根据所述推进梁姿态误差计算推进梁期望角速度，即推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye，并根据所述推进梁期望角速度和推进梁实际角速度，即推进梁实际俯仰角速度ωtpr、推进梁实际滚转角速度ωtrr、推进梁实际偏航角ωtyr计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1、推进梁摆动油缸驱动量Qtr、大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty，其中，推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr由所述推进梁倾角检测装置测得，所述推进梁实际俯仰角速度ωtpr、所述推进梁实际滚转角速度ωtrr、所述推进梁实际偏航角ωtyr由所述推进梁角速度检测装置测得；
所述控制器还用于根据推进梁期望距地距离Le和推进梁实际距地距离Lr计算推进梁距地误差，并根据所述推进梁距离误差计算推进梁期望移动速度值ve，还根据所述推进梁期望移动速度值ve和推进梁实际移动速度值vr计算推进梁补偿液压油缸的驱动量QL，其中，所述推进梁实际距地距离Lr由所述测距装置测得，所述推进梁实际移动速度值vr由所述测距装置测得的所述推进梁实际距地距离Lr计算得到；
所述控制器还用于根据车身期望角度和车身实际角度计算车身姿态误差，所述车身期望角度包括车身期望俯仰角pce，所述车身实际角度包括车身实际俯仰角pcr，并根据车身姿态误差计算车身期望角速度ωcpe，还根据所述车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp，其中，所述车身实际俯仰角pcr由车身所述倾角检测装置测得，所述车身实际角速度ωcpr由所述车身角速度检测装置测得。
本发明实施例的潜孔钻机控制方法及潜孔钻机控制装置中，能根据实时地形、实时工作模式等情况自动调整潜孔钻机的姿态，大大提高了潜孔钻机的安全性，且对驾驶员要求较低。
附图说明
图1为本发明一实施例的潜孔钻机控制方法的流程图。
图2为图1中步骤S15的部分流程图。
图3为图1中步骤S15的另一部分流程图。
图4为图1中步骤S15的又一部分流程图。
图5为本发明一实施例的潜孔钻机控制装置的结构框图。
具体实施方式
为更进一步阐述本发明为达成预定发明目的所采取的技术方式及功效，以下结合附图及实施例，对本发明的具体实施方式、结构、特征及其功效，详细说明如后。
本发明一实施例的潜孔钻机控制方法用于控制潜孔钻机的工作，特别用于调整潜孔钻机的姿态。在本实施例中，设定潜孔钻机的前进方向为X轴，车身左右方向为Y轴，车身竖直方向为Z轴，且绕X轴旋转角度为滚转角r，绕Y轴旋转角度为俯仰角p，绕Z轴旋转角度为滚转角y。
如图1所示，本发明一实施例的潜孔钻机控制方法包括以下步骤：
S11，获取潜孔钻机的工作模式。具体地，潜孔钻机的工作模式包括行走模式和掘进模式，其中行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，水平面行走模式、上坡行走模式和下坡行走模式为常规行走模式。
具体地，工作模式可以是自动获取，也可以是人为输入。自动获取潜孔钻机的工作模式时，可根据车身俯仰角度pc确定，例如当pc＝0为水平面行走模式，当pc>0为上坡行走模式，当pc<0为下坡行走模式，一般来说，出于安全考虑，在横越斜坡行走模式和掘进模式时需要驾驶员根据工况人为选择工作模式，并且其优先级高于系统自动识别。
S13，获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的实际姿态和期望姿态是否一致，如果一致则结束，如果不一致则进入步骤S15。具体地，比较潜孔钻机的实际姿态和期望姿态是否一致时，一般可允许推进梁的实际姿态、推进梁实际距地距离和车身实际姿态与推进梁的期望姿态、推进梁期望距地距离和车身期望姿态之间有一定的差异，即有允许一个误差范围，当差异在允许的误差范围之类时，可判断为一致。
具体地，在步骤S13中，当工作模式为常规行走模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，接收推进梁的期望姿态、车身期望姿态和推进梁期望距地距离，并分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，车身实际姿态和车身期望姿态是否一致，推进梁实际距地距离和推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则进入步骤S15；当工作模式为横越斜坡行走模式时，获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，接收推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对实际姿态，并分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，推进梁实际距地距离和推进梁期望距地距离是否一致，推进梁与车身相对期望姿态与推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则进入步骤S15；当工作模式为掘进模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态，接收推进梁的期望姿态、车身期望姿态，并分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，车身实际姿态和车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则进入步骤S15。
具体地，推进梁的期望姿态包括推进梁期望俯仰角pte和推进梁期望滚转角rte，推进梁期望距地距离包括推进梁期望距地值Le，车身姿态包括车身期望俯仰角pce，推进梁和车身相对期望姿态包括推进梁期望偏航角yte和车身期望偏航角yce的相对期望关系。具体地，潜孔钻机的推进梁的实际姿态包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，推进梁实际距地距离包括推进梁实际距地值Lr，车身姿态包括车身实际俯仰角pcr，推进梁和车身相对实际姿态包括推进梁实际偏航角ytr和车身实际偏航角ycr的相对实际关系。
具体地，当工作模式为水平面行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为上坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为下坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为横越斜坡行走模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，yte＝yce，Le>0；当工作模式为掘进工作模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，pce＝0°。可以理解，各工作模式下的潜孔钻机的期望姿态可以预设在潜孔钻机内，也可以在对应工作模式下人为输入。另外，潜孔钻机的期望姿态下，推进梁期望俯仰角pte等参数值也可允许存在一定的误差范围，在此不做限制。
具体地，潜孔钻机上设有推进梁倾角仪、推进梁陀螺仪、激光测距传感器、车身倾角仪、车身陀螺仪。推进梁倾角仪、推进梁陀螺仪可安装于推进梁顶端，推进梁倾角仪用于测量推进梁的俯仰角pt、滚转角rt，偏航角yt，推进梁陀螺仪用于测量推进梁的俯仰角速度ωtp、滚转角速度ωtr、偏航角ωty，其中车身倾角仪、车身陀螺仪安装于车架上表面，车身倾角仪用于测量车体的俯仰角pc和车身偏航角yc，车身陀螺仪用于测量车身的俯仰角速度ωcp，激光测距传感器安装于推进梁底端位置，用于测量推进梁底面距离地面距离L。
S15，根据潜孔钻机的实际姿态和期望姿态差异计算执行元件调节参数值。
具体地，当工作模式为行走模式时，步骤S15具体包括计算控制推进梁姿态的调节参数值、计算控制推进梁距地距离的调节参数值和计算控制车身姿态的调节参数值。当工作模式为掘进模式时，步骤S15具体包括计算控制推进梁姿态的调节参数值和计算控制车身姿态的调节参数值。其中，推进梁姿态的调节参数值包括推进梁俯仰油缸驱动量Qtp1，推进梁摆动油缸驱动量Qtr，大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty，计算控制推进梁距地距离的调节参数值包括推进梁补偿油缸驱动量QL，计算控制车身姿态的调节参数值包括车架调平油缸驱动量Qcp。可以理解，当工作模式为掘进模式时，推进梁距地距离一般不会改变，因此可不控制推进梁的距地距离，对应地可不计算推挤梁距地距离的调节参数值，当然，当工作模式为掘进模式时，步骤S15也可包括计算控制推进梁距地距离的调节参数值。
请参图2，具体地，计算控制推进梁姿态的调节参数值具体包括：
S151，接收推进梁期望角度，即推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，并获取推进梁实际角度，即推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并据此计算推进梁误差。其中，推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr可由推进梁倾角仪测得。具体地，计算推进梁姿态误差为：
Figure BDA0003090685900000121
S153，根据推进梁姿态误差计算推进梁期望角速度，即推进梁期望俯仰角速度ωtpe、推进梁期望滚转角速度ωtre、推进梁期望偏航角速度ωtye。具体地，计算推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角速度ωtye具体为：
Figure BDA0003090685900000122
S155，根据推进梁期望角速度和推进梁实际角速度计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1，推进梁摆动油缸驱动量Qtr，大臂俯仰油缸驱动量Qtp2，大臂摆动油缸驱动量Qty，推进梁实际角速度包括推进梁实际俯仰角速度ωtpr、推进梁实际滚转角速度ωtrr、推进梁实际偏航角速度ωtyr。具体地，计算推进梁俯仰油缸驱动量Qtp1，推进梁摆动油缸驱动量Qtr，大臂俯仰油缸驱动量Qtp2，大臂摆动油缸驱动量Qty具体为：
Figure BDA0003090685900000123
请参图3，具体地，计算控制推进梁距地距离的调节参数值具体包括：
S157，接收推进梁期望距地距离Le和推进梁实际距地距离Lr，并据此计算推进梁距地误差。其中，推进梁实际距地距离Lr可由测距传感器测得。具体地，计算推进梁距地误差为：E(L)＝Le-Lr。
S158，根据推进梁距地误差计算推进梁期望移动速度值ve。具体地，计算推进梁期望移动速度值ve具体为：
Figure BDA0003090685900000131
S159，根据推进梁期望移动速度值ve和推进梁实际移动速度值vr计算推进梁补偿液压油缸的驱动量QL。其中，推进梁实际移动速度值vr可根据测得的推进梁实际距离距离Lr计算得到。具体地，计算推进梁补偿液压油缸的驱动量QL具体为：
Figure BDA0003090685900000132
请参图4，具体地，计算控制车身姿态的调节参数值具体包括：
S162，接收车身期望角度，即车身期望俯仰角pce，并获取车身实际角度，即车身实际俯仰角pcr，并据此计算车身姿态误差。其中，车身实际俯仰角pcr可由车身倾角仪测得。具体地，计算车身姿态误差为：
E(pc)＝pce-pcr。
S163，根据车身姿态误差计算车身期望角速度ωcpe。计算车身期望角速度ωcpe具体为：
Figure BDA0003090685900000133
S164，根据车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp。其中，车身实际角速度ωcpr可由车身陀螺仪测得。具体地，计算车架调平油缸驱动量Qcp具体为：
Figure BDA0003090685900000134
其中，kp1、kp2……kp11、ki1、ki2......ki11、kd1、kd2……kd11均为常数，可通过实验获得，一般来说，对于不同型号的设备，其值有所不同。
S 17，根据调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态，并返回步骤S13。具体地，执行元件为油缸，可通过控制执行元件的供油来调节执行元件的伸缩量。一般地，每个执行元件均连接比例液压阀，通过控制比例液压阀的输出给相应执行元件的供油。
当工作模式为常规行走模式时，执行元件包括推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸、推进梁补偿液压油缸和车架液压调平油缸；当工作模式为横越斜坡行走模式时，执行元件包括推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸、推进梁补偿液压油缸和车架液压调平油缸；当工作模式为掘进模式时，执行元件包括进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸和车架液压调平油缸。也就是说，推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸控制推进梁姿态，推进梁补偿液压油缸控制推进梁距地距离，车架液压调平油缸控制车身姿态。
具体地，步骤S17包括：
根据所述推进梁俯仰油缸驱动量Qtp1控制推进梁俯仰液压油缸，根据推进梁摆动油缸驱动量Qtr控制推进梁摆动液压油缸，根据大臂俯仰油缸驱动量Qtp2控制大臂俯仰液压油缸，根据大臂摆动油缸驱动量Qty控制大臂摆动液压油缸，根据进梁补偿液压油缸的驱动量QL控制推进梁补充液压油缸，根据车架调平油缸驱动量Qcp控制车架液压调平油缸，车架液压调平油缸可包括左车架液压调平油缸和右车架液压调平油缸，其油缸驱动量是相同的。
可以理解，在另一实施例中，也可采用其他方式计算调节参数值和控制执行元件。例如，预设调节参数值调整值，当潜孔钻机的实际姿态和期望姿态不一致时，根据调节参数值调整值调节执行元件状态，再比较潜孔钻机的实际姿态和期望姿态，然后再根据调节参数值调整值调节执行元件状态，直到潜孔钻机的实际姿态和期望姿态一致。例如，预设推进梁俯仰液压油缸的调节参数值调整值为x1，当推进梁实际俯仰角和推进梁期望俯仰角不一致时，将推进梁俯仰液压油缸的驱动量调整为Qtp当前+x1，然后再比较推进梁实际俯仰角和推进梁期望俯仰角，然后再将推进梁俯仰液压油缸的驱动量调整为Qtp当前+x1，直到推进梁实际俯仰角和推进梁期望俯仰角一致。本发明对执行元件调节参数值的计算方式不做限制。
请参照图5，本发明还提供一种潜孔钻机控制装置，用于控制潜孔钻机的姿态。一实施例的潜孔钻机控制装置包括潜孔钻机姿态获取装置20和控制器41。潜孔钻机姿态获取装置20用于获取潜孔钻机的实际姿态。控制器41内设有潜孔钻机期望姿态，控制器41用于接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的实际姿态和期望姿态是否一致，如果不一致则根据潜孔钻机的实际姿态和期望姿态差异计算执行元件调节参数值，并将调节参数值发送给执行元件以调整潜孔钻机的姿态直到潜孔钻机的实际姿态和期望姿态一致。
本实施例中，潜孔钻机姿态获取装置20包括推进梁倾角检测装置21、推进梁角速度检测装置23、测距装置25、车身倾角检测装置27和车身角速度检测装置29。推进梁倾角检测装置21用于获取推进梁的俯仰角pt、滚转角rt，偏航角yt，推进梁角速度检测装置23用于获取推进梁的俯仰角速度ωtp、滚转角速度ωtr、偏航角速度ωty，测距装置25用于获取推进梁的距地距离L，车身倾角检测装置27用于获取车身俯仰角pc、偏航角yc，车身角速度检测装置29用于获取车身的俯仰角速度ωcp。具体地，推进梁倾角检测装置21可为推进梁倾角仪，推进梁角速度检测装置23可为推进梁陀螺仪，测距装置25可为测距传感器，例如激光测距传感器，车身倾角检测装置27可为车身倾角仪，车身角速度检测装置29可为车身陀螺仪。具体地，推进梁倾角仪、推进梁陀螺仪可安装于推进梁顶端，激光测距传感器可安装于推进梁底端位置，车身倾角仪、车身陀螺仪可安装于车架上表面。
潜孔钻机的工作模式包括行走模式和掘进模式，其中行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，水平面行走模式、上坡行走模式和下坡行走模式为常规行走模式。
当工作模式为常规行走模式时，潜孔钻机姿态获取装置20用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，控制器41用于分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，车身实际姿态和车身期望姿态是否一致，推进梁实际距地距离和推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为横越斜坡行走模式时，潜孔钻机姿态获取装置20用于获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，控制器41用于分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，推进梁实际距地距离和推进梁期望距地距离是否一致，推进梁与车身相对期望姿态与推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为掘进模式时，潜孔钻机姿态获取装置20用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态，控制器41用于分别比较推进梁的实际姿态与推进梁的期望姿态是否一致，车身实际姿态和车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致。推进梁的期望姿态、车身期望姿态、推进梁期望距地距离和推进梁与车身相对期望姿态可预设在控制器41内。
具体地，推进梁的期望姿态包括推进梁期望俯仰角pte和推进梁期望滚转角rte，推进梁期望距地距离包括推进梁期望距地值Le，车身姿态包括车身期望俯仰角pce，推进梁和车身相对期望姿态包括推进梁期望偏航角yte和车身期望偏航角yce的相对期望关系。潜孔钻机的推进梁的实际姿态包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，推进梁实际距地距离包括推进梁实际距地值Lr，车身姿态包括车身实际俯仰角pcr，推进梁和车身相对实际姿态包括推进梁实际偏航角ytr和车身实际偏航角ycr的相对实际关系。
具体地，当工作模式为水平面行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为上坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为下坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为横越斜坡行走模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，yte＝yce，Le>0；当工作模式为掘进工作模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，pce＝0°。可以理解，各工作模式下的潜孔钻机的期望姿态可以预设在潜孔钻机内，也可以在对应工作模式下人为输入。另外，潜孔钻机的期望姿态下，推进梁期望俯仰角pte等参数值也可允许存在一定的误差，在此不做限制。
本实施例中，控制器41具体用于计算控制推进梁姿态的调节参数值、计算控制推进梁距地距离的调节参数值和计算控制车身姿态的调节参数值。一般地，当工作模式为行走模式时，控制器41用于计算控制推进梁姿态的调节参数值、计算控制推进梁距地距离的调节参数值和计算控制车身姿态的调节参数值；当工作模式为掘进模式时，控制器41用于计算控制推进梁姿态的调节参数值和计算控制车身姿态的调节参数值。
本实施例中，更具体地，控制器41用于根据推进梁期望角度和推进梁实际角度计算推进梁误差，推进梁期望角度包括推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，推进梁实际角度包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并根据推进梁姿态误差计算推进梁期望角速度，即推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye，并根据推进梁期望角速度和推进梁实际角速度计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1、推进梁摆动油缸驱动量Qtr、大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty。
控制器41还用于根据推进梁期望距地距离Le和推进梁实际距地距离Lr计算推进梁距地误差，并根据推进梁距离误差计算推进梁期望移动速度值ve，还根据推进梁期望移动速度值ve和推进梁实际移动速度值计算推进梁补偿液压油缸的驱动量QL。
控制器41还用于根据车身期望角度(包括车身期望俯仰角pce)和车身实际角度(包括车身实际俯仰角pcr)计算车身姿态误差，并根据车身姿态误差计算车身期望角速度ωcpe，还根据车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp。
其中，计算推进梁姿态误差为：
Figure BDA0003090685900000181
计算推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye具体为：
Figure BDA0003090685900000182
计算推进梁俯仰油缸驱动量Qtp1，推进梁摆动油缸驱动量Qtr，大臂俯仰油缸驱动量Qtp2，大臂摆动油缸驱动量Qty具体为：
Figure BDA0003090685900000183
计算推进梁距地误差为：E(L)＝Le-Lr；
计算推进梁期望移动速度值ve具体为：
Figure BDA0003090685900000184
计算推进梁补偿液压油缸的驱动量QL具体为：
Figure BDA0003090685900000185
计算车身姿态误差为：
E(pc)＝pce-pcr；
计算车身期望角速度ωcpe具体为：
Figure BDA0003090685900000186
计算车架调平油缸驱动量Qcp具体为：
Figure BDA0003090685900000191
其中，kp1、kp2……kp11、ki1、ki2......ki11、kd1、kd2……kd11均为常数。
本实施例中，执行元件包括推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸、推进梁补偿液压油缸和车架液压调平油缸。控制器41还用于根据所述推进梁俯仰油缸驱动量Qtp1控制推进梁俯仰液压油缸，根据推进梁摆动油缸驱动量Qtr控制推进梁摆动液压油缸，根据大臂俯仰油缸驱动量Qtp2控制大臂俯仰液压油缸，根据大臂摆动油缸驱动量Qty控制大臂摆动液压油缸，根据进梁补偿液压油缸的驱动量QL控制推进梁补充液压油缸，根据车架调平油缸驱动量Qcp控制车架液压调平油缸，车架液压调平油缸可包括左车架液压调平油缸和右车架液压调平油缸，其油缸驱动量是相同的。
本实施例的潜孔钻机控制方法和潜孔钻机控制装置中，能根据实时地形、实时工作模式等情况自动调整潜孔钻机的姿态，大大提高了潜孔钻机的安全性，且对驾驶员要求较低。
以上仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (10)

1.一种潜孔钻机控制方法，其特征在于，包括：
获取潜孔钻机的工作模式；
获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的所述实际姿态和所述期望姿态是否一致，如果一致则结束，如果不一致则根据潜孔钻机的所述实际姿态和所述期望姿态差异计算执行机构调节参数值；
根据所述调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态，并返回获取潜孔钻机的实际姿态，接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的所述实际姿态和所述期望姿态是否一致的步骤。
2.如权利要求1所述的潜孔钻机控制方法，其特征在于，所述潜孔钻机的工作模式包括行走模式和掘进模式，其中所述行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，所述水平面行走模式、所述上坡行走模式和所述下坡行走模式为常规行走模式；
当工作模式为所述常规行走模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，接收推进梁的期望姿态、车身期望姿态和推进梁期望距地距离，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述车身实际姿态和所述车身期望姿态是否一致，所述推进梁实际距地距离和所述推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致；当工作模式为所述横越斜坡行走模式时，获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，接收推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对实际姿态，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述推进梁实际距地距离和所述推进梁期望距地距离是否一致，所述推进梁与车身相对期望姿态与所述推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致；当工作模式为所述掘进模式时，获取潜孔钻机的推进梁的实际姿态、车身实际姿态，接收推进梁的期望姿态、车身期望姿态，并分别比较所述推进梁的实际姿态与所述推进梁的期望姿态是否一致，所述车身实际姿态和所述车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的所述实际姿态和所述期望姿态不一致。
3.如权利要求2所述的潜孔钻机控制方法，其特征在于，所述推进梁的期望姿态包括推进梁期望俯仰角pte和推进梁期望滚转角rte，所述推进梁期望距地距离包括推进梁期望距地值Le，所述车身期望姿态包括车身期望俯仰角pce，所述推进梁和车身相对期望姿态包括推进梁期望偏航角yte和车身期望偏航角yce的相对期望关系，潜孔钻机的所述推进梁的实际姿态包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，所述推进梁实际距地距离包括推进梁实际距地值Lr，所述车身实际姿态包括车身实际俯仰角pcr，所述推进梁和车身相对实际姿态包括推进梁实际偏航角ytr和车身实际偏航角ycr的相对实际关系；当工作模式为所述水平面行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述上坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述下坡行走模式时，潜孔钻机的期望姿态为：pte＝90°，rte＝0°，pce＝0°，Le>0；当工作模式为所述横越斜坡行走模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，yte＝yce，Le>0；当工作模式为所述掘进工作模式时，潜孔钻机的期望姿态为pte＝0°，rte＝0°，pce＝0°。
4.如权利要求2所述的潜孔钻机控制方法，其特征在于，当工作模式为所述行走模式时，计算执行机构调节参数值的步骤具体包括计算控制推进梁姿态的调节参数值、计算控制推进梁距地距离的调节参数值和计算控制车身姿态的调节参数值；当工作模式为所述掘进模式时，计算执行机构调节参数值的步骤具体包括计算控制推进梁姿态的调节参数值和计算控制车身姿态的调节参数值。
5.如权利要求4所述的潜孔钻机控制方法，其特征在于，所述计算控制推进梁姿态的调节参数值具体包括：
接收推进梁期望角度，即推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，并获取推进梁实际角度，即推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并据此计算推进梁误差；
根据所述推进梁姿态误差计算推进梁期望角速度，即推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye；
根据所述推进梁期望角速度和推进梁实际角速度计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1、推进梁摆动油缸驱动量Qtr、大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty；或者，
所述计算控制推进梁距地距离的调节参数值具体包括：
接收推进梁期望距地距离Le和推进梁实际距地距离Lr，并据此计算推进梁距地误差；
根据所述推进梁距离误差计算推进梁期望移动速度值ve；
根据所述推进梁期望移动速度值ve和推进梁实际移动速度值vr计算推进梁补偿液压油缸的驱动量QL；或者，
所述计算控制车身姿态的调节参数值具体包括：
接收车身期望角度，即车身期望俯仰角pce，并获取车身实际角度，即车身实际俯仰角pcr，并据此计算车身姿态误差；
根据所述车身姿态误差计算车身期望角速度ωcpe；
根据所述车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp。
6.如权利要求5所述的潜孔钻机控制方法，其特征在于，所述计算推进梁姿态误差为：
Figure FDA0003090685890000041
计算推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye具体为：
Figure FDA0003090685890000042
计算推进梁俯仰油缸驱动量Qtp1，推进梁摆动油缸驱动量Qtr，大臂俯仰油缸驱动量Qtp2，大臂摆动油缸驱动量Qty具体为：
Figure FDA0003090685890000043
计算推进梁距地误差为：E(L)＝Le-Lr；
计算推进梁期望移动速度值ve具体为：
Figure FDA0003090685890000044
计算推进梁补偿液压油缸的驱动量QL具体为：
Figure FDA0003090685890000045
计算车身姿态误差为：
E(pc)＝pce-pcr；
计算车身期望角速度ωcpe具体为：
Figure FDA0003090685890000046
计算车架调平油缸驱动量Qcp具体为：
Figure FDA0003090685890000047
其中，kp1、kp2……kp11、ki1、ki2......ki11、kd1、kd2……kd11均为常数。
7.如权利要求6所述的潜孔钻机控制方法，其特征在于，所述执行元件包括推进梁俯仰液压油缸、推进梁摆动液压油缸、大臂俯仰液压油缸、大臂摆动液压油缸、推进梁补偿液压油缸和车架液压调平油缸；
所述根据调节参数值控制潜孔钻机的执行元件的状态以调整潜孔钻机的姿态的步骤具体包括：根据所述推进梁俯仰油缸驱动量Qtp1控制所述推进梁俯仰液压油缸，根据所述推进梁摆动油缸驱动量Qtr控制所述推进梁摆动液压油缸，根据所述大臂俯仰油缸驱动量Qtp2控制所述大臂俯仰液压油缸，根据所述大臂摆动油缸驱动量Qty控制所述大臂摆动液压油缸，根据所述推进梁补偿液压油缸的驱动量QL控制所述推进梁补充液压油缸，根据所述车架调平油缸驱动量Qcp控制所述车架液压调平油缸。
8.一种潜孔钻机控制装置，其特征在于，包括潜孔钻机姿态获取装置(20)和控制器(41)；所述潜孔钻机姿态获取装置(20)用于获取潜孔钻机的实际姿态；所述控制器(41)内设有潜孔钻机期望姿态，所述控制器(41)用于接收潜孔钻机在相应工作模式下的期望姿态，并比较潜孔钻机的实际姿态和期望姿态是否一致，如果不一致则根据潜孔钻机的实际姿态和期望姿态差异计算执行元件调节参数值，并将调节参数值发送给执行元件以调整潜孔钻机的姿态直到潜孔钻机的实际姿态和期望姿态一致。
9.如权利要求8所述的潜孔钻机控制装置，其特征在于，潜孔钻机的工作模式包括行走模式和掘进模式，其中所述行走模式包括水平面行走模式、上坡行走模式、下坡行走模式和横越斜坡行走模式，所述水平面行走模式、所述上坡行走模式和所述下坡行走模式为常规行走模式；
当工作模式为所述常规行走模式时，所述潜孔钻机姿态获取装置(20)用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态和推进梁实际距地距离，所述控制器(41)用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述车身实际姿态和车身期望姿态是否一致，所述推进梁实际距地距离和推进梁期望距地距离是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为所述横越斜坡行走模式时，所述潜孔钻机姿态获取装置(20)用于获取潜孔钻机的推进梁的实际姿态、推进梁实际距地距离和推进梁与车身相对期望姿态，所述控制器(41)用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述推进梁实际距地距离和推进梁期望距地距离是否一致，所述推进梁与车身相对期望姿态与推进梁与车身相对实际姿态，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致；当工作模式为所述掘进模式时，所述潜孔钻机姿态获取装置(20)用于获取潜孔钻机的推进梁的实际姿态、车身实际姿态，所述控制器(41)用于分别比较所述推进梁的实际姿态与推进梁的期望姿态是否一致，所述车身实际姿态和车身期望姿态是否一致，如果均一致则结束，如果有任意一项不一致则判断为潜孔钻机的实际姿态和期望姿态不一致，所述推进梁的期望姿态、所述车身期望姿态、所述推进梁期望距地距离和所述推进梁与车身相对期望姿态预设在所述控制器(41)内。
10.如权利要求8所述的潜孔钻机控制装置，其特征在于，所述潜孔钻机姿态获取装置(20)包括推进梁倾角检测装置(21)、推进梁角速度检测装置(23)、测距装置(25)、车身倾角检测装置(27)和车身角速度检测装置(29)；
所述控制器(41)用于根据推进梁期望角度和推进梁实际角度计算推进梁误差，所述推进梁期望角度包括推进梁期望俯仰角pte、推进梁期望滚转角rte和推进梁期望偏航角yte，所述推进梁实际角度包括推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr，并根据所述推进梁姿态误差计算推进梁期望角速度，即推进梁俯仰角速度ωtpe、推进梁滚转角速度ωtre、推进梁偏航角ωtye，并根据所述推进梁期望角速度和推进梁实际角速度，即推进梁实际俯仰角速度ωtpr、推进梁实际滚转角速度ωtrr、推进梁实际偏航角ωtyr计算推进梁驱动油缸的驱动量，即推进梁俯仰油缸驱动量Qtp1、推进梁摆动油缸驱动量Qtr、大臂俯仰油缸驱动量Qtp2、大臂摆动油缸驱动量Qty，其中，推进梁实际俯仰角ptr、推进梁实际滚转角rtr和推进梁实际偏航角ytr由所述推进梁倾角检测装置(21)测得，所述推进梁实际俯仰角速度ωtpr、所述推进梁实际滚转角速度ωtrr、所述推进梁实际偏航角ωtyr由所述推进梁角速度检测装置(23)测得；
所述控制器(41)还用于根据推进梁期望距地距离Le和推进梁实际距地距离Lr计算推进梁距地误差，并根据所述推进梁距离误差计算推进梁期望移动速度值ve，还根据所述推进梁期望移动速度值ve和推进梁实际移动速度值vr计算推进梁补偿液压油缸的驱动量QL，其中，所述推进梁实际距地距离Lr由所述测距装置(25)测得，所述推进梁实际移动速度值vr由所述测距装置(25)测得的所述推进梁实际距地距离Lr计算得到；
所述控制器(41)还用于根据车身期望角度和车身实际角度计算车身姿态误差，所述车身期望角度包括车身期望俯仰角pce，所述车身实际角度包括车身实际俯仰角pcr，并根据车身姿态误差计算车身期望角速度ωcpe，还根据所述车身期望角速度ωcpe和车身实际角速度ωcpr计算车架调平油缸驱动量Qcp，其中，所述车身实际俯仰角pcr由车身所述倾角检测装置(27)测得，所述车身实际角速度ωcpr由所述车身角速度检测装置(29)测得。
CN202110595249.1A 2021-05-28 2021-05-28 潜孔钻机控制方法及潜孔钻机控制装置 Active CN113216842B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110595249.1A CN113216842B (zh) 2021-05-28 2021-05-28 潜孔钻机控制方法及潜孔钻机控制装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110595249.1A CN113216842B (zh) 2021-05-28 2021-05-28 潜孔钻机控制方法及潜孔钻机控制装置

Publications (2)

Publication Number Publication Date
CN113216842A true CN113216842A (zh) 2021-08-06
CN113216842B CN113216842B (zh) 2022-05-17

Family

ID=77099484

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110595249.1A Active CN113216842B (zh) 2021-05-28 2021-05-28 潜孔钻机控制方法及潜孔钻机控制装置

Country Status (1)

Country Link
CN (1) CN113216842B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114233266A (zh) * 2021-12-17 2022-03-25 中联重科股份有限公司 钻机自动控制方法、系统及存储介质
SE2251370A1 (en) * 2021-11-29 2023-05-30 Jiangsu Xcmg Construction Machinery Res Institute Ltd Geotechnical engineering machine, and method of controlling a working arm thereof
RU2820441C2 (ru) * 2021-11-29 2024-06-03 Цзянсу Скмг Констракшн Машинери Рисерч Инститьют Лтд. Машина для инженерно-геологических работ и способ управления ее манипулятором

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105102762A (zh) * 2013-03-29 2015-11-25 普拉德研究及开发股份有限公司 钻井工具面的闭环控制
CN108291426A (zh) * 2015-12-01 2018-07-17 斯伦贝谢技术有限公司 钻井曲率的闭环控制
CN111021956A (zh) * 2020-03-09 2020-04-17 中国铁建重工集团股份有限公司 一种水平钻机调向控制方法及系统
CN111827978A (zh) * 2019-04-18 2020-10-27 山特维克矿山工程机械有限公司 用于在钻进期间确定钻具的位置的设备和方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105102762A (zh) * 2013-03-29 2015-11-25 普拉德研究及开发股份有限公司 钻井工具面的闭环控制
CN110725650A (zh) * 2013-03-29 2020-01-24 普拉德研究及开发股份有限公司 钻井工具面的闭环控制
CN108291426A (zh) * 2015-12-01 2018-07-17 斯伦贝谢技术有限公司 钻井曲率的闭环控制
CN111827978A (zh) * 2019-04-18 2020-10-27 山特维克矿山工程机械有限公司 用于在钻进期间确定钻具的位置的设备和方法
CN111021956A (zh) * 2020-03-09 2020-04-17 中国铁建重工集团股份有限公司 一种水平钻机调向控制方法及系统

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE2251370A1 (en) * 2021-11-29 2023-05-30 Jiangsu Xcmg Construction Machinery Res Institute Ltd Geotechnical engineering machine, and method of controlling a working arm thereof
RU2820441C2 (ru) * 2021-11-29 2024-06-03 Цзянсу Скмг Констракшн Машинери Рисерч Инститьют Лтд. Машина для инженерно-геологических работ и способ управления ее манипулятором
RU2820441C9 (ru) * 2021-11-29 2024-07-02 Цзянсу Скмг Констракшн Машинери Рисерч Инститьют Лтд. Машина для инженерно-геологических работ и способ управления ее манипулятором
CN114233266A (zh) * 2021-12-17 2022-03-25 中联重科股份有限公司 钻机自动控制方法、系统及存储介质
CN114233266B (zh) * 2021-12-17 2023-11-03 中联重科股份有限公司 钻机自动控制方法、系统及存储介质

Also Published As

Publication number Publication date
CN113216842B (zh) 2022-05-17

Similar Documents

Publication Publication Date Title
KR102307806B1 (ko) 작업 기계
CN113216842B (zh) 潜孔钻机控制方法及潜孔钻机控制装置
JP5947477B1 (ja) 作業機械の制御装置、作業機械、及び作業機械の制御方法
AU2013248174B2 (en) Mining vehicle and method of moving boom
AU2014408915B2 (en) Arrangement for controlling a work machine
JP7129873B2 (ja) 自動均し作業ロボット
EP3635181B1 (en) Blade control below design
CN109099033B (zh) 利用传感器融合反馈在机器中控制流体压力的方法和系统
US10895112B2 (en) Slope compensation system for rotary drill machines
US10995472B2 (en) Grading mode integration
JP6894847B2 (ja) 作業機械および作業機械の制御方法
JP6986853B2 (ja) 作業機械および作業機械の制御方法
KR20190032287A (ko) 작업 기계의 제어 시스템 및 작업 기계의 제어 방법
CN111441406B (zh) 用于坡度控制的鸟瞰校准
JP2023524450A (ja) 静水圧旋回動作作動、監視、および制御システム
WO2020158234A1 (ja) 建設機械の制御システム、建設機械、及び建設機械の制御方法
JP6901406B2 (ja) 作業機械および作業機械の制御方法
JPH07119551B2 (ja) 掘削式地中掘進機の運転支援装置
KR102428131B1 (ko) 작업 기계
JP6876623B2 (ja) 作業機械および作業機械の制御方法
JPH07139290A (ja) 埋設管推進型掘進機及びその方向制御方法

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