Unordered grasping posture estimation method based on FPFH and ICP improved algorithm
Through the disorderly grasping attitude estimation method of FPFH and ICP improved algorithms, the problem of insufficient real-time and accuracy of two-dimensional image recognition in industrial sites is solved, and efficient three-dimensional object recognition and attitude estimation are realized, which is suitable for industrial sites without Order crawl task.
Patent Information
- Application Number
- CN202111342724.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-11-12
- Publication Date
- 2024-11-01
- Estimated Expiration
- 2041-11-12
AI Technical Summary
The existing two-dimensional images cannot meet the real-time and accuracy requirements based on object recognition and pose estimation in robot navigation and industrial sites with improved automation, and the three-dimensional pose estimation algorithm takes a long time and cannot achieve real-time grabbing.
The disorderly grasping pose estimation method based on FPFH and ICP improved algorithms is adopted. By obtaining the workpiece surface point cloud data for preprocessing, the rough matching pose transformation matrix is obtained using the FPFH algorithm, and the ICP iterative nearest point algorithm is used for accurate pose estimation, combined with Both compute the final pose transformation matrix.
It improves the accuracy and real-time nature of three-dimensional object recognition and posture estimation, can effectively suppress the influence of noise in industrial sites, meet the accuracy requirements of disorderly grasping tasks, and ensure real-time nature.
Smart Images

Figure CN114092553B_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates to the technical field of three-dimensional posture estimation, and in particular to a method, device, control system and readable storage medium for out-of-order grasping posture estimation based on FPFH and ICP improved algorithms. Background Art
[0002] At present, there have been many research results on object recognition and posture estimation based on images. However, the imaging process of two-dimensional images is to map from three-dimensional space to two-dimensional space, and this process will lose a lot of information. Moreover, a fact that cannot be ignored is that the best visual system should be oriented to the three-dimensional world. With the improvement of automation, many fields such as robot navigation, industrial parts inspection and grasping have higher and higher requirements for computer vision systems. Therefore, object recognition and posture estimation based on two-dimensional images can no longer meet human requirements. At present, the acquisition of three-dimensional point cloud data is very fast. At the same time, the collection of three-dimensional point cloud data is not affected by light, avoiding the problems of light and posture encountered by two-dimensional images. Therefore, three-dimensional object recognition and posture estimation based on point cloud data have also attracted people's attention.
[0003] In the process of disordered grasping, it is necessary to seek a disordered grasping posture estimation method based on the improved FPFH and ICP algorithms that is robust, fast, and meets a certain degree of accuracy. Traditional three-dimensional posture estimation is generally divided into two stages: coarse matching stage and fine registration stage. Traditional object recognition and three-dimensional posture estimation coarse matching methods, such as the point feature histogram PFH proposed by Rusu (Radu Bogdan Rusu, Nico Blodow, Zoltan Csaba Marton, et al. Aligning Point Cloud Views using Persistent Feature Histograms [C] / / 2008IEEE / RSJ International Conference on Intelligent Robots and Systems, September 22-26, 2008, Acropolis Convention Center, Nice, France. IEEE, 2008.), calculate the normal vector angle and distance between any two vertices, map them into the histogram to obtain statistical information, and solve the spatial transformation relationship. This algorithm takes a long time and cannot meet the real-time requirements. Subsequently, Tombari proposed SHOT (Tombari F, Salti S, Di Stefano L. Unique signatures of histograms for local surface description [C] / / European conference on computer vision. Springer, Berlin, Heidelberg, 2010: 356-369.). Compared with PFH, although it improves the computational efficiency, its object pose estimation accuracy and real-time performance are low, and it is not suitable for 3D pose estimation of disordered grasping in the current industry. Summary of the invention
[0004] In view of this, the present invention proposes a method and system for unordered grasping posture estimation based on FPFH and ICP improved algorithms to solve the problems of object posture estimation accuracy and real-time performance in the unordered grasping process.
[0005] According to one aspect of the present disclosure, a method for estimating unordered grasping posture based on FPFH and ICP improved algorithms is provided, comprising the following steps:
[0006] S100, obtaining workpiece surface point cloud data and performing preprocessing; and,
[0007] S200, use FPFH object recognition and 3D posture estimation algorithm to process the pre-processed point cloud data to obtain the rough matching posture transformation matrix ;as well as,
[0008] S300, using the ICP iterative closest point improved algorithm, accurately estimate the pose of the closest point of the model point cloud in the scene point cloud and obtain the pose transformation matrix ;as well as,
[0009] S400, according to the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result.
[0010] In a possible implementation, optionally, in step S200, the pre-processed point cloud data is processed using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix ,include:
[0011] S210, performing multi-threaded normal estimation on the scene point cloud and the model point cloud respectively, and extracting point cloud key points respectively by downsampling; and,
[0012] S220, constructing local reference systems on the scene point cloud and the model point cloud respectively, calculating the topological features of the point cloud key points within the feature calculation radius under the corresponding local reference systems, and saving the topological features in a histogram; and,
[0013] S230, perform object recognition through Hough transform, determine the correct feature matching point pair, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pair, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain a coarse matching pose transformation matrix .
[0014] In a possible implementation, optionally, in step S300, the ICP iterative closest point improvement algorithm is used to accurately estimate the pose of the closest point of the model point cloud in the scene point cloud to obtain the pose transformation matrix ,include:
[0015] S310, searching for the nearest point of the model point cloud in the scene point cloud and calculating the corresponding relationship, and constructing a covariance matrix H according to the corresponding relationship; and
[0016] S320, according to the constructed covariance matrix H and the introduced ICP improved objective function, solving the rotation matrix R and the translation vector that minimize the objective function ;as well as,
[0017] S330: The rotation matrix R and the translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix .
[0018] In a possible implementation, optionally, in step S400, the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result, including:
[0019] S410, obtaining the rough matching posture transformation matrix and the attitude transformation matrix ;as well as,
[0020] S420: The obtained posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix; and
[0021] S430, converting the obtained final posture transformation matrix into the form of Euler angles and translation vectors, and displaying the final posture estimation result according to the conversion result.
[0022] According to another aspect of the present disclosure, a device for implementing the method for estimating the disordered grasping posture based on the improved FPFH and ICP algorithms is provided, comprising a point cloud data acquisition module, an FPFH algorithm processing module, an ICP algorithm processing module and a posture estimation module, wherein:
[0023] The point cloud data acquisition module is used to acquire the workpiece surface point cloud data and perform preprocessing;
[0024] The FPFH algorithm processing module is used to process the pre-processed point cloud data using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix
[0025] The ICP algorithm processing module is used to use the ICP iterative closest point improvement algorithm to accurately estimate the closest point of the model point cloud in the scene point cloud and obtain the attitude transformation matrix
[0026] The posture estimation module is used to match the posture transformation matrix according to the rough matching and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result.
[0027] In a possible implementation manner, optionally, the FPFH algorithm processing module includes:
[0028] Point cloud key point extraction module: used to perform multi-threaded normal estimation on scene point cloud and model point cloud respectively, and extract point cloud key points respectively through downsampling;
[0029] Topological feature calculation module: used to construct local reference systems on the scene point cloud and the model point cloud respectively, calculate the topological features of the key points of the point cloud within the feature calculation radius under the corresponding local reference systems, and save the topological features in the histogram;
[0030] Pose transformation module: used to identify objects through Hough transform, determine the correct feature matching point pairs, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pairs, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain the rough matching pose transformation matrix .
[0031] In a possible implementation, optionally, the ICP algorithm processing module includes:
[0032] Covariance matrix construction module: used to search for the nearest point of the model point cloud in the scene point cloud and calculate the corresponding relationship, and construct the covariance matrix H according to the corresponding relationship;
[0033] Solution calculation module: used to improve the objective function according to the constructed covariance matrix H and the introduced ICP, and solve the rotation matrix R and translation vector that minimize the objective function ;
[0034] Iteration module: used to transform the rotation matrix R and translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix .
[0035] In a possible implementation, optionally, the posture estimation module includes:
[0036] Estimation data acquisition module: used to obtain the rough matching posture transformation matrix and the attitude transformation matrix ;
[0037] Calculation module: used to obtain the posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix;
[0038] Visualization module: used to convert the final posture transformation matrix obtained into the form of Euler angles and translation vectors, and display the final posture estimation result according to the conversion result.
[0039] According to another aspect of the present disclosure, there is also provided a control system, comprising:
[0040] processor;
[0041] a memory for storing processor-executable instructions;
[0042] Wherein, the processor is configured to implement the unordered grasping posture estimation method based on FPFH and ICP improved algorithms when executing the executable instructions.
[0043] According to another aspect of the present disclosure, a non-volatile computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the method for estimating an unordered grasping posture based on the FPFH and ICP improved algorithms is implemented.
[0044] Technical effects of this application:
[0045] The present invention obtains the workpiece surface point cloud data and pre-processes it; and uses FPFH object recognition and three-dimensional posture estimation algorithm to process the pre-processed point cloud data to obtain a rough matching posture transformation matrix. ; And, using the ICP iterative closest point improvement algorithm, the model point cloud is accurately estimated at the closest point in the scene point cloud to obtain the attitude transformation matrix ; and, according to the coarse matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result. The three-dimensional posture estimation method based on FPFH and ICP algorithms in disordered grasping can be used for three-dimensional object recognition and posture estimation in industrial sites, and solve the problem of object posture estimation accuracy and real-time performance in the disordered grasping process. The FPFH algorithm (fast point feature histogram method, fast point feature histograms, FPFH) object recognition and three-dimensional posture estimation method is adopted, which has rotation invariance and robustness to noise, and can well suppress the influence of industrial site noise. In order to further improve the accuracy of posture estimation, it is necessary to apply a fine registration algorithm after the rough registration method of object recognition and three-dimensional posture estimation. This method adopts the (ICP) iterative closest point algorithm, and in view of the existence of certain noise, occlusion, and reflection in industrial scenes, which easily causes uneven distribution of workpiece point clouds and holes, the objective function of ICP is modified to make the registration more accurate, thereby obtaining a more accurate posture transformation matrix.
[0046] Further features and aspects of the present disclosure will become apparent from the following detailed description of exemplary embodiments with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate exemplary embodiments, features, and aspects of the disclosure and, together with the description, serve to explain the principles of the disclosure.
[0048] Figure 1 It is a schematic diagram of the implementation process of the method for estimating the disordered grasping posture based on the FPFH and ICP improved algorithms of the present invention;
[0049] Figure 2 It is shown as a visual schematic diagram of a recognition result;
[0050] Figure 3 The figure shows a comparison diagram of the pose estimation accuracy of the original ICP algorithm and the ICP algorithm with the improved objective function;
[0051] Figure 4 The figure shows a comparison diagram of the pose estimation accuracy of the FPFH algorithm and the improved ICP algorithm according to the present invention. DETAILED DESCRIPTION
[0052] Various exemplary embodiments, features and aspects of the present disclosure will be described in detail below with reference to the accompanying drawings. The same reference numerals in the accompanying drawings represent elements with the same or similar functions. Although various aspects of the embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless otherwise specified.
[0053] The word “exemplary” is used exclusively herein to mean “serving as an example, example, or illustration.” Any embodiment described herein as “exemplary” is not necessarily to be construed as preferred or advantageous over other embodiments.
[0054] In addition, in order to better illustrate the present disclosure, numerous specific details are given in the following specific embodiments. It should be understood by those skilled in the art that the present disclosure can also be implemented without certain specific details. In some examples, methods, means, components and circuits well known to those skilled in the art are not described in detail in order to highlight the subject matter of the present disclosure.
[0055] The present invention provides a good initial position for ICP fine registration with the FPFH rough matching method, constructs the M matrix in the FPFH algorithm, decomposes the eigenvalues, and then constructs a local reference system; calculates the topological features under the corresponding reference system, and saves the results in a histogram, and then uses a random sampling consistency algorithm to eliminate the wrong point pairs from the feature extraction results corresponding to the model and the scene point cloud, and solves to obtain the final rough registration posture estimation result; the ICP algorithm searches for the nearest point, constructs the covariance matrix, and solves the rotation matrix and translation vector that minimize the objective function; according to the problem of uneven distribution and holes of the workpiece point cloud caused by noise, occlusion, and reflection in the industrial scene, the objective function is improved, and the problem of reduced registration accuracy when the traditional ICP is applied to the point cloud with holes collected due to the reflection of the workpiece is solved. The FPFH algorithm has high robustness to noise, rotation invariance, strong recognition ability, and can provide a stable and good initial position for the ICP algorithm; the ICP algorithm further improves the accuracy of posture estimation, so that the system can meet the accuracy requirements in the grasping task, and the two interact to improve the accuracy of posture estimation and ensure real-time performance.
[0056] When the present application is used, the calculation radius of the local coordinate system and the feature descriptor and the interpolation interval of the Hough parameter space are adjusted according to the different characteristics of different workpieces in the industrial field, and the running speed of the method can meet the real-time requirements.
[0057] Example 1
[0058] like Figure 1 As shown, according to one aspect of the present disclosure, a method for estimating unordered grasping posture based on FPFH and ICP improved algorithms is provided, comprising the following steps:
[0059] S100, obtaining workpiece surface point cloud data and performing preprocessing;
[0060] There is no restriction on the method of acquiring the workpiece surface point cloud data.
[0061] This embodiment preferentially uses a Photoneo surface structured light three-dimensional camera to obtain workpiece surface point cloud data (and simultaneously obtains a corresponding two-dimensional image).
[0062] After acquisition, because the surface of the workpiece in the industrial site is smooth, there will be occlusion, reflection, etc., and there will be large holes in the workpiece point cloud, so the acquired point cloud data needs to be pre-processed and point cloud filtering is performed. Figure 2 The visualization diagram of a recognition result is shown in the figure. The object to be tested is a workpiece of a car engine. In the main view window (the large window on the left), the white point cloud is the effect of applying the final posture transformation matrix to transform the model point cloud into the scene; the black and gray content is the scene point cloud; the model view window (the small window on the right) is the initial model point cloud (i.e., the point cloud that has not been processed by the posture transformation matrix). The surface of the workpiece on the industrial site is smooth, and there will be occlusion and reflection, etc., and there are large holes in the workpiece point cloud. The window also includes the loading model, recognition test, disordered grabbing, stop and run buttons. The model point cloud of the workpiece to be tested is loaded by loading the model; the recognition test is used to test whether the workpiece to be tested can be successfully recognized and complete the three-dimensional posture estimation; the disordered grabbing will control the manipulator to perform the corresponding grabbing action according to the recognition test results.
[0063] A surface structured light camera is used to capture two-dimensional images of the workpiece (multiple two-dimensional images are obtained for each type of workpiece that needs to be assembled on the production line, and 5,000 images of each type are selected in this embodiment), and most of the background is filtered out to reduce background interference and the amount of calculation; without affecting the point cloud representation capability, the point cloud is downsampled to further reduce the amount of calculation.
[0064] S200, use FPFH object recognition and 3D posture estimation algorithm to process the pre-processed point cloud data to obtain the rough matching posture transformation matrix ;
[0065] Apply FPFH object recognition and 3D posture estimation algorithms to process the preprocessed point cloud data. Perform multi-threaded normal estimation on the scene and model point clouds (the model point cloud is generated by software simulation or the scene point cloud is formed by completely intercepting the workpiece part to form the model point cloud). Downsampling is performed to extract the key points of the point cloud. The downsampling scale is selected to ensure that the algorithm running time meets the actual application requirements. After sampling, local reference systems are constructed on the scene and model point clouds respectively, and the topological features of the key points within the feature calculation radius in the corresponding reference system are calculated, and the features are saved in the histogram; finally, the Hough transform is used to obtain the posture transformation relationship between the scene and model point clouds, and the transformation relationship is applied to the model point cloud to make the model point cloud closer to the scene point cloud. The rough matching posture transformation matrix is represented by a 4×4 matrix. .
[0066] FPFH is rotationally invariant and robust to noise, and can effectively suppress the influence of industrial site noise.
[0067] S300, using the ICP iterative closest point improved algorithm, accurately estimate the pose of the closest point of the model point cloud in the scene point cloud and obtain the pose transformation matrix ;
[0068] In this embodiment, kdtree is used to search for the nearest point of the model point cloud in the scene point cloud, and the covariance matrix H is constructed according to the corresponding relationship found. Then, based on the constructed covariance matrix H, the rotation matrix R and the translation vector that minimize the objective function are solved. , return the nearest point of the search model point cloud in the scene point cloud for the next iteration; calculate the mean square error between the corresponding points, if it is less than the threshold, the iteration stops; in fact, when the program is implemented, the termination condition of the iteration is to calculate the difference in rotation angle between the two iterations before and after, and if the difference is less than the difference threshold, the problem is considered to have converged and the iteration is terminated; the posture transformation matrix obtained is .
[0069] The ICP iterative closest point improved algorithm can modify the ICP objective function to make the registration more precise, thereby obtaining a more accurate posture transformation matrix, in order to solve the problem of uneven distribution of workpiece point clouds and holes caused by certain noise, occlusion, and reflection in industrial scenes. Figure 3 As shown, it is a comparison chart of the accuracy of the original ICP algorithm and the ICP algorithm with the improved objective function. The horizontal axis is the number of points surrounding the hole, and the vertical axis is the mean square error. In the experiment in the figure, holes of different sizes are artificially created in the workpiece point cloud under the same workpiece and the same scene. On this basis, the mean square error (RMSE) of the original ICP algorithm and the improved ICP algorithm when there are holes of different sizes in the workpiece point cloud is compared. It can be seen that as the hole increases, the improved ICP algorithm significantly improves the registration accuracy compared to the original ICP algorithm, and is more suitable for industrial sites. Regardless of whether the hole is large or small, the mean square error of the ICP algorithm of this application is smaller than that of the original ICP algorithm.
[0070] S400, according to the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result.
[0071] The obtained posture transformation matrix With the attitude transformation matrix Multiply them to get the final attitude transformation matrix; convert the obtained attitude transformation matrix into the form of Euler angles and translation vectors, which is the final attitude estimation result.
[0072] The FPFH algorithm is highly robust to noise, has rotation invariance, and has strong recognition capabilities; it can provide a stable and good initial position for the ICP algorithm; the ICP algorithm further improves the accuracy of pose estimation, enabling the system to meet the accuracy requirements in grasping tasks.
[0073] like Figure 4 The figure shows the comparison of the pose estimation accuracy of the FPFH algorithm and the improved ICP algorithm. The experiment in the figure is conducted in an industrial site. The pose of the workpiece is randomly adjusted each time, and the workpiece point cloud is collected and preprocessed. According to the characteristics of the workpiece (such as the size of the depression or protrusion), the local reference system construction radius, feature calculation radius, and number of iterations of the FPFH feature descriptor are adjusted; the threshold of the rotation angle difference between the two iterations of the ICP algorithm is set to , the maximum number of iterations is limited to 50. On this basis, the FPFH object recognition and initial registration algorithm and the ICP fine registration algorithm are used to estimate the workpiece pose. 100 independent experiments are conducted, and the average value of the point correspondence distance between the model point cloud and the target point cloud (scene point cloud) is calculated each time, which is used as the standard for measuring the error; the average accuracy of 100 experiments is calculated to observe the overall effect.
[0074] like Figure 4 As shown, it can be seen that when the FPFH initial registration algorithm provides the initial pose, ICP can always reduce the error; the horizontal axis represents the number of experiments, and the vertical axis represents the accuracy, in mm. For the case where the initial pose error is large, the effect of ICP in reducing the error is particularly obvious. The average accuracy of FPFH initial registration is 2.59543mm, and the average accuracy (mean ICP) of ICP fine registration is 2.26341mm. Due to the different shooting positions and angles, there are often large differences between the model point cloud and the scene point cloud, and the actual error is smaller than the experimental result. In the experiment, 100 experiments can always make the posture estimation problem converge and achieve a higher accuracy, which proves that the algorithm is more robust and accurate, and can meet the general needs of disordered grasping. In this embodiment, a Lenovo Savior laptop computer, a seventh-generation i5, 2.5GHz, is used, and the detection time for one recognition result is 1200ms.
[0075] In a possible implementation, optionally, in step S200, the pre-processed point cloud data is processed using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix ,include:
[0076] S210, performing multi-threaded normal estimation on the scene point cloud and the model point cloud, and extracting point cloud key points respectively by downsampling;
[0077] Multi-threaded normal estimation and downsampling are performed on the scene and model point clouds (the model point cloud is generated by software simulation or the workpiece part is completely cut out in the scene point cloud to form the model point cloud). The downsampling scale is selected to ensure that the algorithm running time meets the actual application requirements. In order to improve the recognition success rate, the number of scene point cloud key points is kept 3 to 4 times the number of model point cloud key points.
[0078] S220, constructing local reference systems on the scene point cloud and the model point cloud respectively, calculating the topological features of the point cloud key points within the feature calculation radius under the corresponding local reference systems, and saving the topological features in a histogram;
[0079] On the scene and model point clouds, local reference systems are constructed respectively, and the topological features of key points within the feature calculation radius in the corresponding reference system are calculated, and the features are saved in the histogram; the local reference system construction radius r is selected to obtain the size of the part of the workpiece that is different from the plane (such as depressions, protrusions, holes), and the construction radius is kept consistent with it;
[0080] Set the feature calculation radius to 4 / 3 of the reference frame construction radius;
[0081] For each key point p , and p is the center of the sphere and the radius is r Sphere points within q ,calculate:
[0082]
[0083]
[0084]
[0085] Pair Matrix M Do eigenvalue decomposition:
[0086]
[0087] The local reference system is constructed based on the obtained eigenvectors, where V is a matrix consisting of the three eigenvectors of the matrix M. It is a sphere The three eigenvectors with the direction of high point density as the positive direction correspond to the local reference system. x, y, z axis.
[0088] In the local reference system, within the characteristic calculation radius, it is divided into 32 areas according to 8 azimuth, 2 elevation, and 2 radial areas; calculation point q The coordinates in the local reference system and calculate the angle between the origin of the coordinate system and the vector at that point θ At the same time, for the points at the edge, the pitch, radial, and azimuth angles as well as the Euclidean distance to the origin are quadrilinearly interpolated and the results are stored in a histogram.
[0089] S230, perform object recognition through Hough transform, determine the correct feature matching point pair, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pair, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain a coarse matching pose transformation matrix .
[0090] Hough transform is used to identify objects, determine the correct feature matching point pairs, solve the pose transformation relationship between the scene and the model point cloud based on the feature matching point pairs, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud; use a 4×4 matrix It represents the rough matching posture transformation matrix. Its 3×3 matrix at the top corner is the rotation matrix. The first three elements of the last column are translation vectors, the fourth element is 1, and the first three elements of the last row are zero. The interpolation interval of the Hough parameter space is set to be consistent with the sampling size of the model feature point extraction, and the Hough voting threshold is set to 5.0. According to the different characteristics of different workpieces in the industrial site, the local coordinate system and feature calculation radius are adjusted, and the regional division interval of the Hough parameter is adjusted.
[0091] In a possible implementation, optionally, in step S300, the ICP iterative closest point improvement algorithm is used to accurately estimate the pose of the closest point of the model point cloud in the scene point cloud to obtain the pose transformation matrix ,include:
[0092] S310, searching for the nearest point of the model point cloud in the scene point cloud and calculating the corresponding relationship, and constructing a covariance matrix H according to the corresponding relationship;
[0093] Use kdtree to search for the nearest point of the model point cloud in the scene point cloud, and construct the covariance matrix H based on the corresponding relationship found;
[0094] make represents a point in the model point cloud, represents a point in the scene point cloud, yes The nearest neighbor of , then:
[0095] ,
[0096] ,
[0097] ,
[0098] in, is the center point of the model point cloud, is the center point of the scene point cloud (which may be a part of the points) corresponding to the model, N is the total number of point pairs, the matrix H is the constructed covariance matrix.
[0099] S320, according to the constructed covariance matrix H And the introduced ICP improved objective function, solving the rotation matrix that minimizes the objective function R and translation vectors ;
[0100] According to the constructed covariance matrix H , solve the rotation matrix R and translation vector that minimizes the objective function ; This method introduces the improved ICP objective function E for:
[0101]
[0102] in , is the mean distance between corresponding points.
[0103] make , , then the minimization problem of the objective function can be transformed into:
[0104] ;
[0105] (1) Finding the rotation matrix R make minimize;
[0106] (2) Then the translation vector ;
[0107] To solve (1), the available methods include SVD, unit quaternion, dual quaternion, linear least squares, and nonlinear least squares. Here, SVD is used as an example to solve:
[0108] Perform singular value decomposition on the covariance matrix H constructed in step S310:
[0109] ,
[0110] in U is the left singular vector, V are right singular vectors, The main diagonal of is the singular value, and the rest of the elements are 0.
[0111] but
[0112] ,
[0113] Then apply solution (2):
[0114] ,
[0115] Rotation Matrix R and translation vectors T This is the result of minimizing the objective function in the current iteration.
[0116] Using a 4×4 matrix Represents the precise registration posture transformation matrix. Its 3×3 matrix at the top corner is the rotation matrix R, and the first three elements of the last column are the translation vectors T , the fourth element is 1, and the first three elements of the last row are zero.
[0117] S330: The rotation matrix R and the translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix .
[0118] Apply the obtained transformation relationship (i.e., rotation matrix R and translation matrix T) to the model point cloud, return to search for the nearest point of the model point cloud in the scene point cloud, and perform the next iteration; calculate the mean square error between the corresponding points, and if it is less than the threshold, the iteration stops; in fact, when the program is implemented, the termination condition of the iteration is to calculate the difference in rotation angle between the previous and next iterations. If the difference is less than the difference threshold, the problem is considered to have converged and the iteration is terminated; the posture transformation matrix obtained is In the embodiment, the difference threshold value is If it is greater than the difference threshold, return to step 3-1 and continue iterating until the termination condition is met.
[0119] The preset difference threshold and the preset termination condition can be selected and set according to actual needs.
[0120] In a possible implementation, optionally, in step S400, the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result, including:
[0121] S410, obtaining the rough matching posture transformation matrix and the attitude transformation matrix ;as well as,
[0122] S420: The obtained posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix; and
[0123] S430, converting the obtained final posture transformation matrix into the form of Euler angles and translation vectors, and displaying the final posture estimation result according to the conversion result.
[0124] The posture transformation matrix obtained in the second step And the posture transformation matrix obtained in the third step Multiply them to get the final attitude transformation matrix; convert the obtained attitude transformation matrix into the form of Euler angles and translation vectors, which is the final attitude estimation result.
[0125] It should be noted that although kdtree is used as an example to introduce the above search method for searching the nearest point of the model point cloud in the scene point cloud and the Hough transform, those skilled in the art will understand that the present disclosure should not be limited to this. In fact, the user can flexibly set the above method according to personal preferences and / or actual application scenarios, as long as the corresponding results are obtained.
[0126] In this way, the FPFH rough matching method provides a good initial position for ICP fine registration, the M matrix in the FPFH algorithm is constructed, the eigenvalue is decomposed, and then the local reference system is constructed; the topological features under the corresponding reference system are calculated, and the results are saved in the histogram, and then the random sampling consistency algorithm is used to remove the wrong point pairs from the feature extraction results corresponding to the model and scene point cloud, and the final rough registration posture estimation result is obtained by solving; the ICP algorithm finds the nearest point, constructs the covariance matrix, and solves the rotation matrix and translation vector that minimize the objective function; according to the problem of uneven distribution and holes in the workpiece point cloud caused by noise, occlusion, and reflection in industrial scenes, the objective function is improved, and the problem of decreased registration accuracy when the traditional ICP is applied to the point cloud with holes due to the reflection of the workpiece is solved. The FPFH algorithm is highly robust to noise, has rotation invariance, and has strong recognition ability, which can provide a stable and good initial position for the ICP algorithm; the ICP algorithm further improves the accuracy of pose estimation, so that the system can meet the accuracy requirements in the grasping task, and the interaction between the two improves the accuracy of pose estimation and ensures real-time performance.
[0127] Example 2
[0128] Based on the implementation principle of Example 1, this embodiment provides a device for implementing the above method. The functions and implementation principles of each module of the device are specifically described in Example 1 and will not be elaborated here.
[0129] According to another aspect of the present disclosure, a device for implementing the method for estimating the disordered grasping posture based on the improved FPFH and ICP algorithms is provided, comprising a point cloud data acquisition module, an FPFH algorithm processing module, an ICP algorithm processing module and a posture estimation module, wherein:
[0130] The point cloud data acquisition module is used to acquire the workpiece surface point cloud data and perform preprocessing;
[0131] The FPFH algorithm processing module is used to process the pre-processed point cloud data using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix
[0132] The ICP algorithm processing module is used to use the ICP iterative closest point improvement algorithm to accurately estimate the closest point of the model point cloud in the scene point cloud and obtain the attitude transformation matrix
[0133] The posture estimation module is used to match the posture transformation matrix according to the rough matching and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result.
[0134] In this embodiment, the point cloud data acquisition module preferably uses a Photoneo surface structured light 3D camera to acquire the surface point cloud data of the workpiece. The surface structured light camera is used to collect the two-dimensional image of the workpiece (multiple two-dimensional images are acquired for each type of workpiece that needs to be assembled on the production line, and 5,000 images of each type are selected in this embodiment), most of the background is filtered out, background interference is reduced, and the amount of calculation is reduced; on the basis of not affecting the point cloud representation capability, the point cloud is downsampled to further reduce the amount of calculation.
[0135] In a possible implementation manner, optionally, the FPFH algorithm processing module includes:
[0136] Point cloud key point extraction module: used to perform multi-threaded normal estimation on scene point cloud and model point cloud respectively, and extract point cloud key points respectively through downsampling;
[0137] Topological feature calculation module: used to construct local reference systems on the scene point cloud and the model point cloud respectively, calculate the topological features of the key points of the point cloud within the feature calculation radius under the corresponding local reference systems, and save the topological features in the histogram;
[0138] Pose transformation module: used to identify objects through Hough transform, determine the correct feature matching point pairs, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pairs, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain the rough matching pose transformation matrix .
[0139] In a possible implementation, optionally, the ICP algorithm processing module includes:
[0140] Covariance matrix construction module: used to search for the nearest point of the model point cloud in the scene point cloud and calculate the corresponding relationship, and construct the covariance matrix H according to the corresponding relationship;
[0141] Solution calculation module: used to improve the objective function according to the constructed covariance matrix H and the introduced ICP, and solve the rotation matrix R and translation vector that minimize the objective function ;
[0142] Iteration module: used to transform the rotation matrix R and translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix .
[0143] In a possible implementation, optionally, the posture estimation module includes:
[0144] Estimation data acquisition module: used to obtain the rough matching posture transformation matrix and the attitude transformation matrix ;
[0145] Calculation module: used to obtain the posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix;
[0146] Visualization module: used to convert the final posture transformation matrix obtained into the form of Euler angles and translation vectors, and display the final posture estimation result according to the conversion result.
[0147] Obviously, those skilled in the art should understand that the above modules or steps of the present invention can be implemented by a general computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. Thus, the present invention is not limited to any specific combination of hardware and software.
[0148] Example 3
[0149] Furthermore, according to another aspect of the present disclosure, a control system is provided, comprising:
[0150] processor;
[0151] A memory for storing processor executable instructions; wherein the processor is configured to implement the unordered grasping posture estimation method based on FPFH and ICP improved algorithms described in Example 1 when executing the executable instructions.
[0152] The control system of the disclosed embodiment includes a processor and a memory for storing processor executable instructions. The processor is configured to implement any of the above-mentioned methods for estimating unordered grasping postures based on FPFH and ICP improved algorithms when executing the executable instructions.
[0153] Here, it should be noted that the number of processors can be one or more. At the same time, in the control system of the embodiment of the present disclosure, an input device and an output device may also be included. Among them, the processor, memory, input device and output device may be connected through a bus or in other ways, which are not specifically limited here.
[0154] The memory, as a computer-readable storage medium, can be used to store software programs, computer executable programs and various modules, such as: the program or module corresponding to the method for estimating the disordered grasping posture based on the FPFH and ICP improved algorithms in the embodiment of the present disclosure. The processor executes various functional applications and data processing of the control system by running the software programs or modules stored in the memory.
[0155] The input device can be used to receive input numbers or signals. The signal can be a key signal related to user settings and function control of the device / terminal / server. The output device can include a display device such as a display screen.
[0156] Example 4
[0157] According to another aspect of the present disclosure, a non-volatile computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the method for estimating the disordered grasping posture based on the FPFH and ICP improved algorithms described in Example 1 is implemented.
[0158] The embodiments of the present disclosure have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements to the technology in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.
Claims
1. A method for estimating unordered grasping posture based on FPFH and ICP improved algorithms, characterized in that: The steps include: S100, obtaining workpiece surface point cloud data and performing preprocessing; and, S200, use FPFH object recognition and 3D posture estimation algorithm to process the pre-processed point cloud data to obtain the rough matching posture transformation matrix ; as well as, S300, using the ICP iterative closest point improved algorithm, accurately estimate the pose of the closest point of the model point cloud in the scene point cloud and obtain the pose transformation matrix ; as well as, S400, according to the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result; In step S200, the pre-processed point cloud data is processed using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix ,include: S210, performing multi-threaded normal estimation on the scene point cloud and the model point cloud respectively, and extracting point cloud key points respectively by downsampling; and, S220, constructing local reference systems on the scene point cloud and the model point cloud respectively, calculating the topological features of the point cloud key points within the feature calculation radius under the corresponding local reference systems, and saving the topological features in a histogram; and, S230, perform object recognition through Hough transform, determine the correct feature matching point pair, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pair, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain a coarse matching pose transformation matrix ; In step S300, the ICP iterative closest point improvement algorithm is used to accurately estimate the pose of the closest point of the model point cloud in the scene point cloud to obtain the pose transformation matrix. ,include: S310, searching for the nearest point of the model point cloud in the scene point cloud and calculating the corresponding relationship, and constructing a covariance matrix H according to the corresponding relationship; and S320, according to the constructed covariance matrix H and the introduced ICP improved objective function, solving the rotation matrix R and the translation vector that minimize the objective function ;as well as, S330: The rotation matrix R and the translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix ; In step S400, the rough matching posture transformation matrix and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result, including: S410, obtaining the rough matching posture transformation matrix and the attitude transformation matrix ;as well as, S420: The obtained posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix; and S430, converting the obtained final posture transformation matrix into the form of Euler angles and translation vectors, and displaying the final posture estimation result according to the conversion result.
2. A device for implementing the method for estimating disordered grasping posture based on the improved FPFH and ICP algorithms as described in claim 1, characterized in that: It includes point cloud data acquisition module, FPFH algorithm processing module, ICP algorithm processing module and posture estimation module, among which, The point cloud data acquisition module is used to acquire the workpiece surface point cloud data and perform preprocessing; The FPFH algorithm processing module is used to process the pre-processed point cloud data using the FPFH object recognition and three-dimensional posture estimation algorithm to obtain a rough matching posture transformation matrix The ICP algorithm processing module is used to use the ICP iterative closest point improvement algorithm to accurately estimate the closest point of the model point cloud in the scene point cloud and obtain the attitude transformation matrix The posture estimation module is used to match the posture transformation matrix according to the rough matching and the attitude transformation matrix , calculate and obtain the final posture transformation matrix, and transform the final posture transformation matrix to obtain the final posture estimation result; The FPFH algorithm processing module includes: Point cloud key point extraction module: used to perform multi-threaded normal estimation on scene point cloud and model point cloud respectively, and extract point cloud key points respectively through downsampling; Topological feature calculation module: used to construct local reference systems on the scene point cloud and the model point cloud respectively, calculate the topological features of the key points of the point cloud within the feature calculation radius under the corresponding local reference systems, and save the topological features in the histogram; Pose transformation module: used to identify objects through Hough transform, determine the correct feature matching point pairs, solve the pose transformation relationship between the scene point cloud and the model point cloud based on the feature matching point pairs, and apply the transformation relationship to the model point cloud to make the model point cloud closer to the scene point cloud, and construct and obtain the rough matching pose transformation matrix ; The ICP algorithm processing module includes: Covariance matrix construction module: used to search for the nearest point of the model point cloud in the scene point cloud and calculate the corresponding relationship, and construct the covariance matrix H according to the corresponding relationship; Solution calculation module: used to improve the objective function according to the constructed covariance matrix H and the introduced ICP, and solve the rotation matrix R and translation vector that minimize the objective function ; Iteration module: used to transform the rotation matrix R and translation vector Apply to the model point cloud and return the nearest point of the search model point cloud in the scene point cloud, iterate according to the preset difference threshold and preset termination condition to obtain the posture transformation matrix ; The posture estimation module comprises: Estimation data acquisition module: used to obtain the rough matching posture transformation matrix and the attitude transformation matrix ; Calculation module: used to obtain the posture transformation matrix With the posture transformation matrix Multiply them together to calculate the final posture transformation matrix; Visualization module: used to convert the final posture transformation matrix obtained into the form of Euler angles and translation vectors, and display the final posture estimation result according to the conversion result.
3. A control system, characterized in that: include: processor; a memory for storing processor-executable instructions; Wherein, the processor is configured to implement the unordered grasping posture estimation method based on FPFH and ICP improved algorithms described in claim 1 when executing the executable instructions.
4. A non-volatile computer-readable storage medium having computer program instructions stored thereon, characterized in that: When the computer program instructions are executed by the processor, the method for estimating the unordered grasping posture based on the FPFH and ICP improved algorithms described in claim 1 is implemented.
