Pose positioning method for filling port of canning vehicle and storage medium

Abstract

The invention provides a canning vehicle filling port pose positioning method and a storage medium. The method comprises the steps that image data and initial point cloud of a canning vehicle filling port are acquired; constructing a visual detection model according to the image data, and performing coarse positioning on the filling port to obtain a rough pose of the filling port; carrying out segmentation processing on the initial point cloud according to the filling port physical model and the rough pose to obtain local point cloud data only containing the filling port; performing filtering processing on the local point cloud data according to the adaptive filtering threshold function to obtain target point cloud data with the optimal density; performing edge point feature extraction on the target point cloud data with the optimal density to obtain an edge feature point set; clustering the edge feature point set, and constructing a triangular descriptor of a clustering set; pairing the triangular descriptors, and performing coarse registration on the obtained descriptor pair set to obtain a coarse transformation pose; and carrying out fine registration on the rough transformation pose to obtain the accurate pose of the filling port of the canning vehicle.

Description

Technical Field

[0001] The invention belongs to the field of automatic filling of tank trucks, and specifically designs a method for positioning the filling port of a tank truck and a storage medium. Background Art

[0002] At present, the methanol filling process of tank trucks is completely done manually, which mainly includes aiming the filling gun at the filling port to start filling, paying attention to whether the methanol is full and closing the valve to retract the filling gun. At the same time, during the filling process, it is necessary to always pay attention to whether there are accidents such as slipping and methanol leakage, which is both time-consuming and labor-intensive. In addition, methanol, as a chemical substance, is highly toxic and flammable, which can easily cause great harm to human body and property, and has safety hazards.

[0003] So far, there are few studies on the positioning of methanol filling ports, and there is no mature positioning solution. In the existing technology, there are generally problems such as low accuracy, poor robustness, conservative speed in the positioning of methanol filling ports, as well as safety issues in the methanol filling process of tank trucks. Summary of the invention

[0004] In view of the shortcomings of the existing technology, the present invention provides a method and storage medium for positioning the filling port of a tank truck, which can solve the problems of low accuracy, poor robustness, and conservative speed in the positioning of the methanol filling port, and improve the safety of the methanol filling process of the tank truck. Depth cameras have many advantages. They can output 2D images and 3D point cloud data at the same time, which helps to improve the accuracy and robustness of the positioning of the methanol filling port. At the same time, some cameras have the advantages of explosion-proof performance and low power consumption, which can be used to complete the positioning task of the filling port of the tank truck.

[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is: a method for positioning the filling port of a tank truck, comprising the following steps:

[0006] Obtain image data and initial point cloud of the tank truck filling port;

[0007] According to the image data of the tank truck filling port, a visual detection model is constructed, and the tank truck filling port is roughly positioned by the visual detection model to obtain an approximate three-dimensional position and posture of the tank truck filling port;

[0008] Segmenting the initial point cloud according to the physical model of the filling port and the approximate three-dimensional pose to obtain local point cloud data containing only the filling port;

[0009] Performing filtering processing on the local point cloud data according to an adaptive filtering threshold function to obtain target point cloud data with optimal density;

[0010] Based on the smoothness, edge point features are extracted from the target point cloud data with the optimal density to obtain an edge feature point set;

[0011] Clustering the edge feature point set using a Euclidean clustering method to obtain a cluster set;

[0012] Constructing a triangle descriptor of the cluster set according to the stability of the triangle;

[0013] Pairing the triangle descriptors according to a feature association algorithm and a similarity scoring function to obtain a descriptor pair set, and coarsely registering the descriptor pair set to obtain a target descriptor pair and a coarse transformation pose of the target descriptor pair;

[0014] The coarse transformed poses of the target descriptor pair are precisely aligned using a nonlinear optimization function to obtain the precise pose of the tank truck filling port.

[0015] Furthermore, a visual detection model is constructed based on the image data of the tank truck filling port, and the tank truck filling port is roughly positioned by the visual detection model to obtain a rough three-dimensional position and posture of the tank truck filling port, including:

[0016] Using a depth camera to obtain image data of the methanol filling port in real time; the image data includes information about the methanol filling port and the environment around the methanol filling port;

[0017] The acquired image data is preprocessed and feature extracted, and image data of different resolutions are combined and processed using multi-scale information fusion technology to obtain a training data set;

[0018] The training data set is input into the yolov5 model based on temporal information learning and attention mechanism for training to obtain the yolo visual detection model;

[0019] The environmental image of the filling port is input into the YOLO visual detection model for target detection and feature extraction, and the two-dimensional pixel coordinates of the center point of the filling port are output; and the two-dimensional pixel coordinates of the center point are converted into the approximate three-dimensional pose of the tank truck filling port.

[0020] Furthermore, the two-dimensional pixel coordinates of the center point are converted into a rough three-dimensional pose of the tanker filling port, including:

[0021] Obtain the 2D pixel coordinates of the center point of the tanker filling port from the visual inspection model;

[0022] According to the depth map collected by the depth camera, the pixel value of the two-dimensional pixel coordinates of the center point of the filling port is obtained, and the depth value of the center of the filling port of the tank truck is obtained through the affine function;

[0023] According to the depth value, the approximate three-dimensional position and posture of the filling port of the tank truck in the camera coordinate system is obtained using the camera three-dimensional projection model.

[0024] Furthermore, the initial point cloud is segmented according to the physical model of the filling port and the approximate coordinates to obtain local point cloud data containing only the filling port, including:

[0025] According to the actual physical model of the tank truck filling port, it is modeled as a cylindrical object with fixed size, and the physical parameters and the filling port contour model of the tank truck filling port are obtained based on the MSAC fitting algorithm;

[0026] With the approximate three-dimensional pose as the center and the filling port contour model as the outline, the initial point cloud is segmented to obtain a local point cloud containing only the methanol filling port.

[0027] Further, filtering the local point cloud data according to an adaptive filtering threshold function to obtain target point cloud data with optimal density includes:

[0028] According to the depth map collected by the camera, the pixel value at the center of the filling port is obtained, and the depth value of the center of the filling port of the tank truck is obtained through the affine function;

[0029] An adaptive filtering threshold function is constructed according to the depth value to obtain the optimal voxel filtering parameters;

[0030] The local point cloud data is downsampled according to the optimal voxel filtering parameters to obtain the target point cloud data with the optimal density.

[0031] Further, edge point features are extracted from the target point cloud data of the optimal density based on smoothness to obtain an edge feature point set; the edge feature point set is clustered using a Euclidean clustering method to obtain a cluster set; and a triangle descriptor of the cluster set is constructed according to the stability of the triangle, including:

[0032] Select 5 points before and after each point in the target point cloud data with the optimal density as the basis, calculate the curvature of each point in the target point cloud data with the optimal density, and filter out the edge feature point set according to the smoothness threshold;

[0033] Constructing a functional relationship between a clustering threshold and an optimal voxel filtering parameter, and clustering the edge feature point set using a Euclidean clustering algorithm to obtain a cluster set;

[0034] Get the coordinates of the center point of each cluster, get the direction vector of each cluster through PCA principal component analysis, project the coordinates of the center point of each cluster onto the direction vector, get the interval between two adjacent points, and get the length value of each cluster;

[0035] Traverse the cluster set, search for the five nearest neighbor clusters of each cluster, traverse all the pairwise non-repeating combination clusters from the nearest neighbor clusters and use the cluster as the vertex to build a stable triangle descriptor, and at the same time remove the cluster from the cluster set.

[0036] Furthermore, the triangle descriptor includes length information of three vertices, direction vector information and three edge length information.

[0037] Further, the triangle descriptors are paired according to a feature association algorithm and a similarity scoring function to obtain a descriptor pair set, and the descriptor pair set is roughly aligned to obtain a target descriptor pair and a rough transformation pose of the target descriptor pair, including:

[0038] In the first phase, the descriptor set is traversed, and a hash table is constructed with the sum of the side lengths of the descriptors as the key value and the triangle descriptor as the target value;

[0039] Traverse the length clustering set of the target point cloud, query the number of triangle descriptors whose length difference is within the interval in the hash table of the source point cloud, and get the potential descriptor pair set. The number is the first-stage score of each triangle descriptor, and sort the scores from large to small to get the score set and the total descriptor pair set;

[0040] According to the total number of triangle descriptors, the top 20% of the descriptors are retained to enter the second stage for feature association, and the other descriptors are removed from the set to obtain a new set of descriptor pairs;

[0041] Traverse the new set of descriptor pairs, and for a set of paired descriptors, subtract the vertex length information, direction vector information, and three edge length information of the two to obtain the vertex length difference, direction vector difference, and edge length difference;

[0042] For each paired descriptor, determine whether it meets the condition that the three differences are all less than the threshold. If not, remove it. Finally, retain the descriptor pair corresponding to the minimum sum of the three differences in each paired descriptor set;

[0043] Traverse the set of retained descriptor pairs, calculate the pose transformation between each pair of descriptors, perform pose transformation on the remaining descriptor pairs, and calculate whether the distance between the three vertices of the transformed descriptor is within the threshold range. If the threshold requirement is met, the score is increased by 1, and finally a score collection is obtained. The descriptor pair corresponding to the maximum score is the target descriptor pair, and the corresponding pose transformation is the coarse transformation pose.

[0044] Furthermore, a nonlinear optimization function is used to perform fine registration on the coarse transformed pose of the target descriptor pair to obtain the precise pose of the tank truck filling port, including:

[0045] Transforming the target point cloud according to the coarse transformation posture, and establishing a least squares relationship between the transformed target point cloud and the source point cloud;

[0046] The Gauss-Newton method is used to solve the least squares relationship and obtain high-precision transformation posture;

[0047] The product of the high-precision transformed posture and the source point cloud posture is the precise posture of the filling port of the tank truck.

[0048] On the other hand, the present application also provides a computer storage medium storing executable program code; the executable program code is used to execute the above-mentioned method for positioning the filling port of a tank truck.

[0049] Compared with the prior art, the advantages and positive effects of the present invention are:

[0050] 1) The present invention provides a method for positioning the filling port of a tank truck, which integrates 3D point cloud and 2D image data to realize the filling positioning of the tank truck, and can be applied to the technical field of automatic filling of tank trucks. The method of the present invention makes the positioning of the methanol filling port fast, accurate and robust, and can greatly improve the safety of the methanol filling process of the tank truck.

[0051] 2) The present invention provides a method for positioning the filling port of a tank truck, which uses a depth camera to acquire image data of a target area in real time, including the surrounding environment information and potential risk factors of the methanol filling port; preprocesses the acquired image, including noise removal, missing value filling, and image denoising, to ensure the accuracy and reliability of subsequent processing; extracts the features of the methanol filling port, including shape, color, texture, etc., and analyzes the surrounding environment features to achieve effective recognition and positioning of the filling port; combines image data of different resolutions, adopts multi-scale information fusion technology, and comprehensively utilizes image and point cloud information to improve the robustness and accuracy of methanol filling port detection; adds temporal information learning and attention mechanism to the training process of the yolov5 model, learns and pays attention to the visual features of different time steps, so as to improve the recognition speed and cope with factors such as light changes and interference from debris, so as to improve the robustness and adaptability of the filling port detection system; realizes the visual coarse positioning of the methanol filling port through target detection and feature extraction technology, and converts its coordinate information into a data format that can be recognized by subsequent point cloud registration.

[0052] 3) The present invention provides a method for positioning the position of a tank truck filling port. By combining the physical model of the filling port and the visual positioning result, a target point cloud segmentation algorithm suitable for the methanol filling port is designed to obtain a local point cloud containing only the filling port, thereby reducing the interference caused by redundant point clouds. At the same time, an adaptive filtering threshold function considering the depth distance is designed to avoid the adjustment of filtering parameters, thereby achieving optimal processing of the target point cloud and accelerating the 3D point cloud processing speed while ensuring accuracy.

[0053] 4) The present invention provides a method for positioning the filling port of a tank truck, extracts edge points based on the smoothness of the point cloud, uses edge points with greater information content to perform point cloud registration tasks, and obtains multiple edge clusters based on the Euclidean clustering method; a triangle descriptor is proposed with the help of the stability idea of ​​the triangle, and the descriptor has spatial and temporal invariance; at the same time, a two-stage feature association algorithm and a similarity scoring function are designed for pairing of triangle descriptors, which accelerates the pairing process of the descriptors and improves the success rate of pairing.

[0054] In addition to the above-described purposes, features and advantages, the present invention has other purposes, features and advantages. The present invention will be further described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0056] Figure 1 This is a flow chart of a method for positioning the filling port of a tank truck according to an embodiment of the present invention;

[0057] Figure 2 This is a partial image data collection diagram of an embodiment of the present invention;

[0058] Figure 3 This is a diagram of visual inspection results according to an embodiment of the present invention;

[0059] Figure 4 A schematic diagram of point cloud data according to an embodiment of the present invention;

[0060] Figure 5 A local point cloud image after target point cloud segmentation according to an embodiment of the present invention;

[0061] Figure 6 This is a point cloud registration result diagram of an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0063] The present invention provides a method for positioning the filling port of a tank truck, which may be, but is not limited to, the positioning of the filling port of a methanol tank truck, and specifically comprises the following steps:

[0064] Step 101: Obtain image data and initial point cloud of the tank truck filling port.

[0065] Specifically, a depth camera may be used to obtain image data of a target area in real time; the target area preferably includes the methanol filling port and its surroundings, so as to obtain environmental information of the methanol filling port and its surroundings and identify the position and potential risk factors of the filling port. The specific implementation process is as follows:

[0066] S1.1. Use the onboard visual sensor device (such as a high-resolution camera) to perceive the environment around the robot and obtain high-definition environmental image information of the location of the filling port, which is stored as img_environment.

[0067] S1.2. The obtained environment image may include various environmental features such as the filling port, surrounding vegetation, buildings, terrain, etc., with complex scene composition and rich information content, which is stored as the variable environment_features.

[0068] S1.3. Use visual sensors to perform all-round scanning and real-time monitoring of the environment around the robot to obtain accurate information on the location of the refueling port, and feed it back to the system in real time and store it as the variable refueling_port_location.

[0069] S1.4. Use advanced image processing algorithms and deep learning technology to perform complex scene analysis and target detection on the acquired environmental images to identify the exact location of the filling port in the environment and store it as the variable target_detection_results.

[0070] S1.5. In the environment image where the refueling port is located, the refueling port is accurately located through target detection and feature extraction technology, and its coordinate information is converted into a data format recognizable by the robot and stored as a variable refueling_port_coordinates.

[0071] Step 102: construct a visual detection model based on the image data of the tank truck filling port, and roughly locate the tank truck filling port through the visual detection model to obtain an approximate three-dimensional position and posture of the tank truck filling port.

[0072] Specifically, the image data of the methanol filling port is obtained in real time using a depth camera; the obtained image data is preprocessed and feature extracted, and combined with image data of different resolutions, multi-scale information fusion technology is used for processing to obtain a training data set;

[0073] The training data set is input into the yolov5 model based on temporal information learning and attention mechanism for training to obtain the yolo visual detection model;

[0074] The environmental image of the filling port is input into the YOLO visual detection model for target detection and feature extraction, and the two-dimensional pixel coordinates of the center point of the filling port are output; and the two-dimensional pixel coordinates of the center point are converted into the approximate three-dimensional pose of the tank truck filling port.

[0075] The conversion of the two-dimensional pixel coordinates of the center point into the approximate three-dimensional pose of the tanker filling port includes:

[0076] Obtain the 2D pixel coordinates of the center point of the tanker filling port from the visual inspection model;

[0077] According to the depth map collected by the depth camera, the pixel value of the two-dimensional pixel coordinates of the center point of the filling port is obtained, and the depth value of the center of the filling port of the tank truck is obtained through the affine function;

[0078] According to the depth value, the approximate three-dimensional position and posture of the filling port of the tank truck in the camera coordinate system is obtained using the camera three-dimensional projection model.

[0079] Furthermore, the acquired image is preprocessed, including noise removal, missing value filling, and image denoising. The specific implementation process is as follows:

[0080] S2.1, Image denoising: First, the acquired image information is denoised using a Gaussian filter or a median filter to reduce interference and noise in the image. Specifically, for an image I, the denoising operation can be expressed as I_denoised = GaussianBlur (I, θ), where θ is the standard deviation of the Gaussian kernel.

[0081] S2.2, Edge Enhancement: Next, edge enhancement technology is used to process the image to highlight the edge features in the image. Common edge enhancement methods include Sobel operator, Prewitt operator, Canny edge detection, etc., which can effectively detect edge information in the image. Taking the Sobel operator as an example, the edge strength calculation formula in the horizontal direction is:

[0082]

[0083] S2.3. Other processing: Finally, some other preprocessing operations are performed, such as adjustment of image brightness and contrast, conversion of color space, etc., to further optimize image quality and improve the detection effect of the filling port.

[0084] Furthermore, deep learning technology is used to extract the features of the methanol filling port, including shape, color, and texture, and the surrounding environment features are analyzed to achieve effective identification and positioning of the filling port; the specific implementation process is as follows:

[0085] S3.1. Feature extraction: Use image processing algorithms to extract features from preprocessed images. This includes edge detection, corner detection, color histogram calculation, and other operations. Let the image be I, the edge image be E, the corner image be C, and the color histogram be H(I), then it can be expressed as:

[0086]

[0087] Among them, edgeDetection, cornerDetection and colorHistogram represent the functions of edge detection, corner detection and color histogram calculation respectively.

[0088] S3.2, Feature Analysis: Further analyze the extracted features. By calculating the correlation between the edge image E, the corner image C and the color histogram H(I), more abundant feature information can be obtained. Suppose the correlation calculation function is corr(A,B), it can be expressed as:

[0089]

[0090] Among them, R EC represents the correlation between edge image and corner image, R EH represents the correlation between the edge image and the color histogram, R CH Represents the correlation between the corner image and the color histogram.

[0091] S3.3, Target positioning: Target positioning is performed based on the extracted feature information. By comprehensively considering the correlation between edge images, corner images and color histograms, the machine learning model is combined to perform target recognition and positioning. Let the machine learning model be f(·), then it can be expressed as: Position = f(E, C, H(I))

[0092] Where Position represents the position of the filling port, and f(·) represents the machine learning model.

[0093] Furthermore, by combining image data of different resolutions, using multi-scale information fusion technology, and comprehensively utilizing image and point cloud information, the robustness and accuracy of methanol filling port detection can be improved; the specific implementation process includes:

[0094] S4.1. Resolution unification and calibration: The image data obtained by the explosion-proof camera shall be unified and calibrated in resolution to ensure the comparability and compatibility of the data at the same scale.

[0095] S4.2. Feature weighting and combination: According to the importance and reliability of different features, the features are weighted and combined to improve the robustness and accuracy of the methanol filling port detection algorithm.

[0096] Furthermore, temporal information learning and attention mechanism are added to the yolov5 model training process to learn and focus on the visual features of different time steps, so as to improve the recognition speed and cope with factors such as light changes and interference from debris, thereby improving the robustness and adaptability of the filling port detection system; the specific implementation process is as follows:

[0097] S5.1. In the training data, the temporal information of the image data is extracted by means of timestamps or sequence numbers, so as to introduce temporal learning in the model training process.

[0098] S5.2. Design a suitable time series feature representation method to combine time series information with image features to form an input data format suitable for model learning.

[0099] S5.3. Design a time series model structure based on time series features and use LSTM as the model structure for time series data to achieve learning and representation of visual features at different time steps.

[0100] S5.4. Introducing the attention mechanism into the timing model enables the model to dynamically focus on and learn important information at different time steps, so as to improve the adaptability of the filling port detection system to factors such as light changes and interference from debris.

[0101] S5.5, Model training and optimization: Based on the designed timing model and attention mechanism, the training data is trained, and the parameters and structure of the yolov5 model are continuously optimized to achieve accurate recognition and positioning of the filling port.

[0102] S5.6. Performance evaluation and adjustment: Perform performance evaluation on the trained YOLO visual inspection model, adjust and optimize the model based on the evaluation results, and ensure the robustness and adaptability of the filling port inspection system under different environmental conditions.

[0103] Furthermore, the filling port is roughly located through target detection and feature extraction technology, and its coordinate information is converted into a data format that can be recognized by subsequent point cloud registration; the specific implementation process is as follows:

[0104] S6.1. Use the methanol filling port target detection yolov5 algorithm to detect the environmental image where the filling port is located to ensure that the position of the filling port in the image can be accurately located.

[0105] S6.2. Combining the target detection and feature extraction results, the positioning algorithm is used to accurately locate the filling port, determine its exact position in the environmental image, and calculate its coordinate information.

[0106] S6.3, Data format conversion: Convert the coordinate information of the filling port to Coordinate center (u0,v0) is converted into a data format that can be recognized by the subsequent point cloud registration algorithm to ensure that the filling port position information can be accurately recognized and processed by the subsequent point cloud registration algorithm.

[0107] Step 103, segmenting the initial point cloud according to the physical model of the filling port and the approximate three-dimensional pose to obtain local point cloud data containing only the filling port; filtering the local point cloud data according to an adaptive filtering threshold function to obtain target point cloud data with optimal density.

[0108] Among them, the initial point cloud is segmented according to the physical model of the filling port and the approximate three-dimensional posture to obtain local point cloud data containing only the filling port, including: first, according to the actual physical model of the tank truck filling port, it is modeled as a cylindrical object of fixed size, and the physical parameters and the filling port contour model of the tank truck filling port are obtained based on the MSAC fitting algorithm; then, the initial point cloud is segmented with the approximate three-dimensional posture as the center and the filling port contour model as the contour to obtain a local point cloud containing only the methanol filling port.

[0109] The local point cloud data is filtered according to an adaptive filtering threshold function to obtain target point cloud data with optimal density, including: first, obtaining the pixel value at the center of the filling port according to the depth map collected by the camera, and obtaining the depth value of the center of the filling port of the tank truck through an affine function; then constructing an adaptive filtering threshold function according to the depth value to obtain the optimal voxel filtering parameter; finally, performing a voxel downsampling operation on the local point cloud data according to the optimal voxel filtering parameter to obtain the target point cloud data with optimal density.

[0110] Specifically, after the camera reaches the target area, it collects the point cloud data of the tank truck filling port, designs a target point cloud segmentation algorithm that combines the physical model of the filling port with the visual positioning result, and designs an adaptive filtering threshold function with depth distance as a factor to perform voxel filtering on the target point cloud; the specific implementation process is as follows:

[0111] S7.1. Obtain the pixel coordinates of the center of the tanker filling port from the visual recognition stage center (u0,v0), get the Coordinate according to the depth map collected by the camera center The pixel value at the tanker is obtained by applying an affine function to obtain the depth value z at the center of the tanker filling port. center , using the camera 3D projection model to obtain the approximate 3D pose T in the camera coordinate systemcenter (x center ,y center ,z center ), the relevant formula is as follows:

[0112]

[0113] Among them, c x ,c y ,f x ,f y is the camera internal parameter.

[0114] S7.2. According to the actual physical model of the methanol filling port, it can be modeled as a cylindrical object of fixed size. The physical parameters of the tanker filling port {radius: r; cylinder axis vector: (a center ,b center ,c center ); Cylinder height: H center} and the filling port contour model model;

[0115] S7.3, with posture T center The initial point cloud is segmented by fitting the model model as the contour to obtain a local point cloud Pointcloud containing only the methanol filling port local ;

[0116] S7.4, according to the depth value z center Design an adaptive filter threshold function to calculate the optimal voxel filter parameter σ filter , the relevant calculation formula is as follows:

[0117]

[0118] Among them, aim is the ideal accuracy.

[0119] S7.5, according to σ filter For local point cloud Pointcloud local Perform voxel downsampling to obtain the target point cloud with optimal density. target ;

[0120] Furthermore, in step S7.2, the specific implementation process of obtaining the physical parameters of the tank truck filling port based on the MSAC fitting algorithm is as follows:

[0121] 1) Select a number of points on the cylindrical surface and fit the plane formed by these points A , and then get the unit normal vector n of the plane A, which is the normal vector of the point. Perform this operation for each point on the cylinder to obtain the unit normal vector of each point. Treat the unit normal vector of each point as a point P0, and fit these points again to obtain the plane normal vector n0, which is the initial value of the cylinder axis vector (a0, b0, c0);

[0122] 2) After obtaining the axis, perform coordinate transformation on the cylinder, transform the cylinder axis vector (a0, b0, c0) into a vector parallel to the Z axis of the camera coordinate system, so that the (x, y) coordinates of the points on the cylindrical surface form a plane circle. Then fit these points to obtain the center Center0, i.e. (x1, y1, z1) and radius r.

[0123] 3) The distance d from the cylindrical surface point to the cylindrical axis is always the radius. Therefore, according to the above initial value, the error equation f can be established to solve the filling port fitting model. The specific formula is as follows:

[0124]

[0125] Step 104: extract edge point features from the target point cloud data of the optimal density based on smoothness to obtain an edge feature point set; cluster the edge feature point set using the Euclidean clustering method to obtain a cluster set; and construct a triangle descriptor of the cluster set based on the stability of the triangle.

[0126] The specific implementation process is as follows:

[0127] S8.1. Select the target point cloud target The 5 points before and after each point in the target point cloud are used as the basis to calculate the target point cloud. target The curv of each point in is calculated according to the smoothness threshold Threshold smooth Filter out the edge feature point set Edge{Point1,Point2,...,Point n}, the specific calculation formula is as follows:

[0128]

[0129] Among them, S represents the set of 5 points before and after.

[0130] S8.2. Constructing clustering threshold cluster and the optimal voxel filter parameter σ filter The functional relationship between them is used to obtain the cluster set Clusters {cluster1(Point a ,Point b ,...), cluster2(Pointa ,Point b ,...),...,cluster m (Point a ,Point b ,...)}, the specific clustering threshold calculation formula is as follows:

[0131] Threshold cluster =5σ filter ;

[0132] S8.3. For the above cluster sets, calculate the coordinates of their respective center points (x center_cluster ,y center_cluster ), and obtain its main direction through PCA principal component analysis That is, the direction vector of each cluster. Project the points in each cluster point cloud onto the main direction and calculate the distance between the two farthest points to get the length value of each cluster. clusters {length1,length2,...,length m}, the specific calculation formula is as follows:

[0133]

[0134] Among them, Num represents the number of points in the cluster.

[0135] S8.4, traverse the cluster set Clusters and search each cluster cluster i The 5 nearest neighbor clusters near {cluster near_1 ,...,cluster near_5}, to traverse all pairwise non-repeating combinations of clusters and clusters from the nearest neighbor clusters i Construct a stable triangle descriptor Tri_desc for the vertex i {tri_desc1,tri_desc2,...,tri_desc l}, and Clusters i Remove the set, each triangle descriptor tri_desc i Contains the length information of three vertices {length_desc1, length_desc2, length_desc3} and direction vector information And three side length information {length_side1, length_side2, length_side3}.

[0136] Step 105: Pair the triangle descriptors according to a feature association algorithm and a similarity scoring function to obtain a descriptor pair set, and roughly align the descriptor pair set to obtain a target descriptor pair and a rough transformation pose of the target descriptor pair.

[0137] The specific implementation process is:

[0138] S9.1. The first stage traverses the descriptor set Tri_desc of the source point cloud i , to describe the sub-tri_desc i The sum of the side lengths sum_side i Key value, triangle descriptor tri_desc i Build a hash table Hash for the target value desc {(sum_side1:tri_desc1),(sum_side2:tri_desc2),…,(sum_side l :tri_desc l )};

[0139] S9.2. Traverse the length clustering set Sum_side_group of the target point cloud target , for the triangle descriptor tri_desc i_target , in the hash table Hash of the source point cloud desc_source The query length difference is in the interval (sum_side i_target -Threshold hash ,Sum_side i_target +Threshold hash ) The number of triangle descriptors within sum i , and get tri_desc i_target Potential descriptor pair set Target i {(tri_desc i_target ,tri_desc a_source ),(tri_desc i_target ,tri_desc b_source ),…}, quantity sum i That is, the first stage grade of each triangle descriptor i , and sort the scores from large to small to get the score set Grade{grade max ,…,grade min} and the total descriptor pair set Match sum {Match1,Match2,…,Match l};

[0140] S9.3. According to the total number of triangle descriptors, the top 20% of the descriptors are retained to enter the second stage for feature association, and the other descriptors are removed from the set to obtain Match. sum {Match1,Match2,...,Match k}, where k = 0.2*l;

[0141] S9.4. Traversing Match sum , for a pair of descriptors (tri_desc i_target ,tri_desc a_source ), the length information of the two vertices {length_desc1, length_desc2, length_desc3}, the direction vector information And the three side length information {length_side1, length_side2, length_side3} are subtracted to get the vertex length difference Δ desc , direction vector difference Δ n And the side length difference Δ side ;

[0142] S9.5. For each paired descriptor (tri_desc i_target ,tri_desc a_source ) to determine whether the three differences are all less than the threshold {Threshold desc Thereshold n ,Threshold side} condition, if it is not met, it will be removed, and finally each pairing descriptor set Match will be retained i The sum of the three differences Δ sum The descriptor pair corresponding to the minimum value (tri_desc min_target ,tri_desc min_source ), Δ sum The calculation formula is as follows:

[0143] Δ sum =Δ desc +Δ n +Δ side ;

[0144] S9.6. Traverse the set of retained descriptor pairs and calculate the pose transformation T between each pair of descriptors ij (R ij ,t ij ), for the remaining descriptors Tri_desc remain Conduct T ijTransformation, calculate whether the distance between the three vertices of the transformed descriptor is within the threshold Threshold dis If the threshold requirement is met, the vote is increased by 1, and the final score collection vote is obtained. sum {vote1,vote2,...,vote h}, the descriptor pair corresponding to the maximum score vote is the target descriptor pair (tri_desc tar_target ,tri_desc tar_source ), and the corresponding pose transformation T ij That is the coarse transformation pose value.

[0145] Step 106: Use a nonlinear optimization function to perform fine alignment on the coarse transformed pose of the target descriptor pair to obtain the precise pose of the tank truck filling port.

[0146] The specific implementation process is:

[0147] S10.1, according to the rough transformation posture value T ij Get the converted target point cloud Pointcloud` target ;

[0148] S10.2. Create target point cloud Pointcloud` target With the source point cloud Pointcloud source The least squares relationship between them is expressed as follows:

[0149]

[0150] S10.3. Solve the least squares function to obtain the high-precision transformation posture T`(R`,t`), and use T` to obtain the center posture T of the tank truck filling port target , the calculation formula is as follows:

[0151] T target =T`T source ;

[0152] Among them, T source is the pose of the source point cloud.

[0153] The present invention is explained below in conjunction with specific embodiments.

[0154] As attached Figure 1 As shown in FIG. 1 , a flowchart of a fast and high-precision method for positioning the filling port of a tank truck provided by an embodiment of the present invention can be divided into two main processes: visual detection and point cloud registration, and specifically includes the following steps:

[0155] Step 1: Use a depth camera to obtain real-time image data of the target area, including the surrounding environment information and potential risk factors of the methanol filling port, including the following steps:

[0156] Step 1: Use a high-resolution explosion-proof camera to sense the surrounding environment and obtain high-definition environmental image information of the location of the filling port for subsequent analysis and processing: Optimize image quality by adjusting parameters such as camera exposure time and focal length to further improve the efficiency and accuracy of image information acquisition. Use img_environment to store high-definition image information of the location of the filling port.

[0157] Step 2: The obtained environment image may include multiple environmental features of the filling port and its surroundings, with complex scene composition and rich information content, which is stored as the variable environment_features. In project practice, further exploration of image information analysis and processing methods can be conducted to extract and utilize more environmental features. For example, the filling port and various objects or areas around it can be marked and identified to achieve the location and identification of different objects.

[0158] Step 3: Perform a full-scale scan and real-time monitoring of the environment around the refueling port, accurately obtain the location information of the refueling port, and feed it back to the system in real time and store it as the variable refueling_port_location. Optimize the parameters and scanning logic of the explosion-proof camera to improve the accuracy and real-time performance of the refueling port location information. At the same time, an efficient data storage method can be used to store the real-time acquired location information as a variable.

[0159] Step 4: Perform complex scene analysis on the acquired environment image to identify the true value of the position of the filling port in the environment and store it as the variable target_scan_results.

[0160] Step 5: In the environment image where the refueling port is located, convert the refueling port coordinate information into a data format recognizable by the robot and store it as a variable refueling_port_coordinates.

[0161] Step 2: The images and image information obtained by the explosion-proof camera (a total of 400 sets of image acquisition and partial display of information data are shown below) Figure 2 ) for preprocessing, including noise removal, missing value filling, image denoising, etc.; including the following steps:

[0162] Step 1: First, the acquired image information is denoised using a Gaussian filter or a median filter to reduce interference and noise in the image. Specifically, for an image I, the denoising operation can be expressed as I_denoised = GaussianBlur (I, θ), where σ is the standard deviation of the Gaussian kernel, GaussianBlur is the Gaussian filter function, and I_denoised is the denoised image.

[0163] Step 2: Get the denoised image, which is stored as a variable and represented by the symbol I. Define the horizontal convolution kernel of the Sobel operator: G x ; Perform a horizontal convolution operation on image I and calculate the horizontal edge strength value: G x_dege =G x *I; define the vertical convolution kernel of the Sobel operator: G y ; Perform a vertical convolution operation on image I and calculate the vertical edge strength value: G y_edge =G y *I; Calculate the total edge strength of each pixel in the image: According to the total edge strength value G edge Perform edge enhancement on the image to make the edge features more prominent.

[0164] In this embodiment, G x , G y :

[0165]

[0166] Step 3: For some images, other preprocessing operations are performed according to the image data, such as image brightness (img_Brightness) and contrast adjustment, color space (RGB / HSV) conversion, etc., to optimize image quality and improve the subsequent filling port detection effect.

[0167] Step 3: Extract the features of the methanol filling port, including shape, color, and texture, and analyze the surrounding environment features to achieve effective identification and positioning of the filling port; including the following steps:

[0168] Step 1: First, further analyze and process the preprocessed image I to extract important feature information from the image. The process involves edge detection, corner detection, and color histogram calculation. The goal of edge detection is to identify edges and boundaries in an image. In this step, an image processing algorithm is used to perform edge detection on image I to generate an edge image E = edgeDetection(I), which contains edge information in the image. Next, corner detection is performed to identify prominent feature points in the image, which usually represent key locations or structures of objects. Through the corresponding algorithm, a corner image C = cornerDetection(I) is generated, which contains corner information in the image. Finally, a color histogram is calculated, which is a statistical analysis of the frequency of occurrence of various colors in the image, used to describe the color distribution of the image. By calculating the color histogram H(I) = colorHistogram(I) of image I, the distribution of various colors in the image can be obtained.

[0169] Among them, edgeDetection, cornerDetection and colorHistogram represent the functions of edge detection, corner detection and color histogram calculation respectively.

[0170] Step 2: By calculating the correlation between the edge image E, the corner image C and the color histogram H(I), more abundant feature information can be obtained. Let the correlation calculation function be corr(A,B).

[0171] Step 3: After calculating the correlation function, the correlation between the edge image, corner image and color histogram is comprehensively considered and combined with the machine learning model for target recognition and positioning. Let the machine learning model be f(·), then it can be expressed as: Position = f(E, C, H(I))

[0172] Where Position represents the position of the filling port, and f(·) represents the machine learning model.

[0173] Step 4: Combine image data with different resolutions, use multi-scale information fusion technology, and comprehensively utilize image and point cloud information to improve the robustness and accuracy of methanol filling port detection. This includes the following steps:

[0174] Step 1: First, for the image data obtained by the explosion-proof camera, record the original resolution information of each image. Subsequently, unify the resolution of all image data to ensure that they are compared at the same spatial scale. For example, adjust all images to a uniform resolution of 1920×1080 pixels. At the same time, perform camera calibration to determine the camera's internal parameters (such as focal length, principal point, etc.) and external parameters (such as camera position, posture, etc.). This can be achieved by photographing a calibration board or using a specific calibration tool. For example, the pixel coordinates obtained from the calibration board and the actual world coordinates are used to calculate the camera's internal and external parameters using a calibration algorithm. The camera's internal and external parameters are usually represented by an internal parameter matrix K and an external parameter matrix [R|t]. Among them, the internal parameter matrix K describes the camera's internal parameters, including focal length (f_x, f_y), principal point (c_x, c_y) and distortion parameters (k_1, k_2, p_1, p_2), etc., and is usually expressed as:

[0175]

[0176] In this embodiment, K:

[0177]

[0178] The extrinsic matrix [R|t] describes the external parameters of the camera, including the camera's rotation matrix R and translation vector t, which are used to describe the position and orientation of the camera in the world coordinate system.

[0179] The camera projection transformation formula can be expressed as:

[0180]

[0181] Ultimately, the calibration results obtained will be used in subsequent image processing and analysis to ensure the accuracy of the experimental data, and can effectively unify the resolution and calibrate the image data obtained by the explosion-proof camera.

[0182] Step 2: Weight and combine the image features obtained from the explosion-proof camera. Taking the edge image E, corner image C and color histogram H(I) obtained in step 3 as an example, the digital data obtained are as follows: the edge image E contains 1200 edge points, the corner image C identifies 25 corner points, and the color histogram H(I) contains 256 color channels. Next, according to the importance and reliability of different features, the features are weighted and combined to improve the robustness and accuracy of the detection algorithm. For example, linear weighting or nonlinear weighting can be used to weight edge features, corner features and color features to obtain a comprehensive feature vector. Assume that the weighted comprehensive feature vector is F = [0.6·E, 0.3·C, 0.1·H(I)], where the weight of each feature is adjusted according to its importance in the detection algorithm to obtain the weighted and combined feature vector F.

[0183] Step 5: Combine the parameters and data processing results obtained in steps 3 and 4, add temporal information learning and attention mechanism to the yolov5 model training process, learn and pay attention to the visual features of different time steps, so as to improve the recognition speed and cope with factors such as light changes and debris interference, so as to improve the speed and accuracy of the filling port detection system; including the following steps:

[0184] Steps 1, 2, and 3: For the training data, first extract the temporal information of the image data by recording the timestamp of image acquisition or assigning a sequence number to introduce the idea of ​​temporal learning. Subsequently, design a suitable temporal feature representation method to combine the temporal information with the image features to form an input data format suitable for model learning. This includes combining a series of continuous image frames into a temporal sequence, and combining each temporal sequence with the corresponding image features to construct a temporal feature representation. For the temporal features, the design uses a long short-term memory network (LSTM) as the model structure to achieve learning and representation of visual features at different time steps. As a recurrent neural network structure suitable for sequence data modeling, LSTM can effectively capture long-term dependencies in sequence data.

[0185] Step 4: Design a sequential filling port detection algorithm based on the attention mechanism. First, use a convolutional neural network (CNN) to extract features from each frame of the image to obtain an image feature sequence {x_1, x_2, ..., x_T}, where T is the length of the sequence. Design an attention model to dynamically calculate the attention weight of each time step so that the model can focus on the key time steps. Specifically, the SoftAttention mechanism can be used, and the formula for calculating the attention weight is as follows: e_t is the attention energy corresponding to the t-th time step, which can be calculated using a multi-layer perceptron (MLP) or a fully connected layer: t=MLP(x t ); Next, the image feature sequence is weighted and summed according to the attention weight to obtain the weighted feature representation: Finally, the weighted feature z is input into the LSTM model.

[0186] Step 5: Based on the designed timing model and attention mechanism, the training data is trained to continuously optimize the YOLOv5 model parameters and structure to achieve accurate identification and positioning of the filling port.

[0187] Step 6: Evaluate the performance of the trained YOLO visual inspection model, adjust and optimize the model based on the evaluation results, and ensure the robustness and adaptability of the filling port inspection system under different environmental conditions.

[0188] Step 6: Use target detection and feature extraction technology to roughly locate the filling port and convert its coordinate information into a data format that can be recognized by subsequent point cloud registration; including the following steps:

[0189] Step 1: Use the methanol filling port target detection algorithm to detect the environmental image where the filling port is located. The detection results are as follows: Figure 3 shown.

[0190] Step 2: Get the coordinates of the center point of the precise detection and rough positioning, in the txt document data format (including labels detection results and confidence levels). In this embodiment:

[0191] ('exit',0.385,0.523,0.98)('entrance',0.756,0.533,0.98)

[0192] Step 3: Share the detection data position_exit and position_entrance through a dynamic link library or file sharing. In this embodiment:

[0193] position_exit(-215.6,3.5,1417.5); position_entrance(203.7,7.3,1441.4).

[0194] Step 7: After the depth camera reaches the target area, it collects the point cloud data of the tanker filling port, as shown in the attached figure. Figure 4 Design a target point cloud segmentation algorithm that combines the physical model of the filling port with the visual positioning results, and design an adaptive filtering threshold function with depth distance as a factor to perform voxel filtering on the target point cloud:

[0195] Step 1: Obtain visual inspection results. The approximate coordinates of the filling port in the camera coordinate system are: (-215.6, 3.5, 1417.5), (203.7, 7.3, 1441.4) in this embodiment.

[0196] Step 2: According to the actual physical model of the methanol filling port, it can be modeled as a cylindrical object of fixed size, and the physical parameters of the tanker filling port {radius: r; cylinder axis vector: (a center ,b center ,c center ); Cylinder height: H center} and the filling port contour model model;

[0197] Step 2.1: Select a number of points on the cylindrical surface and fit the plane formed by these points A , and then get the unit normal vector n of the plane A , which is the normal vector of the point. Perform this operation for each point on the cylinder to obtain the unit normal vector of each point. Treat the unit normal vector of each point as a point P0, and fit these points again to obtain the plane normal vector n0, which is the initial value of the cylinder axis vector (a0, b0, c0);

[0198] In this embodiment, n0:

[0199] n0=(-0.000993-0.014894-0.999889)

[0200] Step 2.2: After obtaining the axis, perform coordinate transformation on the cylinder, transform the cylinder axis vector (a0, b0, c0) into a vector parallel to the Z axis of the camera coordinate system, so that the (x, y) coordinates of the points on the cylindrical surface form a plane circle. Then fit these points to obtain the center Center0, i.e. (x1, y1, z1) and radius r;

[0201] In this embodiment, the center of the circle is Center0 and the radius is r:

[0202] Center0=(-215.6,3.5,1417.5), (203.7,7.3,1441.4)

[0203] r=75mm、58mm

[0204] Step 2.3: The distance d from the cylindrical surface point to the cylindrical axis is always the radius. Therefore, according to the above initial value, the error equation f can be established to solve the filling port fitting model. The specific formula is as follows:

[0205]

[0206] Step 3: Take pose T center The initial point cloud is segmented by fitting the model model as the contour to obtain a local point cloud Pointcloud containing only the methanol filling port local , as attached Figure 5 As shown;

[0207] Step 4: According to the depth value z center Design an adaptive filter threshold function to calculate the optimal voxel filter parameter σ filter , the relevant calculation formula is as follows:

[0208]

[0209] In this embodiment, aim = 5 mm, therefore:

[0210]

[0211] Step 5: According to σ filter For local point cloud Pointcloud local Perform voxel downsampling to obtain the target point cloud with optimal density. target .

[0212] Step 8: Extract edge point features based on smoothness of the target point cloud, use Euclidean clustering to obtain multiple edge point clusters, and construct a triangle descriptor based on edge point clustering with the help of the stability idea of ​​triangles;

[0213] Step 1: Select 5 points before and after as the basis to calculate the target point cloud Pointcloud target The curv of each point in is calculated according to the smoothness threshold Threshold smooth Filter out the edge feature point set Edge{Point1,Point2,...,Point n}, the specific calculation formula is as follows:

[0214]

[0215] Among them, S represents the set of 5 points before and after.

[0216] Step 2: Construct clustering threshold cluster and the optimal voxel filter parameter σ filter The functional relationship between them is used to obtain the cluster set Clusters {cluster1(Point a ,Point b ,...), cluster2(Point a ,Pointb ,...),...,cluster m (Point a ,Point b ,...)},The specific clustering threshold calculation formula is as follows:

[0217] Threshold cluster =5σ filter ;

[0218] In this embodiment, Threshold cluster =20;

[0219] Step 3: For the above cluster sets, calculate the coordinates of their respective center points (x center_cluster ,y center_cluster ,z center_cluster ), and obtain its main direction through PCA principal component analysis That is, the direction vector of each cluster. Projecting the point onto the main direction and calculating the interval between two points can get the length value of each cluster. clusters {length1,length2,…,length m}, the specific calculation formula is as follows:

[0220]

[0221] Among them, Num represents the number of points in the cluster.

[0222] In this embodiment, the center point coordinates (x center_cluster ,y center_cluster ,z center_cluster )=(394.918,-15.270,1410.790).

[0223] Step 4: Traverse the cluster set Clusters and search each cluster cluster i The 5 nearest neighbor clusters near {cluster near_1 ,…,cluster near_5}, to traverse all pairwise non-repeating combinations of clusters and clusters from the nearest neighbor clusters i Construct a stable triangle descriptor Tri_desc for the vertex i {tri_desc1,tri_desc2,…,tri_desc l}, and Clusters i Remove the set, each triangle descriptor tri_desci Contains the length information of three vertices {length_desc1, length_desc2, length_desc3} and direction vector information And three side length information {length_side1, length_side2, length_side3}.

[0224] In this embodiment, the length information, direction vector information and three edge length information of a cluster Cluster_i are respectively: (20.85317.69723.544), {(0.684429-0.223967-0.693827), (0.1372160.032886-0.989995), (0.0611870.041865-0.997248)} and (267.545197.256103.936).

[0225] Step 9: Design a two-stage feature association algorithm and similarity scoring function based on the vertex information and side length information of the triangle descriptor;

[0226] Step 1: The first stage traverses the descriptor set Tri_desc i , to describe the sub-tri_desc i The sum of the side lengths sum_side i Key value, triangle descriptor tri_desc i Build a hash table Hash for the target value desc {(sum_side1:tri_desc1),(sum_side2:tri_desc2),…,(sum_side l :tri_desc l )};

[0227] Step 2: Traverse the length clustering set Sum_side_group of the target point cloud target , for the triangle descriptor tri_desc i_target , in the hash table Hash of the source point cloud desc_source The query length difference is in the interval (sum_side i_target -Threshold hash ,Sum_side i_target +Threshold hash ) The number of triangle descriptors within sum i , and get tri_desc i_target Potential descriptor pair set Target i {(tri_desc i_target,tri_desc a_source ),(tri_desc i_target ,tri_desc b_source ),…}, quantity sum i That is, the first stage grade of each triangle descriptor i , and sort the scores from large to small to get the score set Grade{grade max ,…,grade min} and the total descriptor pair set Match sum {Match1,Match2,...,Match l};

[0228] Step 3: According to the total number of triangle descriptors, retain the top 20% of the descriptors and enter the second stage for feature association. Remove the other descriptors from the set to get Match. sum {Match1,Match2,...,Match k}, where k = 0.2*l;

[0229] Step 4: Traverse Match sum , for a pair of descriptors (tri_desc i_target ,tri_desc a_source ), the length information of the two vertices {length_desc1, length_desc2, length_desc3}, the direction vector information And the three side length information {length_side1, length_side2, length_side3} are subtracted to get the vertex length difference Δ desc , direction vector difference Δ n And the side length difference Δ side ;

[0230] In this embodiment, the vertex length difference Δ of a certain set of paired descriptors is desc , direction vector difference Δ n And the side length difference Δ side for:

[0231] Δ desc =7.821Δ n =3.459Δ side =12.478

[0232] Step 5: For each paired descriptor (tri_desc i_target ,tri_desc a_source ) to determine whether the three differences are all less than the threshold {Thresholddesc Thereshold n ,Threshold side} condition, if it is not met, it will be removed, and finally each pairing descriptor set Match will be retained i The sum of the three differences Δ sum The descriptor pair corresponding to the minimum value (tri_desc min_target ,tri_desc min_source ), Δ sum The calculation formula is as follows:

[0233] Δ sum =Δ desc +Δ n +Δ side ;

[0234] In this embodiment, a certain maximum Δ sum The minimum value is 10.724.

[0235] Step 6: Traverse the set of retained descriptor pairs and calculate the pose transformation T between each pair of descriptors ij (R ij ,t ij ), for the remaining descriptors Tri_desc remain Conduct T ij Transformation, calculate whether the distance between the three vertices of the transformed descriptor is within the threshold Threshold dis If the threshold requirement is met, the vote is increased by 1, and the final score collection vote is obtained. sum {vote1,vote2,...,vote h}, the descriptor pair corresponding to the maximum score vote is the target descriptor pair (tri_desc tar_target ,tri_desc tar_source ), and the corresponding pose transformation T ij That is the coarse transformation pose value.

[0236] In this embodiment, the coarse transformation pose value is:

[0237]

[0238] Step 10: Design a nonlinear optimization function to obtain the precise position and posture of the tank truck filling port.

[0239] Step 1: According to the rough transformation pose value T ij Get the converted target point cloud Pointcloud` target ;

[0240] Step 2: Create the target point cloud Pointcloud`target With the source point cloud Pointcloud source The least squares relationship between them is expressed as follows:

[0241]

[0242] Step 3: Solve the least squares function to obtain the high-precision transformation pose T`(R`,t`), and use T` to obtain the center pose T of the tank truck filling port target , the calculation formula is as follows:

[0243] T target =T`T source ;

[0244] Among them, T source is the pose of the source point cloud.

[0245] In this embodiment, T', T target , as attached Figure 6 The pairing result of the two filling port point clouds is as follows:

[0246]

[0247] Methanol filling port 1:

[0248]

[0249] Methanol filling port 2:

[0250]

[0251] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0252] On the other hand, the present invention further provides a computer storage medium storing executable program code; the executable program code is used to execute any of the above-mentioned methods for positioning the filling port of a tank truck.

[0253] Exemplarily, the program code may be divided into one or more modules / units, and the one or more modules / units are stored in the memory and executed by the processor to accomplish the present invention.

[0254] The memory may be an internal storage unit of the terminal device, such as a hard disk or a memory. The memory may also be an external storage device of the terminal device, such as a plug-in hard disk, a smart memory card (SmartMedia Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the terminal device. Further, the memory may also include both an internal storage unit of the terminal device and an external storage device. The memory is used to store the program code and other programs and data required by the terminal device. The memory may also be used to temporarily store data that has been output or is to be output.

[0255] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0256] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A method for positioning the filling port of a tank truck, characterized in that: The method comprises: Obtain image data and initial point cloud of the tank truck filling port; According to the image data of the tank truck filling port, a visual detection model is constructed, and the tank truck filling port is roughly positioned by the visual detection model to obtain an approximate three-dimensional position and posture of the tank truck filling port; Segmenting the initial point cloud according to the physical model of the filling port and the approximate three-dimensional pose to obtain local point cloud data containing only the filling port; Performing filtering processing on the local point cloud data according to an adaptive filtering threshold function to obtain target point cloud data with optimal density; Based on the smoothness, edge point features are extracted from the target point cloud data with the optimal density to obtain an edge feature point set; Clustering the edge feature point set using a Euclidean clustering method to obtain a cluster set; Constructing a triangle descriptor of the cluster set according to the stability of the triangle; Pairing the triangle descriptors according to a feature association algorithm and a similarity scoring function to obtain a descriptor pair set, and coarsely registering the descriptor pair set to obtain a target descriptor pair and a coarse transformation pose of the target descriptor pair; The coarse transformed poses of the target descriptor pair are precisely registered using a nonlinear optimization function to obtain the precise pose of the tank truck filling port.

2. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: According to the image data of the tank truck filling port, a visual detection model is constructed, and the tank truck filling port is roughly positioned by the visual detection model to obtain the approximate three-dimensional position and posture of the tank truck filling port, including: Using a depth camera to obtain image data of the methanol filling port in real time; the image data includes information about the methanol filling port and the environment around the methanol filling port; The acquired image data is preprocessed and feature extracted, and image data of different resolutions are combined and processed using multi-scale information fusion technology to obtain a training data set; The training data set is input into the yolov5 model based on temporal information learning and attention mechanism for training to obtain the yolo visual detection model; The environmental image of the filling port is input into the YOLO visual detection model for target detection and feature extraction, and the two-dimensional pixel coordinates of the center point of the filling port are output; and the two-dimensional pixel coordinates of the center point are converted into the approximate three-dimensional pose of the tank truck filling port.

3. A method for positioning the filling port of a tank truck according to claim 2, characterized in that: Convert the 2D pixel coordinates of the center point into the approximate 3D pose of the tanker filling port, including: Obtain the 2D pixel coordinates of the center point of the tanker filling port from the visual inspection model; According to the depth map collected by the depth camera, the pixel value of the two-dimensional pixel coordinates of the center point of the filling port is obtained, and the depth value of the center of the filling port of the tank truck is obtained through the affine function; According to the depth value, the approximate three-dimensional position and posture of the filling port of the tank truck in the camera coordinate system is obtained using the camera three-dimensional projection model.

4. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: The initial point cloud is segmented according to the physical model of the filling port and the approximate coordinates to obtain local point cloud data containing only the filling port, including: According to the actual physical model of the tank truck filling port, it is modeled as a cylindrical object with fixed size, and the physical parameters and the filling port contour model of the tank truck filling port are obtained based on the MSAC fitting algorithm; With the approximate three-dimensional pose as the center and the filling port contour model as the outline, the initial point cloud is segmented to obtain a local point cloud containing only the methanol filling port.

5. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: The local point cloud data is filtered according to an adaptive filtering threshold function to obtain target point cloud data with optimal density, including: According to the depth map collected by the camera, the pixel value at the center of the filling port is obtained, and the depth value of the center of the filling port of the tank truck is obtained through the affine function; An adaptive filtering threshold function is constructed according to the depth value to obtain the optimal voxel filtering parameters; The local point cloud data is downsampled according to the optimal voxel filtering parameters to obtain the target point cloud data with the optimal density.

6. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: Based on the smoothness, edge point features are extracted from the target point cloud data with the optimal density to obtain an edge feature point set; Clustering the edge feature point set using a Euclidean clustering method to obtain a cluster set; According to the stability of the triangle, a triangle descriptor of the cluster set is constructed, including: Select 5 points before and after each point in the target point cloud data with the optimal density as the basis, calculate the curvature of each point in the target point cloud data with the optimal density, and filter out the edge feature point set according to the smoothness threshold; Constructing a functional relationship between a clustering threshold and an optimal voxel filtering parameter, and clustering the edge feature point set using a Euclidean clustering algorithm to obtain a cluster set; Get the coordinates of the center point of each cluster, get the direction vector of each cluster through PCA principal component analysis, project the coordinates of the center point of each cluster onto the direction vector, get the interval between two adjacent points, and get the length value of each cluster; Traverse the cluster set, search for the five nearest neighbor clusters of each cluster, traverse all the pairwise non-repeating combination clusters from the nearest neighbor clusters and use the cluster as the vertex to build a stable triangle descriptor, and at the same time remove the cluster from the cluster set.

7. A method for positioning the filling port of a tank truck according to claim 6, characterized in that: The triangle descriptor includes length information of three vertices, direction vector information and three edge length information.

8. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: The triangle descriptors are paired according to a feature association algorithm and a similarity scoring function to obtain a descriptor pair set, and the descriptor pair set is roughly aligned to obtain a target descriptor pair and a rough transformation pose of the target descriptor pair, including: In the first phase, the descriptor set is traversed, and a hash table is constructed with the sum of the side lengths of the descriptors as the key value and the triangle descriptor as the target value; Traverse the length clustering set of the target point cloud, query the number of triangle descriptors whose length difference is within the interval in the hash table of the source point cloud, and get the potential descriptor pair set. The number is the first-stage score of each triangle descriptor, and sort the scores from large to small to get the score set and the total descriptor pair set; According to the total number of triangle descriptors, the top 20% of the descriptors are retained to enter the second stage for feature association, and the other descriptors are removed from the set to obtain a new set of descriptor pairs; Traverse the new set of descriptor pairs, and for a set of paired descriptors, subtract the vertex length information, direction vector information, and three edge length information of the two to obtain the vertex length difference, direction vector difference, and edge length difference; For each paired descriptor, determine whether it meets the condition that the three differences are all less than the threshold. If not, remove it. Finally, retain the descriptor pair corresponding to the minimum sum of the three differences in each paired descriptor set; Traverse the set of retained descriptor pairs, calculate the pose transformation between each pair of descriptors, perform pose transformation on the remaining descriptor pairs, and calculate whether the distance between the three vertices of the transformed descriptor is within the threshold range. If the threshold requirement is met, the score is increased by 1, and finally a score collection is obtained. The descriptor pair corresponding to the maximum score is the target descriptor pair, and the corresponding pose transformation is the coarse transformation pose.

9. A method for positioning the filling port of a tank truck according to claim 1, characterized in that: The coarse transformation pose of the target descriptor pair is precisely aligned using a nonlinear optimization function to obtain the precise pose of the tank truck filling port, including: Transforming the target point cloud according to the coarse transformation posture, and establishing a least squares relationship between the transformed target point cloud and the source point cloud; The least squares relationship is solved by using the Gauss-Newton method to obtain a high-precision transformed posture; The product of the high-precision transformed posture and the source point cloud posture is the precise posture of the filling port of the tank truck.

10. A computer storage medium storing executable program code, characterized in that: The executable program code is used to execute a method for positioning the filling port of a tank truck as described in any one of claims 1-9.

Images