A method for controlling a hole searching operation of a downhole boring jumbo

Abstract

The application discloses a kind of hole searching operation control methods of underground tunneling trolley, it is related to underground engineering equipment automation control technical field.The method includes two modules of decision solving and control execution;First, obtain the target hole position coordinates that drilling boom of tunneling trolley will carry out drilling, in decision solving module, with the position difference and angle difference between the end pose of drilling boom and target hole position as optimization goal, corresponding target joint pose is solved by using optimization solver;Subsequently in control execution module, each joint is controlled in turn by PID controller, and control error threshold is set for each joint to avoid invalid fine tuning;In the process of each joint in turn execution, the actual pose of current drilling boom is calculated in real time using the posture value returned by joint sensor combined with D-H forward kinematics method, when the position error and angle error of hole searching are all less than the set overall error threshold of hole searching, it is determined that hole searching is completed, directly enter subsequent drilling link.The application effectively avoids long time invalid fine tuning of multi-joint hydraulic system near target position under the premise of ensuring the drilling boom hole searching precision, greatly improves the work efficiency of tunneling trolley.

Description

Technical Field

[0001] This invention relates to the field of industrial robotic arms and automated control technology for mining machinery, and in particular to a method for controlling the borehole-finding operation of an underground tunneling trolley. Background Technology

[0002] Currently, underground mining and tunneling projects face challenges such as high labor intensity and low efficiency, urgently requiring improvements in intelligent and unmanned mine operations. The harsh environment during deep mining and tunneling operations poses significant challenges to personnel safety and health. As a key piece of equipment in underground construction, the intelligent operation of tunneling trolleys, especially automatic borehole finding (drill arm positioning and alignment with the borehole), is crucial for achieving unmanned rock drilling and blasting.

[0003] In existing tunneling trolley control systems, borehole finding primarily relies on traditional hierarchical control or simple inverse kinematics solutions. Subsequently, a lower-level controller (such as a PID controller) drives each hydraulic joint sequentially to the desired position. However, the tunneling trolley's borehole arm is a heavy-duty, multi-degree-of-freedom hydraulic manipulator, characterized by complex structure, strong nonlinearity, and large inertia. In practical operations, it faces the following problems: First, solving the inverse kinematics analytical solution for a multi-degree-of-freedom manipulator is difficult and involves multiple solutions. Second, due to factors such as the dead zone of the hydraulic cylinders, friction, and system back pressure, the lower-level PID controller is prone to oscillations or prolonged ineffective fine-tuning (i.e., "over-adjustment") within a very small error range when driving the joints to approach the target position, resulting in extremely long borehole finding times. Third, traditional sequential control requires strict waiting for all joints to fully converge to the target value before considering the position complete. However, in reality, drilling operations assess the overall posture deviation of the borehole arm's end effector; slight deviations in some joints can often be compensated for by the movement of other joints. Therefore, how to avoid unnecessary waiting and fine-tuning at the bottom of the joint while ensuring the accuracy of hole finding at the end point, and improve the hole finding and operation efficiency of the trolley, has become an urgent technical problem to be solved. Summary of the Invention

[0004] This invention addresses the problems of difficulty in solving inverse kinematics and low operational efficiency caused by prolonged ineffective fine-tuning in the borehole-finding control process of heavy-duty hydraulic drill arms on underground tunneling trolleys. It proposes a borehole-finding operation control method for underground tunneling trolleys.

[0005] To solve the above-mentioned technical problems, the present invention provides the following technical solution:

[0006] A method for controlling borehole tracing operations on an underground tunneling trolley, comprising a calibration conversion module, a decision-solving module, and a control execution module, specifically including the following steps:

[0007] S1. Before the tunneling trolley is parked in place and the hole-finding and drilling operation officially begins, the calibration between the face coordinate system and the trolley coordinate system must be completed first to obtain the target hole position coordinates (i.e., target pose) of the drilling arm of the tunneling trolley to be drilled.

[0008] S2. In the decision-solving module, an optimization function is constructed with the objective of minimizing the positional and angular differences between the drill arm end pose and the target hole coordinates. The optimization solver is used to solve for the corresponding target poses of each joint.

[0009] S3. In the control execution module, according to the target pose of each joint, the PID controller controls each hydraulic joint of the drill arm to perform actions in sequence, and sets the joint control error threshold for each joint. When the joint error is less than the corresponding joint control error threshold, the fine adjustment of the current joint is stopped and the next joint is executed.

[0010] S4. During the sequential execution of actions by each joint, the current attitude value of each joint is obtained in real time through the joint sensor, and the actual position and pose of the current drill arm end is calculated by combining the DH forward kinematics method.

[0011] S5. Determine whether the overall position error and overall angle error between the actual pose of the current drill arm end and the target hole position coordinates are both less than the set hole-finding error threshold. If yes, directly determine that the hole-finding is completed, stop the joint movements and enter the subsequent drilling stage; if no, continue to execute the joint control process in step S3.

[0012] Optionally, the decision-solving module in S2 sets a solution accuracy threshold for the optimization process during the solution process. When the value of the optimization function reaches the solution accuracy threshold, it outputs the corresponding poses of each joint target. The optimization solver includes, but is not limited to, an SLSQP (Sequential Least Squares Programming) optimization solver.

[0013] Optionally, the step in S4 of calculating the actual pose of the current drill arm end in combination with the DH forward kinematics method includes: establishing a coordinate system based on the geometric relationship of the robotic arm structure, collecting displacement / angle signals of the cylinder or joint through joint sensors, completing the transformation from the base coordinate system to the coordinate system of the robotic arm end according to the DH method, and obtaining the Euclidean coordinates and attitude angle of the current drill arm end.

[0014] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following:

[0015] By using a decision solver and optimizer instead of the traditional inverse kinematics equation solving method, and taking the minimization of position and angle differences as the optimization objective, the problem of no analytical solution or multiple solutions for the inverse kinematics of multi-degree-of-freedom heavy-duty drill arms can be solved more flexibly and reliably.

[0016] To address the control characteristics of hydraulic robotic arms, a joint control error threshold was set for the PID controller, effectively preventing the hydraulic drive system from making unnecessary and lengthy integral fine-tuning within a very small error range, thus shortening the control time of a single joint.

[0017] A pre-termination hole-finding control strategy based on overall pose monitoring is proposed. During the sequential execution of joints, the actual end-effector pose is monitored in real time using forward kinematics. When the overall position and angle errors reach the thresholds required by the drilling process, hole finding is completed directly. This breaks away from the rigid process of waiting for all joints to reach absolute accuracy one by one, greatly improving the working efficiency of the tunneling trolley while ensuring hole-finding quality. Attached Figure Description

[0018] Figure 1 To control the flowchart,

[0019] Figure 2 Coordinate alignment flowchart

[0020] Figure 3 Control architecture diagram

[0021] Figure 4 Closed-loop solution - execution process. Detailed Implementation

[0022] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with specific embodiments.

[0023] Example: This embodiment of the invention provides a control method for borehole tracing operations of an underground tunneling trolley. This control method is mainly implemented based on two functional modules: decision solving and control execution. The processing flow of this method includes the following steps:

[0024] like Figure 1 As shown, the hole-finding operation control method in this embodiment proceeds sequentially in the order of "calibration conversion—decision solving—control execution—real-time pose calculation—hole-finding operation determination". Specifically, calibration conversion transforms the target hole position from the face coordinate system to the trolley base coordinate system; decision solving calculates the target pose of each joint based on the target hole position; control execution drives each hydraulic joint to move sequentially via a PID controller; real-time pose calculation continuously acquires the actual pose of the drill arm end during joint movement; and hole-finding operation determination determines whether the current end-position error and angle error meet set thresholds. Through this process, this embodiment can complete hole finding ahead of time when the overall pose of the drill arm end meets the drilling process requirements, without waiting for all joints to reach the theoretical target position.

[0025] S1. Obtain the coordinates of the target hole position where the drilling arm of the tunneling trolley will drill.

[0026] In actual tunnel excavation scenarios, the host computer sends down the three-dimensional spatial coordinates and desired orientation of the target hole to be drilled. These coordinates and orientation constitute the target pose. To align the coordinate system of the target hole with the vehicle's coordinate system, a calibration transformation is required. For example... Figure 2 As shown, the coordinate alignment process in this embodiment includes steps such as the trolley stopping in place, the drill arm perpendicularly contacting the tunnel face, collecting joint sensor data, calculating the attitude matrix based on DH forward kinematics, calculating the inverse transformation matrix, performing coordinate transformation, and outputting the target hole position coordinates. Through this process, the tunnel face coordinates or global tunnel coordinates issued by the borehole planning system can be converted into the target hole position coordinates in the trolley's base coordinate system, enabling the subsequent decision-making and control execution modules to perform borehole finding control based on the trolley's own coordinate system. The process used in this embodiment is as follows:

[0027] The drill arm is positioned vertically against the working face. The operator or an automated control program drives the drill arm to move, causing its tip (the drill bit / lead end) to vertically press against the rock face. At this point, the axis of the drill arm tip aligns with the normal direction of the working face, forming a reference orientation for the working face coordinate system.

[0028] Acquire current joint sensor data, calculate the transformation matrix using the DH forward kinematics method, and extract the rotation matrix. Record the readings of each joint sensor (angle encoder or wire displacement sensor) at this time, and calculate the homogeneous transformation matrix from the trolley base coordinate system to the drill arm end coordinate system using the DH forward kinematics method. Its form is shown in equation (4). Extract the 3×3 submatrix from the top left corner of this transformation matrix, which is the rotation matrix. This matrix represents the orientation transformation relationship between the trolley coordinate system and the working face coordinate system.

[0029] For rotation matrix Inverse the matrix to obtain the coordinate transformation matrix. Since the rotation matrix... It is an orthogonal matrix (satisfying) Its inverse matrix is ​​equal to its transpose matrix, that is:

[0030] (1)

[0031] This property makes matrix inversion degenerate into matrix transpose, resulting in extremely low computational complexity and eliminating the need for general matrix inversion operations. Utilizing... Including translation components, a complete inverse transformation matrix can be constructed to transform coordinate points in the face coordinate system to the trolley coordinate system:

[0032] (2)

[0033] After completing the borehole coordinate transformation, the system proceeds to the subsequent borehole locating control stage. The target borehole coordinates issued by the borehole planning system are usually expressed in the face coordinate system (or the tunnel global coordinate system). After the above coordinate transformation, the three-dimensional coordinates of each target borehole in the trolley coordinate system are obtained. Then, the system enters the inverse kinematics decision module and the control execution module to complete the borehole locating of the drill arm.

[0034] By establishing a coordinate calibration benchmark based on the contact posture between the drill arm and the working face, reliance on external measurement systems is reduced. Alignment between the working face coordinate system and the rig coordinate system can be achieved using the drill arm's own posture and sensor data without the need for additional complex external measuring equipment. Simultaneously, coordinate unification is achieved using DH forward kinematics and inverse rotation matrix transformation, allowing the borehole layout data to be corrected based on the actual docking state of the rig. This reduces the impact of rig docking deviations and working face posture deviations on borehole finding accuracy, improving applicability in complex downhole environments.

[0035] S2, the decision-solving module solves for the target joint pose.

[0036] Considering that hydraulic drill arms typically have multiple degrees of freedom, establishing accurate analytical solutions for inverse kinematics is quite difficult. Therefore, this embodiment employs an optimized solver (such as the SLSQP algorithm) to solve for joint pose.

[0037] In the specific implementation, an objective function is constructed to evaluate the difference between the drill arm pose corresponding to the joint variables and the target hole position. This difference includes two parts: "position difference" and "angle difference," which are weighted and summed using weighting coefficients as the optimization objective. During the solution process, the optimization solver continuously iterates over the joint variables and calls the DH forward kinematics model to predict the end effector pose and calculate the objective function value. Simultaneously, a solution accuracy threshold is set for the optimizer. When the position and angle differences calculated by the objective function are both less than this solution accuracy threshold, the solution is considered converged, and the corresponding joint position command is output as the target pose for subsequent joint control.

[0038] Compared to existing technologies that directly establish inverse kinematics analytical equations and solve for joint variables, this embodiment employs an optimization solver to solve for the target joint pose. This transforms the complex inverse kinematics problem of a multi-degree-of-freedom hydraulic drill arm into an optimization problem that minimizes both position and attitude angle errors. On one hand, this approach does not rely on obtaining a unique, explicit inverse kinematics analytical solution, avoiding the difficulties in analytical modeling caused by the complex structure, coupled degrees of freedom, and numerous motion constraints of heavy-duty drill arms. On the other hand, by simultaneously introducing position and angle differences into the objective function, the solution results not only meet the positional requirements of the target hole but also ensure the attitude consistency between the drill arm's end axis and the drilling direction, thereby improving the matching degree between the hole-finding results and the actual drilling process requirements. Furthermore, the optimization solver can iteratively optimize by combining joint travel, attitude constraints, and solution accuracy thresholds. When multiple feasible joint solutions exist, it can output a target joint pose more suitable for subsequent hydraulic actuation control, reducing the risk of large joint swings, increased control time, or increased end-effector attitude deviations caused by unreasonable inverse kinematic solutions.

[0039] S3, the control execution module drives the joints sequentially.

[0040] After obtaining the target pose of each joint, the underlying control system drives the hydraulic joints one by one based on a PID controller. For example... Figure 3 As shown, the control architecture in this embodiment includes a host computer decision-solving module, PID controllers for each joint, an EPEC controller, an electrical system, a hydraulic system, and joint attitude sensors. After the target hole position is input into the host computer, the inverse kinematics solver or optimization solver calculates the control target value for each joint. The PID controllers for each joint generate control outputs based on the target values ​​and sensor feedback values, and drive the electrical and hydraulic systems through the EPEC controller. Joint attitude sensors such as angle encoders and cable displacement sensors provide real-time feedback on the current joint state, forming a closed-loop control structure consisting of "target hole position - joint target - hydraulic actuation - sensor feedback".

[0041] In this embodiment, to address the common issues of frictional nonlinearity and end-effector oscillations in hydraulic systems due to fluid characteristics, an independent "joint control error threshold" is set for each joint. In PID closed-loop control, once the error between the feedback value of the current joint and its target pose enters the corresponding joint control error threshold range, the PID controller stops outputting fine-tuning commands for that joint and maintains the current valve opening or lockout state. Subsequently, the system automatically triggers the action of the next hydraulic joint. This design effectively avoids the unfavorable situation where the PID controller engages in prolonged ineffective fine-tuning to eliminate extremely small steady-state errors, potentially leading to mechanical oscillations. Figure 4As shown, after obtaining the target pose of each joint, the control system does not require all joints to reach the target pose simultaneously at once. Instead, it executes control actions sequentially according to the joint order. During execution, each joint undergoes closed-loop adjustment via a PID controller. When the error of the current joint enters the corresponding joint control error threshold, the system stops further fine-tuning of that joint and triggers the next joint action. During this process, if the overall pose of the end effector has not yet met the aperture finding requirements, the system can continue to execute subsequent joint control or perform compensation solutions; if the overall pose of the end effector has already met the aperture finding requirements, the aperture finding process can be terminated directly.

[0042] S4. Calculate the current end pose of the drill arm in real time using the forward kinematics method.

[0043] During the sequential actions of the control and execution module, since each joint does not stop fine-tuning prematurely after reaching its absolute target value, there is a certain risk of error accumulation. Furthermore, the drilling process is truly concerned with the relative relationship between the final drill arm end and the target hole position. Therefore, a global monitoring mechanism needs to be introduced.

[0044] Specifically, during the execution of each joint, the system acquires signals in real time via hydraulic cylinder displacement sensors or joint angle encoders. Based on the structural geometry, the state of each joint is obtained, and then the DH forward kinematics method is used to complete the transformation from the trolley base coordinate system to the drill arm end coordinate system. The specific mathematical transformation relationship is as follows:

[0045] First, construct the homogeneous transformation matrix for each joint. Then, the overall transformation matrix of the terminal coordinate system relative to the base coordinate system is obtained through matrix multiplication:

[0046] (3)

[0047] (4)

[0048] The current three-dimensional coordinates of the drill arm end can be extracted using the overall transformation matrix. The attitude matrix is ​​then converted into the current Euler angles, the specific expression of which is shown in equation (3) below:

[0049] (5)

[0050] The advantages of using the above-mentioned real-time forward kinematics calculation method are as follows: This embodiment does not simply rely on whether each joint reaches the target value as the basis for hole finding completion. Instead, it collects joint sensor data in real time and calculates the actual three-dimensional position and posture of the drill arm end in the trolley base coordinate system using the DH forward kinematics model, judging the hole finding status from the overall end pose level. Compared with the existing technology that only relies on joint-level control errors, this method can directly reflect the true deviation between the drill arm end and the target hole position, avoiding misjudgment of hole finding failure due to slight errors in a single joint, and also avoiding the problem of not being detected in time due to the accumulation of errors in multiple joints. At the same time, since drilling operations actually focus on whether the drill bit position and the drilling axis direction meet the construction requirements, obtaining the end position error and angle error through real-time forward kinematics calculation is more in line with the process judgment logic of rock drilling. When the overall end position error and angle error are less than the preset hole finding error threshold, even if some joints have not fully reached the theoretical target value, the system can terminate subsequent invalid fine adjustments in advance, thereby shortening the hole finding time while ensuring hole finding accuracy and improving the continuous operation efficiency of the downhole tunneling trolley.

[0051] S5. Hole-finding operation judgment and control termination.

[0052] After the actual position and orientation of the drill arm are calculated in real time, the control system performs a difference calculation between the actual position and orientation of the drill arm and the target hole position coordinates in step S1 to obtain the overall "position error" and the overall "angle error".

[0053] The system pre-sets "position error thresholds" and "angle error thresholds" for borehole locating control based on the requirements of the drill-and-blast method. When the system determines that the position error and angle error of the current actual pose are both less than the corresponding thresholds, it indicates that even if the sequential execution of each joint has not been completed, or some joints have slight deviations, the current alignment of the robotic arm end effector fully meets the process accuracy requirements for rock drilling. At this point, the system immediately interrupts any subsequent irrelevant joint fine-tuning or remaining actions, determines "hole locating complete," locks the hydraulic cylinders of each joint, and directly issues a command signal to proceed to subsequent drilling stages (such as hole opening, propulsion, impact, etc.).

[0054] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for controlling borehole finding operations on an underground tunneling trolley, characterized in that, The method includes the following steps: S1. Obtain the coordinates of the target hole position where the drilling arm of the tunneling trolley will drill. S2, the decision-solving module solves for the target joint pose. S3. The control execution module sequentially drives the joints. S4. Calculate the current end-effector pose in real time using the forward kinematics method. S5. Hole-finding operation judgment and control termination.

2. The borehole tracing operation control method for an underground tunneling trolley according to claim 1, characterized in that, S1. Obtain the coordinates of the target hole position where the drilling arm of the tunneling trolley will drill. The process adopted is as follows: The drill arm is positioned vertically against the working face. The operator or an automated control program drives the drill arm to move, causing its tip (the drill bit / lead end) to vertically press against the rock wall of the working face. At this point, the axis of the drill arm tip aligns with the normal direction of the working face, forming a reference position for the working face coordinate system. Collect current joint sensor data, calculate the transformation matrix using the DH forward kinematics method and extract the rotation matrix, record the readings of each joint sensor (angle encoder or wire displacement sensor) at this time, and calculate the homogeneous transformation matrix from the trolley base coordinate system to the drill arm end coordinate system using the DH forward kinematics method. Its form is shown in equation (4). The 3×3 submatrix in the upper left corner of the transformation matrix is ​​extracted, which is the rotation matrix. This matrix represents the orientation transformation relationship between the trolley coordinate system and the working face coordinate system. For rotation matrix Inverse the matrix to obtain the coordinate transformation matrix. Since the rotation matrix... It is an orthogonal matrix (satisfying) Its inverse matrix is ​​equal to its transpose matrix, that is: （1） This property makes matrix inversion degenerate into matrix transpose, resulting in extremely low computational complexity. It eliminates the need for a general matrix inversion operation. Including translation components, a complete inverse transformation matrix can be constructed to transform coordinate points in the working face coordinate system to the trolley coordinate system: （2） in, This represents the coordinate vector of the target hole location in the working face coordinate system, and is usually issued by the hole layout planning system or the host computer. This represents the coordinate vector of the same target hole position transformed into the trolley base coordinate system; Let be the reverse rotation transformation matrix from the working face coordinate system to the trolley coordinate system in equation (1). After completing the coordinate transformation of the borehole layout, the subsequent borehole locating control stage begins. The target borehole coordinates issued by the borehole planning system are usually expressed in the face coordinate system (or the tunnel global coordinate system). After the above coordinate transformation, the three-dimensional coordinates of each target borehole in the trolley coordinate system are obtained. Then, the inverse solution decision module and control execution module complete the borehole locating of the drill arm.

3. The borehole finding operation control method for an underground tunneling trolley according to claim 1, characterized in that, S2, the decision-solving module solves for the target joint pose, as detailed below. An objective function is constructed to evaluate the difference between the drill arm pose corresponding to the joint variables and the target hole position. This difference includes two parts: "position difference" and "angle difference". Weighting coefficients are introduced to sum these two parts as the target of minimization. During the solution process, the optimization solver continuously iterates the joint variables and calls the DH forward kinematics model to predict the end effector pose and calculate the objective function value. At the same time, a solution accuracy threshold is set for the optimizer. When the position and angle differences calculated by the objective function are both less than the solution accuracy threshold, the solution is considered to have converged, and the corresponding joint position command is output as the target pose of each joint for subsequent control.

4. The borehole finding operation control method for an underground tunneling trolley according to claim 1, characterized in that, S3. The control execution module sequentially drives the joints, as detailed below. After obtaining the target pose of each joint, the underlying control system drives the hydraulic joints one by one based on a PID controller. To address the common issues of frictional nonlinearity and end-effector oscillations in hydraulic systems caused by fluid characteristics, an independent "joint control error threshold" is set for each joint. In PID closed-loop control, once the error between the feedback value of the current joint and its target pose enters the corresponding joint control error threshold range, the PID controller stops outputting fine-tuning commands for that joint and maintains the current valve core opening or lockout state. Subsequently, the system automatically triggers the action of the next hydraulic joint.

5. The borehole finding operation control method for an underground tunneling trolley according to claim 1, characterized in that, S4. Calculate the current end-effector pose in real time using the forward kinematics method, as detailed below. While each joint is in motion, the system acquires signals in real time through cylinder displacement sensors or joint angle encoders, obtains the state of each joint based on the structural geometry, and then uses the DH forward kinematics method to complete the transformation from the trolley base coordinate system to the drill arm end coordinate system. The specific mathematical transformation relationship is as follows: First, construct the homogeneous transformation matrix for each joint. Then, the overall transformation matrix of the terminal coordinate system relative to the base coordinate system is obtained through matrix multiplication: （3） （4） In the formula Represents the homogeneous transformation matrix of the i-th joint coordinate system relative to the (i-1)-th joint coordinate system; This represents the joint angle or equivalent rotation angle of the i-th joint; This indicates the angle of twist between two adjacent joint axes; This indicates the length of the link between adjacent joint coordinate systems along the common normal direction; This represents the offset or equivalent displacement of the i-th joint along the joint axis. The current three-dimensional coordinates of the drill arm end can be extracted using the overall transformation matrix. The attitude matrix is ​​then converted into the current Euler angles, the specific expression of which is shown in equation (3) below: （5） In the formula , , This represents the current Euler angles at the end of the drill arm, calculated from the attitude matrix, and is used to characterize the spatial attitude of the drill arm end. This represents the arctangent function in the four quadrants, which can determine the quadrant of the angle based on two input components. , , , , The parameters are derived from the overall homogeneous transformation matrix in equation (4), which represents the directional components of the end attitude matrix.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements a borehole locating operation control method for a downhole tunneling trolley as described in any one of claims 1 to 7.

7. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instructions are executed by the processor, they implement a borehole locating operation control method for a downhole tunneling trolley as described in any one of claims 1-7.

Images