Hydraulic drive mechanical arm control method of disc type rock tunneling machine and disc type rock tunneling machine

Abstract

The invention relates to the technical field of tunneling machine construction, in particular to a hydraulic drive mechanical arm control method of a disc type rock tunneling machine and the disc type rock tunneling machine. The hydraulic drive mechanical arm control method of the disc type rock tunneling machine comprises the steps that the set target pose of a cutting cutterhead and operation point positions on a working face are determined; the set target pose comprises a set cut-in depth and a set cut-in angle; the control module controls the hydraulic drive mechanical arm module to drive the cutting cutterhead to conduct tunneling operation at the operation point position of the working face in the set target posture. According to the method, the parallel three-oil-cylinder assembly is decoupled, a solving algorithm of the oil cylinder stroke and the cutting cutterhead pose is constructed, the decoupling algorithm is efficient in calculation, and a solid foundation is laid for achieving high-real-time and low-delay tail end pose control; the control module can control the hydraulic mechanical arm to drive the cutting cutterhead to rotate, so that the cutting cutterhead carries out cutting operation according to the set target posture, high-precision control is achieved, and the operation precision of the cutting cutterhead is effectively improved.

Description

Technical Field

[0001] This invention relates to the field of tunneling machine construction technology, specifically to a hydraulically driven robotic arm control method for a disc rock tunneling machine and the disc rock tunneling machine itself. Background Technology

[0002] Currently, hard rock tunnel construction mainly employs methods such as full-face hard rock tunnel boring machines (TBMs), drill-and-blast methods, and cantilever tunneling machines. However, these methods all have significant limitations. For example, TBMs are bulky, difficult to move, have long manufacturing cycles, complex supporting systems, and require cumbersome construction preparation and auxiliary operations; the drill-and-blast method involves complex procedures, significant disturbance to the surrounding rock caused by blasting vibrations, and requires a large amount of support work with low efficiency; cantilever tunneling machines have limited rock-breaking capabilities, and the separation of tunneling and anchoring operations leads to long cycle times. In contrast, disc-type rock tunneling machines can effectively address the industry challenge of rapid tunneling in hard rock with irregular cross-sections, achieving continuous construction without blasting and possessing significant technological advantages.

[0003] Disc tunneling machines rely on the cutterhead at the end of the robotic arm to cut the rock surface at specific angles and depths, thus requiring extremely high control precision at the end of the robotic arm. Existing hydraulic robotic arm control methods are mostly designed for serially connected robotic arms, using rotary encoders to obtain joint angle signals for control, which is difficult to directly apply to the series-parallel drive mechanism of the cylinders in disc tunneling machines. Furthermore, existing control methods for parallel-connected robotic arms typically break them down into single open-chain processes, resulting in poor real-time performance and insufficient control precision. Summary of the Invention

[0004] The purpose of this invention is to provide a hydraulically driven robotic arm control method for a disc-type rock tunneling machine and a disc-type rock tunneling machine itself, aiming to solve the problem of insufficient control accuracy of the cutterhead position and posture in existing equipment. The specific technical solution is as follows: This invention provides a hydraulically driven robotic arm control method for a disc-type rock tunneling machine. The method controls the cutting disc of the disc-type rock tunneling machine to perform tunneling operations at the working face in a set target posture, and includes the following steps: S1: Determine the target position of the cutting head and the working point on the working surface; setting the target position includes setting the cutting depth and setting the cutting angle; S2: The control module controls the hydraulically driven robotic arm module to drive the cutting head to perform tunneling operations at the work point on the working face in a set target posture, specifically including: S2.1 Establish the joint coordinate system of the disc rock tunneling machine. The joint coordinate system includes the rotary joint coordinate system, the pitch joint coordinate system, the cutterhead translation coordinate system, the cutterhead pitch coordinate system, and the cutterhead yaw coordinate system. S2.2. Based on the target pose of the cutting head in the rotary joint coordinate system and the working point position on the working surface, the adjustment posture parameters are calculated. The adjustment posture parameters include the rotation angle of the rotary joint. Rotation angle of the pitch joint and the left and right swing angle of the cutting head relative to the hydraulically driven robotic arm module. Up and down swing angle Translation length ; S2.3, Convert the attitude adjustment parameters obtained in S2.2 into the extension and retraction of the rotary cylinder, pitch cylinder and parallel three-cylinder assembly respectively; S2.4 The control module controls the rotary cylinder, pitch cylinder and parallel three-cylinder assembly to extend and retract according to the corresponding extension and retraction amount to adjust the end of the cutting disc to the set cutting angle, and pushes the cutting disc to vibrate and cut into the working face to the set cutting depth through the parallel three-cylinder assembly, and then controls the cutting disc of the disc rock tunneling machine to perform tunneling operation in the set target posture.

[0005] Optionally, S2.1 includes: Establish a rotary joint coordinate system with the rotation axis of the rotary joint as the Z-axis, the width direction of the disc rock tunnel boring machine as the X-axis, and the length direction of the disc rock tunnel boring machine as the Y-axis. ; Establish a pitch coordinate system with the rotation axis of the pitch joint as the Z-axis, the vertical direction as the X-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis. ; Establish three joint coordinate systems with the center of the movable connector as the center, namely the tool head translation coordinate system. Cutterhead pitch coordinate system and the tool head yaw coordinate system The cutterhead translation coordinate system has the vertical direction as the X-axis, the length direction of the disc rock tunnel boring machine as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis; the cutterhead pitch coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the width direction of the disc rock tunnel boring machine as the Z-axis, and the vertical direction as the Y-axis; the cutterhead yaw coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the vertical direction as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis.

[0006] Optionally, S2.2 includes: Based on the coordinates of the cutting head in the cutting head yaw coordinate system And set the target pose coordinates in the rotary joint coordinate system. Establish the objective function The specific formula is as follows: ; in: To adjust attitude parameters, ; This represents the transformation relationship between the tool turret tilt coordinate system and the trolley coordinate system. This refers to the number of the joint coordinate system; Constraints are established based on the set entry angle of the target attitude. The specific formula is as follows: ; in: For setting value, , Let Z be the coordinate axis of the tool turret yaw coordinate system. To set the z-axis under the target attitude; Based on the objective function and constraints The attitude parameters are solved using the constrained Newton method.

[0007] Optionally, the transformation relationship between the tool turret yaw coordinate system and the trolley coordinate system. The specific formula is as follows: .

[0008] Optionally, based on the objective function and constraints The constrained Newton method is used to solve for the attitude adjustment parameters, specifically including: ① Based on the objective function and constraints, construct the Lagrangian function using the quasi-Newton method. The specific formula is as follows: ; in: For Lagrange multipliers; ② Calculate the Hessian matrix and Lagrange gradient of the Lagrange function. The specific formula for the Hessian matrix is ​​as follows: ; Lagrange gradient The specific formula is as follows: ; An iterative formula for adjusting the attitude parameters is constructed based on the Hessian matrix, as follows: ; in: This represents the number of iterations. These are the variable and multiplier for the k-th iteration, respectively; The attitude adjustment parameters are obtained by solving the iterative formula and the working points on the working surface.

[0009] Optionally, S2.3 includes: Based on the geometric relationship between the hydraulic cylinder and the rotating axis, the rotation angles of the rotary joint and the pitch joint are converted into the extension and retraction amounts of the rotary and pitch hydraulic cylinders, respectively. Specifically: ① Obtain the conversion formula between the extension / retraction length and the rotation angle of the hydraulic cylinder based on the geometric relationship between the hydraulic cylinder and the rotation axis; ② Based on the initial length of the hydraulic cylinder, the conversion formula between the extension length and rotation angle of the hydraulic cylinder is solved in reverse to obtain the extension amount of the rotary hydraulic cylinder and the pitch hydraulic cylinder respectively. The rotation angle and translation length of the cutting disc relative to the hydraulically driven robotic arm module are respectively converted into the extension and retraction of the parallel three-cylinder assembly, specifically including: ① Construct the left and right swing angle Up and down swing angle Translation length The coordinates of the moving end of the cylinder in the parallel three-cylinder assembly The conversion relationship between them is shown in the following formula: ; in: These are the coordinates of the moving end of the hydraulic cylinder after conversion; Number each cylinder in the parallel three-cylinder assembly. ; The translation length; The left and right swing angle of the cutting head relative to the hydraulically driven robotic arm module; The vertical swing angle of the cutting head relative to the hydraulically driven robotic arm module; It is a translation matrix; It is a rotation matrix; ② Construct a system of nonlinear equations based on the coordinates of the fixed end of the cylinder and the transformed coordinates of the movable end of the cylinder in a parallel three-cylinder assembly; ③ Based on the nonlinear equations and the transformation relationship between the cutterhead yaw coordinate system and the trolley coordinate system, the extension and retraction of the parallel three-cylinder assembly is obtained by inverse solution, specifically: Based on the transformation relationship between the cutter head yaw coordinate system and the trolley coordinate system, the transformed cylinder fixed end coordinates are obtained by converting the adjustment posture parameters obtained in S2.3. and the coordinates of the moving end of the hydraulic cylinder The specific formula is as follows: ; ; Based on the nonlinear equations, the coordinates of the transformed fixed end of the hydraulic cylinder are used. and the coordinates of the moving end of the hydraulic cylinder The extension / retraction amount of the parallel three-cylinder assembly is obtained by inverse calculation, and the specific formula is as follows: ; in: This is the length when the hydraulic cylinder is fully retracted.

[0010] Alternatively, the specific formulas for the nonlinear equation system are as follows: ; in: The extension length of the hydraulic cylinder; For length error, =0.

[0011] Optionally, the conversion formula between the cylinder extension / retraction length and the rotation angle is as follows: ; in: This is the distance from the axis of the rotary joint to the fixed end of the rotary cylinder. The distance from the axis of the rotary joint to the moving end of the rotary cylinder. This is the length when the rotary cylinder is fully retracted. This refers to the extension and retraction of the rotary cylinder; ; in: This is the distance from the axis of the pitch joint to the fixed end of the pitch cylinder. The distance from the axis of the pitch joint to the moving end of the pitch cylinder. This is the length when the pitch cylinder is fully retracted. This refers to the extension and retraction of the pitch cylinder.

[0012] Optionally, the specific formula for the extension and retraction of the rotary cylinder is as follows: ; in: This is the initial angle of the rotary joint. ; The specific formula for the extension and retraction of the pitch cylinder is as follows: ; in: This represents the initial angle of the pitch joint. .

[0013] Alternatively, the specific formula for the translation matrix is ​​as follows: ; in: The translation length; The specific formula for the rotation matrix is ​​as follows: ; Indicates rotation about the z-axis. This indicates rotation about the y-axis.

[0014] The present invention also provides a disc rock tunneling machine for implementing the hydraulically driven manipulator control method of the disc rock tunneling machine as described above, comprising a tunneling machine body, a hydraulically driven manipulator module, a cutting disc, and a control module. The hydraulically driven manipulator module is disposed on the tunneling machine body; the cutting disc is disposed on the hydraulically driven manipulator module; the control module is electrically connected to the hydraulically driven manipulator module, and the control module is capable of controlling the hydraulic manipulator to drive the cutting disc to rotate, so that the cutting disc performs cutting operations in a set target posture.

[0015] Optionally, the hydraulically driven robotic arm module includes a rotary joint, a pitch joint, a movable connector, a rotary cylinder, a pitch cylinder, and a parallel three-cylinder assembly. The rotary joint is rotatably mounted on the tunneling machine body, and the first end of the pitch joint is rotatably connected to the rotary joint. The cutterhead is movably connected to the second end of the pitch joint via the movable connector. The rotary cylinder is rotatably connected between the tunneling machine body and the pitch joint, and drives the pitch joint to move the rotary joint relative to the tunneling machine body. The tunneling machine body rotates left and right; the pitch cylinder is rotatably connected between the pitch joint and the tunneling machine body, and the pitch cylinder can drive the pitch joint to rotate up and down relative to the tunneling machine body; the parallel three-cylinder assembly is rotatably connected between the pitch joint and the cutting disc; the parallel three-cylinder assembly can drive the cutting disc to rotate left and right and up and down relative to the pitch joint; the slewing cylinder, the pitch cylinder, and the parallel three-cylinder assembly are respectively electrically connected to the control module.

[0016] Optionally, the parallel three-cylinder assembly includes three adjusting cylinders arranged in parallel. One adjusting cylinder is located in the vertical plane containing the axis of the cutting disc, and the other two adjusting cylinders are symmetrically arranged about the adjusting cylinders in the vertical plane containing the axis of the cutting disc.

[0017] Optionally, the rotary cylinder, the pitch cylinder, and the adjusting cylinder are all equipped with cylinder stroke sensors.

[0018] This invention calculates the joint angles and lengths of a hydraulically driven robotic arm module based on a built-in cylinder stroke sensor, eliminating the need for an external rotary encoder and offering strong environmental adaptability, particularly suitable for the harsh working conditions of disc tunneling machines. By decoupling the parallel three-cylinder assembly, this invention constructs an algorithm for solving the relationship between cylinder stroke and cutterhead posture. This decoupling algorithm is computationally efficient, laying a solid foundation for achieving high real-time, low-latency end-effector posture control. The control module of this invention can control the hydraulic robotic arm to drive the cutterhead to rotate, enabling the cutterhead to perform cutting operations in a set target posture, achieving high-precision control and effectively improving the working accuracy of the cutterhead. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0020] Figure 1 This is a schematic diagram of the structure of the disc-type rock tunneling machine in Embodiment 1 of the present invention; Figure 2 This is a schematic diagram of the hydraulically driven robotic arm module in Embodiment 3 of the present invention; Figure 3 This is a schematic diagram of the joint coordinate system arrangement in Embodiment 3 of the present invention.

[0021] Explanation of icon numbers: 1. Tunneling machine body; 2. Hydraulic-driven robotic arm module; 2.1 Rotary joint; 2.2 Pitch joint; 2.2.1 Mounting frame; 2.3 Movable connector; 2.4 Pitch cylinder; 2.5 Parallel three-cylinder assembly; 3. Cutting head; 3.1 Cutting frame; 3.2 Cutting head body; 4. Control module; 5. Stabilizing device; 6. Drilling and anchoring device; 7. Electrical platform; 8. Advanced drilling rig; 9. Temporary support device; 10. Shovel plate device.

[0022] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0023] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0024] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indication will also change accordingly.

[0025] Furthermore, in this invention, descriptions involving "first," "second," etc., are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0026] In this invention, unless otherwise explicitly specified and limited, the terms "connection," "fixed," etc., should be interpreted broadly. For example, "fixed" can mean a fixed connection, a detachable connection, or an integral part; it can mean a mechanical connection or an electrical connection; it can mean a direct connection or an indirect connection through an intermediate medium; it can mean the internal communication of two components or the interaction between two components, unless otherwise explicitly limited. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0027] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible for those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0028] This invention proposes a hydraulically driven robotic arm control method for a disc rock tunneling machine and a disc rock tunneling machine, aiming to solve the problem of insufficient control accuracy of the cutting disc position and posture in existing equipment.

[0029] See Figure 1 This embodiment provides a disc-type rock tunneling machine, including a tunneling machine body 1, a hydraulically driven robotic arm module 2, a cutting disc 3, and a control module 4. The hydraulically driven robotic arm module 2 is mounted on the tunneling machine body 1; the cutting disc 3 is mounted on the hydraulically driven robotic arm module 2; the control module 4 is electrically connected to the hydraulically driven robotic arm module 2, and the control module 4 can control the hydraulic robotic arm to drive the cutting disc 3 to rotate, so that the cutting disc 3 performs cutting operations in a set target posture. In this embodiment, the control module 4 can control the hydraulic robotic arm to drive the cutting disc 3 to rotate, so that the cutting disc 3 performs cutting operations in a set target posture, achieving high-precision control and effectively improving the working accuracy of the cutting disc 3.

[0030] The hydraulically driven robotic arm module 2 includes a rotary joint 2.1, a pitch joint 2.2, a movable connector 2.3, a rotary cylinder, a pitch cylinder 2.4, and a parallel three-cylinder assembly 2.5. The rotary joint 2.1 is rotatably mounted on the tunneling machine body 1, and the first end of the pitch joint 2.2 is rotatably connected to the rotary joint 2.1. The cutterhead 3 is movably connected to the second end of the pitch joint 2.2 through the movable connector 2.3. The rotary cylinder is rotatably connected between the tunneling machine body 1 and the pitch joint 2.2, and drives the pitch joint 2.2 to drive the rotary joint 2. The slewing cylinder 2.4 rotates left and right relative to the tunneling machine body 1; the pitch cylinder 2.4 is rotatably connected between the pitch joint 2.2 and the tunneling machine body 1, and the pitch cylinder 2.4 can drive the pitch joint 2.2 to rotate up and down relative to the tunneling machine body 1; the parallel three-cylinder assembly 2.5 is rotatably connected between the pitch joint 2.2 and the cutting disc 3; the parallel three-cylinder assembly 2.5 can drive the cutting disc 3 to rotate left and right and up and down relative to the pitch joint 2.2; the slewing cylinder, the pitch cylinder 2.4, and the parallel three-cylinder assembly 2.5 are respectively electrically connected to the control module 4. The rotary cylinder and the pitch cylinder 2.4 drive the rotary joint 2.1 and the pitch joint to perform left and right rotation and up and down swinging movements, respectively, to coarsely adjust the posture of the cutting disc 3, so that the cutting disc 3 is initially matched with the set target posture. Then, the parallel three-cylinder assembly 2.5 further drives the cutting disc 3 to rotate left and right and up and down, so as to finely adjust the position and posture of the cutting disc 3, so that the position and posture of the cutting disc 3 corresponds to the set target posture, which helps to improve control accuracy and thus improve the working accuracy of the cutting disc 3.

[0031] In this embodiment, the pitch joint 2.2 is provided with a mounting bracket 2.2.1, which is rotatably connected to the movable connector 2.3 in the vertical direction; the cutting disc 3 includes a cutting frame 3.1 and a disc body 3.2, which is rotatably connected to the movable connector 2.3 in the horizontal direction, and the disc body 3.2 is disposed on the cutting frame 3.1, thereby realizing the up-down and left-right rotation of the cutting disc 3 relative to the pitch joint 2.2.

[0032] Furthermore, the parallel three-cylinder assembly 2.5 includes three adjusting cylinders arranged in parallel. One adjusting cylinder is located in the vertical plane containing the axis of the cutting disc 3, and the other two adjusting cylinders are symmetrically arranged about the vertical plane containing the axis of the cutting disc 3. In this embodiment, the cutting disc 3 can achieve multi-degree-of-freedom rotation relative to the pitch joint 2.2 under the action of the three adjusting cylinders, so as to accurately adjust the position and posture of the cutting disc 3, thereby adapting to complex rock-breaking environments and effectively solving the problems of difficulty in rapid rock breaking and low efficiency of existing equipment.

[0033] Furthermore, the rotary cylinder, the pitch cylinder 2.4, and the adjusting cylinder are all equipped with cylinder stroke sensors. This embodiment uses built-in cylinder stroke sensors to calculate the joint angles and lengths of the hydraulically driven robotic arm module 2, eliminating the need for an external rotary encoder. This makes it highly adaptable to various environments, especially suitable for the harsh working conditions of disc tunneling machines.

[0034] In this embodiment, the control module 4 includes a controller and a hydraulic platform. The controller controls the extension and retraction of each cylinder through the hydraulic platform.

[0035] In this embodiment, the disc rock tunneling machine also includes stabilizing devices 5. The number of stabilizing devices 5 is one or more, and each stabilizing device is symmetrically arranged at the bottom of the tunneling machine body 1. The stabilizing devices 5 can contact the ground to provide support for the tunneling machine body 1. The number of stabilizing devices 5 can be set according to actual operational needs. When the machine body is tilted, the stabilizing devices 5 can adjust the machine body posture through telescopic hydraulic cylinders to ensure the stability of the equipment during operation. In this embodiment, several stabilizing devices 5 can be set to solve the problems of large vibrations during cantilever tunneling and the reaction force caused by the forward extension and retraction of the cutting arm while the chassis remains stationary, thereby improving the tunneling machine's excavation stability and efficiency, and ensuring the stability and safety of the tunneling machine's excavation.

[0036] In this embodiment, the disc rock tunneling machine also includes a drilling and anchoring device 6, a shovel device 10, an electrical platform 7, an advance drilling rig 8, and a temporary support device 9, all mounted on the main body 1 of the tunneling machine. While the cutterhead 3 is breaking rocks, the shovel device 10 simultaneously cleans up the slag, preventing slag accumulation from affecting the normal operation of the disc rock tunneling machine. The drilling and anchoring device 6 is used to anchor and support the inner wall of the tunnel, and the shovel device 10 collects and transports the slag generated during the rock breaking operation, realizing the simultaneous operation of excavation, anchoring support, and slag cleaning. The electrical platform 7 is used to provide power; the advance drilling rig 8 is used for drilling operations; and the temporary support device 9 is used for temporary support inside the tunnel.

[0037] This embodiment also provides a hydraulically driven robotic arm control method for a disc-type rock tunneling machine. The method described above controls the cutting disc 3 of the disc-type rock tunneling machine to perform tunneling operations at the working face in a set target posture. The method includes the following steps: S1: Determine the target pose of the cutting head 3 and the working point on the working surface; the target pose includes setting the cutting depth and setting the cutting angle; S2: Control module 4 controls hydraulically driven robotic arm module 2 to drive cutting head 3 to perform tunneling operations at the work point on the working face in a set target posture, specifically including: S2.1 Establish the joint coordinate system of the disc rock tunneling machine. The joint coordinate system includes the rotary joint 2.1 coordinate system, the pitch joint 2.2 coordinate system, the cutterhead translation coordinate system, the cutterhead pitch coordinate system, and the cutterhead yaw coordinate system. S2.1 includes: A coordinate system for rotary joint 2.1 is established with the rotation axis of rotary joint 2.1 as the Z-axis, the width direction of the disc rock tunnel boring machine as the X-axis, and the length direction of the disc rock tunnel boring machine as the Y-axis. ; Establish a pitch coordinate system with the rotation axis of pitch joint 2.2 as the Z-axis, the vertical direction as the X-axis, and the width direction of the disc rock tunneling machine as the Y-axis. ; Three joint coordinate systems are established with the center of the movable connector 2.3 as the center, namely the tool head translation coordinate system. Cutterhead pitch coordinate system and the tool head yaw coordinate system The cutterhead translation coordinate system has the vertical direction as the X-axis, the length direction of the disc rock tunnel boring machine as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis; the cutterhead pitch coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the width direction of the disc rock tunnel boring machine as the Z-axis, and the vertical direction as the Y-axis; the cutterhead yaw coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the vertical direction as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis.

[0038] like Figure 2 and Figure 3 As shown, the coordinate system of the rotary joint is shown in Figure 2.1. The X-axis is x1, the Y-axis is y1, and the Z-axis is z1, in the pitch coordinate system. The X-axis is x2, the Y-axis is y2, and the Z-axis is z2. This is the tool turret translation coordinate system. The X-axis is x3, the Y-axis is y3, and the Z-axis is z3. (Tool head pitch coordinate system) The X-axis is x4, the Y-axis is y4, and the Z-axis is z4. (Tool turret yaw coordinate system) The X-axis is x5, the Y-axis is y5, and the Z-axis is z5. Figure 3 The dashed line in the image represents the parallel three-cylinder assembly of the entire cutter head.

[0039] S2.2. Based on the target pose of the cutting head 3 in the coordinate system of the rotary joint 2.1 and the working point on the working surface, the adjustment posture parameters are calculated. The adjustment posture parameters include the rotation angle of the rotary joint 2.1. 2.2 Rotation angle of the pitch joint And the left and right swing angle of the cutting head 3 relative to the hydraulically driven robotic arm module 2 Up and down swing angle Translation length ; S2.2 includes: Based on the coordinates of the cutting head 3 in the cutter head yaw coordinate system And set the target pose in the coordinate system of the rotary joint 2.1. Establish the objective function The specific formula is as follows: ; in: To adjust attitude parameters, ; This represents the transformation relationship between the tool turret tilt coordinate system and the trolley coordinate system. This refers to the number of the joint coordinate system; In this embodiment, the coordinates of the cutting head 3 in the head yaw coordinate system And set the target pose in the coordinate system of the rotary joint 2.1. Obtained through existing positioning methods.

[0040] In this embodiment, the transformation relationship between the cutter head yaw coordinate system and the trolley coordinate system is described. The specific formula is as follows: ; Among them, the transformation parameters of each joint coordinate system can be obtained by calibrating the kinematic parameters of the robotic arm before leaving the factory.

[0041] Constraints are established based on the set entry angle of the target attitude. The specific formula is as follows: ; in: For setting value, , Let Z be the coordinate axis of the tool turret yaw coordinate system. To set the z-axis under the target attitude; The calculation is replaced by the unit vector (0, 0, 1); The Z-axis position is obtained by adjusting the vertically set Z-axis on the cutting disc 3 in the initial posture to the Z-axis position in the set target posture by the hydraulic robotic arm module 2. Based on the objective function and constraints The constrained Newton's method is used to solve for the attitude parameters. Specifically, this includes: ① Based on the objective function and constraints, construct the Lagrangian function using the quasi-Newton method. The specific formula is as follows: ; in: For Lagrange multipliers; ② Calculate the Hessian matrix and Lagrange gradient of the Lagrange function. The specific formula for the Hessian matrix is ​​as follows: ; Lagrange gradient The specific formula is as follows: ; An iterative formula for adjusting the attitude parameters is constructed based on the Hessian matrix, as follows: ; in: This represents the number of iterations. These are the variable and multiplier for the k-th iteration, respectively; The attitude adjustment parameters are obtained by solving the iterative formula and the working points on the working surface.

[0042] S2.3, Convert the attitude adjustment parameters obtained in S2.2 into the extension and retraction of the rotary cylinder, pitch cylinder 2.4 and parallel three-cylinder assembly 2.5 respectively; S2.3 includes: Based on the geometric relationship between the cylinder and the rotating axis, the rotation angles of the rotary joint 2.1 and the pitch joint 2.2 are converted into the extension and retraction amounts of the rotary cylinder and the pitch cylinder 2.4, respectively. Specifically: ① Obtain the conversion formula between the extension / retraction length and the rotation angle of the hydraulic cylinder based on the geometric relationship between the hydraulic cylinder and the rotation axis; The specific formula for converting the extension / retraction length of the hydraulic cylinder to the rotation angle is as follows: ; in: This is the distance from the axis of the rotary joint 2.1 to the fixed end of the rotary cylinder. The distance from the axis of the rotary joint 2.1 to the moving end of the rotary cylinder is... This is the length when the rotary cylinder is fully retracted. This refers to the extension and retraction of the rotary cylinder; ; in: This is the distance from the axis of the pitch joint 2.2 to the fixed end of the pitch cylinder 2.4. The distance from the axis of the pitch joint 2.2 to the moving end of the pitch cylinder 2.4 is... This refers to the length of the pitch cylinder 2.4 when it is fully retracted. This refers to the extension / retraction amount of the pitch cylinder 2.4.

[0043] ② Based on the initial length of the hydraulic cylinder, the conversion formula between the cylinder's extension / retraction length and rotation angle is solved in reverse to obtain the extension / retraction amounts of the rotary and pitch cylinders, respectively; specifically: The specific formula for the extension and retraction of the rotary cylinder is as follows: ; in: The initial angle of the rotary joint 2.1, ; The specific formula for the extension / retraction of the pitch cylinder 2.4 is as follows: ; in: The initial angle of the pitch joint 2.2, .

[0044] The rotation angle and translation length of the cutting disc 3 relative to the hydraulically driven robotic arm module 2 are respectively converted into the extension and retraction of the parallel three-cylinder assembly 2.5, specifically including: ① Construct the left and right swing angle Up and down swing angle Translation length The coordinates of the moving end of the cylinder in the parallel three-cylinder assembly 2.5 The conversion relationship between them is shown in the following formula: ; in: These are the coordinates of the moving end of the hydraulic cylinder after conversion; The cylinder numbers are used for each cylinder in the parallel three-cylinder assembly 2.5. ; The translation length; The left and right swing angle of the cutting disc 3 relative to the hydraulically driven robotic arm module 2; The vertical swing angle of the cutting head 3 relative to the hydraulically driven robotic arm module 2; It is a translation matrix; It is a translation matrix; The specific formula for the translation matrix is ​​as follows: ; The translation length; The specific formula for the rotation matrix is ​​as follows: ; Indicates rotation about the z-axis. Indicates rotation about the y-axis ② Construct a system of nonlinear equations based on the coordinates of the fixed end of the cylinder and the transformed coordinates of the movable end of the cylinder in the parallel three-cylinder assembly 2.5; The specific formulas for the nonlinear equation system are as follows: ; in: The extension length of the hydraulic cylinder; For length error, =0.

[0045] ③ Based on the nonlinear equations and the transformation relationship between the cutterhead yaw coordinate system and the trolley coordinate system, the extension / retraction of the parallel three-cylinder assembly 2.5 is obtained by inverse solution, specifically: Based on the transformation relationship between the cutter head yaw coordinate system and the trolley coordinate system, the transformed cylinder fixed end coordinates are obtained by converting the adjustment posture parameters obtained in S2.3. and the coordinates of the moving end of the hydraulic cylinder The specific formula is as follows: ; ; Based on the nonlinear equations, the coordinates of the transformed fixed end of the hydraulic cylinder are used. and the coordinates of the moving end of the hydraulic cylinder The extension / retraction of the parallel three-cylinder assembly (2.5) is obtained by reverse calculation, and the specific formula is as follows: ; in: This is the length when the hydraulic cylinder is fully retracted.

[0046] S2.4 and control module 4 control the rotary cylinder, pitch cylinder 2.4 and parallel three-cylinder assembly 2.5 to extend and retract according to the corresponding extension and retraction amount to adjust the end of the cutting disc 3 to the set cutting angle, and push the cutting disc to vibrate and cut into the working face to the set cutting depth through the parallel three-cylinder assembly 2.5, and then control the cutting disc 3 of the disc rock tunneling machine to perform tunneling operation in the set target posture.

[0047] During milling operations, the coordinate system of the rotary joint (2.1) is obtained through positioning. After determining the relationship with the working face, and knowing that the cutterhead and the working face cutting position are between 5° and 10°, the relationship between the cylinder length and the cutting cutterhead 3 obtained through steps S2.1 to S2.2 of this embodiment can accurately determine the cylinder length that needs to be controlled at any point on the working face. Then, the control module 4 controls the rotary cylinder, the pitch cylinder 2.4, and the parallel three-cylinder assembly 2.5 to extend and retract according to the corresponding extension and retraction amount to adjust the end of the cutting cutterhead 3 to the set cutting angle. The parallel three-cylinder assembly 2.5 pushes the cutting cutterhead to vibrate and cut into the working face to the set cutting depth. Finally, the cutting cutterhead 3 of the disc rock tunneling machine is controlled to perform tunneling operations in the set target posture. This embodiment uses a built-in cylinder stroke sensor to calculate the joint angle and length of the hydraulically driven robotic arm module 2, eliminating the need for an external rotary encoder. It is highly adaptable to various environments, especially suitable for the harsh working conditions of disc tunneling machines. This invention decouples the parallel three-cylinder assembly 2.5 to construct an algorithm for solving the cylinder stroke and the pose of the cutting disc 3. This decoupling algorithm is computationally efficient and lays a solid foundation for achieving high real-time and low-latency end-effector pose control.

[0048] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural transformations made using the contents of the present invention's specification and drawings under the inventive concept of the present invention, or direct / indirect applications in other related technical fields, are included within the patent protection scope of the present invention.

Claims

1. A hydraulically driven robotic arm control method for a disc-type rock tunneling machine, comprising controlling the cutting disc (3) of the disc-type rock tunneling machine to perform tunneling operations at the working face in a set target posture, characterized in that, Includes the following steps: S1: Determine the target pose of the cutting head (3) and the working point on the working surface; the target pose includes setting the cutting depth and setting the cutting angle; S2: The control module (4) controls the hydraulically driven robotic arm module (2) to drive the cutting disc (3) to perform tunneling operations at the working point on the working face in a set target posture, specifically including: S2.1 Establish the joint coordinate system of the disc rock tunneling machine. The joint coordinate system includes the rotary joint (2.1) coordinate system, the pitch joint (2.2) coordinate system, the cutterhead translation coordinate system, the cutterhead pitch coordinate system, and the cutterhead yaw coordinate system. S2.

2. The adjustment posture parameters are calculated based on the target pose of the cutting head (3) in the coordinate system of the rotary joint (2.1) and the working point on the working surface. The adjustment posture parameters include the rotation angle of the rotary joint (2.1). 2.2 Rotation angle of the pitch joint and the left and right swing angle of the cutting head (3) relative to the hydraulically driven robotic arm module (2). Up and down swing angle Translation length ; S2.3, The attitude adjustment parameters obtained in S2.2 are converted into the extension and retraction of the rotary cylinder, pitch cylinder (2.4), and parallel three-cylinder assembly (2.5), respectively; S2.4, the control module (4) controls the rotary cylinder, the pitch cylinder (2.4) and the parallel three-cylinder assembly (2.5) to extend and retract according to the corresponding extension and retraction amount to adjust the end of the cutting disc (3) to the set cutting angle, and pushes the cutting disc to vibrate and cut into the working face to the set cutting depth through the parallel three-cylinder assembly (2.5), and then controls the cutting disc (3) of the disc rock tunneling machine to perform tunneling operation in the set target posture.

2. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 1, characterized in that, S2.1 includes: A coordinate system for the rotary joint (2.1) is established with the rotation axis of the rotary joint (2.1) as the Z-axis, the width direction of the disc rock tunnel boring machine as the X-axis, and the length direction of the disc rock tunnel boring machine as the Y-axis. ; A pitch coordinate system is established with the rotation axis of the pitch joint (2.2) as the Z-axis, the vertical direction as the X-axis, and the width direction of the disc rock tunneling machine as the Y-axis. ; Three joint coordinate systems are established with the center of the movable connector (2.3) as the center, namely the tool head translation coordinate system. Cutterhead pitch coordinate system and the tool head yaw coordinate system The cutterhead translation coordinate system has the vertical direction as the X-axis, the length direction of the disc rock tunnel boring machine as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis; the cutterhead pitch coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the width direction of the disc rock tunnel boring machine as the Z-axis, and the vertical direction as the Y-axis; the cutterhead yaw coordinate system has the length direction of the disc rock tunnel boring machine as the X-axis, the vertical direction as the Z-axis, and the width direction of the disc rock tunnel boring machine as the Y-axis.

3. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 2, characterized in that, S2.2 includes: According to the coordinates of the cutting head (3) in the cutting head yaw coordinate system And set the target pose in the coordinate system of the rotary joint (2.1). Establish objective function The specific formula is as follows: ； in: To adjust attitude parameters, ; This represents the transformation relationship between the tool turret tilt coordinate system and the trolley coordinate system. This refers to the number of the joint coordinate system; Constraints are established based on the set entry angle of the target attitude. The specific formula is as follows: ； in: For setting value, , Let Z be the coordinate axis of the tool turret yaw coordinate system. To set the z-axis under the target attitude; Based on the objective function and constraints The attitude parameters are solved using the constrained Newton method.

4. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 3, characterized in that, Transformation relationship between the tool head yaw coordinate system and the trolley coordinate system The specific formula is as follows: 。 5. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 3, characterized in that, Based on the objective function and constraints The constrained Newton method is used to solve for the attitude adjustment parameters, specifically including: ① Based on the objective function and constraints, construct the Lagrangian function using the quasi-Newton method. The specific formula is as follows: ； in: For Lagrange multipliers; ② Calculate the Hessian matrix and Lagrange gradient of the Lagrange function. The specific formula for the Hessian matrix is ​​as follows: ； Lagrange gradient The specific formula is as follows: ； An iterative formula for adjusting the attitude parameters is constructed based on the Hessian matrix, as follows: ； in: This represents the number of iterations. These are the variable and multiplier for the k-th iteration, respectively; The attitude adjustment parameters are obtained by solving the iterative formula and the working points on the working surface.

6. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to any one of claims 1-5, characterized in that, S2.3 includes: Based on the geometric relationship between the cylinder and the rotating axis, the rotation angles of the rotary joint (2.1) and the pitch joint (2.2) are converted into the extension and retraction amounts of the rotary cylinder and the pitch cylinder (2.4), respectively. Specifically: ① Obtain the conversion formula between the extension / retraction length and the rotation angle of the hydraulic cylinder based on the geometric relationship between the hydraulic cylinder and the rotation axis; ② Based on the initial length of the cylinder, the conversion formula between the cylinder extension length and the rotation angle is solved in reverse to obtain the extension amount of the rotary cylinder and the pitch cylinder (2.4); The rotation angle and translation length of the cutting disc (3) relative to the hydraulically driven robotic arm module (2) are respectively converted into the extension and retraction of the parallel three-cylinder assembly (2.5), specifically including: ① Construct the left and right swing angles Up and down swing angle Translation length Coordinates of the moving end of the cylinder in the parallel three-cylinder assembly (2.5) The conversion relationship between them is shown in the following formula: ； in: These are the coordinates of the moving end of the hydraulic cylinder after conversion; The cylinders in the parallel three-cylinder assembly (2.5) are numbered. ; The translation length; The left and right swing angle of the cutting head (3) relative to the hydraulically driven robotic arm module (2); The vertical swing angle of the cutting head (3) relative to the hydraulically driven robotic arm module (2); It is a translation matrix; It is a rotation matrix; ② Construct a system of nonlinear equations based on the coordinates of the fixed end of the cylinder and the transformed coordinates of the movable end of the cylinder in the parallel three-cylinder assembly (2.5); ③ Based on the nonlinear equations and the transformation relationship between the cutterhead yaw coordinate system and the trolley coordinate system, the extension and retraction of the parallel three-cylinder assembly (2.5) is obtained by inverse solution, specifically: Based on the transformation relationship between the cutter head yaw coordinate system and the trolley coordinate system, the transformed cylinder fixed end coordinates are obtained by converting the adjustment posture parameters obtained in S2.

3. and the coordinates of the moving end of the hydraulic cylinder The specific formula is as follows: ； ； Based on the nonlinear equations, the coordinates of the transformed fixed end of the hydraulic cylinder are used. and the coordinates of the moving end of the hydraulic cylinder The extension / retraction of the parallel three-cylinder assembly (2.5) is obtained by reverse calculation, and the specific formula is as follows: ； in: This is the length when the hydraulic cylinder is fully retracted.

7. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 6, characterized in that, The specific formulas for the nonlinear equation system are as follows: ； in: The extension length of the hydraulic cylinder; For length error, =0.

8. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 6, characterized in that, The specific formula for converting the extension / retraction length of the hydraulic cylinder to the rotation angle is as follows: ； in: The distance from the axis of the rotary joint (2.1) to the fixed end of the rotary cylinder is... The distance from the axis of the rotary joint (2.1) to the moving end of the rotary cylinder is... This is the length when the rotary cylinder is fully retracted. This refers to the extension and retraction of the rotary cylinder; ； in: The distance from the axis of the pitch joint (2.2) to the fixed end of the pitch cylinder (2.4) is the distance. The distance from the axis of the pitch joint (2.2) to the moving end of the pitch cylinder (2.4) is... The length of the pitch cylinder (2.4) when fully retracted. The extension / retraction of the pitch cylinder (2.4).

9. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 6, characterized in that, The specific formula for the extension and retraction of the rotary cylinder is as follows: ； in: Let be the initial angle of the rotary joint (2.1). ; The specific formula for the extension and retraction of the pitch cylinder (2.4) is as follows: ； in: Let be the initial angle of the pitch joint (2.2). .

10. The hydraulically driven robotic arm control method for a disc-type rock tunneling machine according to claim 6, characterized in that, The specific formula for the translation matrix is ​​as follows: ； in: The translation length; The specific formula for the rotation matrix is ​​as follows: ； Indicates rotation about the z-axis. This indicates rotation about the y-axis.

11. A disc-type rock tunneling machine, used to implement the hydraulically driven robotic arm control method of the disc-type rock tunneling machine as described in any one of claims 1 to 10, characterized in that, The machine includes a tunneling machine body (1), a hydraulically driven robotic arm module (2), a cutting disc (3), and a control module (4). The hydraulically driven robotic arm module (2) is mounted on the tunneling machine body (1). The cutting disc (3) is mounted on the hydraulically driven robotic arm module (2). The control module (4) is electrically connected to the hydraulically driven robotic arm module (2). The control module (4) can control the hydraulic robotic arm to drive the cutting disc (3) to rotate so that the cutting disc (3) can perform cutting operations in a set target posture.

12. The disc-type rock tunneling machine according to claim 11, characterized in that, The hydraulically driven robotic arm module (2) includes a rotary joint (2.1), a pitch joint (2.2), a movable connector (2.3), a rotary cylinder, a pitch cylinder (2.4), and a parallel three-cylinder assembly (2.5). The rotary joint (2.1) is rotatably mounted on the tunneling machine body (1). The first end of the pitch joint (2.2) is rotatably connected to the rotary joint (2.1). The cutterhead (3) is movably connected to the second end of the pitch joint (2.2) through the movable connector (2.3). The rotary cylinder is rotatably connected between the tunneling machine body (1) and the pitch joint (2.2). The rotary cylinder drives the pitch joint (2.2) to drive the rotary joint (2.1). The pitch cylinder (2.4) is rotatably connected between the pitch joint (2.2) and the tunneling machine body (1), and can drive the pitch joint (2.2) to rotate up and down relative to the tunneling machine body (1); the parallel three-cylinder assembly (2.5) is rotatably connected between the pitch joint (2.2) and the cutting disc (3); the parallel three-cylinder assembly (2.5) can drive the cutting disc (3) to rotate left and right and up and down relative to the pitch joint (2.2); the slewing cylinder, the pitch cylinder (2.4) and the parallel three-cylinder assembly (2.5) are electrically connected to the control module (4).

13. The disc-type rock tunneling machine according to claim 12, characterized in that, The parallel three-cylinder assembly (2.5) includes three adjusting cylinders arranged in parallel. One of the adjusting cylinders is located in the vertical plane where the axis of the cutting disc (3) is located, and the other two adjusting cylinders are symmetrically arranged about the vertical plane where the axis of the cutting disc (3) is located.

14. The disc-type rock tunneling machine according to claim 13, characterized in that, The rotary cylinder, the pitch cylinder (2.4), and the adjusting cylinder are all equipped with cylinder stroke sensors.

Images