Drilling robot automatic rod adding recognition system based on 3D vision and control method

Abstract

The invention relates to the technical field of industrial robot control, in particular to a drilling robot automatic rod adding recognition system and control method based on 3D vision, and the system comprises a visual feature collection module, a drilling state analysis module, a fragmentation strategy decision module, an instruction dynamic generation module and a rod adding state verification module. According to the method, through axial sampling interval parameter setting of three-dimensional point cloud data and thread form depth gradient value extraction, in combination with guide groove space coordinate registration, precise quantification of thread lead angle density dispersion is realized, and spatial feature reconstruction precision is improved; dynamically coupling the density change rate of a thread meshing area with a hydraulic propulsion pressure value, and superposing a deflection angle detection value to construct a multi-dimensional offset model, so as to enhance the fault-tolerant capability of drill rod butt joint state analysis; and on the basis of real-time calculation of a density gradient parameter and a mechanical arm angular velocity ratio, three-dimensional fragmentation unit parameters are divided to form a self-adaptive threshold value, and the point cloud data processing efficiency is optimized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial robot control, and in particular to a 3D vision-based automatic rod adding recognition system and control method for a drilling robot. BACKGROUND

[0002] The technical field of industrial robot control includes key technical elements such as mechanical structure design, sensor integration, motion control system and programmed operation, and its core is to realize automation through multi-dimensional perception and accurate execution. This field involves systematic technical architecture such as spatial positioning accuracy control of mechanical arm, dynamic response optimization of end effector, multi-axis linkage trajectory planning, focuses on solving the problems of equipment autonomous decision-making and high-precision operation under complex working conditions, and forms a complete technical chain from environment perception to action execution.

[0003] Among them, the 3D vision-based automatic rod adding recognition system for a drilling robot refers to a technical solution constructed for the real-time monitoring and automatic connection needs of drill rod length in deep hole drilling operation. The spatial coordinates of the drill rod end are obtained through a three-dimensional point cloud data real-time acquisition device, a dynamic compensation model is established in combination with the drilling depth parameters, a spatial geometric solving method is used to determine the drill rod connection opportunity, and finally a multi-degree-of-freedom mechanical arm performs new drill rod grabbing and precise docking operation, realizing full automation control of the drill rod supply process.

[0004] The existing technology has three significant shortcomings: first, the spatial positioning of the mechanical arm relies on single sensor data, and multi-dimensional feature parameter fusion is not achieved, resulting in accumulation of spatial coordinate solving errors during drill rod docking. Actual measurement data shows that this error can reach ±1.5mm in deep hole drilling operation; second, the motion control system uses a fixed slicing strategy, lacks a dynamic threshold adjustment mechanism based on real-time working conditions, and the point cloud data processing delay increases by 56% under complex geological conditions, affecting the timeliness of drill rod connection; third, the end effector action verification relies on single physical quantity monitoring, and a multi-parameter composite evaluation model has not been established, resulting in a high error rate of 18% under equipment vibration conditions, causing unplanned equipment downtime. These shortcomings directly lead to the existing system requiring manual intervention 2.3 times per hundred meters in deep hole drilling operation, significantly reducing operation continuity. SUMMARY

[0005] The purpose of the present application is to solve the shortcomings in the prior art and to propose a 3D vision-based automatic rod adding recognition system and control method for a drilling robot.

[0006] To achieve the above purpose, the present application adopts the following technical scheme: the 3D vision-based automatic rod adding recognition system for a drilling robot comprises:

[0007] The visual feature acquisition module acquires three-dimensional point cloud data of threads through a drill rod joint contour scanner, sets an axial sampling interval according to a standard pitch, extracts thread profile depth gradient values, fuses guide groove positioning sensor coordinates for spatial registration, calculates thread lead angle point cloud density dispersion, and generates a point cloud reconstruction feature matrix;

[0008] The drill hole state analysis module calls the point cloud reconstruction feature matrix, calculates the product of the thread meshing area point cloud density variation rate and the hydraulic propulsion pressure sensor reading, and generates a drill rod butt joint offset when the value exceeds a threshold value by superimposing the data of the drill rod deflection angle detector;

[0009] The slice strategy decision module calculates the point cloud density gradient and the mechanical arm angular velocity ratio based on the drill rod butt joint offset, divides a three-dimensional slice unit to generate a dynamic slice threshold when the value exceeds a reference value;

[0010] The instruction dynamic generation module calls the dynamic slice threshold to perform interpolation operation on the point cloud slice number and the joint temperature, and generates a torque compensation instruction parameter by extracting the reflection intensity range when the value exceeds a threshold value;

[0011] The rod adding state verification module acquires the torque compensation instruction parameter, compares the thread gradient and the standard curve to calculate the three-axis similarity, and generates a rod adding completion degree coefficient by superimposing the angular displacement deviation of the rotary encoder.

[0012] As a further scheme of the application, the point cloud reconstruction feature matrix specifically includes an axial sampling interval parameter, a thread profile depth gradient value, a guide groove space coordinate, and a thread lead angle density dispersion, the drill rod butt joint offset includes a meshing area density variation rate, a hydraulic propulsion pressure value, and a deflection angle detection value, the dynamic slice threshold specifically refers to a density gradient parameter, an angular velocity ratio coefficient, and a three-dimensional slice unit parameter, the torque compensation instruction parameter includes a slice number threshold, a joint temperature interpolation amount, and a reflection intensity range value, and the rod adding completion degree coefficient specifically refers to a three-axis curve similarity and an angular displacement deviation value.

[0013] As a further scheme of the application, the visual feature acquisition module includes:

[0014] A data acquisition and processing module, a feature extraction submodule, and a density calculation submodule;

[0015] The data acquisition and processing module acquires three-dimensional point cloud data of threads through a drill rod joint contour scanner, sets an axial sampling interval according to a standard pitch, and generates an axial sampling point set;

[0016] The feature extraction submodule extracts thread profile depth gradient values based on the axial sampling point set, fuses guide groove positioning sensor coordinates for spatial registration, and establishes a spatial registration parameter

[0017] Density calculation submodule: call space registration parameters, use formula:

[0018]

[0019] Operation obtains discrete quantitative value, generates point cloud reconstruction feature matrix in combination with thread rise angle parameter;Wherein, rho i Represent the point cloud density of the i-th sampling point, mu indicates the average density of the axial sampling point set, theta i Thread rise angle measurement value, sigma is the standard deviation of the guide groove space coordinates, and n is the number of effective sampling points.

[0020] As a further scheme of the application, the drill hole state analysis module further comprises a product calculation submodule and an offset synthesis submodule.

[0021] Product calculation submodule: call point cloud reconstruction feature matrix, calculate the product of the thread engagement area point cloud density change rate and the hydraulic propulsion pressure sensor reading, and generate a dynamic coupling product value;

[0022] Offset synthesis submodule: monitor the dynamic coupling product value compared with the preset pressure threshold value, super-threshold value, superimpose drill pipe deflection angle detector data, use formula:

[0023]

[0024] Operation obtains spatial offset, establishes drill pipe butt joint offset;Wherein, P represents the dynamic coupling product value, alpha indicates the deflection angle detection value, beta is the thread engagement area density reference value, gamma is the current density change rate, delta corresponds to the guide groove coordinate offset, and eta is the hydraulic system response coefficient.

[0025] As a further scheme of the application, the fragmentation strategy decision module further comprises a ratio calculation submodule and a fragmentation generation submodule.

[0026] Ratio calculation submodule: calculate the ratio of point cloud density gradient and mechanical arm angular velocity based on drill pipe butt joint offset, generate dynamic ratio coefficient;

[0027] Fragmentation generation submodule: monitor the dynamic ratio coefficient compared with the preset fragmentation reference value, super-reference value, divide the solid fragmentation unit, and establish dynamic fragmentation threshold.

[0028] As a further scheme of the application, the instruction dynamic generation module comprises an interpolation operation submodule and a range extraction submodule.

[0029] Interpolation operation submodule: call dynamic fragmentation threshold, perform cubic spline interpolation operation on point cloud fragmentation number and joint temperature, and generate fragmentation temperature interpolation amount;

[0030] Range extraction submodule: monitor the slice temperature interpolation quantity compared with the preset slice threshold value, extract the reflection intensity range when exceeding the threshold value, and establish the torque compensation instruction parameter.

[0031] As a further scheme of the present application, the rod adding state verification module comprises a similarity calculation submodule and a composite verification submodule.

[0032] Similarity calculation submodule: obtain the torque compensation instruction parameter, compare the thread gradient with the standard curve, calculate the matching degree of three-axis directions, and generate three-axis similarity values;

[0033] Composite verification submodule: call the three-axis similarity values, superimpose the angular displacement deviation of the rotary encoder, and use the formula:

[0034]

[0035] Operation to obtain the composite verification coefficient and establish the rod adding completion coefficient; wherein, S i represents the i-th axis similarity value, φ i is the corresponding axis phase angle, and Δθ i represents the angular displacement deviation of each axis.

[0036] The automatic rod adding recognition control method of the drilling robot based on 3D vision comprises the following steps:

[0037] S1: Obtain the spatial position image of the drill rod end and the drilling axis through the 3D vision sensor, extract the drill rod end center point coordinates and the drilling axis direction vector, calculate the lateral offset distance and longitudinal inclination deviation of the drill rod end and the target drilling axis, combine the drill rod length parameter and the pushing speed parameter, and generate the drill rod pose parameter set containing the spatial coordinates, offset, and inclination value;

[0038] S2: Call the lateral offset distance and longitudinal inclination deviation in the drill rod pose parameter set, combine the drilling rig manipulator joint angle parameter, establish the drill rod end motion trajectory coordinate system, calculate the joint angle compensation amount through inverse kinematics, and generate the offset compensation parameter containing X / Y / Z three-axis compensation amount;

[0039] S3: Based on the pushing speed parameter and the drilling depth parameter in the drill rod pose parameter set, calculate the current drill rod remaining stroke, difference the remaining stroke and the preset single rod length, and when the remaining stroke is less than the product of the single rod length and the safety factor, generate a binary rod adding trigger signal;

[0040] S4: Activate the mechanical arm motion trajectory planning according to the rod adding trigger signal, call the three-axis compensation amount in the offset compensation parameter, combine the drill rod library positioning coordinates and the mechanical arm end gripper parameter, and generate the control instruction sequence containing the grabbing path, compensation motion trajectory, and docking positioning accuracy;

[0041] S5: Monitor the mechanical arm joint torque parameters and visual positioning feedback parameters during the execution of the control instruction sequence, and when the gripper pressure value reaches the preset docking threshold and the pose offset is less than the positioning accuracy threshold, update the drilling depth parameter and the drill pipe inventory quantity parameter, and generate a state update parameter containing the new rod length and the updated drilling depth.

[0042] Compared with the prior art, the advantages and positive effects of the present application are that:

[0043] In the present application, by setting the axial sampling interval parameter of the three-dimensional point cloud data and extracting the thread tooth type depth gradient value, combined with the guide groove space coordinate registration, the precision quantization of the thread rise angle density dispersion is realized, and the spatial feature reconstruction accuracy is improved; the thread engagement area density change rate is dynamically coupled with the hydraulic propulsion pressure value, and a multi-dimensional offset model is constructed by superimposing the deflection angle detection value, to enhance the fault tolerance capability of the drill pipe docking state analysis; based on the real-time calculation of the density gradient parameter and the mechanical arm angular velocity ratio, the adaptive threshold is formed by dividing the three-dimensional slicing unit parameter, to optimize the point cloud data processing efficiency; by the collaborative operation of the slicing number threshold and the joint temperature interpolation amount, combined with the extraction of the reflection intensity range value, a dynamic torque compensation mechanism is established, to improve the control stability under complex working conditions; the three-axis curve similarity and the angular displacement deviation value are used for composite verification, to construct a rod adding completion quantization evaluation system, to ensure the operation reliability of the drill pipe splicing process, and the processing logic realizes the technical indexes of improving the spatial positioning accuracy by 37%, improving the docking operation response speed by 42%, and increasing the abnormal working condition recognition accuracy by 29% in the drilling operation through multi-source data fusion, dynamic threshold adjustment and composite verification mechanism. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 The system flowchart of the present application is shown in Figure 1.

[0045] Figure 2 The system block diagram of the present application is shown in Figure 2.

[0046] Figure 3 The acquisition module flowchart of the present application is shown in Figure 3. DETAILED DESCRIPTION

[0047] In order to make the purpose, technical scheme and advantages of the present application clearer and more understandable, the present application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and do not limit the present application.

[0048] In the description of the present application, it should be understood that the terms "length", "width", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer" and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation on the present application. In addition, in the description of the present application, the meaning of "a plurality of" is two or more, unless otherwise specifically limited.

[0049] Referring to Figure 1 and Figure 2 , a 3D vision-based automatic rod recognition system for a drilling robot includes:

[0050] A visual feature acquisition module: obtains a three-dimensional point cloud data stream of a threaded contact surface through a drill rod joint contour scanner, sets an axial sampling interval of 0.18mm matching a standard pitch of a drill rod, extracts a depth gradient value of each thread profile, performs spatial registration of point cloud data of adjacent two thread periods with coordinate data of a guide groove positioning sensor, calculates a point cloud density distribution dispersion in a thread lead angle direction, and generates a point cloud reconstruction feature matrix;

[0051] When obtaining a three-dimensional point cloud data stream through a contour scanner, set the axial sampling interval to 0.18mm, use laser triangulation to collect thread profile surface coordinates, in the example, when scanning a Φ73mm drill rod joint, obtain an axial cross-sectional contour at a rate of 2000 points per second, calculate the gradient of the height difference between each thread top and thread bottom, trigger a compensation scan when the thread profile height difference is detected to be less than 0.8mm, register the point cloud of adjacent thread periods with the guide groove positioning data through the ICP algorithm, select the number of point clouds within 1mm in the thread lead angle direction when calculating, and generate a 32x32 feature matrix when the standard deviation of the point cloud density exceeds 15 / mm 3 , in the example, the point cloud density of the APINC50 joint in the lead angle direction of 25° is measured to be 82 / mm 3 , and the dispersion coefficient is 0.18;

[0052] A drilling state analysis module: based on the point cloud reconstruction feature matrix, calls real-time readings of a hydraulic propulsion pressure sensor, calculates the product of the point cloud density change rate of the thread engagement area and the axial pressure, when the product value exceeds a sealing ring deformation warning threshold, superimposes the radial offset collected by a drill rod deflection angle detector to generate a drill rod butt joint offset;

[0053] When retrieving pressure sensor data, pressure values ​​within the 0-40MPa range are read at 10ms intervals. When calculating the rate of change in point cloud density in the thread meshing area, the ratio of the difference in point cloud count over five consecutive 50ms sampling periods to the time interval is taken. In this example, when the density increases from 120 points / mm²... 3 Increased to 145 / mm 3 The rate of change is 500 units / (mm) 3 Multiplying the pressure by the real-time pressure of 25 MPa (·s) gives 12500 MPa·per / (mm) 3 When the value exceeds the warning threshold of 10000, the radial offset detection value is superimposed. In the example, the laser displacement sensor measured a radial offset of 0.35mm. When generating the offset parameter, the formula (pressure product value / 1000) + (radial offset × 100) is used to obtain 12.5 + 35 = 47.5 docking offset.

[0054] Segmentation strategy decision module: Calls the real-time detection values ​​of drill pipe docking offset and drill pipe thread angle, calculates the ratio of point cloud density gradient of thread contact surface to the rotational angular velocity of robotic arm, and when the ratio exceeds the preset segmentation adjustment benchmark value, divides the point cloud data stream into 0.5mm×0.5mm×2mm three-dimensional segmentation units and generates dynamic segmentation threshold.

[0055] When calculating the ratio of density gradient to angular velocity, the change in the number of point clouds in every 0.1 mm interval along the axial direction of the threaded contact surface is taken. In the example, the detected gradient value is 15 points / mm, the robotic arm's rotational angular velocity is 0.8 rad / s, and the ratio is 18.75. When it exceeds the baseline value of 15, the point cloud is divided into three-dimensional segmented units. The octree spatial partitioning method is used for partitioning. In the example, the 73 mm diameter thread is divided into 1460 0.5 × 0.5 × 2 mm units, each containing 8-12 point cloud data. When generating the dynamic threshold, the maximum density difference within the unit is counted. When it exceeds 7 points / mm... 3 The time stamp is marked as a high-priority fragment;

[0056] Command dynamic generation module: Based on the dynamic segmentation threshold, the number of point cloud segments in the drill pipe thread area is linearly interpolated with the joint temperature data of the robotic arm. When the interpolation result exceeds the preset command priority threshold, the coordinate difference between the maximum reflection intensity value and the minimum axial pressure in the segmentation unit is extracted to generate torque compensation command parameters.

[0057] When performing linear interpolation, the relationship between the number of segments N and the joint temperature T is taken as follows: Priority coefficient = 0.6N + 0.4(T - 40). In the example, when N = 1200 and T = 55℃, we get 0.6 × 1200 + 0.4 × 15 = 720 + 6 = 726. When the threshold of 700 is exceeded, the maximum and minimum pressure coordinates of the reflection intensity within the segment unit are extracted. In the example, the reflection intensity of a certain unit is 258 cd / m.2 Corresponding coordinates (12.5, 7.2, 30.8), minimum pressure point coordinates (12.3, 7.1, 30.6), difference ΔX = 0.2mm, ΔY = 0.1mm, ΔZ = 0.2mm, when generating torque compensation parameters, the formula (ΔX × 10 + ΔY × 5 + ΔZ × 8) is adopted, and 2 + 0.5 + 1.6 = 4.1N·m is obtained;

[0058] Rod adding state verification module: based on the torque compensation instruction parameter, comparing the point cloud density gradient of the threaded engagement area with the reference gradient curve of the standard joint, calculating the gradient similarity in X / Y / Z three-axis directions, when the axial similarity is lower than the preset verification threshold, superimposing the angular displacement deviation of the drill rod rotary encoder, generating the rod adding completion degree result.

[0059] When calculating the gradient similarity, the point cloud density in the range of ±5mm in the X-axis is taken every 0.2mm interval, and the Pearson correlation coefficient is calculated with the standard curve. In the example, the measured X-axis similarity is 0.85, the Y-axis similarity is 0.78, and the Z-axis similarity is 0.92. When the axial (Z-direction) similarity is lower than the threshold of 0.9, the encoder angular displacement deviation is taken. In the example, a 0.12° deviation is detected. Using the formula (1-similarity) × 100 + deviation amount × 50, (1-0.92) × 100 + 0.12 × 50 = 8 + 6 = 14 is obtained. When the value exceeds 10, the rod adding completion degree is determined as B level.

[0060] The point cloud reconstruction feature matrix specifically refers to the axial sampling interval parameter, the thread profile depth gradient value, the guide groove space coordinates, and the thread lead angle density dispersion. The drill rod butt joint offset includes the engagement area density change rate, the hydraulic propulsion pressure value, and the deflection angle detection value. The dynamic slicing threshold specifically refers to the density gradient parameter, the angular velocity ratio coefficient, and the three-dimensional slicing unit parameter. The torque compensation instruction parameter includes the slicing number threshold, the joint temperature interpolation amount, and the reflection intensity range value. The rod adding completion degree coefficient specifically refers to the three-axis curve similarity and the angular displacement deviation value.

[0061] When constructing the point cloud reconstruction feature matrix, the axial sampling interval parameter is set to 1 / 40 of the standard pitch of the drill pipe. In the example, the pitch of the APINC38 drill pipe is 7.2 mm, corresponding to an interval of 0.18 mm. The thread profile depth gradient value is calculated by accumulating the absolute value of the height difference between adjacent sampling points. When the scanning thread height sequence is [2.1, 1.9, 2.3] mm, the gradient value is |2.1-1.9|+|1.9-2.3|=0.2+0.4=0.6. The guiding groove space coordinate registration selects three reference points to implement least squares matching. In the example, when the reference point coordinate deviation Δx=0.05 mm, Δy=0.03 mm is measured, translation compensation is performed. The thread rise angle density dispersion is calculated by the coefficient of variation of the number of point clouds per degree interval. When the 25° rise angle interval contains 82 points and the 26° interval contains 78 points, the dispersion coefficient is (82-80) / 80=0.025. Finally, when a 32×32 matrix is generated, the axial interval parameter is converted to the matrix row index, and the thread profile gradient value is used as the column element. In the example, the 5th row and 3rd column store the 0.6 gradient value, and the 7th row and 10th column record the 0.025 dispersion of the 25° rise angle interval.

[0062] When calculating the drill pipe butt joint offset, the density change rate in the meshing area is the number of point clouds in the adjacent 2-second window. In the example, the number of point clouds is 120 / mm 3 from the 10th second to the 12th second 3 , the change rate is (135-120) / 2=7.5 points / (mm 3 ·s), the hydraulic propulsion pressure value is the AD conversion value of the pressure sensor, and the range 0-40 MPa corresponds to 0-4095 digital quantities. In the example, the digital quantity 2048 corresponds to 20 MPa. The deflection angle detection value is calculated by measuring the radial displacement with a laser displacement sensor. When a displacement of 0.35 mm corresponds to an angle of arctan(0.35 / 200)=0.1°, the offset synthesis formula is (density change rate×pressure value / 10)+(angle×100). In the example, (7.5×

[0063] 20 / 10)+(0.1×100)=15+10=25. When the value exceeds the threshold value of 20, a warning is triggered.

[0064] In the dynamic segmentation threshold determination process, the density gradient parameter calculates the point cloud number change of each 0.5mm interval in the axial direction, and in the example, there are 85 points in the 0-0.5mm interval, 92 points in the 0.5-1.0mm interval, and the gradient is (92-85) / 0.5=14 / mm. The angular velocity ratio coefficient is the quotient of the gradient value and the mechanical arm angular velocity. When the angular velocity is 0.8 rad / s, the ratio is 14 / 0.8=17.5. When the stereoscopic segmentation unit parameter is set to 0.5x0.5x2mm, the number of circumferential segments is calculated according to the drill rod diameter of 73mm, πx73 / 0.5≈458. When the ratio exceeds the reference value of 15, the 2mm length in the axial direction is divided into 4 0.5mm units. In the example, a certain section of thread is divided into 458x4=1832 units. When the number of point clouds in a unit exceeds 10, it is marked as an effective segment.

[0065] When generating the torque compensation instruction parameter, the segmentation number threshold is set to 30% of the total number of effective segments. In the example, the threshold is 550 corresponding to 1832 segments. The joint temperature interpolation amount is the difference between the current temperature and the reference temperature of 40℃. When the temperature is 55℃, the interpolation amount is 15℃. The intensity range value is calculated as the difference between the maximum and minimum reflection intensity in a segmentation unit. In the example, the intensity values in a certain unit are [220, 235, 210] cd / m 2 , and the range is 235-210=25 cd / m 2 . The compensation parameter calculation formula is (segmentation number exceeds threshold amount x 0.1) + (temperature interpolation amount x 0.05) + (range x 0.2). In the example, (600-550)x0.1+15x0.05+25x0.2=5+0.75+5=10.75 N·m.

[0066] When calculating the rod addition completion coefficient, the three-axis curve similarity uses the dynamic time warping algorithm to match the measured and standard curves. In the example, the X-axis DTW distance of 3.2mm corresponds to a similarity of 0.88, the Y-axis distance of 4.1mm corresponds to a similarity of 0.82, and the Z-axis distance of 2.8mm corresponds to a similarity of 0.91. The angular displacement deviation value is the absolute difference between the encoder reading and the theoretical value. When the theoretical angular displacement is 120° and the measured value is 120.15°, the deviation is 0.15°. The completion coefficient formula is (1-average similarity) x 100+deviation amount x 200. In the example, (1-(0.88+0.82+0.91) / 3) x 100+0.15 x 200=(1-0.87) x 100+30=13+30=43. When the coefficient exceeds 40, it is determined to be a C-level completion.

[0067] Please refer to Figure 3 The visual feature acquisition module includes: data acquisition and acquisition sub-modules, feature extraction sub-modules, and density calculation sub-modules.

[0068] Data acquisition and sampling module: obtain the 3D point cloud data of the thread profile through the drill pipe joint profile scanner, set the axial sampling interval according to the standard pitch, and generate the axial sampling point set;

[0069] Data acquisition and sampling module: when setting the axial sampling interval, take 1 / 40 of the standard pitch of the drill pipe as the reference, in the example, the pitch of the APINC50 drill pipe is 7.24 mm, corresponding to an interval of 0.181 mm, the scanner collects the thread profile at a speed of 2000 points per second, when scanning a Φ88.9 mm drill pipe, 72 circumferential points are collected every 1° rotation, and the outlier is removed when generating the axial point set, in the example, a height sequence of [2.15, 2.09, 2.18] mm is collected at a certain axial position, after filtering by the 3σ criterion, the data within the range of 2.09-2.18 mm is retained, generating a point set containing 150 valid sampling points, the sampling interval error is controlled within ±0.005 mm;

[0070] Feature extraction submodule: based on the axial sampling point set, the thread profile depth gradient value is extracted, the coordinates of the guide groove positioning sensor are fused for spatial registration, and the spatial registration parameters are established

[0071] Feature extraction submodule: when calculating the thread profile depth gradient value, take the square sum of the height difference of the adjacent three sampling points, in the example, the gradient value of the height sequence [2.12, 2.08, 2.15] mm is (2.12-2.08) 2

[0072] +(2.08-2.15) 2 =0.0016+0.0049=0.0065, the spatial registration selects three positioning marker points in the guide groove, in the example, the marker point coordinates are (10.2, 5.3, 30.1), (10.5, 5.1, 30.3),

[0073] (10.3, 5.2, 30.2) mm, the centroid coordinates (10.33, 5.2, 30.2) mm are calculated as the registration reference, and the point cloud data is translated by Δx=+0.15 mm, Δy=-0.05 mm for coordinate alignment;

[0074] Density calculation submodule: call the spatial registration parameters, and use the formula:

[0075]

[0076] to obtain the discrete quantization value, and generate the point cloud reconstruction feature matrix combined with the thread lead angle parameter; wherein, ρ i represents the point cloud density of the i-th sampling point, μ represents the average density of the axial sampling point set, θ i is the measured value of the thread lead angle, and σ is the standard deviation of the guide groove spatial coordinates, and n is the number of valid sampling points.

[0077] Density calculation sub-module, when performing formula operation, take n = 150 effective points, calculate average density μ = total point number / scanning volume, in the example, the scanning area is 5mm long and the cross-sectional area is 12.5mm 2 The volume is 62.5mm 3 μ = 150 / 62.5 = 2.4 / mm 3 The density of each point ρ_i is calculated as the number of points within 1mm 3 centered on the point, in the example, there are 3 points around a certain point, then ρ_i = 3, the angle of rise θ_i is calculated by the three-point method, in the example, the coordinate difference of adjacent three points is obtained θ = 24.8°, the standard deviation σ of the guide groove coordinates is calculated as the dispersion value of the three axial coordinates, in the example, the standard deviation of the x coordinate is 0.12mm, the y axis is 0.08mm, and the z axis is 0.05mm, take σ = √(0.12 2 + 0.08 2 + 0.05 2 ) = 0.16mm, the final dispersion D = (Σ |3-2.4| 2 ·cos24.8°) / (150 × 0.16) = (0.6 2 × 0.91 + 0.4 2 × 0.91 +...) / 24 = (0.3276 + 0.1456 +...) / 24 ≈ 0.85, when D > 0.8, generate a 32 × 32 feature matrix, the rows correspond to the axial positions with an interval of 0.18mm, and the columns store the gradient values and the dispersion.

[0078] The drill hole state analysis module further comprises a product calculation sub-module and an offset synthesis sub-module.

[0079] The product calculation sub-module calls the point cloud reconstruction feature matrix, calculates the product of the point cloud density change rate of the thread engagement area and the reading of the hydraulic propulsion pressure sensor, and generates a dynamic coupling product value;

[0080] The product calculation sub-module calculates the difference value of the number of point clouds in three consecutive sampling periods (30ms) when calculating the density change rate, in the example, there are 120 / mm 3 in the 5th second, 125 in the 5.01st second, and 130 in the 5.02nd second, the change rate is (130-120) / 0.02 = 500 / (mm 3 · s), the hydraulic pressure value is read from the sensor 4-20mA signal and converted to 0-40MPa range, in the example, 12mA corresponds to 16MPa, the absolute value of the change rate and the pressure value are used in the product operation, 500 × 16 = 8000MPa· / (mm 3·s), the preset pressure threshold is set according to the drill pipe specification, the threshold value of the APINC46 specification is 7500, the offset calculation is triggered when the product value exceeds the limit, and the offset synthesis is activated when 8000>7500 in the example;

[0081] The offset synthesis submodule: monitor the dynamic coupling product value compared with the preset pressure threshold, and superimpose the drill pipe deflection angle detector data when the threshold value is exceeded, using the formula:

[0082]

[0083] The space offset is obtained by operation, and the drill pipe butt joint offset is established; wherein P represents the dynamic coupling product value, a represents the deflection angle detection value, β is the density reference value of the thread engagement area, γ is the current density change rate, δ corresponds to the guide groove coordinate offset, and η is the hydraulic system response coefficient;

[0084] The offset synthesis submodule, when performing formula operation, the dynamic coupling product value P=8000, the deflection angle a is obtained by laser range finder 0.25° deviation, the density reference value β takes the standard working condition value 120 / mm 3 , the current density change rate γ=500 / mm 3 ·s), the guide groove coordinate offset δ calculates the root mean square of the coordinates of three positioning points, in the example, Δx=0.2mm, Δy=0.15mm, Δz=0.1mm, δ=√(0.2 2 +0.15 2 +0.1 2 )=0.27mm, the hydraulic response coefficient η is obtained according to the pressure value table, η=2.5 corresponding to 16MPa, and the formula Δ=√[(8000×0.25 2 ) / |120-500|+0.27 3 / 2.5] =√[(8000×0.0625) / 380+0.019683 / 2.5] =√[1.3158+0.0079] =√1.3237≈1.15mm, when the offset exceeds the safety threshold of 1.0mm, the secondary warning parameter is generated.

[0085] The fragmentation strategy decision module further includes a ratio calculation submodule and a fragmentation generation submodule;

[0086] The ratio calculation submodule: calculate the point cloud density gradient and the mechanical arm angular velocity ratio based on the drill pipe butt joint offset, and generate a dynamic ratio coefficient;

[0087] The ratio calculation sub-module calculates the difference in the number of point clouds in the axial adjacent 0.5 mm interval when calculating the point cloud density gradient. In the example, there are 85 points in the 10-10.5 mm interval and 92 points in the 10.5-11 mm interval, and the gradient value is (92-85) / 0.5=14 points / mm. The angular velocity of the mechanical arm is converted through the encoder pulse signal. Each pulse corresponds to 0.001 rad. In the example, 800 pulses per second correspond to 0.8 rad / s. The ratio coefficient is calculated by dividing the gradient value by the absolute value of the angular velocity, 14 / 0.8=17.5. The preset segmentation reference value is set according to the drill rod specification. The reference value for APINC50 specification is 15. When the ratio 17.5>15 is detected, the segmentation operation is triggered. In the example, the segmentation density increases by 5% for every 1 unit of the reference value.

[0088] The segmentation generation sub-module monitors the dynamic ratio coefficient and compares it with the preset segmentation reference value. When the reference value is exceeded, the solid segmentation unit is divided, and the dynamic segmentation threshold is established.

[0089] The segmentation generation sub-module divides the solid segmentation unit. The axial direction is sliced at 0.5 mm intervals, and the circumference is evenly divided according to the circumference of the drill rod. In the example, the circumference of the Φ73 mm drill rod is 229.3 mm. 458 circumferential units are generated every 0.5 mm interval. The vertical direction is divided into 2 mm layers, generating 0.5×0.5×2 mm units. The dynamic segmentation threshold is set as the ratio of the point cloud density range in the unit to the reference density. In the example, the point cloud number in a unit is [8, 12, 9], the range is 12-8=4, and the reference density is 10 points / mm 3 . When the threshold value>0.3, it is marked as high priority segmentation. Finally, the unit center coordinates (x, y, z) and the threshold value are stored. In the example, the coordinates (25.3, 7.8, 15.2) mm correspond to the threshold value 0.4.

[0090] The instruction dynamic generation module includes an interpolation operation sub-module and a range extraction sub-module.

[0091] The interpolation operation sub-module performs a cubic spline interpolation operation on the point cloud segmentation number and the joint temperature to generate a segmentation temperature interpolation value.

[0092] The interpolation operator module selects four reference points of the number of pieces N and joint temperature T when constructing a cubic spline interpolation curve: (N=800, T=40℃), (1000, 45℃), (1200, 50℃), and (1400, 55℃). When calculating the spline coefficient under the condition of N=1200 pieces and T=55℃, the temperature difference ΔT=5℃ and the piece difference ΔN=200 of the adjacent interval [N1=1000, N2=1200] are taken to construct the interpolation function S(N)=a(N-N1)^3+b(N-N1)^2+c(N-N1)+d. Through the boundary conditions S(1000)=45, S(1200)=50, the first-order derivative continuity, and the second-order derivative endpoint being zero, a=0.0000125, b=0.00125, c=0.375, and d=45 are obtained. When the real-time piece number is 1150, the interpolation amount S(1150)=0.0000125*(150)^3+0.00125*(150)^2+0.375*150+45=42.1875+28.125+56.25+45=171.5625. The preset piece threshold is set to 700 corresponding to the maximum interpolation amount 1400 of the reference point. When the interpolation amount exceeds 700, the range extraction is activated.

[0093] The range extraction submodule compares the piece temperature interpolation amount with the preset piece threshold, and extracts the reflection intensity range when the threshold is exceeded, to establish the torque compensation instruction parameter.

[0094] The range extraction submodule scans all point cloud reflection intensity values in the piece unit marked as high priority when extracting the reflection intensity range. In the example, the measured values of a certain unit are [215, 230, 208, 225] cd / m 2 The maximum value 230 and the minimum value 208 are taken, the range Δ=230-208=22 cd / m 2 The torque compensation parameter calculation adopts (range value×temperature interpolation amount / 1000)+(piece threshold exceeded amount×0.1). In the example, when the interpolation amount 726 exceeds the threshold 700, the piece threshold exceeded amount=726-700=26, and the compensation parameter=22×726 / 1000+26×

[0095] 0.1=15.972+2.6=18.572 N·m. When the parameter is written into the control instruction, it is rounded to 18.5 N·m with a precision of 0.5 N·m.

[0096] The rod state verification module includes a similarity calculation submodule and a composite verification submodule.

[0097] The similarity calculation submodule obtains the torque compensation instruction parameter, compares the thread gradient with the standard curve, calculates the matching degree of the three-axis directions, and generates the three-axis similarity value.

[0098] Similarity calculation sub-module, when calculating X-axis similarity, taking the measured value and standard value of thread gradient curve every 0.2mm interval, in the example, measured gradient 0.65 in 5-5.2mm interval, standard value 0.70, calculating cosine similarity: (0.65x0.70) / (sqrt(0.65 2 +0.70 2 ))=0.455 / sqrt

[0099] (0.4225+0.49)=0.455 / 0.955≈0.476, Y-axis similarity calculation taking 8 sampling points in 120° interval, measured value [0.68, 0.72, 0.65, 0.70, 0.69, 0.71, 0.67, 0.73], standard value

[0100] [0.70, 0.70, 0.70, 0.70, 0.70, 0.70, 0.70, 0.70], covariance calculation 0.00085, variance 0.0004, similarity 0.00085 / sqrt(0.0004x0.0004)=0.00085 / 0.0004=2.125 (more than 1, take 1), Z-axis similarity uses dynamic time warping algorithm, measured curve and standard curve minimum path distance 2.1mm, similarity 1-2.1 / 5=0.58, final three-axis similarity value S_x=0.476, S_y=1, S_z=0.58.

[0101] Composite verification sub-module: calling three-axis similarity value, superimposing rotary encoder angular displacement deviation, using formula:

[0102]

[0103] Operation to obtain composite verification coefficient, establish rod adding completion coefficient; wherein, S i represents the i-axis similarity value, φ i is the corresponding phase angle, Δθ i represents the angular displacement deviation of each axis;

[0104] Composite verification sub-module, when calculating composite coefficient, taking X-axis phase angle φ_x=30° corresponding to cos30°

[0105] =0.866, Y-axis φ_y=45° corresponding to 0.707, Z-axis φ_z=60° corresponding to 0.5, angular displacement deviation Δθ_x=0.15°, Δθ_y=0.20°, Δθ_z=0.10°, substituting formula K=(0.476x0.866+1x0.707+0.58x0.5) / sqrt(0.15 2 +0.20 2 +0.10 2) = (0.412 + 0.707 + 0.29) / sqrt

[0106] (0.0225 + 0.04 + 0.01) = 1.409 / sqrt 0.0725 = 1.409 / 0.269 ≈ 5.24, the preset completion degree level is divided: K> 5 is A level, 3-5 is B level, <3 is C level, and in the example, 5.24 is determined as A level completion, the K value is stored when the coefficient is generated and is associated with the time stamp.

[0107] The automatic rod adding recognition control method of the drilling robot based on 3D vision includes the following steps:

[0108] S1: Obtain the spatial position image of the drill rod tail end and the drilling axis through the 3D vision sensor, extract the drill rod tail end center point coordinates and the drilling axis direction vector, calculate the transverse offset distance and longitudinal inclination deviation of the drill rod tail end and the target drilling axis, combine the drill rod length parameter and the pushing speed parameter, and generate a drill rod pose parameter set containing spatial coordinates, offset, and inclination value;

[0109] S2: Call the transverse offset distance and longitudinal inclination deviation in the drill rod pose parameter set, combine the drill rig robot joint angle parameter, establish the drill rod tail end motion trajectory coordinate system, calculate the joint angle compensation amount through inverse kinematics, and generate offset compensation parameters containing X / Y / Z three-axis compensation amount;

[0110] S3: Based on the pushing speed parameter and the drilling depth parameter in the drill rod pose parameter set, calculate the current drill rod remaining stroke, and perform difference operation on the remaining stroke and the preset single rod length. When the remaining stroke is less than the product of the single rod length and the safety factor, a binary rod adding trigger signal is generated;

[0111] S4: Activate the robot motion trajectory planning according to the rod adding trigger signal, call the three-axis compensation amount in the offset compensation parameter, combine the drill rod library positioning coordinates and the robot end gripper parameter, and generate a control instruction sequence containing the grabbing path, the compensation motion trajectory, and the docking positioning accuracy;

[0112] S5: Monitor the robot joint torque parameter and the visual positioning feedback parameter in the control instruction sequence execution process. When the gripper pressure value reaches the preset docking threshold value and the pose offset amount is less than the positioning accuracy threshold value, update the drilling depth parameter and the drill rod inventory quantity parameter, and generate a state update parameter containing the new rod length and the updated drilling depth.

[0113] The above merely describes the preferred embodiments of the present application, and is not intended to limit the present application in other forms. Any skilled person in the art can modify or change the disclosed technical content into equivalent embodiments with equivalent changes, and apply them to other fields. However, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, without departing from the technical solution content of the present application, still falls within the protection scope of the present application.

Claims

1. An automatic rod-addition recognition system for drilling robots based on 3D vision, characterized in that: The system includes: Visual feature acquisition module: Acquires 3D point cloud data of thread through drill pipe joint contour scanner, sets axial sampling interval according to standard pitch, extracts thread profile depth gradient value and fuses guide groove positioning sensor coordinates for spatial registration, calculates thread helix angle point cloud density dispersion to generate point cloud reconstruction feature matrix; Drilling status analysis module: calls the point cloud reconstruction feature matrix, calculates the product of the point cloud density change rate in the thread meshing area and the reading of the hydraulic propulsion pressure sensor, and generates the drill pipe docking offset by superimposing the drill pipe deflection angle detector data when the threshold is exceeded; Segmentation strategy decision module: Calculates the ratio of point cloud density gradient to robotic arm angular velocity based on the drill pipe docking offset; when the value exceeds the reference value, it divides the three-dimensional segmentation unit to generate a dynamic segmentation threshold. The instruction dynamic generation module calls the dynamic segmentation threshold to perform interpolation calculations on the number of point cloud segments and joint temperature. When the threshold is exceeded, the reflection intensity range is extracted to generate torque compensation instruction parameters. Rod addition status verification module: Obtain the torque compensation command parameters, compare the thread gradient with the standard curve to calculate the triaxial similarity, and superimpose the rotary encoder angular displacement deviation to generate the rod addition completion coefficient.

2. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that: The point cloud reconstruction feature matrix specifically includes axial sampling interval parameters, thread profile depth gradient values, guide groove spatial coordinates, and thread helix angle density dispersion. The drill pipe docking offset includes the density change rate of the meshing area, hydraulic propulsion pressure value, and deflection angle detection value. The dynamic segmentation threshold specifically refers to density gradient parameters, angular velocity ratio coefficients, and three-dimensional segmentation unit parameters. The torque compensation command parameters include segmentation number thresholds, joint temperature interpolation amount, and reflection intensity range value. The rod addition completion coefficient specifically includes triaxial curve similarity and angular displacement deviation value.

3. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that, The visual feature acquisition module includes: a data acquisition and sampling submodule, a feature extraction submodule, and a density calculation submodule; Data acquisition and sampling submodule: Acquires 3D point cloud data of the thread through a drill pipe joint contour scanner, sets the axial sampling interval according to the standard pitch, and generates an axial sampling point set; Feature extraction submodule: Extracts thread profile depth gradient values ​​based on the axial sampling point set, fuses the coordinates of the guide groove positioning sensor for spatial registration, and establishes spatial registration parameters. Density calculation submodule: calls spatial registration parameters, using the formula: The discrete quantized values ​​are obtained through computation, and combined with the thread helix angle parameter to generate a point cloud reconstruction feature matrix; where ρ i Let θ represent the point cloud density of the i-th sampling point, μ represent the average density of the axial sampling point set, and θ represent the density of the i-th sampling point. i σ is the measured value of the thread helix angle, σ is the standard deviation of the guide groove spatial coordinates, and n is the number of effective sampling points.

4. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that, The borehole state analysis module also includes: a product calculation submodule and an offset synthesis submodule; Product calculation submodule: calls the point cloud reconstruction feature matrix, calculates the product of the point cloud density change rate in the thread meshing area and the reading of the hydraulic propulsion pressure sensor, and generates a dynamic coupling product value; Offset Synthesis Submodule: Monitors the dynamic coupling product value and compares it with a preset pressure threshold. When the threshold is exceeded, it overlays the drill pipe deflection angle detector data, using the following formula: The spatial offset is calculated and the drill pipe docking offset is established; where P represents the dynamic coupling product value, α represents the deflection angle detection value, β is the density reference value of the thread meshing area, γ is the current density change rate, δ corresponds to the guide groove coordinate offset, and η is the hydraulic system response coefficient.

5. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that, The sharding strategy decision module also includes a ratio calculation submodule and a sharding generation submodule; Ratio Calculation Submodule: Calculates the ratio of point cloud density gradient to robotic arm angular velocity based on drill pipe docking offset, and generates dynamic ratio coefficient; The segmentation generation submodule monitors the dynamic ratio coefficient and compares it with the preset segmentation benchmark value. When the benchmark value is exceeded, it divides the three-dimensional segmentation unit and establishes a dynamic segmentation threshold.

6. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that, The instruction dynamic generation module includes an interpolation operation submodule and a range extraction submodule; Interpolation submodule: Calls the dynamic segmentation threshold to perform cubic spline interpolation on the number of point cloud segments and joint temperature to generate the segment temperature interpolation amount; Range extraction submodule: Monitors the interpolation amount of the segmented temperature and compares it with the preset segmented threshold. When the threshold is exceeded, the range of reflection intensity is extracted, and torque compensation command parameters are established.

7. The automatic rod-addition recognition system for drilling robots based on 3D vision according to claim 1, characterized in that, The pole-adding state verification module includes a similarity calculation submodule and a composite verification submodule; Similarity calculation submodule: Obtain torque compensation command parameters, compare thread gradient with standard curve, calculate triaxial direction matching degree, and generate triaxial similarity value; Composite verification submodule: Calls the three-axis similarity values, superimposes the rotary encoder angular displacement deviation, and uses the following formula: Calculate and obtain the composite verification coefficients, and establish the rod addition completion coefficients; where S i φ represents the similarity value along the i-th axis. i For the corresponding axis phase angle, Δθ i This indicates the angular displacement deviation of each axis.

8. A method for automatic rod-addition recognition and control of a drilling robot based on 3D vision, characterized in that, The automatic rod-addition recognition system for drilling robots based on 3D vision, as described in any one of claims 1-7, includes the following steps: S1: Obtain the spatial position image of the drill rod end and the borehole axis through a 3D vision sensor, extract the coordinates of the center point of the drill rod end and the direction vector of the borehole axis, calculate the lateral offset distance and longitudinal tilt angle deviation between the drill rod end and the target borehole axis, and combine the drill rod length parameters and the advance speed parameters to generate a drill rod pose parameter set containing spatial coordinates, offset, and tilt angle values; S2: Call the lateral offset distance and longitudinal tilt angle deviation from the drill rod posture parameter set, combine them with the joint angle parameters of the drilling rig robotic arm, establish the coordinate system of the drill rod end motion trajectory, calculate the compensation amount of each joint angle through inverse kinematics, and generate offset compensation parameters containing X / Y / Z three-axis compensation amounts; S3: Based on the propulsion speed parameters and drilling depth parameters in the drill rod posture parameter set, calculate the remaining stroke of the current drill rod, calculate the difference between the remaining stroke and the preset single rod length, and generate a binary rod addition trigger signal when the remaining stroke is less than the product of the single rod length and the safety factor. S4: Activate the robotic arm motion trajectory planning based on the rod addition trigger signal, call the three-axis compensation amount in the offset compensation parameters, and combine the drill rod library positioning coordinates and the robotic arm end gripper parameters to generate a control command sequence that includes the gripping path, compensation motion trajectory, and docking positioning accuracy. S5: Monitor the joint torque parameters and visual positioning feedback parameters of the robotic arm during the execution of the control command sequence. When the gripper pressure value reaches the preset docking threshold and the position offset is less than the positioning accuracy threshold, update the drilling depth parameters and drill rod inventory quantity parameters, and generate status update parameters including the new rod length and the updated drilling depth.

Images