Grabbing point pose calculation method based on geometric clustering algorithm and application thereof

Abstract

The invention discloses a grasp point pose calculation method based on a geometric clustering algorithm without deep learning and a GPU (Graphics Processing Unit) and application of the grasp point pose calculation method. The grabbing point pose calculation method comprises the steps that 100, three-dimensional point cloud data of a to-be-grabbed object are obtained through an industrial-grade structured light three-dimensional camera, a geometric clustering algorithm is adopted for screening the three-dimensional point cloud data, and a unique target point cloud cluster conforming to the target form and size is extracted; step 200, performing rough matching on the target point cloud cluster and the template point cloud through the FPFH feature vector and RANSAC to obtain a rough matching result; step 300, performing fine matching on the target point cloud cluster and the template point cloud through the rough matching result and the GICP to obtain a fine registration result; step 400, judging whether a precise registration result meets a preset requirement or not according to the interior point root-mean-square error and the overlap ratio, if so, entering the next step, and otherwise, terminating the process; and 500, according to the fine registration result and coordinate transformation, the grabbing point pose of the to-be-grabbed object is obtained through calculation.

Description

Technical Field

[0001] This invention relates to the technical fields of industrial automation and computer vision, and more specifically, to a method for calculating the pose of grasping points based on geometric clustering algorithms and its application. Background Technology

[0002] Traditional robotic arm grasping control mainly relies on preset fixed trajectories and manually set pose parameters, making it difficult to flexibly handle the dynamic recognition and precise positioning of various types of workpieces. Especially when dealing with complex industrial parts with high reflectivity, high precision requirements, and occlusion interference, traditional methods are significantly insufficient in terms of efficiency and accuracy, making it difficult to meet the requirements of intelligent manufacturing for high adaptability and automation levels.

[0003] The applicant of this application has disclosed a method for calculating the coordinates of a defect center from a two-dimensional image in patent application CN2024116137510. This method solves for the three-dimensional position coordinates (X, Y, Z) of the defect center in the robot's base coordinate system, allowing the robot gripper to move the defect center to the imaging center of the three-dimensional camera, serving the subsequent three-dimensional imaging and inspection process. However, to achieve "high-precision vision-guided gripping," a complete six-degree-of-freedom gripping pose (X, Y, Z, Rx, Ry, Rz) that simultaneously includes position and orientation is required to guide the robot arm to move precisely to the object to be gripped for accurate grasping. Existing technologies have not yet solved this technical problem. Summary of the Invention

[0004] The technical problem to be solved by this invention is to provide a method for calculating the pose of grasping points based on geometric clustering algorithms without deep learning or GPU, and its application. This solution can efficiently handle the recognition of multiple types of targets and automatic pose registration using only a CPU, and has stronger versatility and adaptability. The technical solution is as follows:

[0005] The method for calculating the pose of grasping points based on geometric clustering algorithm is characterized by the following steps:

[0006] Step 100: Acquire the 3D point cloud data of the object to be grasped using an industrial-grade structured light 3D camera, and use a geometric clustering algorithm to filter the 3D point cloud data to extract the unique target point cloud cluster that matches the target shape and size.

[0007] Step 200: Perform coarse matching between the target point cloud cluster and the template point cloud using FPFH feature vectors and RANSAC to obtain coarse matching results;

[0008] Step 300: Perform fine matching between the target point cloud cluster and the template point cloud using the coarse matching results and GICP to obtain fine registration results;

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

[0010] Step 500: Based on the fine registration results and coordinate transformation, calculate the pose of the grasping point of the object to be grasped.

[0011] Step 100 includes the following steps:

[0012] Step 110: Filter the points in the 3D point cloud data according to the depth range along the Z-axis, retaining the specified point cloud PCDs within the specified distance range. roi _ filtered ;

[0013] Step 120, convert the template point cloud PCD target and the selected point cloud PCD roi _ filtered The NumPy format data were converted into template point cloud PCD in Open3D respectively. target _ o3d and specified point cloud PCD roi _ o3d ;

[0014] Step 130: Using a uniform sampling method, the specified point cloud PCD roi _ o3d and template point cloud PCD target _ o3d They are respectively converted into PCD of a specified point cloud with lower density. roi _ o3d _ down and template point cloud PCD target _ o3d _ down ;

[0015] Step 140, for the specified point cloud PCD roi _ o3d _ down Perform filtering and denoising to obtain the specified point cloud PCD. roi _ o3d _ filtered ;

[0016] Step 150, for the specified point cloud PCD roi _ o3d _ filtered Clustering is performed to obtain multiple clusters, PCD;

[0017] Step 160: Filter the geometric distances of the clusters PCD to obtain the target point cloud clusters PCD. selected

[0018] An automated guided grasping method includes the following steps: the robotic arm performs the grasping operation based on the grasping point pose calculated by the above calculation method.

[0019] Compared with existing technologies, the grasping point pose calculation method based on geometric clustering algorithm of the present invention and its application have the following advantages:

[0020] (1) This invention classifies the original point cloud using a geometric clustering algorithm, ultimately retaining only the target point cloud cluster that uniquely matches the target definition. This not only avoids multi-target confusion in subsequent pose calculations but also significantly reduces the number of invalid points input to subsequent algorithms, directly lowering the time cost of FPFH feature calculation and RANSAC iterative verification. Furthermore, eliminating noise points improves the effectiveness of FPFH feature vectors, reduces the number of iterations for RANSAC coarse matching, and lowers the convergence difficulty of GICP fine matching, ultimately improving the speed and stability of the overall pose calculation process. The target point cloud cluster extracted by the geometric clustering algorithm can cover the complete surface of the object to be grasped, providing sufficient geometric information for subsequent pose calculations and ensuring the accuracy and real-time performance of the final grasp point pose calculation.

[0021] (2) In industrial settings, many existing devices (such as outdated industrial control computers and embedded controllers) are typically equipped only with basic CPUs, lacking GPU hardware support and being difficult to integrate with deep learning frameworks (requiring complex environment configuration and dependency library installation). In contrast, this invention, which classifies raw point clouds using a geometric clustering algorithm (non-deep learning), relies solely on CPU operation. This eliminates the need for additional GPU card purchases and hardware upgrades to existing equipment, allowing direct deployment on traditional industrial control terminals. Furthermore, it eliminates the need for deep learning frameworks and their associated dependency libraries; it can be implemented simply by calling traditional point cloud processing libraries using C++ / Python, reducing environmental compatibility issues. Consequently, hardware procurement and maintenance costs are reduced, resulting in significant cost optimization.

[0022] (3) The geometric clustering of this invention, as well as the subsequent FPFH feature extraction, RANSAC coarse matching, and GICP fine matching, are all modular, low-computing-power traditional algorithms. Unlike deep learning models, they do not rely on a large number of parallel computing units (GPU cores). They can be completed within a specified time using only single-core / multi-core serial computing of the CPU. The logic is rigorous and the anti-interference ability is strong. The failure rate during continuous operation is lower, and the latency can be stably controlled within the industrial demand range, avoiding the lag in the grasping rhythm caused by the algorithm not being compatible with the CPU. Moreover, it can be easily ported to devices with different CPU architectures and different operating systems without the need for redevelopment or debugging for different hardware, and it is compatible with multiple brands of robotic arms and multiple types of industrial cameras.

[0023] (4) In the coarse matching stage, this invention employs FPFH feature vectors (which can effectively capture local geometric features of the point cloud) and the RANSAC algorithm (which excels at removing outliers caused by noise and occlusion). This allows for the rapid acquisition of reliable initial registration results even in the presence of partial occlusion and point cloud noise, avoiding the shortcomings of traditional coarse matching which is susceptible to interference. In the fine matching stage, the coarse matching result is used as the initial value. Combining the advantages of the GICP algorithm, which converges faster and has higher accuracy, this invention solves the problems of traditional ICP algorithm being prone to getting trapped in local optima and having slow convergence, ultimately obtaining a high-precision registration result. Thus, this invention, through the combination of coarse and fine matching, not only ensures the robustness of registration but also significantly improves the registration accuracy, solving the problems of insufficient accuracy or poor robustness of traditional single matching methods, and laying the foundation for subsequent pose calculation.

[0024] (5) Before the pose coordinate transformation, this invention quantifies and evaluates the fine registration result by using the root mean square error of the interior points and the degree of overlap, actively eliminating unqualified results, reducing invalid grasping attempts from the process, and ensuring that only results that meet the grasping accuracy requirements enter the subsequent steps, significantly improving the grasping reliability. Since the sources of error in the coordinate transformation are strictly controlled, the pose calculation error that depends on the fine registration result is smaller. It has been verified that the positioning accuracy error of the grasping point pose calculation of this invention is ±0.1mm, which can more accurately reflect the actual spatial pose of the object to be grasped, and meet the high precision requirements of grasping for "position + attitude" (such as the angle and position deviation of the robotic arm grasping need to be controlled at the millimeter level).

[0025] In summary, the grasping point pose calculation method based on geometric clustering algorithm of the present invention and its application process are simple, easy to implement and control. By calculating the pose using geometric clustering algorithm and 3D geometric information, the entire process of "efficient data screening - robust fine registration - quality verification - accurate pose calculation" is optimized. It is superior to traditional methods in terms of efficiency, accuracy, reliability and scene adaptability. It can efficiently handle the identification of multiple types of targets and automatic fine registration of poses, and significantly improve the application effect and intelligence level of industrial grasping systems in complex environments.

[0026] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0027] The accompanying drawings, which form part of this invention, are used to aid in understanding the invention. The content provided in the drawings and their related descriptions can be used to explain the invention, but do not constitute an undue limitation of the invention. In the drawings:

[0028] Figure 1 This is a schematic diagram of the three-dimensional point cloud data of the object to be grasped in an embodiment of the present invention.

[0029] Figure 2 For the specified point cloud PCD in the embodiments of the present invention roi _ filtered A schematic diagram.

[0030] Figure 3 This is a schematic diagram of the clustering PCD in an embodiment of the present invention.

[0031] Figure 4 PCD of point cloud clusters in this embodiment of the invention selected1 A schematic diagram.

[0032] Figure 5 PCD of point cloud clusters in this embodiment of the invention selected2 A schematic diagram.

[0033] Figure 6 This is a schematic diagram illustrating the coarse matching effect in an embodiment of the present invention.

[0034] Figure 7 This is a schematic diagram illustrating the fine matching effect in an embodiment of the present invention. Detailed Implementation

[0035] The present invention will now be clearly and completely described in conjunction with the accompanying drawings. Those skilled in the art will be able to implement the present invention based on these descriptions. Before describing the present invention in conjunction with the accompanying drawings, it should be particularly noted that:

[0036] The technical solutions and features provided in the various parts of this invention, including the following description, can be combined with each other without conflict.

[0037] Furthermore, the embodiments of the present invention described below are generally only some, not all, of the embodiments of the present invention. Therefore, all other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort should fall within the scope of protection of the present invention.

[0038] Regarding the terminology and units used in this invention: The terms "comprising," "having," and any variations thereof in the specification, claims, and related parts of this invention are intended to cover non-exclusive inclusion.

[0039] The specific implementation of the grasping point pose calculation method based on geometric clustering algorithm of the present invention includes steps 100 to 400, as follows:

[0040] Step 100: Acquire 3D point cloud data of the object to be grasped using an industrial-grade structured light 3D camera. Filter the 3D point cloud data using a geometric clustering algorithm to extract the unique target point cloud cluster that matches the target's shape and size. This specifically includes steps 110-160, as follows:

[0041] Step 110: Filter the points in the 3D point cloud data according to the depth range along the Z-axis, retaining the specified point cloud PCDs within the specified distance range. roi _ filtered ;

[0042] Step 120, convert the template point cloud PCD target and the selected point cloud PCD roi _ filtered The NumPy format data were converted into template point cloud PCD in Open3D respectively. target _ o3d and specified point cloud PCD roi _ o3d , can be represented as: _

[0043] ;

[0044] ;

[0045] Step 130: To reduce point cloud density and computational overhead, a uniform sampling method is used to reduce the PCD of the specified point cloud. roi _ o3d and template point cloud PCD target _ o3d They are respectively converted into PCD of a specified point cloud with lower density. roi _ o3d _ down and template point cloud PCD target _ o3d _ down , can be represented as:

[0046] ;

[0047] ;

[0048] In the formula, every_k_points is based on different input levels (Class). inputThe pre-defined sampling parameters mean "how many points are left before retaining one point". The specific value of every_k_points is pre-stored in the level-parameter mapping table. The principle for its value is that it is proportional to the geometric complexity of the target object. If the target is larger and the number of point clouds collected is greater, a larger every_k_points value is set in order to obtain higher data processing efficiency.

[0049] Step 140, for the specified point cloud PCD roi _ o3d _ down Perform filtering and denoising to obtain the specified point cloud PCD. roi _ o3d _ filtered ;

[0050] Specifically, outliers are first removed based on neighbor statistics (statistical filtering), then isolated points with too few surrounding points are removed (radius filtering), and finally, outliers in edge or coarse regions are removed using the rate of change of normal (normal smoothing filtering), thus obtaining the filtered and denoised PCD of the specified point cloud. roi _ o3d _ filtered .

[0051] Step 150: Use the DBSCAN clustering algorithm to perform PCD analysis on the filtered and denoised specified point cloud. roi _ o3d _ filtered It is divided into multiple clusters, PCD.

[0052] Step 160: Filter the geometric distances of the clusters PCD to obtain the target point cloud clusters PCD. selected Specifically, steps 161 and 162 are included, as follows:

[0053] Step 161: Calculate the distances d1 and d2 from the two diagonal points P1 and P2 of each cluster PCD to the centroid C of the current cluster, and discard the distance differences. Clusters with a distance greater than a preset distance δ (PCD)

[0054] The filtering criteria can be expressed as:

[0055] ;

[0056] ;

[0057] This step can remove elongated clusters, and the remaining point cloud clusters are represented as PCD. selected1 .

[0058] Step 162, calculate the PCD for each point cloud cluster. selected1 diagonal distance Select point cloud clusters (PCDs) whose diagonal distance falls within a specified range. selected1 ;

[0059] The filtering criteria can be expressed as:

[0060] ;

[0061] In the formula, d min and d max These are the maximum and minimum values ​​for the specified diagonal distance, respectively.

[0062] If point cloud clusters PCD selected2 If the quantity is 1, then the target point cloud cluster PCD is obtained. selected If point cloud clusters PCD selected2 The number of point cloud clusters (PCDs) is more than one or does not meet the preset distance and preset range. selected2 If so, change the input level of the test object and repeat steps 161-162. The input level, Class... input The upstream production management system (MES) or enterprise resource planning system (ERP) automatically generates and sends the data to this system based on production order information, serving as the target parameters for the visually guided grasping task.

[0063] Step 200 involves performing a coarse matching between the target point cloud cluster and the template point cloud using FPFH (Fast Point Feature Histograms) feature vectors and RANSAC to obtain the coarse matching result; specifically, steps 210 to 230 are as follows:

[0064] Step 210, based on the template point cloud PCD target _ o3d _ down normals and target point cloud clusters The normal vector is calculated by searching up to 50 points in a neighborhood with a radius of 0.8 meters for each point using the KD-Tree method.

[0065] Step 220: Use a hybrid KD-Tree method to find up to 200 points within a 0.8-meter neighborhood of each point to calculate the PCD of the template point cloud. target _ o3d _ down and target point cloud cluster PCD selected The FPFH feature vector of each point in the vector.

[0066] The FPFH feature vector of each point is a 33-dimensional vector (in Open3D) that describes the geometry around the point, including geometric relationships such as relative normals, angles, and distances between surrounding point pairs.

[0067] Template Point Cloud PCD target _ o3d _ down The FPFH feature vector of each point is represented as FPFH target (q), where q∈PCD target _ o3d _ down .

[0068] Target point cloud cluster PCD selected The FPFH vector of each point in the vector is represented as FPFH. selected (p), where p∈PCD selected .

[0069] Step 230: Based on the FPFH feature vector, the target point cloud cluster PCD is generated using RANSAC. selected With template point cloud PCD target _ o3d _ down Alignment, i.e., obtaining the transformation matrix M of the coarse matching result. RANSAC (Includes rotation and translation information).

[0070] Step 300 involves performing a fine matching between the target point cloud cluster and the template point cloud using the coarse matching result and GICP (Generalized ICP, an improved iterative nearest point algorithm) to obtain the fine registration result; specifically, steps 310 to 320 are as follows:

[0071] Step 310, PCD of the target point cloud cluster is obtained through GICP. selected With template point cloud PCD target _ o3d _ down Alignment yields the initial transformation matrix M of the fine registration result. GICP .

[0072] GICP is a locally iterative algorithm that heavily relies on a good initial value. The ultimate goal of GICP is to find an optimal rotation matrix R and translation vector t through iterative optimization, minimizing the weighted sum of corresponding point distances. This invention utilizes the transformation matrix M of the coarse matching result. RANSAC As the starting point of this iterative optimization process, the specific steps are as follows: First, use the transformation matrix M RANSAC To initialize the corresponding rotation matrix Translation vector Then gradually adjust the rotation matrix. Translation vector It eventually converges to the optimal solution M described by the following formula. GICP This makes the target point cloud cluster PCD selected Points in After transformation, it can be most closely matched with the template point cloud PCD. target _ o3d _ down The corresponding point in Alignment is performed to minimize the cost function under local covariance constraints, ultimately yielding the target point cloud cluster PCD after GICP registration. selected Compared to template point cloud PCD target _ o3d _ down The initial transformation matrix .

[0073] ;

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

[0075] Step 320: Calculate the PCD of the target point cloud cluster. 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. .

[0076] Although GICP has performed precise registration on the target point cloud cluster in step 310, it does not guarantee perfect alignment of the "geometric centers" of the two point clouds because GICP is based on local minimum error optimization. Therefore, after registration, the difference between the center points of the two point clouds is calculated and added as an additional translation compensation vector to the initial transformation matrix M of GICP. GICP This allows for higher precision position alignment.

[0077] After obtaining the initial transformation using GICP, the target point cloud cluster is PCD selected The geometric center transformation is as follows:

[0078] ;

[0079] In the formula, PCD for target point cloud cluster selected The geometric center, The value range is 1 to ,common One source point.

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

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

[0082] .

[0083] Step 400: Determine whether the fine registration result meets the preset requirements based on the root mean square error of the inlier and the fit. If it does, proceed to the next step; otherwise, terminate the process.

[0084] The fitness parameter is used to calculate the overlapping area (internal point correspondence / target point count). Both inlier_rmse and fitness are preliminary screening quality standards set to quickly eliminate completely failed fine-match results, and are both empirical parameter values.

[0085] The preset requirements are that inlier_rmse is less than 0.5m and fitness is greater than 0.1 (ratio); 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.

[0086] Because point clouds acquired by 3D cameras contain a large amount of background interference, the fine registration results may contain local mismatches. The root mean square error of inliers (inlier_rmse) and the fit 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:

[0087] Step 410, calculate the PCD of the template point cloud. target _ o3d _ down Each point in the matrix is ​​transformed to the matrix M after the first transformation. GICPThe distance set D1(PCD1, PCD) is formed by the minimum Euclidean distance of the transformed target point cloud cluster PCD1. target _ o3d _ down The calculation expression is:

[0088] ;

[0089] ;

[0090] Step 420, from D1(PCD1, PCD) target _ o3d _ down The distance value d1 is taken from the 20th element after sorting (all elements are numerically sorted from smallest to largest). The 20th element is the 20th largest value among all elements, which 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.

[0091] Step 430, calculate the PCD of the template point cloud. target _ o3d _ down The compensation transformation matrix for each point in the matrix The distance set D2(PCD2, PCD) is formed by the minimum Euclidean distances of the transformed target point cloud cluster PCD2. target _ o3d _ down The calculation expression is:

[0092] ;

[0093] ;

[0094] In the formula, Represents the transformed target point cloud cluster The first in One point; Represents the transformed target point cloud cluster The first in One point; represent The first in One point.

[0095] Step 440, from D2(PCD2, PCD) target _ o3d _ downThe distance value d2 is taken from the 20th position from the bottom of the sorted list. If d2 is less than or equal to 15mm and d2 is less than d1, the filtering is complete; otherwise, the process terminates. Analysis of historical registration data shows that when d2 > 15mm, the capture failure rate is high; while when d2 ≤ 15mm, the capture success rate is high and stable.

[0096] 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:

[0097] Step 510, from the compensation transformation matrix Extract the rotation matrix R, and transform the rotation matrix R from the camera coordinate system to the gripper coordinate system to obtain the rotation matrix R. gripper , can be represented as;

[0098] ;

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

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

[0101] ;

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

[0103] Step 530: Correct the Z-axis angle to obtain the Euler angle change;

[0104] If the rotation angle around the Z-axis 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. Invert the angle around the X-axis (multiply by -1), Become With everything else remaining unchanged, the corrected Euler angles are: .

[0105] Euler angles Limited to The range, if it exceeds Then subtract If less than Then add The updated Euler angle change is obtained. .

[0106] Step 540: Obtain the robot arm's posture based on the Euler angle change and the robot arm's current posture angle. According to the compensation transformation matrix The pose of the robotic arm's end effector is obtained by combining the linear transformation matrix T. ;

[0107] In the formula, The current posture angle of the robotic arm, in degrees. o T is a linear transformation matrix, representing the posture correction of the robotic arm due to the deviation of the vision-guided camera. It has been pre-calculated and input before the process begins.

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

[0109] Suppose n sets of robotic arm end-effector pose data are collected, each set containing:

[0110] : No. The translation vector in the camera coordinate system obtained after matching the data sets via GICP;

[0111] : Corresponding to the first The coordinates of the robotic arm's end effector position in the dataset.

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

[0113] ; .

[0114] According to the least squares solution formula Such that for all i ( satisfy That is, to obtain the linear transformation matrix. .

[0115] Step 550, change the current pose r of the robotic arm current =[x,y,z,r x ,r y ,r z Subtract the end effector pose r pred The new pose point r of the robotic arm is obtained. target ;

[0116] Step 560, based on the robotic arm posture e new and the new pose point r of the robotic arm target , thus obtaining the pose of the grasping point of the object to be grasped;

[0117] The expression for calculating the pose of the grasping point (base_point) of the object to be grasped is:

[0118] ;

[0119] In the formula, sub is the fixed offset between the photo capture point and the capture point.

[0120] The specific implementation of the automatic guided grasping method of the present invention includes the following steps: the robotic arm performs the operation of grasping the object to be grasped according to the grasping point pose base_point calculated by the above calculation method.

[0121] The beneficial effects of the present invention will be illustrated below through specific embodiments. The structure of the applied device and the object to be grasped are described in Chinese Invention Patent CN2024116137510.

[0122] 1. Data Preparation

[0123] (1) The transformation matrix M from the camera coordinates to the gripper coordinate system obtained by the eye-on-hand calibration method camera2gripper :

[0124] 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]];

[0125] R camera2gripper The transformation matrix M camera2gripper Use The extracted rotation matrix.

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

[0127] (3) every_k_points=6.

[0128] (4) Linear transformation matrix =[[-0.66061021, 0.76010278, 0.03095112], [0.76627342, 0.65027625, 0.01655252], [0.03792626, 0.02132504, -1.01296402]].

[0129] (5) Fixed offset sub=[4.55886881, -112.94048847, 179.23292355, 89.73, 0.0, 0.00998].

[0130] 2. Perform pose calculation for the grab point.

[0131] Step 100

[0132] Figure 1 To acquire 3D point cloud data of the object to be grasped using an industrial-grade structured light 3D camera.

[0133] After filtering according to the Z-axis depth range in step 110, the following results are obtained: Figure 2 The specified point cloud PCD shown roi _ filtered .

[0134] After processing in steps 120-150, the result is as follows: Figure 3 The cluster shown is PCD.

[0135] After step 161, the result is as follows: Figure 4 The point cloud cluster PCD shown selected1 .

[0136] After step 162, only one point cloud cluster PCD remains. selected2 like Figure 5 As shown, this is the target point cloud cluster PCD. selected .

[0137] Step 200

[0138] Coarse matching of the target point cloud cluster and the template point cloud is performed using FPFH feature vectors and RANSAC. The coarse matching result is as follows: Figure 6 As shown, the transformation matrix M yields the coarse matching result. RANSAC =[[ 0.9980247,-0.1922363,0.02907352,-1],[ 0.09893429,0.99921837,0.0268385,0],[-0.04930339,-0.02491435, 0.99877176,0.55858927],[ 0,0,0,1]。

[0139] Step 300

[0140] The translation vector t = [-1.2767879, 0.0808927, 0.58689312].

[0141] The rotation matrix R = [[0.99958284,-0.02428889,0.01562692],[0.02425204,0.99970264,0.0025431],[-0.01568404,-0.00216305,0.99987466]。

[0142] Transformation matrix =[[ 0.99958284,-0.02428889,0.01562692,-1.2767879],[0.02425204,0.99970264,0.0025431,0.0808927],[ -0.01568404,-0.00216305,0.99987466,0.58689312],[ 0,0,0,1]。

[0143] Translation compensation vector Δt = [0.11346623, -0.02492787, -0.04078774].

[0144] Fine matching effect such as Figure 7 As shown, the compensation transformation matrix =[[ 0.99958284,-0.02428889,0.01562692,-1.16332167],[ 0.02425204,0.99970264,0.0025431,0.05596483],[-0.01568404,-0.00216305,0.99987466,0.54610538],[ 0,0,0,1]。

[0145] Step 400

[0146] The fit of the fine match is 1.000000, the root mean square error of the inlier is 0.1273413m, and the number of corresponding point pairs involved in the registration is 8106, which satisfies that inlier_rmse is less than 0.5m and fit is greater than 0.1.

[0147] Through steps 410-440, we obtain d1=0.5417807543059476mm and d2=0.4961663116106391mm, which satisfies that d2 is less than 15mm and d2 is less than d1.

[0148] Following the steps above, all 8106 point pairs were successfully matched.

[0149] Step 500

[0150] Step 510: =[[0.99987654,−0.01234567,0.00567891],[0.01234567,0.99987654,0.00345678],[−0.00567891,−0.00345678,0.99998765]。

[0151] Step 520: Use the rotation matrix R gripper Converted to Euler angles relative to the robotic arm: e = [2.66101393, -0.03327033, 2.67023842].

[0152] Step 530: Corrected Euler Angles =[2.66101393, -0.03327033, 2.67023842].

[0153] Updated Euler angle variation =[2.66101393, -0.03327033, 2.67023842].

[0154] Step 540: =[32.79101393, 1.71672967, -179.38791158].

[0155] =[[−0.64278761,0.76604444,−0.00000001],[0.76604444,0.64278761,0.00000000],[0.03792626,0.02132504,−1.01296402]。

[0156] Step 550: r target =[-171.77991179, 1251.45200723, 688.68151982, 32.79101393, 1.71672967, -179.38791158].

[0157] Step 550: base_point=[-176.3387806, 1364.3924957, 509.44859627, -56.93898607, 1.71672967, -179.39789158].

[0158] The total time for calculating the pose of the grasping points was 2.0447165489196777 seconds. This processing time is considered above average in industrial vision guidance and fast in 3D vision guidance, even surpassing the speed of grasping point pose calculations based on object detection models (built on a deep learning architecture). Based on rigorous mathematical coordinate transformation, the positioning accuracy of this grasping point pose calculation is ±0.1 mm, which is higher than traditional methods.

[0159] The foregoing has described the relevant content of the present invention. Those skilled in the art will be able to implement the present invention based on these descriptions. All other embodiments obtained by those skilled in the art based on the above description of the present invention without inventive effort should fall within the scope of protection of the present invention.

Claims

1. A method for calculating the pose of grasping points based on geometric clustering algorithm, characterized in that: Includes the following steps: Step 100: Acquire the 3D point cloud data of the object to be grasped using an industrial-grade structured light 3D camera, and use a geometric clustering algorithm to filter the 3D point cloud data to extract the unique target point cloud cluster that matches the target shape and size. Step 200: Perform coarse matching between the target point cloud cluster and the template point cloud using FPFH feature vectors and RANSAC to obtain coarse matching results; Step 300: Perform fine matching between the target point cloud cluster and the template point cloud using the coarse matching results and GICP to obtain fine registration results; Step 400: Determine whether the fine registration result meets the preset requirements based on the root mean square error of the interior points and the degree of overlap. If it does, proceed to the next step; otherwise, terminate the process. Step 500: Based on the fine registration results and coordinate transformation, calculate the pose of the grasping point of the object to be grasped; Step 100 includes the following steps: Step 110: Filter the points in the 3D point cloud data according to the depth range along the Z-axis, retaining the specified point cloud PCDs within the specified distance range. roi _ filtered ; Step 120, convert the template point cloud PCD target and the selected point cloud PCD roi _ filtered The NumPy format data were converted into template point cloud PCD in Open3D respectively. target _ o3d and specified point cloud PCD roi _ o3d ; Step 130: Using a uniform sampling method, the specified point cloud PCD roi _ o3d and template point cloud PCD target _ o3d They are respectively converted into PCD of a specified point cloud with lower density. roi _ o3d _ down and template point cloud PCD target _ o3d _ down ; Step 140, for the specified point cloud PCD roi _ o3d _ down Perform filtering and denoising to obtain the specified point cloud PCD. roi _ o3d _ filtered ; Step 150, for the specified point cloud PCD roi _ o3d _ filtered Clustering is performed to obtain multiple clusters, PCD; Step 160: Filter the geometric distances of the clusters PCD to obtain the target point cloud clusters PCD. selected .

2. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 1, characterized in that: Step 160 includes the following steps: Step 161: Calculate the distances from the two diagonal points of each cluster PCD to the centroid of the current cluster, and remove clusters PCD with distance differences greater than a preset distance. The remaining point cloud clusters are represented as PCD. selected1 ; Step 162, calculate the PCD for each point cloud cluster. selected1 The diagonal distance is used to filter out point cloud clusters (PCDs) whose diagonal distance falls within a specified range. selected1 , represented as point cloud cluster PCD selected2 ; If point cloud clusters PCD selected2 If the quantity is 1, then the target point cloud cluster PCD is obtained. selected If point cloud clusters PCD selected2 The number of point cloud clusters (PCDs) is more than one or does not meet the preset distance and preset range. selected2 If so, change the input level of the test object and repeat steps 161-162.

3. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 2, characterized in that: Step 200 includes: Step 210, based on the template point cloud PCD target _ o3d _ down normals and target point cloud clusters The normal vector is calculated by searching up to 50 points in a neighborhood with a radius of 0.7 to 0.9 meters for each point using the KD-Tree method. Step 220: Use a hybrid KD-Tree method to find up to 200 points in the 0.7–0.9 meter neighborhood of each point to calculate the PCD of the template point cloud. target _ o3d _ down and target point cloud cluster PCD selected The FPFH feature vector of each point in the vector; Step 230: Based on the FPFH feature vector, the target point cloud cluster PCD is generated using RANSAC. selected With template point cloud PCD target _ o3d _ down Alignment yields the transformation matrix of the coarse matching result. .

4. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 3, characterized in that: Step 300 includes: Step 310, PCD of the target point cloud cluster is obtained through GICP. selected With template point cloud PCD target _ o3d _ down Alignment yields the initial transformation matrix M of the fine registration result. GICP ; Step 320: Calculate the PCD of the target point cloud cluster. 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. In the process, the compensation transformation matrix of the fine registration result is obtained. .

5. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 4, characterized in that: Step 400 also includes filtering the fine registration results that meet the capture requirements, including the following steps: Step 410, calculate the PCD of the template point cloud. target _ o3d _ down Each point in the matrix is ​​transformed to the matrix M after the first transformation. GICP The distance set D1(PCD1, PCD) is formed by the minimum Euclidean distance of the transformed target point cloud cluster PCD1. target _ o3d _ down ); Step 420, from D1(PCD1, PCD) target _ o3d _ down Take the distance value d1 from the 20th element from the end of the sorted sequence. Step 430, calculate the PCD of the template point cloud. target _ o3d _ down The compensation transformation matrix for each point in the matrix The distance set D2(PCD2, PCD) is formed by the minimum Euclidean distances of the transformed target point cloud cluster PCD2. target _ o3d _ down ); Step 440, from D2(PCD2, PCD) target _ o3d _ down If the distance value of the 20th item from the bottom of the sorted list is d2, and if d2 is less than or equal to the preset value and d2 is less than d1, then the filtering is complete; otherwise, the process is terminated.

6. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 5, characterized in that: D1(PCD1, PCD target _ o3d _ down The calculation expression for ) is: ； ； D2(PCD2, PCD target _ o3d _ down The calculation expression for ) is: ； ； In the formula, Represents the transformed target point cloud cluster The first in One point, Represents the transformed target point cloud cluster The first in One point, represent The first in One point.

7. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 5, characterized in that: Step 500 includes the following steps: Step 510, from the compensation transformation matrix Extract the rotation matrix R, and transform the rotation matrix R from the camera coordinate system to the gripper coordinate system to obtain the rotation matrix R. gripper ; Step 520, rotate matrix R gripper Converted to Euler angles relative to the robotic arm; Step 530: Correct the Z-axis angle to obtain the Euler angle change; Step 540: Based on the Euler angle change and the current attitude angle of the robotic arm, obtain the robotic arm attitude e. new According to the compensation transformation matrix The linear transformation matrix T is used to obtain the end effector pose r of the robotic arm. pred ; Step 550: Subtract the robotic arm's end effector pose r from the current pose of the robotic arm. pred The new pose point r of the robotic arm is obtained. target ; Step 560, based on the robotic arm posture e new and the new pose point r of the robotic arm target , thus obtaining the pose of the grasping point of the object to be grasped; The expression for calculating the pose of the grasping point (base_point) of the object to be grasped is: ； In the formula, sub is the fixed offset between the photo capture point and the capture point.

8. The method for calculating the pose of grasping points based on geometric clustering algorithm as described in claim 7, characterized in that: The expression for calculating the linear transformation matrix T in step 540 is: ； ； ； In the formula, n is the number of data sets of the robot arm end-effector pose collected; Indicates the first The translation vector in the camera coordinate system obtained after matching the data sets via GICP; Indicates the first The coordinates of the robotic arm's end effector position in the dataset.

9. An automatic guided crawling method, characterized in that: The steps include: the robotic arm calculates the gripping point pose according to the calculation method of any one of claims 1-8 and performs the operation of gripping the object to be gripped.

Images