Target detection model-based grasp point pose calculation method and application thereof

Abstract

The application belongs to the technical field of industrial automation and computer vision, and discloses a grasping point pose calculation method based on a target detection model and application thereof. FPFH The grasping point pose calculation method comprises the following steps: step 100, extracting a corresponding sub-region point cloud from a three-dimensional point cloud of the object to be grasped according to a target center region obtained by processing a gray image of the object to be grasped according to the target detection model; step 200, performing coarse matching on the sub-region point cloud and a template point cloud to obtain a coarse matching result; step 300, performing fine matching on the sub-region point cloud and the template point cloud according to the coarse matching result to obtain a fine matching result; step 400, judging whether the fine matching result meets a preset requirement according to an inlier root mean square error and a coincidence degree, if yes, proceeding to the next step, otherwise terminating the process; and step 500, calculating a grasping point pose of the object to be grasped according to the fine matching result and coordinate transformation. RANSAC GICP The application has simple process steps and is easy to implement and control.​

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial automation and computer vision, in particular to a grasping point pose calculation method based on a target detection model and application thereof. BACKGROUND

[0002] Traditional mechanical arm grasping control mainly relies on preset fixed trajectories and manually set pose parameters, and it is difficult to flexibly cope with dynamic recognition and accurate positioning of various types of workpieces. Especially when dealing with complex industrial parts with high reflectivity, high precision requirements and shielding interference, the traditional method has obvious deficiencies in efficiency and accuracy, and it is difficult to meet the requirements of intelligent manufacturing for high adaptability and automation level.

[0003] The Chinese patent application with the title of "A method for obtaining the coordinates of the center of a defect from a two-dimensional image and its application", application number CN2024116137510, submitted by the applicant of the present application, discloses a method for obtaining the coordinates of the center of a defect from a two-dimensional image. The method solves the three-dimensional position coordinates (X, Y, Z) of the defect center in the robot base coordinate system. The robot gripper can move the defect center to the imaging center of the three-dimensional camera to serve the subsequent three-dimensional imaging detection link. However, in order to achieve "high-precision visual guidance grasping", a complete six-degree-of-freedom grasping pose (X, Y, Z, Rx, Ry, Rz) containing both position and attitude is needed to guide the robot arm to accurately move to the grasped object for accurate grasping. The prior art has not solved this technical problem. SUMMARY

[0004] The technical problem to be solved by the present application is to provide a grasping point pose calculation method based on a target detection model with stronger universality and self-adaptability, which can efficiently cope with the recognition and automatic pose accurate registration of multiple types of targets, and application thereof. The technical solution is as follows:

[0005] The grasping point pose calculation method based on a target detection model is characterized by comprising the following steps:

[0006] Step 100, extracting a corresponding sub-region point cloud from a three-dimensional point cloud of the grasped object from a target center region obtained by processing a gray image of the grasped object according to a target detection model;

[0007] Step 200, performing coarse matching on the sub-region point cloud and the template point cloud through FPFH the feature vector and RANSAC obtaining a coarse matching result;

[0008] Step 300, performing fine matching on the sub-region point cloud and the template point cloud through the coarse matching result and GICP obtaining a fine registration result;

[0009] Step 400, according to the inner point root mean square error and the coincidence degree, judging whether the fine registration result meets the preset requirement, if yes, entering the next step, otherwise terminating the process;

[0010] Step 500, according to the fine registration result and the coordinate transformation, calculating the grasping point pose of the to-be-grasped object.

[0011] The automatic guiding grasping method comprises the steps of: the robot arm performing the operation of grasping the to-be-grasped object according to the grasping point pose calculated by the above-mentioned calculation method.

[0012] Compared with the prior art, the grasping point pose calculation method based on the target detection model and the application thereof have the following advantages:

[0013] (1) The prior art often directly processes the full-scene point cloud, which contains a large amount of irrelevant background data, resulting in calculation redundancy and low efficiency. The present application locks the target range through the target detection model and extracts only the effective sub-region point cloud, greatly reducing the data amount and the complexity of subsequent matching and calculation, and improving the overall process efficiency. At the same time, the present application deeply integrates 2D target detection (gray image) and 3D point cloud processing, first provides accurate guidance for 3D point cloud region extraction through 2D target detection, and then ensures the spatial accuracy of pose calculation through 3D point cloud processing, thereby balancing the positioning speed and spatial accuracy, and being more suitable for the real-time and accuracy requirements of actual grasping scenes (such as industrial assembly lines and robot grasping).

[0014] (2) The present application provides a visual guidance method based on target detection and three-dimensional point cloud clustering and filtering fusion, which discards the pixel-level segmentation which is computationally intensive and sensitive to errors, and instead uses efficient target detection to quickly lock the region of interest (ROI), and places the core processing in the three-dimensional domain. Through subsequent point cloud clustering, geometric filtering and accurate selection, the three-dimensional geometric features of the grasping point are directly extracted from the ROI to solve the six-degree-of-freedom pose. Compared with the existing instance segmentation scheme, the present application effectively overcomes the problems of calculation delay, two-dimensional error transmission and information redundancy caused by instance segmentation, while ensuring high accuracy, significantly improves the real-time performance and robustness of the system. It has been verified that the calculation time of the present application for each to-be-grasped object (the size range of the object positioned surface: 8mm x 8mm - 80mm x 50mm, the depth range of the object positioned surface: ±15mm) is within 3s, and it is especially suitable for industrial high-speed precision grasping scenes.

[0015] (3) The present application uses FPFH feature vectors (which can effectively capture local geometric features of point cloud) and RANSACThe algorithm is good at eliminating outliers caused by noise and occlusion, and can quickly obtain a reliable initial registration result in the presence of partial occlusion and point cloud noise, avoiding the defects that the traditional coarse matching is easily disturbed. GICP The algorithm converges faster and has higher precision, solves the problems that the traditional ICP algorithm is easy to fall into local optimum and converges slowly, and finally obtains a high-precision registration result.

[0016] (4) The present application quantitatively evaluates the fine registration result by the root mean square error of the inlier and the degree of coincidence before the pose coordinate transformation, actively eliminates unqualified results, reduces invalid grabbing attempts from the process, ensures that only the results meeting the grabbing accuracy requirements enter the subsequent steps, and significantly improves the grabbing reliability. Since the error source of the coordinate transformation is strictly controlled, the pose calculation error depending on the fine registration result is smaller. It has been verified that the positioning accuracy error of the grabbing point pose calculation of the present application is ±0.1mm, which can more accurately reflect the actual spatial pose of the object to be grabbed, and meets the high-precision requirements of "position + attitude" (such as the angle and position deviation of the mechanical arm grabbing needs to be controlled in millimeter level).

[0017] In summary, the grabbing point pose calculation method based on the target detection model and the process steps of the application thereof are simple and easy to implement and control. Through the pose calculation of deep fusion of two-dimensional texture information and three-dimensional geometric information, the whole process optimization of "efficient data screening-robust fine registration-quality verification-accurate pose calculation" is realized, which is superior to the traditional method in efficiency, accuracy, reliability and scene adaptability. It can efficiently cope with multi-class target recognition and automatic fine pose registration, and significantly improve the application effect and intelligent level of the industrial grabbing system in complex environment.

[0018] The present application will be further described below in conjunction with the accompanying drawings and specific embodiments. Additional aspects and advantages of the present application will be partially given in the following description, partially become apparent from the following description, or be understood by practicing the present application. BRIEF DESCRIPTION OF DRAWINGS

[0019] The accompanying drawings, which form a part of the present application, are used to aid in the understanding of the present application and the content provided therein and its related description in the present application can be used to explain the present application, but do not constitute an improper limitation on the present application. In the drawings:

[0020] Figure 1 The schematic diagram of the sub-region point cloud in the embodiment of the present application. PCD roi The schematic diagram of the sub-region point cloud in the embodiment of the present application.

[0021] Figure 2 Fig. 1 is a schematic diagram of a designated point cloud in an embodiment of the present application. PCD roi Fig. 2 is a schematic diagram of a designated point cloud in an embodiment of the present application. o3d Fig. 3 is a schematic diagram of a designated point cloud in an embodiment of the present application. filtered Fig. 4 is a schematic diagram of a designated point cloud in an embodiment of the present application.

[0022] Figure 3 Fig. 5 is a schematic diagram of a designated point cloud in an embodiment of the present application. PCD selected Fig. 6 is a schematic diagram of a designated point cloud in an embodiment of the present application.

[0023] Figure 4 Fig. 7 is a schematic diagram of a designated point cloud in an embodiment of the present application.

[0024] Figure 5 Fig. 8 is a schematic diagram of a designated point cloud in an embodiment of the present application. DETAILED DESCRIPTION

[0025] The present application will be described in detail below with reference to the drawings. Those skilled in the art will be able to implement the present application based on these descriptions. Before the present application is described in detail with reference to the drawings, it is particularly important to note that:

[0026] The technical solutions and technical features provided in each part of the present application, including the following descriptions, can be combined with each other without conflict.

[0027] In addition, the embodiments of the present application involved in the following description are generally only a part of the embodiments of the present application, not all. Therefore, all other embodiments obtained by those skilled in the art based on the embodiments in the present application without creative labor should belong to the scope of protection of the present application.

[0028] Regarding the terms and units in the present application. The terms "include", "have" and any variations thereof in the specification and claims of the present application and related parts are intended to cover non-exclusive inclusion.

[0029] The specific implementation of the target detection model-based grasping point pose calculation method of the present application comprises steps 100-400, as follows:

[0030] Step 100, according to the target center region obtained by processing the gray image of the target to be grasped by the target detection model, extracting the corresponding sub-region point cloud from the three-dimensional point cloud of the target to be grasped PCD roi ; wherein:

[0031] The gray image and three-dimensional point cloud of the target to be grasped are obtained by an industrial-grade structured light three-dimensional camera.

[0032] The target detection model used is a high-performance lightweight model optimized for industrial part detection based on a deep learning architecture such as YOLOv10. This model is trained on a large dataset of objects to be grasped in different models, different poses, and complex backgrounds. It can robustly identify the category of the object to be grasped and regress its accurate axis bounding box. The key features of the model are: first, it adopts lightweight design, ensuring high precision while meeting industrial real-time requirements; second, it has strong generalization ability for the shape, surface texture, and illumination changes of the object to be grasped, effectively resisting light interference and background clutter in industrial sites; third, the output axis bounding box not only contains the category confidence, but also provides the accurate pixel coordinates and size of the target in the image, providing a basis for subsequent accurate sub-region extraction from three-dimensional point cloud.

[0033] The target detection model processes the grayscale image to obtain the grade category Class detceted and the center pixel coordinates u center , v center If the grade category Class detceted matches the input grade Class input , the next step is entered, otherwise the process is terminated; wherein the input grade Class input is automatically generated by the upstream production management system (MES) or enterprise resource planning system (ERP) according to production order information and issued to the system as a target parameter for visual guided grasping task.

[0034] For grayscale images with grade category Class detceted matching the input grade Class input , the target center area is a rectangular area u center , v center ) determined on the grayscale image according to the center pixel coordinates length u , v ); wherein the target region size length corresponds to the grade category Class detceted , and the corresponding relationship is pre-input, and the target region size length can be obtained according to the grade category Class detceted identified by the target detection model.

[0035] ​The rectangular area ( u , v This can be represented as: ;in, .

[0036] Then extract the rectangular region ( u , v The 3D point cloud within the region is obtained as the sub-region point cloud. PCD roi .

[0037] To obtain clean, well-structured point cloud data for subsequent matching or recognition, sub-region point clouds were also analyzed. PCD roi The following processing was performed, including the following steps:

[0038] Step 110, convert the sub-region point cloud PCD roi according to Z The axis depth range is filtered to obtain the values ​​located at... Z Specified point cloud within the axis depth range PCD roi _ filtered .

[0039] Step 120, convert the template point cloud PCD target and the selected point cloud PCD roi _ filtered of NumPy Convert the formatted data to the formatted data respectively. Open 3 D Template point cloud PCD target _ o3d and designated point cloud PCD roi _ o3d , can be represented as:

[0040] ;

[0041] ;

[0042] Step 130: To reduce point cloud density and computational overhead, uniform sampling is used. uniform_down_sample The method will generate a specified point cloud. PCD roi _ o3d and template point cloud PCD target _ o3d They were dynamically adjusted and transformed into specified point clouds with lower density. PCD roi _ o3d _down and template point cloud PCD target o3d down , which can be expressed as:

[0043]

[0044]

[0045] wherein, every_k_points is determined according to different level categories Class detceted The preset sampling parameter means "how many points are kept every other points". every_k_points The specific value of is stored in a level-parameter mapping table in advance, and the principle of the value is: a positive correlation with the geometric complexity of the target object, if the target is larger, the number of collected point clouds is more, and a larger is set, the purpose is to obtain higher data processing efficiency. every_k_points

[0046] Step 140, the specified point cloud PCD roi o3d down is filtered and denoised to obtain the specified point cloud PCD roi o3d filtered

[0047] Specifically, first, the outliers are removed according to the neighbor statistics value of the point (statistical filtering), then the isolated points with too few surrounding points are removed (radius filtering), and finally the abnormal points in the edge or rough area are removed by using the normal change rate (normal smoothing filtering), that is, the specified point cloud after filtering and denoising PCD roi o3d filtered

[0048] Step 150, using DBSCAN a clustering algorithm, the specified point cloud after filtering and denoising PCD roi o3d filtered is divided into multiple clusters, and the source point cloud that meets the size requirement (located between conn_minval and conn_maxval) is obtained by screening the number of point clouds in each cluster PCD selected

[0049] Step 200, by FPFH Fast Point Feature Histograms characteristic vectors and RANSAC ​​​​​​​​​​​​​​​​​A coarse matching is performed between the sub-region point cloud and the template point cloud to obtain the coarse matching result; specifically, steps 210 to 230 are as follows:

[0050] Step 210, based on the template point cloud PCD target _ o3d _ down normals and source point clouds The normal, using KD-Tree The method calculates the normal vector by searching up to 50 points within a 0.8-meter radius neighborhood of each point.

[0051] Step 220, using a mixture KD-Tree The method searches for up to 200 points within a 0.8-meter neighborhood of each point to calculate the template point cloud. PCD target _ o3d _ down Heyuan Point Cloud PCD selected Each point in FPFH Eigenvectors.

[0052] Each point FPFH The feature vector is a 33-dimensional vector (in Open 3 D (in Chinese) describes the geometric environment around the point, including the geometric relationships such as the relative normal, included angle, and distance between pairs of surrounding points.

[0053] Template point cloud PCD target _ o3d _ down Each point in FPFH The feature vector is represented as FPFH target ( q ),in q ∈ PCD target _ o3d _ down .

[0054] Source Cloud PCD selected Each point in FPFH Vector representation is FPFH selected ( p ),in p ∈ PCD selected .

[0055] Step 230, based on FPFH Feature vector, through RANSAC Source cloud PCD selectedaligning the source point cloud PCD target o3d down alignment, i.e. the transformation matrix of the coarse matching result M RANSAC

[0056] Step 300, fine matching the sub-region point cloud and the template point cloud through the coarse matching result and the improved Iterative Closest Point (ICP) algorithm, to obtain a fine registration result; specifically comprising steps 310-320, as follows: GICP Generalized ICP, improved Iterative Closest Point (ICP) algorithm

[0057] Step 310, through GICP aligning the source point cloud PCD selected PCD target o3d down alignment, i.e. the initial transformation matrix of the fine registration result M GICP

[0058] GICP is a local iterative algorithm, which heavily depends on a good initial value. The final goal of the GICP algorithm is to find an optimal rotation matrix R and translation vector t through iterative optimization, so that the sum of the weighted distances between the corresponding points is minimized. The transformation matrix of the coarse matching result M RANSAC is used as the starting point of this iterative optimization process, and the specific steps are as follows: first, the transformation matrix M RANSAC is used to initialize the corresponding rotation matrix and translation vector , then the rotation matrix and translation vector are gradually adjusted, and finally the optimal solution described by the following formula is converged M GICP , so that the points PCD selected in the source point cloud are aligned with the corresponding points PCD target o3d down in the template point cloud after transformation, thereby minimizing the cost function under local covariance constraint, and finally obtaining GICP the registered source point cloud PCD selected relative to the template point cloud​​​​​​​​​​PCD target _ o3d _ down The initial transformation matrix .

[0059] ;

[0060] In the formula, Source Point Cloud PCD selected The first in One point; Point cloud template PCD target _ o3d _ down Zhongyu The first match One point; for The local covariance matrix; for The local covariance matrix.

[0061] Step 320, Calculate the source point cloud PCD selected and template point cloud PCD target _ o3d _ down The center point difference is calculated and used as a translation compensation vector added to the initial transformation matrix. M GICP In the process, the compensation transformation matrix of the fine registration result is obtained. .

[0062] Although in step 310 GICP The source point cloud has been accurately registered, but due to GICP Based on local minimum error optimization, it does not necessarily guarantee that the "geometric centers" of the two point clouds are perfectly aligned. Therefore, after registration, the difference between the center points of the two point clouds is calculated and added as an additional translation compensation vector. GICP The first transformation matrix M GICP This allows for higher precision position alignment.

[0063] application GICP After obtaining the initial transformation, the source point cloud PCD selected The geometric center transformation is as follows:

[0064] ;

[0065] In the formula, Source Point Cloud PCDselected The geometric center, The value range is 1 to ,common One source point.

[0066] The translation compensation vector is ,in Point cloud template PCD target _ o3d _ down The geometric center, The value range is 1 to ,common Template points.

[0067] The translation compensation vector is added as compensation to the initial transformation matrix. M GICP In this way, the compensation transformation matrix is ​​obtained. :

[0068]

[0069] Step 400, based on the root mean square error of the interior points ( inlier_rmse ) and overlap ( fitness Determine whether the registration result meets the preset requirements. If it does, proceed to the next step; otherwise, terminate the process.

[0070] The fitness To calculate the overlapping region (internal point correspondence / number of target points). inlier_rmse and fitness These are preliminary screening quality standards set to quickly eliminate completely unsuccessful fine-match results; they are all empirical parameter values.

[0071] The preset requirements refer to inlier_rmse Less than 0.5m and fitness The ratio is greater than 0.1; if the preset requirements are not met, it means that the point clouds do not overlap at all, the matching accuracy is too poor, and the overlap is too low.

[0072] Because the point cloud acquired by the 3D camera contains a lot of background interference, the fine registration result may have local mismatches, and the root mean square error of the interior points (RMSE) may also be present. inlier_rmse ) and overlap ( fitness It cannot determine whether local features are aligned. Therefore, a specific distance filtering algorithm is needed to specifically verify the alignment of the target region after fine matching. The specific distance filtering algorithm includes the following steps:

[0073] Step 410, Calculate the template point cloud PCD target _ o3d _ downEach point in the matrix after the initial transformation M GICP Transformed source point cloud PCD The set of distances consisting of the minimum Euclidean distances of 1 D 1( PCD 1, PCD target _ o3d _ down The calculation expression is:

[0074] ;

[0075] ;

[0076] Step 420, from D 1( PCD 1, PCD target _ o3d _ down The distance value of the 20th element from the end of the sorted array (which sorts all elements numerically in ascending order) is... d 1. The 20th value is the 20th largest value among all elements. It is more robust than the extreme value and can avoid individual outliers from affecting the judgment. Instead of taking the maximum value, the 20th largest value is taken, which means that a maximum of 19 such "poorly matched" points are allowed.

[0077] Step 430, Calculate the template point cloud PCD target _ o3d _ down The compensation transformation matrix for each point in the matrix Transformed source point cloud PCD The set of distances formed by the minimum Euclidean distance of 2 D 2( PCD 2, PCD target _ o3d _ down The calculation expression is:

[0078] ;

[0079] ;

[0080] In the formula, Represents the transformed source point cloud The first in One point; Represents the transformed source point cloud The first in One point; represent The first in One point.

[0081] Step 440, from D 2( PCD 2, PCD target _ o3d _ down The distance value is taken from the 20th element from the end of the sorted sequence. d 2, if d 2 less than or equal to 15mm and d 2 less than d If 1 is selected, the filtering is complete; otherwise, the process terminates. Analysis of historical registration data revealed that when... d When 2>15mm, the grasping failure rate is high; while when d When 2 ≤ 15 mm, the grasping success rate is high and stable.

[0082] Step 500: Based on the fine registration results and coordinate transformation, calculate the pose of the grasping point of the object to be grasped; specifically, steps 510 to 560 are as follows:

[0083] Step 510, from the compensation transformation matrix Extract the rotation matrix R , rotate matrix R Transform from the camera coordinate system to the gripper coordinate system to obtain the rotation matrix. R gripper , can be represented as:

[0084] ;

[0085] In the formula, R camera2gripper The rotation matrix from the camera to the gripper is calculated using the eye-on-hand calibration method.

[0086] Step 520, rotate the matrix R gripper Converted to Euler angles relative to the robotic arm e , can be represented as:

[0087] ;

[0088] In the formula, The unit is angle ( o ).

[0089] Step 530, targeting Z The axis angle is corrected to obtain the Euler angle change;

[0090] If around Z The rotation angle of the shaft is too large (greater than 110° or less than -110°, i.e.) This indicates that the object to be grasped is upside down or flipped over, so adjust the Euler angles. Counter-clockwise X The angle of the axis is reversed (multiplied by -1, becomes ), and the rest remains unchanged. The corrected Euler angles are .

[0091] The Euler angles are limited to the range of If it exceeds , subtract ; if it is less than , add , to get the updated Euler angle change .

[0092] Step 540, according to the Euler angle change and the current pose angle of the robot arm, get the robot arm pose ; according to the compensation transformation matrix and the linear transformation matrix T , get the robot arm end pose ;

[0093] In the formula, is the current pose angle of the robot arm, unit is degree ( o ); T is the linear transformation matrix, which represents the pose correction of the robot arm due to the deviation of the vision guide camera. It is calculated in advance and input before the process starts.

[0094] The calculation process of the linear transformation matrix T is as follows:

[0095] Suppose n sets of robot arm end pose data are collected, each set containing:

[0096] : the translation vector in the camera coordinate system obtained after GICP matching of the th data set;

[0097] : the corresponding robot arm end position coordinates in the th data set.

[0098] These points are represented by a matrix as follows:

[0099] .

[0100] According to the least squares solution formula , so that for all i ( satisfy , that is, the linear transformation matrix .

[0101] Step 550, subtracting the current pose of the mechanical arm from the pose of the camera to obtain a new pose point of the mechanical arm ; ;

[0102] Step 560, obtaining the grasp point pose of the object to be grasped according to the pose of the mechanical arm and the new pose point of the mechanical arm ;

[0103] wherein the calculation expression of the grasp point pose of the object to be grasped base _ point is as follows:

[0104] ;

[0105] wherein, sub is a fixed offset between the photographing point and the grasp point.

[0106] The specific implementation of the automatic guiding grasping method of the present application comprises the following steps: the mechanical arm performs the operation of grasping the object to be grasped according to the grasp point pose calculated by the above calculation method base _ point .

[0107] The beneficial effects of the present application will be described below through specific embodiments, and the device structure and the object to be grasped applied can be but are not limited to the Chinese invention patent CN2024116137510.

[0108] 1. Data preparation

[0109] (1) The transformation matrix of the camera coordinates to the gripper coordinates system obtained by using the eye-in-hand calibration method M camera2gripper :

[0110] M camera2gripper = [[-1.92790159e-01, 9.80060267e-01, 4.81022692e-02, 4.69654646e+01], [-9.81087617e-01, -1.93392398e-01, 8.15277166e-03, -9.67066874e+01], [1.72928207e-02, -4.56207665e-02, 9.98809143e-01, -1.11994426e+02], [0, 0, 0, 1]];

[0111] R camera2gripper is the transformation matrix M camera2gripper used in extracted rotation matrix.

[0112] (2) current pose of the robot arm r current =[-171.06, 1252.08997, 688.73999, 30.13, 1.75, -182.05815].

[0113] (3) target region size length =[540, 300].

[0114] (4) every_k_points = 6.

[0115] (5) linear transformation matrix =[[-0.66061021, 0.76010278, 0.03095112], [0.76627342, 0.65027625, 0.01655252], [0.03792626, 0.02132504, -1.01296402]].

[0116] (6) fixed offset sub =[4.55886881, -112.94048847, 179.23292355, 89.73, 0.0, 0.00998].

[0117] 2. execute the grasp point pose calculation

[0118] Step 100

[0119] input level Class input is level 2; the target detection model is used to process the grayscale image GrayImage ( u , v ) to obtain the center pixel coordinates u center , v center ) = [[705, 520]], and the level category Class detceted is level 2, which is consistent with the input level Class input .

[0120] According to the center pixel coordinates u center , v center ) and the target region size length , the grayscale image GrayImage ( u , vthe rectangular region determined above u , v ), and then extract the three-dimensional point cloud in the rectangular region u , v ), that is, obtain the sub-region point cloud as shown in Figure 1 PCD roi .

[0121] The sub-region point cloud PCD roi is processed by steps 110-150 to obtain the specified point cloud containing four clusters as shown in Figure 2 PCD roi _ o3d _ filtered (the template point cloud PCD target _ o3d _ down ) and the source point cloud as shown in Figure 3 PCD selected .

[0122] Step 200

[0123] The sub-region point cloud and the template point cloud are coarsely matched by FPFH feature vectors and RANSAC , and the coarse matching result is as shown in Figure 4 , to obtain the transformation matrix M RANSAC [[0.99245615, -0.1206392, 0.02183962, -1], [0.11999028, 0.99235367, 0.02892285, -0], [-0.02516186, -0.02608412, 0.99934303, 0.56740733], [0, 0, 0, 1]].

[0124] Step 300

[0125] The translation vector t = [-1.21852729, 0.07356604, 0.59915458].

[0126] The rotation matrix R = [[0.99786077, -0.05003862, 0.04207163], [0.04966324, 0.99871674, 0.00992139], [-0.04251409, -0.00781076, 0.99906534]]. ​​​

[0127] Transformation matrix =[[ 0.99786077,-0.05003862,0.04207163,-1.21852729],[0.04966324,0.99871674,0.00992139,0.07356604],[-0.04251409,-0.00781076,0.99906534,0.59915458],[ 0,0,0,1]。

[0128] Translation compensation vector △ t =[0.11823834, -0.02756017, -0.00067016].

[0129] Fine matching effect such as Figure 5 As shown, the compensation transformation matrix =[[ 0.99786077,-0.05003862,0.04207163,-1.10028896],[ 0.04966324,0.99871674,0.00992139,0.04600587],[-0.04251409,-0.00781076,0.99906534,0.59848442],[ 0,0,0,1]。

[0130] Step 400

[0131] The degree of overlap in fine matching ( fitness The root mean square error of the interior points is 1.000000. inlier_rmse The value is 0.115916m, and the number of corresponding point pairs participating in the registration is 8313, which satisfies the requirement. inlier_rmse Less than 0.5m and fitness Greater than 0.1.

[0132] Calculated through steps 410-440 d 1 = 0.3783434168009353 mm d 2 = 0.33537015333589143 mm, which satisfies the condition. d 2 less than 15mm and d 2 less than d 1.

[0133] Following the steps above, all 8313 point pairs were successfully matched.

[0134] Step 500

[0135] Step 510: [[0.99886026, -0.04768770, 0.00201361], [0.04772992, 0.99782185, -0.04553468], [0.00016221, 0.04557889, 0.99896073]].

[0136] Step 520: Convert the rotation matrix R gripper to Euler angles relative to the robot arm e= [2.61238326, -0.00929413, 2.73576251].

[0137] Step 530: Corrected Euler angles = [2.61238326, -0.00929413, 2.73576251].

[0138] Updated Euler angle changes = [2.61238326, -0.00929413, 2.73576251].

[0139] Step 540: = [32.74576256, 1.74070595, -179.32238981].

[0140] = [[-0.69594466, 0.726835, -0.0125604], [0.73285833, 0.68740252, 0.0080184], [0.05999088, -0.01608912, -1.01346239]].

[0141] Step 550: r target = [-171.75594466, 1251.363135, 688.7525504, 32.74576256, 1.74070595, -179.32238981].

[0142] Step 550: base _ point = [-176.31481347, 1364.30362347, 509.51962685, -56.98423744, 1.74070595, -179.33236981].

[0143] The total time for calculating the pose of the grabbing point is 2.8070168495178223s, and the processing time is in the upper middle level in industrial vision guidance and in the fast processing level in three-dimensional vision guidance. Based on strict mathematical coordinate transformation, the positioning accuracy error of the pose calculation of the grabbing point is ±0.1mm, which is higher than that of the traditional method.

[0144] The above describes the relevant content of the present application. Those skilled in the art can implement the present application based on the above description. Based on the above content of the present application, all other embodiments obtained by those skilled in the art without creative labor shall belong to the protection scope of the present application.

Claims

1. A method for computing a pose of a grasp point based on a target detection model, characterized in that: The method comprises the following steps: Step 100, extracting a corresponding sub-region point cloud from a three-dimensional point cloud of the object to be grabbed according to a target center region obtained by processing a gray image of the object to be grabbed according to a target detection model; Step 200, by FPFH feature vectors and RANSAC perform coarse matching on the sub-region point cloud and the template point cloud to obtain a coarse matching result; Step 300, using the coarse matching results and GICP Fine matching is performed between the sub-region point cloud and the template point cloud to obtain the fine registration result; Step 400, judging whether the fine registration result meets a preset requirement according to an inlier root mean square error and a coincidence degree, and if yes, proceeding to the next step, otherwise terminating the process; Step 500, calculating a grabbing point pose of the object to be grabbed according to the fine registration result and a coordinate transformation; The step 300 comprises: Step 310, by GICP aligning the source point cloud PCD selected with the template point cloud PCD target _ o3d _ down , a preliminary transformation matrix of the fine registration result is obtained M GICP ; Step 320, calculating the source point cloud PCD selected and the center point difference of the template point cloud PCD target o3d down , and adding the center point difference as a translation compensation vector to the initial transformation matrix M GICP , to obtain a compensation transformation matrix of the fine registration result ;​​ The step 400 further comprises screening the fine registration result meeting the grabbing requirement, comprising the steps of: Step 410, calculate the template point cloud PCD target o3d down each point to the initial transformation matrix M GICP Transformed source point cloud PCD 1, the minimum Euclidean distance of each point to the initial transformation matrix D 1, PCD 1, PCD target o3d down ​​​​​ Step 420, from D 1( PCD 1, PCD target _ o3d _ down ) the 20th last distance value in ascending order is taken as d 1; Step 430, calculate the template point cloud PCD target _ o3d _ down Each point in the middle to the compensation transformation matrix Transformed source point cloud PCD The distance set composed of the minimum Euclidean distances of 2 D 2( PCD 2, PCD target _ o3d _ down ); Step 440, from D 2( PCD 2, PCD target _ o3d _ down ) the 20th last distance value in ascending order is d 2, if d 2 is less than or equal to a preset value and d 2 is less than d 1, the screening is completed, otherwise the process is terminated.

2. The target detection model based grasp pose calculation method of claim 1, wherein: In the step 100: The gray image and the three-dimensional point cloud of the object to be grabbed are acquired by an industrial-grade structured light three-dimensional camera; The target detection model processes the gray image of the object to be grabbed to obtain a grade category and a center pixel coordinate, and if the grade category is consistent with an input grade, the next step is entered, otherwise the process is terminated; The target center region is a rectangular region determined on the gray image according to the center pixel coordinate and a target region size, wherein the target region size is preset according to the grade category.

3. The target detection model based grasp pose calculation method of claim 2, wherein: The step 100 further comprises processing the sub-region point cloud, and the processing steps comprise: Step 110, convert the sub-region point cloud PCD roi according to Z The axis depth range is filtered to obtain the values ​​located at... Z Specified point cloud within the axis depth range PCD roi _ filtered ; Step 120, converting the template point cloud PCD target and the specified point cloud obtained by screening PCD roi filtered NumPy format data into Open 3 D template point cloud PCD target o3d and specified point cloud PCD roi o3d ;​​​​ Step 130, using a uniform sampling method, converts the specified point cloud PCD roi o3d and the template point cloud PCD target o3d into the specified point cloud PCD roi o3d down and the template point cloud PCD target o3d down ;​​​​​​ Step 140, filtering and denoising the specified point cloud PCD roi o3d down to obtain the specified point cloud PCD roi o3d filtered ​​​​​ Step 150, using DBSCAN The clustering algorithm divides the specified point cloud PCD roi o3d filtered into multiple clusters, and filters the source point cloud that meets the size requirement according to the number of point clouds of each cluster PCD selected .​​ 4. The target detection model based grasp pose calculation method of claim 3, wherein: The step 200 comprises: Step 210, based on the template point cloud PCD target _ o3d _ down normals and source point clouds PCD selected The normal, using KD- Tree The method involves searching for up to 50 points within a neighborhood of each point with a radius of 0.7 to 0.9 meters to calculate the normal vector. Step 220, using the mixed KD-Tree Method finds up to 200 points in a 0.7-0.9 meter neighborhood of each point to compute the template point cloud PCD target o3d down and the source point cloud PCD selected feature vector of each point in the source point cloud FPFH ​​​ Step 230, based on FPFH characteristic vectors, by RANSAC source point cloud PCD selected with the template point cloud PCD target o3d down alignment, to obtain a transformation matrix of the coarse matching result M RANSAC .​​ 5. The grabbing point pose calculation method based on a target detection model according to claim 4, characterized in that: D 1( PCD 1, PCD target _ o3d _ down ) is: ； ； D 2( PCD 2, PCD target _ o3d _ down The computational expression for the calculation of the value of the variable X is: ； ； wherein represent the transformed source point cloud PCD the first i point in 1, represent the transformed source point cloud PCD the first i point in 2, t j represent PCD target _ o3d _ down the first j point in.

6. The target detection model based grasp pose calculation method of claim 4, wherein: The step 500 comprises the steps of: Step 510, extracting a rotation matrix from the compensation transformation matrix R R R gripper ;​​​ Step 520, the rotation matrix R gripper is converted to Euler angles relative to the robot arm; Step 530, the axis angle is corrected to obtain an Euler angle change amount; Z Step 530, the axis angle is corrected to obtain an Euler angle change amount; At step 540, the pose of the robot arm is obtained according to the Euler angle change amount and the current pose angle of the robot arm e new ; and the end pose of the robot arm is obtained according to the compensation transformation matrix and the linear transformation matrix T r pred ;​ Step 550, subtract the current pose of the robot arm from the robot arm end pose r pred , to obtain a new pose point of the robot arm r target ; Step 560, according to the mechanical arm posture e new and the new pose point of the mechanical arm r target , get the grasp point pose of the object to be grasped; In the formula, the pose of the pickup point of the object to be picked up base point The calculation expression of the pose of the pickup point of the object to be picked up is:​ ； In the formula, sub is a fixed offset between the point of taking a picture and the point of grabbing.

7. The target detection model based grasp pose calculation method of claim 6, wherein: The linear transformation matrix in step 540 T The computational expression for is ； ； ； In the formula, n is the number of data groups of the collected end position of the mechanical arm; c i represents the first i translation vector in the camera coordinate system obtained after the group data is matched by GICP; r i represents the first i end position coordinate of the mechanical arm in the group data.

8. A method of automatically guiding a gripping device, characterized in that: The mechanical arm performs the operation of grabbing the object to be grabbed according to the grabbing point pose calculated by the calculation method according to any one of claims 1-7.

Images