Improved bolt pose estimation method

Abstract

The invention relates to the technical field of visual positioning, in particular to an improved bolt pose estimation method. The method comprises the following steps: S1, creating a virtual bolt data set for training; s2, using a deep learning-based method FPCC instance segmentation network to extract features of each point, and using a non-maximum suppression mode to search for a center point for each instance; s3, the point with the highest score is set as the geometric center point of each instance, and the remaining points are clustered to the nearest geometric center based on a network clustering algorithm, so that the instances are segmented; and S4, after the instance is segmented, performing cylinder fitting, re-estimating parameters of a cylinder by using an improved RANSAC algorithm, obtaining a direction vector of an axis of the cylinder, and determining a central axis direction of the bolt. Based on an FPCC instance segmentation network, a new bolt pose estimation method is provided in combination with an SAC-IDBSCA coarse registration algorithm, an NDT fine registration algorithm and an improved RANSAC algorithm, the FPCC instance segmentation network provided by the invention can still realize rapid and effective segmentation of bolts under a high-degree stacking condition, any training of manual annotation data is not needed, the algorithm is simple, the algorithm is easy to implement, and the method is suitable for large-scale popularization and application. The improved RANSAC algorithm provided by the invention can effectively compensate the negative influence of the traditional algorithm on the algorithm effect due to improper threshold selection.

Description

Technical Field

[0001] The present invention relates to the technical field of visual positioning, and specifically to an improved bolt pose estimation method. Background Art

[0002] The visual positioning method is a positioning technology implemented using computer vision technology. Usually, it analyzes the image of an object to find the position and orientation of the object. This technology has been widely applied in fields such as robotics, autonomous driving, and intelligent equipment. However, for bolts, when grasping in an occlusion and overlap scenario, it will have a huge impact on the recognition and positioning of the object, and using traditional visual positioning algorithms will greatly increase the failure rate of robot grasping. Therefore, there is an urgent need to study a pose estimation method for bolts so that the robot can grasp bolts more accurately.

[0003] Traditional image segmentation algorithms first perform target segmentation on the scene, and then perform point cloud registration on the multiple segmented targets to achieve pose estimation. The general approach is to determine several poses convenient for the robotic arm to grasp for the model point cloud file, and then register the instance point cloud segmented from the scene point cloud with the model point cloud, calculate the rigid transformation matrix between the two, and thus calculate the pose matrix of the graspable instance in the scene point cloud. However, due to the severe stacking property of bolts and the single-view nature of the sensor, directly invoking traditional segmentation methods such as region growing clustering segmentation based on normal and curvature and Euclidean clustering segmentation based on point distance cannot achieve an ideal segmentation effect. Summary of the Invention

[0004] To solve the above problems, the present invention proposes an improved bolt pose estimation method.

[0005] An improved bolt pose estimation method, the specific steps are as follows:

[0006] S1. Create a virtual bolt dataset for training;

[0007] S2. Use the FPCC instance segmentation network based on deep learning to extract the features of each point, and use non-maximum suppression to find the center point for each instance;

[0008] S3. Set the point with the highest score as the geometric center point of each instance, and cluster the remaining points to the nearest geometric center based on the network clustering algorithm to segment the instance;

[0009] S4. After segmenting the instance, perform cylinder fitting, use the improved RANSAC algorithm to re-estimate the parameters of a cylinder, obtain the direction vector of the cylinder axis, and determine the direction of the bolt central axis.

[0010] After the end of the step S3, it is also necessary to calculate the rigid body transformation matrix between corresponding points, and then judge the performance of the current registration transformation by solving the distance error sum function of the transformed corresponding points.

[0011] The specific steps for calculating the rigid body transformation matrix between corresponding points are as follows:

[0012] a. Select n sampling points from the point cloud P to be registered: the distance between the sampling points satisfies being greater than a pre-given minimum distance threshold d, ensuring that the sampled points have different FPFH features;

[0013] b. Search for one or more points in the target point cloud Q that have similar FPFH features to the sampling points in the point cloud P, and randomly select one point from these similar points as the corresponding point of the point cloud P in the target point cloud Q;

[0014] c. Calculate the rigid body transformation matrix between corresponding points, and then judge the performance of the current registration transformation by solving the distance error sum function of the transformed corresponding points.

[0015] The distance error sum function here is represented by the Huber penalty function, where mi is a pre-given value and li is the distance difference after transformation of the i-th group of corresponding points.

[0016] The registration of corresponding points is specifically realized by combining SAC_IA rough registration and NDT fine registration.

[0017] The specific steps of the NDT algorithm are as follows:

[0018] A. Take the two point clouds P' and Q after initial registration as the initial point sets for fine registration;

[0019] B. For each point pi in the source point cloud P' after coordinate transformation: find the closest corresponding point qi in the target point cloud Q as the corresponding point of this point in the target point cloud, and form the initial corresponding point pairs;

[0020] C. Eliminate the wrong corresponding point pairs: the corresponding relationships in the initial corresponding point set are not all correct, and the wrong corresponding relationships will affect the final registration result. Use the direction vector threshold to eliminate the wrong corresponding point pairs;

[0021] D. Calculate the rotation matrix R and the translation vector T: make the value minimum, that is, the mean square error between the corresponding point sets is the minimum;

[0022] E. Set the value: set a certain threshold ε = dk - dk-1 and the maximum number of iterations Nmax. Apply the rigid body transformation obtained in the previous step to the source point cloud P' to obtain the new point cloud P′′, and calculate the distance error between P" and Q.

[0023] Use the improved RANSAC algorithm to re - estimate the parameters of a cylinder. The specific steps are as follows:

[0024] a. Randomly select a set of points as the initial inlier set;

[0025] b. Randomly select the minimum sample set from the inlier set, and estimate the parameters of the cylinder according to the minimum sample set;

[0026] c. Calculate the distance from other points to the estimated cylinder model. For each inlier, calculate its error from the fitting model. If the error is less than Tolerance, regard the error as cost; if the error is greater than Tolerance, set "Tolerance" as cost; regard the cost of all inliers as the total cost;

[0027] d. If the number of the current inlier set is greater than the previous maximum inlier set, update the maximum inlier set;

[0028] e. Repeat steps b to d until the preset number of iterations is reached;

[0029] f. Use the maximum inlier set to re - estimate the cylinder parameters;

[0030] g. Considering the axial symmetry of the bolt and the obvious axial characteristics, fuse the cylindrical fitting in the improved RANSAC algorithm to obtain the direction vector of the cylinder axis and determine the direction of the bolt central axis.

[0031] The beneficial effects of the present invention are as follows: Based on the FPCC instance segmentation network, combined with the SAC - IDBSCA coarse registration algorithm, the NDT fine registration algorithm and the improved RANSAC algorithm, a new bolt pose estimation method is proposed. The FPCC instance segmentation network proposed by the present invention can still achieve fast and effective segmentation for bolts in a highly stacked situation without any training of manually labeled data, and has a high segmentation accuracy. The improved RANSAC algorithm proposed by the present invention can effectively compensate for the negative impact on the algorithm effect caused by improper threshold selection in the traditional algorithm. Description of the Drawings

[0032] The following further illustrates the present invention in conjunction with the drawings and embodiments.

[0033] Figure 1 It is the effect diagram of the FPCC instance segmentation network of the present invention;

[0034] Figure 2 It is the pose estimation effect of the improved RANSAC algorithm of the present invention Figure 1 ;

[0035] Figure 3Pose Estimation Effect of the Improved RANSAC Algorithm of the Present Invention Figure 2 。 Detailed Implementation Manner

[0036] In order to make the technical means, creative features, achieved purposes and effects realized by the present invention easy to understand, the present invention will be further described below.

[0037] As Figures 1 to 3 shown, an improved bolt pose estimation method has the following specific steps:

[0038] S1. Create a virtual bolt dataset for training in the Pybullet simulation environment in combination with Open3D; randomly drop N bolts in a fixed bin, and record the corresponding color images, depth images, and segmentation information;

[0039] S2. Use the FPCC instance segmentation network based on deep learning to extract the features of each point, and use non-maximum suppression to find the center point for each instance; first, consider the points with a score greater than 0.6 as candidate points, then select the point with the highest score as the center point, and remove the remaining points according to the distance from the center point to the farthest point on the instance;

[0040] S3. Introduce a feature distance matrix to make the points belonging to the same instance as close as possible in the feature space, while the points of different instances need to be distinguished from other instances as much as possible; in order to make the point cloud features of the same instance as similar as possible, researchers introduced the following metrics to define the feature distance matrix, so that the point clouds of the same instance are as close as possible in the feature space;

[0041]

[0042] S4. Introduce a binary effective distance matrix; in the inference stage of the model, the point cloud is clustered through both the feature distance and the Euclidean distance between points. If the Euclidean distance exceeds twice the maximum distance d_max, these two points will not belong to the same instance; then this point needs to be ignored so that it does not generate a loss; this method enables the network to effectively judge whether the points within a certain Euclidean distance belong to the same instance, and its definition is as follows:

[0043]

[0044] S5. In addition, the design of the center score is for the distance between a point and the center of its instance; within the boundary, the closer the point is to the center, the higher the center score will be, and the center score is defined in the following form:

[0045]

[0046] where β is a constant exponent and c_i are the coordinates of the center points; usually, β is set to 2, so that the scores of points near the boundary are close to 0, while the scores of points near the center are close to 1;

[0047] S6. Set the point with the highest score as the geometric center point of each instance; the network clustering algorithm clusters the remaining points to the nearest geometric center to segment the instance; this clustering algorithm does not generate candidate groups, but directly generates instances based on the feature distance between the object center point and other points;

[0048] S7. Select n sampling points from the point cloud P to be registered. To ensure that the sampled points have different FPFH features as much as possible, the distance between the sampling points should satisfy being greater than a pre-given minimum distance threshold d;

[0049] S8. Find one or more points in the target point cloud Q that have similar FPFH features to the sampling points in the point cloud P, and randomly select one point from these similar points as the corresponding point of the point cloud P in the target point cloud Q;

[0050] S9. Calculate the rigid body transformation matrix between the corresponding points, and then judge the performance of the current registration transformation by solving the "distance error sum" function after the corresponding points are transformed; here, the distance error sum function is mostly represented by the Huber penalty function, where mi is a pre-given value and li is the distance difference after the transformation of the i-th group of corresponding points; the ultimate goal of the above registration is to find an optimal transformation among all transformations to minimize the value of the error function. At this time, the transformation is the final registration transformation matrix, and the registration result can be further obtained;

[0051] Since the transformation matrix obtained by SAC-IA is inaccurate, it can only be used for rough registration. When the number of points is large, calculating the FPFH feature is slow, making the SAC-IA algorithm very inefficient. At this time, it is necessary to first perform downsampling processing on the point cloud to reduce the number of points, but this will cause some feature points to be lost, reducing the registration accuracy; therefore, it is necessary to adopt the iterative closest point algorithm, that is, the NDT algorithm, which has a better effect on the segmentation of bolt point clouds in a stacked scenario compared to traditional point cloud segmentation algorithms;

[0052] S10. Use the two point clouds P′ and Q after initial registration as the initial point sets for fine registration;

[0053] S11. For each point pi in the source point cloud P’ after coordinate transformation, find the corresponding point qi with the closest distance in the target point cloud Q as the corresponding point of this point in the target point cloud, and form the initial corresponding point pairs;

[0054] S12. The corresponding relationships in the initial corresponding point set are not all correct. The incorrect corresponding relationships will affect the final registration result. Use the direction vector threshold to eliminate the incorrect corresponding point pairs;

[0055] S13. Calculate the rotation matrix R and the translation vector T to minimize, i.e., minimize the mean square error between the corresponding point sets.

[0056] S14. Set a certain threshold ε = dk - dk-1 and the maximum number of iterations Nmax. Apply the rigid body transformation obtained in the previous step to the source point cloud P′ to obtain a new point cloud P″. Calculate the distance error between P″ and Q. If the error between two iterations is less than the threshold ε or the current number of iterations is greater than Nmax, the iteration ends. Otherwise, update the initially registered point sets to P″ and Q, and continue to repeat the above steps until the convergence condition is satisfied.

[0057] In cylinder fitting, the traditional RANSAC algorithm determines the inlier set completely based on Tolerance. The algorithm is too sensitive to the selection of Tolerance. If the selected value is too large, the algorithm will fail; if the selected value is too small, the algorithm will be unstable. An improved RANSAC algorithm can be used to re-estimate the parameters of a cylinder, reducing the impact on the bolt fitting effect caused by improper threshold selection, such as the center of the circle and the radius. The specific steps are as follows:

[0058] S15. Randomly select a set of points as the initial inlier set.

[0059] S16. Randomly select the minimum sample set from the inlier set and estimate the parameters of the cylinder according to the minimum sample set.

[0060] S17. Calculate the distances from other points to the estimated cylinder model. For each inlier, calculate its error from the fitted model. If the error is less than Tolerance, regard the error as cost; if the error is greater than Tolerance, set "Tolerance" as cost. Consider the cost of all inliers as the total cost.

[0061] S18. If the number of the current inlier set is greater than the previous maximum inlier set, update the maximum inlier set.

[0062] S19. Repeat steps S16 to S18 until the preset number of iterations is reached.

[0063] S20. Re-estimate the cylinder parameters using the maximum inlier set.

[0064] S21. Considering the obvious axial symmetry and axial characteristics of the bolt, fuse the cylindrical fitting in the improved RANSAC algorithm to obtain the direction vector of the cylinder axis and determine the direction of the bolt central axis.

[0065] Due to the severe stacking property of bolts, traditional image segmentation algorithms have poor processing effects on bolt scenarios, which will have a serious impact on bolt pose estimation and robot grasping.

[0066] Based on the FPCC instance segmentation network, this invention proposes a new bolt pose estimation method by combining the SAC-IA coarse registration algorithm, the NDT fine registration algorithm and the improved RANSAC algorithm.

[0067] The FPCC instance segmentation network proposed in this invention can still achieve fast and effective segmentation for bolts in a highly stacked situation, without the training of any manually labeled data, and has a high segmentation accuracy.

[0068] The improved RANSAC algorithm proposed in this invention can effectively compensate for the negative impact of the traditional algorithm on the algorithm effect due to improper threshold selection.

[0069] The above shows and describes the basic principles, main features and advantages of this invention. Those skilled in the art should understand that this invention is not limited by the above embodiments. What is described in the above embodiments and the specification is only the principle of this invention. Without departing from the spirit and scope of this invention, this invention will have various changes and improvements, and these changes and improvements all fall within the scope of this invention claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. An improved bolt pose estimation method, characterized in that: The specific steps are as follows: S1. Create a virtual bolt dataset for training; S2, using the FPCC instance segmentation network based on deep learning to extract the features of each point, and using non-maximum suppression to find the center point for each instance; S3, set the point with the highest score as the geometric center point of each instance, and cluster the remaining points to the nearest geometric center based on the network clustering algorithm to segment the instance; S4. After segmenting the instance, cylinder fitting is performed, and the improved RANSAC algorithm is used to re-estimate the parameters of a cylinder, obtain the direction vector of the cylinder axis and determine the direction of the bolt centerline.

2. The improved bolt pose estimation method according to claim 1, characterized in that: After the step S3 is completed, it is necessary to calculate the rigid body transformation matrix between the corresponding points, and then determine the performance of the current registration transformation by solving the distance error and function after the corresponding points are transformed.

3. An improved bolt pose estimation method according to claim 2, characterized in that: The specific steps for calculating the rigid body transformation matrix between corresponding points are: a. Select n sampling points from the point cloud P to be registered: the distance between the sampling points is greater than the pre-given minimum distance threshold d, ensuring that the sampled points have different FPFH features; b. Find one or more points in the target point cloud Q that have similar FPFH features to the sampling points in the point cloud P, and randomly select a point from these similar points as the corresponding point of the point cloud P in the target point cloud Q; c. Calculate the rigid body transformation matrix between the corresponding points, and then determine the performance of the current registration transformation by solving the distance error and function after the corresponding points are transformed.

4. The improved bolt pose estimation method according to claim 3, characterized in that: The distance error and function here are expressed using the Huber penalty function, where mi is a predetermined value and li is the distance difference after the transformation of the i-th group of corresponding points.

5. The improved bolt pose estimation method according to claim 3, characterized in that: The registration of corresponding points is specifically achieved by combining SAC_IA coarse registration with NDT fine registration.

6. The improved bolt pose estimation method according to claim 5, characterized in that: The specific steps of the NDT algorithm are as follows: A. Use the two point clouds P' and Q after initial registration as the initial point set for fine registration; B. For each point pi in the source point cloud P' after coordinate transformation: find the closest corresponding point qi in the target point cloud Q as the corresponding point of the point in the target point cloud to form an initial corresponding point pair; C. Eliminate incorrect corresponding point pairs: The corresponding relationships in the initial corresponding point set are not all correct. Incorrect corresponding relationships will affect the final registration results. Direction vector thresholds are used to eliminate incorrect corresponding point pairs. D. Calculate the rotation matrix R and the translation vector T: minimize the value, that is, minimize the mean square error between the corresponding point sets; E. Set the value: Set a threshold ε=dk-dk-1 and the maximum number of iterations Nmax, apply the rigid body transformation obtained in the previous step to the source point cloud P', obtain the new point cloud P", and calculate the distance error between P" and Q.

7. The improved bolt pose estimation method according to claim 1, characterized in that: The improved RANSAC algorithm is used to re-estimate the parameters of a cylinder. The specific steps are as follows: a. Randomly select a set of points as the initial internal point set; b. Randomly select a minimum sample set from the interior point set and estimate the parameters of the cylinder based on the minimum sample set; c. Calculate the distances from other points to the estimated cylindrical model. For each in-game point, calculate the error between it and the fitted model. If the error is less than Tolerance, the error is regarded as cost. If the error is greater than Tolerance, "Tolerance" is set as cost. The cost of all in-game points is regarded as the total cost. d. If the number of the current internal point set is greater than the previous maximum internal point set, update the maximum internal point set; e. Repeat steps b to d until the preset number of iterations is reached; f. Re-estimate the cylinder parameters using the maximum interior point set; g. Considering the axial symmetry and obvious axial characteristics of the bolt, the cylindrical fitting in the improved RANSAC algorithm is integrated to obtain the direction vector of the cylinder axis and determine the direction of the bolt centerline.

Images