Pose estimation method and device for robot flexible operation, equipment and medium

Abstract

The application discloses a pose estimation method and device for robot flexible operation, equipment and medium, and relates to the field of object pose estimation. A reference point cloud with a known pose and a scene observation point cloud to be estimated is obtained, target separation and translation space decoupling are performed on the scene observation point cloud, a target point cloud region is extracted, a translation constraint vector is generated based on geometric features of the target point cloud region, a six-degree-of-freedom registration space is reduced to a three-degree-of-freedom rotation search space based on the translation constraint vector, a candidate rotation set is constructed, the reference point cloud, the translation constraint vector and the candidate rotation set are mapped into a high-dimensional tensor, rigid body transformation and fitness evaluation are synchronously executed by using parallel computing, a fitness result is obtained, a global coarse pose is screened according to the fitness result, and iterative optimization is performed with the global coarse pose as an initial constraint until a convergence condition is met, so that target six-degree-of-freedom pose data is determined. The method improves the operation efficiency, real-time performance and pose solution accuracy of point cloud registration in an unordered scene.

Description

Technical Field

[0001] This application relates to the field of object pose estimation, and in particular to a pose estimation method, apparatus, device and medium for robot flexible operation. Background Technology

[0002] In the field of industrial automation, especially in scenarios involving embodied intelligence, disordered industrial grasping, and flexible assembly, target parts (such as mechanical components, electronic components, or irregularly shaped workpieces) are usually present in a highly random and scattered stacked state in bins or conveyor belts.

[0003] In existing technologies, 6D pose estimation schemes for industrial disordered grasping mainly fall into two categories. One category relies on local geometric feature descriptors such as FPFH and SHOT, combined with feature matching and the RANSAC algorithm to complete global point cloud registration; the other category is a multi-starting-point random ICP search strategy, which randomly generates initial poses in 6D space, iteratively performs nearest neighbor matching and rigid transformation to solve for the optimal pose result.

[0004] However, traditional geometric registration algorithms are prone to getting stuck in local optima and have unstable detection success rates; the global brute-force search mode has a huge computational load and high complexity, making it difficult to adapt to the requirements of real-time industrial operations. Summary of the Invention

[0005] This application provides a pose estimation method, apparatus, device, and medium for flexible robot operations, which solves the problems that traditional geometric registration algorithms are prone to getting trapped in local optima and have unstable detection success rates; and that the global brute-force search mode has a large computational load and high complexity, making it difficult to adapt to the requirements of real-time industrial operations.

[0006] In a first aspect, embodiments of this application provide a pose estimation method for flexible robot operations, including:

[0007] Obtain the reference point cloud with known pose and the scene observation point cloud with pose to be estimated;

[0008] The scene observation point cloud is subjected to target separation and translational spatial decoupling, the target point cloud region is extracted, and a translational constraint vector is generated based on the geometric features of the target point cloud region;

[0009] Based on the translation constraint vector, the six-degree-of-freedom registration space is reduced to a three-degree-of-freedom rotation search space, and a candidate rotation set is constructed.

[0010] The reference point cloud, the translation constraint vector and the candidate rotation set are mapped to high-dimensional tensors, and rigid body transformation and fitness evaluation are performed synchronously using parallel computing to obtain fitness results.

[0011] The global coarse pose is selected based on the fitness results, and the global coarse pose is used as the initial constraint for iterative optimization until the convergence condition is met, thereby determining the target six-degree-of-freedom pose data.

[0012] As an optional implementation, the step of performing target separation and translational spatial decoupling on the scene observation point cloud, extracting the target point cloud region, and generating a translational constraint vector based on the geometric features of the target point cloud region includes:

[0013] The target point cloud region is segmented from the scene observation point cloud using point cloud density background removal, Euclidean clustering, or region growing clustering algorithms.

[0014] Calculate the geometric center of the target point cloud region, where the geometric center is the coordinate mean or the minimum bounding box center;

[0015] The three-dimensional coordinates of the geometric center are extracted as translation constraint vectors, which are three-dimensional translation components that constrain the six-degree-of-freedom registration space.

[0016] As an optional implementation, the step of reducing the six-DOF registration space to a three-DOF rotation search space based on the translation constraint vector, and constructing a candidate rotation set, includes:

[0017] The Fibonacci grid sampling method is used to generate uniform sampling points of a three-dimensional rotation group and map them into a rotation matrix, or the rotation matrix is ​​generated by discretizing the Euler angle space according to a preset step size.

[0018] A candidate rotation set is constructed based on the rotation matrix, and the number of rotation matrices in the candidate rotation set is adjusted according to the sampling accuracy or the step size.

[0019] As an optional implementation, the step of mapping the reference point cloud, the translation constraint vector, and the candidate rotation set into a high-dimensional tensor, and simultaneously performing rigid body transformation and fitness evaluation using parallel computing to obtain fitness results includes:

[0020] The candidate rotation set is processed into a rotation tensor, and the reference point cloud is processed into a batched point cloud tensor that matches the dimension of the rotation tensor.

[0021] By utilizing the parallel matrix multiplication operator of the graphics processor, rigid body rotation transformations are performed in batches and combined with translation constraint vectors to generate candidate registration point clouds;

[0022] The bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud is calculated in parallel to obtain the fitness evaluation results.

[0023] As an optional implementation, the fitness result is the chamfer distance; the parallel calculation of the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud to obtain the fitness evaluation result includes:

[0024] Calculate the sum of the distances from each point in the candidate registration point cloud to the nearest point in the scene observation point cloud, and determine the forward distance term;

[0025] Calculate the sum of the distances from each point in the scene observation point cloud to the nearest point in the candidate registration point cloud, and determine the backward distance term;

[0026] The forward distance term and the backward distance term are summed to determine the chamfer distance corresponding to the candidate pose.

[0027] As an optional implementation, the step of filtering the global coarse pose based on the fitness results and iteratively optimizing the global coarse pose as the initial constraint until the convergence condition is met to determine the target six-DOF pose data includes:

[0028] Perform a parallel minimum indexing operation to extract the candidate rotation pose with the lowest fitness score as the global coarse pose;

[0029] Starting from the global coarse pose, the point-to-surface iterative nearest point algorithm is called to perform local convergence iteration until the preset convergence threshold is met, and the target six-degree-of-freedom pose data is determined.

[0030] As an optional implementation, the step of using the global coarse pose as the initial starting point and calling the point-to-surface iterative nearest point algorithm for local convergence iteration until a preset convergence threshold is met to determine the target six-DOF pose data includes:

[0031] Using the global coarse pose as the initial transformation matrix, a point-to-surface error measurement function is constructed between the candidate registration point cloud and the scene observation point cloud.

[0032] The error metric function is iteratively solved using an optimization algorithm to calculate the rigid body transformation update amount for the current iteration step;

[0033] If the rigid body transformation update amount is less than the preset pose change threshold and the number of iterations reaches the preset value, then the iteration stops and the currently accumulated transformation matrix is ​​used as the target six-degree-of-freedom pose data.

[0034] If the rigid body transformation update is greater than or equal to the preset pose change threshold, or the number of iterations has not reached the preset value, then the iterative optimization step continues.

[0035] Secondly, embodiments of this application provide a pose estimation device for flexible robot operations, comprising:

[0036] The acquisition module is used to acquire the reference point cloud with known pose and the scene observation point cloud with the pose to be estimated.

[0037] The generation module is used to perform target separation and translational spatial decoupling on the scene observation point cloud, extract the target point cloud region, and generate translational constraint vectors based on the geometric features of the target point cloud region;

[0038] The construction module is used to reduce the dimensionality of the six-degree-of-freedom registration space to a three-degree-of-freedom rotation search space based on the translation constraint vector, and construct a candidate rotation set;

[0039] The determination module is used to map the reference point cloud, the translation constraint vector and the candidate rotation set into a high-dimensional tensor, and use parallel computing to simultaneously perform rigid body transformation and fitness evaluation to obtain fitness results.

[0040] The determining module is further configured to filter the global coarse pose based on the fitness result, and iteratively optimize the global coarse pose as the initial constraint until the convergence condition is met, thereby determining the target six-degree-of-freedom pose data.

[0041] As an optional implementation, the pose estimation device for robot flexible operation further includes: a processing module, a calculation module, and an extraction module;

[0042] The processing module is also used to segment the target point cloud region from the scene observation point cloud using point cloud density background removal, Euclidean clustering or region growing clustering algorithms.

[0043] The calculation module is used to calculate the geometric center of the target point cloud region, where the geometric center is the coordinate mean or the minimum bounding box center.

[0044] The extraction module is used to extract the three-dimensional coordinates of the geometric center as a translation constraint vector, wherein the translation constraint vector is a three-dimensional translation component constraining the six-degree-of-freedom registration space.

[0045] As an optional implementation, the generation module is further configured to generate uniform sampling points of a three-dimensional rotation group using Fibonacci grid sampling and map them to a rotation matrix, or to generate a rotation matrix by discretizing the Euler angle space according to a preset step size.

[0046] The construction module is also used to construct a candidate rotation set based on the rotation matrix, wherein the number of rotation matrices in the candidate rotation set is adjusted according to the sampling accuracy or the offset length.

[0047] As an optional implementation, the processing module is further configured to process the candidate rotation set into a rotation tensor and process the reference point cloud into a batched point cloud tensor that matches the dimension of the rotation tensor.

[0048] The generation module is also used to use the parallel matrix multiplication operator of the graphics processor to perform rigid body rotation transformations in batches and combine them with translation constraint vectors to generate candidate registration point clouds.

[0049] The determination module is also used to calculate the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud in parallel, so as to obtain the fitness evaluation result.

[0050] As an optional implementation, the determining module is further configured to calculate the sum of the distances from each point in the candidate registration point cloud to the nearest point in the scene observation point cloud, and determine the forward distance term;

[0051] The determining module is further configured to calculate the sum of the nearest distances from each point in the scene observation point cloud to the candidate registration point cloud, and determine the backward distance term;

[0052] The determining module is further configured to sum the forward distance term and the backward distance term to determine the chamfer distance corresponding to the candidate pose.

[0053] As an optional implementation, the extraction module is also used to perform a parallel minimum indexing operation to extract the candidate rotation pose with the lowest fitness score as the global coarse pose.

[0054] The determining module is further configured to use the global coarse pose as the initial starting point, call the point-to-surface iterative nearest point algorithm to perform local convergence iteration until a preset convergence threshold is met, and determine the target six-degree-of-freedom pose data.

[0055] As an optional implementation, the construction module is further configured to use the global coarse pose as an initial transformation matrix to construct a point-to-surface error measurement function between the candidate registration point cloud and the scene observation point cloud;

[0056] The calculation module is also used to iteratively solve the error metric function using an optimization algorithm to calculate the rigid body transformation update amount of the current iteration step;

[0057] The determining module is further configured to stop the iteration if the rigid body transformation update amount is less than a preset pose change threshold and the number of iterations reaches a preset value, and use the currently accumulated transformation matrix as the target six-degree-of-freedom pose data.

[0058] The processing module is further configured to continue executing the iterative optimization steps if the rigid body transformation update amount is greater than or equal to a preset pose change threshold, or if the number of iterations has not reached a preset value.

[0059] Thirdly, embodiments of this application provide a pose estimation device for flexible robot operations, including: a receiver, a transmitter, a memory, and a processor;

[0060] Receiver, used to receive instructions and data;

[0061] A transmitter is used to send commands and data;

[0062] The memory stores computer-executed instructions;

[0063] The processor executes computer execution instructions stored in the memory, causing the processor to perform the first aspect and / or various possible implementations of the first aspect as described above.

[0064] Fourthly, embodiments of this application provide a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, are used to implement the first aspect and / or various possible implementations of the first aspect.

[0065] Fifthly, embodiments of this application provide a computer program product, including a computer program that, when executed by a processor, implements the first aspect and / or various possible implementations of the first aspect.

[0066] The pose estimation method for flexible robot operations provided in this application accurately divides the target point cloud region by separating the target and decoupling the translation space. It constructs translation constraint vectors based on geometric features, effectively reducing the dimensionality from 6D registration space to 3D rotation search space, and significantly compressing the search range for pose solving. Combined with high-dimensional tensor mapping and parallel computing, it simultaneously completes rigid body transformation and fitness evaluation, significantly improving computational efficiency and reducing the resource consumption of traditional successive traversal calculations. At the same time, it adopts a coarse-fine combination strategy of coarsely screening global coarse poses with candidate rotation sets and then iteratively optimizing and refining the poses. This avoids redundant calculations and invalid searches in disordered and complex scenarios, and ensures the stability of iterative convergence by using coarse poses as reliable initial constraints. Ultimately, it effectively reduces the overall computational load of 6D pose solving, significantly enhances the real-time response capability of point cloud registration in disordered working conditions, and comprehensively improves the accuracy, environmental adaptability, and operational robustness of target pose solving. Attached Figure Description

[0067] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0068] Figure 1 A flowchart illustrating the pose estimation method for flexible robot operations provided in this application. Figure 1 ;

[0069] Figure 2 A flowchart illustrating the pose estimation method for flexible robot operations provided in this application. Figure 2 ;

[0070] Figure 3 The diagrams show a comparison between traditional pose estimation methods and the pose estimation method of this application. (a) is a diagram of local optimal pose estimation, (b) is a diagram of parallel estimation of multiple candidate poses, and (c) is a diagram of global optimal pose determination.

[0071] Figure 4 A schematic diagram of the pose estimation device for flexible robot operation provided in this application;

[0072] Figure 5 This is a schematic diagram of the pose estimation device for flexible robot operations provided in this application.

[0073] The accompanying drawings illustrate specific embodiments of this application, which will be described in more detail below. These drawings and descriptions are not intended to limit the scope of the concept in any way, but rather to illustrate the concept of this application to those skilled in the art through reference to particular embodiments. Detailed Implementation

[0074] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application.

[0075] In practical applications of industrial automation, embodied intelligent operations, disordered industrial grasping, and flexible precision assembly have become core application directions. Production materials such as mechanical hardware parts, micro-electronic components, and irregularly shaped workpieces are generally distributed in the working environment such as material bins and conveyor belts in a disorderly and random manner, with the orientation, placement angle, and spatial position of the parts being completely uncontrollable.

[0076] In current industrial unordered grasping tasks, 6D pose estimation is the key to achieving intelligent grasping, and the mainstream implementation technologies mainly fall into two major approaches. The first approach is based on classic local geometric feature descriptors such as FPFH and SHOT, relying on point cloud feature extraction, cross-frame feature matching, and the RANSAC random sampling consensus algorithm to complete the global registration and pose calculation of the workpiece point cloud. The second approach adopts a multi-starting point random ICP iterative search scheme, which generates initial pose parameters randomly in batches within the complete 6D space, repeatedly performs nearest neighbor point matching and rigid matrix transformation iterative calculations, and finally selects the optimal workpiece pose data from multiple sets of results.

[0077] However, geometric registration algorithms based on local feature matching are highly susceptible to workpiece occlusion, surface homogenization, and stacking interference. The iterative convergence process is prone to getting stuck in local optima, resulting in insufficient pose detection accuracy and large fluctuations in job success rate. On the other hand, the random ICP global brute-force search method requires traversing a large area of ​​space, resulting in a huge overall computational load, high algorithm time complexity, and uncontrollable running delay, which cannot meet the high-speed, real-time, and continuous operation requirements of industrial production lines.

[0078] To address the aforementioned issues, this application provides a pose estimation method for flexible robot operations. Based on a known pose reference point cloud and scene observation point cloud, it first precisely segments the target point cloud through target separation and translational spatial decoupling. Then, it constructs translational constraint vectors based on the target's geometric features, achieving dimensionality reduction from a six-DOF registration space to a three-DOF rotation space, thus compressing the pose search range. Next, it constructs a rotation candidate set and combines it with the reference point cloud and translational constraint vectors to complete high-dimensional tensor mapping. Parallel computing is then used to simultaneously perform rigid body transformation and adaptation evaluation, rapidly filtering the global coarse pose. Finally, using the coarse pose as the initial condition, it iteratively refines the pose until convergence, outputting an accurate 6D target pose. This scheme integrates spatial dimensionality reduction, tensor parallel computation, and a two-stage coarse-fine optimization strategy, effectively reducing the computational overhead of solving the 6D pose of point clouds in disordered scenes, while balancing real-time registration efficiency, solution accuracy, and environmental robustness.

[0079] The technical solution of this application and how the technical solution of this application solves the above-mentioned technical problems are described in detail below with specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of this application will now be described with reference to the accompanying drawings.

[0080] Figure 1 A flowchart illustrating the pose estimation method for flexible robot operations provided in this application. Figure 1 The execution entity in this embodiment is, for example, a pose estimation system for flexible robotic operations. Figure 1 As shown, the method includes:

[0081] S101: Obtain the reference point cloud with known pose and the scene observation point cloud with the pose to be estimated.

[0082] Among them, the reference point cloud refers to the point cloud data obtained after downsampling the ideal 3D CAD model of the target part, and its 6D pose (translation + rotation) is known.

[0083] Scene observation point cloud refers to the industrial scene point cloud acquired by 3D vision sensors (such as LiDAR and structured light cameras), which includes scattered stacked target parts and background information, and the 6D pose of the target parts is the state to be estimated.

[0084] Specifically, firstly, a 3D CAD model of the target part is acquired, and a voxel downsampling algorithm is used to sparsify the model point cloud, preserving the core geometric features of the part while reducing the amount of data, to obtain a reference point cloud and record its known pose parameters; then, a 3D vision sensor is used to scan the target scene such as disordered grasping and flexible assembly, and 2.5D point cloud data of the scene is collected. After noise reduction preprocessing (such as Gaussian filtering), the scene observation point cloud containing the target part is obtained.

[0085] S102: Perform target separation and translation space decoupling on the scene observation point cloud, extract the target point cloud region, and generate translation constraint vector based on the geometric features of the target point cloud region.

[0086] Among them, target separation refers to removing background point clouds and interfering object point clouds from the cluttered scene observation point cloud, and retaining only the point cloud region corresponding to the target part (i.e., the target point cloud region).

[0087] Translation space decoupling refers to splitting the 6D pose search space (3D translation + 3D rotation) into independent translation and rotation spaces, and prioritizing the locking of translation parameters to achieve dimensionality reduction.

[0088] Translation constraint vector is the 3D translation parameter of the target part. ), used to fix the spatial position of parts and constrain the rotation search range.

[0089] Specifically, a point cloud preprocessing workflow with extremely low computing power is used to achieve target separation. First, a background culling algorithm is used to remove fixed background points (such as the point cloud of the inner wall of the grabbing bin) from the scene observation point cloud. Then, the Euclidean clustering segmentation algorithm is used to divide the remaining point cloud into multiple independent clusters based on the spatial distance characteristics of the point cloud. The target point cloud region is selected by combining the size and geometric features of the target part. Subsequently, the geometric centroid or bounding box center of the target point cloud region is calculated, and the coordinates of the center are directly used as the translation constraint vector to complete the decoupling of translation space and rotation space.

[0090] S103: Based on the translation constraint vector, the six-degree-of-freedom registration space is reduced to a three-degree-of-freedom rotation search space, and a candidate rotation set is constructed.

[0091] The six-DOF registration space refers to a space containing three translational degrees of freedom. and 3 rotational degrees of freedom The complete pose search space.

[0092] The three-degree-of-freedom rotational search space refers to the simplified space that only requires searching three rotational degrees of freedom after fixing the translation constraint vector.

[0093] The candidate rotation set refers to the set of multiple rotation matrices generated by sampling within the three-dimensional rotation group SO(3), which is used to cover all possible rotational orientations of the part.

[0094] Specifically, using the generated translation constraint vector as a fixed parameter, the original 6D pose registration space is directly reduced to a search space containing only 3 rotational degrees of freedom, completely avoiding the curse of dimensionality in 6D space search; a candidate rotation set is constructed using a high-density uniform sampling method, which can choose Fibonacci grid sampling or Euler angles. Discrete the matrix with equal step size to generate a set containing 500 to 2000 candidate rotation matrices. This ensures that the sampling density can cover all possible rotational orientations of the part, avoiding the omission of globally optimal rotational parameters.

[0095] S104: Map the reference point cloud, translation constraint vector, and candidate rotation set into a high-dimensional tensor, and use parallel computing to simultaneously perform rigid body transformation and fitness evaluation to obtain fitness results.

[0096] Among them, the high-dimensional tensor refers to the unified data structure formed by integrating multi-dimensional data such as the reference point cloud, translation constraint vector, and candidate rotation set, which is convenient for GPU parallel computing.

[0097] Rigid body transformation refers to transforming the reference point cloud to the spatial pose corresponding to the scene observation point cloud while keeping the geometry of the reference point cloud unchanged, combined with translation constraint vectors and candidate rotation matrices.

[0098] Fitness assessment refers to the degree of matching between the transformed reference point cloud and the target point cloud region. The higher the fitness score, the more accurate the pose matching.

[0099] Specifically, the baseline point cloud, translation constraint vectors, and candidate rotation sets are first mapped to a high-dimensional tensor to eliminate data format differences and adapt to GPU parallel computing. Then, using the GPU's parallel matrix multiplication operator, rigid body transformations are performed synchronously within the same computational graph, transforming the baseline point cloud into its corresponding candidate pose by combining the translation constraint vectors and each candidate rotation matrix. Simultaneously, the fitness scores (e.g., Chamfer Distance) of the transformed baseline and target point cloud regions are calculated in parallel for each candidate pose. Finally, the fitness results for all candidate rotation poses are output. Figure 3 As shown in (b), by generating M parallel candidate rotation matrices, a comprehensive coverage of rotational attitudes can be achieved, avoiding the trap of getting stuck in local optima due to a single initial value, as is the case with traditional methods.

[0100] S105: Filter the global coarse pose based on the fitness results, and iterate and optimize the global coarse pose as the initial constraint until the convergence condition is met, and determine the target six-degree-of-freedom pose data.

[0101] Among them, global coarse pose refers to the pose with the highest fitness score selected from all candidate rotation poses and combined with translation constraint vectors to form a complete 6D pose.

[0102] Iterative optimization refers to using the global coarse pose as the initial constraint and gradually adjusting the pose parameters through a geometric registration algorithm until the preset convergence condition is met.

[0103] The target six-degree-of-freedom pose data refers to the final output 6D pose (3D translation + 3D rotation) that meets the requirements of industrial precision operation.

[0104] Specifically, a parallel Argmin() operation is performed on the obtained fitness results to quickly extract the candidate rotation pose with the highest fitness score. Combined with the determined translation constraint vector, a global coarse pose is formed. Then, a high-precision geometric pose refinement module (such as the point-to-surface ICP algorithm and ray-tracing-based geometric optimization algorithm) is invoked to... The system performs local convergence iterations based on initial constraints, continuously adjusting rotation and translation parameters until the pose error between two adjacent iterations is less than a preset threshold (meeting the convergence condition). Finally, it outputs accurate target six-DOF pose data. Figure 3 As shown in (c), the parallel Argmin operation is the core of global optimum selection. It can filter out the globally optimal coarse pose from multiple generated candidate poses, completely breaking away from the traditional single ICP initialization, as shown in (c). Figure 3 As shown in (a), the existing method is prone to local minima of ICP and local optimal traps. Then, through post-precision refinement, the coarse pose is optimized into the final 6D pose that meets the requirements of industrial precision operation.

[0105] The pose estimation method for flexible robot operations provided in this application acquires a reference point cloud with known poses and a scene observation point cloud with the pose to be estimated. It performs target separation and translation space decoupling on the scene observation point cloud, extracts the target point cloud region, and generates translation constraint vectors based on the geometric features of the target point cloud region. Based on the translation constraint vectors, the six-degree-of-freedom registration space is reduced to a three-degree-of-freedom rotation search space, constructing a candidate rotation set. The reference point cloud, translation constraint vectors, and candidate rotation set are mapped to a high-dimensional tensor. Rigid body transformation and fitness evaluation are performed simultaneously using parallel computing to obtain fitness results. A global coarse pose is selected based on the fitness results, and iterative optimization is performed using the global coarse pose as the initial constraint until convergence conditions are met, thus determining the target six-degree-of-freedom pose data. This method significantly reduces the computational load and search range for 6D pose solving through point cloud target separation and decoupling, translation constraint dimensionality reduction registration space, high-dimensional tensor parallel computation, and coarse-fine combined iterative optimization. It also improves the computational efficiency, real-time performance, pose solving accuracy, and robustness of point cloud registration in disordered scenes.

[0106] Figure 2 A flowchart illustrating the pose estimation method for flexible robot operations provided in this application. Figure 2 ,like Figure 2 As shown, in this embodiment... Figure 1 Based on the embodiments, a pose estimation method for flexible robot operations is described in detail. The method includes:

[0107] S201: Obtain the reference point cloud with known pose and the scene observation point cloud with pose to be estimated.

[0108] Among them, steps S201 and S101.

[0109] S202: Use point cloud density background removal, Euclidean clustering, or region growing clustering algorithms to segment the target point cloud region from the scene observation point cloud.

[0110] Among them, point cloud density background removal refers to removing background point clouds (such as the inner wall of the material bin and the worktable surface) whose density is significantly lower than that of the target part based on the spatial density difference of the point cloud.

[0111] Euclidean clustering algorithm refers to the method of clustering points that are close to each other in space into a whole based on the Euclidean distance between points, so as to achieve point cloud segmentation.

[0112] Region growing clustering algorithms refer to starting from a seed point and gradually merging adjacent points that meet preset similarity conditions (such as consistent normal vectors and curvature) to form a complete point cloud cluster.

[0113] The target point cloud region refers to the set of point clouds separated from the scene observation point cloud that contains only the target part.

[0114] Specifically, a point cloud density background removal algorithm can be used to calculate the local density of the observed point cloud in the scene, set a density threshold, and remove background point clouds with densities below the threshold to initially screen out point cloud regions that may contain the target part. Subsequently, Euclidean clustering or region growing clustering algorithms are used for further segmentation. If the part surface is smooth and the geometric features are continuous, the region growing clustering algorithm is selected, starting from random seed points and merging adjacent points based on the similarity of point cloud normal vectors. If the part shape is irregular and the point cloud distribution is discrete, the Euclidean clustering algorithm is selected, a distance threshold is set, and points with a distance less than the threshold are clustered into independent point cloud clusters. Finally, combined with the preset size, volume, and other parameters of the target part, the target point cloud region is screened from all point cloud clusters to complete the separation of the target from the background and interference.

[0115] S203: Calculate the geometric center of the target point cloud region. The geometric center is the mean of the coordinates or the center of the smallest bounding box.

[0116] Among them, the geometric center refers to the spatial center position of the target point cloud region, which is the core parameter characterizing the spatial position of the target part.

[0117] The mean coordinate refers to the average of the X, Y, and Z coordinates of all points within the target point cloud region, which is the three-dimensional coordinate point obtained.

[0118] The minimum bounding box center refers to the center coordinates of the smallest cuboid (or cube) that can completely enclose the target point cloud region, which can better fit the actual space occupied by the part.

[0119] Specifically, firstly, the coordinates of all points in the segmented target point cloud region are extracted. If the target point cloud is uniformly distributed and has a regular geometric shape, the geometric center is calculated using the coordinate mean method. The X, Y, and Z coordinates of all points are traversed, and the mean of each coordinate axis is calculated. The combined mean of the coordinates is used as the geometric center. If the target point cloud is unevenly distributed and the part has an irregular shape, the minimum bounding box method is used. By traversing the extreme values ​​of the coordinates of all points in the target point cloud, the length, width, height, and vertex coordinates of the smallest cuboid that encloses the target point cloud are determined. The center coordinates of this cuboid are then calculated as the geometric center. The two methods can be selected according to the characteristics of the part to ensure that the geometric center can accurately represent the spatial position of the target part.

[0120] S204: Extract the three-dimensional coordinates of the geometric center as the translation constraint vector. The translation constraint vector is the three-dimensional translation component that constrains the six-degree-of-freedom registration space.

[0121] Among them, the translation constraint vector refers to the three-dimensional vector characterizing the spatial translation position of the target part. ), which is directly composed of the calculated three-dimensional coordinates of the geometric center.

[0122] A six-DOF registration space refers to a space containing three translational degrees of freedom. and 3 rotational degrees of freedom The complete pose search space.

[0123] Translational components refer to the three-dimensional parameters used to describe the spatial position of a part in a six-degree-of-freedom registration space, which together with rotational components constitute a complete 6D pose.

[0124] Specifically, the calculated three-dimensional coordinates of the geometric center Extract directly as a translation constraint vector The translation constraint vector fixes the spatial translation position of the target part, so that the 6D registration space, which originally required searching for translation and rotation parameters at the same time, is directly constrained to a 3D space that only needs to search for rotation parameters, thereby reducing the dimensionality of the registration space and avoiding the dimensionality curse of 6D space search from the root.

[0125] S205: Generate uniform sampling points for a three-dimensional rotation group using Fibonacci grid sampling and map them to a rotation matrix, or generate a rotation matrix by discretizing the Euler angle space according to a preset step size.

[0126] Among them, Fibonacci grid sampling refers to a method for achieving uniform sampling within a three-dimensional rotation group SO(3), which can generate uniformly distributed and fully covered rotation sampling points, avoiding sparse or locally dense sampling.

[0127] The three-dimensional rotation group SO(3) refers to the set of all 3×3 orthogonal rotation matrices, which is used to describe all possible rotational orientations of the part.

[0128] A rotation matrix is ​​a 3×3 orthogonal matrix used to characterize the rotational attitude of a part, which can rotate a reference point cloud from its initial attitude to its target attitude.

[0129] Euler angle space refers to the rotation parameter space with three dimensions: Roll, Pitch, and Yaw. The preset step size refers to the uniform discrete interval set in the three dimensions.

[0130] Specifically, two methods can be selected to generate the rotation matrix, and the choice can be made flexibly according to the sampling accuracy requirements; Method 1: Using the Fibonacci grid sampling method, uniformly distributed sampling points are generated in the three-dimensional rotation group SO(3), and each sampling point is mapped to the corresponding 3×3 rotation matrix through matrix transformation. This method has good sampling uniformity and can effectively cover all rotational attitudes; Method 2: In Euler angle Within the space, the dimensions are discretized according to a preset step size (e.g., 1°~5° per dimension). For each discrete Euler angle combination, a corresponding 3×3 rotation matrix is ​​generated using the Euler angle to rotation matrix conversion formula. This method is simple to calculate and easy to adjust the sampling density.

[0131] S206: Construct a candidate rotation set based on the rotation matrix. The number of rotation matrices in the candidate rotation set is adjusted according to the sampling precision or the step size.

[0132] The candidate rotation set refers to the set of all generated rotation matrices, which covers all possible rotational orientations of the target part.

[0133] Sampling precision refers to the fineness of rotation sampling. The higher the sampling precision, the more rotation matrices are required. Step size refers to the interval of the discrete division of Euler angle space. The smaller the step size, the more rotation matrices are required, and the finer the sampling is.

[0134] Specifically, all generated rotation matrices are integrated to construct a candidate rotation set. The number of rotation matrices in the set can be flexibly adjusted according to the accuracy requirements of the actual industrial scenario. If the accuracy requirement for attitude estimation is high, the sampling density of the Fibonacci grid can be increased or the offset length of the Euler angle can be reduced, so that the number of rotation matrices in the candidate rotation set reaches 1,000 to 2,000. If the real-time requirement is higher, the sampling density can be reduced or the offset length can be increased, so that the number of rotation matrices is controlled between 500 and 1,000, ensuring a balance between sampling accuracy and computational efficiency, avoiding the omission of the globally optimal rotation attitude, and avoiding the waste of computing power.

[0135] S207: Process the candidate rotation set into a rotation tensor, and process the reference point cloud into a batched point cloud tensor that matches the dimension of the rotation tensor.

[0136] Here, the rotation tensor refers to a high-dimensional tensor (e.g., with dimensions of 1) formed by integrating multiple 3×3 rotation matrices from the candidate rotation set. , where M is the number of rotation matrices), to facilitate GPU parallel computing calls.

[0137] Batch point cloud tensors refer to copying and expanding a base point cloud according to the batch dimension (M) of the rotation tensor to form a high-dimensional point cloud tensor (e.g., dimension M) that matches the dimension of the rotation tensor. , where N is the number of reference point clouds), ensuring that each rotation matrix can process a batch of reference point clouds.

[0138] Specifically, first read the candidate rotation set. The M 3×3 rotation matrices in the array are integrated into a single matrix of dimension M through a tensor reshaping operation. The rotation tensor is used to standardize the data format for GPU parallel computing; then the baseline point cloud (containing N points, with dimensions of ...) is read. Through batch copying, the baseline point cloud is copied M times, forming a dimension of The batched point cloud tensor; ensure that the batch dimension (M) of the batched point cloud tensor is completely matched with the batch dimension (M) of the rotation tensor.

[0139] S208: Utilizes the parallel matrix multiplication operator of the graphics processor to perform rigid body rotation transformations in batches and combine them with translation constraint vectors to generate candidate registration point clouds.

[0140] Among them, the GPU parallel matrix multiplication operator refers to the hardware operator built into the GPU for efficiently performing large-scale matrix multiplication operations. It can handle multiple matrix multiplication tasks at the same time, greatly improving computational efficiency.

[0141] Rigid body rotation transformation refers to the process of rotating the reference point cloud from its initial pose to a candidate rotation pose by using a rotation matrix while keeping the geometry of the reference point cloud unchanged.

[0142] Candidate registration point cloud refers to the point cloud that, after rigid body rotation transformation and combined with translation constraint vector, is mapped to the scene observation point cloud space from the reference point cloud, and is used for matching and comparison with the target point cloud region.

[0143] Specifically, the GPU's parallel matrix multiplication operator is invoked to obtain the rotation tensor ( ) and batch point cloud tensors ( Using M candidate rotation postures as input, perform matrix multiplication operations in batches to complete rigid body rotation transformations, obtaining the batch-processed point cloud tensor after rotation. ); then the resulting translation constraint vector Add them in batches to the coordinates of each point after rotation, i.e., each point The coordinates are each added with the corresponding component of the translation constraint vector to generate M candidate registration point clouds, each of which corresponds to a candidate rotational attitude.

[0144] S209: Calculate the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud in parallel to obtain the fitness evaluation results.

[0145] Among them, the bidirectional nearest neighbor distance refers to calculating the nearest point distance from each point in the candidate registration point cloud to the scene observation point cloud, and the nearest point distance from each point in the scene observation point cloud to the candidate registration point cloud, respectively. It is the core indicator for measuring the degree of matching between the two.

[0146] The fitness evaluation result refers to the score, which is calculated based on the bidirectional nearest neighbor distance and represents the matching accuracy between the candidate registration point cloud and the scene observation point cloud. The lower the score, the higher the matching accuracy and the closer the corresponding pose is to the true pose.

[0147] Specifically, leveraging the parallel computing capabilities of the GPU, fitness evaluation is performed simultaneously on M candidate registration point clouds. For each candidate registration point cloud, all its points are traversed, and the distance from each point to the nearest point in the scene observation point cloud (or target point cloud region) is calculated. Relevant statistical values ​​(such as summation and mean) of all distances are recorded. Simultaneously, all points in the scene observation point cloud are traversed, and the distance from each point to the nearest point in the candidate registration point cloud is calculated. Relevant statistical values ​​are recorded. Combining the results of the two calculations, the fitness score corresponding to the candidate registration point cloud is obtained, and finally, the fitness evaluation results corresponding to the M candidate poses are output.

[0148] Optionally, the fitness result is the chamfer distance; the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud is calculated in parallel to obtain the fitness evaluation result, including:

[0149] Calculate the sum of the distances from each point in the candidate registration point cloud to the nearest point in the scene observation point cloud, and determine the forward distance term;

[0150] Calculate the sum of the distances from each point in the scene observation point cloud to the nearest point in the candidate registration point cloud, and determine the backward distance term;

[0151] The forward distance and backward distance are summed to determine the chamfer distance corresponding to the candidate pose.

[0152] Chamfer distance is a commonly used metric for point cloud matching similarity. It quantifies the degree of matching between two point clouds by calculating the sum of their bidirectional nearest neighbor distances. The smaller the value, the higher the overlap between the two point clouds and the better the matching accuracy.

[0153] The forward distance term refers to the sum of the one-way nearest neighbor distances from the candidate registration point cloud to the scene observation point cloud.

[0154] The backward distance term refers to the sum of the one-way nearest neighbor distances from the scene observation point cloud to the candidate registration point cloud.

[0155] Specifically, when the chamfer distance is selected as the fitness result, the GPU parallel computing module is used to calculate the sum of the bidirectional nearest neighbor distances in two steps. First, the forward distance term is calculated: traversing all points in each candidate registration point cloud, finding the nearest point in the scene observation point cloud, calculating the Euclidean distance between the two points, and summing the nearest neighbor distances of all points to obtain the forward distance term for the candidate pose. Second, the backward distance term is calculated: traversing all points in the scene observation point cloud, finding the nearest point in the corresponding candidate registration point cloud, calculating the Euclidean distance between the two points, and summing the nearest neighbor distances of all points to obtain the backward distance term. Finally, the forward and backward distance terms are summed to obtain the chamfer distance corresponding to the candidate pose, which is used as the fitness evaluation result.

[0156] S210: Perform parallel minimum indexing operation to extract the candidate rotation pose with the lowest fitness score as the global coarse pose.

[0157] Among them, the parallel minimum indexing operation refers to using the parallel computing power of the GPU to compare the fitness scores of all candidate poses at the same time and quickly find the index position corresponding to the candidate pose with the lowest score.

[0158] Global coarse pose refers to the complete 6D pose formed by selecting the rotational pose with the lowest fitness score (highest matching accuracy) from all candidate rotational poses and combining it with a determined translational constraint vector.

[0159] Specifically, the parallel minimum indexing operator of the GPU is invoked, and the fitness scores of the M candidate poses are input. All scores are compared simultaneously to quickly locate the index corresponding to the candidate pose with the lowest fitness score. Based on this index, the corresponding rotation matrix is ​​extracted from the constructed candidate rotation set as the optimal candidate rotation pose. This optimal rotation matrix is ​​combined with the obtained translation constraint vector to form the complete 6D pose, which is the global coarse pose. This pose is a globally optimal coarse solution that escapes the local optimum trap. For example... Figure 3 The global optimal selection process shown in (c) uses a parallel minimum indexing operation (i.e., a parallel Argmin operation) to accurately select the optimal coarse pose from multiple candidate poses, and... Figure 3 In contrast to the traditional method (i.e., single ICP initialization → getting trapped in local minima) shown in Figure (a), this paper solves the problem that the traditional method is prone to getting trapped in local optima.

[0160] S211: Starting from the global coarse pose, the point-to-surface iterative nearest point algorithm is called to perform local convergence iteration until the preset convergence threshold is met, and the target six-degree-of-freedom pose data is determined.

[0161] Among them, the Iterative Closest Point to Surface (ICP) algorithm is a high-precision geometric registration algorithm that achieves precise alignment of point clouds by iteratively optimizing pose parameters by minimizing the distance error between a point in a point cloud and the surface of another point cloud.

[0162] Local convergence iteration refers to taking the global coarse pose as the initial value and adjusting the pose parameters iteratively to gradually reduce the pose error until a preset standard is reached.

[0163] The preset convergence threshold refers to a pre-set error standard used to determine whether the iteration should stop.

[0164] The target six-degree-of-freedom pose data refers to the final output 6D pose (3D translation + 3D rotation) that meets the requirements of industrial precision operation.

[0165] Specifically, the obtained global coarse pose As the initial transformation matrix, the point-to-surface ICP algorithm is invoked to begin local convergence iteration. First, the point-to-surface distance error between the candidate registration point cloud (based on global coarse pose generation) and the scene observation point cloud (or target point cloud region) is calculated. Then, the pose parameters (translation and rotation components) are adjusted using an optimization algorithm to reduce the error. This process is repeated, calculating the pose error after each iteration until the pose error is less than a preset convergence threshold, at which point the iteration stops. The final pose parameters (rotation matrix + translation vector) are used as the target six-DOF pose data. Figure 3 In section (c), the global coarse pose is iteratively optimized using the point-to-surface ICP algorithm, so that the pose accuracy meets the requirements of industrial precision operation, and the final transformation from "coarse pose" to "precise pose" is completed.

[0166] Optionally, starting from the global coarse pose, the point-to-surface iterative nearest point algorithm is invoked for local convergence iteration until a preset convergence threshold is met, thereby determining the target six-DOF pose data, including:

[0167] Using the global coarse pose as the initial transformation matrix, a point-to-surface error measurement function is constructed between the candidate registration point cloud and the scene observation point cloud.

[0168] The error metric function is solved iteratively using an optimization algorithm to calculate the rigid body transformation update amount for the current iteration step;

[0169] If the rigid body transformation update is less than the preset pose change threshold and the number of iterations reaches the preset value, then the iteration stops and the currently accumulated transformation matrix is ​​used as the target six-degree-of-freedom pose data.

[0170] If the rigid body transformation update is greater than or equal to the preset pose change threshold, or if the number of iterations has not reached the preset value, then the iterative optimization steps continue.

[0171] The initial transformation matrix refers to the 6D pose matrix (including rotation matrix and translation vector) constructed based on the global coarse pose and used to initialize the point-to-surface ICP iteration.

[0172] The point-to-area error metric function is a function used to quantify the matching error between candidate registration point clouds and scene observation point clouds. The smaller the value, the higher the matching accuracy.

[0173] Rigid body transformation update refers to the amount of adjustment of pose parameters (rotation, translation) during each iteration, and is used to measure the iteration convergence speed.

[0174] The pose change threshold refers to a preset standard for judging whether the pose adjustment amount has reached a stable level.

[0175] The preset number of iterations refers to the maximum number of iterations set in advance for optimization, used to avoid the iteration from getting stuck in an infinite loop.

[0176] Specifically, the first step is to obtain the global coarse pose. As the initial transformation matrix, an initial candidate registration point cloud is generated based on this matrix. A point-to-surface error metric function (i.e., the sum of squares of the distances from all points to their corresponding surfaces) is constructed between the candidate registration point cloud and the scene observation point cloud. In the second step, optimization algorithms such as gradient descent are used to iteratively solve the error metric function to calculate the rigid body transformation update amount (rotation update matrix + translation update vector) for the current iteration step. In the third step, the iteration termination condition is determined: if the rigid body transformation update amount is less than the preset pose change threshold (indicating that the pose has tended to stabilize) and the number of iterations reaches the preset value, the iteration stops, and the currently accumulated transformation matrix is ​​used as the target six-degree-of-freedom pose data; if the rigid body transformation update amount is greater than or equal to the preset threshold, or the number of iterations does not reach the preset value, the current transformation update amount is added to the initial transformation matrix to update the candidate registration point cloud, and the above iterative steps are repeated until the termination condition is met.

[0177] The pose estimation method for flexible robot operations provided in this application first acquires a reference point cloud with known poses and a scene observation point cloud to be calculated. Then, it segments the effective target point cloud using density filtering to remove background, Euclidean clustering, and region growing clustering. The geometric center is obtained by calculating the mean or minimum bounding box center of the target point cloud coordinates, and a translation constraint vector is constructed to constrain the registration translation range. Next, multiple sets of rotation matrices are generated using uniform sampling of a Fibonacci mesh 3D rotation group or discrete sampling in Euler angle space. The sampling scale is adjusted according to accuracy requirements to form a candidate rotation set. After tensor transformation and dimensionality adaptation, the method utilizes GPU parallel computing capabilities to batch-process the reference point cloud. The quasi-point cloud undergoes rotation transformation, and combined with translation constraints, generates a large number of candidate registration point clouds. The bidirectional nearest neighbor distance calculation is performed in parallel to achieve pose evaluation. The globally optimal coarse pose is selected, and then the coarse pose is used as the initial condition to perform fine-grained iterative convergence using the point-to-surface iterative nearest point algorithm. Finally, the accurate six-degree-of-freedom pose result is output, which effectively improves the computation speed and real-time performance of point cloud registration in complex and disordered scenes. At the same time, it enhances the accuracy of pose solution and environmental anti-interference robustness. Through the steps of parallel search, global optimization, and fine refinement, the technical pain point of traditional ICP method being prone to getting trapped in local optima is solved, and high accuracy and high real-time performance of 6D pose estimation in industrial scenarios are achieved.

[0178] Figure 4 This is a schematic diagram of the pose estimation device for flexible robot operation provided in this application, as shown below. Figure 4 As shown, the pose estimation device 400 for flexible robot operations provided in this embodiment includes:

[0179] The acquisition module 401 is used to acquire the reference point cloud with known pose and the scene observation point cloud with the pose to be estimated.

[0180] The generation module 402 is used to perform target separation and translation space decoupling on the scene observation point cloud, extract the target point cloud region, and generate translation constraint vector based on the geometric features of the target point cloud region;

[0181] Module 403 is used to reduce the dimensionality of the six-degree-of-freedom registration space to a three-degree-of-freedom rotation search space based on the translation constraint vector, and to construct a candidate rotation set;

[0182] The determination module 404 is used to map the reference point cloud, translation constraint vector and candidate rotation set into a high-dimensional tensor, and to perform rigid body transformation and fitness evaluation simultaneously using parallel computing to obtain fitness results.

[0183] The determination module 404 is also used to filter the global coarse pose based on the fitness results, and iteratively optimize the global coarse pose as the initial constraint until the convergence condition is met, thereby determining the target six-degree-of-freedom pose data.

[0184] As an optional implementation, the pose estimation device for robot flexible operation further includes: a processing module 405, a calculation module 406, and an extraction module 407.

[0185] The processing module 405 is also used to segment the target point cloud region from the scene observation point cloud using point cloud density background removal, Euclidean clustering or region growing clustering algorithms;

[0186] Calculation module 406 is used to calculate the geometric center of the target point cloud region, where the geometric center is the mean of the coordinates or the center of the minimum bounding box;

[0187] The extraction module 407 is used to extract the three-dimensional coordinates of the geometric center as the translation constraint vector, which is the three-dimensional translation component that constrains the six-degree-of-freedom registration space.

[0188] As an optional implementation, the generation module 402 is also used to generate uniform sampling points of a three-dimensional rotation group using Fibonacci grid sampling and map them to a rotation matrix, or to generate a rotation matrix by discretizing the Euler angle space according to a preset step size.

[0189] The construction module 403 is also used to construct a candidate rotation set based on the rotation matrix, wherein the number of rotation matrices in the candidate rotation set is adjusted according to the sampling precision or the step size.

[0190] As an optional implementation, the processing module 405 is further configured to process the candidate rotation set into a rotation tensor and process the reference point cloud into a batched point cloud tensor that matches the dimension of the rotation tensor.

[0191] The generation module 402 is also used to perform rigid body rotation transformations in batches and generate candidate registration point clouds by combining translation constraint vectors using the parallel matrix multiplication operator of the graphics processor.

[0192] The determination module 404 is also used to calculate the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud in parallel, so as to obtain the fitness evaluation results.

[0193] As an optional implementation, the determining module 404 is also used to calculate the sum of the distances from each point in the candidate registration point cloud to the nearest point in the scene observation point cloud, and to determine the forward distance term;

[0194] The determination module 404 is also used to calculate the sum of the distances from each point in the scene observation point cloud to the nearest point in the candidate registration point cloud, and to determine the backward distance term;

[0195] The determination module 404 is also used to sum the forward distance term and the backward distance term to determine the chamfer distance corresponding to the candidate pose.

[0196] As an optional implementation, the extraction module 407 is also used to perform a parallel minimum indexing operation to extract the candidate rotation pose with the lowest fitness score as the global coarse pose.

[0197] The determination module 404 is also used to use the global coarse pose as the initial starting point, call the point-to-surface iterative nearest point algorithm to perform local convergence iteration until the preset convergence threshold is met, and determine the target six-degree-of-freedom pose data.

[0198] As an optional implementation, the construction module 403 is also used to construct a point-to-surface error measurement function between the candidate registration point cloud and the scene observation point cloud by using the global coarse pose as the initial transformation matrix.

[0199] The calculation module 406 is also used to iteratively solve the error metric function using an optimization algorithm and calculate the rigid body transformation update amount of the current iteration step;

[0200] The determination module 404 is also used to stop the iteration if the rigid body transformation update amount is less than the preset pose change threshold and the number of iterations reaches the preset value, and to use the currently accumulated transformation matrix as the target six-degree-of-freedom pose data.

[0201] The processing module 405 is also used to continue the iterative optimization steps if the rigid body transformation update amount is greater than or equal to the preset pose change threshold, or if the number of iterations has not reached the preset value.

[0202] Figure 5 This is a schematic diagram of the pose estimation device for flexible robot operations provided in this application. Figure 5As shown, this application provides a pose estimation device for flexible robot operations. The pose estimation device 500 for flexible robot operations includes: a receiver 501, a transmitter 502, a processor 503, and a memory 504.

[0203] Receiver 501 is used to receive instructions and data;

[0204] Transmitter 502 is used to send commands and data;

[0205] Memory 504 is used to store instructions executed by the computer;

[0206] The processor 503 is used to execute computer execution instructions stored in the memory 504 to implement the various steps of the pose estimation method for robot flexible operations in the above embodiments. For details, please refer to the relevant descriptions in the foregoing embodiments of the pose estimation method for robot flexible operations.

[0207] Optionally, the memory 504 can be either standalone or integrated with the processor 503.

[0208] When the memory 504 is set up independently, the electronic device also includes a bus for connecting the memory 504 and the processor 503.

[0209] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the above-described method.

[0210] This application also provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, implement the above-described method.

[0211] The aforementioned readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. The readable storage medium can be any available medium accessible to a general-purpose or special-purpose computer.

[0212] An exemplary readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can reside in an Application Specific Integrated Circuit (ASIC). Alternatively, the processor and the readable storage medium can exist as discrete components in the device.

[0213] The division of units is merely a logical functional division; in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or units, and may be electrical, mechanical, or other forms.

[0214] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0215] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0216] If a function is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0217] Those skilled in the art will understand that all or part of the steps of the above-described method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above-described method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0218] Finally, it should be noted that other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This invention is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein, and is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A pose estimation method for flexible robot operations, characterized in that, include: Obtain the reference point cloud with known pose and the scene observation point cloud with pose to be estimated; The scene observation point cloud is subjected to target separation and translational spatial decoupling, the target point cloud region is extracted, and a translational constraint vector is generated based on the geometric features of the target point cloud region; Based on the translation constraint vector, the six-degree-of-freedom registration space is reduced to a three-degree-of-freedom rotation search space, and a candidate rotation set is constructed. The reference point cloud, the translation constraint vector and the candidate rotation set are mapped to high-dimensional tensors, and rigid body transformation and fitness evaluation are performed synchronously using parallel computing to obtain fitness results. The global coarse pose is selected based on the fitness results, and the global coarse pose is used as the initial constraint for iterative optimization until the convergence condition is met, thereby determining the target six-degree-of-freedom pose data.

2. The method according to claim 1, characterized in that, The process of separating the target and decoupling the translation space of the scene observation point cloud, extracting the target point cloud region, and generating a translation constraint vector based on the geometric features of the target point cloud region includes: The target point cloud region is segmented from the scene observation point cloud using point cloud density background removal, Euclidean clustering, or region growing clustering algorithms. Calculate the geometric center of the target point cloud region, where the geometric center is the coordinate mean or the minimum bounding box center; The three-dimensional coordinates of the geometric center are extracted as translation constraint vectors, which are three-dimensional translation components that constrain the six-degree-of-freedom registration space.

3. The method according to claim 1, characterized in that, The process of reducing the six-degree-of-freedom registration space to a three-degree-of-freedom rotation search space based on the translation constraint vector, and constructing a candidate rotation set, includes: The Fibonacci grid sampling method is used to generate uniform sampling points of a three-dimensional rotation group and map them into a rotation matrix, or the rotation matrix is ​​generated by discretizing the Euler angle space according to a preset step size. A candidate rotation set is constructed based on the rotation matrix, and the number of rotation matrices in the candidate rotation set is adjusted according to the sampling accuracy or the step size.

4. The method according to claim 1, characterized in that, The process of mapping the reference point cloud, the translation constraint vector, and the candidate rotation set into a high-dimensional tensor, and simultaneously performing rigid body transformation and fitness evaluation using parallel computing to obtain fitness results includes: The candidate rotation set is processed into a rotation tensor, and the reference point cloud is processed into a batched point cloud tensor that matches the dimension of the rotation tensor. By utilizing the parallel matrix multiplication operator of the graphics processor, rigid body rotation transformations are performed in batches and combined with translation constraint vectors to generate candidate registration point clouds; The bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud is calculated in parallel to obtain the fitness evaluation results.

5. The method according to claim 4, characterized in that, The fitness result is the chamfer distance; the parallel calculation of the bidirectional nearest neighbor distance between each candidate registration point cloud and the scene observation point cloud yields the fitness evaluation result, including: Calculate the sum of the distances from each point in the candidate registration point cloud to the nearest point in the scene observation point cloud, and determine the forward distance term; Calculate the sum of the distances from each point in the scene observation point cloud to the nearest point in the candidate registration point cloud, and determine the backward distance term; The forward distance term and the backward distance term are summed to determine the chamfer distance corresponding to the candidate pose.

6. The method according to claim 4, characterized in that, The step of filtering the global coarse pose based on the fitness results, and iteratively optimizing the global coarse pose as the initial constraint until the convergence condition is met, to determine the target six-DOF pose data, includes: Perform a parallel minimum indexing operation to extract the candidate rotation pose with the lowest fitness score as the global coarse pose; Starting from the global coarse pose, the point-to-surface iterative nearest point algorithm is called to perform local convergence iteration until the preset convergence threshold is met, and the target six-degree-of-freedom pose data is determined.

7. The method according to claim 6, characterized in that, The process of using the global coarse pose as the initial starting point and calling the point-to-surface iterative nearest point algorithm for local convergence iteration until a preset convergence threshold is met to determine the target six-DOF pose data includes: Using the global coarse pose as the initial transformation matrix, a point-to-surface error measurement function is constructed between the candidate registration point cloud and the scene observation point cloud. The error metric function is iteratively solved using an optimization algorithm to calculate the rigid body transformation update amount for the current iteration step; If the rigid body transformation update amount is less than the preset pose change threshold and the number of iterations reaches the preset value, then the iteration stops and the currently accumulated transformation matrix is ​​used as the target six-degree-of-freedom pose data. If the rigid body transformation update is greater than or equal to the preset pose change threshold, or the number of iterations has not reached the preset value, then the iterative optimization step continues.

8. A pose estimation device for flexible robot operations, characterized in that, include: The acquisition module is used to acquire the reference point cloud with known pose and the scene observation point cloud with the pose to be estimated. The generation module is used to perform target separation and translational spatial decoupling on the scene observation point cloud, extract the target point cloud region, and generate translational constraint vectors based on the geometric features of the target point cloud region; The construction module is used to reduce the dimensionality of the six-degree-of-freedom registration space to a three-degree-of-freedom rotation search space based on the translation constraint vector, and construct a candidate rotation set; The determination module is used to map the reference point cloud, the translation constraint vector and the candidate rotation set into a high-dimensional tensor, and use parallel computing to simultaneously perform rigid body transformation and fitness evaluation to obtain fitness results. The determining module is further configured to filter the global coarse pose based on the fitness result, and iteratively optimize the global coarse pose as the initial constraint until the convergence condition is met, thereby determining the target six-degree-of-freedom pose data.

9. A pose estimation device for flexible robot operations, characterized in that, include: Memory, processor; The memory stores computer-executed instructions; The processor executes computer execution instructions stored in the memory, causing the processor to perform the method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the method as described in any one of claims 1-7.

Images