Multi-target pose estimation and sorting method based on optimization template

Abstract

The invention relates to a multi-target pose estimation and sorting method based on an optimization template, and the method comprises the steps: importing a model of a target workpiece, and making a model point cloud library; collecting a scene point cloud of the target workpiece by using a depth camera; extracting model point cloud and scene features; loading the scene point cloud; removing scene background information through an RANSAC (Random Sample Consensus) algorithm; sparsification is carried out on the scene point cloud after the background is removed; segmenting the sparsified scene point clouds through clustering, analyzing point cloud features in each cluster according to PCA, dividing the point clouds with the same features into the same category, and obtaining point cloud information of each object in each category; further adopting a point pair feature PPF voting algorithm to carry out coarse registration on the object point cloud and the model point cloud to obtain a pose cluster; and on the basis of the coarse registration, performing fine registration on the object point cloud and the template by adopting an iterative closest point (ICP) algorithm. The method has the advantages that all target objects in a scattered state can be recognized and distinguished through one-time shooting, the poses of all the targets are obtained, the robot is controlled to sequentially execute sorting according to the estimated poses, and the operation efficiency is high.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of robot engineering application, and specifically proposes a multi-target pose estimation and sorting method based on optimized template, which is used for efficient workpiece identification, detection and 6D pose estimation in industrial production, so as to accurately control the robot and perform sorting operation. BACKGROUND

[0002] With the improvement of industrial automation in China, the traditional operation mode of sorting different types of workpieces by manual work cannot meet the working requirements of the increasingly accelerated production line, especially some simple and repetitive application scenarios urgently need to realize efficient automatic sorting. Therefore, the visual servo system based on machine vision and industrial robot is born, which can effectively solve the simple and repetitive work, and with the improvement of sorting efficiency and identification accuracy, it promotes the transformation of traditional industry to intelligence and automation.

[0003] Since the traditional robot operation is mostly equipped with an industrial two-dimensional vision system, the two-dimensional vision needs to identify and locate the key points in the image when performing pose estimation, and these key points are easily affected by external factors such as light and shielding. In addition, for some complex scenes or specific applications, the evaluation index is difficult to comprehensively reflect the accuracy of pose estimation.

[0004] Therefore, the present application proposes a multi-target pose estimation and sorting method based on optimized template, which aims to solve the problem of low efficiency and poor precision caused by using the original workpiece template for registration due to the inability of the depth sensor to capture the complete point cloud of the target workpiece. SUMMARY

[0005] In view of the problems of low efficiency and poor robustness of the existing pose estimation method, the present application provides a multi-target pose estimation and sorting method based on optimized template, which quickly solves the pose matrix of the target workpiece relative to the robot through optimized template, and then realizes accurate grabbing of multiple categories.

[0006] The optimized template method proposed by the present application forms a new template by filtering out the redundant points in the template, thereby improving the accuracy and speed of registration. The pose estimation method based on 3D vision proposed by the present application is a new key technology for realizing efficient and accurate estimation of workpiece pose and further realizing intelligent and autonomous sorting, which has very important significance for the transformation of traditional industry.

[0007] The technical solution adopted by the present application to achieve the above-mentioned purpose is: a multi-target pose estimation and sorting method based on optimized template, comprising the following steps:

[0008] Obtain the template point cloud of multiple workpieces, and pre-process to construct the point pair features of the template point cloud of each workpiece to form a template;

[0009] acquire a scene point cloud using a camera, perform plane segmentation on the scene point cloud using a RANSAC algorithm to remove the background, and sparsify the scene point cloud containing the target;

[0010] perform clustering segmentation on the sparsified scene point cloud to segment multiple targets;

[0011] analyze the geometric features of each cluster point cloud using a PCA algorithm, classify point clouds with the same features into the same category of point clouds, and calculate the point pair normal vector features of the point clouds in the same category;

[0012] perform coarse registration on the point pair normal vector features and the point pair features of the template point cloud, and obtain the pose with the highest number of votes as the coarse registration pose from multiple poses generated by the coarse registration result;

[0013] perform ICP fine registration based on the coarse registration pose to obtain a transformation matrix of the target relative to the depth camera;

[0014] obtain a pose matrix of the target relative to the robot base according to the transformation matrix of the target relative to the depth camera, a conversion matrix of the depth camera relative to the end of the robot arm obtained through calibration, and a current robot end pose matrix;

[0015] send the pose matrix to a robot control system to control the robot to sort the target workpieces in the scene one by one.

[0016] The preprocessing is specifically:

[0017] For each type of workpiece template point cloud, filter out the occluded part and only keep the upper quarter of the template point cloud to form a new template point cloud.

[0018] The point pair features of the template point cloud of each type of workpiece include the following steps:

[0019] For the new template point cloud, two points p1 and p2 are randomly selected and the normal vectors n1 and n2 of the local plane where they are located and the Euclidean distance ||m|| between them are calculated to construct the point pair feature F;

[0020] F=(||m||, ∠(n1,m), ∠(n2,m), ∠(n1,n2))

[0021] Where m=P2-P1, m represents the vector between p1 and p2, containing the relative position and direction information of the two points, ∠(n1,m) is the angle between the normal vector n1 and the point pair vector m, ∠(n2,m) is the angle between the normal vector n2 and the point pair vector m, and ∠(n1,n2) is the angle between the two normal vectors n1 and n2.

[0022] The step of using the PCA algorithm to analyze the geometric features of each clustered point cloud, grouping point clouds with the same features into the same category, and analyzing the point pair normal vector features of the same category of point cloud computing scenarios includes the following steps:

[0023] 1) For each clustered point cloud, the data matrix is ​​X:

[0024]

[0025] Each row represents a point, and each column represents a dimension. This represents the average value for each dimension;

[0026] Central data matrix of point cloud It can be obtained through the following formula:

[0027]

[0028] 2) Calculate the covariance matrix and perform eigenvalue decomposition:

[0029]

[0030] Gv i =γ i v i

[0031] Where G represents the covariance matrix, for transpose; v i =(v1,v2,v3,...,v d ) is the eigenvector of the covariance matrix, γ i =(γ1,γ2,γ3,...,γ d ) is the corresponding eigenvalue;

[0032] 3) Sort the eigenvalues ​​from largest to smallest, and select the first k eigenvalues ​​as principal components:

[0033] {v1,v2,...,v k}=f spc (v1,v2,...,v i ,...,v d )

[0034] Among them, f spc (·) is the selection function for PCA;

[0035] The PCA eigenvalue ratio is calculated using the following formula:

[0036]

[0037] Among them, P ri(i∈0,1,...,k) is the ratio of eigenvalues ​​in PCA;

[0038] Iterate through the feature ratios of each PCA. When the feature ratio of the current PCA is closest to a certain target category threshold, the cluster point cloud to which the feature ratio of the current PCA belongs is a certain target category.

[0039] The point-pair normal vector features for the same category of point cloud computing scenarios include the following steps:

[0040] For each cluster of point clouds of the same category, it is divided into multiple local regions;

[0041] For each local region, an optimal plane is fitted using the least squares plane fitting method;

[0042] For each local region, construct the normal vector of the optimal plane. The set of normal vectors of multiple planes is the point-pair normal vector feature of the current clustered scene point cloud.

[0043] The coarse registration of the point-to-point normal vector features with the point-to-point features of the model point cloud used as a template includes the following steps:

[0044] First, select a pair of points s from the scene point cloud. r ,s i And select s r As a reference point, the corresponding point pair m is found in the model point cloud. r ,m i And select m r With s r correspond;

[0045] The point m in the model point cloud is transformed using the transformation matrix. r normal vector N m Align with the X-axis in space, and simultaneously move point s from the scene point cloud. r normal vector N s Also align to the X-axis to ensure that the poses of point pairs are compared in the same coordinate system;

[0046] After aligning the normal vectors, s r ,m r The translation is denoted as t, and the translation amount is denoted as t.

[0047] s r ,s i The direction vector of the connecting line and m r ,m iThe angle between the direction vectors of the connecting lines is α, which determines the rotation relationship between the scene point cloud and the model point cloud. The transformation relationship between the scene point cloud and the model point cloud is determined by the rotation angle α and the corresponding translation amount t, including the rotation matrix R and the translation vector T, thus forming a complete transformation matrix and realizing coarse registration of the point clouds.

[0048] A multi-target pose estimation and sorting method based on optimized templates includes:

[0049] The template construction module is used to acquire template point clouds of various workpieces, perform preprocessing, and construct point-pair features of the template point cloud for each workpiece to form a template.

[0050] The target segmentation module is used to acquire scene point clouds using a camera, perform planar segmentation of the scene point clouds using the RANSAC algorithm to remove the background, and sparsify the scene point clouds containing targets; and perform clustering segmentation on the sparsified scene point clouds to segment multiple targets.

[0051] The pose registration module is used to analyze the geometric features of each clustered point cloud using the PCA algorithm, grouping point clouds with the same features into the same category of point clouds, and calculating the point-to-point normal vector features of the same category of point clouds in the scene; coarse registration is performed between the point-to-point normal vector features and the point-to-point features of the template point cloud; for multiple poses generated by the coarse registration result, the pose with the highest number of votes is selected as the coarse registration pose; and fine ICP registration is performed based on the coarse registration pose to obtain the transformation matrix of the target relative to the depth camera.

[0052] The pose acquisition module is used to obtain the pose matrix of the target relative to the robot base based on the transformation matrix of the target relative to the depth camera, the transformation matrix of the depth camera relative to the end effector of the robotic arm obtained through calibration, and the current pose matrix of the robot end effector.

[0053] The sorting control module is used to send the pose matrix to the robot control system to control the robot to sort the target workpieces in the scenario in sequence.

[0054] A multi-target pose estimation and sorting device based on an optimized template includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement the multi-target pose estimation and sorting method based on an optimized template when the computer program is executed.

[0055] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the aforementioned multi-target pose estimation and sorting method based on an optimized template.

[0056] The present invention has the following beneficial effects and advantages:

[0057] 1) The method proposed in this invention can accurately obtain the pose of the target workpiece in a scattered state with only one shot, and can identify and distinguish different types of target objects.

[0058] 2) The method proposed in this invention can greatly improve registration efficiency while improving accuracy by optimizing the template.

[0059] 3) The hardware sensors used in the method proposed in this invention are low in cost but do not lose accuracy. They are easy to operate and implement, highly portable, and have high solution efficiency.

[0060] 4) The method proposed in this invention has good practical effects and can be integrated into a complete system. It is suitable for industrial applications of robot sorting systems. At the same time, by reserving interfaces, it is convenient to upgrade and improve the system in the future. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the 3D vision sorting system of the present invention.

[0062] Figure 2a This is a schematic diagram of the scene point cloud obtained by the depth sensor in this invention. Figure 1 .

[0063] Figure 2b This is a schematic diagram of the scene point cloud obtained by the depth sensor in this invention.

[0064] Figure 3 This is a schematic diagram of the point cloud template used for registration in this invention; wherein, (a) is an unprocessed point cloud template; (b) is a sparse point cloud template; and (c) is an optimized point cloud template.

[0065] Figure 4 This is a schematic diagram of the PPF registration process in this invention.

[0066] Figure 5 This is a flowchart of pose estimation in this invention.

[0067] Figure 6 This is a schematic diagram illustrating the coordinate systems and their corresponding transformation relationships in the hand-eye calibration of this invention.

[0068] Figure 7 This is a flowchart illustrating the visualization process for single-category target pose estimation in this invention.

[0069] Figure 8 This is a flowchart illustrating the visualization process for multi-category target pose estimation in this invention.

[0070] Figure 9 This is a flowchart illustrating the execution process of the visual sorting system of the present invention. Detailed Implementation

[0071] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0072] This invention relates to key technologies for the identification and detection of various static workpieces in industrial production, 6-Dimensional (6D) pose estimation, and target sorting, including hand-eye calibration, target measurement, and grasping and sorting applications. The method is characterized by employing a depth camera, unaffected by lighting conditions. Based on the point cloud characteristics of the actual scene, the target point cloud template is optimized. Through sparsification and geometric cutting, a point cloud template suitable for practical applications is obtained, thereby achieving point cloud registration. The specific steps include: importing the Computer Aided Design (CAD) model of the target workpiece and creating a model point cloud library; optimizing the point cloud template; acquiring the scene point cloud of the target workpiece using a depth camera; extracting model point cloud and scene features; loading the scene point cloud; removing scene background information using the Random Sample Consensus (RANSAC) algorithm (after background removal, only the target workpiece point cloud remains); sparsifying the scene point cloud after background removal; segmenting the sparsified scene point cloud by clustering, analyzing the point cloud features in each cluster using Principal Component Analysis (PCA), classifying point clouds with the same features into the same category, and obtaining the point cloud information of each object in each category; further using the Point Pair Feature (PPF) voting algorithm to perform coarse registration of the object point cloud and model point cloud to obtain pose clustering; based on the coarse registration, using the Iterative Closest Point (ICP) algorithm to perform fine registration of the object point cloud and template. The advantage of this method is that it can identify and distinguish all target objects in a scattered state with a single shot, and obtain the pose of all targets. Based on the estimated pose, the robot is controlled to perform sorting in sequence, resulting in high work efficiency.

[0073] like Figure 1 , Figure 5 As shown, the present invention includes the following steps:

[0074] Step 1: Perform hand-eye calibration to obtain the transformation matrix of the depth camera relative to the end effector of the robotic arm.

[0075] Step 2: Load the model point cloud and preprocess it. Calculate the diameter of the model point cloud and set the sampling radius, normalization radius, and voxel mesh size parameters according to the distance diameter.

[0076] Step 3: Optimize the model point cloud to create a new model point cloud file. Calculate the normal vectors of the model surface and store the model point pairs with normal vectors in a hash table.

[0077] Step 4: Use a depth camera to capture and store the scene point cloud.

[0078] Step 5: Acquire scene point cloud information using a depth camera, instantiate a class, collect data, and write a custom function according to the required point cloud format. Extract model point cloud and scene features, process model point pairs and scene point pairs, and use point pair features to establish a description of the relative position and direction of two directed points. Specifically, arbitrarily select two points p1 and p2 and calculate their corresponding normal vectors n1 and n2, as well as the Euclidean distance between them, to construct point pair features.

[0079] Step 6: Use the RANSAC algorithm to perform planar segmentation on the scene point cloud and remove the largest plane information. By manually setting a threshold, points whose distance to the fitted plane exceeds the threshold are considered invalid data. Randomly fit multiple planes and select the plane with the fewest invalid data as the fitted result.

[0080] Step 7: Based on the characteristics of the obtained point cloud data, construct a 3D voxel mesh, forming multiple tiny cubes to reduce the number of samples in the point cloud dataset, thereby improving efficiency. Replace the points within the cube with the centroid of the small cube to sparsify the dense point cloud data.

[0081] Step 8: Cluster and segment the sparsified point cloud. If the entire scene point cloud is used to register with the model point cloud, there may be duplicate or incorrect pose information. This situation can be avoided by separating each target point cloud in the entire scene and processing it separately. Each registration only processes one target point cloud.

[0082] Step 9: For objects with different shapes, matching with the corresponding template is required. Therefore, it is necessary to distinguish the shape category of each object. The PCA algorithm is used to analyze the features of the point clouds in each cluster, placing point clouds with similar features into the same category to facilitate subsequent matching using the corresponding template.

[0083] Step 10: Normal vectors are an important feature of point clouds. Calculate the point-to-point normal vector features in the scene to achieve accurate normal vector estimation.

[0084] Step 11: For each type of point cloud, perform coarse registration between the model point cloud and the scene point cloud. Use any point in the scene point cloud as a reference point, assuming it's on the surface of an object. If this assumption is correct, there will also be a corresponding point on the model. Register these two reference points by first aligning their positions and normal vectors simultaneously. Then, use a transformation matrix to move the model's feature point pair to the origin in a new space, aligning its normal vector with the X-axis of that space. Similarly, perform a similar operation on the feature point pairs of the scene point cloud.

[0085] Step 12: Perform pose estimation and use a voting strategy to vote. The pose with the most votes is regarded as the pose of PPF coarse registration, which provides good initial data for subsequent fine registration.

[0086] Step 13: Perform ICP fine registration, setting a maximum of 100 iterations. A greedy algorithm optimization strategy is used to perform iterative nearest neighbor matching for fine registration. Starting from Step 10, each processing step yields a pose matrix for a target point cloud. All target point clouds after automatic cyclic segmentation are sequentially registered to obtain their corresponding pose matrices. These matrices are then appended to obtain the final pose matrices for all targets in the scene.

[0087] In summary, the pose matrix of the target object relative to the camera has been obtained. Combined with the pose matrix of the camera relative to the robot obtained through hand-eye calibration before this step, the 6D pose estimation result of the target relative to the robot has been completed.

[0088] Example:

[0089] Step 1: Perform hand-eye calibration to obtain the transformation matrix between the depth camera and the robotic arm end effector, which is used to calculate the pose matrix of the target workpiece relative to the robot. The calibration process involves the following coordinate systems and transformation relationships: robotic arm base coordinate system: base; robotic arm end effector coordinate system: end; RGB camera coordinate system: rgb; calibration board coordinate system: board; depth camera coordinate system: depth.

[0090] The transformation relationships between the above coordinate systems are as follows:

[0091]

[0092] in, This is the conversion matrix between the RGB camera and the calibration board; This is the transformation matrix between the base and the end; This is the transformation matrix between the calibration plate and the base; This is the conversion matrix between the RGB camera and the endpoint;

[0093] The registration process yields a transformation matrix between the scene point cloud and the depth camera, while the calibration process yields a transformation matrix between the RGB camera and the end effector. Therefore, this matrix cannot be used directly and needs to be converted into a transformation matrix between the depth camera and the end effector using the following formula:

[0094]

[0095] This is the conversion matrix between the depth camera and the RGB camera; The transformation matrix between the depth camera and the end effector is the final hand-eye matrix used.

[0096] The calibration process is as follows: Figure 6 As shown, after mounting the depth camera to the end effector of the robotic arm, hand-eye calibration is performed using a calibration board. Approximately twenty sets of data are captured, with images occupying one-third to two-thirds of the entire frame. Intersections are kept sharp, poses are varied, and the field of view is covered as much as possible. After hand-eye calibration, the camera is used to capture scene point clouds to obtain scene data information.

[0097] Step 2: Load the model point cloud into memory, downsample the input point cloud, calculate the size of the sampled point cloud, and use the point cloud size and set the downsampling rate to sparsify the point cloud.

[0098] Step 3: Optimize the sparsified point cloud model from Step 2. Extensive experiments revealed that the target point cloud captured by the depth sensor is incomplete. The depth camera, placed at the end of the robotic arm, captures the scene point cloud from a top-down view, resulting in a target point cloud consisting only of a very thin layer on the upper surface and a very small amount of point cloud on the sides. (Refer to...) Figure 2a , Figure 2b As shown, the target point cloud in the scene is not entirely the same as the model point cloud. Compared with the target point cloud captured by the depth sensor, the model point cloud contains a large number of useless points. These redundant points not only affect the accuracy of registration but also greatly reduce the efficiency of registration. Therefore, these redundant points need to be filtered out. For a typical workpiece, 75% of its lower half is filtered out, retaining only the upper 25% of the model to form a new point cloud model, thereby improving the efficiency and accuracy of subsequent registration. Figure 3 As shown. For a new model point cloud, two points p1 and p2 are arbitrarily selected, and the normal vectors n1 and n2 of their local planes and the Euclidean distance ||m|| between them are calculated to construct the point pair feature F.

[0099] F=(||m||,∠(n1,m),∠(n2,m),∠(n1,n2))

[0100] Where m = P2 - P1 represents the vector between p1 and p2, which contains the relative position and direction information of the two points. ∠(n1,m) is the angle between the normal vector n1 and the point pair vector m. ∠(n2,m) is the angle between the normal vector n2 and the point pair vector m. ∠(n1,n2) is the angle between the two normal vectors n1 and n2.

[0101] Step 4: Use a depth camera to capture and store the scene point cloud.

[0102] Step 5: Remove planes from the scene point cloud captured in Step 4. Using the RANSAC algorithm, set a threshold parameter and determine that points whose distance to the same fitted plane exceeds the threshold are considered invalid data. After fitting multiple planes, select the plane with the fewest invalid data as the final fitted result, which is the processed scene point cloud.

[0103] Step 6: Perform sparse sampling on the scene point cloud processed in Step 5. Set a cubic mesh with a volume of 0.001 cubic meters and use its centroid to replace the point cloud data in it to achieve the purpose of sparsification, that is, to make the template point cloud sparser by 1000 times.

[0104] Step 7: Perform clustering segmentation on the sparsified scene point cloud from Step 6, dividing the complete scene target point cloud into multiple individual target point clouds. Set the clustering tolerance to 0.01 meters; points exceeding this distance will be considered not to belong to the same cluster. The maximum number of points in a cluster is 10,000, and the minimum number of points in a cluster is 100. After clustering, the entire scene point cloud is divided into many individual target point clouds, referred to as clustered point clouds.

[0105] Step 8: Based on the clustering results from Step 7, calculate the geometric features of each cluster of point clouds. Point clouds with the same features are grouped into the same category, and registration is performed using a unified template corresponding to that category. For objects with different shapes, PCA can be used to calculate the eigenvalue ratio of their point clouds to distinguish point cloud clusters of different shapes. The formula is expressed as follows:

[0106] First, the data needs to be centralized and a data matrix needs to be constructed to eliminate errors between different dimensions. The point cloud data matrix is ​​X:

[0107]

[0108]

[0109] Each row represents a point, and each column represents a dimension. This represents the average value of each dimension. Its core data matrix... It is obtained from the following formula:

[0110]

[0111] Calculate the covariance matrix and perform eigenvalue decomposition:

[0112]

[0113] G represents the covariance matrix. for The transpose of .

[0114] Eigenvalue decomposition is performed by the following transformation:

[0115] Gv i =γ i v i

[0116] Where v i =(v1,v2,v3,...,v d ) is the eigenvector of the covariance matrix, γ i =(γ1,γ2,γ3,...,γ d ) is the corresponding eigenvalue.

[0117] Finally, principal components are selected and eigenvalue ratios are calculated. Principal components are chosen based on the magnitude of the eigenvalues. Larger eigenvalues ​​indicate greater dispersion of the data in the corresponding direction. The eigenvalues ​​are sorted from largest to smallest, and the top K eigenvalues ​​are selected as principal components.

[0118] {v1,v2,...,v k}=f spc (v1,v2,...,v i ,...,v d )

[0119] Where f spc (·) is the selection function for PCA. After selecting the first K eigenvalues, the PCA eigenvalue ratio is calculated using the following formula:

[0120]

[0121] Where P ri (i∈0,1,...,k) is the ratio of eigenvalues ​​in PCA.

[0122] Extensive testing revealed that different geometries exhibit different PCA eigenvalue ratios. For this invention, the difference in PCA second eigenvalue ratios between point clouds of tire-shaped and cube-shaped workpieces captured using a depth camera is particularly pronounced. For example, the second eigenvalue ratio of the tire-shaped workpiece is significantly different in P... r2 The value is between 0 and 20, while the P value of the cube workpiece is... r2 It's between 200 and 600. Therefore, you can set P... r2 A threshold is used to determine the point cloud of the target workpiece, and the clustered point cloud of the target workpiece is classified into groups, with P... r2 Values ​​within the specified range are grouped into the same category.

[0123] Step 9: Based on the classification results of Step 8, calculate the point-pair normal vectors for point clouds of the same category. Calculating the normal vectors of point pairs in the scene involves dividing the scene point cloud of each cluster into multiple local regions. Each local region contains a set of mutually close points. For each local region, an optimal plane is fitted using the least squares plane fitting method. Then, the normal vectors of these planes are taken; the sum of these plane normal vectors is the point-pair normal vector feature of the point cloud. A K-dimension tree (KD-tree) is constructed to accelerate the nearest neighbor search. A search radius is set to determine which neighboring points to calculate the normal vectors on, and the final calculation results are stored in the point cloud object. The normal vector estimation method adopted transforms the points in the neighborhood into a local surface fitting method, that is, an approximate problem of estimating the normal of a tangent plane on the surface. This is transformed into a least squares plane fitting estimation problem, which is simple in principle and highly efficient, and also performs well on point cloud data in complex scenes. The plane equation can be described as:

[0124] Ax + By + Cz - D = 0

[0125] N = [A, B, C] T

[0126] Where N is the normal vector of the plane, D is the distance from the origin to the plane, and p is any point in the neighborhood. i (x i ,y i ,z i The distance from the plane is:

[0127] d i =|Ax i +By i +Cz i -D|

[0128] To extract local geometric features from a point cloud, optimal plane fitting is required for the local data points. The least squares plane fitting method is used to solve for the plane parameters A, B, C, and D that minimize the sum of squared distances F from all neighborhood points to the plane, thus obtaining an optimally fitted plane. These parameters not only define the plane's normal vector [A, B, C] but also determine the plane's position D, making the plane best suited to the neighborhood point cloud data.

[0129]

[0130] To find the solution that minimizes F, we construct the Lagrangian function L(A,B,C,D,λ):

[0131]

[0132] Where λ is a Lagrange multiplier used to introduce constraints.

[0133] Taking the partial derivative of the Lagrange function, we can obtain A, B, C, and D that minimize F.

[0134] After obtaining the normal vector, calculate the corresponding point pair features of the scene point cloud.

[0135] Step 10: Based on the results of Steps 3 and 9, perform PPF coarse registration. First, select a pair of points s from the scene point cloud. r ,s i And select s r As a reference point, the corresponding point pair m is found in the model point cloud. r ,m i And select m r With s r Corresponding. Next, the point m in the model is transformed using a transformation matrix. r normal vector N m Align with the X-axis of space, and simultaneously align point s in the scene. r normal vector N s Also align to the X-axis to ensure the pose of point pairs can be compared in the same coordinate system. After aligning the normal vectors, s r ,m r The point is translated to the origin of the spatial coordinate system, and the translation amount is denoted as t. The main difference between scene point pairs and model point pairs lies in the rotation angle α (s). r ,s i The direction vector of the connecting line and m r ,m i (The angle between the direction vectors of the connecting lines). By calculating this rotation angle, the rotational relationship between the scene point cloud and the model point cloud can be determined. Finally, using this rotation angle α and the corresponding translation t, the complete transformation relationship between the scene point cloud and the model point cloud can be determined, including the rotation matrix R and the translation vector T, thus forming a complete transformation matrix and achieving coarse registration of the point clouds. Figure 4 As shown.

[0136] Step 11: Because multiple similar poses may be generated during coarse registration, to further improve the accuracy of registration, a 3D voting accumulator needs to be constructed based on the coarse registration results from Step 10. By voting on all matching point pairs, the pose with the most consistency can be effectively selected. Finally, the pose with the highest number of votes in the accumulator is considered the optimal preliminary pose estimate of the target object. Figure 7 , Figure 8 As shown.

[0137] Step 12: Based on the results generated in Step 11, the ICP algorithm is used to further register the model point cloud and the scene point cloud. Although the initial pose can roughly align the model and scene point clouds, there may still be some errors. Therefore, ICP can be used to reduce the distance between them iteratively. The ICP algorithm gradually reduces the error by repeatedly matching the nearest point pairs and adjusting the model's pose until a set threshold is reached. This process can accurately align the model and scene point clouds, ultimately obtaining the accurate transformation matrix of the target relative to the depth camera.

[0138] Step 13: After obtaining the final transformation matrix in step 12, the hand-eye matrix obtained in step 1 can be used. And read the current robot end effector pose matrix Find the pose matrix of the target workpiece relative to the robot base.

[0139]

[0140] By sending the pose matrices of all target workpieces to the robot control system, the robot can sequentially sort the target workpieces within the scene, thus realizing a complete robotic sorting system. For example... Figure 9 As shown.

Claims

1. A multi-target pose estimation and sorting method based on optimized templates, characterized in that, Includes the following steps: Acquire template point clouds of various workpieces, perform preprocessing, and construct point-pair features of the template point cloud for each workpiece to form a template; The scene point cloud is acquired using a camera, and the RANSAC algorithm is used to perform planar segmentation of the scene point cloud to remove the background. And the point cloud containing the target is sparsified; Clustering and segmentation are performed on the sparsed scene point cloud to separate multiple targets; The PCA algorithm is used to analyze the geometric features of each clustered point cloud, and point clouds with the same features are classified into the same category of point clouds. The point normal vector features of the cloud computing scene are then analyzed. Perform coarse registration between the point-to-point normal vector features and the point-to-point features of the template point cloud; For multiple poses generated by coarse registration, the pose with the highest number of votes is selected as the coarse registration pose. ICP fine registration is performed based on the pose of coarse registration to obtain the transformation matrix of the target relative to the depth camera; Based on the transformation matrix of the target relative to the depth camera, the transformation matrix of the depth camera relative to the end effector of the robotic arm obtained through calibration, and the current pose matrix of the robot end effector, the pose matrix of the target relative to the robot base is obtained. The pose matrix is ​​sent to the robot control system to control the robot to sort the target workpieces in the scene in sequence.

2. The method for multi-target pose estimation and sorting based on optimized templates according to claim 1, characterized in that, The preprocessing process specifically includes: For each workpiece's template point cloud, the occluded parts are filtered out, and only the upper quarter of the template point cloud is retained to form a new template point cloud.

3. The multi-target pose estimation and sorting method based on optimized templates according to claim 1, characterized in that, The construction of the point pair features of the template point cloud for each workpiece includes the following steps: For a new template point cloud, arbitrarily select two points p1 and p2 and calculate the normal vectors n1 and n2 of their local planes and the Euclidean distance ||m|| between them, thereby constructing the point pair feature F; F=(||m||,∠(n1,m),∠(n2,m),∠(n1,n2)) Where m = P2 - P1, m represents the vector between p1 and p2, containing the relative position and direction information of the two points, ∠(n1,m) is the angle between the normal vector n1 and the point-pair vector m, ∠(n2,m) is the angle between the normal vector n2 and the point-pair vector m, and ∠(n1,n2) is the angle between the two normal vectors n1 and n2.

4. The multi-target pose estimation and sorting method based on optimized templates according to claim 1, characterized in that, The step of using the PCA algorithm to analyze the geometric features of each clustered point cloud, grouping point clouds with the same features into the same category, and analyzing the point pair normal vector features of the same category of point cloud computing scenarios includes the following steps: 1) For each clustered point cloud, the data matrix is ​​X: Each row represents a point, and each column represents a dimension. This represents the average value for each dimension; Central data matrix of point cloud It can be obtained through the following formula: 2) Calculate the covariance matrix and perform eigenvalue decomposition: Where G represents the covariance matrix, for transpose; v i =(v1,v2,v3,...,v d ) is the eigenvector of the covariance matrix, γ i =(γ1,γ2,γ3,...,γ d ) is the corresponding eigenvalue; 3) Sort the eigenvalues ​​from largest to smallest, and select the first k eigenvalues ​​as principal components: {v1,v2,...,v k }=f spc (v1,v2,...,v i ,...,v d ) Among them, f spc (·) is the selection function for PCA; The PCA eigenvalue ratio is calculated using the following formula: Among them, P ri (i∈0,1,...,k) is the ratio of eigenvalues ​​in PCA; Iterate through the feature ratios of each PCA. When the feature ratio of the current PCA is closest to a certain target category threshold, the cluster point cloud to which the feature ratio of the current PCA belongs is a certain target category.

5. The multi-target pose estimation and sorting method based on optimized templates according to claim 1, characterized in that, The point-pair normal vector features for the same category of point cloud computing scenarios include the following steps: For each cluster of point clouds of the same category, it is divided into multiple local regions; For each local region, an optimal plane is fitted using the least squares plane fitting method; For each local region, construct the normal vector of the optimal plane. The set of normal vectors of multiple planes is the point-pair normal vector feature of the current clustered scene point cloud.

6. The multi-target pose estimation and sorting method based on optimized templates according to claim 1, characterized in that, The coarse registration of the point-to-point normal vector features with the point-to-point features of the model point cloud used as a template includes the following steps: First, select a pair of points s from the scene point cloud. r ,s i And select s r As a reference point, the corresponding point pair m is found in the model point cloud. r ,m i And select m r With s r correspond; The point m in the model point cloud is transformed using the transformation matrix. r normal vector N m Align with the X-axis in space, and simultaneously move point s from the scene point cloud. r normal vector N s Also align to the X-axis to ensure that the poses of point pairs are compared in the same coordinate system; After aligning the normal vectors, s r ,m r The translation is denoted as t, and the translation amount is denoted as t. s r ,s i The direction vector of the connecting line and m r ,m i The angle between the direction vectors of the connecting lines is α, which determines the rotation relationship between the scene point cloud and the model point cloud. The transformation relationship between the scene point cloud and the model point cloud is determined by the rotation angle α and the corresponding translation amount t, including the rotation matrix R and the translation vector T, thus forming a complete transformation matrix and realizing coarse registration of the point clouds.

7. A multi-target pose estimation and sorting method based on optimized templates, characterized in that, include: The template construction module is used to acquire template point clouds of various workpieces, perform preprocessing, and construct point-pair features of the template point cloud for each workpiece to form a template. The target segmentation module is used to acquire scene point clouds using a camera, perform planar segmentation of the scene point clouds using the RANSAC algorithm to remove the background, and sparsify the scene point clouds containing targets. Clustering and segmentation are performed on the sparsed scene point cloud to separate multiple targets; The pose registration module is used to analyze the geometric features of each clustered point cloud using the PCA algorithm, classify point clouds with the same features into the same category of point clouds, and perform coarse registration between the point cloud normal vector features of the same category of point clouds and the point cloud normal vector features of the template point cloud. For the multiple poses generated by the coarse registration result, the pose with the highest number of votes is selected as the coarse registration pose; ICP fine registration is performed based on the coarse registration pose to obtain the transformation matrix of the target relative to the depth camera. The pose acquisition module is used to obtain the pose matrix of the target relative to the robot base based on the transformation matrix of the target relative to the depth camera, the transformation matrix of the depth camera relative to the end effector of the robotic arm obtained through calibration, and the current pose matrix of the robot end effector. The sorting control module is used to send the pose matrix to the robot control system to control the robot to sort the target workpieces in the scenario in sequence.

8. A multi-target pose estimation and sorting device based on an optimized template, characterized in that, It includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement, when executing the computer program, a multi-target pose estimation and sorting method based on an optimized template as described in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements a multi-target pose estimation and sorting method based on an optimized template as described in any one of claims 1-6.

Images