Intelligent drilling rig pipe automatic loading and unloading system with precise positioning and attitude monitoring compensation

Abstract

The application discloses an intelligent drilling rig drill rod automatic loading and unloading system with precise positioning and attitude monitoring compensation, which comprises a drill rod box, a mechanical arm and a hand claw at the end and a host machine which are sequentially installed on a vehicle platform; the drill rod box has an automatic alignment positioning function and comprises a box support, a drill rod positioning frame, a blocking rod, a front warehouse door, a radial positioning thrust mechanism and an axial positioning thrust mechanism; the hand claw end of the mechanical arm is provided with three laser ranging sensors and constitutes a fixed plane as a measuring point plane; the host machine has an attitude monitoring compensation function and comprises a centralizer, a gripper and a power head which are sequentially arranged on a feeding machine body, a high-precision laser ranging sensor, an open hole orientation instrument, a displacement sensor I, an inclination sensor, a displacement sensor II, a controller and a target plate. The application can improve the initial positioning accuracy of the drill rod, the target positioning accuracy of the host machine and the attitude centering accuracy of the mechanical arm end in the intelligent drilling rig drill rod automatic loading and unloading system in a coal mine, thereby improving the overall working efficiency.

Description

Technical Field

[0001] This invention belongs to the technical field of gas extraction and control equipment, and relates to an intelligent drilling rig automatic loading and unloading system with precise positioning and attitude monitoring and compensation functions. Background Technology

[0002] Traditional drilling rigs used for underground gas drainage drilling in coal mines lack automatic drill rod loading and unloading capabilities, relying primarily on manual labor to transfer drill rods between the surface and the rig. However, with advancements in electro-hydraulic control technology for underground coal mine drilling rigs, their automation levels have significantly improved. More and more automatic drilling rigs are being deployed in underground coal mines, featuring built-in drill rod storage, automatic drill rod loading and unloading, and automatic drilling. This saves time and labor during construction, significantly improving overall efficiency and safety.

[0003] The intelligent drilling rig's automatic drill pipe loading and unloading system mainly consists of three parts: the drill pipe magazine, the robotic arm, and the main unit. The drill pipe magazine is responsible for storing drill pipes, the robotic arm is responsible for transferring drill pipes between the drill pipe magazine and the main unit, and the main unit is responsible for connecting or disconnecting drill pipes transferred by the robotic arm. These three parts are independent yet interconnected. The positional control accuracy of the drill pipe from its initial position in the drill pipe magazine to its working position on the main unit directly determines the reliability and efficiency of the automatic drilling rig's drill pipe loading and unloading system. However, even if all three parts are fixed on the same drilling rig platform during the machining and assembly process, cumulative errors caused by machining, assembly, and sensor accuracy cannot be avoided. The presence of secondary transfer mechanisms further increases the factors influencing these errors. Therefore, improving the control accuracy of each mechanism is crucial for enhancing the reliability of the automatic drilling rig's drill pipe loading and unloading system.

[0004] Existing intelligent drilling rigs generally use a row-style drill rod magazine, with baffles between each row. A robotic arm systematically grabs drill rods from each baffle. While the precision required for controlling the robotic arm's gripping of drill rods is relatively low, the presence of baffles occupies space in the drill rod storage device, significantly reducing the storage capacity. Furthermore, because the row spacing of the row-style magazine is fixed, it can only accommodate drill rods of a single specification, limiting its applicability. Additionally, while the fixed distance between each row in the row-style magazine ensures contact between the drill rods, the radial gap between the drill rod and the baffle facilitates the robotic arm's loading and unloading of drill rods. Therefore, the centers of each row of drill rods are not perfectly aligned, resulting in some error. Additionally, a large amount of space is left in the axial direction of the drill rods. This is to facilitate automatic or manual placement of drill rods into the rod magazine, reducing the precision requirements for placement. However, this amplifies the axial positioning error. Therefore, after the robotic arm grasps the drill rods, a subsequent transfer mechanism is needed to position each drill rod radially and axially to improve the initial positioning accuracy of the drill rods to be added. However, this method is relatively complex in structure and steps, inefficient, and adds a secondary transfer structure, increasing the factors influencing error.

[0005] To meet the needs of different inclination angles and different hole opening operations, there are many types of luffing devices for existing downhole drilling rigs. For conventional fully hydraulic drilling rigs, there is no precision requirement for the final attitude of the luffing. It is sufficient to roughly match the design attitude parameters (such as pitch angle, inclination angle, distance from the hole opening, and hole opening height). Moreover, once adjusted, there is no correction or compensation function in the later stage. However, for current intelligent downhole drilling rigs, the final attitude accuracy of the rig is directly related to the target position of the robotic arm. Theoretically, the target position of the drill rod to be added is relatively fixed, and the robotic arm can accurately find the target point for the drill rod delivery. However, in actual construction, due to inertia or after long-term continuous work, vibration will cause changes in the attitude of the drilling rig, which will lead to changes in the target position of the drill rod to be added. Existing sensors can only monitor without feedback error function. If no intervention and adjustment are made, it will cause the drill rod loading and unloading system to malfunction. Therefore, the attitude error information of the amplitude transformer needs to be returned to the controller in real time, and a feedback compensation model needs to be established so that the controller can clearly judge the real-time status of the host and correct it. However, the existing technology for host attitude control is generally open-loop and offline control, which has problems such as low accuracy, large positioning error and inability to compensate for error. It is difficult to achieve accurate positioning of the host attitude, which has become a key factor restricting the efficiency of the automatic loading and unloading system of intelligent drilling rigs.

[0006] The drill pipe loading and unloading system's multi-degree-of-freedom robotic arm is responsible for transferring drill pipes from the rod magazine to the main unit's gripper, placing the drill pipe in a suitable position between the main unit's active drill pipe and the gripper, and ensuring complete alignment with the center of their axes. Since the rod magazine, robotic arm, and main unit are on a relatively fixed platform, the robotic arm's end effector can be accurately calculated using matrix transformation based on the sensor's detection of the rod magazine's initial position and end effector coordinates. However, this calculated coordinate value is theoretical and does not consider issues such as the robotic arm's own errors, the accuracy of the main unit's sensors, the main unit's attitude transformation, and assembly errors between the three components. If the robotic arm's end effector coordinates deviate significantly from the final target position, it may cause the robotic arm's gripper to collide with the active drill pipe or the gripper. Furthermore, if the axis is severely eccentric when the drill pipe is inserted into the gripper, the robotic arm's end effector will bear a huge reaction force from the drill pipe after clamping, severely impacting the robotic arm's joint structure and positioning accuracy, and significantly affecting the loading and unloading system's accuracy and stability. Summary of the Invention

[0007] To address the shortcomings of existing technologies, the present invention aims to provide an intelligent drilling rig automatic drill rod loading and unloading system with precise positioning and attitude monitoring compensation. This system features a staggered drill rod magazine that can quickly and automatically adapt to different drill rod specifications, automatically adjust the spacing between different robotic arms gripping drill rods, and automatically position the drill rods. This improves the adaptability of the drill rod storage device to intelligent drilling rigs and enhances the initial position accuracy of the drill rod to be added. The system analyzes the causes of host posture errors and, addressing the non-real-time nature of classic error compensation methods, proposes an online host posture error detection method and establishes an error compensation model to achieve precise positioning of the host and the target position of the drill rod to be added. Finally, a monitoring and centering accuracy feedback device is added to the end effector of the robotic arm gripper to correct the end effector's posture and improve the positioning accuracy of the drill rod delivery target.

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

[0009] The intelligent drilling rig drill pipe automatic loading and unloading system with precise positioning and attitude monitoring compensation includes a drill pipe box, a robotic arm and its end gripper, and a main unit, which are sequentially installed on the vehicle platform.

[0010] The drill rod box has an automatic alignment and positioning function, including a box support, a drill rod positioning frame, a stop bar, a front compartment door, a radial positioning thrust mechanism, and an axial positioning thrust mechanism. The drill rod positioning frame is installed at the bottom of the box support, and the drill rod positioning frame has equally spaced grooves to place and position the drill rods. The stop bar is detachably installed on the upper part of the box support to limit the height of the drill rods inside the box and prevent the drill rods from shaking during transportation. The drill rod positioning frame and the stop bar are parallel to each other and perpendicular to the drill rods. The front compartment door is detachably installed at the front end of the box support. The radial positioning thrust mechanism is installed on the side plates on both sides of the box support and is tangent to the drill rods located at the even-numbered end. It can push the drill rods radially to ensure that the spacing between adjacent drill rods is equal, thus ensuring the radial positioning accuracy of the drill rods. The axial positioning thrust mechanism is installed on the rear baffle at the rear end of the box support and can move along the drill rod axis to push the entire drill rod to the front compartment door so that the front ends of each drill rod are aligned, thus ensuring the axial positioning accuracy of the drill rods.

[0011] The robotic arm's gripper end is equipped with three laser rangefinders, which are arranged at 90° intervals along the left, top, and right ends of the drill rod to be drilled. The three laser rangefinders are parallel to the drill rod to be drilled, and the measuring points of the three laser rangefinders form a fixed plane as the measuring point plane.

[0012] The host machine has attitude monitoring and compensation functions, including a stabilizer, a gripper, and a power head sequentially mounted on the feed body. It also includes a high-precision laser rangefinder, a hole orientation instrument, displacement sensor I, a tilt sensor, displacement sensor II, a controller, and a target plate. The high-precision laser rangefinder is located at the rear of the power head, and the laser emitted from its laser generator window can pass through the inner hole of the power head, the central through hole of the drill rod, the center of the gripper and the unlatcher, and be projected onto the target plate in front of the host machine. The laser reflected from the target plate can also be projected onto the laser receiving window. The hole orientation instrument is located at the high-precision... Behind the laser rangefinder sensor, a reference is provided for the initial zero position of the tilt angle; displacement sensor I is used to measure the lifting height of the host; the tilt angle sensor is used to measure the initial input tilt angle value; displacement sensor II is used to measure the translational displacement of the host; the data acquisition board in the controller is connected to the high-precision laser rangefinder sensor to obtain the distance value of the high-precision laser rangefinder sensor in real time. The data acquisition board in the controller is also connected to the hole orientation instrument, displacement sensor I, tilt angle sensor and displacement sensor II to dynamically adjust the host attitude to determine the zero position value measured by the hole orientation instrument.

[0013] The present invention also includes the following technical features:

[0014] Specifically, the box support includes a square frame at the bottom, vertical columns, side panels, and a rear baffle. The square frame is formed by two bottom end beams and two bottom side beams. Four vertical columns are located at the four corners of the square frame. The four side panels are connected to the four vertical columns respectively, and every two side panels are located on the same bottom side beam. The rear baffle is located on the rear bottom end beam and is connected to the two vertical columns at its two ends.

[0015] The drill pipe positioning frame is installed between two bottom side beams. The groove of the drill pipe positioning frame can be a wave-shaped groove, a square groove, a trapezoidal groove, a triangular groove, or a semi-circular groove.

[0016] The stop bar is detachably connected to the upper part of two opposing side plates.

[0017] Specifically, the front compartment door and the rear baffle are parallel to each other and detachably installed on two vertical columns at both ends of the front bottom end beam. The two ends of the front compartment door are screwed to the vertical columns and can be removed after the drill rod is automatically aligned, so as to facilitate the robot arm to grab the drill rod. The outer wall of the front compartment door is bolted with a stop strip, the two ends of the stop strip are inserted into the slots on the vertical columns and the stop strip is detachable.

[0018] Specifically, the radial positioning thrust mechanism includes a thrust cylinder I and a pad connected to its end. The thrust cylinder I passes through and is fixed on the side plate. The pad is parallel to the inside of the side plate and can move radially along the drill rod to be tangent to the drill rod at the end of the even-numbered layers and ensure that the spacing between adjacent drill rods is equal.

[0019] The axial positioning thrust mechanism includes a thrust cylinder II and a thrust plate connected to its end. The thrust cylinder II passes through and is fixed on the rear baffle. The thrust plate is arranged parallel to the inner side of the rear baffle and can move along the drill rod axial direction to push the drill rod to the front chamber door so that the front ends of each drill rod are aligned as a whole to ensure the axial positioning accuracy of the drill rod.

[0020] An intelligent drilling rig automatic drill rod loading and unloading method with precise positioning and attitude monitoring compensation, implemented through the aforementioned system, includes the following steps:

[0021] Step 1: Input the specifications of the drill rods in the drill rod box and the required drill rod spacing on the controller. The drill rod positioning frame corresponding to the specifications of the drill rods rises from the bottom of the drill rod box. The pads of the radial positioning thrust mechanism on both sides move parallel to the inside and outside of the drill rod box to limit the initial position of the drill rods at both ends. After the drill rods are filled in the drill rod box, the rear axial positioning thrust mechanism pushes the drill rods in the drill rod box axially in multiple layers. Under the limiting action of the front compartment door, the drill rods are axially positioned and aligned.

[0022] Step 2: Place the hole orientation instrument at the tilt angle rotation center position, adjust the tilt angle attitude using the host attitude adjustment module, find the tilt angle zero position using the feedback signal of the hole orientation instrument, and read the distance measured by the high-precision laser rangefinder at the tilt angle zero position; after changing the tilt angle, read the distance measured by the high-precision laser rangefinder again, thereby obtaining multiple sets of tilt angle input values ​​and the actual tilt angle value calculated by the distance measured by the high-precision laser rangefinder.

[0023] Step 3: Calculate the opening position coordinates using the actual tilt angle, and determine the offset of the tilt angle error in the height direction;

[0024] Step 4: When the tilt angle changes, the position of the end opening will have an azimuth deflection angle, and at the same time, it will also have an x-axis offset. Based on the changes in the tilt angle deflection angle and offset, a virtual joint is introduced, and a coordinate transformation matrix is ​​added to compensate for the error caused by the tilt angle joint. The tilt angle pose error is compensated, and the tilt angle error compensation model is established, which is the host attitude error compensation model.

[0025] Step 5: Use the RBF neural network to solve the inverse solution of the error compensation model matrix. The obtained inverse solution is the new attitude control input after error compensation, ensuring that the target position remains unchanged.

[0026] Step 6: The robotic arm clamps the drill rod center position positioned in Step 1 from the drill rod box. Using the new attitude control input after error compensation in Step 5, the robotic arm transfers the drill rod to be added to the target position of the host. The three laser rangefinders at the end of the robotic arm gripper form a measuring point plane, and the projection target point is set on the edge of the target disk at the end of the active drill rod. When the monitoring distances of the three laser rangefinders are exactly the same and the same as the set value, the axis of the drill rod to be added is completely aligned with the center position of the active drill rod and the gripper. When the monitoring distance of one or more laser rangefinders is not equal to the set value, the position of the drill rod to be added is not aligned. Error compensation must be performed on the coordinate calculation of the attitude of the robotic arm end until the drill rod to be added is completely aligned with the center of the gripper and the active drill rod. Only then can the gripper clamp the drill rod, and the robotic arm gripper releases to proceed to the next action.

[0027] Specifically, in step 1, the pads of the radial positioning thrust mechanism on both sides of the drill rod box are tangent to the drill rods located at the even-numbered layer ends, and ensure that the spacing between each layer of drill rods is always equal to the spacing between the bottom layer positioning drill rods. The layer height and the spacing change in a linear proportion to ensure the radial and odd-even layer positioning accuracy of the drill rods.

[0028] The distance between the two thrusters on both sides of the drill pipe box is L, n 偶 Let be the number of even-numbered drill pipe layers in the drill pipe box, D be the drill pipe diameter, a be the spacing between adjacent drill pipes in the same layer, and h be the height difference between two adjacent drill pipe layers, defined as follows:

[0029] L = n 偶 D+(nl)a (1)

[0030]

[0031] Specifically, in step 1, the thrust cylinder II of the axial positioning thrust mechanism is a single-acting cylinder. Under normal conditions, the thrust cylinder II remains in a retracted state, and the thrust plate does not contact the drill rod. When the action is performed, oil enters the rodless chamber of the thrust cylinder II, causing the piston rod to extend and push the thrust plate. At the same time, it drives the entire drill rod to move towards the front door position. After the front end of the drill rod is aligned, the thrust plate retracts to ensure the axial positioning accuracy of the drill rod. After the drill rod is aligned along the axis, the stop bar is removed from the slot and taken off together with the front door, so that the front door is open, which facilitates the lifting action of the robot arm after grasping the drill rod.

[0032] Specifically, in step 2, the actual tilt angle calculated based on the measured distance is:

[0033]

[0034] In the above formula, θ2' is the actual tilt angle, and L c1 It is the distance L measured by a high-precision laser rangefinder after the drill arm passes the initial horizontal point found by the hole positioning instrument. d1This refers to the distance between the mounting point of the high-precision laser rangefinder sensor in the Y direction and the center of rotation. When the tilt angle θ2 is input, the change in the tilt angle causes a change in the measurement distance of the high-precision laser rangefinder sensor. z1 It is the distance measured by a high-precision laser rangefinder after the tilt angle changes, L y This refers to the translation distance of the drill arm translation cylinder.

[0035] Specifically, in step 3, the x, y, and z coordinates of the opening position are calculated based on the actual tilt angle:

[0036]

[0037] In the above formula, H0 is the initial distance of the drill arm lifting, and the lifting height is △H1; the difference between the z-coordinate of the end of the borehole opening position under the actual inclination angle and the z-coordinate of the theoretical inclination angle is the offset of the inclination angle error in the height direction.

[0038] Specifically, in step 4, when the tilt angle changes, the end opening position will generate an azimuth deflection angle Δθ2, and also an x-axis offset Δz. A virtual joint is introduced, and a coordinate transformation matrix is ​​added to compensate for the error caused by the tilt angle joint, thus compensating for the tilt angle pose error. The tilt angle error compensation model after introducing the virtual joint is as follows:

[0039]

[0040] In the above formula, It is the homogeneous transformation matrix between the coordinate systems of the second and first adjacent joints;

[0041] Trans(Δz,0,0) is a translation Δz transformation about the x-axis; Rot(y,Δθ2) is a rotation Δθ2 transformation about the y-axis.

[0042] Specifically, in step 6, the distance from the center of the three laser rangefinders to the center of the drill rod to be added is R, and the monitoring distances of the three laser rangefinders are L1, L2 and L3 respectively; there are two target disks at the end of the active drill rod that are parallel to each other, namely a primary target disk and a secondary target disk. The radius of the primary target disk is also R, and the radius of the secondary target disk is R1, and R1 is greater than R. The distance between the two target disks is m.

[0043] Specifically, in step 6, when the three laser rangefinders monitor the same distance and the same as the set value, the axis of the drill rod to be added is completely aligned with the center position of the active drill rod and the clamp. At this time, the points emitted by the three laser rangefinders to the edge of the target disk are Z1, Z2, and Z3, which satisfy the relationship: L1 = L2 = L3 = L (6). In the above formula, the set value of the distance from the three laser rangefinders to the active drill rod is L.

[0044] Specifically, in step 6, if the monitoring distance of one or more laser rangefinders is not equal to the set value, i.e., does not satisfy equation (6), then the position of the drill rod to be added is not aligned. At this time, the actual points emitted by the three laser rangefinders to the edge of the target disk are Z1'(x1 y1 z1), Z2'(x2 y2 z2), and Z3'(x3 y3 z3). Error compensation must be performed on the coordinates of the robot arm end posture calculation, including the following two situations:

[0045] (I) When the absolute value of the distance difference between any two laser rangefinders is equal to 0 or m, it proves that the plane of the measuring points of the three laser rangefinders is parallel to the target disk. At this time, the axis of the drill rod to be added held by the end of the robotic arm gripper is parallel to the axis of the active drill rod. Then the error between the posture of the end of the robotic arm and the target point is the eccentric displacement error of the X and Y axes. The tilt angle posture only has the rotation angle around the Z axis. Then its total error value is Equation (8). The error compensation model is as shown in Equation (9). First, the rotation angle error is compensated to make the coordinate system of the end of the robotic arm parallel to the coordinate system of the target point. Then, the displacement is compensated according to the eccentric error.

[0046]

[0047] Δ T 1=Trans(x,Δx)×Rot(Z,Δθ z )×Trans(y, Δy)×Trans(z, Δz) (9)

[0048] In the above formula, ΔT1 is the total error under condition (I), x1 is the calculated value of point Z1 on the X-axis in the end coordinate system, y1 is the calculated value of point Z1 on the Y-axis in the end coordinate system, L1 is the laser sensor ranging value of point Z1, and x3 is the calculated value of point Z3 on the X-axis in the end coordinate system.

[0049] (II) When the distance difference between any two laser rangefinders does not satisfy the conditions of equation (6) and case (I), it is proven that the plane of the measuring point of the three laser rangefinders is not parallel to the target disk. First, the posture of the end of the robot arm is transformed by the angle compensation in the rotation direction around the X and Y axes so that the plane of the measuring point of the three laser rangefinders is parallel to the target disk. Then, according to the compensation of the eccentric displacement error in the X and Y axis directions and the rotation angle around the Z axis in step (I), the final corrected posture is obtained.

[0050] When performing angle compensation transformation on the end effector posture of the robotic arm in the rotational directions around the X and Y axes, the offset tilt angle Δθ y :

[0051]

[0052] In the above formula, Δθ yR is the tilt angle error of the actual end attitude around the Y-axis of the end coordinate system, R is the distance from the center of the three laser rangefinders to the center of the drill rod to be added, m is the distance between the target disks at the ends of the two active drill rods, L2 is the laser ranging value at point Z2, L3 is the laser ranging value at point Z3, R is the radius of the first-level target disk, and R1 is the radius of the second-level target disk.

[0053] Offset azimuth angle Δθ x :

[0054]

[0055] In the above formula, Δθ x L1 is the azimuth error of the actual end attitude around the X-axis of the end coordinate system, and L1 is the laser ranging value at point Z1.

[0056] Compensation for eccentric displacement errors in the X and Y axes and rotation angles around the Z axis:

[0057] ΔT2=Rot(x,Δθ x )×Rot(y,Δθ y )×ΔT1 (12)

[0058] In the above formula, ΔT2 is the total error under condition (II).

[0059] Compared with the prior art, the present invention has the following technical effects:

[0060] This invention enables automatic and rapid matching of drill rods of different specifications within the drill rod chamber of an intelligent drilling rig, as well as automatic axial and radial positioning, improving the initial positioning accuracy of the drill rod to be added, eliminating the need for a secondary positioning mechanism, and reducing the impact of error factors. It also achieves precise closed-loop control of the host machine's attitude, providing the controller with compensated, precise spatial coordinates of the drill rod to be added, thus improving the target positioning accuracy of the drill rod loading and unloading system. Employing a six-degree-of-freedom robotic arm structure, it features end-effector alignment detection and error compensation functions, resulting in higher repeatability. With the drill rod chamber, robotic arm, and host machine fixed on a unified platform, their spatial coordinates are relatively fixed and unaffected by orientation, attitude, or platform leveling, thereby improving the efficiency and adaptability of the intelligent drilling rig's automatic drill rod loading and unloading system. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the overall system structure of the present invention;

[0062] Figure 2 This is a schematic diagram of the drill pipe box structure of the present invention;

[0063] Figure 3 This is a schematic diagram of the drill pipe box structure of the present invention;

[0064] Figure 4 This is a schematic diagram of the automatic radial positioning of the drill pipe according to the present invention;

[0065] Figure 5 This is a schematic diagram of the automatic axial positioning of the drill pipe according to the present invention;

[0066] Figure 6 This is a schematic diagram of the system and target plate layout of the present invention;

[0067] Figure 7 This invention is based on a high-precision laser ranging pose error detection and calculation model for calculating the actual tilt angle error.

[0068] Figure 8 This is the coordinate transformation for the tilt error compensation model of the present invention;

[0069] Figure 9 This is a schematic diagram of the robotic arm with a lever attached at the end of the present invention.

[0070] Figure 10 This is a schematic diagram of the end-effector compensation principle of the robotic arm of the present invention;

[0071] Figure 11 This is the robotic arm end-effector posture error compensation model in Case I of the present invention;

[0072] Figure 12 This is the robotic arm end-effector attitude azimuth error compensation model in Scenario II of the present invention;

[0073] Figure 13 This is the robotic arm end-effector tilt angle error compensation model for Scenario II of the present invention.

[0074] Meaning of the symbols in the attached image:

[0075] 1. Drill rod box, 2. Robotic arm, 3. Gripper, 4. Main unit, 5. Vehicle platform, 6. Drill rod, 7. Active drill rod, 8. Primary target disk, 9. Secondary target disk; 11. Box support, 12. Drill rod positioning frame, 13. Stop bar, 14. Front door, 15. Radial positioning thrust mechanism, 16. Axial positioning thrust mechanism, 111. Square frame, 112. Vertical column, 113. Side plate, 114. Rear baffle, 141. Stop bar, 151. Thrust cylinder I, 152. Pad plate, 161. Thrust cylinder II, 162. Thrust plate, 41. Centralizer, 42. Clamp, 43. Power head, 44. High-precision laser rangefinder, 45. Hole opening orientation instrument, 46. Target plate. Detailed Implementation

[0076] The following are specific embodiments of the present invention. It should be noted that the present invention is not limited to the following specific embodiments. All equivalent modifications made based on the technical solutions of this application fall within the protection scope of the present invention.

[0077] Example 1:

[0078] This embodiment provides an intelligent drilling rig drill pipe automatic loading and unloading system with precise positioning and attitude monitoring compensation, such as... Figures 1 to 6 As shown, it includes a drill pipe box, a robotic arm and its end gripper, and a main unit, which are sequentially installed on the vehicle platform;

[0079] The drill rod box features automatic alignment and positioning, comprising a box support, a drill rod positioning frame, a stop bar, a front compartment door, a radial positioning thrust mechanism, and an axial positioning thrust mechanism. The drill rod positioning frame is located at the bottom of the box support, with equally spaced grooves to hold and position the drill rods. The stop bar is detachably mounted on the upper part of the box support to limit the height of the drill rods inside the box and prevent them from swaying during transport. The drill rod positioning frame and the stop bar are parallel to each other and perpendicular to the drill rods. The front compartment door is detachably mounted on the front end of the box support. The radial positioning thrust mechanism is mounted on the side plates on both sides of the box support and tangent to the drill rods located at the even-numbered end. It can push and position the drill rods radially to ensure equal spacing between adjacent drill rods. The axial positioning thrust mechanism is mounted on the rear baffle at the rear end of the box support and can move axially along the drill rods to push the entire drill rod to the front compartment door, aligning the front ends of each drill rod and ensuring axial positioning accuracy.

[0080] The robotic arm's gripper end is equipped with three laser rangefinders, which are arranged at 90° intervals along the left, top, and right ends of the drill rod to be drilled. The three laser rangefinders are perpendicular to the side of the gripper end and parallel to the drill rod to be drilled, so that the measuring points of the three laser rangefinders form a fixed plane as the measuring point plane.

[0081] The main unit has attitude monitoring and compensation functions, including a stabilizer, a gripper, and a power head sequentially mounted on the feed body. It also includes a high-precision laser rangefinder, a hole orientation instrument, displacement sensor I, an inclination sensor, displacement sensor II, a controller, and a target plate (the target plate surface is flat and parallel to the main unit's body; the target plate size is set according to the designed hole height and inclination angle; the target plate can receive and reflect laser signals). The high-precision laser rangefinder is located at the rear of the power head, and the laser emitted from its laser generator window can pass through the inner hole of the power head, the central through hole of the drill rod, the center of the gripper and the unlatcher, and be projected onto the target plate in front of the main unit. The laser reflected from the board can also be projected onto the laser receiving window; the hole orientation instrument is located behind the high-precision laser rangefinder and is used as a reference for the initial zero position of the tilt angle; displacement sensor I is used to measure the lifting height of the host; tilt sensor is used to measure the initial input tilt angle value; displacement sensor II is used to measure the translational displacement of the host; the data acquisition board in the controller is connected to the high-precision laser rangefinder to obtain the distance value of the high-precision laser rangefinder in real time. The data acquisition board in the controller is also connected to the hole orientation instrument, displacement sensor I, tilt sensor and displacement sensor II to dynamically adjust the host attitude to determine the zero position value measured by the hole orientation instrument.

[0082] The box frame includes a square frame at the bottom, vertical columns, side panels, and a rear baffle. The square frame is formed by two bottom end beams and two bottom side beams. Four vertical columns are located at the four corners of the square frame. The four side panels are connected to the four vertical columns respectively, and every two side panels are located on the same bottom side beam. The rear baffle is located on the rear bottom end beam and is connected to the two vertical columns at its two ends.

[0083] The drill pipe positioning frame is set between two bottom side beams. The groove of the drill pipe positioning frame can be wavy, square, trapezoidal, triangular, or semi-circular, etc. Various specifications of drill pipe positioning frames are arranged and combined according to different outer diameters and spacings of drill pipes. The drill pipe positioning frames are all placed on the bottom plate of the box support. Each type of frame is arranged in pairs along the drill pipe axis and fixed to the bottom thrust mechanism (the bottom thrust mechanism includes thrust cylinder III and the push plate connected to its end). When the appropriate drill pipe specifications and spacing are determined, the corresponding drill pipe positioning frame is raised under the action of the bottom thrust mechanism. The drill pipe positioning frame is installed in conjunction with the slot on the bottom plate of the box support to ensure that the number and position of the first layer of drill pipes and the odd-numbered layers of drill pipes stacked on top are always equal after positioning.

[0084] The stop bar is detachably connected to the upper part of two opposite side plates. When the drilling rig is working, the stop bar is removed and placed into the two sets of support through holes behind the box bracket.

[0085] The front compartment door and the rear baffle are parallel to each other and detachably installed on two vertical columns at both ends of the front bottom end beam. The two ends of the front compartment door are screwed to the vertical columns and can be removed after the drill rod is automatically aligned, so that the robot can easily grab the drill rod. The outer wall of the front compartment door is bolted with a stop strip, the two ends of the stop strip are inserted into the slots on the vertical columns and the stop strip is detachable.

[0086] The radial positioning thrust mechanism includes a thrust cylinder I and a pad connected to its end. The thrust cylinder I passes through and is fixed to the side plate. The pad is parallel to the inside of the side plate and can move radially along the drill rod to be tangent to the drill rods at the ends of even-numbered layers and ensure that the spacing between adjacent drill rods is equal. The spacing between adjacent drill rods in each layer is always equal to the spacing between adjacent drill rods in the bottom positioning drill rod. The drill rods are stacked in the drill rod box. By adjusting the stroke of the radial positioning thrust mechanism and adapting to the corresponding drill rod positioning frame, the positioning requirements of drill rods of different diameters can be met to adapt to different forms of hand gripping. Based on the specifications of the replaced drill rod positioning frame, the specifications and spacing of the drill rods from the bottom first layer are restricted.

[0087] In other embodiments, the radial positioning thrust mechanism can be an adjustment pad, which is installed in a slot on the inner wall of the side plate and fixed with bolts. This restricts the even-numbered drill rods stacked from the second layer from the bottom upwards. Depending on the specifications of the replaced drill rod positioning frame, adjustment pads of different thicknesses are adapted so that the adjustment pads on both sides are exactly tangent to the outermost drill rod, and the spacing between the drill rods in this layer is always equal to the initial spacing between the bottom layer drill rods, thereby reducing the requirements of the robot arm for the control accuracy of gripping the drill rods.

[0088] The axial positioning thrust mechanism includes a thrust cylinder II and a thrust plate connected to its end. The thrust cylinder II passes through and is fixed to the rear baffle. The thrust plate is parallel to the inner side of the rear baffle and can move along the drill rod axially to push the drill rod to the front compartment door, ensuring that the front ends of each drill rod are aligned and guaranteeing the axial positioning accuracy of the drill rod. In this embodiment, the cylinder barrel of the thrust cylinder II is fixed to the rear baffle of the housing support, and the piston rod is fixed together with the thrust plate. The thrust cylinder II is a single-acting cylinder. Normally, the thrust cylinder II remains in the retracted state, that is, the thrust plate is not in contact with the drill rod. When it is activated, oil enters the rodless chamber of the thrust cylinder II, causing the piston rod to extend and push the thrust plate. At the same time, it drives the drill rod as a whole to move towards the front compartment door position of the housing. After the front ends of the drill rod are aligned, the thrust plate retracts. After the drill rod is aligned along the axis, the stop bar is removed from the slot at the front end of the housing support and removed together with the front compartment door, opening the front compartment door and facilitating the lifting action of the robot after grasping the drill rod. Because the drill pipe capacity in the rod magazine is large, it is not easy to push the drill pipe as a whole by the thrust cylinder. Therefore, the axial positioning thrust mechanism can be distributed into multiple parts from top to bottom along the rod magazine according to the actual situation, and pushed in layers to reduce the number of drill pipes pushed by a single axial positioning thrust mechanism and achieve flexible pushing.

[0089] This embodiment relies on the bottom drill rod positioning frame and the radial positioning thrust mechanisms or adjusting pads on both sides to achieve staggered storage of drill rods from bottom to top, increasing the storage capacity of drill rods within a limited space. Simultaneously, different specifications of drill rod positioning frames and adjusting pads on both sides can be replaced to match different specifications of drill rods and adapt to different spacing requirements for the robotic arm's gripping of drill rods. To achieve axial alignment of the drill rods within the rod magazine, improve the initial positioning accuracy of the drill rods, and simplify the positioning and alignment mechanism after the robotic arm grips the drill rods, the front compartment door fixed at the front end cooperates with the rear axial positioning thrust mechanism to achieve axial alignment of the drill rods within the rod magazine.

[0090] Example 2:

[0091] This embodiment provides an intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation. The method is implemented through the system of Embodiment 1 and includes the following steps:

[0092] Step 1: Input the specifications of the drill rods in the drill rod box and the required drill rod spacing on the controller. The drill rod positioning frame corresponding to the specifications of the drill rods rises from the bottom of the drill rod box. The pads of the radial positioning thrust mechanism on both sides move parallel to the inside and outside of the drill rod box to limit the initial position of the drill rods at both ends. After the drill rods are filled in the drill rod box, the rear axial positioning thrust mechanism pushes the drill rods in the drill rod box axially in multiple layers. Under the limiting action of the front compartment door, the drill rods are axially positioned and aligned.

[0093] Specifically, in step 1, the pads of the radial positioning thrust mechanism on both sides of the drill rod box are tangent to the drill rods located at the even-numbered layer ends, and ensure that the spacing between drill rods in each layer is always equal to the spacing between the bottom layer positioning drill rods. The layer height and the spacing change in a linear proportion to ensure the radial and odd-even layer positioning accuracy of the drill rods.

[0094] like Figure 4 As shown, the distance between the two thrusters on both sides of the drill pipe box is L, n 偶 Let be the number of even-numbered drill pipe layers in the drill pipe box, D be the drill pipe diameter, a be the spacing between adjacent drill pipes in the same layer, and h be the height difference between two adjacent drill pipe layers, defined as follows:

[0095] L = n 偶 D+(nl)a (I)

[0096]

[0097] Specifically, in step 1, the thrust cylinder II of the axial positioning thrust mechanism is a single-acting cylinder. Under normal conditions, thrust cylinder II remains in a retracted state, and the thrust plate does not contact the drill rod. During operation, oil enters the rodless chamber of thrust cylinder II, causing the piston rod to extend and push the thrust plate. At the same time, it drives the entire drill rod to move towards the front door position. After the front end of the drill rod is aligned, the thrust plate retracts to ensure the axial positioning accuracy of the drill rod. After the drill rod is aligned along the axis, the stop bar is removed from the slot and taken off together with the front door, opening the front door end to facilitate the lifting action of the robot arm after grasping the drill rod. Due to the large capacity of the drill rod in the rod chamber and the contact arrangement of the drill rods, it is not easy for thrust cylinder II to push the drill rod as a whole. Therefore, the axial positioning thrust mechanism can be distributed into multiple parts from top to bottom along the rod chamber according to the actual situation, and pushed in layers to reduce the number of drill rods pushed by a single axial positioning thrust mechanism and achieve flexible pushing.

[0098] Step 2: Place the hole orientation instrument at the tilt rotation center position, adjust the tilt attitude using the host attitude adjustment module, find the tilt zero position using the feedback signal of the hole orientation instrument, and read the distance measured by the high-precision laser rangefinder at the tilt zero position, which is recorded as the initial distance; then change the tilt angle again using the host attitude adjustment module and read the distance measured by the high-precision laser rangefinder again, thereby obtaining multiple sets of tilt angle input values ​​and the actual tilt angle value calculated by the distance measured by the high-precision laser rangefinder.

[0099] Specifically, in step 2, the actual tilt angle calculated based on the measured distance is:

[0100]

[0101] In the above formula, θ2' is the actual tilt angle, and L c1 It is the distance L measured by a high-precision laser rangefinder after the drill arm passes the initial horizontal point found by the hole positioning instrument. d1 This refers to the distance between the mounting point of the high-precision laser rangefinder sensor in the Y direction and the center of rotation. When the tilt angle θ2 is input, the change in the tilt angle causes a change in the measurement distance of the high-precision laser rangefinder sensor. z1 It is the distance measured by a high-precision laser rangefinder after the tilt angle changes, L y This refers to the translation distance of the drill arm translation cylinder; specifically, as follows: Figure 7 As shown, points O1 and O2 are the tilt rotation centers, point A1 is the installation point of the high-precision laser rangefinder sensor, H0 is the initial distance of the drill arm lifting, and the lifting height is ΔH1. z The height of the laser rangefinder's projection point above the vehicle's ground level after the tilt angle changes and the translation cylinder moves is used as a remeasured value for calculating the angle.

[0102] Step 3: Calculate the opening position coordinates using the actual tilt angle and determine the tilt angle error offset in the height direction; using the numerical relationship between the tilt angle from zero degrees to the changed value, formulate the equation between the changed actual tilt angle and the measured distance, and solve the equation to obtain the tilt angle error value at a certain position.

[0103] Specifically, in step 3, the x, y, and z coordinates of the opening position are calculated based on the actual tilt angle:

[0104]

[0105] In the above formula, H0 is the initial distance of the drill arm lifting, and the lifting height is △H1; the difference between the z-coordinate of the end of the borehole opening position under the actual inclination angle and the z-coordinate of the theoretical inclination angle is the offset of the inclination angle error in the height direction.

[0106] Step 4: When the tilt angle changes, the position of the end opening will have an azimuth deflection angle, and at the same time, it will also have an x-axis offset. Based on the changes in the tilt angle deflection angle and offset, a virtual joint is introduced, and a coordinate transformation matrix is ​​added to compensate for the error caused by the tilt angle joint. The tilt angle pose error is compensated, and the tilt angle error compensation model is established, which is the host attitude error compensation model.

[0107] Specifically, in step 4, when the tilt angle changes, the position of the end opening will generate an azimuth angle Δθ2, and will also generate an x-axis offset Δz, such as... Figure 8As shown, the tilt angle rotation center of X2O2Y2 will experience an azimuthal deviation Δθ2 at the center O3 opening of the end X3O3Y3 when the tilt angle and translation distance of the hydraulic cylinder change. Simultaneously, an x-axis offset Δz will also occur. Therefore, a virtual joint is introduced after O2 to add a coordinate transformation matrix to compensate for the error caused by the tilt angle joint, thus compensating for the tilt angle pose error. The tilt angle error compensation model after introducing the virtual joint is as follows:

[0108]

[0109] In the above formula, It is the homogeneous transformation matrix between the coordinate systems of the second and first adjacent joints;

[0110] Trans(Δz,0,0) is a translation Δz transformation about the x-axis; Rot(y,Δθ2) is a rotation Δθ2 transformation about the y-axis.

[0111] Step 5: Use the RBF neural network to solve the error compensation model matrix inversely. The inverse solution obtained is the new attitude control input after error compensation, ensuring that the target position remains unchanged. The tilt angle error transformation is obtained from equation (5), and the end hole pose is obtained from equation (4). Then, the tilt angle is obtained from the hole pose to solve the inverse problem. Therefore, the RBF neural network method is used to solve the error compensation matrix of equation (5) inversely. The inverse solution obtained is the new tilt angle control quantity after error compensation.

[0112] Step 6: The robotic arm clamps the drill rod center position, positioned in Step 1, from the drill rod box. Using the new attitude control input after error compensation in Step 5, the robotic arm transfers the drill rod to be added to the target position on the main unit. Figure 9 Three laser rangefinders at the end of the robotic arm's gripper form a measuring plane, with the projection target point located at the edge of the target disk at the end of the active drill rod. When the distances monitored by the three laser rangefinders are identical and match the set values, the axis of the drill rod to be added is perfectly aligned with the center of the active drill rod and the gripper. If the distance monitored by one or more laser rangefinders is not equal to the set value, the position of the drill rod to be added is not aligned. Error compensation must be performed on the coordinate calculation of the robotic arm's end-effector until the drill rod to be added is perfectly aligned with the center of the gripper and the active drill rod. Only then can the gripper hold the drill rod, and the robotic arm's gripper releases to proceed to the next action. Specifically, three laser rangefinders are arranged around the drill rod along the direction parallel to the end of the gripper. When the drill rod to be added is sent to the target position on the host, it is perpendicular to the active drill rod end of the rotary unit. Two target disks of different sizes are set at the end of the active drill rod. The distances between these disks are measured using the three laser rangefinders, and the alignment of the drill rod axis with the center of the active drill rod and the gripper is analyzed and determined.

[0113] Specifically, in step 6, the distance from the center of the three laser rangefinders to the center of the drill rod to be added is R, and the monitoring distances of the three laser rangefinders are L1, L2 and L3 respectively; there are two target disks at the end of the active drill rod that are parallel to each other, namely the primary target disk and the secondary target disk. The radius of the primary target disk is also R, and the radius of the secondary target disk is R1, and R1 is greater than R. The distance between the two target disks is m.

[0114] When the three laser rangefinders monitor the same distance in step 6 and the distance is the same as the set value, the axis of the drill rod to be added is completely aligned with the center of the active drill rod and the clamp. At this time, the points emitted by the three laser rangefinders to the edge of the target disk are Z1, Z2, and Z3, which satisfy the relationship: L1 = L2 = L3 = L (6). In the above formula, the set value of the distance from the three laser rangefinders to the active drill rod is L.

[0115] If, in step 6, the monitoring distance of one or more laser rangefinders is not equal to the set value, i.e., does not satisfy equation (6), then the position of the drill rod to be added is not aligned. At this time, the actual points emitted by the three laser rangefinders to the edge of the target disk are Z1'(x1 y1 z1), Z2'(x2 y2 z2), and Z3'(x3 y3 z3). Error compensation must be performed on the coordinates of the end effector of the robotic arm.

[0116] More specifically, in this embodiment, a Cartesian rectangular O4 coordinate system is established with the center of the target disk of the active drill rod. When the drill rod to be added is aligned concentrically with the active drill rod and the gripper, the coordinates Z1, Z2, and Z3 of the points emitted by the three laser rangefinders to the edge of the target disk should be equal to (0, R, 0), (-R, 0, 0), and (R, 0, 0). The robotic arm can obtain the actual position coordinates of the three points of the laser rangefinders of the end effector Z1'(x1 y1 z1), Z2'(x2 y2 z2), and Z3'(x3 y3 z2) by solving the matrix forward motion. The calculated coordinate plane of the three points (z3) should be parallel to and separated by a distance L from the plane formed by the target coordinates Z1, Z2, and Z3 in the first-level disk O4 coordinate system, i.e., the drill rod to be added should be aligned with the center of the target. The O4 coordinate system can be transformed to the end effector sensor coordinate system O5 in the robot world coordinate system—the center of the plane formed by the three laser sensors around the central axis of the end effector—to obtain new coordinates and the coordinate points of the end effector sensor, such as... Figure 10 As shown, the Z-axis is the direction of the drill pipe axis, the X-axis is the horizontal direction perpendicular to the side of the main unit, and the y-axis is the direction perpendicular to the plane of the main unit guide rail. The error comparison is performed as shown in equation (7):

[0117]

[0118] The monitoring feedback value is the sensor ranging value, consisting of two side points as a group. By analyzing the ranging values ​​from the two groups of sensors to the primary and secondary target disks and their interrelationships, the end-effector coordinate attitude can be determined and compensated for. Based on different conditions and regular combinations of the three measuring points, the following two hypothetical scenarios can be obtained:

[0119] Case (I): When two measuring points are grouped together, and their distance measurements satisfy the condition that the absolute value of the distance difference between any two groups of points is equal to 0 or m, it proves that the sensor measuring point plane is parallel to the target disk, and the two planes are in a parallel state, such as... Figure 11 As shown, it can be proven that the axis of the drill rod to be added held by the end of the robotic arm gripper is parallel to the axis of the active drill rod. It can be determined that the error between the end attitude and the target point is mainly the eccentric displacement error of the X and Y axes, while the tilt angle attitude only involves the rotation angle around the Z axis. The error value can be taken as the two points with the smallest distance measurement value among the three measuring points as reference. Let's assume Z1 is taken. ’ At point Z3', the total error value is given by equation (8), and the error compensation model is given by equation (9). First, the rotation angle error is compensated to make the coordinate system of the end of the robotic arm parallel to the coordinate system of the target point. Then, the displacement is compensated based on the eccentricity error.

[0120]

[0121] ΔT1=Trans(x,Δx)×Rot(Z,Δθ z )×Trans(y, Δy)×Trans(z, Δz) (9)

[0122] In the above formula, ΔT1 is the total error under condition (I), x1 is the calculated value of point Z1 on the X-axis in the end coordinate system, y1 is the calculated value of point Z1 on the Y-axis in the end coordinate system, L1 is the laser sensor ranging value of point Z1, and x3 is the calculated value of point Z3 on the X-axis in the end coordinate system.

[0123] Case (II): When two measuring points are grouped together and their ranging values ​​do not meet the above conditions, it proves that the sensor measuring point plane may have errors in the tilt angle and azimuth angle of the host machine. At this time, we first analyze the two points with a 180° interval as a group. For example, Z2' and Z3' are grouped together and the difference is compared. Due to the existence of the secondary target disk on the active drill rod, when the two points with a 180° interval on the sensor measuring point plane are tilted, the two points will inevitably be projected onto the primary and secondary target disks respectively, and the following relationship (10) can be obtained to obtain the azimuth offset angle Δθ. y ,like Figure 12 As shown, the attitude is corrected by azimuth angle. If Z2' and Z3' satisfy the first assumption, but Z1' does not, then it proves that there is an error Δθ between the end effector and the main engine tilt angle. xAt this point, the distance between two points Z1' and Z3' with a spacing of 90° is measured. When the measured distance is greater than L+m, there is a depression angle error. When the measured distance is less than L+m but greater than m, there is an elevation angle error. The relationship (11) can be obtained to obtain the offset angle Δθ on the tilt angle. x ,like Figure 13 As shown, tilt and azimuth angles are corrected for the attitude.

[0124]

[0125]

[0126]

[0127] but:

[0128] In the above formula, Δθ y Δθ is the tilt error of the actual end-effector attitude around the Y-axis of the end-effector coordinate system; R is the distance from the center of the three laser rangefinders to the center of the drill rod to be added; m is the distance between the target disks at the two ends of the active drill rod; L2 is the laser ranging value at point Z2; L3 is the laser ranging value at point Z3; R is the radius of the first-stage target disk; R1 is the radius of the second-stage target disk; x L1 is the azimuth error of the actual end-effector attitude around the X-axis of the end-effector coordinate system, and L1 is the laser ranging value at point Z1.

[0129] Angle compensation transformation is performed on the end-effector attitude in the rotation directions around the X and Y axes. The transformed attitude matches the sensor measurement plane of Hypothesis 1. It is also necessary to compensate for the eccentric displacement error in the X and Y axes and the tilt attitude in the rotation direction around the Z axis before the final corrected attitude can be obtained. That is, the error compensation model is as shown in Equation (12). Then, the inverse solution is obtained through the robotic arm joints to correct the parameters of each joint; ΔT2=Rot(x,Δθ x )×Rot(y,Δθ y Less × ΔT1(12), ΔT2 is the total error under condition (II).

Claims

1. An intelligent drilling rig drill rod automatic loading and unloading system with precise positioning and attitude monitoring compensation, characterized in that, This includes the drill pipe box, the robotic arm and its end gripper, and the main unit, which are sequentially installed on the vehicle platform; The drill rod box has an automatic alignment and positioning function, including a box support, a drill rod positioning frame, a stop bar, a front compartment door, a radial positioning thrust mechanism, and an axial positioning thrust mechanism. The drill rod positioning frame is installed at the bottom of the box support, and the drill rod positioning frame has equally spaced grooves to place and position the drill rods. The stop bar is detachably installed on the upper part of the box support to limit the height of the drill rods inside the box and prevent the drill rods from shaking during transportation. The drill rod positioning frame and the stop bar are parallel to each other and perpendicular to the drill rods. The front compartment door is detachably installed at the front end of the box support. The radial positioning thrust mechanism is installed on the side plates on both sides of the box support and is tangent to the drill rods located at the even-numbered end. It can push the drill rods radially to ensure that the spacing between adjacent drill rods is equal, thus ensuring the radial positioning accuracy of the drill rods. The axial positioning thrust mechanism is installed on the rear baffle at the rear end of the box support and can move along the drill rod axis to push the entire drill rod to the front compartment door so that the front ends of each drill rod are aligned, thus ensuring the axial positioning accuracy of the drill rods. The robotic arm's gripper end is equipped with three laser rangefinders, which are arranged at 90° intervals along the left, top, and right ends of the drill rod to be drilled. The three laser rangefinders are parallel to the drill rod to be drilled, and the measuring points of the three laser rangefinders form a fixed plane as the measuring point plane. The host machine has attitude monitoring and compensation functions, including a stabilizer, a gripper, and a power head sequentially mounted on the feed body. It also includes a high-precision laser rangefinder, a hole orientation instrument, displacement sensor I, a tilt sensor, displacement sensor II, a controller, and a target plate. The high-precision laser rangefinder is located at the rear of the power head, and the laser emitted from its laser generator window can pass through the inner hole of the power head, the central through hole of the drill rod, the center of the gripper and the unlatcher, and be projected onto the target plate in front of the host machine. The laser reflected from the target plate can also be projected onto the laser receiving window. The hole orientation instrument is located at the high-precision... Behind the laser rangefinder sensor, a reference is provided for the initial zero position of the tilt angle; displacement sensor I is used to measure the lifting height of the host; the tilt angle sensor is used to measure the initial input tilt angle value; displacement sensor II is used to measure the translational displacement of the host; the data acquisition board in the controller is connected to the high-precision laser rangefinder sensor to obtain the distance value of the high-precision laser rangefinder sensor in real time. The data acquisition board in the controller is also connected to the hole orientation instrument, displacement sensor I, tilt angle sensor and displacement sensor II to dynamically adjust the host attitude to determine the zero position value measured by the hole orientation instrument.

2. The intelligent drilling rig drill rod automatic loading and unloading system with precise positioning and attitude monitoring compensation as described in claim 1, characterized in that, The box support includes a square frame at the bottom, vertical columns, side panels and a rear baffle. The square frame is formed by two bottom end beams and two bottom side beams. Four vertical columns are located at the four corners of the square frame. The four side panels are connected to the four vertical columns respectively, and every two side panels are located on the same bottom side beam. The rear baffle is located on the rear bottom end beam and is connected to the two vertical columns at its two ends. The drill pipe positioning frame is installed between two bottom side beams. The groove of the drill pipe positioning frame can be a wave-shaped groove, a square groove, a trapezoidal groove, a triangular groove, or a semi-circular groove. The stop bar is detachably connected to the upper part of two opposing side plates.

3. The intelligent drilling rig drill rod automatic loading and unloading system with precise positioning and attitude monitoring compensation as described in claim 2, characterized in that, The front compartment door and the rear baffle are parallel to each other and detachably installed on two vertical columns at both ends of the front bottom end beam. The two ends of the front compartment door are screwed to the vertical columns and can be removed after the drill rod is automatically aligned, so that the robot can grab the drill rod. The outer wall of the front compartment door is bolted with a stop strip, the two ends of the stop strip are inserted into the slots on the vertical columns and the stop strip is detachable.

4. The intelligent drilling rig drill rod automatic loading and unloading system with precise positioning and attitude monitoring compensation as described in claim 3, characterized in that, The radial positioning thrust mechanism includes a thrust cylinder I and a pad connected to its end. The thrust cylinder I passes through and is fixed on the side plate. The pad is parallel to the inside of the side plate and can move radially along the drill rod to be tangent to the drill rod at the end of the even-numbered layers and ensure that the spacing between adjacent drill rods is equal. The axial positioning thrust mechanism includes a thrust cylinder II and a thrust plate connected to its end. The thrust cylinder II passes through and is fixed on the rear baffle. The thrust plate is arranged parallel to the inner side of the rear baffle and can move along the drill rod axial direction to push the drill rod to the front chamber door so that the front ends of each drill rod are aligned as a whole to ensure the axial positioning accuracy of the drill rod.

5. A method for automatic loading and unloading of drill rods in intelligent drilling rigs with precise positioning and attitude monitoring compensation, characterized in that... This method is implemented using the system described in claim 4, and includes the following steps: Step 1: Input the specifications of the drill rods in the drill rod box and the required drill rod spacing on the controller. The drill rod positioning frame corresponding to the specifications of the drill rods rises from the bottom of the drill rod box. The pads of the radial positioning thrust mechanism on both sides move parallel to the inside and outside of the drill rod box to limit the initial position of the drill rods at both ends. After the drill rods are filled in the drill rod box, the rear axial positioning thrust mechanism pushes the drill rods in the drill rod box axially in multiple layers. Under the limiting action of the front compartment door, the drill rods are axially positioned and aligned. Step 2: Place the hole orientation instrument at the tilt angle rotation center position, adjust the tilt angle attitude using the host attitude adjustment module, find the tilt angle zero position using the feedback signal of the hole orientation instrument, and read the distance measured by the high-precision laser rangefinder at the tilt angle zero position; after changing the tilt angle, read the distance measured by the high-precision laser rangefinder again, thereby obtaining multiple sets of tilt angle input values ​​and the actual tilt angle value calculated by the distance measured by the high-precision laser rangefinder. Step 3: Calculate the opening position coordinates using the actual tilt angle, and determine the offset of the tilt angle error in the height direction; Step 4: When the tilt angle changes, the position of the end opening will have an azimuth deflection angle, and at the same time, it will also have an x-axis offset. Based on the changes in the tilt angle deflection angle and offset, a virtual joint is introduced, and a coordinate transformation matrix is ​​added to compensate for the error caused by the tilt angle joint. The tilt angle pose error is compensated, and the tilt angle error compensation model is established, which is the host attitude error compensation model. Step 5: Use the RBF neural network to solve the inverse solution of the error compensation model matrix. The obtained inverse solution is the new attitude control input after error compensation, ensuring that the target position remains unchanged. Step 6: The robotic arm clamps the drill rod center position positioned in Step 1 from the drill rod box. Using the new attitude control input after error compensation in Step 5, the robotic arm transfers the drill rod to be added to the target position of the host. The three laser rangefinders at the end of the robotic arm gripper form a measuring point plane, and the projection target point is set on the edge of the target disk at the end of the active drill rod. When the monitoring distances of the three laser rangefinders are exactly the same and the same as the set value, the axis of the drill rod to be added is completely aligned with the center position of the active drill rod and the gripper. When the monitoring distance of one or more laser rangefinders is not equal to the set value, the position of the drill rod to be added is not aligned. Error compensation must be performed on the coordinate calculation of the attitude of the robotic arm end until the drill rod to be added is completely aligned with the center of the gripper and the active drill rod. Only then can the gripper clamp the drill rod, and the robotic arm gripper releases to proceed to the next action.

6. The intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 1, the pads of the radial positioning thrust mechanism on both sides of the drill rod box are tangent to the drill rods located at the even-numbered layer ends, and ensure that the spacing between each layer of drill rods is always equal to the spacing between the bottom layer positioning drill rods. The layer height and the spacing change in a linear proportion to ensure the radial and odd-even layer positioning accuracy of the drill rods. The distance between the two thrusters on both sides of the drill pipe box is L, n 偶 Let be the number of even-numbered drill pipe layers in the drill pipe box, D be the drill pipe diameter, a be the spacing between adjacent drill pipes in the same layer, and h be the height difference between two adjacent drill pipe layers, defined as follows: L=n 偶 D+(n-1)a (1) 7. The intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 1, the thrust cylinder II of the axial positioning thrust mechanism is a single-acting cylinder. Under normal conditions, the thrust cylinder II remains in a retracted state, and the thrust plate does not contact the drill rod. When it is activated, oil enters the rodless chamber of the thrust cylinder II, causing the piston rod to extend and push the thrust plate. At the same time, it drives the drill rod as a whole to move towards the front door position. After the front end of the drill rod is aligned, the thrust plate retracts to ensure the axial positioning accuracy of the drill rod. After the drill rod is aligned along the axis, the stop bar is removed from the slot and taken off together with the front door, so that the front door is open, which facilitates the lifting action of the robot arm after grasping the drill rod.

8. The intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 2, the actual tilt angle calculated based on the measured distance is: In the above formula, θ2' is the actual tilt angle, and L c1 It is the distance L measured by a high-precision laser rangefinder after the drill arm passes the initial horizontal point found by the hole positioning instrument. d1 This refers to the distance between the mounting point of the high-precision laser rangefinder sensor in the Y direction and the center of rotation. When the tilt angle θ2 is input, the change in the tilt angle causes a change in the measurement distance of the high-precision laser rangefinder sensor. z1 It is the distance measured by a high-precision laser rangefinder after the tilt angle changes, L y This refers to the translation distance of the drill arm translation cylinder.

9. The intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 3, the specific coordinates of the opening position (x, y, z) are calculated based on the actual tilt angle: In the above formula, H0 is the initial distance of the drill arm lifting, and the lifting height is △H1; the difference between the z-coordinate of the end of the borehole opening position under the actual inclination angle and the z-coordinate of the theoretical inclination angle is the offset of the inclination angle error in the height direction.

10. The intelligent drilling rig drill rod automatic loading and unloading method with precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 4, when the tilt angle changes, the end opening position will generate an azimuth deflection angle Δθ2, and also an x-axis offset Δz. A virtual joint is introduced, and a coordinate transformation matrix is ​​added to compensate for the error caused by the tilt angle joint, thus compensating for the tilt angle pose error. The tilt angle error compensation model after introducing the virtual joint is as follows: In the above formula, It is the homogeneous transformation matrix between the coordinate systems of the second and first adjacent joints; Trans(Δz,0,0) is a translation Δz transformation about the x-axis; Rot(y,Δθ2) is a rotation Δθ2 transformation about the y-axis.

11. The intelligent drilling rig automatic loading and unloading method for precise positioning and attitude monitoring compensation as described in claim 5, characterized in that, In step 6, the distance from the center of the three laser rangefinders to the center of the drill rod to be added is R, and the monitoring distances of the three laser rangefinders are L1, L2 and L3 respectively; there are two target disks at the end of the active drill rod, which are parallel to each other. The two target disks are a primary target disk and a secondary target disk, respectively. The radius of the primary target disk is also R, and the radius of the secondary target disk is R1, and R1 is greater than R. The distance between the two target disks is m.

12. The intelligent drilling rig automatic loading and unloading method for precise positioning and attitude monitoring compensation as described in claim 11, characterized in that, In step 6, when the three laser rangefinders monitor the same distance and the same as the set value, the axis of the drill rod to be added is completely aligned with the center position of the active drill rod and the clamp. At this time, the points emitted by the three laser rangefinders to the edge of the target disk are Z1, Z2, and Z3, which satisfy the relationship: L1 = L2 = L3 = L (6). In the above formula, the set value of the distance from the three laser rangefinders to the active drill rod is L.

13. The intelligent drilling rig automatic loading and unloading method for precise positioning and attitude monitoring compensation as described in claim 12, characterized in that, In step 6, if the monitoring distance of one or more laser rangefinders is not equal to the set value, i.e., equation (6) is not satisfied, then the position of the drill rod to be added is not aligned. At this time, the actual points emitted by the three laser rangefinders to the edge of the target disk are Z1'(x1 y1 z1), Z2'(x2 y2 z2), and Z3'(x3 y3 z3). Error compensation must be performed on the coordinate calculation of the robot arm end posture, including the following two cases: (I) When the absolute value of the distance difference between any two laser rangefinders is equal to 0 or m, it proves that the plane of the measuring points of the three laser rangefinders is parallel to the target disk. At this time, the axis of the drill rod to be added held by the end of the robotic arm gripper is parallel to the axis of the active drill rod. Then the error between the posture of the end of the robotic arm and the target point is the eccentric displacement error of the X and Y axes. The tilt angle posture only has the rotation angle around the Z axis. Then its total error value is Equation (8). The error compensation model is as shown in Equation (9). First, the rotation angle error is compensated to make the coordinate system of the end of the robotic arm parallel to the coordinate system of the target point. Then, the displacement is compensated according to the eccentric error. ΔT1=Trans(x,Δx)×Rot(Z,Δθ Z )×Trans(y,Δy)×Trans(z,Δz) (9) In the above formula, ΔT1 is the total error under condition (I), x1 is the calculated value of point Z1 on the X-axis in the end coordinate system, y1 is the calculated value of point Z1 on the Y-axis in the end coordinate system, L1 is the laser sensor ranging value of point Z1, and x3 is the calculated value of point Z3 on the X-axis in the end coordinate system. (II) When the distance difference between any two laser rangefinders does not satisfy the conditions of equation (6) and case (I), it is proven that the plane of the measuring point of the three laser rangefinders is not parallel to the target disk. First, the posture of the end of the robot arm is transformed by the angle compensation in the rotation direction around the X and Y axes so that the plane of the measuring point of the three laser rangefinders is parallel to the target disk. Then, according to case (I), the eccentric displacement error in the X and Y axes and the rotation angle around the Z axis are compensated to obtain the final corrected posture. When performing angle compensation transformation on the end effector posture of the robotic arm in the rotational directions around the X and Y axes, the offset tilt angle Δθ y : In the above formula, Δθ y R is the tilt angle error of the actual end attitude around the Y-axis of the end coordinate system, R is the distance from the center of the three laser rangefinders to the center of the drill rod to be added, m is the distance between the target disks at the ends of the two active drill rods, L2 is the laser ranging value at point Z2, and L3 is the laser ranging value at point Z3. Offset azimuth angle Δθ x : In the above formula, Δθ x L1 is the azimuth error of the actual end attitude around the X-axis of the end coordinate system, and L1 is the laser ranging value at point Z1. Compensation for eccentric displacement errors in the X and Y axes and rotation angles around the Z axis: ΔT2=Rot(x, Δθ x )×Rot(y, Δθ y )×ΔT1 (12) In the above formula, ΔT2 is the total error under condition (II).

Images