Method for Measuring Rotary Angle of Pipe Laying Machine Based on Fusion of LiDAR and Visible Light

Abstract

The present invention provides a method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light. This method is executed based on the automated operation system of the pipe laying machine. The automated operation system of the pipe laying machine includes two sets of visible light and lidar fusion devices, a signal processing computer, a centralized control PLC, and a switch connected to the switch. The rotation angle of the pipe laying machine is measured by applying the fusion technology of visible light image data and three-dimensional point cloud data and combining the changes in the position and direction of the drill pipe. The present invention has the advantages of realizing the automated operation of the pipe laying machine, reducing the manual participation in the handover process, and improving the operation efficiency.

Description

Technical Field

[0001] The present invention relates to the field of intelligent petroleum equipment, and in particular to a method for measuring the rotation angle of a pipe laying machine based on the fusion of laser radar and visible light. Background Art

[0002] Pipe handling is one of the main tasks in drilling construction, which is labor-intensive and has a low safety factor. Statistics from the International Association of Drilling Contractors show that drilling accidents during pipe handling account for 80% of the total number of accidents. It is extremely important to improve the automation of pipe handling and its working environment. The pipe stacker is the core of pipe handling and one of the busiest systems in platform operations. The pipe stacker adopts push-and-help operation, replacing the driller to take the drilling tool out of the elevator and put it into the second-level platform, or taking the drilling tool out of the second-level platform and putting it into the elevator, replacing the operator to climb up, reducing labor intensity, and improving operation safety.

[0003] Due to the uncertainty of the position and angle of the elevator, there is a certain degree of uncertainty in the position of the pipe laying machine when it is connected to the elevator. In the current automated drilling rig, when the pipe laying machine is in the process of drilling, the operation of taking the drilling tools out of the elevator and putting the drilling tools into the elevator requires manual adjustment of the position of the mechanical arm to ensure that the drilling tools can be connected to the elevator. Specifically, the driller controls the movement of the pipe laying machine by operating the touch screen and handle on the operating console, and manually operates the start and stop buttons to drive the movement of the motor. However, such manual operation has the following problems in actual operation:

[0004] 1. It places high demands on the subjective experience of operators and long-term high-intensity work;

[0005] 2. Handover errors caused by manual observation often occur, resulting in low operational efficiency and even safety accidents. Summary of the invention

[0006] The purpose of the present invention is to provide a method for measuring the rotation angle of a pipe laying machine based on the fusion of laser radar and visible light, so as to solve one or more of the above-mentioned problems in the prior art.

[0007] The present invention proposes a method for measuring the rotation angle of a pipe laying machine based on the fusion of laser radar and visible light. The method is executed based on an automated operation system of the pipe laying machine. The automated operation system of the pipe laying machine includes two sets of visible light and laser radar fusion devices, a signal processing computer, a centralized control PLC and a switch. The two sets of visible light and laser radar fusion devices, the signal processing computer and the centralized control PLC are all connected to the switch.

[0008] Two sets of visible light and lidar fusion devices are respectively aligned with the pipe laying machine and the elevating bail for installation. The fields of view of the two sets of visible light and lidar fusion devices both contain the drill pipe to be grabbed. Each set of visible light and lidar fusion devices includes a visible light module and a lidar. The visible light module is used to collect image data in front of the visible light module, and the lidar is used to collect three-dimensional point cloud data in front of the lidar;

[0009] The signal processing device is used to receive and decode the image data and three-dimensional point cloud data collected by the visible light and lidar fusion device, execute the application program of the method for measuring the slewing angle of the pipe laying machine based on the fusion of lidar and visible light, and send the result of the measured slewing angle to the centralized control PLC;

[0010] The centralized control PLC is used to drive the pipe laying machine and the elevating bail to work, receive the result of the measured slewing angle, and drive the pipe laying machine to rotate according to the result.

[0011] In some embodiments, when the two sets of visible light and lidar fusion devices are assembled but not installed, the visible light module and the lidar in the visible light and lidar fusion device are calibrated. The calibration content includes the internal parameters of the visible light module and the external parameters between the visible light module and the lidar.

[0012] In some embodiments, the internal parameters of the visible light module are calibrated by using the Zhang Zhengyou calibration method in the form of a checkerboard to obtain the focal lengths f x and f y of the x-axis and y-axis, the pixel coordinates c x and c y of the image center along the x-axis and y-axis, the radial distortion coefficients k1, k2, k3, and the tangential distortion coefficients p1, p2. Let the three-dimensional coordinates of a point in the visible light module coordinate system be (X c , Y c , Z c ). The formulas for calculating the pixel coordinates u and v of this point in the visible light module coordinate system are as follows:

[0013] x = X c / Z c

[0014] y = Y c / Z c

[0015]

[0016] x corrected = x(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p1xy + p2(r 2 + 2x 2 )

[0017] y corrected = y(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p2xy + p1(r 2 + 2y 2 )

[0018] u = f x x corrected + c x

[0019] v = f y y corrected + c y 。

[0020] In some embodiments, when calibrating the extrinsic parameters of the visible light module and the radar (R ∈ R 3×3 , T ∈ R 3×1 ), rectangular calibration plates of Aruco at different positions are used. The coordinates of the rectangular corner points of the calibration plate in the visible light module coordinate system, the coordinates of the rectangular corner points of the calibration plate in the radar coordinate system, and the relative rotation matrix R and relative translation amount T between the radar coordinate system and the visible light coordinate system are obtained respectively. After calibration, the formula for converting a point (X l , Y l , Z l ) in the radar coordinate system to the visible light module coordinate system is:

[0021]

[0022] In some embodiments,

[0023] After the assembly and installation of two sets of visible light and lidar fusion devices are completed, using the calibration results of the intrinsic parameters of the visible light module and the extrinsic parameters of the visible light module and the radar, the coordinate systems of the pipe laying machine and the lifting clamp are calibrated. The calibration parameters include the center positions of the pipe column claws and the lifting clamp jaws and the directions of the coordinate axes in the initial state. The center positions of the pipe column claws and the lifting clamp jaws in the initial state are the origin of the pipe laying machine coordinate system and the origin of the lifting clamp coordinate system respectively, and the formulas for converting a point in the radar coordinate system to the pipe laying machine coordinate system and the lifting clamp coordinate system are obtained.

[0024] In some embodiments,

[0025] The direction of the X coordinate axis in the pipe laying machine coordinate system is the same as the opening direction of the drill pipe grabbed by the pipe laying machine and is denoted as The Z coordinate axis is perpendicular to the plane of the pipe laying machine and the direction is upward and is denoted as The Y coordinate axis is the result of the cross product of the X and Z coordinate axes and is denoted as A point (x l , y l , z l ) in the radar coordinate system to the point (X p , Y p , Z p ) in the pipe handler coordinate system has the transformation formula as shown below:

[0026]

[0027] In some embodiments,

[0028] The direction of the X-axis in the elevating chuck coordinate system is the same as the opening direction of the jaws on the elevating chuck for gripping the drill pipe, denoted as The Z-axis is perpendicular to the elevating chuck plane and the direction is upward, denoted as The Y-axis is the result of the cross product of the first two coordinate axes, denoted as A point (x l , y l , z l ) in the radar coordinate system to the point (X d , Y d , Z d ) in the elevating chuck coordinate system has the transformation formula as shown below:

[0029]

[0030] In some embodiments,

[0031] Set the expected position of the drill pipe removal in place in the pipe handler coordinate system in the signal processing computer,

[0032] The centralized control PLC sends a drill pipe extraction instruction to the signal processing computer. After receiving the instruction, the signal processing computer calculates the rotation angle of the pipe handler. The specific calculation process includes the following:

[0033] Initial rotation angle calculation: Obtain the image data and 3D point cloud data acquired by the visible light and lidar fusion device currently aligned with the pipe handler. Convert the 3D point cloud data to the point cloud data in the pipe handler coordinate system according to the formula for converting a point in the radar coordinate system. Screen the point cloud data corresponding to the drill pipe. Combine the relevant parameters of the pipe handler itself to calculate the position information and direction fitting of the drill pipe in the pipe handler coordinate system. Calculate the projection position of the drill pipe on the z = 0 plane in the pipe handler coordinate system. Combine the parameters of the pipe handler itself to calculate the first position vector of the drill pipe relative to the rotation axis of the pipe handler, and calculate the angle between the first position vector and the x-axis direction, which is the initial rotation angle of the pipe handler;

[0034] Send the initial rotation angle to the centralized control PLC to control the pipe handler to rotate to the corresponding angle, move forward, grip the drill pipe, and open the jaws of the elevating chuck;

[0035] After grasping the drill pipe, perform a secondary rotation angle calculation, set the desired position where the drill pipe is taken out in place in the pipe handling machine coordinate system, and based on the height difference between the elevating collar and the pipe handling machine, combined with the direction of the drill pipe, obtain the position of the drill pipe in the z = 0 plane after the elevating collar is completely taken out. Combine the parameters of the pipe handling machine itself to calculate the second position vector of the drill pipe relative to the rotation axis of the pipe handling machine, and calculate the angle between the first vector coordinate and the second vector coordinate. The angle of this angle is the secondary rotation angle of the pipe handling machine;

[0036] The signal processing computer transmits the secondary rotation angle to the centralized control PLC. The centralized control PLC controls the pipe handling machine to rotate the corresponding angle, and then moves backward to take out the drill pipe from the elevating collar.

[0037] In some embodiments,

[0038] The centralized control PLC sends a pipe lowering instruction to the signal processing computer. At this time, the jaws of the elevating collar remain open. The pipe handling machine grabs the drill pipe and rotates to the initial angle. The signal processing computer receives the pipe lowering instruction and calculates the rotation angle of the pipe handling machine. The specific calculation process is as follows:

[0039] Obtain the image data and three-dimensional point cloud data acquired by the visible light and lidar fusion device currently aligned with the elevating collar. Convert the three-dimensional point cloud data to the point cloud data in the pipe handling machine coordinate system according to the formula for converting a point in the lidar coordinate system to the elevating collar coordinate system, and screen the point cloud data corresponding to the drill pipe;

[0040] Calculate the position information and fit the direction of the drill pipe in the elevating collar coordinate system;

[0041] Set the desired position after the drill pipe is placed in the elevating collar;

[0042] According to the position information, direction, height difference between the elevating collar and the pipe handling machine, relevant parameters of the pipe handling machine itself, and the desired position, calculate the rotation angle required by the pipe handling machine when in place;

[0043] The signal processing computer transmits the rotation angle to the centralized control PLC. The centralized control PLC controls the pipe handling machine to rotate the corresponding angle, and then moves forward to place the drill pipe into the jaws of the elevating collar, and then closes the jaws of the elevating collar.

[0044] In some embodiments, the relevant parameters of the pipe handling machine include the distance from the center point of the pipe string claw on the pipe handling machine to the center point of rotation of the rotation axis of the pipe handling machine, and the height of the pipe handling machine from the drill floor plane.

[0045] The advantages of the method for measuring the rotation angle of the pipe handling machine based on the fusion of lidar and visible light according to the present invention are:

[0046] The automated operation of the pipe racking machine is realized, greatly reducing the manual participation in the handover process. It solves the problems such as the dependence on experience in the pure manual operation during the pipe tripping operation of the pipe racking machine, and the hidden dangers of inefficient and high-risk operations caused by long-term high-intensity work, improving the operation efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a schematic structural diagram of the pipe racking machine automated operation system in some embodiments of the present invention;

[0048] Figure 2 It is the pipe racking machine rotation angle measurement process when taking out the drill in some embodiments of the present invention;

[0049] Figure 3 It is the pipe racking machine rotation angle measurement process under the pipe tripping condition in some embodiments of the present invention;

[0050] Figure 4 It is a schematic diagram of the pipe racking machine coordinate system in some embodiments of the present invention;

[0051] Figure 5 It is a schematic diagram of the elevator bottom surface coordinate system in some embodiments of the present invention;

[0052] Figure 6 It is a schematic diagram of the drill pipe identification in front of the pipe racking machine in some embodiments of the present invention;

[0053] Figure 7 It is a schematic diagram of the drill pipe identification in front of the elevator in some embodiments of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0054] This embodiment proposes a pipe racking machine automated operation system based on the pipe racking machine rotation angle measurement method of the fusion of lidar and visible light,

[0055] Combined with Figure 1 as shown in the content, the pipe racking machine automated operation system includes two sets of visible light and lidar fusion devices, a signal processing computer, a centralized control PLC, and a switch,

[0056] Each set of visible light and lidar fusion devices includes a visible light module and a lidar. For example:

[0057] The DJI Livox Mid-70 laser detection and ranging sensor is selected as the lidar, which outputs a 905-nanometer band laser beam that is invisible to humans, avoiding affecting the line of sight of the operating personnel. This lidar can have an observation blind area as close as 0.05 meters, and the working distance can reach 260 meters. The field of view is a 70.4-degree circular field of view, and the point cloud density is 100,000 points per second, which can well meet the measurement and observation requirements of high resolution at close range;

[0058] The IMX385 low-light sensor is selected as the visible light module to meet the shooting requirements during night operations. The imaging resolution is 1920*1080, and a lens with a focal length of 5.1 mm is selected to meet the shooting requirements for a medium and small field of view.

[0059] After the two sets of visible light and lidar fusion devices are assembled, the internal parameters of the camera (i.e., the internal parameters of the visible light module) and the external parameters of the camera and the lidar (the external parameters of the visible light module and the lidar) are calibrated in a laboratory environment. Among them

[0060] The internal parameters of the camera are calibrated using the Zhang Zhengyou calibration method with a checkerboard to obtain the focal lengths f x and f y of the x-axis and y-axis, the pixel coordinates c x and c y of the image center along the x-axis and y-axis, the radial distortion coefficients k1, k2, k3, and the tangential distortion coefficients p1, p2. Let the three-dimensional coordinates of a point in the visible light module coordinate system be (X c , Y c , Z c ). The formulas for calculating the pixel coordinates u and v of this point in the visible light module coordinate system are as follows:

[0061] x = X c / Z c

[0062] y = Y c / Z c

[0063]

[0064] x corrected = x(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p1xy + p2(r 2 + 2x 2 )

[0065] y corrected = y(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p2xy + p1(r 2 + 2y 2 )

[0066] u = f x x corrected + c x

[0067] v = f y y corrected + cy ;

[0068] Extrinsic parameters of the camera and the radar During calibration, Aruco rectangular calibration plates at different positions are used. The coordinates of the rectangular corners of the calibration plate in the visible light module coordinate system are obtained through the homography matrix. The coordinates of the rectangular corners of the calibration plate in the radar coordinate system are obtained through existing operations such as plane estimation and edge extraction. The relative rotation matrix R and relative translation T between the radar and the visible light module coordinate systems are calculated by the 3D-ICP method. After calibration, the formula for converting a point (X l , Y l , Z l ) in the radar coordinate system to the visible light module coordinate system is:

[0069]

[0070] The algorithms and the like applied in this calibration process can all be directly implemented by existing technologies, so they will not be elaborated here.

[0071] After calibration, on-site installation is carried out. Two sets of visible light and lidar fusion devices are respectively aligned with the pipe laying machine and the lifting clamp for installation, and the fields of view of the two sets of visible light and lidar fusion devices both include the drill pipes to be grabbed.

[0072] After installation, using the calibration results of the internal parameters of the camera and the extrinsic parameters of the camera and the radar obtained previously, the coordinate systems of the pipe laying machine and the lifting clamp are calibrated through the visible light and lidar fusion device, forming a pipe laying machine coordinate system as shown in Figure 4 and a bottom surface coordinate system of the lifting clamp as shown in Figure 5 . Determine the spatial range of the drill pipe in the pipe laying machine coordinate system and the lifting clamp coordinate system, and obtain the formula for converting a point (x l , y l , z l ) in the radar coordinate system to the pipe laying machine coordinate system and the lifting clamp coordinate system. The calibration parameters include the center positions of the pipe column claws and the lifting clamp jaws and the directions of the coordinate axes in the initial state. The center positions of the pipe column claws and the lifting clamp jaws in the initial state are the origin of the pipe laying machine coordinate system and the origin of the lifting clamp coordinate system respectively.

[0073] The direction of the X coordinate axis in the pipe laying machine coordinate system is the same as the opening direction of the drill pipe grabbed on the pipe laying machine, denoted as The Z coordinate axis is perpendicular to the plane of the pipe laying machine, and the direction is upward, denoted as The Y coordinate axis is the result of the cross product of the X and Z coordinate axes, denoted as A point (x l , y l , z l ) in the radar coordinate system to the point (X p, Y p , Z p ) The transformation formula is as follows:

[0074]

[0075] In the elevating bail unit coordinate system, the direction of the X-axis is the same as the opening direction of the jaws on the elevating bail unit for gripping the drill pipe, denoted as The Z-axis is perpendicular to the elevating bail unit plane and the direction is upward, denoted as The Y-axis is the result of the cross product of the first two coordinate axes, denoted as A point (x l , y l , z l ) in the radar coordinate system to a point (X d , Y d , Z d ) in the elevating bail unit coordinate system has the following transformation formula:

[0076]

[0077] The process of calibrating the coordinate systems of the above-mentioned pipe racking machine and elevating bail unit can be realized by using existing technologies. For example: Set the center positions of the pipe column claws of the pipe racking machine and the jaws of the elevating bail unit in their corresponding visible light module coordinate systems at the initial state.

[0078] Connect two sets of visible light and lidar fusion devices to the switch located in the driller's cabin through two gigabit Ethernet cables respectively. The signal processing computer and the centralized control PLC are also connected to the switch. Use the switch for communication. The connection reliability between the centralized control PLC and the signal processing computer is ensured by obtaining the heartbeat packet of the signal processing computer. The visible light and lidar fusion device sends the 1080P video signal and the lidar point cloud signal to the signal processing computer in real time. The signal processing computer uses a high-performance computing server for computing and fusing processing algorithms.

[0079] During the operation of the pipe racking machine automation system, the signal processing computer decodes the video obtained by the visible light module through the network and the switch to obtain real-time image data, and at the same time obtains the three-dimensional point cloud data collected by the lidar in real time through the network and the switch.

[0080] The pipe racking machine only makes telescopic and rotational movements, and the vertical height remains unchanged.

[0081] Combined with Figure 2 As shown in the content, when the elevating bail unit arrives at the position and takes the drill, the elevating bail unit is above the pipe racking machine. During the movement of the pipe racking machine from the finger board area to the waiting area, the centralized control PLC sends an identification instruction for the pipe racking machine to take the drill to the signal processing computer. After receiving the instruction, the signal processing computer starts the identification and obtains as Figure 6Align the current moment shown with the image data obtained by the visible light and lidar fusion device of the pipe racking machine and the three-dimensional point cloud data within the corresponding time period, and identify the rotation angle A (the rotation angle A is the angle of the first rotation when the pipe racking machine picks up the drill). The identification process is as follows: Convert the three-dimensional point cloud data in the radar coordinate system to the pipe racking machine coordinate system according to the transformation formula from a point in the radar coordinate system to a point in the pipe racking machine coordinate system. Set the constraint relationship in the pipe racking machine coordinate system (that is, set the value range of the x, y, and z coordinate values), and screen the point cloud data that meets the constraint relationship, which is the point cloud data of the drill pipe. According to the point cloud data of the drill pipe and combined with the relevant parameters of the pipe racking machine itself (the relevant parameters include the distance from the center point of the pipe string claw on the pipe racking machine to the rotation axis of the pipe racking machine and the height of the pipe racking machine from the drill floor plane), calculate the position of the drill pipe in the pipe racking machine coordinate system and fit the direction, obtain the projection of the current drill pipe on the z = 0 plane in the pipe racking machine coordinate system, calculate the center point of the projection of the current drill pipe, and calculate the projection position of the center point of rotation of the rotation axis of the pipe racking machine on the z = 0 plane in the pipe racking machine coordinate system in combination with the parameters of the pipe racking machine itself. Obtain the first position vector of the center point of the projection of the current drill pipe relative to the center point of the projection of the center of rotation of the pipe racking machine rotation axis, and calculate the angle between the first position vector and the x-axis direction in the pipe racking machine coordinate system. The angle of this angle is the angle of the rotation angle A. The signal processing computer sends the identified rotation angle A to the target rotation angle position in the driller control software in the centralized control PLC to control the pipe racking machine to rotate to the corresponding target angle A. The driller controls the telescopic mechanism of the pipe racking machine to move forward (that is, extend in the direction where the drill pipe is located), and the pipe racking machine successfully grabs the drill pipe and the jaws of the elevator open. When grabbing the drill pipe and taking it out of the jaws of the elevator, set the expected position after the drill pipe is taken out in the pipe racking machine coordinate system in the signal processing computer in advance. Let the height difference between the elevator and the pipe racking machine be d_h, and identify the target angle B of the pipe racking machine when the drill pipe is taken out. The identification process is as follows: Calculate the horizontal position of the drill pipe on the z = d_h plane after the pipe racking machine completely takes out the drill pipe, and then combine the direction of the drill pipe to obtain the position of the drill pipe on the z = 0 plane in the pipe racking machine coordinate system when the drill pipe is completely taken out and in place at the expected position. Calculate the projection position of the center point of rotation on the current pipe racking machine rotation axis on the z = 0 plane in the pipe racking machine coordinate system in combination with the parameters of the pipe racking machine itself. Obtain the second vector coordinate of the position of the drill pipe on the z = 0 plane in the pipe racking machine coordinate system at the expected position after taking out and in place relative to the projection position of the center point of rotation on the current pipe racking machine rotation axis on the z = 0 plane in the pipe racking machine coordinate system. The angle between the second vector and the x-axis in the pipe racking machine coordinate system is the target angle B, and calculate the angle between the first vector coordinate and the second vector coordinate. The angle of this angle is the angle of the second rotation.The angle of the second rotation is sent to the driller control software in the centralized control PLC. The centralized control PLC controls the pipe laying machine to rotate the corresponding angle to reach the target angle B. After the rotation is completed, the driller controls the telescopic mechanism of the pipe laying machine to move backward (i.e., retract in the direction opposite to the direction where the drill rod is located), and the pipe laying machine takes the drill rod out of the elevator.

[0082] Combination Figure 3 As shown in the figure, when drilling, the elevator stays near the second-floor platform, and the jaws of the elevator remain open. The pipe laying machine grabs the drill rod from the finger beam area to the preparation area and rotates to the 0-angle position (the 0-angle position refers to the position where the angle between the rotation axis of the pipe laying machine and the plane z=0 in the elevator coordinate system and the x-axis is 0). The pipe laying machine automation system sends a recognition command to the algorithm through the centralized control PLC. After receiving the command, the signal processing computer starts the recognition and obtains the following information: Figure 7 As shown, the image data obtained by the visible light and laser radar fusion device aimed at the elevator at the current moment and the three-dimensional point cloud data in the corresponding time period are converted from the radar coordinate system to the elevator coordinate system according to the transformation formula from a point in the radar coordinate system to the point in the elevator coordinate system, and the constraint relationship in the elevator coordinate system is set (that is, the value range of the x, y, and z coordinate values ​​is set), and the point cloud data that meets the constraint relationship is screened, which is the point cloud data of the drill rod. The position of the drill rod in the elevator coordinate system is calculated and the direction is fitted according to the point cloud data of the drill rod, and the coordinate information and direction of the current drill rod are obtained. The expected position of the drill rod after the elevator is put into place is set in the signal processing computer in advance. At this time, the elevator is above the pipe laying machine. The height difference between the elevator and the pipe laying machine is set to d_h, and the horizontal position in the z=d_h plane is set after the pipe laying machine puts the drill rod into the elevator in the expected position, and then combined with the direction of the drill rod, the result is obtained. The position of the drill rod in the z=0 plane when the drill rod is placed in the expected position after the elevator is in place, and the projection position of the center point of rotation on the current pipe racking machine rotation axis on the z=0 plane in the pipe racking machine coordinate system is calculated in combination with the parameters of the pipe racking machine itself, and the third vector coordinates of the position of the drill rod in the z=0 plane relative to the projection position of the center point of rotation on the current pipe racking machine rotation axis on the z=0 plane in the pipe racking machine coordinate system when the drill rod is placed in the expected position after the elevator is in place are obtained, and the angle between the third vector coordinates and the x-axis is calculated, and the angle of the angle is the rotation angle of the current pipe racking machine. The signal processing computer sends the recognition result (i.e., the rotation angle) to the target rotation angle position in the driller control software in the centralized control PLC, and controls the pipe racking machine to rotate to the corresponding angle to reach the target angle C. The driller controls many mechanisms of the pipe racking machine to move forward, and places the drill rod in the tiger's mouth of the elevator. After confirming that the drill rod is in place, the tiger's mouth of the elevator is closed, and the pipe column claw of the pipe racking machine is opened.

[0083] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the inventive concept of the present invention, several similar modifications and improvements can be made, and these should also be regarded as the protection scope of the present invention.

Claims

1. A method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light, characterized in that, The measuring method for the swing angle of the pipe laying machine is executed based on the automatic operation system of the pipe laying machine. The automatic operation system of the pipe laying machine includes two sets of visible light and lidar fusion devices, a signal processing computer, a centralized control PLC, and a switch. The two sets of visible light and lidar fusion devices, the signal processing computer, and the centralized control PLC are all connected to the switch. The two sets of visible light and lidar fusion devices are respectively aligned with the pipe laying machine and the lifting clamp for installation. The fields of view of the two sets of visible light and lidar fusion devices both include the drill pipe to be grabbed. Each set of visible light and lidar fusion device includes a visible light module and a lidar. The visible light module is used to collect image data in front of the visible light module, and the lidar is used to collect three-dimensional point cloud data in front of the lidar. The signal processing device is used to receive and decode the image data and three-dimensional point cloud data collected by the visible light and lidar fusion device, execute the application program of the measuring method for the swing angle of the pipe laying machine based on the fusion of lidar and visible light, and send the result of the measured swing angle to the centralized control PLC. The centralized control PLC is used to drive the pipe laying machine and the lifting clamp to work, receive the result of the measured swing angle, and drive the pipe laying machine to rotate according to the result. Set the expected position where the drill pipe is taken out in place in the coordinate system of the pipe laying machine in the signal processing computer. The centralized control PLC sends a drill pipe taking instruction to the signal processing computer. After receiving the instruction, the signal processing computer calculates the swing angle of the pipe laying machine. The specific calculation process is as follows: Initial rotation angle calculation: Obtain the image data and three-dimensional point cloud data acquired by the visible light and lidar fusion device currently aligned with the pipe laying machine. Convert the three-dimensional point cloud data in the lidar coordinate system to the coordinate system of the pipe laying machine according to the transformation formula from a point in the lidar coordinate system to a point in the coordinate system of the pipe laying machine, and screen the point cloud data corresponding to the drill pipe. Combine the relevant parameters of the pipe laying machine itself to calculate the position information and direction fitting of the drill pipe in the coordinate system of the pipe laying machine, calculate the projection position of the drill pipe on the z = 0 plane in the coordinate system of the pipe laying machine, and calculate the first position vector of the drill pipe relative to the rotation axis of the pipe laying machine in combination with the parameters of the pipe laying machine itself, and calculate the angle between the first position vector and the x-axis direction, which is the initial rotation angle of the pipe laying machine. Send the initial rotation angle to the centralized control PLC to control the pipe laying machine to rotate to the corresponding angle, move forward, grab the drill pipe, and open the jaws of the lifting clamp. After grabbing the drill pipe, perform the secondary rotation angle calculation. Set the expected position after the drill pipe is taken out in place in the coordinate system of the pipe laying machine in the signal processing computer in advance. According to the height difference between the lifting clamp and the pipe laying machine, combined with the direction of the drill pipe, obtain the position of the drill pipe on the z = 0 plane in the coordinate system of the pipe laying machine when the drill pipe is completely taken out in place from the lifting clamp. Combine the relevant parameters of the pipe laying machine itself to calculate the second position vector of the drill pipe relative to the rotation axis of the pipe laying machine, and calculate the angle between the first position vector and the second position vector. The angle of this angle is the secondary rotation angle of the pipe laying machine. The signal processing computer transmits the secondary rotation angle to the centralized control PLC. The centralized control PLC controls the pipe laying machine to rotate the corresponding angle, and then moves backward to take out the drill pipe from the lifting clamp. The centralized control PLC sends a pipe-lowering instruction to the signal processing computer. At this time, the jaws of the elevating bail remain open, the pipe handler grabs the drill pipe and rotates to the initial angle. The signal processing computer receives the pipe-lowering instruction and calculates the rotation angle of the pipe handler. The specific calculation process is as follows: Obtain the image data and three-dimensional point cloud data acquired by the visible light and lidar fusion device that is currently aligned with the elevating bail. Convert the three-dimensional point cloud data in the lidar coordinate system to the elevating bail coordinate system according to the transformation formula from a point in the lidar coordinate system to a point in the elevating bail coordinate system, and filter the point cloud data corresponding to the drill pipe; Calculate the position information of the drill pipe and fit the direction in the elevating bail coordinate system; Set the expected position after the drill pipe is placed in the elevating bail; Calculate the rotation angle required by the pipe handler when in place according to the position information, direction of the drill pipe, height difference between the elevating bail and the pipe handler, relevant parameters of the pipe handler itself, and the expected position; The signal processing computer transmits the rotation angle to the centralized control PLC. The centralized control PLC controls the pipe handler to rotate by the corresponding angle, then moves forward, places the drill pipe into the jaws of the elevating bail, and then closes the jaws of the elevating bail.

2. The method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light according to claim 1, wherein, When the two sets of visible light and lidar fusion devices are assembled but not installed, calibrate the visible light module and lidar inside the visible light and lidar fusion device. The calibration content includes the internal parameters of the visible light module and the external parameters between the visible light module and the lidar.

3. The method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light according to claim 2, wherein, The internal parameters of the visible light module are calibrated using the Zhang Zhengyou calibration method with a checkerboard pattern to obtain the focal lengths f of the x-axis and y-axis x and f y , the pixel coordinates c of the image center along the x-axis and y-axis x and c y , the radial distortion coefficients k1, k2, k3, and the tangential distortion coefficients p1, p2. Let the three-dimensional coordinates of a point in the visible light module coordinate system be (X c , Y c , Z c ). The formulas for calculating the pixel coordinates u and v of this point in the visible light module coordinate system are as follows: x = X c / Z c y = Y c / Z c x corrected = x(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p1xy + p2(r 2 + 2x 2 ) y corrected = y(1 + k1r 2 + k2r 4 + k3r 6 ) + 2p2xy + p1(r 2 + 2y 2 ) u = f x x corrected + c x v = f y y corrected + c y 。 4. The method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light according to claim 2, wherein, External parameters of the visible light module and the radar (R∈R 3×3 , T∈R 3×1 ) When calibrating, Aruco rectangular calibration plates at different positions are used to obtain the coordinates of the rectangular corner points of the calibration plate in the visible light module coordinate system, the coordinates of the rectangular corner points of the calibration plate in the radar coordinate system, and the relative rotation matrix R and relative translation amount T between the radar coordinate system and the visible light coordinate system. After calibration, the formula for converting a point (X l , Y l , Z l ) in the radar coordinate system to the visible light module coordinate system is:

5. The method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light according to claim 2, wherein, After the two sets of visible light and lidar fusion devices are assembled and installed, use the calibration results of the internal parameters of the visible light module and the external parameters between the visible light module and the lidar to calibrate the coordinate systems of the pipe handler and the elevating bail. The calibration parameters include the central positions of the pipe column claws of the pipe handler and the jaws of the elevating bail and the directions of the coordinate axes in the initial state. The central positions of the pipe column claws of the pipe handler and the jaws of the elevating bail in the initial state are the origin of the pipe handler coordinate system and the origin of the elevating bail coordinate system respectively, and obtain the formula for converting a point in the lidar coordinate system to the pipe handler coordinate system and the elevating bail coordinate system.

6. The method for measuring the rotation angle of a pipe laying machine based on the fusion of lidar and visible light according to claim 5, wherein, In the pipe laying machine coordinate system, the direction of the X-axis is the same as the opening direction of the gripper for the drill pipe on the pipe laying machine, denoted as The Z-axis is perpendicular to the plane of the pipe laying machine, and the upward direction is denoted as The Y-axis is the result of the cross product of the X and Z axes, denoted as A point (x l , y l , z l ) in the radar coordinate system to the point (X p , Y p , Z p ) in the pipe laying machine coordinate system has the transformation formula shown below:

7. The method for measuring the slewing angle of the pipe laying machine based on the fusion of lidar and visible light according to claim 5, wherein, In the elevating bowl coordinate system, the direction of the X-axis is the same as the opening direction of the jaws on the elevating bowl, denoted as The Z-axis is perpendicular to the elevating bowl plane, and the upward direction is denoted as The Y-axis is the result of the cross product of the first two coordinate axes, denoted as A point (x l , y l , z l ) in the radar coordinate system to a point (X d , Y d , Z d ) in the elevating bowl coordinate system has the transformation formula as follows:

8. The method for measuring the slewing angle of the pipe laying machine based on the fusion of lidar and visible light according to claim 1, wherein, The relevant parameters of the pipe handler itself include the distance from the center point of the pipe column claws on the pipe handler to the center point of the rotation of the pipe handler's rotation axis and the height of the pipe handler from the drill floor plane.

Images