Visual servoing control method and system driven by oil outlet pose perception

Abstract

This invention belongs to the field of electric robot technology. It proposes a visual servoing control method and system driven by pose perception at the oil extraction port. The method generates a frustum region by mapping a target detection box to filter point cloud noise; it uses a network with embedded self-attention mechanism for semantically enhanced instance segmentation to obtain the target mask; and it combines residual compensation and 3D bounding box prediction to transform the point cloud to the real coordinate system to calculate the accurate pose. Finally, a hybrid visual servoing strategy is adopted: pose error control is used at long distances, while image feature-based control is used at close distances, achieving efficient and accurate automatic docking of the oil extraction robot.

Description

Technical Field

[0001] This invention relates to the field of electric robot technology, specifically to a visual servo control method and system for oil port pose perception and driving. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Traditional automated oil extraction tasks primarily rely on high-precision mechanical positioning or manual assistance. The former demands a high degree of scene structuring and struggles to adapt to complex and variable field or non-standardized environments, while the latter suffers from low efficiency and associated safety risks. With the rapid development of computer vision and deep learning technologies, visual servoing-based autonomous robot operation has become a research hotspot. Among these approaches, integrating the rich semantic information of 2D images with the precise geometric features of 3D point clouds is considered an effective way to improve target perception capabilities in unstructured environments. Existing end-to-end target pose estimation methods attempt to directly establish a mapping relationship from raw sensor data to robot execution commands through deep neural networks, aiming to reduce error accumulation in intermediate processing stages. Especially when dealing with targets such as cylindrical oil extraction ports that possess rotational symmetry or indistinct local features, techniques such as frustum clipping, instance segmentation, and residual compensation can theoretically significantly enhance the system's understanding of the target's spatial position and orientation, providing a solid technical foundation for high-precision automated docking.

[0004] However, existing technologies still have significant shortcomings when dealing with oil intake port pose estimation and subsequent servo control. First, raw point cloud data often contains a large amount of background noise. Direct processing not only incurs a high computational load but also easily leads to feature extraction bias. Traditional filtering methods struggle to balance preserving target details with noise removal, resulting in insufficient initial positioning accuracy. Second, for targets like oil intake ports with relatively simple geometric features, conventional segmentation networks struggle to capture their subtle local geometric structures, often resulting in blurred instance segmentation boundaries or category confusion, thus affecting the accuracy of pose regression. More critically, a single visual servoing strategy cannot simultaneously meet the needs of rapid approach over large areas and precise docking over small areas: pose-based servoing is robust over large distances but has limited end-effector accuracy, while image-based servoing, although achieving pixel-level alignment, is prone to getting trapped in local minima or losing the target at large initial distances. Existing technologies lack a mechanism to dynamically switch control strategies based on the working distance, causing the robot to easily jitter or fail to align during the final docking stage, failing to meet the stringent requirements of highly reliable automated oil extraction operations. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a visual servo control method and system driven by oil port pose perception, which improves the overall efficiency, docking accuracy, and adaptability and reliability of the oil extraction robot in unstructured environments.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, the present invention provides a visual servo control method driven by oil port pose perception.

[0007] A vision servo control method driven by oil port pose perception includes the following steps: View frustum point cloud generation steps: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, combine the camera intrinsic parameters and projection matrix to map the target detection box to the three-dimensional space to generate the view frustum region, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. Semantic Enhancement Instance Segmentation Steps: Input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with embedded self-attention mechanism. Capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance. The residual compensation pose regression steps are as follows: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use the transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. Hybrid visual servo drive steps: Calculate the pose error between the current pose and the desired pose based on the target pose data. When the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error; otherwise, perform image-based visual servo control.

[0008] In one implementation of the first aspect of the present invention, the training process is optimized using a multi-task loss function, which is expressed as follows: ,in, This represents the total loss of the network; The improved PointNet network instance segmentation loss; and These are the weight parameters in the loss function. and These are the centroid regression losses for the Hole-Net network and the 3D bounding box prediction network, respectively. and These represent the classification loss and regression loss for the 3D bounding box, respectively. and These are the classification loss and regression loss for the heading angle, respectively; Corner loss for 3D bounding box prediction.

[0009] As a further limitation of the first aspect of the present invention, an elastic network regularization term is added to the multi-task loss function, and the total loss after adding the weight decay term is expressed as: ; in, The total loss after adding the weight decay term. This refers to the multitasking loss before regularization. The regularization coefficient is . This is the regularization scaling parameter. The L1 regularization term is used to push the weights toward zero. The L2 regularization term is used to constrain weight smoothing. This represents the learnable weight parameters in a neural network.

[0010] In one implementation of the first aspect of the present invention, the computation process of the self-attention mechanism in the semantically enhanced instance segmentation step includes: A linear transformation is performed on the input feature sequence to generate a query vector, a key vector, and a value vector. The query vector, key vector, and value vector are all obtained by multiplying the learnable parameter matrix with the input features. Calculate the attention score between points. The attention score is the product of the query vector and the transposed key vector divided by the scaling factor. The attention scores are normalized using the Softmax function to obtain attention weights. The attention weights are then weighted and summed with the value vectors to obtain the output feature representation.

[0011] In one implementation of the first aspect of the present invention, in the semantically enhanced instance segmentation step, a multi-head attention mechanism is used to learn the relevance of different subspaces in parallel. The output of the attention head is determined by the first... The projection matrix corresponding to the first attention head and the first attention head The enhanced feature is obtained by multiplying the attention outputs of each attention head and then concatenating the outputs of all attention heads and linearly transforming the final output through a projection matrix.

[0012] In one implementation of the first aspect of the present invention, in the residual compensation pose regression step, the transformation network is used to predict the residual between the true centroid and the origin of the mask coordinate system, and the three-dimensional bounding box prediction network outputs the length, width, height, centroid coordinates and heading angle of the bounding box through a fully connected layer. The residual method is used to fuse the residual with the centroid coordinates to obtain the accurate target center point.

[0013] In one implementation of the first aspect of the present invention, target feature points in the current image are extracted and the image feature error between them and the desired image features corresponding to the desired pose state is calculated. The image feature error is then used to perform image-based visual servo control.

[0014] Secondly, the present invention provides a vision servo control system driven by oil port pose perception.

[0015] A vision servo control system driven by oil port pose perception includes: The view frustum point cloud generation unit is configured to: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, map the target detection box to the three-dimensional space to generate the view frustum region by combining the camera intrinsic parameters and projection matrix, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. The semantically enhanced instance segmentation unit is configured to: input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with an embedded self-attention mechanism, capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance; The residual compensation pose regression unit is configured to: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use a transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. The hybrid visual servo drive unit is configured to: calculate the pose error between the current pose and the desired pose based on the target pose data; when the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error; otherwise, perform image-based visual servo control.

[0016] Thirdly, the present invention provides a computer device, comprising: a processor and a computer-readable storage medium; A processor, adapted to execute computer programs; A computer-readable storage medium storing a computer program, which, when executed by a processor, implements the visual servo control method for oil port pose perception driven by the first aspect of the present invention.

[0017] Fourthly, the present invention provides a computer-readable storage medium storing a computer program adapted to be loaded by a processor and executed by the visual servo control method for oil port pose perception driven by the first aspect of the present invention.

[0018] Compared with the prior art, the beneficial effects of the present invention are: This invention innovatively proposes a visual servo control method driven by the pose perception of the oil intake port, and develops a visual servo control system. Through the collaborative operation of four core steps—view frustum point cloud generation, semantically enhanced instance segmentation, residual-compensated pose regression, and hybrid visual servo driving—the original point cloud is filtered using a view frustum region generated by a 2D detection box mapping, constructing a closed-loop control mechanism from perception to execution. This solves the problems of severe noise interference in complex backgrounds, difficult feature extraction, large computational redundancy in traditional global processing, and blurred boundary recognition. It overcomes the shortcomings of traditional global processing methods, such as large computational redundancy and susceptibility to environmental clutter. By introducing a point cloud segmentation network with an embedded self-attention mechanism and combining it with semantic category vectors, it achieves the processing of geometric features such as the oil intake port. The accurate capture of local geometric features of a single target avoids the ambiguity and confusion in instance boundary recognition of conventional segmentation networks; the use of a transformation network to predict the centroid residual vector and combine it with 3D bounding box parameters for coordinate transformation overcomes the shortcomings of insufficient pose accuracy in direct regression and improves the robustness of target center point and heading angle estimation; the dynamic switching between pose-based and image-based visual servoing strategies based on distance thresholds solves the contradiction that a single control mode cannot simultaneously handle large-scale rapid approach and small-scale precise docking, avoiding jitter and alignment failures caused by loss of field of view or getting stuck in local minima during robot end-effector operations, improving the docking accuracy and overall efficiency of automated oil extraction operations, and enhancing the system's adaptability and reliability in unstructured environments.

[0019] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0020] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0021] Figure 1 A flowchart illustrating a vision servo control method driven by oil port pose perception, provided as an exemplary embodiment of the present invention. Figure 2 A schematic diagram of an end-to-end target pose estimation network provided as an exemplary embodiment of the present invention; Figure 3 A schematic diagram of point cloud information of an oil intake port provided as an exemplary embodiment of the present invention; Figure 4 A schematic diagram illustrating model pose convergence as provided in an exemplary embodiment of the present invention; Figure 5 A schematic diagram of a point cloud segmentation accuracy curve provided for an exemplary embodiment of the present invention; Figure 6 A schematic diagram of a loss curve provided for an exemplary embodiment of the present invention; Figure 7 This is a schematic diagram illustrating the detection effect provided in an exemplary embodiment of the present invention. Figure 7 (a) shows the detection results in the positive direction; Figure 7 (b) shows the detection results when the image is tilted upwards by 30°. Figure 7 Image (c) shows the detection result when the image is deflected 30° to the left. Figure 7 The middle (d) image shows the detection result when the image is tilted downwards by 30°. Figure 7 Image (e) shows the detection result when the image is deflected 30° to the right. Figure 7 Image (f) shows the detection result when rotated 30°; Figure 8 A schematic diagram of a vision servo control system driven by oil port pose perception provided as an exemplary embodiment of the present invention. Figure 9 A schematic diagram of a computer device provided for an exemplary embodiment of the present invention. Detailed Implementation

[0022] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0023] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used in this invention have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0024] This implementation proposes a vision servo control method driven by the pose perception of the oil sampling port, such as... Figure 1 As shown, the process includes the following: S101: Steps for generating the view frustum point cloud.

[0025] First, the original image and point cloud data of the scene to be connected are acquired. A two-dimensional object detector based on the Feature Pyramid Network (FPN) model is used, and the Fast R-CNN method is employed to estimate the two-dimensional object detection boxes and extract the object detection boxes from the original image.

[0026] By combining camera intrinsic parameters and the projection matrices of the camera and LiDAR, the 2D target detection bounding box is mapped to a 3D space to generate a view frustum region. Since the orientation of the 2D detection bounding box is different for each target during 2D detection, the orientation of the generated 3D point cloud view frustum also differs. To facilitate subsequent point cloud data processing, the orientation of the generated 3D point cloud view frustum is normalized by transforming the camera coordinate system to the view frustum coordinate system.

[0027] By generating a 3D view frustum region, noise points located outside the view frustum region in the original point cloud are filtered out to obtain the view frustum point cloud. This step effectively reduces the range of point cloud data input to the network, decreases the network learning scale, and improves detection speed and accuracy.

[0028] S102: Semantic Enhancement Instance Segmentation Steps.

[0029] The view frustum point cloud and the semantic category vector (one-hot vector) generated based on the object detection box are input into a point cloud segmentation network with an embedded self-attention mechanism, such as Figure 2 As shown, the system is divided into three main modules: original point cloud and image acquisition, a point cloud segmentation network with embedded self-attention mechanism, and target pose generation. First, in the orange area on the left, the system starts from the input point cloud and image, locates the view frustum region using a 2D target detection model, and generates view frustum point clouds (n×c and n×C) by combining camera intrinsic parameters and projection matrices. Simultaneously, through the view frustum cloud generation and filtering process, including generating the view frustum region, filtering noise points, and outputting semantic category vectors, a point cloud mask containing the target instance is obtained. The blue area in the middle represents the 3D instance segmentation network PointNet, which introduces a self-attention mechanism. It extracts local geometric features by querying the vector Q, key vector K, and value vector V, segments the point cloud, and outputs the segmented point cloud (m×c) and the point cloud mask after mask processing. The green area on the right is responsible for the final pose prediction. First, a mask coordinate system with the centroid as the origin is established from the segmented point cloud. After transformation network and residual compensation, the 3D bounding box prediction network outputs the 3D dimensions of the target. Simultaneously, another path generates the target pose data through coordinate transformation (including residual compensation), 3D bounding box prediction network, coordinate transformation and pose fusion. The entire process realizes end-to-end inference from multimodal input to accurate 3D pose output.

[0030] S102-1: Basic feature extraction.

[0031] like Figure 3 As shown, this is the original point cloud. Using the original point cloud data as input, the PointNet network encodes the three-dimensional coordinates of each point through a multilayer perceptron (MLP) and extracts the overall features through global max pooling, thereby obtaining the global structural representation capability of the point cloud.

[0032] Given an input feature sequence: (1); in, Indicates the input feature sequence. Indicates the first Feature vectors of points, This represents the total number of points.

[0033] S102-2: Calculation of self-attention mechanism.

[0034] To enhance PointNet's ability to model local structures, a Transformer-based self-attention mechanism is introduced. Three linear transformations are used to generate query, key, and value vectors, respectively. (2); in, Represents the query vector. Represents the key vector. Represents a value vector. , , is a learnable parameter matrix.

[0035] point Point Attention score: (3); in, Point Point Attention score and Points and points Query and key vector, This is a scaling factor used to stabilize the gradient.

[0036] The above scores are normalized using the Softmax function to obtain the attention weights: (4); in, Indicates attention weights. This represents an exponential function.

[0037] Then, using these weights, a weighted sum is taken of the value vector to obtain the point. The output feature representation: (5); in, Point The output feature representation, Point The value vector.

[0038] The above process can be written as a matrix: (6); in, This represents the output of the attention mechanism. Represents the normalization function. This indicates the transpose operation.

[0039] To enhance feature representation capabilities, a multi-head attention mechanism is introduced. This mechanism learns the correlation between different subspaces by using multiple attention heads in parallel. The focus of attention is: (7); in, Indicates the first The output of each attention head, , , The first Projection matrix of the head.

[0040] The outputs of multiple attention heads are concatenated and then subjected to a linear transformation. The final result is: (8); in, This represents the final output of the multi-head attention mechanism. This indicates a splicing operation. Indicates the number of attention heads. It is the final output projection matrix.

[0041] In terms of network structure, two self-attention modules are embedded in the original PointNet network. The improved network flow is as follows: Input point cloud The algorithm uses an MLP to extract initial features for each point; inputs the features of all points into a two-layer multi-head self-attention mechanism module for encoding; uses max pooling to extract global features; and uses the global features in subsequent classification or segmentation tasks. It captures local geometric features of the point cloud through interactive computation of query vectors, key vectors, and value vectors, and outputs a point cloud mask containing the target instance.

[0042] Optionally, in some other implementations, during the point cloud acquisition process at the oil intake port, the acquired point cloud mask is often incomplete (non-closed surface) due to oil smudges, limitations in LiDAR scanning angle, or environmental noise. Directly calculating the point cloud's geometric centroid will produce significant deviations, leading to positional errors during robot docking. Existing simple regression networks lack explicit constraints on the geometric characteristics of the point cloud distribution, making it difficult to accurately infer the true centroid when data is missing.

[0043] Therefore, optionally, the enhanced features output by formula (8) can be utilized. A geometric moment-constrained residual regression branch is constructed. This branch not only predicts the residual vector, but also introduces the second-order central moments (covariance matrix) of the point cloud as confidence weights to dynamically adjust the compensation strength of the residuals.

[0044] Define the adaptive centroid residual vector The calculation formula is: (9); Then, the corrected true centroid coordinates are calculated. : (10); in, The normalized residual offset vector is dimensionless (after...). Activation indicates the offset ratio relative to the target size; These are the true centroid coordinates, in meters (m). Let the geometric centroid of the point cloud mask be denoted by . The calculation yields a result in meters (m). and For the learnable weight matrix and bias vector of the fully connected layer; This is a global average pooling operation that compresses spatial features into channel vectors; This is the Hadamard product (element-wise multiplication). To enhance features The reconstructed local covariance matrix of the point cloud reflects the degree of dispersion of the current point cloud distribution, with units of square meters (m²) (before normalization in the feature space). The reference covariance matrix for the standard complete oil intake model is in square meters (m²). The trace of the matrix is ​​a scalar that represents the overall variance of the point cloud distribution. Let be the shape confidence coefficient, which is dimensionless. This is relevant when point cloud defects are severe. It will be significantly smaller than This coefficient automatically reduces the residual compensation magnitude to prevent overcorrection; conversely, it increases the compensation. The target feature size (e.g., diameter) is the output of the 3D bounding box prediction network, in meters (m), used to restore the normalized residual to the physical distance; This is the principal direction rotation matrix of the point cloud obtained by principal component analysis (PCA), used to transform the residuals from the characteristic coordinate system back to the world coordinate system, and is dimensionless.

[0045] By introducing the trace ratio of the covariance matrix The algorithm can perceive the integrity of the point cloud. When the oil intake is partially occluded, causing the point cloud to become sparse, the weight of the residual compensation is automatically reduced to avoid the network making illusory predictions. Explicitly correlating the black-box output of the neural network with the geometric moments (second-order statistics) of the point cloud improves the interpretability and robustness of pose estimation.

[0046] Calculated It can replace the original simple centroid as the centroid regression loss in formula (13). The Ground Truth reference is also the core input for calculating translation error in the subsequent hybrid vision servo drive steps, directly determining the initial positioning accuracy of the robot's end effector.

[0047] In another implementation, the oil intake port is typically a cylindrical structure with rotational symmetry about a central axis. In visual perception, this symmetry can lead to a "pose ambiguity" problem (i.e., rotational ambiguity). (Even if any angle appears geometrically similar), conventional regression networks are prone to oscillations near the axis of symmetry, leading to non-convergence or abrupt changes in the yaw estimation, severely impacting the smooth execution of insertion / removal operations. Therefore, alternatively, we can abandon the approach of directly regressing a single angle value and propose a discretized probability-weighted angle decoupling algorithm. Spatial discretization The algorithm identifies candidate angle intervals, uses enhanced features to predict the probability distribution of each interval, and then calculates the smooth heading angle using expectation.

[0048] Define the probability distribution vector of the heading angle and final estimated heading angle The calculation formula is: (11); (12); in, Let be the probability distribution vector of the heading angle, with dimension . , dimensionless, and ; This is the final estimated heading angle, in radians (rad). This is a global max pooling operation that extracts the most significant directional features; The weights and biases of the classification heads; The number of discretized angle intervals (e.g.) ,Every (a range); For the first The predicted probability of each angle interval is dimensionless. For the first The center angle value of each interval, in radians (rad); max(P yaw The maximum probability value represents the network's confidence in the current prediction and is dimensionless. This is a small angular correction, in radians (rad), output from another parallel branch for the fine-grained regression residuals.

[0049] when When the value is close to 1 (high confidence), the second term approaches 0, and the result is mainly determined by discrete probability weighting, ensuring stability; when... When the confidence level is small (low confidence, typically occurring in symmetrical fuzzy regions), the weight of the second term increases, introducing... Perform fine-grained corrections in continuous space to avoid quantization errors.

[0050] By modeling probability distributions, networks can learn multimodal distributions (e.g., and With high probability, the correct peak value is automatically selected by combining contextual features (such as the handle or sign next to the oil outlet), which solves the ambiguity problem of traditional regression methods on symmetrical objects; the expectation calculation method makes the angle output continuous and smooth with feature changes, avoiding the step-like jumps common in classification methods, which is beneficial to the stability of the visual servo control system.

[0051] Calculated It can be used as the heading angle regression loss in formula (13) The predicted values ​​are used to supervise network training.

[0052] S103: Residual compensation pose regression steps.

[0053] A mask coordinate system is established with the centroid of the point cloud mask as the origin. Since the target point cloud acquired by the lidar may have incompleteness, leading to deviations in the centroid extracted based on the segmentation results, a Hole-Net transform network is used to estimate the true centroid of the target. Hole-Net is then used to predict the residual between the true centroid and the mask origin.

[0054] Simultaneously, a 3D bounding box prediction module is used to estimate the center, size, and orientation of the 3D bounding box through a PointNet network. This module uses fully connected layers (FCs) to output the length, width, height, centroid coordinates, and heading angle of the bounding box, among other 3D detection parameters.

[0055] By combining a 3D bounding box estimation network, Hole-Net, and the residual of the point cloud mask center, a residual method is used to further accurately estimate the target center point. A transformation network is used to predict the residual vector of the true centroid relative to the origin. Combined with the size parameters and heading angle parameters output by the 3D bounding box prediction network, the view frustum point cloud is transformed from the mask coordinate system to the true coordinate system to obtain the target pose data.

[0056] The multi-task loss function of the network model is designed as follows: (13); in, This represents the total loss of the network; The improved PointNet network instance segmentation loss; and These are the weight parameters in the loss function. and These are the centroid regression losses for the Hole-Net network and the 3D bounding box prediction network, respectively. and These represent the classification loss and regression loss for the 3D bounding box, respectively. and These are the classification loss and regression loss for the heading angle, respectively; Corner loss for 3D bounding box prediction.

[0057] To further improve network performance, an Elastic Net regularization term was added to the original multi-task loss function to suppress overfitting. The overall loss function is expressed as follows: (14); in, The total loss after adding the weight decay term. This refers to the multitasking loss before regularization. and These are the regularization coefficient and the regularization ratio parameters, respectively, which are set to values ​​of [values ​​to be filled in] in this invention. , , express Regularization terms are used to shrink the weights toward zero, thereby achieving feature selection. express Regularization terms are used to constrain weight smoothing and improve model stability. This represents the learnable weight parameters in a neural network.

[0058] Specifically, formula (1) defines the data input. Formula (2) will input Linear mapping is This is a prerequisite for the attention mechanism; formulas (3) and (4) describe a single point. How to calculate for all other points? attention weights Formula (5) describes the use of these weights value vector Weighted summation yields the output of a single point. Formula (6) is a matrix encapsulation of formulas (3), (4), and (5), which will... The point-by-point calculation (loop process) is uniformly expressed as an efficient matrix operation. Formula (5) is the expansion of Formula (6) in the single-point dimension. Formula (6) is the compact expression of Formula (5) in the overall batch dimension. Formulas (7) and (8) extend the above single attention mechanism to multi-head parallelism, enhance the feature expression capability, and the extracted features are used for subsequent segmentation (output mask), and then enter the loss function optimization stage defined by Formulas (13) and (14).

[0059] The results show that introducing a self-attention mechanism module after either the local or global feature layer can improve the accuracy of 3D object detection. Furthermore, when the self-attention mechanism is applied to both the local and global feature layers, the model achieves superior overall performance. This indicates that the self-attention mechanism helps the model more fully capture the contextual relationships and feature representations in point cloud data. In addition, introducing a regularized weight decay term into the loss function effectively alleviates the overfitting problem, further improving the model's generalization ability and enabling it to converge with higher accuracy during training. The pose convergence during training is as follows... Figure 4 As shown.

[0060] Based on the above improvements, the proposed model demonstrates significant advantages in 3D object detection tasks. The point cloud segmentation accuracy curve and loss curve during its training process are shown below. Figure 5 , Figure 6 and Figure 7 As shown, the model exhibits a fast convergence speed: after more than 10,000 weight updates, the loss value and segmentation accuracy gradually stabilize, with the segmentation accuracy consistently remaining above 90%, thus validating the model's effectiveness and robustness.

[0061] S104: Hybrid vision servo drive steps.

[0062] The pose error between the current pose and the desired pose is calculated based on the target pose data, and an integrated approach of "visual servoing + model predictive control (MPC)" is adopted.

[0063] This step details how to construct and execute a hybrid closed-loop control system of "visual servoing + model predictive control" based on the real-time pose calculated in the previous steps, using formulas (15) to (22). The entire control loop runs at a fixed frequency, and the specific process is as follows: S104-1: Real-time pose calculation and state initialization.

[0064] At the beginning of each control cycle, the system first acquires the current image through the camera and uses formulas (15) to (18) to complete the pose calculation from pixel space to robot base coordinate system, providing accurate state feedback for the controller.

[0065] Intrinsic parameter calibration and projection modeling: First, load the pre-calibrated camera intrinsic parameter matrix. (Formula 11) Establish the mapping relationship between pixel coordinates and camera normalized coordinates.

[0066] (15); in, Represents the camera intrinsic parameter matrix; , Indicates focal length in the image shaft and Pixel values ​​along the axial direction; , This represents the principal point coordinates, i.e., the projection of the optical center onto the image plane.

[0067] Relative pose determination: Extracting the pixel coordinates of feature points at the oil extraction port in the image. Combined with known three-dimensional model points of the oil intake port Using the pinhole projection model described by formula (16), the rotation matrix of the current camera coordinate system relative to the oil intake port coordinate system is solved by the PnP algorithm. Translation vector If the oil port is taken as a planar feature, the homography matrix of formula (17) can be used as an aid. Perform optimized calculations.

[0068] (16) ； (17); in, Represents a two-dimensional pixel coordinate vector of a feature point on the image plane; Represents the three-dimensional spatial coordinate vector of a feature point in the object coordinate system; express Rotation matrix; express Translation vector; express Homography matrix; The unit vector representing the normal vector of the oil inlet plane; This represents the vertical distance from the camera's optical center to the oil intake plane; This represents the inverse of the camera intrinsic parameter matrix.

[0069] Coordinate System 1: The calculated relative pose of the camera The external parameters obtained by combining robot hand-eye calibration The actual pose of the current end effector in the robot's base coordinate system is obtained by using formula (18). .

[0070] (18); in, The transformation matrix representing the camera coordinate system relative to the robot's base coordinate system (i.e., the actual pose of the current end effector) ); This represents the transformation matrix of the marker coordinate system relative to the robot's base coordinate system; This represents the transformation matrix between the camera coordinate system and the marker coordinate system. At this point, the controller's state variables... It is initialized with the current pose information.

[0071] S104-2: Judgment of hybrid strategy mode switching.

[0072] The system reads the current pose. depth component and with the preset distance threshold Comparison: like If the distance is determined to be in the long-distance phase, PBVS-MPC mode is activated; like The system was determined to be in the close-range precision docking phase, and the IBVS-MPC mode was activated.

[0073] S104-3: Long-distance stage: Pose-based visual servoing (PBVS) combined with MPC.

[0074] At this stage, the control objective is to quickly eliminate large pose deviations while ensuring smooth motion.

[0075] Error calculation (PBVS), specifically, includes: Calculate the current pose using formula (19) Desired docking pose Lie algebra error between .

[0076] (19); in, Represents the pose-based visual servoing error vector; Represents the desired pose transformation matrix; This represents the current pose transformation matrix; Represents the logarithmic mapping operation from Lie groups to Lie algebras; This represents an operator that converts a Lie algebra matrix into a vector.

[0077] The logarithmic mapping of the attitude part is specifically calculated by formula (20), which converts the rotation matrix into a rotation vector.

[0078] (20); in, Indicates the equivalent rotation angle; The unit vector representing the equivalent axis of rotation, this error vector That is, it serves as a reference benchmark for the state tracking error of the MPC controller.

[0079] MPC scrolling optimization, specifically, includes: The first step is to build the prediction model. (Based on the current state) Starting from this point, the nonlinear kinematic model described by formula (21) is used to predict the future. The state trajectory of the step. In order to solve in real time, linearization is performed at the current working point using formula (22) to obtain the linear state space equation.

[0080] (twenty one); (twenty two); in, , These represent the system state vectors at the next and current moments, respectively. This represents the control input vector at the current moment; Represents a nonlinear state transition function; , These represent the state increments at the next and current time points, respectively. Indicates the increment of the control input; Represents the state transition matrix; This represents the control matrix.

[0081] The second step is objective function optimization. The objective function shown in formula (23) is constructed. Wherein, the error term... This is the future pose error predicted based on formula (19). The controller uses a quadratic programming solver to find a set of control increment sequences that minimizes the weighted sum of squared errors in the prediction time domain.

[0082] (twenty three); in, Represents the objective function value; Indicates the length of the prediction time domain; Indicates the length of the control time domain; Indicates in Time prediction The pose error vector at time t; Represents the state error weight matrix; Indicates the increment of the control input; This represents the control increment weight matrix; This represents the terminal weight matrix.

[0083] The third step is constraint handling. During the optimization process, the constraints defined by formula (24) are strictly applied. In particular, the approach cone constraint is applied to ensure that the robot always stays within the allowable angle range of the oil intake port normal during the approach process.

[0084] (twenty four); in, Represents the set of state constraints; Represents the set of control input constraints; This represents a set of constraints that are close to a cone.

[0085] Step 4: Control the output. Take the first control variable of the optimized sequence. Send to the robot actuator.

[0086] S104-4: Near-field phase: Image-based visual servoing (IBVS) combined with MPC.

[0087] When the distance is less than the threshold and the system enters the close-range phase, the control loop switches to the IBVS mode based on image features, but the overall optimization is still handled by the MPC framework.

[0088] Error calculation (IBVS), specifically, includes: The state error at this point is no longer directly calculated using 3D pose difference, but is defined as image feature error. Interaction matrix The calculation relies on formula (25), where for each feature point, an interaction matrix is ​​constructed based on its current normalized coordinates and depth.

[0089] (25); in, Image interaction matrix representing single-point features; This represents the depth value of the feature point in the camera coordinate system; , This represents the coordinates of the feature point on the camera normalization plane.

[0090] All feature points Stacking yields the total interaction matrix The traditional IBVS velocity law is described by formula (26). In this invention, this relationship is embedded in the prediction model of MPC to describe the sensitivity of image features to changes in control variables.

[0091] (26); in, Represents the velocity vector of the camera (or robot end effector); Indicates the gain coefficient; Representing the interaction matrix The Moore-Penrose pseudo-inverse; This represents the image feature error vector.

[0092] MPC rolling optimization (IBVS mode), specifically includes: In this mode, the prediction model (the logic of formulas (21) and (22)) is based on the image Jacobian matrix (i.e., the interaction matrix). ) Construction, state variables This is transformed into an image feature error vector. The error term in the objective function (the logic of formula (23)) This represents the error in the predicted future image features. The constraints (the logic of formula (24)) are primarily examined in terms of state constraints. This ensures that the feature points in the predicted trajectory are always within the camera's field of view. By solving this optimization problem, MPC calculates the optimal robot motion speed, explicitly handles the singularity problem that may be caused by pseudo-inverses in formula (26), and avoids the loss of feature points.

[0093] S104-5: Iterative loop.

[0094] After executing the control commands for any of the above modes, the time step... Increment by 1, the system returns to step 1 to reacquire images and calculate pose, until the pose error is resolved. Converge to the allowable range and complete the docking task.

[0095] Through the above process, formulas (15) to (18) provide the basis for perception, formulas (19) to (20) define the error measurement in the PBVS stage, and formulas (21) to (24) constitute the MPC optimization framework in the PBVS mode. In the IBVS stage, formulas (25) to (26) define the image interaction relationship and are also substituted into the MPC optimization framework (reusing the logical structure of formulas (21) to (24), but with different state definitions), thus realizing a seamless and smooth switch from coarse positioning to fine docking.

[0096] Figure 8 A vision servo control system driven by oil port pose perception is shown, comprising: The view frustum point cloud generation unit 801 is configured to: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, map the target detection box to the three-dimensional space to generate the view frustum region by combining the camera intrinsic parameters and projection matrix, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. The semantic enhancement instance segmentation unit 802 is configured to: input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with an embedded self-attention mechanism, capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance; The residual compensation pose regression unit 803 is configured to: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use a transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. The hybrid visual servo drive unit 804 is configured to: calculate the pose error between the current pose and the desired pose based on the target pose data; when the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error; otherwise, perform image-based visual servo control.

[0097] It is understood that the aforementioned units can be individually or entirely merged into one or more other units, or some of the units can be further divided into multiple functionally smaller units. This achieves the same operation without affecting the technical effects of the embodiments of the present invention. The aforementioned units are based on logical functional division. In practical applications, the function of one unit can be implemented by multiple units, or the function of multiple units can be implemented by one unit. In other embodiments of the present invention, the system may also include other units. In practical applications, these functions can also be implemented with the assistance of other units, and can be implemented collaboratively by multiple units.

[0098] According to another embodiment of the present invention, the system of this embodiment can be constructed by running a computer program (including program code) capable of performing the steps involved in the corresponding method of the present invention on a general-purpose computing device, such as a computer, which includes processing elements and storage elements such as a central processing unit (CPU), random access memory (RAM), and read-only memory (ROM). The computer program can be recorded on, for example, a computer-readable recording medium, loaded into the aforementioned computing device through the computer-readable recording medium, and run therein.

[0099] Figure 9 A computer device is shown, which includes a processor 901, a communication interface 902, and a computer-readable storage medium 903. The processor 901, communication interface 902, and computer-readable storage medium 903 can be connected via a bus or other means.

[0100] The communication interface 902 is used to receive and send data. The computer-readable storage medium 903 can be stored in the memory of the electronic device. The computer-readable storage medium 903 is used to store computer programs, which include program instructions. The processor 901 is used to execute the program instructions stored in the computer-readable storage medium 903.

[0101] The processor 901 is the computing and control core of electronic devices. It is suitable for implementing one or more instructions, specifically for loading and executing one or more instructions to achieve corresponding methods or functions.

[0102] Processor 901 is configured to perform the following procedure: View frustum point cloud generation steps: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, combine the camera intrinsic parameters and projection matrix to map the target detection box to the three-dimensional space to generate the view frustum region, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. Semantic Enhancement Instance Segmentation Steps: Input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with embedded self-attention mechanism. Capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance. The residual compensation pose regression steps are as follows: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use the transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. Hybrid visual servo drive steps: Calculate the pose error between the current pose and the desired pose based on the target pose data. When the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error; otherwise, perform image-based visual servo control.

[0103] This invention also provides a computer-readable storage medium, which is a memory device in an electronic device for storing programs and data. It is understood that the computer-readable storage medium here may include both built-in storage media in the electronic device and extended storage media supported by the electronic device. The computer-readable storage medium provides storage space for storing the processing system of the electronic device.

[0104] Furthermore, this storage space also contains one or more instructions suitable for loading and execution by the processor. These instructions can be one or more computer programs (including program code). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory; alternatively, it can also be at least one computer-readable storage medium located remotely from the aforementioned processor.

[0105] In one embodiment, the computer-readable storage medium stores one or more instructions; the processor loads and executes the one or more instructions stored in the computer-readable storage medium to perform the following process: View frustum point cloud generation steps: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, combine the camera intrinsic parameters and projection matrix to map the target detection box to the three-dimensional space to generate the view frustum region, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. Semantic Enhancement Instance Segmentation Steps: Input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with embedded self-attention mechanism. Capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance. The residual compensation pose regression steps are as follows: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use the transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. Hybrid visual servo drive steps: Calculate the pose error between the current pose and the desired pose based on the target pose data. When the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error; otherwise, perform image-based visual servo control.

[0106] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can implement the described functions using different methods for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0107] In the above embodiments, implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, as a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the flow or function according to the embodiments of the present invention is generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in or transmitted through a computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic cable, digital cable) or wireless (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that a computer can access or a data processing device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid-state drive), etc.

[0108] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A visual servo control method driven by oil port pose perception, characterized in that, Includes the following processes: View frustum point cloud generation steps: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, combine the camera intrinsic parameters and projection matrix to map the target detection box to the three-dimensional space to generate the view frustum region, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. Semantic Enhancement Instance Segmentation Steps: Input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with embedded self-attention mechanism. Capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance. The residual compensation pose regression steps are as follows: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use the transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. Hybrid visual servo drive steps: Calculate the pose error between the current pose and the desired pose based on the target pose data. When the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold, perform pose-based visual servo control based on the pose error. Otherwise, perform image-based visual servo control.

2. The visual servo control method driven by oil port pose perception as described in claim 1, characterized in that, The training process employs a multi-task loss function for optimization, which is expressed as: ； in, This represents the total loss of the network; The improved PointNet network instance segmentation loss; and These are the weight parameters in the loss function. and These are the centroid regression losses for the Hole-Net network and the 3D bounding box prediction network, respectively. and These represent the classification loss and regression loss for the 3D bounding box, respectively. and These are the classification loss and regression loss for the heading angle, respectively; Corner loss for 3D bounding box prediction.

3. The visual servo control method driven by oil port pose perception as described in claim 2, characterized in that, The total loss after adding an elastic network regularization term and a weight decay term to the multi-task loss function is expressed as follows: ； in, The total loss after adding the weight decay term. This refers to the multitasking loss before regularization. The regularization coefficient is . This is the regularization scaling parameter. The L1 regularization term is used to push the weights toward zero. The L2 regularization term is used to constrain weight smoothing. This represents the learnable weight parameters in a neural network.

4. The visual servo control method driven by oil port pose perception as described in claim 1, characterized in that, In the semantically enhanced instance segmentation step, the computation process of the self-attention mechanism includes: A linear transformation is performed on the input feature sequence to generate a query vector, a key vector, and a value vector, wherein the query vector, key vector, and value vector are all obtained by multiplying a learnable parameter matrix with the input features; Calculate the attention score between points, where the attention score is the product of the query vector and the transposed key vector divided by a scaling factor; The attention score is normalized using the Softmax function to obtain the attention weights, and the attention weights are weighted and summed with the value vector to obtain the output feature representation.

5. The visual servo control method driven by oil port pose perception as described in claim 1, characterized in that, In the semantically enhanced instance segmentation step, a multi-head attention mechanism is used to learn the relevance of different subspaces in parallel. The output of the attention head is determined by the first... The projection matrix corresponding to the first attention head and the first attention head The enhanced feature is obtained by multiplying the attention outputs of each attention head and then concatenating the outputs of all attention heads and linearly transforming the final output through a projection matrix.

6. The visual servo control method driven by oil port pose perception as described in claim 1, characterized in that, In the residual compensation pose regression step, the transformation network is used to predict the residual between the true centroid and the origin of the mask coordinate system. The three-dimensional bounding box prediction network outputs the length, width, height, centroid coordinates, and heading angle of the bounding box through a fully connected layer. The residual method is used to fuse the residual with the centroid coordinates to obtain the accurate target center point.

7. The visual servo control method for oil port pose perception driven as described in any one of claims 1-6, characterized in that, Extract target feature points from the current image and calculate the image feature error between them and the desired image features corresponding to the desired pose state. Use the image feature error to perform image-based visual servo control.

8. A vision servo control system driven by oil port pose perception, characterized in that, include: The view frustum point cloud generation unit is configured to: acquire the original image and original point cloud of the scene to be docked, extract the target detection box in the original image using a two-dimensional target detection model, map the target detection box to the three-dimensional space to generate the view frustum region by combining the camera intrinsic parameters and projection matrix, and filter out noise points located outside the view frustum region in the original point cloud to obtain the view frustum point cloud. The semantically enhanced instance segmentation unit is configured to: input the view frustum point cloud and the semantic category vector generated based on the target detection box into the point cloud segmentation network with an embedded self-attention mechanism, capture the local geometric features of the point cloud through the interactive calculation of query vector, key vector and value vector, and output a point cloud mask containing the target instance; The residual compensation pose regression unit is configured to: establish a mask coordinate system with the centroid of the point cloud mask as the origin, use a transformation network to predict the residual vector of the real centroid relative to the origin, and combine the size parameters and heading angle parameters output by the 3D bounding box prediction network to transform the view frustum point cloud from the mask coordinate system to the real coordinate system to obtain the target pose data. The hybrid vision servo drive unit is configured to: calculate the pose error between the current pose and the desired pose based on the target pose data; and perform pose-based vision servo control based on the pose error when the distance between the working end of the oil-taking robot and the oil-taking port is greater than a set threshold. Otherwise, perform image-based visual servo control.

9. A computer device, characterized in that, include: Processor and computer-readable storage media; A processor, adapted to execute computer programs; A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the visual servo control method for oil port pose perception driving as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program adapted to be loaded by a processor and executed by the visual servo control method for oil port pose perception drive as described in any one of claims 1 to 7.

Images