A well repair robot control algorithm fusion method

Abstract

This invention discloses a method for fusing control algorithms for well workover robots, relating to the field of well workover in oil and gas fields. The method includes the following steps: acquiring video streams via a high-resolution camera; performing target detection and multi-dimensional feature extraction using an improved YOLOv7 and Res-Net-50 network; generating an interactive 3D model by combining classifier results; fusing point cloud density and image texture features to train a lightweight semantic segmentation model to classify the target object; selecting the optimal grasping point and strategy based on the category and features; constructing a working environment map using the A* algorithm and multi-sensor data; planning a safe path; achieving dynamic trajectory adjustment and pose control through coordinate system alignment and real-time pose feedback; and finally, controlling the manipulator's finger joints to grasp the target object according to the grasping strategy, calculating the comprehensive index impact score, and determining whether re-grabbing is necessary. This achieves precise grasping control of the robot, improving system stability and reliability.

Description

Technical Field

[0001] This invention belongs to the field of oil and gas field well workover, specifically involving a method for fusing control algorithms for well workover robots. Background Technology

[0002] In the field of well workover in oil and gas fields, the traditional approach is to use a workover rig plus wellhead personnel. Wellhead personnel are required to push or deliver pipes and clamps, operate clamps to open and close, align pipes, operate hydraulic wrenches to engage or disengage, and operate slips. This usually requires 2-3 workers to work together, which is labor-intensive. In addition, the wellhead equipment is numerous and heavy, which can easily lead to safety accidents and personnel injuries.

[0003] In recent years, although automated wellhead equipment consisting of robotic arms, hydraulic tilting clamps, and automatic hydraulic shears has emerged to replace wellhead workers, these devices suffer from drawbacks such as complexity and dispersion, high cost, frequent malfunctions, and low efficiency in coordination. They are unable to accurately detect target objects and extract multi-dimensional features, nor can they efficiently integrate multimodal data to generate interactive 3D models. They also lack the ability to combine multiple algorithms to generate safe paths and dynamically avoid obstacles, and lack a comprehensive scoring system based on quantified indicators to determine whether the target object needs to be re-grabbed.

[0004] Therefore, a method for integrating control algorithms for well repair robots has emerged. Summary of the Invention

[0005] This invention aims to solve at least one of the technical problems existing in the prior art; to this end, this invention proposes a well workover robot control algorithm fusion method to solve the following technical problem:

[0006] It is difficult to accurately detect target objects and extract multi-dimensional features, and it is also difficult to efficiently integrate multimodal data and generate interactive 3D models. It lacks the ability to combine multiple algorithms to generate safe paths and dynamically avoid obstacles, and it lacks a comprehensive score based on the quantification of multiple indicators to determine whether the target object needs to be re-captured.

[0007] To address the above problems, this invention provides a method for fusing control algorithms for well workover robots, comprising the following steps:

[0008] S1: Video streams are acquired through high-resolution cameras and preprocessed. The improved YOLOv7 is used to detect the target region, and a multi-branch structure is designed based on the Res-Net-50 network to extract multi-dimensional features of the target object.

[0009] S2: The YOLOv7 object detection results, Res-Net-50 image features, and classifier recognition results are fused to generate an interactive 3D model;

[0010] S3: Design a dynamic weight fusion network to allocate modal weights based on point cloud density and image texture confidence, generate joint feature representations, train a lightweight semantic segmentation model based on the joint features, and distinguish different target object categories;

[0011] S4: Select the optimal grasping point and grasping strategy based on the target object category and characteristics. Based on the A* algorithm and combined with obstacle expansion processing, plan a safe path, fuse multi-sensor data to generate a working environment map, and mark the obstacle type.

[0012] S5: Define the wellhead center coordinate system and robot coordinate system, calculate pose and joint angles, acquire real-time pose through IMU and camera, calculate error and dynamically adjust trajectory, and combine sensor feedback for pose control and trajectory correction.

[0013] S6: Based on the grasping plan, control each finger joint of the robotic arm to grasp the target object. After normalizing the contact force, pose change, visual detection results and stability index, the weighted sum of the comprehensive indexes affects the score, and it is determined whether the grasping needs to be repeated.

[0014] Preferably, step S1 includes the following steps:

[0015] A high-resolution camera is used to capture real-time video streams of the workspace, and the captured images are preprocessed, including noise reduction, color correction, and white balance.

[0016] YOLOv7 was chosen as the basic object detection model. Deformable convolution was introduced into its E-ELAN backbone network, and a coordinate attention mechanism was added to the detection head. Gaussian heatmap regression was used to replace traditional bounding box regression, and feature maps of different scales were dynamically fused through BiFPN weighted bidirectional feature pyramid.

[0017] Res-Net-50 was chosen as the base network, and CBAM was embedded in the residual block to design a multi-branch network. The shape and topology of the object, RGB-HSV color space features and LBP texture features were extracted respectively. The features extracted by different branches were fused to form a multi-dimensional semantic information representation.

[0018] Preferably, step S2 includes the following steps:

[0019] The target region detected by YOLOv7 is input into the Res-Net-50 network with enhanced attention mechanism to extract multi-dimensional features of the target object;

[0020] Based on the extracted multi-dimensional features, a classifier is used to classify and identify the target objects;

[0021] The sub-pixel level bounding boxes output by YOLOv7 are further refined, and the multi-dimensional semantic information extracted by ResNet-50 is fused with the object detection results to form a complete object description, including geometric attributes: bounding box coordinates and shape contour; appearance attributes: color histogram and LBP texture features; and semantic attributes: category label and multi-dimensional semantic vector.

[0022] The integrated target description is output as an interactive 3D model.

[0023] Preferably, step S3 includes the following steps:

[0024] In the workspace, the target object is scanned using a laser rangefinder to obtain raw point cloud data, which is then preprocessed.

[0025] The point cloud is aligned with the high-resolution image through a time synchronization mechanism, and the coordinate system is unified based on the IMU attitude data;

[0026] Features are extracted from point cloud data, including normal vectors, curvature, and local surface features, and adaptive downsampling is performed on high-density point clouds.

[0027] Combine Res-Net-50 network to extract multi-dimensional features of target objects;

[0028] Design a cross-membrane feature projection module: map the multi-dimensional features extracted by the Res-Net-50 network to the point cloud coordinate system, and perform spatial alignment through feature matching;

[0029] Design a dynamic weighted fusion network to dynamically allocate modal weights based on point cloud density and image texture confidence, and generate joint feature representations;

[0030] A lightweight semantic segmentation model is trained based on joint features. The model outputs a multi-class confidence score for each voxel, and a semantic label is assigned to each voxel based on the confidence score output by the model.

[0031] By fusing the spatial location features of voxels with image features through a neural network radiation field, the radiation intensity or color of voxels is generated.

[0032] Preferably, the preprocessing of the point cloud data includes the following steps:

[0033] The voxel grid of PCL is used to calculate the point density of each voxel, and high density thresholds and low density thresholds are set. The octree nodes are recursively subdivided for high density regions and adjacent voxels are merged for low density regions. A KD-tree is built inside the octree nodes for fast neighborhood search.

[0034] Huffman coding is performed on the semantic tags of voxels;

[0035] The filtering intensity is adjusted based on the voxel semantic labels, and bilateral filtering is used to preserve the sharpness of voxel boundaries, specifically:

[0036]

[0037] Where W(p) is the weight value of point p, p is the position vector of the current point, and p c Let I(p) and I(p) be the position vectors of the center point. c Points p and p' are respectively. c The strength value, σ d Let σ be the scale parameter of the spatial domain. r is the scale parameter of the intensity domain;

[0038] An axial attention module is introduced to calculate the position of the point cloud projected onto the image plane and align it with the image segmentation mask.

[0039] Preferably, the step of generating the radiation intensity or color of a voxel by fusing the spatial location features of the voxel with image features through a neural network radiation field includes the following steps:

[0040] The spatial location features of voxels, along with the color and texture features extracted by the ResNet-50 network, are input into the neural network radiation field to generate the radiation intensity or color of the voxels. Specifically:

[0041] c = σ(ω1·P_f + ω2·I_f)

[0042] Where c is the radiation intensity or color of the voxel, P_f is the spatial location feature, I_f is the image feature, including color and texture features, σ is the activation function, and ω1 and ω2 are the corresponding weights;

[0043] The material library is matched based on the voxel semantic tags, and the material texture is applied to the surface of the 3D model using UV mapping.

[0044] Preferably, step S4 includes the following steps:

[0045] Based on the 3D model and voxel semantic labels of the target object, the optimal grasping point is selected and a grasping strategy is formulated: for regular large smooth objects, a multi-point grasping strategy is selected and the grasping points are evenly distributed; for irregular small rough objects, a single-point grasping strategy is selected, with the geometric center or centroid as the grasping point.

[0046] The A* algorithm is used to plan the robot's path from its current position to the target grasping point.

[0047] The robot uses cameras and LiDAR to acquire information about its working environment, aligns point cloud data with high-resolution images to generate a working environment map, and combines semantic segmentation results to label the types of obstacles.

[0048] In the path planning process of the A* algorithm, obstacles are treated as impassable nodes, and the obstacle area is expanded using an expansion operation.

[0049] Preferably, step S5 includes the following steps:

[0050] Define the wellhead center coordinate system and the robot coordinate system, and align the origin of the robot coordinate system with the wellhead center;

[0051] The DH parameters are used to describe the robot's joint and link structure. The pose of the end effector is calculated based on the joint angles, and the joint angles are calculated based on the target pose.

[0052] The joint angle is obtained through a numerical iteration method, and the optimal solution is selected based on actual needs. The joint angle θ is... i i = 1, 2, ..., n, where n is the number of joints;

[0053] Discretize the joint angle sequence and assign it to the time axis. Use interpolation methods to generate smooth trajectories. Map the joint angle sequence to the time axis to generate trajectories with continuous velocity and acceleration.

[0054] The real-time pose of the end effector is acquired by an IMU sensor, the error between the real-time pose and the target pose is calculated, and the joint angle is adjusted according to the error. At the same time, an image of the grasping point is acquired by a camera, the real-time pose of the grasping point is estimated by a visual algorithm, and the trajectory is adjusted according to visual feedback.

[0055] Preferably, defining the wellhead center coordinate system and the robot coordinate system, and aligning the origin of the robot coordinate system with the wellhead center, includes the following steps:

[0056] Define the wellhead center coordinate system: the origin is located at the center of the wellhead, the Z-axis is perpendicular to the wellhead plane and points upwards, and the X-axis and Y-axis are along the horizontal direction;

[0057] Define the robot coordinate system: the origin is located at the robot base, the Z-axis is along the robot's main axis, and the X-axis and Y-axis point to the robot's front-back and left-right directions, respectively;

[0058] Based on the robot's current position and orientation, calculate the transformation matrix between the robot coordinate system and the wellhead center coordinate system, and transform the pose of the grasping point from the robot coordinate system to the wellhead center coordinate system, specifically:

[0059] P w =T r_w ·P r

[0060] Among them, P w Let T be the coordinate system at the wellhead center. r_w P is the coordinate system transformation matrix. rThis is the robot's coordinate system.

[0061] Preferably, step S6 includes the following steps:

[0062] Based on the grasping plan, control each joint of the robotic arm to grasp the target object;

[0063] Force sensors are installed at the finger joints of the robotic arm to measure contact forces in real time; IMU sensors are installed on the target object to measure its acceleration and angular velocity in real time, and displacement changes are calculated by integration; an image of the target object is captured by a camera, and pose changes are calculated by feature point matching; a high-resolution camera is installed above the robotic arm, and a deep learning algorithm is used to detect whether the target object is in the robotic arm. If the target object is in the robotic arm, the binarized output is 1, otherwise it is 0; stability indicators are calculated based on the contact force distribution of each finger joint and the center of gravity of the object.

[0064] The values ​​of each indicator are normalized, and each indicator is assigned a weight, with the total weight being 1. These weighted values ​​are then summed to obtain the comprehensive indicator impact score, specifically:

[0065]

[0066] Where E represents the comprehensive indicator impact score, and F... ′ ΔP ′ l ′ S ′ M′ represents the normalized contact force, pose change, visual detection result, and stability index, respectively, and ω represents the stability index. F ω p ω l ω and ω represent the corresponding weights, respectively;

[0067] If E ≥ the threshold, the crawling is successful and no re-crawl is needed; if E < the threshold, the crawling fails, the crawling strategy is re-planned, and the crawling is attempted again.

[0068] The beneficial effects of this invention are:

[0069] This invention, by combining an improved YOLOv7 with a Res-Net-50 multi-branch structure, can effectively extract multi-dimensional features of target objects, improve detection accuracy in low-light and high-noise environments downhole, and generate an interactive 3D model by fusing YOLOv7 detection results, Res-Net-50 image features and classifier results. This solves the problem of inaccurate modeling of single-modal data in complex downhole scenes and supports subsequent path planning and grasping control.

[0070] This invention dynamically allocates modal weights based on point cloud density and image texture confidence to generate joint feature representations, trains a lightweight semantic segmentation model, and improves the accuracy of downhole tool classification.

[0071] This invention generates a working environment map by fusing multi-sensor data based on the A* algorithm, marks obstacle types, and plans collision-free paths, thereby improving the working efficiency of the well repair robot. At the same time, by defining the wellhead center coordinate system and the robot coordinate system, and combining the IMU and camera to acquire pose in real time, the error is calculated and the trajectory is dynamically adjusted to achieve high-precision positioning and control.

[0072] This invention generates a comprehensive score by weighting and summing normalized contact force, pose change, visual detection results, and stability indicators to determine whether re-grabbing is necessary, thus solving the problem of unstable grasping of downhole tools caused by irregular shape or heavy weight. Attached Figure Description

[0073] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0074] Figure 2 This is a schematic diagram of the interactive 3D modeling process of the present invention;

[0075] Figure 3 This is a schematic diagram of the multimodal perception weighting decision-making grasping and evaluation process of the present invention. Detailed Implementation

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

[0077] Please see Figure 1 As shown, this invention is a method for fusing control algorithms for well workover robots, comprising the following steps:

[0078] S1: Video streams are acquired through high-resolution cameras and preprocessed. The improved YOLOv7 is used to detect the target region, and a multi-branch structure is designed based on the Res-Net-50 network to extract multi-dimensional features of the target object.

[0079] S2: The YOLOv7 object detection results, Res-Net-50 image features, and classifier recognition results are fused to generate an interactive 3D model;

[0080] S3: Design a dynamic weight fusion network to allocate modal weights based on point cloud density and image texture confidence, generate joint feature representations, train a lightweight semantic segmentation model based on the joint features, and distinguish different target object categories;

[0081] S4: Select the optimal grasping point and grasping strategy based on the target object category and characteristics. Based on the A* algorithm and combined with obstacle expansion processing, plan a safe path, fuse multi-sensor data to generate a working environment map, and mark the obstacle type.

[0082] S5: Define the wellhead center coordinate system and robot coordinate system, calculate pose and joint angles, acquire real-time pose through IMU and camera, calculate error and dynamically adjust trajectory, and combine sensor feedback for pose control and trajectory correction.

[0083] S6: Based on the grasping plan, control each finger joint of the robotic arm to grasp the target object. After normalizing the contact force, pose change, visual detection results and stability index, the weighted sum of the comprehensive indexes affects the score, and it is determined whether the grasping needs to be repeated.

[0084] In one embodiment of the present invention, step S1 includes the following steps:

[0085] A high-resolution camera is used to capture real-time video streams of the workspace, and the captured images are preprocessed, including noise reduction, color correction, and white balance.

[0086] YOLOv7 was chosen as the basic object detection model. Deformable convolution was introduced into its E-ELAN backbone network, and a coordinate attention mechanism was added to the detection head. Gaussian heatmap regression was used to replace traditional bounding box regression, and feature maps of different scales were dynamically fused through BiFPN weighted bidirectional feature pyramid.

[0087] Res-Net-50 was chosen as the base network, and CBAM was embedded in the residual block to design a multi-branch network. The shape and topology of the object, RGB-HSV color space features and LBP texture features were extracted respectively. The features extracted by different branches were fused to form a multi-dimensional semantic information representation.

[0088] Specifically, Gaussian filtering or bilateral filtering is used to remove image noise, color deviation is corrected through the gray-world hypothesis or white balance algorithm, and adaptive white balance algorithm is used to adjust image color temperature; deformable convolution is inserted into the convolutional layer of E-ELAN in the YOLOv7-based object detection model; a coordinate attention mechanism is added to the detection head, and the CA module is used to weight the feature map; traditional bounding box regression is replaced with Gaussian heatmap regression to generate a Gaussian heatmap of the target center point and predict the center coordinates and size; a weighted bidirectional feature pyramid is used to dynamically fuse feature maps of different scales, and high-level and low-level features are weighted and fused to improve multi-scale object detection capability; Res-Net-50 is used as the backbone network to extract preliminary features, and the multi-branch design includes using edge detection or skeleton extraction algorithms to extract shape features, converting RGB images to HSV color space to extract color histogram features, and using the local binary mode algorithm to extract texture features; the features extracted from each branch, including shape, color, and texture, are spliced ​​or weighted and fused to form a multi-dimensional semantic representation, and channel and spatial attention modules are embedded in the residual block to improve feature selection capability.

[0089] In one embodiment of the present invention, step S2 includes the following steps:

[0090] The target region detected by YOLOv7 is input into the Res-Net-50 network with enhanced attention mechanism to extract multi-dimensional features of the target object;

[0091] Based on the extracted multi-dimensional features, a classifier is used to classify and identify the target objects;

[0092] The sub-pixel level bounding boxes output by YOLOv7 are further refined, and the multi-dimensional semantic information extracted by ResNet-50 is fused with the object detection results to form a complete object description, including geometric attributes: bounding box coordinates and shape contour; appearance attributes: color histogram and LBP texture features; and semantic attributes: category label and multi-dimensional semantic vector.

[0093] The integrated target description is output as an interactive 3D model.

[0094] Specifically, based on the bounding box coordinates output by YOLOv7, the target region is cropped from the original image and input into the attention-enhanced Res-Net-50 network. Multi-dimensional features are extracted from the output of Res-Net-50. Using the output features of Res-Net-50 as input, classification is performed through a fully connected layer. The multi-dimensional features are input into the classifier, which outputs the class label. The edge features or shape contours extracted by Res-Net-50 are used to further refine the bounding box of YOLOv7. The bounding box coordinates of YOLOv7 and the multi-dimensional features extracted by Res-Net-50 are fused to form a complete target description. A 3D model of the target object is generated based on its geometric properties.

[0095] In one embodiment of the present invention, step S3 includes the following steps:

[0096] In the workspace, the target object is scanned using a laser rangefinder to obtain raw point cloud data, which is then preprocessed.

[0097] The point cloud is aligned with the high-resolution image through a time synchronization mechanism, and the coordinate system is unified based on the IMU attitude data;

[0098] Features are extracted from point cloud data, including normal vectors, curvature, and local surface features, and adaptive downsampling is performed on high-density point clouds.

[0099] Combine Res-Net-50 network to extract multi-dimensional features of target objects;

[0100] Design a cross-membrane feature projection module: map the multi-dimensional features extracted by the Res-Net-50 network to the point cloud coordinate system, and perform spatial alignment through feature matching;

[0101] Design a dynamic weighted fusion network to dynamically allocate modal weights based on point cloud density and image texture confidence, and generate joint feature representations;

[0102] A lightweight semantic segmentation model is trained based on joint features. The model outputs a multi-class confidence score for each voxel, and a semantic label is assigned to each voxel based on the confidence score output by the model.

[0103] By fusing the spatial location features of voxels with image features through a neural network radiation field, the radiation intensity or color of voxels is generated.

[0104] Specifically, point cloud frames and image frames are aligned using timestamps, and the point cloud and image are transformed to the global coordinate system using the IMU's pose count. PCA or neighborhood fitting is used to calculate the normal vector of the plane, curvature is estimated through neighborhood point density, and FPFH features are extracted as local surface features. Res-Net-50 is used to extract multi-dimensional features of the image. A cross-modal feature projection module is designed: image features are projected to the point cloud coordinate system through bilinear interpolation or spatial transformation, and point cloud and image features are aligned through nearest neighbor search or feature similarity, such as cosine similarity. Confidence is calculated based on the local density of the point cloud and the feature response of Res-Net-50. Weights of the point cloud and image are dynamically allocated based on the confidence. The point cloud is converted into a voxel mesh, with each voxel containing joint features. A 3D convolutional network is used to process the voxel features, and semantic labels are assigned to each voxel based on the confidence of the model output. Cross-entropy loss is used to optimize the model.

[0105] In one embodiment of the present invention, the preprocessing of point cloud data includes the following steps:

[0106] The voxel grid of PCL is used to calculate the point density of each voxel, and high density thresholds and low density thresholds are set. The octree nodes are recursively subdivided for high density regions and adjacent voxels are merged for low density regions. A KD-tree is built inside the octree nodes for fast neighborhood search.

[0107] Huffman coding is performed on the semantic tags of voxels;

[0108] The filtering intensity is adjusted based on the voxel semantic labels, and bilateral filtering is used to preserve the sharpness of voxel boundaries, specifically:

[0109]

[0110] Where W(p) is the weight value of point p, p is the position vector of the current point, and p c Let I(p) and I(p) be the position vectors of the center point. c Points p and p' are respectively. c The strength value, σ d Let σ be the scale parameter of the spatial domain. r is the scale parameter of the intensity domain;

[0111] An axial attention module is introduced to calculate the position of the point cloud projected onto the image plane and align it with the image segmentation mask.

[0112] Specifically, the point density of each voxel is calculated using PCL's VoxelGrid. High-density thresholds and low-density thresholds are set. The octree nodes are recursively subdivided for high-density voxels, and adjacent voxels are merged for low-density voxels. A KD-tree is constructed inside the octree nodes for fast neighborhood search. The frequency of each semantic label is counted, and a Huffman tree is constructed based on the label frequency. The Huffman tree is traversed to generate the Huffman code for each label, and the voxel label is replaced with the Huffman code. The weight values ​​of the points are adjusted using a bilateral filtering formula. Each point is traversed, its weight value is calculated, and its position is updated. The point cloud is projected onto the image plane to obtain pixel coordinates. An axial attention module is designed to calculate the alignment weight between the point cloud and the image segmentation mask, and the point cloud features are aligned with the image segmentation mask.

[0113] In one embodiment of the present invention, the step of generating the radiation intensity or color of a voxel by fusing the spatial location features and image features of a voxel through a neural network radiation field includes the following steps:

[0114] The spatial location features of voxels, along with the color and texture features extracted by the ResNet-50 network, are input into the neural network radiation field to generate the radiation intensity or color of the voxels. Specifically:

[0115] c = σ(ω1·P_f + ω2·I_f)

[0116] Where c is the radiation intensity or color of the voxel, P_f is the spatial location feature, I_f is the image feature, including color and texture features, σ is the activation function, and ω1 and ω2 are the corresponding weights;

[0117] The material library is matched based on the voxel semantic tags, and the material texture is applied to the surface of the 3D model using UV mapping.

[0118] Specifically, the point cloud is divided into a voxel grid using PCL's VoxelGrid, and the center coordinates of each voxel are recorded as the spatial location feature x. The ResNet-50 model is used to extract the color and texture features of the image. A simple neural network is designed to combine the extracted feature f with the spatial location feature x of the voxels as input. The output is either radiance or color, and the activation function is ReLU with weights ω1 and ω2 of 0.4 and 0.6, respectively. The network is trained using the true radiance or color of the voxels as the supervision signal. Based on the semantic labels of the voxels, the corresponding material map is selected from the material library and loaded using OpenCV or PIL. The UV coordinates are calculated for each vertex of the 3D model, and the material map is mapped onto the surface of the 3D model according to the UV coordinates.

[0119] In one embodiment of the present invention, step S4 includes the following steps:

[0120] Based on the 3D model and voxel semantic labels of the target object, the optimal grasping point is selected and a grasping strategy is formulated: for regular large smooth objects, a multi-point grasping strategy is selected and the grasping points are evenly distributed; for irregular small rough objects, a single-point grasping strategy is selected, with the geometric center or centroid as the grasping point.

[0121] The A* algorithm is used to plan the robot's path from its current position to the target grasping point.

[0122] The robot uses cameras and LiDAR to acquire information about its working environment, aligns point cloud data with high-resolution images to generate a working environment map, and combines semantic segmentation results to label the types of obstacles.

[0123] In the path planning process of the A* algorithm, obstacles are treated as impassable nodes, and the obstacle area is expanded using an expansion operation.

[0124] Specifically, based on the 3D model and voxel semantic labels of the target object, the object is classified into "regular large smooth objects" or "irregular small rough objects". For regular large smooth objects: a multi-point grasping strategy is used, with grasping points evenly distributed, the surface normal vector of the object is calculated, and points with suitable normal vector directions for grasping are selected. For irregular small rough objects, a single-point grasping strategy is used, with the geometric center or centroid as the grasping point. The optimal grasping point and grasping strategy are output based on the classification results. The working environment information is acquired using cameras and LiDAR, and the point cloud data is aligned with high-resolution images to generate a working environment map. Combined with the semantic segmentation results, the types of obstacles are labeled. In the A* algorithm, obstacles are treated as impassable nodes, and the obstacle area is expanded using an expansion operation to increase the safety distance. The robot is controlled to move along the planned path to the target grasping point and execute the grasping action according to the grasping strategy.

[0125] In one embodiment of the present invention, step S5 includes the following steps:

[0126] Define the wellhead center coordinate system and the robot coordinate system, and align the origin of the robot coordinate system with the wellhead center;

[0127] The DH parameters are used to describe the robot's joint and link structure. The pose of the end effector is calculated based on the joint angles, and the joint angles are calculated based on the target pose.

[0128] The joint angle is obtained through a numerical iteration method, and the optimal solution is selected based on actual needs. The joint angle θ is... i i = 1, 2, ..., n, where n is the number of joints;

[0129] Discretize the joint angle sequence and assign it to the time axis. Use interpolation methods to generate smooth trajectories. Map the joint angle sequence to the time axis to generate trajectories with continuous velocity and acceleration.

[0130] The real-time pose of the end effector is acquired by an IMU sensor, the error between the real-time pose and the target pose is calculated, and the joint angle is adjusted according to the error. At the same time, an image of the grasping point is acquired by a camera, the real-time pose of the grasping point is estimated by a visual algorithm, and the trajectory is adjusted according to visual feedback.

[0131] Specifically, a wellhead center coordinate system is established with the wellhead center as the origin, defining the Z-axis perpendicular to the wellhead plane and pointing upwards, and the X and Y axes along the horizontal direction. A robot coordinate system is established with the robot base as the origin, defining the Z-axis along the robot's main axis, and the X and Y axes pointing forward / backwards and left / right / right, respectively. Based on the robot's current position and attitude, the transformation matrix between the robot coordinate system and the wellhead center coordinate system is calculated to transform the pose of the grasping point from the robot coordinate system to the wellhead center coordinate system. A DH parameter table is defined, containing the robot's link length ai, link torsion angle αi, joint offset di, and joint angle θi. The DH parameters are then used... The transformation matrix of each joint is calculated step by step using numerical methods to obtain the pose of the end effector. Inverse kinematics is used to solve for the joint angles. The joint angle sequence is discretized and distributed on the time axis. Interpolation methods, such as polynomial interpolation or spline interpolation, are used to generate a smooth trajectory. The velocity and acceleration are obtained by differentiating the joint angle trajectory. The real-time pose of the end effector is acquired through an IMU, and the error between the real-time pose and the target pose is calculated. The joint angles are adjusted according to the error. Images of the grasping points are acquired using a camera, and the real-time pose of the grasping points is estimated through a visual algorithm. The trajectory is adjusted according to visual feedback. Combining IMU and visual feedback, the joint angle trajectory is adjusted in real time.

[0132] In one embodiment of the present invention, defining the wellhead center coordinate system and the robot coordinate system, and aligning the origin of the robot coordinate system with the wellhead center, includes the following steps:

[0133] Define the wellhead center coordinate system: the origin is located at the center of the wellhead, the Z-axis is perpendicular to the wellhead plane and points upwards, and the X-axis and Y-axis are along the horizontal direction;

[0134] Define the robot coordinate system: the origin is located at the robot base, the Z-axis is along the robot's main axis, and the X-axis and Y-axis point to the robot's front-back and left-right directions, respectively;

[0135] Based on the robot's current position and orientation, calculate the transformation matrix between the robot coordinate system and the wellhead center coordinate system, and transform the pose of the grasping point from the robot coordinate system to the wellhead center coordinate system, specifically:

[0136] P w =T r_w ·P r

[0137] Among them, Pw Let T be the coordinate system at the wellhead center. r_w P is the coordinate system transformation matrix. r This is the robot's coordinate system.

[0138] In one embodiment of the present invention, step S6 includes the following steps:

[0139] Based on the grasping plan, control each joint of the robotic arm to grasp the target object;

[0140] Force sensors are installed at the finger joints of the robotic arm to measure contact forces in real time; IMU sensors are installed on the target object to measure its acceleration and angular velocity in real time, and displacement changes are calculated by integration; an image of the target object is captured by a camera, and pose changes are calculated by feature point matching; a high-resolution camera is installed above the robotic arm, and a deep learning algorithm is used to detect whether the target object is in the robotic arm. If the target object is in the robotic arm, the binarized output is 1, otherwise it is 0; stability indicators are calculated based on the contact force distribution of each finger joint and the center of gravity of the object.

[0141] The values ​​of each indicator are normalized, and each indicator is assigned a weight, with the total weight being 1. These weighted values ​​are then summed to obtain the comprehensive indicator impact score, specifically:

[0142]

[0143] Where E represents the comprehensive indicator impact score, and F... ′ ΔP ′ l ′ S ′ M′ represents the normalized contact force, pose change, visual detection result, and stability index, respectively, and ω represents the stability index. F ω p ω l ω and ω represent the corresponding weights, respectively;

[0144] If E ≥ the threshold, the crawling is successful and no re-crawl is needed; if E < the threshold, the crawling fails, the crawling strategy is re-planned, and the crawling is attempted again.

[0145] Specifically, based on the shape, size, weight, and pose of the target object, a grasping strategy is generated, including the optimal grasping point and grasping strategy. Using robot kinematics and dynamics models, the motion trajectory of the manipulator is planned, and the grasping planning results are sent to the manipulator controller to control each joint to move according to the planned trajectory, completing the grasping of the target object. Force sensors are installed at the ends of the manipulator's joints to collect contact force data of each joint in real time. An IMU sensor is installed on the target object to collect acceleration and angular velocity data of the target object in real time, and displacement changes are calculated by integration. A high-resolution camera is installed above the manipulator to capture images of the target object in real time. A deep learning algorithm is used to detect whether the target object is in the manipulator, and the output is binarized: if the target object is in the manipulator, output L=1; otherwise, output L=0. The contact force distribution of each joint and the center of gravity position of the target object are obtained, and the uniformity of the contact force distribution is calculated. Where S represents force uniformity, F i Let i be the contact force of the i-th finger joint. For the average contact force, N is the number of finger joints. The mass distribution of the target object is obtained, and its center of gravity and coordinates are calculated geometrically. The center of gravity coordinates are projected onto the palm plane to obtain the projection point. If the palm plane is tilted, coordinate transformation is required to project the center of gravity onto the palm plane. The stable region is usually a polygonal region (such as a rectangle, circle, or custom shape) of the robotic hand, determined by the structure of the robotic hand. For example, for a parallel gripping robotic hand, the stable region might be the central area between two fingers; for a multi-finger robotic hand, the stable region might be a polygon formed by the finger joints. Different judgment methods are used to determine whether the projection is within the stable region. For example, geometric judgment defines the stable region as a set of polygon vertices, and uses the ray crossing method or the point-within-the-polygon algorithm to determine whether the projection point is inside the polygon. If the projection point is within the stable region, it is assigned a value of 1; otherwise, it is assigned a value of 0. The above indicators are normalized, and each indicator is assigned a weight, and a weighted sum is obtained to obtain the comprehensive indicator influence score, ω. F ω p ω l The values ​​of ω are 0.2, 0.2, 0, 3 and 0.3 respectively; based on the relationship between the comprehensive index influence score and the threshold, it is determined whether to re-fetch; in the multiple fetching experiments, the comprehensive index influence score and fetching result of each fetch are recorded, the corresponding distribution chart is drawn, the distribution of successful and failed values ​​is observed, and the dividing point between success and failure is found as the threshold.

[0146] The above embodiments are only used to illustrate the technical methods of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical methods of the present invention without departing from the spirit and scope of the technical methods of the present invention.

Claims

1. A method for fusing control algorithms for a well workover robot, characterized in that, Includes the following steps: S1: Video streams are acquired through high-resolution cameras and preprocessed. The improved YOLOv7 is used to detect the target region, and a multi-branch structure is designed based on the Res-Net-50 network to extract multi-dimensional features of the target object. S2: The YOLOv7 object detection results, Res-Net-50 image features, and classifier recognition results are fused to generate an interactive 3D model; S3: Design a dynamic weight fusion network to allocate modal weights based on point cloud density and image texture confidence, generate joint feature representations, train a lightweight semantic segmentation model based on the joint features, and distinguish different target object categories; S4: Select the optimal grasping point and grasping strategy based on the target object category and characteristics, generate a working environment map based on the A* algorithm and fuse multi-sensor data, mark obstacle types, and plan a safe path; S5: Define the wellhead center coordinate system and robot coordinate system, calculate pose and joint angles, acquire real-time pose through IMU and camera, calculate error and dynamically adjust trajectory, and combine sensor feedback for pose control and trajectory correction. S6: Based on the grasping plan, control each finger joint of the robotic arm to grasp the target object. After normalizing the contact force, pose change, visual detection results and stability index, the weighted sum of the comprehensive indexes affects the score, and it is determined whether the grasping needs to be repeated.

2. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S1 includes the following steps: A high-resolution camera is used to capture real-time video streams of the workspace, and the captured images are preprocessed, including noise reduction, color correction, and white balance. YOLOv7 was chosen as the basic object detection model. Deformable convolution was introduced into its E-ELAN backbone network, and a coordinate attention mechanism was added to the detection head. Gaussian heatmap regression was used to replace traditional bounding box regression, and feature maps of different scales were dynamically fused through BiFPN weighted bidirectional feature pyramid. Res-Net-50 was chosen as the base network, and CBAM was embedded in the residual block to design a multi-branch network. The shape and topology of the object, RGB-HSV color space features and LBP texture features were extracted respectively. The features extracted by different branches were fused to form a multi-dimensional semantic information representation.

3. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S2 includes the following steps: The target region detected by YOLOv7 is input into the Res-Net-50 network with enhanced attention mechanism to extract multi-dimensional features of the target object; Based on the extracted multi-dimensional features, a classifier is used to classify and identify the target objects; The sub-pixel level bounding boxes output by YOLOv7 are further refined, and the multi-dimensional semantic information extracted by ResNet-50 is fused with the object detection results to form a complete object description, including geometric attributes: bounding box coordinates and shape contour; appearance attributes: color histogram and LBP texture features; and semantic attributes: category label and multi-dimensional semantic vector. The integrated target description is output as an interactive 3D model.

4. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S3 includes the following steps: In the workspace, the target object is scanned using a laser rangefinder to obtain raw point cloud data, which is then preprocessed. The point cloud is aligned with the high-resolution image through a time synchronization mechanism, and the coordinate system is unified based on the IMU attitude data; Features are extracted from point cloud data, including normal vectors, curvature, and local surface features, and adaptive downsampling is performed on high-density point clouds; Combine Res-Net-50 network to extract multi-dimensional features of target objects; Design a cross-membrane feature projection module: map the multi-dimensional features extracted by the Res-Net-50 network to the point cloud coordinate system, and perform spatial alignment through feature matching; Design a dynamic weighted fusion network to dynamically allocate modal weights based on point cloud density and image texture confidence, and generate joint feature representations; A lightweight semantic segmentation model is trained based on joint features. The model outputs a multi-class confidence score for each voxel, and a semantic label is assigned to each voxel based on the confidence score output by the model. By fusing the spatial location features of voxels with image features through a neural network radiation field, the radiation intensity or color of voxels is generated.

5. The well workover robot control algorithm fusion method according to claim 4, characterized in that, The preprocessing of point cloud data includes the following steps: The voxel grid of PCL is used to calculate the point density of each voxel, and high density thresholds and low density thresholds are set. The octree nodes are recursively subdivided for high density regions and adjacent voxels are merged for low density regions. A KD-tree is built inside the octree nodes for fast neighborhood search. Huffman coding is performed on the semantic tags of voxels; The filtering intensity is adjusted based on the voxel semantic labels, and bilateral filtering is used to preserve the sharpness of voxel boundaries, specifically: Where W(p) is the weight value of point p, p is the position vector of the current point, and p c Let I(p) and I(p) be the position vectors of the center point. c Points p and p' are respectively. c The strength value, σ d Let σ be the scale parameter of the spatial domain. r is the scale parameter of the intensity domain; An axial attention module is introduced to calculate the position of the point cloud projected onto the image plane and align it with the image segmentation mask.

6. The well workover robot control algorithm fusion method according to claim 4, characterized in that, The process of generating voxel radiation intensity or color by fusing the spatial location features of voxels with image features through a neural network radiation field includes the following steps: The spatial location features of voxels, along with the color and texture features extracted by the ResNet-50 network, are input into the neural network radiation field to generate the radiation intensity or color of the voxels. Specifically: c = σ(ω1·P_f + ω2·I_f) Where c is the radiation intensity or color of the voxel, P_f is the spatial location feature, I_f is the image feature, including color and texture features, σ is the activation function, and ω1 and ω2 are the corresponding weights. The material library is matched based on the voxel semantic tags, and the material texture is applied to the surface of the 3D model using UV mapping.

7. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S4 includes the following steps: Based on the 3D model and voxel semantic labels of the target object, the optimal grasping point is selected and a grasping strategy is formulated: for regular large smooth objects, a multi-point grasping strategy is selected and the grasping points are evenly distributed; for irregular small rough objects, a single-point grasping strategy is selected, with the geometric center or centroid as the grasping point. The A* algorithm is used to plan the robot's path from its current position to the target grasping point. The robot uses cameras and LiDAR to acquire information about its working environment, aligns point cloud data with high-resolution images to generate a working environment map, and combines semantic segmentation results to label the types of obstacles. In the path planning process of the A* algorithm, obstacles are treated as impassable nodes, and the obstacle area is expanded using an expansion operation.

8. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S5 includes the following steps: Define the wellhead center coordinate system and the robot coordinate system, and align the origin of the robot coordinate system with the wellhead center; The DH parameters are used to describe the robot's joint and link structure. The pose of the end effector is calculated based on the joint angles, and the joint angles are calculated based on the target pose. The joint angle is obtained through a numerical iteration method, and the optimal solution is selected based on actual needs. The joint angle θ is... i i = 1, 2, ..., n, where n is the number of joints; Discretize the joint angle sequence and assign it to the time axis. Use interpolation methods to generate smooth trajectories. Map the joint angle sequence to the time axis to generate trajectories with continuous velocity and acceleration. The real-time pose of the end effector is acquired by an IMU sensor, the error between the real-time pose and the target pose is calculated, and the joint angle is adjusted according to the error. At the same time, an image of the grasping point is acquired by a camera, the real-time pose of the grasping point is estimated by a visual algorithm, and the trajectory is adjusted according to visual feedback.

9. The well workover robot control algorithm fusion method according to claim 8, characterized in that, The process of defining the wellhead center coordinate system and the robot coordinate system, and aligning the origin of the robot coordinate system with the wellhead center, includes the following steps: Define the wellhead center coordinate system: the origin is located at the center of the wellhead, the Z-axis is perpendicular to the wellhead plane and points upwards, and the X-axis and Y-axis are along the horizontal direction; Define the robot coordinate system: the origin is located at the robot base, the Z-axis is along the robot's main axis, and the X-axis and Y-axis point to the robot's front-back and left-right directions, respectively; Based on the robot's current position and orientation, calculate the transformation matrix between the robot coordinate system and the wellhead center coordinate system, and transform the pose of the grasping point from the robot coordinate system to the wellhead center coordinate system, specifically: P w =T r_w ·P r Among them, P w Let T be the coordinate system at the wellhead center. r_w P is the coordinate system transformation matrix. r This is the robot's coordinate system.

10. The well workover robot control algorithm fusion method according to claim 1, characterized in that, Step S6 includes the following steps: Based on the grasping plan, control each joint of the robotic arm to grasp the target object; Force sensors are installed at the finger joints of the robotic arm to measure contact forces in real time; IMU sensors are installed on the target object to measure its acceleration and angular velocity in real time, and displacement changes are calculated by integration; an image of the target object is captured by a camera, and pose changes are calculated by feature point matching; a high-resolution camera is installed above the robotic arm, and a deep learning algorithm is used to detect whether the target object is in the robotic arm. If the target object is in the robotic arm, the binarized output is 1, otherwise it is 0; stability indicators are calculated based on the contact force distribution of each finger joint and the center of gravity of the object. The values ​​of each indicator are normalized, and each indicator is assigned a weight, with the total weight being 1. These weighted values ​​are then summed to obtain the comprehensive indicator impact score, specifically: Where E represents the comprehensive indicator impact score, and F... ′ ΔP ′ l ′ S ′ M′ represents the normalized contact force, pose change, visual detection result, and stability index, respectively, and ω represents the stability index. F ω p ω l ω and ω represent the corresponding weights, respectively; If E ≥ the threshold, the crawling is successful and no re-crawl is needed; if E < the threshold, the crawling fails, the crawling strategy is re-planned, and the crawling is attempted again.

Images