A system and method for detecting the spatial pose of a drilling rig

Abstract

The application discloses a kind of detection system and method of rig space pose, it is related to prospecting equipment field, the system includes: three-dimensional laser scanner, setting in the sliding joint of drilling anchor machine, for obtaining the initial position coordinate of rig and sending initial position coordinate to first computer;Encoder, setting in the sliding joint of drilling anchor machine, for obtaining the displacement of sliding joint and sending displacement to first computer;First inclinometer, setting on drill arm, for obtaining the first rotation angle of drill arm and transmitting first rotation angle to first computer;Second inclinometer, setting on rig, for obtaining the second rotation angle of rig and transmitting second rotation angle to first computer;First computer, setting in the inside of the body of drilling anchor machine, for calculating the space pose data of rig according to initial position coordinate, displacement, first rotation angle and second rotation angle.The application can measure rig pose data in real time.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of exploration equipment, in particular to a drilling machine space pose detection system and method. BACKGROUND

[0002] The drill anchor machine is a kind of high intelligent new type drilling equipment for anchor rod support, which integrates machine, electricity, liquid and computer technology. It drills holes in target hole positions in turn by the forward and backward translation of the sliding joint connected with the drill arm, the left and right rotation of the drill arm and the forward swing of the drill machine, and then the anchor rod is manually assisted to be driven into the anchor net to form a complete cross section roadway. The pose of the target is generally represented by the horizontal deviation angle and the front and back lifting angle. At present, there is no mature method for measuring the space pose of the drill machine. The commonly used measurement methods for the pose of the cutting arm of the heading machine include the strapdown inertial navigation method and the laser target method. The inertial navigation measurement method mainly uses the combination of gyroscopes and accelerometers and other inertial elements to obtain the pose by integral calculation. The disadvantage of this method is that the measurement accuracy is low due to the cumulative error with the passage of time. The method of using a total station with a single laser target to measure the space pose of the target has the advantages of large range and high precision, but unlike the cutting arm, the drill anchor machine includes multiple groups of low static height drill arms, and has the working characteristic of left and right rotation, so it is difficult to accurately measure the pose data of the drill machine in real time. SUMMARY

[0003] To solve the above technical problems, the present application provides a drilling machine space pose detection system and method, which can measure the drilling machine pose data in real time.

[0004] To achieve the above purpose, the present application provides the following solutions:

[0005] A drilling machine space pose detection system, the system comprising a three-dimensional laser scanner, an encoder, a first inclinometer, a second inclinometer and a first computer;

[0006] The three-dimensional laser scanner is arranged on the sliding joint of the drill anchor machine and connected with the first computer, and is used to obtain the initial position coordinates of the drilling machine and send the initial position coordinates to the first computer;

[0007] The encoder is arranged on the sliding joint of the drill anchor machine and connected with the first computer, and is used to obtain the displacement of the sliding joint and send the displacement to the first computer;

[0008] The first inclinometer is arranged on the drill arm and connected with the first computer, and is used to obtain the first rotation angle of the drill arm and transmit the first rotation angle to the first computer;

[0009] The second inclinometer is arranged on the drilling machine and connected with the first computer, and is configured to acquire a second rotation angle of the drilling machine and transmit the second rotation angle to the first computer.

[0010] The first computer is arranged inside the body of the drilling anchor machine, and is configured to calculate spatial pose data of the drilling machine according to the initial position coordinates, the displacement, the first rotation angle and the second rotation angle.

[0011] Optionally, the center line of the first inclinometer is in the same plane as and parallel to the center line of the drilling arm, and the center line of the second inclinometer is in the same plane as and parallel to the center line of the drilling machine.

[0012] Optionally, the system further comprises a damping device, the first inclinometer is fixed to the drilling arm through the damping device, and the second inclinometer is fixed to the drilling machine through the damping device.

[0013] Optionally, the system further comprises a support, and the three-dimensional laser scanner is fixed to the sliding joint through the support.

[0014] Optionally, the system further comprises a second computer, the second computer is connected with the first computer, and is configured to receive and store the initial position coordinates, the displacement, the first rotation angle, the second rotation angle and the spatial pose data transmitted by the first computer.

[0015] Optionally, the three-dimensional laser scanner is arranged at the tail of the sliding joint.

[0016] Optionally, the center line of the three-dimensional laser scanner is in the same plane as and parallel to the center line of the sliding joint.

[0017] Optionally, the encoder is arranged at the tail of the sliding joint and located between the three-dimensional laser scanner and a connection point, and the connection point is the connection point of the drilling arm and the sliding joint.

[0018] A method for detecting spatial pose of a drilling machine, applied to the detection system for spatial pose of a drilling machine, and the method comprises the following steps of:

[0019] Acquiring initial position coordinates of the drilling machine collected by the three-dimensional laser scanner;

[0020] Establishing a spatial rectangular coordinate system with the initial position coordinates as the origin;

[0021] Acquiring a displacement collected by the encoder;

[0022] Acquiring a first rotation angle collected by the first inclinometer;

[0023] acquire a second rotation angle collected by a second inclinometer;

[0024] apply a DH solving algorithm to determine spatial pose data of the drilling rig in the spatial rectangular coordinate system according to the displacement, the first rotation angle and the second rotation angle.

[0025] Optionally, the application of the DH solving algorithm to determine the spatial pose data of the drilling rig in the spatial rectangular coordinate system according to the displacement, the first rotation angle and the second rotation angle specifically comprises:

[0026] applying the DH solving algorithm to establish a three-dimensional coordinate system of the sliding joint, a three-dimensional coordinate system of the drilling arm and a three-dimensional coordinate system of the drilling rig;

[0027] acquiring actual sizes of the sliding joint, the drilling arm and the drilling rig;

[0028] determining a coordinate system conversion parameter table according to the actual sizes of the sliding joint, the drilling arm and the drilling rig;

[0029] determining a coordinate transformation matrix of the drilling rig end and the spatial rectangular coordinate system according to the coordinate system conversion parameter table;

[0030] determining the spatial pose data of the drilling rig in the spatial rectangular coordinate system according to the coordinate transformation matrix and the displacement, the first rotation angle and the second rotation angle.

[0031] According to the specific embodiments of the present application, the following technical effects are provided:

[0032] This invention provides a drilling rig spatial pose detection system, comprising: a 3D laser scanner, an encoder, a first inclinometer, a second inclinometer, and a first computer; the 3D laser scanner is mounted on the sliding joint of the drilling rig and connected to the first computer, used to acquire the initial position coordinates of the drilling rig and send the initial position coordinates to the first computer; the encoder is mounted on the sliding joint of the drilling rig and connected to the first computer, used to acquire the displacement of the sliding joint and send the displacement to the first computer; the first inclinometer is mounted on the drill arm and connected to the first computer, used to acquire the first rotation angle of the drill arm and transmit the first rotation angle to the first computer; the second inclinometer is mounted on the drilling rig and connected to the first computer, used to acquire the second rotation angle of the drilling rig and transmit the second rotation angle to the first computer; the first computer is located inside the body of the drilling rig and used to calculate the spatial pose data of the drilling rig based on the initial position coordinates, displacement, first rotation angle, and second rotation angle. This invention utilizes a 3D laser scanner and encoder mounted on the sliding joint, and an inclinometer mounted on the drill arm and rig. The collected initial position, displacement, and angle data are transmitted to an explosion-proof computer for calculation, processing, and analysis to determine the attitude change of the drill rig in three-dimensional space. This makes the measuring device more stable, improves the measurement accuracy of the drill rig's spatial posture under harsh conditions such as visibility and environment in mines, reduces labor costs, decreases the risk factor for operators, and is easy to install. Attached Figure Description

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

[0034] Figure 1 A schematic diagram of the drilling rig spatial pose detection system provided by the present invention;

[0035] Figure 2 A schematic diagram illustrating the principle of the drilling rig spatial pose detection system provided by the present invention;

[0036] Figure 3 A flowchart of the drilling rig spatial pose detection method provided by the present invention;

[0037] Figure 4 This is a schematic diagram of the establishment of the DH solution coordinate system for drilling rigs provided by the present invention;

[0038] Figure 5 The flowchart shows the process of correcting the spatial pose of a drilling rig using a 3D laser scanner, as provided in this invention.

[0039] Symbol explanation:

[0040] 1-3D laser scanner, 2-stand, 3-encoder, 4-first inclinometer, 5-second inclinometer, 6-drill arm, 7-drilling rig, 8-body, 9-sliding joint, 10-first computer, 11-second computer. Detailed Implementation

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

[0042] The purpose of this invention is to provide a drilling rig spatial pose detection system and method that can measure drilling rig pose data in real time.

[0043] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0044] Example 1

[0045] like Figure 1 and Figure 2 As shown, the present invention provides a drilling rig spatial pose detection system, the system including a three-dimensional laser scanner 1, an encoder 3, a first inclinometer 4, a second inclinometer 5, and a first computer 10.

[0046] The 3D laser scanner 1 is mounted on the sliding joint 9 of the drilling rig 7 and connected to the first computer 10. It is used to acquire the initial position coordinates of the drilling rig 7 and send these coordinates to the first computer 10. Specifically, the 3D laser scanner 1 is located at the tail end of the sliding joint 9. The centerline of the 3D laser scanner 1 is in the same plane and parallel to the centerline of the sliding joint 9.

[0047] In practical applications, the 3D laser scanner 1 is used to acquire real-time spatial pose data of the drilling rig 7 and construct the initial spatial pose data of the drilling rig 7. When the displacement of the sliding joint 9, the rotational deflection angle of the drill arm 6, and the rotational lifting angle of the drilling rig 7 are all zero, the 3D coordinate system established by the data acquired by the 3D laser scanner 1 is the initial position.

[0048] The encoder 3 is mounted on the sliding joint 9 of the drilling and anchoring machine and connected to the first computer 10. It is used to acquire the displacement of the sliding joint 9 and send the displacement to the first computer 10. Specifically, the encoder 3 is mounted at the end of the sliding joint 9 and is located between the three-dimensional laser scanner 1 and the connection point. The connection point is the connection point between the drill arm 6 and the sliding joint 9.

[0049] Furthermore, encoder 3 is an absolute encoder, which can translate along with the sliding joint 9 connected to the drill arm 6 and can obtain the displacement of the sliding joint 9.

[0050] The first inclinometer 4 is mounted on the drill arm 6 and connected to the first computer 10. It is used to acquire the first rotation angle of the drill arm 6 and transmit this first rotation angle to the first computer 10. The centerline of the first inclinometer 4 is in the same plane and parallel to the centerline of the drill arm 6. Specifically, the first inclinometer 4 rotates left and right with the drill arm 6 and can acquire the rotation deflection angle.

[0051] Furthermore, the first inclinometer 4 is mounted on the outer plane of the drill arm 6.

[0052] The second inclinometer 5 is mounted on the drilling rig 7 and connected to the first computer 10. It is used to acquire the second rotation angle of the drilling rig 7 and transmit the second rotation angle to the first computer 10. The centerline of the second inclinometer 5 is in the same plane and parallel to the centerline of the drilling rig 7. Specifically, the second inclinometer 5 rotates forward with the drilling rig 7 and can acquire the rotational lifting angle.

[0053] Furthermore, the second inclinometer 5 is installed on the outer plane of the drilling rig 7 facing the cross section.

[0054] The first computer 10 is installed inside the body 8 of the drilling and anchoring machine, and is used to calculate the spatial pose data of the drilling rig 7 based on the initial position coordinates, the displacement, the first rotation angle, and the second rotation angle. Specifically, the first computer 10 is an explosion-proof computer capable of processing and analyzing the data collected by the 3D laser scanner 1, the absolute encoder, and the inclinometer.

[0055] The system also includes a vibration damping device; the first inclinometer 4 is fixed to the drill arm 6 via the vibration damping device; the second inclinometer 5 is fixed to the drilling rig 7 via the vibration damping device. Specifically, the vibration damping device is a rubber ring or rubber pad, which is low-cost, readily available, and economical. By damping the vibrations of the first inclinometer 4 and the second inclinometer 5, the vibration frequency and amplitude of the first inclinometer 4 and the second inclinometer 5 are reduced, enabling the explosion-proof computer to more accurately analyze and calculate the pose data, resulting in smaller and more accurate calculation results.

[0056] The system also includes a bracket 2; the 3D laser scanner 1 is fixed to the sliding joint 9 via the bracket 2. Specifically, the bracket 2 is a dedicated bracket for the 3D laser scanner 1. The dedicated bracket is installed at the tail end of the sliding joint 9. The dedicated bracket for mounting the 3D laser scanner 1 should be installed at the tail end of the sliding joint 9 as much as possible. To ensure that the drill arm 6 and its angular variable range are always within the field of view of the 3D laser scanner 1, the larger the variable range of the drill arm 6 and the drilling rig 7, the greater the difficulty for the explosion-proof computer in calculation and analysis, and the greater the error in the calculation results.

[0057] The system also includes a second computer 11; the second computer 11 is connected to the first computer and is used to receive and store the initial position coordinates, the displacement, the first rotation angle, the second rotation angle, and the spatial pose data sent by the first computer. Specifically, the second computer 11 is located in the ground control center, and the explosion-proof computer is connected to the second computer 11 via a signal line.

[0058] The working principle of the drilling rig spatial pose detection system provided by this invention is as follows:

[0059] The displacement and rotation angle of the sliding joint, drill arm, and drilling rig are displayed on the 3D laser scanner for analysis and processing by an explosion-proof computer. The sliding joint, drill arm, and drilling rig must always be within the field of view of the 3D laser scanner lens. The absolute encoder is fixedly installed at the end of the sliding joint and moves the same distance as the sliding joint. The first inclinometer is fixedly installed on the outer plane of the drill arm and rotates the same angle as the drill arm rotates. The second inclinometer is fixedly installed on the outer plane of the drilling rig facing the cross-section and rotates the same angle as the drilling rig rotates. The sliding joint slides, causing a displacement change in the absolute encoder. The drill arm rotates outward, causing a deflection angle change in the first inclinometer. The drilling rig rotates forward, causing a lifting angle change in the second inclinometer. The 3D laser scanner, absolute encoder, first inclinometer, and second inclinometer are connected to the explosion-proof computer via a communication module. The 3D laser scanner, absolute encoder, first inclinometer, and second inclinometer transmit the collected spatial pose data to the explosion-proof computer for analysis and processing. Specifically, the explosion-proof computer processes the drilling rig's spatial pose data using DH calculation. The first and second inclinometers consist of an accelerometer sensor head, an A / D converter, a microprocessor, and an output interface. The explosion-proof computer receives the data collected by the absolute encoder, first inclinometer, and second inclinometer, constructs a 3D coordinate system for each joint through DH calculation, and establishes a transformation relationship between the coordinate systems of each joint through matrix transformation, thereby obtaining the spatial pose data of the target drilling rig. The explosion-proof computer is connected to a second computer, transmitting both pose data and raw data to the second computer for storage and recording. Operators can clearly understand the working status of the drilling rig and the explosion-proof computer through the second computer.

[0060] When the drilling rig is operating normally, the sliding joint will generate a certain displacement and slide. The absolute encoder fixed on the sliding joint will generate the same displacement. The drill arm will generate a certain rotational deflection angle. The first inclinometer fixed on the drill arm will record the same deflection angle. The drilling rig will generate a certain rotational lifting angle. The second inclinometer fixed on the drilling rig will record the same lifting angle. The 3D scanner acquires the spatial pose data of the initial position. The spatial pose data of the initial position, the displacement, the deflection angle, and the lifting angle are sent to the explosion-proof computer. The explosion-proof computer establishes a spatial rectangular coordinate system with the initial position coordinates as the origin based on the spatial pose data of the initial position. Based on the data obtained by the encoder, the first inclinometer, and the second inclinometer, it obtains the spatial pose data of the drilling rig in the spatial rectangular coordinate system and transmits the original data and the spatial pose data of the drilling rig to the second computer for storage and recording.

[0061] In addition, the three-dimensional laser scanner will record the spatial pose data of the drilling rig in real time and transmit the recorded data to the explosion-proof computer. The explosion-proof computer calculates the spatial pose of the drill arm according to a fixed algorithm, compares and corrects the data obtained by the three-dimensional laser scanner, the absolute encoder, the first inclinometer, and the second inclinometer, and transmits the data to the second computer for storage and recording.

[0062] Specifically, the displacement and rotation angle of the sliding joint, drill arm, and drilling rig are reflected on the 3D laser scanner for analysis and processing by an explosion-proof computer. The sliding joint, drill arm, and drilling rig must always be within the field of view of the 3D laser scanner lens, so that the 3D laser scanner can acquire and construct the real-time 3D coordinate system of the drilling rig. The variables of the sliding joint, drill arm, and drilling rig will generate a new 3D coordinate system image in the 3D laser scanner, which facilitates the verification and validation of the real-time 3D coordinate system data of the drilling rig constructed by the 3D laser scanner. This avoids the 3D laser scanner being unable to acquire and construct complete spatial pose information of the drilling rig due to changes in the position of the 3D laser scanner caused by the translation of the sliding joint, or even the 3D laser scanner being unable to acquire the spatial pose information of the drilling rig at all.

[0063] In practical applications, a 3D laser scanner emits a laser to scan the spatial pose of the drilling rig in real time, acquiring a point cloud image of the rig's spatial pose. This real-time point cloud image data is then transmitted to an explosion-proof computer. The PointPillars point cloud processing algorithm is used to identify the drilling rig's pose, allowing the explosion-proof computer to obtain the drilling rig's spatial pose based on the 3D laser scanner data in real time. This data is then compared and verified with the data obtained from the encoder, the first inclinometer, and the second inclinometer using the DH algorithm, thereby improving the accuracy of the drilling rig's spatial pose data. Specifically, after the drilling rig starts working, the 3D laser scanner, encoder, and inclinometer collect initial data for comparison and calculation. This includes averaging the X, Y, and Z coordinates of the drilling rig's spatial pose obtained in real time by the 3D laser scanner with the corresponding X, Y, and Z coordinates obtained through DH calculation to obtain the averaged spatial pose coordinates of the drilling rig. These averaged coordinates are then output and displayed as the drilling rig's spatial pose data.

[0064] This invention, by installing inclinometers on two sections of the drill arm, along with a three-dimensional laser scanner mounted on the tail end of the sliding joint and an absolute encoder mounted at the end, can accurately detect the position and posture parameters of the drill rig body in real time. It also features high stability, strong anti-interference ability, and convenient installation.

[0065] Example 2

[0066] In order to execute the system corresponding to Embodiment 1 above and achieve the corresponding functions and technical effects, a method for detecting the spatial pose of a drilling rig is provided below, such as... Figure 3 As shown, the method includes:

[0067] Step S1: Obtain the initial position coordinates of the drilling rig acquired by the 3D laser scanner.

[0068] Step S2: Establish a spatial rectangular coordinate system with the initial position coordinates as the origin.

[0069] Step S3: Obtain the displacement data collected by the encoder.

[0070] Step S4: Obtain the first rotation angle collected by the first inclinometer.

[0071] Step S5: Obtain the second rotation angle collected by the second inclinometer.

[0072] Step S6: Apply the DH algorithm to determine the spatial pose data of the drilling rig in the spatial rectangular coordinate system based on the displacement, the first rotation angle, and the second rotation angle.

[0073] S6 specifically includes:

[0074] Step S61: Apply the DH solution algorithm to establish the three-dimensional coordinate system of the sliding joint, the three-dimensional coordinate system of the drill arm, and the three-dimensional coordinate system of the drilling rig.

[0075] Step S62: Obtain the actual dimensions of the sliding joint, the actual dimensions of the drill arm, and the actual dimensions of the drilling rig.

[0076] Step S63: Determine the coordinate system transformation parameter table based on the actual dimensions of the sliding joint, the actual dimensions of the drill arm, and the actual dimensions of the drilling rig.

[0077] Step S64: Determine the coordinate transformation matrix between the drilling rig end and the spatial rectangular coordinate system according to the coordinate system transformation parameter table.

[0078] Step S65: Determine the spatial pose data of the drilling rig in the spatial rectangular coordinate system based on the coordinate transformation matrix, the displacement, the first rotation angle, and the second rotation angle.

[0079] In practical applications, such as Figure 4 As shown, based on the structure of the drilling and anchoring machine, the coordinate systems of the sliding joint, drill arm, and drilling rig are established using the DH method. The initial coordinate system o0x0y0z0 established by the 3D laser scanner is fixedly connected to the sliding joint of the drilling and anchoring machine. The parameter table of DH, which is the transformation parameter between each coordinate system, is obtained from the actual dimensions of the drilling and anchoring machine, as shown in Table 1.

[0080] Table 1D-H Parameter Table

[0081]

[0082] Therefore, the coordinate transformation matrix between the drilling rig end and the base coordinate system is:

[0083]

[0084] in

[0085] n x = -sin(θ3); n y =0; n z =cos(θ3);

[0086] o x = -cos(θ3)sin(θ4); o y =cos(θ4); o z = -sin(θ3)sin(θ4);

[0087] a x = -cos(θ3)cos(θ4); a y = -sin(θ4); a z= -sin(θ3)cos(θ4);

[0088] p x =-d5cos(θ3)cos(θ4)-a5sin(θ3)-b4cos(θ3)-d4sin(θ3);

[0089] p y = -sin(θ4)d5;

[0090] p z =-d5sin(θ3)cos(θ4)+a5cos(θ3)-b4sin(θ3)+d4cos(θ3);

[0091] By solving the problem, the position and orientation of the drill rig's actuator end, i.e. the drill rig itself, relative to a fixed reference coordinate can be determined based on the known geometric parameters and rotation vector.

[0092] In addition, such as Figure 5 As shown, the method further includes:

[0093] Step S7: The 3D laser scanner emits a laser to scan the spatial pose of the drilling rig in real time.

[0094] Step S8: Obtain the point cloud image of the drilling rig's spatial pose.

[0095] Step S9: Transmit the real-time point cloud image data to the explosion-proof computer.

[0096] Step S10: Use the PointPillars point cloud processing algorithm to complete the drilling rig pose recognition.

[0097] Step S11: Obtain the real-time spatial pose of the drilling rig.

[0098] Step S12: Verify and validate the DH data obtained from the encoder, the first inclinometer, and the second inclinometer. Specifically, this includes averaging the spatial pose coordinates of the drilling rig obtained in real time from the 3D laser scanner with the corresponding X, Y, and Z coordinates of the drilling rig obtained through DH calculation to obtain the averaged spatial pose coordinates of the drilling rig. This averaged coordinate is then output and displayed as the spatial pose data of the drilling rig.

[0099] Step S13: Reduce the measurement errors caused by the encoder and inclinometer due to the accumulation of time.

[0100] The drilling rig spatial pose detection system and method provided by this invention have the following technical effects:

[0101] (1) By setting up a three-dimensional laser scanner and fixing it on the sliding joint, the present invention can collect and establish the initial three-dimensional coordinate system and the real-time three-dimensional coordinate system during the drilling process. The operation is not affected by harsh environment and the performance is stable.

[0102] (2) This invention uses an absolute encoder in conjunction with a three-dimensional laser scanner to collect data on the displacement of the sliding joint in real time and feeds the collected displacement data back to the explosion-proof computer. It is simple to operate and easy to implement.

[0103] (3) The present invention uses an inclinometer, which has good low-frequency characteristics and good transient response capability. When used in conjunction with a three-dimensional laser scanner, it can collect the deflection angle and lifting angle of the drill arm and drill rig in real time, and feed the collected data back to the explosion-proof computer.

[0104] (4) This invention uses an explosion-proof computer to process and analyze the data collected by the three-dimensional laser scanner, absolute encoder, and inclinometer, records the initial coordinate system constructed by the three-dimensional laser scanner, and processes and analyzes the data collected by the inclinometer to obtain the deflection angle and lifting angle of the drill arm and drill rig, and compares them with the three-dimensional coordinate system constructed in real time by the three-dimensional laser scanner. This increases the reliability and accuracy of the analysis and calculation by the explosion-proof computer and improves the measurement accuracy.

[0105] In summary, this invention, by installing a three-dimensional laser scanner and an absolute encoder on the sliding joint, and by installing an inclinometer on the drill arm and drill rig to transmit the collected displacement and angle data to an explosion-proof computer, which then performs calculations and analyses to determine the attitude change of the drill rig in three-dimensional space, makes the measuring device more stable, improves measurement accuracy, reduces labor costs, reduces the risk factor for operators, and is easy to install.

[0106] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0107] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A drilling rig spatial pose detection system, characterized in that, The system includes a 3D laser scanner, an encoder, a first inclinometer, a second inclinometer, and a first computer; The 3D laser scanner is mounted on the sliding joint of the drilling rig and connected to the first computer. It is used to acquire the initial position coordinates of the drilling rig and send the initial position coordinates to the first computer. It is also used to collect real-time spatial pose data of the drilling rig. When the displacement of the sliding joint, the rotation deflection angle of the drill arm, and the rotation lifting angle of the drilling rig are all zero, the 3D coordinate system established by the data acquired by the 3D laser scanner is the initial position. A spatial rectangular coordinate system is established with the initial position coordinates as the origin. The encoder is mounted on the sliding joint of the drilling rig and connected to the first computer. It is used to acquire the displacement of the sliding joint and send the displacement to the first computer. The encoder is mounted at the tail of the sliding joint and is located between the three-dimensional laser scanner and the connection point. The connection point is the connection point between the drill arm and the sliding joint. The first inclinometer is mounted on the drill arm and connected to the first computer, used to acquire the first rotation angle of the drill arm and transmit the first rotation angle to the first computer. The second inclinometer is mounted on the drilling rig and connected to the first computer. It is used to acquire the second rotation angle of the drilling rig and transmit the second rotation angle to the first computer. The first computer is installed inside the body of the drilling rig and is used to calculate the spatial pose data of the drilling rig based on the initial position coordinates, the displacement, the first rotation angle, and the second rotation angle. The system further includes a second computer; the second computer is connected to the first computer and is used to receive and store the initial position coordinates, the displacement, the first rotation angle, the second rotation angle, and the spatial pose data sent by the first computer; the first computer is an explosion-proof computer; the second computer is located in the ground control center, and the explosion-proof computer is connected to the second computer via a signal line; The DH algorithm is applied to determine the spatial pose data of the drilling rig in the spatial rectangular coordinate system based on the displacement, the first rotation angle, and the second rotation angle. The average of the spatial pose coordinates of the drilling rig obtained in real time by the three-dimensional laser scanner and the corresponding X, Y, and Z coordinates of the drilling rig obtained by DH solution is taken to obtain the averaged spatial pose coordinates of the drilling rig. This coordinate is then used as the spatial pose data of the drilling rig for output and display. The system also includes a support frame; the three-dimensional laser scanner is fixed to the end of the sliding joint via the support frame so that the drill arm and the range of angular variables of the drill arm are always within the field of view of the three-dimensional laser scanner.

2. The drilling rig spatial pose detection system according to claim 1, characterized in that, The centerline of the first inclinometer is in the same plane and parallel to the centerline of the drill arm; the centerline of the second inclinometer is in the same plane and parallel to the centerline of the drilling rig.

3. The drilling rig spatial pose detection system according to claim 1, characterized in that, The system also includes a shock absorption device; the first inclinometer is fixed to the drill arm by the shock absorption device; the second inclinometer is fixed to the drilling rig by the shock absorption device.

4. The drilling rig spatial pose detection system according to claim 1, characterized in that, The centerline of the 3D laser scanner is in the same plane and parallel to the centerline of the sliding joint.

5. A method for detecting the spatial pose of a drilling rig, characterized in that, The drilling rig spatial pose detection method is applied to the drilling rig spatial pose detection system as described in any one of claims 1-4, the method comprising: Obtain the initial position coordinates of the drilling rig acquired by the 3D laser scanner; A spatial rectangular coordinate system is established with the initial position coordinates as the origin; Obtain the displacement value acquired by the encoder; Obtain the first rotation angle acquired by the first inclinometer; Obtain the second rotation angle acquired by the second inclinometer; The DH algorithm is applied to determine the spatial pose data of the drilling rig in the spatial rectangular coordinate system based on the displacement, the first rotation angle, and the second rotation angle.

6. The method for detecting the spatial pose of a drilling rig according to claim 5, characterized in that, The application of the DH algorithm determines the spatial pose data of the drilling rig in the Cartesian coordinate system based on the displacement, the first rotation angle, and the second rotation angle, specifically including: The DH solution algorithm is used to establish the three-dimensional coordinate system of the sliding joint, the three-dimensional coordinate system of the drill arm, and the three-dimensional coordinate system of the drilling rig. Obtain the actual dimensions of the sliding joint, the actual dimensions of the drill arm, and the actual dimensions of the drilling rig; Based on the actual dimensions of the sliding joint, the actual dimensions of the drill arm, and the actual dimensions of the drilling rig, determine the coordinate system transformation parameter table; Based on the coordinate system transformation parameter table, determine the coordinate transformation matrix between the drilling rig end and the spatial rectangular coordinate system; Based on the coordinate transformation matrix, the displacement, the first rotation angle, and the second rotation angle, the spatial pose data of the drilling rig in the spatial rectangular coordinate system is determined.

Images