Method and system for estimating flange pose in real time

Abstract

The invention discloses a method and a system for estimating the pose of a flange in real time, and the system comprises a camera shooting module which is used for shooting an image in real time; the detection module is used for identifying a flange and a flange bolt hole in the shot image and obtaining a flange image and a flange bolt hole image; the first-frame flange pose recognition and positioning module is used for calculating an initial flange pose T0 of the first-frame flange based on the first-frame bolt hole image and a PNP algorithm; and the non-first-frame flange pose recognition and positioning module calculates the real flange pose Tb of the b-th frame flange based on the flange image and the initial flange pose T0, and detects the real flange pose in real time. According to the invention, the flange edge doing low-speed translational motion in the space can be extracted in real time and flange positioning is realized; flange bolt holes need to be used for auxiliary positioning except for first frame positioning, only flange images need to be processed for subsequent continuous frame flange edge extraction and positioning, and resource waste is avoided while precision is guaranteed.

Description

Technical Field

[0001] The present invention belongs to the technical field of image processing, and in particular relates to a method and system for real-time estimation of flange position and posture. Background Art

[0002] Edge detection is one of the most fundamental problems in computer vision and has a wide range of applications, such as image segmentation, object detection, and video object segmentation. Given an input image, edge detection aims to extract precise object boundaries and visually significant edges. However, most edge detection algorithms are based on low-level local cues (such as color and texture) to obtain edges, and the edge detection effect is poor in complex image backgrounds. When docking flanges or grasping flange workpieces, the working environment is relatively complex, and the flange is in operation during operation. The existing edge detection algorithms have poor detection effects on flanges and are difficult to meet actual operating requirements. Summary of the invention

[0003] Purpose of the invention: The first purpose of the present invention is to provide a method for real-time estimation of flange posture that is suitable for complex working environments, can extract moving flange edges and has high detection accuracy.

[0004] A second object of the present invention is to provide a system for estimating flange posture in real time.

[0005] Technical solution: A method for real-time estimation of flange posture disclosed in the present invention comprises the following steps:

[0006] S1: The camera captures images in real time during the operation;

[0007] S2: Use the YOLO neural network algorithm to pre-train a detection model for identifying flanges, and use the trained detection model to identify flanges and flange bolt holes in the captured image to obtain flange images and flange bolt hole images;

[0008] S3: Combine the bolt hole image and PNP algorithm to calculate the initial flange pose T of the first frame flange 0 ;

[0009] S4: cropping the flange image in the b-th frame image;

[0010] S5: using the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image;

[0011] S6: Convert the coordinate point set of the original flange edge image to the flange pose T of the flange in the b-1th frame b-1 The plane point cloud of the XY plane;

[0012] S7: Use consistency check algorithm to perform circle fitting on the plane point cloud to obtain the coordinates of the center of the flange;

[0013] S8: Filter the target point cloud according to the flange radius to form a flange edge point set;

[0014] S9: back-projecting the flange edge point set into the captured image in step S1 to obtain a point set of the target flange edge image;

[0015] S10: using the least square method to perform ellipse fitting on the point set of the target flange edge image to obtain a fuzzy flange pose with ambiguity;

[0016] S11: Eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose T of the flange in the current b-th frame b ;

[0017] S12: Repeat steps S6 to S11 to detect the actual flange position and posture of the flange in real time until the operation is completed.

[0018] Furthermore, in step S3, the initial flange position T of the first frame flange is calculated. 0 The steps are as follows:

[0019] Crop the flange bolt hole image in the first frame image;

[0020] Use the Canny edge detection algorithm to detect the flange bolt hole edge in each flange bolt hole image;

[0021] Perform Hough circle fitting on the flange bolt hole image after edge detection to calculate the image coordinates of the center point of the flange bolt hole;

[0022] Establish a world coordinate system at the center point of the flange, and obtain the actual coordinates of the center point of the flange bolt hole in the world coordinate system based on the data of the flange bolt hole and the flange center point obtained by actual measurement in advance;

[0023] Based on the image coordinates of the center point, calculate the distance between all flange bolt holes and the coordinates of the upper left corner of the flange YOLO frame, record the center of the flange bolt hole corresponding to the minimum distance as Center, calculate the polar angles between the center points of the remaining flange bolt holes and the upper left corner of the flange YOLO frame, and sort the polar angles from large to small, and determine the quadrants to which all flange bolt holes belong in the world coordinate system based on the sorting;

[0024] The initial flange position T of the first frame flange is calculated by combining the actual coordinates, the quadrant to which it belongs, and the image coordinates. 0 .

[0025] Furthermore, the steps of converting to obtain the plane point cloud in step S6 are as follows:

[0026] Get the coordinate point set Q of the original edge image;

[0027] Map the coordinate point set Q to the camera coordinate system to obtain the three-dimensional point set P mapped to the camera coordinate system c ;

[0028] The three-dimensional point set P c Change to transform to flange posture T b-1 The planar point cloud of the XY plane.

[0029] Furthermore, the coordinate point set Q is converted to a three-dimensional point set P c The formula is:

[0030] Q = {q i =(u i ,v i )|q i ∈R 2 ,i∈(1,2,…,n)}

[0031]

[0032] Q and P c Satisfaction between Where R is the set of real numbers, R 2 Used to describe the coordinates of a two-dimensional point, R 3 Used to describe the coordinates of three-dimensional points, K refers to the camera internal parameter matrix, (c x ,c y ) refers to the principal point of the plane of the captured image in step S1, (f x ,f y ) refers to the focal length of the camera.

[0033] Furthermore, the three-dimensional point set P c Change to transform to flange posture T b-1 The transformation of the plane point cloud in the XY plane is as follows:

[0034] Assume that the three-dimensional point set P c Transform to flange pose T b-1 The three-dimensional point set under the coordinates is

[0035] 3D point set P c With the three-dimensional point set P f The conversion relationship is

[0036]

[0037] The three-dimensional point set P f Projection to flange pose T b-1 On the XY plane, eliminating the influence of the Z axis, the projected plane point cloud is

[0038] Furthermore, the steps for obtaining the intermediate point set in step S8 are as follows: traverse all point clouds of the plane point cloud, calculate the distance from all point clouds to the center of the flange, set the distance threshold according to the flange radius, select the point clouds that meet the threshold condition to form the intermediate point set, and reassign the z component on the Z axis back to the intermediate point set to obtain the flange edge point set

[0039] Furthermore, the formula for obtaining the point set of the target flange edge image in step S9 is as follows:

[0040] The point set of the target flange edge image is Q′={q′ i =(u′ i , v′ i )|q′ i ∈R 2 , i∈(1, 2, ..., n)},

[0041] And P w Satisfies Q′

[0042] Furthermore, the steps of obtaining the ambiguous flange pose in step S10 are as follows:

[0043] The points of the flange edge image are determined by the coordinates of the captured image (u′ i , v′ i ) is projected to the camera coordinate system as (X, Y, Z), and the two satisfy the following relationship:

[0044]

[0045] The least squares method is used to perform ellipse fitting on the point set Q′ of the target flange edge image to obtain the general formula of the ellipse:

[0046] Au ' 2 +Bu′v′+Cv′ 2 +Du′+Ev′+F= 0 ;

[0047]

[0048] Combining the above formula, we can get

[0049]

[0050] This formula is an expression of the elliptical cone formed by the optical center of the camera and the edge of the target flange. The ellipse projected by the flange edge on the captured image is a cross section of the elliptical cone.

[0051] According to the general formula of ellipse, we can solve the circular pose in space and find that there exists a real symmetric matrix P that satisfies

[0052]

[0053]

[0054] There exists an orthogonal matrix G that can diagonalize the real symmetric matrix P, that is: G T PG=diag(λ 1 ,λ 2 ,λ 3 );

[0055] The point (X, Y, Z) in the camera coordinate system is transformed into the point (X', Y', Z') in the world coordinate system after being rotated by the P matrix. The transformation formula is as follows:

[0056]

[0057] From the above formula, we can get the general formula of standard elliptical cone: 1 X' 2 +λ 2 Y' 2 +λ 3 Z' 2 =0;

[0058] The calculation formulas for the center and normal vector of the cross-sectional space circle are as follows:

[0059]

[0060]

[0061]

[0062]

[0063] Where r is the flange radius, t 1 ,t 2 is the world coordinate of the flange surface, is the flange surface normal vector, λ 1 , 2 , 3 is the eigenvalue of the matrix P.

[0064] Furthermore, the steps to eliminate ambiguity in step S11 are as follows: Calculate With N b-1 The angle θ 1 ,θ 2 , the flange posture corresponding to the minimum angle is taken as the true flange posture T b , where N b-1 Finger T b-1 The corresponding normal vector in the XY plane is

[0065]

[0066]

[0067] Based on the same inventive concept, the present invention also discloses a system for real-time estimation of flange position and posture, comprising:

[0068] A camera shooting module, used for shooting images in real time;

[0069] A detection module, used for identifying the flange and the flange bolt hole in the captured image, and obtaining the flange image and the flange bolt hole image;

[0070] The first frame flange position recognition and positioning module calculates the initial flange position T of the first frame flange based on the bolt hole image of the first frame and the PNP algorithm. 0 ;

[0071] The non-first frame flange pose recognition and positioning module is based on the flange image and the initial flange pose T 0 , calculate the actual flange position T of the flange in frame b b , detect the real position and posture of the flange in real time; the non-first frame flange position and posture recognition and positioning module includes the following sub-modules:

[0072] The acquisition submodule is used to crop the flange image in the b-th frame of the captured image;

[0073] The edge detection submodule uses the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image;

[0074] The plane point cloud submodule is used to obtain the coordinate point set of the original flange edge image and transform it into the flange pose T of the flange of the b-1th frame. b-1 The plane point cloud of the XY plane;

[0075] The perfect circle fitting submodule uses a consistency check algorithm to fit a plane point cloud into a circle and obtain the coordinates of the center of the flange.

[0076] The screening submodule screens the target point cloud according to the flange radius to form a flange edge point set;

[0077] A back-projection submodule, used for back-projecting the flange edge point set into the captured image of the camera capture module to obtain the point set of the target flange edge image;

[0078] The ellipse fitting submodule uses the least square method to perform ellipse fitting on the point set of the target flange edge image to obtain the ambiguous flange pose;

[0079] The ambiguity elimination module is used to eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose T of the flange in the current b-th frame.b .

[0080] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: the present invention can extract the flange edge that performs low-speed translational motion in space in real time and realize flange positioning; the present invention eliminates the need to use flange bolt holes to assist in positioning in the first frame positioning, and subsequent continuous frame flange edge extraction and positioning only require image processing of the flange image, thereby avoiding waste of resources while ensuring accuracy; and the present invention can extract the flange edge with clearer edges and less noise through the mapping-projection method, which improves accuracy compared to direct ellipse detection on the image. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 is a flow chart of the method of the present invention;

[0082] Figure 2 is the initial flange position T calculated by the method of the present invention 0 Flowchart of

[0083] Figure 3 It is a schematic diagram of the structure of the system of the present invention. DETAILED DESCRIPTION

[0084] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0085] Example 1

[0086] The method for real-time estimation of flange position and posture of the present invention is as follows: Figure 1 and Figure 2 As shown, the following steps are included:

[0087] S1: The camera captures images in real time during the operation.

[0088] S2: Use the YOLO neural network algorithm to pre-train a detection model for identifying flanges, and use the trained detection model to identify flanges and flange bolt holes in the captured image to obtain flange images and flange bolt hole images. When pre-training the detection model, select an image of the target flange, obtain the size of the target flange, and input the target flange image into the YOLO neural network algorithm model for training.

[0089] Preferably, when the detection model identifies the flange and flange bolt holes in the captured image, it first determines whether the coordinates of the flange bolt hole YOLO frame are within the range of the flange YOLO frame, and then combines the flange and the flange bolt holes according to the number of flange bolt holes on the flange, that is, the number of flange bolt holes in the flange YOLO frame is determined based on prior knowledge, and flange images and flange bolt hole images that meet the conditions are screened, which is beneficial to reduce the probability of misidentification.

[0090] S3: Combine the bolt hole image and PNP algorithm to calculate the initial flange pose T of the first frame flange 0 .

[0091] Crop the flange bolt hole image in the first frame image;

[0092] Preprocessing the flange bolt hole images, and then using the Canny edge detection algorithm to detect the flange bolt hole edges in each flange bolt hole image; preferably, median filtering is performed on the flange bolt hole images before edge detection;

[0093] Perform Hough circle fitting on the flange bolt hole image after edge detection to calculate the image coordinates of the center point of the flange bolt hole;

[0094] Establish a world coordinate system at the center point of the flange, and obtain the actual coordinates of the center point of the flange bolt hole in the world coordinate system based on the data of the flange bolt hole and the flange center point obtained by actual measurement in advance;

[0095] Based on the image coordinates of the center point, calculate the distance between all flange bolt holes and the coordinates of the upper left corner of the flange YOLO frame, record the center of the flange bolt hole corresponding to the minimum distance as Center, calculate the polar angles between the center points of the remaining flange bolt holes and the upper left corner of the flange YOLO frame, and sort the polar angles from large to small, and determine the quadrants to which all flange bolt holes belong in the world coordinate system based on the sorting;

[0096] The initial flange position T of the first frame flange is calculated by combining the actual coordinates, the quadrant to which it belongs, and the image coordinates. 0 .

[0097] S4: Crop the flange image in the b-th frame of the captured image. Preferably, the redundant information of the captured image in step S1 is removed.

[0098] S5: Use the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image.

[0099] S6: Convert the coordinate point set of the original flange edge image to the flange pose T of the flange in the b-1th frame b-1 When solving the flange pose of the first frame flange, the coordinate point set of the flange edge image is converted to T 0 The planar point cloud of the XY plane.

[0100] Map the original flange edge image to the camera coordinate system as follows:

[0101] Get the coordinate point set Q of the original flange edge image = {q i =(u i ,v i)|q i ∈R 2 ,i∈(1,2,…,n)}, where R is the set of real numbers, R 2 Used to describe the coordinates of two-dimensional points;

[0102] Suppose the coordinate point set Q of the original flange edge image is mapped to the three-dimensional point set in the camera coordinate system: Where R 3 Used to describe the coordinates of three-dimensional points;

[0103] Q and P c The conversion relationship between them is

[0104]

[0105] Where K refers to the camera intrinsic parameter matrix,

[0106] Where (c x ,c y ) refers to the principal point of the plane of the captured image in step S1 (“principal point” refers to the optical center of the camera), (f x ,f y ) refers to the focal length of the camera.

[0107] The three-dimensional point set P in the camera coordinate system c Transformed to flange pose T b-1 The plane point cloud of the XY plane is transformed as follows:

[0108] Assume that the three-dimensional point set P c Transform to flange pose T b-1 The three-dimensional point set under the coordinates is

[0109] 3D point set P c With the three-dimensional point set P f The conversion relationship is

[0110]

[0111] The three-dimensional point set P f Projection to flange pose T b-1 On the XY plane, eliminating the influence of the Z axis, the projected plane point cloud is

[0112] S7: Use the consistency check algorithm to perform circle fitting on the plane point cloud to obtain the coordinates of the flange center.

[0113] S8: Filter the target point cloud according to the flange radius to form a flange edge point set.

[0114] Traverse all point clouds, calculate the distance from all point clouds to the center of the flange, set the distance threshold according to the flange radius, select the point clouds that meet the threshold conditions to form the intermediate point set, and reassign the Z-axis component back to the intermediate point set to obtain the flange edge point set

[0115] S9: Back-project the flange edge point set into the image to obtain the point set of the target flange edge image.

[0116] The flange edge point set P w The target flange edge image is projected onto the captured image in step S1, and the projection method is as follows:

[0117] Suppose the point set under the coordinates of the image captured in step S1 after the target flange edge image is imaged is Q' = {q' i =(u' i ,v' i )|q' i ∈R 2 ,i∈(1,2,…,n)},

[0118] P w The conversion relationship with Q′ is

[0119]

[0120] S10: Use the least square method to perform ellipse fitting on the point set of the target flange edge image to obtain an ambiguous flange pose.

[0121] The points of the flange edge image are determined by the coordinates of the captured image (u′ i , v′ i ) is projected to the camera coordinate system as (X, Y, Z), and the two satisfy the following relationship:

[0122]

[0123] The least squares method is used to perform ellipse fitting on the point set Q′ of the target flange edge image to obtain the general formula of the ellipse:

[0124] Au′ 2 +Bu′v′+Cv′ 2 +Du′+Ev′+F=0;

[0125]

[0126] Combining the above formula, we can get

[0127]

[0128] This formula is an expression of the elliptical cone formed by the optical center of the camera and the edge of the target flange, and the ellipse projected by the flange edge on the captured image is a cross section of the elliptical cone;

[0129] The spatial circular pose is solved according to the general formula of the ellipse. According to the general formula of the ellipse and the projection formula of the world coordinates to the coordinates of the captured image in step S1, it is known that there exists a real symmetric matrix P that satisfies

[0130]

[0131]

[0132] From the properties of the matrix, we know that there exists an orthogonal matrix G that can diagonalize the real symmetric matrix P, that is: G T PG=diag(λ 1 ,λ 2 ,λ 3 );

[0133] The point (X, Y, Z) in the camera coordinate system is transformed into the point (X′, Y′, Z′) in the world coordinate system after being rotated by the P matrix.

[0134]

[0135] From the above formula, we can get the general formula of standard elliptical cone: 1 X′ 2 +λ 2 Y′ 2 +λ 3 Z′ 2 =0

[0136] The spatial circle pose derived from the general formula of the ellipse is ambiguous, and the calculation formulas for the center and normal vector of the cross-sectional spatial circle are as follows:

[0137]

[0138]

[0139]

[0140]

[0141] Where r is the flange radius, t 1 ,t 2 is the world coordinate of the flange surface, is the flange surface normal vector, λ 1 , 2 , 3 is the eigenvalue of the matrix P, and b refers to the b-th frame image.

[0142] S11: Eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose Tb of the flange in the current b-th frame.

[0143] Eliminate the ambiguity of the spatial circular pose and calculate With N b-1 The angle θ 1 ,θ 2 , the flange posture corresponding to the minimum angle is taken as the true flange posture T b , N b-1 Finger T b-1 The corresponding normal vector in the XY plane;

[0144]

[0145] When solving the flange position of the first frame flange, according to T 0 Normal vector N of the XY plane 0 Eliminate ambiguity.

[0146] S12: Repeat steps S6 to S11 to detect the actual flange position and posture of the flange in real time until the operation is completed.

[0147] This embodiment can extract the flange edge location in real time, that is, this embodiment can detect the flange edge and estimate the position and posture in real time. The first frame positioning is used for mapping-projection and eliminating the ambiguous flange position and posture obtained by bolt hole positioning. b The subsequent flange edge detection and positioning are used for mapping-projection and eliminating the ambiguity of the flange posture, which is the flange posture T obtained by positioning in the previous frame. b-1 . In this embodiment, except for the need to use flange bolt holes to assist in positioning in the first frame, the flange edge extraction and positioning in subsequent consecutive frames only require image processing of the flange image, thereby ensuring accuracy while avoiding waste of resources. In addition, the mapping-projection method can extract the flange edge with clearer edges and less noise, which improves accuracy compared to directly performing ellipse detection on the image.

[0148] Example 2

[0149] The system for real-time estimation of flange position and posture of the present invention is as follows: Figure 3 As shown, it includes a camera shooting module, a detection module, a first-frame flange posture recognition and positioning module, and a non-first-frame flange posture recognition and positioning module.

[0150] The camera shooting module is used to shoot images in real time.

[0151] The detection module is used to identify the flange and flange bolt holes in the captured image and obtain the flange image and flange bolt hole image. The image of the target flange is selected to obtain the size of the target flange, the target flange image is input into the YOLO neural network algorithm model for training, and a detection model for identifying the flange is constructed. The detection model is used to identify the flange and flange bolt holes in the captured image.

[0152] Preferably, when the detection model identifies the flange and flange bolt holes in the captured image, it first determines whether the coordinates of the flange bolt hole YOLO frame are within the range of the flange YOLO frame, and then combines the flange and the flange bolt holes according to the number of flange bolt holes on the flange, that is, the number of flange bolt holes in the flange YOLO frame is determined based on prior knowledge, and flange images and flange bolt hole images that meet the conditions are screened, which is beneficial to reduce the probability of misidentification.

[0153] The first frame flange position recognition and positioning module calculates the initial flange position T of the first frame flange based on the bolt hole image of the first frame and the PNP algorithm. 0 .

[0154] Crop the flange bolt hole image in the first frame image;

[0155] Preprocessing the flange bolt hole images, and then using the Canny edge detection algorithm to detect the flange bolt hole edges in each flange bolt hole image; preferably, median filtering is performed on the flange bolt hole images before edge detection;

[0156] Perform Hough circle fitting on the flange bolt hole image after edge detection to calculate the image coordinates of the center point of the flange bolt hole;

[0157] Establish a world coordinate system at the center point of the flange, and obtain the actual coordinates of the center point of the flange bolt hole in the world coordinate system based on the data of the flange bolt hole and the flange center point obtained by actual measurement in advance;

[0158] Based on the image coordinates of the center point, calculate the distance between all flange bolt holes and the coordinates of the upper left corner of the flange YOLO frame, record the center of the flange bolt hole corresponding to the minimum distance as Center, calculate the polar angles between the center points of the remaining flange bolt holes and the upper left corner of the flange YOLO frame, and sort the polar angles from large to small, and determine the quadrants to which all flange bolt holes belong in the world coordinate system based on the sorting;

[0159] The initial flange position T of the first frame flange is calculated by combining the actual coordinates, the quadrant to which it belongs, and the image coordinates. 0 .

[0160] The non-first frame flange pose recognition and positioning module is based on the flange image and the initial flange pose T 0, calculate the actual flange position T of the flange in frame b b , real-time detection of the flange's true posture; the non-first frame flange posture recognition and positioning module includes the following sub-modules: acquisition sub-module, edge detection sub-module, plane point cloud sub-module, perfect circle fitting sub-module, screening sub-module, back projection sub-module, ellipse fitting sub-module and ambiguity elimination module.

[0161] The acquisition submodule is used to crop the flange image in the b-th frame of the captured image. Preferably, redundant information of the captured image in the camera capture module is removed.

[0162] The edge detection submodule uses the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image.

[0163] The plane point cloud submodule is used to obtain the coordinate point set of the original flange edge image and transform it into the flange pose T of the flange of the b-1th frame. b-1 When solving the flange pose of the first frame flange, the coordinate point set of the flange edge image is converted to T 0 The planar point cloud of the XY plane.

[0164] Map the original flange edge image to the camera coordinate system as follows:

[0165] Get the coordinate point set Q of the original flange edge image = {q i =(u i ,v i )|q i ∈R 2 ,u∈(1,2,…,n)}, where R is the set of real numbers, R 2 Used to describe the coordinates of two-dimensional points;

[0166] Suppose the coordinate point set Q of the original flange edge image is mapped to the three-dimensional point set in the camera coordinate system: Where R 3 Used to describe the coordinates of three-dimensional points;

[0167] Q and P c The conversion relationship between them is

[0168]

[0169] Where K refers to the camera intrinsic parameter matrix,

[0170] Where (c x ,c y ) refers to the principal point of the plane where the image is taken, (f x ,f y ) refers to the focal length of the camera.

[0171] The three-dimensional point set P in the camera coordinate system c Transformed to flange pose T b-1 The plane point cloud of the XY plane is transformed as follows:

[0172] Assume that the three-dimensional point set P c Transform to flange pose T b-1 The three-dimensional point set under the coordinates is

[0173] 3D point set P c With the three-dimensional point set P f The conversion relationship is

[0174]

[0175] The three-dimensional point set P f Projection to flange pose T b-1 On the XY plane, eliminating the influence of the Z axis, the projected plane point cloud is

[0176] The perfect circle fitting submodule uses a consistency check algorithm to fit a circle to the plane point cloud and obtain the coordinates of the flange center.

[0177] The screening submodule screens the target point cloud according to the flange radius to form a flange edge point set.

[0178] Traverse all point clouds, calculate the distance from all point clouds to the center of the flange, set the distance threshold according to the flange radius, select the point clouds that meet the threshold conditions to form the intermediate point set, and reassign the Z-axis component back to the intermediate point set to obtain the flange edge point set

[0179] The back-projection submodule is used to back-project the flange edge point set into the captured image to obtain the point set of the target flange edge image.

[0180] The flange edge point set P w The target flange edge image is projected onto the captured image. The projection method is as follows:

[0181] Suppose the point set of the target flange edge image in the image coordinates after imaging is Q' = {q' i =(u' i ,v' i )|q' i ∈R 2 ,i∈(1,2,…,n)},

[0182] P w The conversion relationship with Q' is

[0183]

[0184] The ellipse fitting submodule uses the least squares method to perform ellipse fitting on the point set of the target flange edge image to obtain the ambiguous flange pose.

[0185] The points of the flange edge image are determined by the coordinates of the captured image (u' i ,v' i ) is projected to the camera coordinate system as (X, Y, Z), and the two satisfy the following relationship:

[0186]

[0187] The least squares method is used to perform ellipse fitting on the point set Q' of the target flange edge image to obtain the general formula of the ellipse.

[0188] Au' 2 +Bu'v'+Cv' 2 +Du'+Ev'+F=0;

[0189]

[0190] Combining the above formula, we can get

[0191]

[0192] This formula is an expression of the elliptical cone formed by the optical center of the camera and the edge of the target flange, and the ellipse projected by the flange edge on the captured image is a cross section of the elliptical cone;

[0193] The spatial circular pose is solved according to the general formula of the ellipse. According to the general formula of the ellipse and the projection formula of the world coordinates to the coordinates of the captured image in step S1, it is known that there exists a real symmetric matrix P that satisfies

[0194]

[0195]

[0196] From the properties of the matrix, we know that there exists an orthogonal matrix G that can diagonalize the real symmetric matrix P, that is: G T PG=diag(λ 1 ,λ 2 ,λ 3 );

[0197] The point (X, Y, Z) in the camera coordinate system is transformed into the point (X', Y', Z') in the world coordinate system after being rotated by the P matrix.

[0198]

[0199] From the above formula, we can get the general formula of standard elliptical cone: 1 X' 2 +λ 2 Y' 2 +λ 3 Z' 2 =0

[0200] The spatial circle pose derived from the general formula of the ellipse is ambiguous, and the calculation formulas for the center and normal vector of the cross-sectional spatial circle are as follows:

[0201]

[0202]

[0203]

[0204]

[0205] Where r is the flange radius, t 1 ,t 2 is the world coordinate of the flange surface, is the normal vector of the flange surface, and λ 1 , 2 , 3 is the eigenvalue of the matrix P.

[0206] The ambiguity elimination module is used to eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose T of the flange in the current b-th frame. b .

[0207] Eliminate the ambiguity of the spatial circular pose and calculate With N b-1 The angle θ 1 ,θ 2 , the flange posture corresponding to the minimum angle is taken as the true flange posture T b , N b-1 Finger T b-1 Normal vector on the XY plane;

[0208]

[0209]

[0210] When solving the flange position of the first frame flange, according to T 0 Normal vector N of the XY plane 0 Eliminate ambiguity.

Claims

1. A method for real-time estimation of flange position and posture, characterized in that: The following steps are involved: S1: The camera captures images in real time during the operation; S2: Use the YOLO neural network algorithm to pre-train a detection model for identifying flanges, and use the trained detection model to identify flanges and flange bolt holes in the captured image to obtain flange images and flange bolt hole images; S3: Combine the bolt hole image and the PNP algorithm to calculate the initial flange pose T0 of the first frame flange; S4: cropping the flange image in the b-th frame image; S5: using the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image; S6: Convert the coordinate point set of the original flange edge image to the flange pose T of the flange in the b-1th frame b-1 The plane point cloud of the XY plane; S7: Use consistency check algorithm to perform circle fitting on the plane point cloud to obtain the coordinates of the center of the flange; S8: Filter the target point cloud according to the flange radius to form a flange edge point set; S9: back-projecting the flange edge point set into the captured image in step S1 to obtain a point set of the target flange edge image; S10: using the least square method to perform ellipse fitting on the point set of the target flange edge image to obtain a fuzzy flange pose with ambiguity; S11: Eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose T of the flange in the current b-th frame b ; S12: Repeat steps S6 to S11 to detect the actual flange position and posture of the flange in real time until the operation is completed.

2. The method for real-time estimation of flange position and posture according to claim 1, characterized in that: The steps for calculating the initial flange position T0 of the first frame flange in step S3 are as follows: Crop the flange bolt hole image in the first frame image; Use the Canny edge detection algorithm to detect the flange bolt hole edge in each flange bolt hole image; Perform Hough circle fitting on the flange bolt hole image after edge detection to calculate the image coordinates of the center point of the flange bolt hole; Establish a world coordinate system at the center point of the flange, and obtain the actual coordinates of the center point of the flange bolt hole in the world coordinate system based on the data of the flange bolt hole and the flange center point obtained by actual measurement in advance; Based on the image coordinates of the center point, calculate the distance between all flange bolt holes and the coordinates of the upper left corner of the flange YOLO frame, record the center of the flange bolt hole corresponding to the minimum distance as Center, calculate the polar angles between the center points of the remaining flange bolt holes and the upper left corner of the flange YOLO frame, and sort the polar angles from large to small. According to the sorting, determine the quadrant to which all flange bolt holes belong in the world coordinate system; The initial flange pose T0 of the first frame flange is calculated using the PNP algorithm combined with the actual coordinates, the quadrant to which it belongs, and the image coordinates.

3. The method for real-time estimation of flange position and posture according to claim 1, characterized in that: The steps of converting to obtain a planar point cloud in step S6 are as follows: Get the coordinate point set Q of the original edge image; Map the coordinate point set Q to the camera coordinate system to obtain the three-dimensional point set P mapped to the camera coordinate system c ; The three-dimensional point set P c Change to transform to flange posture T b-1 The planar point cloud of the XY plane.

4. The method for real-time estimation of flange position and posture according to claim 3, characterized in that: The coordinate point set Q is converted to a three-dimensional point set P c The formula is: Q={q i =(u i ,v i )|q i ∈R 2 ,i∈(1,2,…,n)} Q and P c Satisfaction between Where R is the set of real numbers, R 2 Used to describe the coordinates of a two-dimensional point, R 3 Used to describe the coordinates of three-dimensional points, K refers to the camera internal parameter matrix, (c x ,c y ) refers to the principal point of the plane of the captured image in step S1, (f x ,f y ) refers to the focal length of the camera.

5. The method for real-time estimation of flange position and posture according to claim 4, characterized in that: The three-dimensional point set P c Change to transform to flange posture T b-1 The transformation of the plane point cloud in the XY plane is as follows: Assume that the three-dimensional point set P c Transform to flange pose T b-1 The three-dimensional point set under the coordinates is 3D point set P c With the three-dimensional point set P f The conversion relationship is The three-dimensional point set P f Projection to flange pose T b-1 On the XY plane, eliminating the influence of the Z axis, the projected plane point cloud is 6. The method for real-time estimation of flange position and posture according to claim 5, characterized in that: The steps for obtaining the intermediate point set in step S8 are as follows: traverse all point clouds of the plane point cloud, calculate the distance from all point clouds to the center of the flange, set the distance threshold according to the flange radius, select the point clouds that meet the threshold condition to form the intermediate point set, and reassign the z component on the Z axis back to the intermediate point set to obtain the flange edge point set 7. The method for real-time estimation of flange position and posture according to claim 6, characterized in that: The formula for obtaining the point set of the target flange edge image in step S9 is as follows: The point set of the target flange edge image is Q' = {q' i =(u' i ,v' i )|q' i ∈R 2 ,i∈(1,2,…,n)}, And P w Meet Q' 8. The method for real-time estimation of flange position and posture according to claim 1, characterized in that: The steps of obtaining the ambiguous flange pose in step S10 are as follows: The points of the flange edge image are determined by the coordinates of the captured image (u' i ,v' i ) is projected to the camera coordinate system as (X, Y, Z), and the two satisfy the following relationship: The least squares method is used to perform ellipse fitting on the point set Q' of the target flange edge image to obtain the general formula of the ellipse. Oh' 2 +This'v'+Cv' 2 +Du'+Ev'+F=0; Combining the above formula, we can get This formula is an expression of the elliptical cone formed by the optical center of the camera and the edge of the target flange. The ellipse projected by the flange edge on the captured image is a cross section of the elliptical cone. According to the general formula of ellipse, we can solve the circular pose in space and find that there exists a real symmetric matrix P that satisfies There exists an orthogonal matrix G that can diagonalize the real symmetric matrix P, that is: G T PG = diag(λ1,λ2,λ3); The point (X, Y, Z) in the camera coordinate system is transformed into the point (X', Y', Z') in the world coordinate system after being rotated by the P matrix. The transformation formula is as follows: From the above formula, we can get the general formula of standard elliptical cone: λ1X' 2 +λ2Y' 2 +λ3Z' 2 =0; The calculation formulas for the center and normal vector of the cross-sectional space circle are as follows: Where r is the flange radius, t1 and t2 are the world coordinates of the flange surface, is the flange surface normal vector, λ1, λ2, λ3 are the eigenvalues ​​of the matrix P, and b refers to the b-th frame image.

9. The method for real-time estimation of flange position and posture according to claim 1, characterized in that: The steps to eliminate ambiguity in step S11 are as follows: Calculate The angles θ1 and θ2 with N0 are taken as the flange posture corresponding to the minimum angle as the true flange posture T b , where N0 refers to the normal vector corresponding to T0 in the XY plane, 10. A system for real-time estimation of flange position and posture, characterized in that: include, A camera shooting module, used for shooting images in real time; A detection module, used for identifying the flange and the flange bolt hole in the captured image, and obtaining the flange image and the flange bolt hole image; The first-frame flange position recognition and positioning module calculates the initial flange position T0 of the first-frame flange based on the bolt hole image of the first frame and the PNP algorithm; The non-first frame flange pose recognition and positioning module calculates the actual flange pose T of the flange in the bth frame based on the flange image and the initial flange pose T0. b , detect the real position and posture of the flange in real time; the non-first frame flange position and posture recognition and positioning module includes the following sub-modules: The acquisition submodule is used to crop the flange image in the b-th frame of the captured image; The edge detection submodule uses the Canny edge detection algorithm to detect the flange edge in the cropped flange image to obtain the original flange edge image; The plane point cloud submodule is used to obtain the coordinate point set of the original flange edge image and transform it into the flange pose T of the flange of the b-1th frame. b-1 The plane point cloud of the XY plane; The perfect circle fitting submodule uses a consistency check algorithm to fit a plane point cloud to obtain the coordinates of the flange center. The screening submodule screens the target point cloud according to the flange radius to form a flange edge point set; A back-projection submodule, used for back-projecting the flange edge point set into the captured image of the camera capture module to obtain the point set of the target flange edge image; The ellipse fitting submodule uses the least square method to perform ellipse fitting on the point set of the target flange edge image to obtain the ambiguous flange pose; The ambiguity elimination module is used to eliminate the ambiguity of the fuzzy flange pose and obtain the true flange pose T of the flange in the current b-th frame. b .

Images