Workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraint

Abstract

The application discloses a workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraint, relates to the technical fields of industrial robot environment perception, three-dimensional computer vision, neural scene representation and intelligent manufacturing, and comprises the following steps: constructing a measurable target workpiece three-dimensional model based on reference image data, and constructing an enhanced continuous field representation model based on multi-view observation data; extracting a semantic entity instance set according to the enhanced continuous field representation model, and constructing a semantic topological graph; mapping the semantic topological graph into a continuous topological correlation strength distribution, and generating a continuous semantic topological field and a continuous space occupation representation; solving an initial six-degree-of-freedom pose according to a current observation image and the target workpiece three-dimensional model, and optimizing the initial six-degree-of-freedom pose in combination with the continuous semantic topological field and the continuous space occupation representation. The application improves the accuracy and physical consistency of workpiece pose estimation in a scene with occlusion, weak texture and complex spatial relationship.

Description

Technical Field

[0001] This invention relates to the fields of industrial robot environmental perception, 3D computer vision, neural scene representation and intelligent manufacturing technology, and more specifically, to a workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints. Background Technology

[0002] In industrial automation production processes, industrial robots are widely used in tasks such as grasping, handling, assembly, sorting, inspection, and collaborative manufacturing. To achieve precise operations, it is usually necessary to obtain the six-degree-of-freedom pose information of the target workpiece in three-dimensional space, that is, the three-dimensional translation and three-dimensional rotation parameters of the target workpiece relative to the robot coordinate system or the scene coordinate system.

[0003] Existing methods for estimating the six degrees of freedom (DOF) pose of workpieces mainly include those based on 3D model matching and those based on deep learning. Methods based on 3D model matching typically rely on point cloud registration, edge matching, template matching, or feature correspondence to solve for the target pose; methods based on deep learning typically utilize image, point cloud, or multimodal sensor data to learn the pose features of the target workpiece. These methods achieve good results in scenarios where the target appearance is clear, occlusion is minimal, and the background is relatively simple.

[0004] However, in complex industrial environments, target workpieces are often adjacent to workbenches, fixtures, material frames, robotic arm end effectors, or other workpieces, making them susceptible to local occlusion, strong reflections, weak textures, similar background objects, and sensor noise. This results in incomplete observable geometric and visual texture information. Existing methods primarily rely on the target workpiece's own image features, point cloud features, or 3D model geometric features, failing to adequately utilize the spatial relationships between the target workpiece and environmental entities, such as support, adjacency, occlusion, non-collision, and relative orientation. Consequently, when observational information is missing or interference exists, pose estimation results that are inconsistent with the actual physical space are easily produced.

[0005] In recent years, technologies such as neural radiation fields, semantically enhanced 3D representations, and open-vocabulary visual understanding have enabled unified modeling of scene geometric and semantic information, providing new technical means for complex scene perception. However, existing related methods are usually mainly used for scene reconstruction, semantic query, target recognition, or visualization. The semantic information and spatial relationships obtained are mostly used as auxiliary information before and after pose estimation, and have not yet been fully transformed into continuous spatial constraints that can be directly used in workpiece pose optimization.

[0006] Furthermore, existing scene graph or semantic relationship modeling methods typically represent spatial relationships between entities as discrete relationships or static priors, making it difficult to form a continuous and optimizable constraint distribution in three-dimensional space. When there are errors in the initial semantic associations, or when the spatial relationship between the target workpiece and environmental entities changes during pose optimization, existing semantic constraints usually cannot be dynamically corrected based on topological consistency deviations, leading to erroneous semantic relationships potentially continuing to affect pose estimation results.

[0007] Therefore, there is an urgent need for a six-degree-of-freedom workpiece pose estimation method that can integrate multi-view geometric information, semantic feature information, and environmental entity spatial topological relationships. This method can transform environmental semantic relationships from discrete relationships into continuous spatial constraints and update the pose estimation based on topological consistency deviations, thereby improving the accuracy, stability, and physical consistency of workpiece pose estimation in complex industrial scenarios.

[0008] No effective solutions have yet been proposed to address the problems in the relevant technologies. Summary of the Invention

[0009] To address the problems in related technologies, this invention proposes a six-degree-of-freedom pose estimation method for workpieces based on continuous semantic topological field feedback constraints, in order to overcome the aforementioned technical problems existing in existing related technologies.

[0010] Therefore, the specific technical solution adopted by the present invention is as follows:

[0011] According to one aspect of the present invention, a method for estimating the pose of a workpiece with six degrees of freedom based on continuous semantic topological field feedback constraints is provided, the method comprising:

[0012] Acquire reference image data of the target workpiece, construct a measurable 3D model of the target workpiece based on the reference image data, and construct an enhanced continuous field representation model based on multi-view observation data;

[0013] Extract a set of semantic entity instances based on the enhanced continuous field representation model, and construct a semantic topology graph based on the set of semantic entity instances;

[0014] The relation edges in the semantic topology graph are mapped to a continuous topological association strength distribution in three-dimensional space. A continuous semantic topology field is generated based on the continuous topological association strength distribution, and a topological consistency constraint is applied to the continuous semantic topology field to generate a continuous space occupancy representation.

[0015] The initial six-degree-of-freedom pose is solved based on the current observed image and the measurable 3D model of the target workpiece. A joint pose optimization objective is constructed by combining the continuous semantic topological field and the continuous spatial occupancy representation. The initial six-degree-of-freedom pose is optimized based on the joint pose optimization objective to obtain the optimized six-degree-of-freedom pose of the target workpiece.

[0016] According to another aspect of the present invention, a six-degree-of-freedom pose estimation system for a workpiece based on continuous semantic topological field feedback constraints is provided, the system comprising:

[0017] The continuous field representation model construction module is used to acquire reference image data of the target workpiece, construct a measurable 3D model of the target workpiece based on the reference image data, and construct an enhanced continuous field representation model based on multi-view observation data.

[0018] The semantic topology graph construction module is used to extract a set of semantic entity instances based on the enhanced continuous field representation model, and to construct a semantic topology graph based on the set of semantic entity instances.

[0019] The continuous semantic topology field generation module is used to map the relation edges in the semantic topology graph to a continuous topological association strength distribution in three-dimensional space, generate a continuous semantic topology field based on the continuous topological association strength distribution, and apply topological consistency constraints to the continuous semantic topology field to generate a continuous space occupancy representation.

[0020] The joint pose optimization module is used to solve the initial six-DOF pose based on the current observed image and the measurable target workpiece 3D model, and to construct the joint pose optimization objective by combining the continuous semantic topological field and the continuous spatial occupancy representation. Based on the joint pose optimization objective, the initial six-DOF pose is optimized to obtain the optimized target workpiece six-DOF pose.

[0021] The beneficial effects of this invention are as follows:

[0022] 1. This invention unifies the geometric information, color appearance information and semantic feature information of multi-view scenes into a continuous semantic radiation field, that is, a continuous field representation model. Compared with pose estimation methods that rely only on single-frame images, point clouds or the features of the target workpiece itself, it can provide a more stable scene space representation under conditions of occlusion, weak texture and background interference.

[0023] 2. This invention does not merely use environmental semantic information as discrete labels or static priors, but transforms the semantic topological relationships between environmental entities into a continuous semantic topological field, enabling spatial relationships such as support, adjacency, occlusion, non-collision, and relative orientation to form a continuous constraint distribution in three-dimensional space.

[0024] 3. This invention maps continuous semantic topological fields into parameterized spatial constraints and introduces a six-degree-of-freedom pose optimization objective for the workpiece. This allows the pose estimation process to be simultaneously subject to the combined effects of visual observation consistency, semantic topological consistency, spatial occupancy consistency, and geometric relationship constraints, thereby reducing physical inconsistencies such as spatial penetration, suspension, and incorrect occlusion order.

[0025] 4. In the pose optimization process, the present invention updates the continuous semantic topological field and parameterized spatial constraints based on the topological consistency deviation, so that the environmental semantic constraints are no longer one-time static constraints, but can be dynamically corrected with the pose optimization process, thereby improving the robustness and physical consistency of pose estimation in complex industrial scenarios.

[0026] 5. This invention combines open-vocabulary semantic query, target workpiece reference image and target workpiece rotation video data to form the basis for target region extraction, which can reduce the dependence on large-scale manually labeled data and specific workpiece-specific training sets, and is conducive to improving the rapid deployment capability in scenarios of new workpieces or small batches of workpieces. Attached Figure Description

[0027] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0028] Figure 1 This is a flowchart of a six-degree-of-freedom pose estimation method for a workpiece based on continuous semantic topological field feedback constraints according to an embodiment of the present invention;

[0029] Figure 2 This is a flowchart illustrating the practical application of a six-degree-of-freedom pose estimation method for a workpiece based on continuous semantic topological field feedback constraints, as shown in an embodiment of the present invention.

[0030] Figure 3 This is a schematic diagram illustrating the continuous semantic topological field generation process according to an embodiment of the present invention;

[0031] Figure 4 This is a schematic diagram illustrating how a continuous semantic topological field is mapped to a parameterized spatial constraint and participates in pose optimization according to an embodiment of the present invention;

[0032] Figure 5 This is a block diagram of a six-degree-of-freedom pose estimation system for a workpiece according to an embodiment of the present invention;

[0033] Figure 6 This is a schematic diagram of the feedback iterative optimization process according to an embodiment of the present invention. Detailed Implementation

[0034] To further illustrate the various embodiments, the present invention provides accompanying drawings, which are part of the disclosure of the present invention. These drawings are mainly used to illustrate the embodiments and can be used in conjunction with the relevant descriptions in the specification to explain the operating principles of the embodiments. With reference to these drawings, those skilled in the art should be able to understand other possible implementation methods and the advantages of the present invention.

[0035] According to an embodiment of the present invention, a six-degree-of-freedom pose estimation method for a workpiece based on continuous semantic topological field feedback constraints is provided.

[0036] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments, such as... Figure 1 As shown, according to an embodiment of the present invention, a six-degree-of-freedom pose estimation method for a workpiece based on continuous semantic topological field feedback constraints is provided. The method includes:

[0037] Acquire reference image data of the target workpiece, construct a measurable 3D model of the target workpiece based on the reference image data, and construct an enhanced continuous field representation model based on multi-view observation data.

[0038] Specifically, the construction of a measurable 3D model of the target workpiece based on reference image data, and the construction of an enhanced continuous field representation model based on multi-view observation data, include:

[0039] Based on the reference image data, a three-dimensional model of the target workpiece is constructed using three-dimensional geometric reconstruction. The three-dimensional model of the target workpiece is then corrected for scale consistency based on the actual size parameters of the target workpiece to obtain the corrected three-dimensional model of the target workpiece.

[0040] The corrected 3D model of the target workpiece is defined in the target workpiece coordinate system and mapped to the unified scene coordinate system through pose transformation to obtain a measurable 3D model of the target workpiece.

[0041] Acquire multi-view observation data of the scene to be estimated, construct a continuous field representation model based on the multi-view observation data, and optimize the continuous field representation model to obtain an enhanced continuous field representation model.

[0042] Specifically, a continuous field representation model is constructed based on multi-view observation data, and the continuous field representation model is optimized to obtain an enhanced continuous field representation model, including:

[0043] Based on multi-view observation data, a continuous field representation model is constructed using parameterized approximation operations of a three-dimensional continuous field;

[0044] Continuous field representation models include: spatial density field, color radiation field, and semantic feature field;

[0045] By utilizing cross-view consistency loss, the continuous field representation model is optimized for cross-view consistency, resulting in an enhanced continuous field representation model.

[0046] Specifically, the process involves acquiring reference image data of the target workpiece, multi-view video data of the scene to be estimated, and the current observation image of the workpiece to be measured; and constructing a three-dimensional model of the workpiece with real physical scale (i.e., a measurable three-dimensional model of the target workpiece) based on the reference image data.

[0047] The 3D model of the target workpiece is obtained through single-view geometric reconstruction, multi-view geometric reconstruction, or prior template fitting. Specifically, it is obtained through 3D geometric reconstruction and then scale consistency correction is performed using the known actual size parameters of the target workpiece. The currently observed image includes a color image, or a combination of a color image and a depth image. The target workpiece region in the currently observed image is obtained through a target region extraction model, which is trained using pixel-level region data generated from target workpiece reference image data or rotated video data. In other words, pixel-level region data of the target workpiece is generated based on its semantic description and cross-frame consistency to train or update the target region extraction model.

[0048] A semantically enhanced continuous field representation model (i.e., the enhanced continuous field representation model) is constructed based on multi-view video data. The continuous field representation model includes at least a spatial density field, a color radiation field, and a semantic feature field. Cross-view consistency optimization is performed on the continuous field representation model based on multi-view observation results.

[0049] Specifically, the spatial density field, color radiation field, and semantic feature field are jointly represented under a unified spatial coordinate system. The continuous semantic radiation field model (i.e., the continuous field representation model) is optimized through cross-view consistency constraints, so that the scene geometry, appearance information, and semantic features can form a continuous expression in three-dimensional space.

[0050] Among them, the semantically enhanced continuous field representation model is used to establish a continuous mapping relationship between spatial location, viewing direction, spatial density, color response and semantic features, and cross-view consistency optimization is achieved by jointly constraining the differences in color response, spatial density and semantic features of the same spatial location under different viewpoints.

[0051] This invention is used to estimate the six-degree-of-freedom pose of a target workpiece relative to the scene coordinate system or the robot coordinate system in scenarios such as grasping, handling, assembling, and inspecting by industrial robots. The six-degree-of-freedom pose includes three-dimensional rotation parameters and three-dimensional translation parameters.

[0052] The method in this embodiment includes the following steps: acquiring data and constructing a three-dimensional model of the target workpiece with real physical scale. Specifically, it involves acquiring reference image data of the target workpiece, which may include a single image, multiple images, image frames extracted from a rotated video of the target workpiece, a contour image of the target workpiece, key point images of the target workpiece, or an image of the target workpiece containing a small amount of annotation information. Multi-view video data can be acquired by a fixed camera, a robotic arm end effector camera, an RGB-D camera, or a multi-camera system. The currently observed image can be a color image or a combination of a color image and a depth image.

[0053] A 3D model of the target workpiece is constructed based on reference image data. Specifically, the 3D model can be obtained using multi-view 3D reconstruction, single-view reconstruction, point cloud reconstruction, CAD model matching, or prior template fitting. To enable the 3D model of the target workpiece to be used for pose calculation in real industrial scenarios, scale consistency correction is further performed on the 3D model based on the known actual size parameters of the target workpiece. The actual size parameters can be the length, width, height, hole spacing, edge spacing, or the distance between any two known feature points of the target workpiece. Based on the known actual size parameters of the target workpiece, the reconstructed 3D model of the target workpiece is scale consistent with the real physical scale. The scale-corrected 3D model of the target workpiece is defined in the target workpiece coordinate system and can be mapped to a unified scene coordinate system through pose transformation, providing a unified spatial reference for subsequent continuous field representation model construction and pose optimization.

[0054] This step yields a 3D model of the target workpiece with realistic physical dimensions, providing a foundation for subsequent initial pose calculation, space occupancy calculation, and geometric constraint calculation.

[0055] The steps for constructing a semantically enhanced continuous field representation model based on multi-view video data are as follows:

[0056] A semantically enhanced continuous field representation model is constructed based on multi-view video data of the scene to be estimated. The continuous field representation model is used to represent the spatial density, color radiation information, and semantic feature information of the scene in a unified three-dimensional spatial coordinate system.

[0057] In one implementation, the continuous field representation model is expressed as:

[0058] ;

[0059] in, Represents three-dimensional spatial position. Indicates the direction of observation. Indicates spatial density, Indicates color response, Represents semantic features, The parameter is The continuous field representation model.

[0060] Continuous field representation models can be implemented using neural radiation fields, three-dimensional Gaussian representations, voxel feature fields, hash grid feature fields, or three-plane feature fields. Continuous field representation models are not limited to specific network structures; any continuous three-dimensional representation capable of outputting spatial density, color response, and semantic features can be used in this implementation. When using three-dimensional Gaussian representation, continuous interpolation or weighted aggregation of spatial positions is performed using a Gaussian kernel function to obtain the corresponding density, color, and semantic feature responses.

[0061] To improve the consistency of scene representation across different viewpoints, cross-view consistency optimization is performed on the continuous field representation model. Specifically, for the same 3D spatial location, its color response, spatial density response, and semantic feature response are constrained to remain consistent across different viewpoints. The cross-view consistency loss can be expressed as:

[0062] ;

[0063] in, This indicates cross-perspective consistency loss; Represents a viewpoint on a set; and This indicates two different observation perspectives; Indicates perspective and perspective The publicly observable three-dimensional spatial region; and Representing three-dimensional spatial positions respectively From the perspective and perspective The projection position below; and These represent the image color response at the corresponding viewpoints; and These represent the spatial density response from the corresponding viewpoints; and These represent the semantic feature responses from the corresponding perspectives; These represent the weight coefficients of the color response term, spatial density term, and semantic feature term, respectively. This represents the normalization coefficient.

[0064] This step yields a semantically enhanced continuous 3D scene representation, enabling the geometric structure, appearance information, and semantic information in the scene to be queried and computed in a unified spatial coordinate system.

[0065] Extract a set of semantic entity instances based on the enhanced continuous field representation model, and construct a semantic topology graph based on the set of semantic entity instances.

[0066] Specifically, the extraction of a semantic entity instance set based on the enhanced continuous field representation model, and the construction of a semantic topology graph based on the semantic entity instance set, includes:

[0067] The three-dimensional semantic feature vector output by the enhanced continuous field representation model is normalized to obtain the normalized semantic feature vector.

[0068] The normalized semantic feature vector is compared with the preset category prototype feature vector, text encoding feature vector or the output feature of the classification network to obtain the semantic similarity.

[0069] The semantic similarity is compared with a preset matching threshold to obtain the semantic category matching result. The spatial sampling points are then filtered based on the semantic category matching result to obtain a three-dimensional sampling point set.

[0070] Three-dimensional connected component clustering is performed on the three-dimensional sampling point set to obtain a set of semantic entity instances, and the spatial relationships between semantic entity instances are determined based on the set of semantic entity instances.

[0071] Specifically, the three-dimensional sampling point set is clustered into three-dimensional connected components to obtain a set of semantic entity instances, and the spatial relationships between semantic entity instances are determined based on the set of semantic entity instances, including:

[0072] Obtain the spatial distance between three-dimensional sampling points in the three-dimensional sampling point set, and connect three-dimensional sampling points whose spatial distance is less than the preset neighborhood radius to obtain the sampling point adjacency relationship;

[0073] Based on the adjacency relationship of sampling points, connected component analysis is performed using depth-first search or breadth-first search to obtain several connected regions. Each connected region is then treated as a semantic entity instance, resulting in a set of semantic entity instances.

[0074] Determine the 3D scene representation elements of each semantic entity instance in the semantic entity instance set, and calculate the instance center, spatial boundary, and occupied area based on the 3D scene representation elements;

[0075] Based on instance center, spatial boundary, and occupied area, and combined with preset relationship determination rules, the spatial relationship between semantic entity instances is determined.

[0076] Specifically, spatial relationships include at least one of the following: support relationship, adjacency relationship, occlusion relationship, non-collision constraint relationship, and relative orientation relationship.

[0077] Each semantic entity instance is treated as a semantic entity instance node, and the set of relationship edges between semantic entity instances is determined based on spatial relationships. A semantic topology graph is constructed based on the semantic entity instance nodes and the set of relationship edges.

[0078] Specifically, semantic entity instances in the scene are extracted based on the semantic feature field, and a semantic topology graph is constructed based on the spatial relationships between the semantic entity instances.

[0079] In the semantic topology graph, nodes represent semantic entity instances, and edges represent at least one of the following relationships between semantic entity instances: support relationship, adjacency relationship, occlusion relationship, non-collision constraint relationship, or relative orientation relationship. The relation edges in the semantic topology graph have relation type, relation direction, relation weight, and reference topology state. The reference topology state is determined by the spatial relations of semantic entities that satisfy the consistency condition in the multi-view observation results. The semantic topology stability is quantified by the deviation of the spatial relations between semantic entity instances relative to the reference topology state.

[0080] Specifically, extracting semantic entity instances and constructing a semantic topology graph includes the following steps:

[0081] like Figure 3 As shown, after obtaining the semantically enhanced continuous field representation model, semantic entity instances in the scene are extracted based on the semantic feature information, spatial density response, and 3D spatial distribution results in the continuous field representation model. The 3D spatial distribution results can be obtained through uniform sampling in the scene space, sampling along camera rays, or importance sampling based on the spatial density response. Semantic entity instances can include target workpieces, workbenches, fixtures, material frames, support surfaces, adjacent workpieces, robotic arm end effectors, or other environmental entities related to the pose estimation of the target workpiece. Specifically, the continuous semantic topological field generation process includes the following steps: 1. Constructing a semantically enhanced continuous field representation, i.e., fusing spatial density, color information, and semantic features to construct a semantically enhanced continuous field representation; 2. Extracting semantic entity instances, i.e., obtaining semantic entity instances of the target workpiece, workbench, fixture, material frame, and adjacent workpieces through 3D semantic response, instance segmentation and clustering, and entity occupancy area acquisition; 3. Calculating entity geometric attributes, i.e., solving for instance center, spatial boundary, occupancy area, and contact area; 4. Determining spatial relationships, identifying support relationships, adjacency relationships, occupancy relationships, etc. 5. Semantic topology graph construction: using semantic entity instances as nodes (e.g., target workpiece, workbench, fixture, material frame, and adjacent workpieces) and entity spatial relationships as edges (e.g., adjacency, support, occlusion, non-collision, and orientation), and combining the generated relationship type encoding and relationship weights to complete the semantic topology graph construction; 6. Relationship edge continuous fielding: mapping discrete relationship edges and using a continuous influence function to obtain the topological association strength distribution; 7. Outputting a continuous semantic topology field, which allows querying the topological association strength at any spatial location. The continuous semantic topology field can map discrete entity relationships in the semantic topology graph to a continuous topological association strength distribution in three-dimensional space, allowing support, adjacency, occlusion, non-collision, and relative orientation relationships to participate in subsequent pose optimization.

[0082] In one implementation, semantic category matching, threshold filtering, and three-dimensional connected component clustering are performed on the semantic features in the continuous field representation model to obtain a set of semantic entity instances.

[0083] ;

[0084] in, Represents a collection of semantic entity instances. Let represent the i-th semantic entity instance, and N represent the number of semantic entity instances. Semantic category matching specifically includes: normalizing the 3D semantic feature vector output by the enhanced continuous field representation model; then calculating the similarity between the normalized semantic feature vector and the preset category prototype feature vector, text encoding feature vector, or classification network output features to obtain semantic similarity; wherein, the similarity calculation uses cosine similarity: when the maximum similarity is greater than the preset matching threshold, the category corresponding to the semantic category matching result is determined as the semantic category to which the current spatial sampling point belongs.

[0085] The preset category prototype feature vector includes at least one of the following: visual feature vector, geometric shape feature vector, and semantic description feature vector corresponding to the target workpiece category. The visual feature vector can be obtained from the target workpiece image through a feature encoding network; the geometric shape feature vector includes three-dimensional size proportions, surface curvature distribution, and local structure description; the semantic description feature vector is obtained by converting the target workpiece name, functional description, or category text through a text encoding model. The category classifier training process includes: acquiring training samples containing different industrial workpiece categories; labeling the target regions of the training samples and extracting corresponding visual features; inputting the visual features into the classification network and optimizing the network parameters through cross-entropy loss; the trained category classifier is used to output the workpiece category probability corresponding to the spatial sampling region. The spatial density threshold is determined based on the spatial density response distribution of the foreground and background regions in the continuous field representation model, preferably within the range of 1-3 standard deviations above the mean background density. The semantic confidence threshold is determined based on the confidence distribution of the semantic matching results, preferably set to 0.5-0.8. The occupancy probability threshold is determined based on the spatial occupancy probability distribution, preferably set to 0.3-0.8. The specific steps of 3D connected component clustering include: establishing adjacency relationships between 3D sampling points (i.e., 3D sampling point sets) that meet the semantic filtering conditions according to spatial distance; connecting sampling points whose distance is less than the preset neighborhood radius; obtaining multiple connected regions through depth-first search or breadth-first search; and treating each connected region as an independent semantic entity instance.

[0086] For each semantic entity instance, based on its 3D sampling points, spatial density response, or semantic response—that is, 3D scene representation elements—the instance center, spatial boundary, and occupied region are calculated. The instance center can be the geometric center, density-weighted center, or semantic confidence-weighted center of the corresponding 3D sampling point; the spatial boundary can be an axis-aligned bounding box, an oriented bounding box, a convex hull, or an implicit boundary; the occupied region can be determined by 3D regions whose spatial density is greater than a preset density threshold or whose occupancy probability is greater than a preset occupancy threshold. After obtaining the semantic entity instances, a semantic topology graph is constructed based on the spatial relationships between the semantic entity instances.

[0087] ;

[0088] Where G represents a semantic topology graph. Represents a collection of semantic entity instance nodes. This represents the set of edges that represent the relationships between semantic entity instances.

[0089] In a semantic topology graph, nodes represent semantic entity instances, and edges represent spatial relationships between semantic entity instances. Spatial relationships include at least one of the following: support relationship, adjacency relationship, occlusion relationship, non-collision constraint relationship, and relative orientation relationship.

[0090] Specifically, in one implementation, the preset relationship determination rules include: if the bottom region of the first semantic entity is located near the upper surface of the second semantic entity, and the height difference between them is less than a preset support threshold, then a support relationship is determined to exist between them; if the three-dimensional boundary distance between the two semantic entities is less than a preset adjacency threshold and there is no obvious spatial overlap, then an adjacency relationship is determined to exist between them; if the projections of the two semantic entities in the observation direction overlap and have a stable front-back order, then an occlusion relationship is determined to exist between them; if the overlap of the occupied areas of the two semantic entities is less than a preset overlap threshold, or the minimum spatial distance between them is greater than a preset safety gap threshold, i.e., a safety distance threshold, then a non-collision constraint relationship is determined to exist between them; if the direction vector between the centers of the two semantic entities satisfies a preset direction interval, then a relative orientation relationship is determined to exist between them. The preset support threshold is determined based on the target workpiece size and the sensor depth measurement error, preferably set to 0.5%-5% of the target workpiece height, or set to 1mm-10mm based on the RGB-D camera depth error. The preset adjacency threshold is determined based on the expected proximity distance between the target workpiece and environmental entities, preferably set to 5mm-50mm. When the boundary distance between two entities is less than this threshold, they are considered to satisfy an adjacency relationship. The preset overlap threshold is determined based on the overlap ratio of the three-dimensional occupied areas of two entities, specifically calculated using the intersection-union ratio of the occupied areas of the two entities, preferably set to 0.05-0.2. When the intersection-union ratio of the occupied areas of two entities is less than the preset overlap threshold, it is considered that there is no obvious spatial conflict between the two entities, satisfying the non-collision constraint relationship. The preset safety gap threshold is determined comprehensively based on the minimum spatial distance between two entities, sensor measurement error, and task safety requirements, preferably set to 1mm-10mm. When the minimum spatial distance between two entities is greater than the preset safety gap threshold, it is considered that there is no obvious spatial conflict between the two entities, satisfying the non-collision constraint relationship. The preset direction interval is determined based on the reference direction in the unified scene coordinate system, judged based on the angle between the direction of the line connecting the centers of the two entities and the coordinate axis direction, preferably set to 15°-45°. When the direction vector between the centers of the two entities is within the preset direction interval, it is considered that the two entities satisfy the corresponding relative orientation relationship.

[0091] Each relation edge can be represented as:

[0092] ;

[0093] in, Represents the relationship edge between the i-th semantic entity instance and the j-th semantic entity instance; Indicates the relation type; Indicates the weight of the relation edge; Indicates relation type encoding or relation direction encoding; Indicates the reference topology state. Relationship edge weights. The determination specifically includes: statistically analyzing similar spatial relationships in multi-view observations to obtain the frequency of relationship occurrence; integrating the frequency of relationship occurrence, semantic instance recognition confidence, spatial relationship measurement stability, inter-entity distance reliability, and historical observation consistency to obtain a relationship reliability score; mapping the relationship reliability score to obtain the relationship edge weights, and normalizing the relationship edge weights to a preset range.

[0094] The reference topology is determined by the spatial relationships of semantic entities that meet consistency conditions from multi-view observations. Consistency conditions include at least one of the following: consistent semantic entity categories across multiple views; consistent relative positional orientation between two semantic entities; support height difference within a preset range; entity boundary distance within a preset adjacency range; consistent occlusion order across multiple views; and no overlap of the 3D occupied areas of two entities exceeding a preset threshold. The preset range for support height difference is determined based on the semantic entity size, 3D reconstruction accuracy, and depth measurement error, preferably set to 1mm-10mm; the preset adjacency range is determined based on the entity spatial scale and the distance relationship between entities in the operational scenario, preferably set to 5mm-50mm. When the height difference or boundary distance between two semantic entities meets the corresponding range conditions, their spatial relationship is considered to meet the consistency requirements. The preset threshold is determined comprehensively based on the overlap ratio of the 3D occupied areas of two entities, 3D reconstruction error, and task safety requirements. The overlap ratio is calculated using the intersection-union ratio of the occupied areas of two entities, preferably set to 0.05-0.2; when the intersection-union ratio of the occupied areas of two entities is less than the preset threshold, there is no significant spatial conflict between the two entities.

[0095] To evaluate whether semantic topological relationships are stable during pose optimization, a semantic topological stability loss is defined:

[0096] ;

[0097] in, This represents the semantic topological stability loss; Represents the set of relation edges; Indicates the weight of the relation edge; This represents the relative position vector between two semantic entity instances; Indicates the support height relationship quantity; Indicates the order of occlusion; Represents non-collision constraint relationships; Indicates relative orientation; superscript Indicates the current iteration state; superscript Indicates the reference topology state; These represent the weighting coefficients of each relation error term.

[0098] By organizing discrete semantic entity relationships into a weighted semantic topology graph and recording the reference topology state, subsequent steps can map discrete spatial relationships into a continuous spatial constraint field. This step yields a semantic topology graph that characterizes scene semantic entities and their spatial relationships, providing a foundation for the subsequent generation and pose optimization of the continuous semantic topology field.

[0099] The relation edges in the semantic topology graph are mapped to a continuous topological association strength distribution in three-dimensional space. A continuous semantic topology field is generated based on the continuous topological association strength distribution, and a topological consistency constraint is applied to the continuous semantic topology field to generate a continuous space occupancy representation.

[0100] Specifically, the relation edges in the semantic topology graph are mapped to a continuous topological association strength distribution in three-dimensional space. A continuous semantic topology field is generated based on the continuous topological association strength distribution, and topological consistency constraints are applied to the continuous semantic topology field to generate a continuous space occupancy representation, including:

[0101] Determine the spatial center of action of relation edges in the semantic topology graph, and substitute the spatial center of action, spatial influence range parameters and spatial location into the continuous influence function to calculate the influence intensity, thereby obtaining the continuous topology association intensity distribution;

[0102] Based on the weights of relation edges and the encoding of relation types in the semantic topology graph, and combined with the spatial field superposition and aggregation of the continuous topology association strength distribution, a continuous semantic topology field is obtained.

[0103] The continuous semantic topological field is converted into a scalar topological intensity, and then weighted and fused with spatial density and semantic entity response values ​​of semantic entity instances to obtain a continuous spatial occupancy representation.

[0104] Specifically, based on the three-dimensional spatial location, relation type, and relation edge weight of each semantic entity instance in the semantic topology graph, a continuous semantic topology field is generated, which transforms the discrete environmental entity semantic relations into a continuous topological association strength distribution in three-dimensional space. Based on the continuous semantic topology field, topological consistency constraints are applied to the spatial response region of the target semantic entity to generate the corresponding continuous spatial occupancy representation.

[0105] The continuous semantic topological field is mapped into a parameterized spatial constraint representation for workpiece six-degree-of-freedom pose optimization, and the spatial response distribution of the target semantic entity is obtained by combining open vocabulary semantic query. The spatial response distribution is subjected to topological consistency constraint based on the continuous semantic topological field to generate semantically consistent spatial regions and continuous spatial occupancy representations to characterize the spatial occupancy, boundary relationship and physical constraint relationship between the target workpiece and the environmental entity.

[0106] Generating a continuous semantic topological field and forming a continuous spatial occupancy representation includes the following steps:

[0107] To enable discrete semantic entity relations to participate in pose optimization in continuous space, relation edges in the semantic topology graph are mapped to a continuous topological association strength distribution in three-dimensional space, generating a continuous semantic topology field.

[0108] In one implementation, a continuous semantic topological field is represented as:

[0109] ;

[0110] in, Indicates spatial location Continuous semantic topological field at; Represents the set of relation edges in a semantic topology graph; Indicates the weight of the relation edge; Represents the relationship edge (i,j) with respect to spatial location. Continuous influence function; This indicates the relation type encoding or relation direction encoding. When... When encoding multidimensional relations, Let it be a vector-valued field. To use continuous semantic topological fields for continuous spatial occupancy representations, we can... This is converted to scalar topological strength τ(p). In one implementation, τ(p) can be expressed as: Or it can be expressed as: , where u represents the pre-set or learned relation type weight vector.

[0111] Continuous influence functions, which can be Gaussian functions, radial basis functions, truncation distance functions, signed distance functions, voxel interpolation functions, or neural implicit functions, are used to characterize the strength of the influence of relation edges on spatial location. In one implementation, a Gaussian spatial influence function is used, expressed as:

[0112] ;

[0113] in, Let (i,j) represent the spatial center of action corresponding to the relation edge (i,j). This parameter represents the spatial influence range of the relation edge (i,j). The closer the spatial position p is to the spatial influence region corresponding to the relation edge, the greater the influence range. The larger the value of , the farther the spatial location p is from the spatial region of influence. The value of gradually decreases.

[0114] Spatial center of action The relationship edge (i,j) can be determined based on the spatial positions of the two semantic entity instances connected by it. In one implementation, if the relationship edge represents an adjacency relationship, a non-collision constraint relationship, or a relative orientation relationship, then... The midpoint of the center points of two semantic entity instances can be taken; if the relation edge represents a supporting relation, then... The center of the contact area between the bottom region of the first semantic entity and the top surface of the second semantic entity can be taken; if the relation edge represents an occlusion relationship, then... The center position of the overlapping area of ​​two semantic entities projected in the corresponding observation direction can be obtained by backprojecting it into three-dimensional space.

[0115] Furthermore, a continuous spatial occupancy representation is generated based on the continuous semantic topology field, spatial density field, and the occupancy region of semantic entity instances. This continuous spatial occupancy representation characterizes the occupancy probability of environmental entities in three-dimensional space and is used to subsequently determine whether there is a spatial conflict between the target workpiece and the environmental entities. When used for target workpiece pose optimization, the continuous spatial occupancy representation is preferably generated from environmental entities outside the target workpiece, or the occupancy region corresponding to the target workpiece itself is excluded when calculating the conflict cost.

[0116] In one implementation, contiguous space occupancy is represented as:

[0117] ;

[0118] in, σ(p) represents the probability of continuous spatial occupancy at spatial location p; σ(p) represents the spatial density; R(p) represents the semantic entity response value at spatial location p, which can be the semantic confidence of the semantic entity instance at spatial location p, the instance belonging probability, the maximum semantic response value, or the environmental entity category response value. Represents a continuous semantic topological field The scalar topology strength obtained from the transformation; a, b, and c represent the fusion weight coefficients, which can be set empirically, determined by calibration data, or learned through the training process.

[0119] Through this step, discrete semantic topological relations are transformed into a topological constraint distribution that can be continuously sampled in three-dimensional space, forming a continuous spatial occupancy representation that can be used for spatial conflict judgment.

[0120] The initial six-degree-of-freedom pose is solved based on the current observed image and the measurable 3D model of the target workpiece. A joint pose optimization objective is constructed by combining the continuous semantic topological field and the continuous spatial occupancy representation. The initial six-degree-of-freedom pose is optimized based on the joint pose optimization objective to obtain the optimized six-degree-of-freedom pose of the target workpiece.

[0121] Specifically, the initial six-DOF pose is solved based on the current observed image and the measurable 3D model of the target workpiece. A joint pose optimization objective is then constructed by combining continuous semantic topological fields and continuous spatial occupancy representations. Based on this joint pose optimization objective, the initial six-DOF pose is optimized to obtain the optimized six-DOF pose of the target workpiece, including:

[0122] Acquire the current observation image of the workpiece to be tested, and input the current observation image into the measurable three-dimensional model of the target workpiece to solve the initial six-degree-of-freedom pose of the target workpiece;

[0123] The continuous semantic topological field is mapped to a parameterized spatial constraint representation for pose optimization, and the spatial constraint cost between the target workpiece and the environment entity is calculated based on the parameterized spatial constraint representation.

[0124] Specifically, mapping a continuous semantic topological field to a parameterized spatial constraint representation for pose optimization includes:

[0125] Based on the continuous semantic topological field, the relation edges, relation type codes, relation edge weights, and spatial influence functions of the target workpiece are extracted, and the original feature set of topological association is obtained by summarizing them.

[0126] Based on the original feature set of topological association, the constraint objects between the target workpiece and the environmental entity are determined, and the constraint parameters corresponding to the constraint objects are extracted. A parameterized spatial constraint representation is generated based on the constraint parameters.

[0127] The constraint objects include at least one of the following: supporting entities, adjacent entities, occluding entities, non-collision entities, and relative orientation reference entities;

[0128] The constraint parameters include at least one of the following: support surface height, support surface normal, distance between adjacent boundaries, occlusion order, safety distance threshold, relative direction vector, and relation edge weight.

[0129] The six-degree-of-freedom pose of the target workpiece is used as the optimization variable, and a joint pose optimization objective is constructed by combining continuous space occupancy representation and space constraint cost.

[0130] The topology consistency deviation is calculated based on the initial six-DOF pose. The parameterized spatial constraint representation is updated according to the topology consistency deviation to obtain the updated parameterized spatial constraint representation. The initial six-DOF pose is iteratively optimized based on the updated parameterized spatial constraint representation and the joint pose optimization objective to obtain the optimized target workpiece six-DOF pose.

[0131] Specifically, the topology consistency deviation is calculated based on the initial six-DOF pose. The parameterized spatial constraint representation is then updated according to the topology consistency deviation to obtain the updated parameterized spatial constraint representation. Finally, the initial six-DOF pose is iteratively optimized based on the updated parameterized spatial constraint representation and the joint pose optimization objective to obtain the optimized target workpiece six-DOF pose, including:

[0132] The measurable 3D model of the target workpiece is transformed into the scene coordinate system according to the initial six-degree-of-freedom pose, and the current spatial relationship between the target workpiece and the environmental entities is calculated. The current spatial relationship is compared with the reference topology state to obtain the topology consistency deviation.

[0133] The relation edge weights in the semantic topology graph are updated based on the topology consistency deviation, and the updated relation edge weights are normalized to obtain the updated relation edge weights.

[0134] The continuous semantic topology is updated based on the updated relation edge weights to obtain the updated continuous semantic topology. The parameterized space constraint representation is then updated using the updated continuous semantic topology to obtain the updated parameterized space constraint representation.

[0135] The joint pose optimization objective is recalculated based on the updated parameterized space constraint representation to obtain the updated objective result. The initial six-DOF pose is then iteratively optimized based on the updated objective result until convergence, resulting in the optimized target workpiece six-DOF pose.

[0136] Specifically, the initial six-degree-of-freedom pose of the target workpiece is solved based on the current observed image of the workpiece to be tested, the target region extraction result, and the measurable three-dimensional model of the target workpiece, and the measurable three-dimensional model of the target workpiece is transformed into the scene space corresponding to the initial six-degree-of-freedom pose.

[0137] The continuous semantic topology field is mapped to a parameterized spatial constraint representation for pose optimization. The spatial constraint cost between the target workpiece and the environment entity is calculated based on the parameterized spatial constraint representation, and the pose optimization target is constructed by combining the visual reprojection error. During the pose optimization process, the relation edge weights in the semantic topology graph are updated based on the topological consistency deviation between the spatial relationship between the target workpiece and the environment entity and the reference topology state. The continuous semantic topology field and parameterized spatial constraint representation are regenerated based on the updated relation edge weights to obtain the six-degree-of-freedom pose of the target workpiece through iterative optimization.

[0138] Specifically, the spatial constraint cost between the target workpiece and the environmental entity is calculated based on the parameterized spatial constraint representation, and a joint pose optimization objective is constructed by combining visual reprojection error, topological consistency cost, space occupancy conflict cost and geometric relationship cost.

[0139] During pose optimization, this invention updates the weights of relational edges in the semantic topology graph based on the topological consistency deviation caused by changes in the spatial relationship between the target workpiece and the environmental entities. It then regenerates the continuous semantic topology field based on the updated relational edge weights, thereby updating the parameterized spatial constraint representation so that the updated spatial constraints can participate in the next round of pose optimization. Through the above feedback iteration process, the joint convergence of the target workpiece pose parameters, the continuous semantic topology field, and the parameterized spatial constraints is achieved, resulting in the optimized six-degree-of-freedom pose of the target workpiece.

[0140] The feedback update includes: updating the relation edge weights in the semantic topology graph based on the topology consistency deviation caused by the change in the spatial relationship between the target workpiece and the environmental entity during the pose optimization process; regenerating the continuous semantic topology field based on the updated relation edge weights; updating the parameterized spatial constraint representation based on the regenerated continuous semantic topology field; and using the updated parameterized spatial constraint representation again in the next round of pose optimization. The feedback update is performed iteratively and stops when at least one of the following conditions is met: the change in pose parameters is less than a preset threshold, the topology consistency deviation is less than a preset threshold, the change in the pose optimization target is less than a preset threshold, or the number of iterations reaches a preset upper limit.

[0141] The parameterized spatial constraint representation is obtained by mapping a continuous semantic topological field and is used to describe at least one of the support constraints, adjacency constraints, occlusion constraints, non-collision constraints, or relative orientation constraints between the target workpiece and environmental entities. The continuous spatial occupancy representation is generated jointly based on the spatial response region of the target semantic entity and the continuous semantic topological field, and is used to characterize the continuous occupancy probability distribution of the target semantic entity in three-dimensional space. During pose optimization, the spatial constraint cost includes the topological consistency cost determined by the continuous semantic topological field, the spatial occupancy conflict cost determined by the continuous spatial occupancy representation, and the geometric relationship cost determined by the parameterized spatial constraint representation. Based on the joint pose optimization objective, the optimized six-DOF pose of the target workpiece is output.

[0142] Mapping a continuous semantic topological field to a parameterized spatial constraint representation includes the following steps:

[0143] like Figure 4As shown, after generating the continuous semantic topology field, it is mapped to parameterized spatial constraints for workpiece six-DOF pose optimization. The mapping process includes: determining the supporting entities, adjacent entities, occluding entities, non-collision entities, and relative orientation reference entities between the target workpiece and environmental entities based on the relation edges, relation type encoding, relation edge weights, and spatial influence functions related to the target workpiece in the continuous semantic topology field, i.e., the original feature set of topological associations, and extracting the corresponding constraint parameters. The parameterized spatial constraints represent the spatial relation constraints that the target workpiece should satisfy in the current scene. The constraint parameters include at least one of the following: support surface height, support surface normal, adjacent boundary distance, occlusion order quantity, safety distance threshold, relative direction vector, and corresponding relation edge weight. Specifically, the mapping and pose optimization process from continuous semantic topology field to parameterized spatial constraints includes: inputting a continuous semantic topology field containing the target workpiece, adjacent workpieces, fixtures, and worktable, which possesses continuous topological association strength and semantic entity spatial relationships that can be continuously queried in three-dimensional space; through the parameterized spatial constraint generation stage, support constraints are generated to limit the height relationship between the bottom and the support surface, adjacency constraints to limit the distance between entity boundaries, occlusion constraints to maintain the projection occlusion order, non-collision constraints to limit space occupancy conflicts, and constraints to limit relative orientation relationships, i.e., relative orientation constraints about front, back, left, and right; then, joint pose optimization is performed, i.e., combining the current observed image and the real-scale three-dimensional model of the workpiece, introducing visual observation consistency, topology consistency cost, space occupancy conflict cost, and geometric relationship cost, to jointly optimize the six-degree-of-freedom pose of the workpiece, and outputting the optimized six-degree-of-freedom pose including rotation and translation parameters; at the same time, the continuous semantic topology field and parameterized spatial constraints are updated based on the topology consistency deviation. Figure 4 The blue arrows represent the observation or model data flow, the green arrows represent the topological field and spatial constraint information flow, and the red dashed arrows represent the feedback update flow. This continuous semantic topological field is not only used as a semantic label, but is mapped to parameterized spatial constraints such as support, adjacency, occlusion, non-collision, and relative orientation, and participates in the six-degree-of-freedom pose optimization of the workpiece together with visual observation.

[0144] In one implementation, the parameterized space constraint representation includes:

[0145] ;

[0146] Each type of parameterized space constraint can be represented as a structure consisting of constraint objects, constraint parameters, constraint weights, and constraint cost functions:

[0147] ;

[0148] in, Indicates support constraints. Indicates adjacency constraints. Indicates occlusion constraint. Indicates non-collision constraints. Indicates relative orientation constraints. and This represents the semantic entity instances involved in the constraint. Indicates the corresponding constraint parameters. This represents the weight of relation edges determined by a semantic topological graph or a continuous semantic topological field. This represents the corresponding constraint cost function, where m represents the type of constraint: support, adjacency, occlusion, non-collision, or relative orientation.

[0149] Support constraints are used to limit the height relationship between the bottom of the target workpiece and the upper surface of the supporting entity; adjacency constraints are used to limit the boundary distance between the target workpiece and adjacent environmental entities; occlusion constraints are used to limit the front and rear occlusion order between the target workpiece and the environmental entities; non-collision constraints are used to prevent spatial penetration between the target workpiece and the environmental entities; relative orientation constraints are used to limit the relative orientation relationship between the target workpiece and the environmental entities.

[0150] In one implementation, the cost of the non-collision constraint is expressed as:

[0151] ;

[0152] in, This indicates the pose of the 3D model of the target workpiece. The set of sampling points below; Indicates sampling point The distance to the area occupied by the environmental entity or the boundary of the environmental entity; This indicates the safe distance threshold. It can be represented by contiguous space occupancy. It can be obtained through conversion, or calculated from environmental entity point clouds, mesh models, voxel occupancy maps, or symbolic distance fields.

[0153] Through this step, the continuous semantic topological field is no longer used only to describe scene relationships, but is transformed into a computable spatial constraint term, providing constraints for subsequent pose optimization.

[0154] Solving for the initial pose and performing joint pose optimization based on feedback constraints includes the following steps:

[0155] The initial six-DOF pose of the target workpiece is determined based on the current observed image of the workpiece and the 3D model of the target workpiece. The initial six-DOF pose can be obtained through PnP, ICP, template matching, feature matching, edge matching, deep point cloud registration, or learned pose regression. The target workpiece's six-DOF pose includes 3D rotation and translation parameters. Based on the initial pose, the 3D model of the target workpiece is transformed into the scene coordinate system, and the optimized rotation and translation parameters are output after optimization.

[0156] After obtaining the initial pose, a joint pose optimization objective is constructed using the six-DOF pose T of the target workpiece as the optimization variable. The joint pose optimization objective includes visual reprojection error, semantic topological consistency cost, spatial occupancy conflict cost, and geometric relation cost.

[0157] ;

[0158] in, This represents the joint pose optimization objective; Indicates visual reprojection error; Indicates the cost of semantic topological consistency; Indicates the cost of space occupancy conflicts; Represents the cost of geometric relationships; These represent the weighting coefficients for topological consistency cost, space occupancy conflict cost, and geometric relation cost, respectively. Space occupancy conflict cost By posing the 3D model of the target workpiece Map the surface points, voxel points, or sampling points to the scene coordinate system, and query the continuous spatial occupancy probability at the corresponding spatial location. Or non-collision constraint cost get.

[0159] Among them, visual reprojection error is used to constrain the projection result of the target workpiece's 3D model to be consistent with the target region, edge, key point or contour in the currently observed image; semantic topological consistency cost is used to constrain the relationship features between the target workpiece and the environmental entity to not deviate from the reference topological state; spatial occupancy conflict cost is used to constrain the target workpiece to not enter the high occupancy area of ​​the environmental entity; geometric relationship cost is used to constrain the local geometric contact, boundary distance, normal consistency or point cloud registration residual between the target workpiece and the environmental entity.

[0160] After obtaining the initial six-DOF pose and parametric spatial constraints of the target workpiece, the 3D model of the target workpiece is transformed into the scene coordinate system according to the current pose, and the current spatial relationship between the target workpiece and the environment entities is calculated. The current spatial relationship is compared with the reference topology state to obtain the topology consistency deviation. The topology consistency deviation can be expressed as:

[0161] ;

[0162] in, This represents the topological consistency deviation of the relation edge (i,j) at the k-th iteration; This represents the relational features in the current iteration state; Represents the relational features in the reference topological state. Relational features in the current iteration state. This can include at least one of the following: relative position between the target workpiece and the environmental entity at the current pose, support height relationship, occlusion sequence relationship, non-collision gap or overlap, and relative orientation relationship, and is related to the reference topology state. The corresponding quantities in the data are compared.

[0163] The weights of relation edges in the semantic topology graph are updated based on topological consistency deviation. In one implementation, the relation edge weights are updated as follows:

[0164] ;

[0165] in, This represents the updated relation edge weights; Indicates the current relation edge weight; Indicates the deviation suppression coefficient; Indicates the observation enhancement coefficient; This represents the credibility score of the relation edge (i,j) in the current observation. The weights can be determined based on at least one of the following: visibility of the entity corresponding to the relation edge in the current observation and multi-view observation; consistent segmentation confidence; depth consistency; semantic matching score; or edge matching score. The preferred value range is [0,1]. To prevent the relation edge weights from growing or decaying unbounded during iteration, the updated relation edge weights can be further normalized or truncated to a preset interval. .

[0166] When a relation edge produces a large topological consistency deviation in the current pose, the relation edge weight is reduced to decrease the impact of erroneous semantic relations on pose optimization; when a relation edge remains consistent in multi-view observations and the current pose state, the relation edge weight is increased to enhance the constraint effect of stable semantic relations on pose optimization.

[0167] The continuous semantic topology is regenerated based on the updated relation edge weights, and the parameterized spatial constraint representation is further updated. The updated parameterized spatial constraints then participate in the next round of pose optimization. Through the above feedback iteration, the target workpiece pose parameters, the continuous semantic topology, and the parameterized spatial constraints are coupled together and gradually converge during the optimization process.

[0168] In one implementation, such as Figure 6 As shown, the feedback iterative optimization steps based on topology consistency deviation include: 1. Inputting the current six-degree-of-freedom pose of the workpiece and transforming the workpiece's 3D model to the scene space; 2. Calculating the current spatial relationships between the target workpiece and environmental entities, such as support, adjacency, occlusion, non-collision, and relative orientation; 3. Comparing it with the reference topology state to complete the topology consistency evaluation and calculating the topology consistency deviation; 4. Updating the relation edge weights based on the deviation, i.e., enhancing stable relationships and suppressing abnormal relationships, and updating the corresponding relation edge weights; 5. Regenerating the continuous semantic topology field based on the updated relation edge weights. To complete the continuous semantic topology field update, 6. Perform parameterized spatial constraint update, updating support, adjacency, occlusion, non-collision and relative orientation constraints. 7. Carry out joint pose optimization, combining visual observation consistency, topology consistency, space occupancy conflict and geometric relationship cost to update the workpiece's six-degree-of-freedom pose. If the stopping conditions of pose change less than threshold, topology consistency deviation less than threshold, optimization target change less than threshold, and reaching the upper limit of iteration number are not met, then enter the next iteration loop. When the stopping conditions are met, output the optimized workpiece six-degree-of-freedom pose including rotation and translation parameters. Figure 6 The green solid arrows represent the optimization process, the red dashed arrows represent feedback updates, and the blue solid arrows represent result outputs. Throughout the feedback iteration process, the relation edge weights, continuous semantic topology fields, and parameterized spatial constraints are dynamically updated based on the deviation between the spatial relations and the reference topology state under the current pose, allowing the spatial constraints to participate in the next round of pose optimization.

[0169] Outputting the optimized six-DOF pose of the target workpiece includes the following steps:

[0170] Once the stopping condition is met, the optimized six-DOF pose of the target workpiece is output:

[0171] ;

[0172] in, This represents the optimized six-DOF pose of the target workpiece. This represents the optimized rotation matrix. This represents the optimized translation vector.

[0173] The output six-DOF pose of the target workpiece can be used for grasping planning, motion control, assembly positioning, inspection and measurement, or workpiece state recognition of industrial robots.

[0174] In this embodiment, the spatial relationships of semantic entities in the scene are first constructed into a semantic topology graph, then continuousized into a continuous semantic topology field, and mapped to parameterized spatial constraints in pose optimization. During pose optimization, the weights of the relationship edges are updated based on the topology consistency deviation, and the continuous semantic topology field and parameterized spatial constraints are regenerated based on the updated relationship edge weights. Therefore, this embodiment can improve the accuracy, stability, and physical consistency of the six-degree-of-freedom pose estimation of the target workpiece when it has occlusion, weak texture, reflection, or background interference.

[0175] According to another embodiment of the present invention, a six-degree-of-freedom pose estimation system for a workpiece based on continuous semantic topological field feedback constraints is provided, the system comprising:

[0176] The continuous field representation model construction module is used to acquire reference image data of the target workpiece, construct a measurable 3D model of the target workpiece based on the reference image data, and construct an enhanced continuous field representation model based on multi-view observation data.

[0177] The semantic topology graph construction module is used to extract a set of semantic entity instances based on the enhanced continuous field representation model, and to construct a semantic topology graph based on the set of semantic entity instances.

[0178] The continuous semantic topology field generation module is used to map the relation edges in the semantic topology graph to a continuous topological association strength distribution in three-dimensional space, generate a continuous semantic topology field based on the continuous topological association strength distribution, and apply topological consistency constraints to the continuous semantic topology field to generate a continuous space occupancy representation.

[0179] The joint pose optimization module is used to solve the initial six-DOF pose based on the current observed image and the measurable target workpiece 3D model, and to construct the joint pose optimization objective by combining the continuous semantic topological field and the continuous spatial occupancy representation. Based on the joint pose optimization objective, the initial six-DOF pose is optimized to obtain the optimized target workpiece six-DOF pose.

[0180] In summary, this invention aims to address the problems in existing six-DOF pose estimation techniques for workpieces, which mainly rely on the visual or geometric features of the target workpiece itself for pose estimation. These techniques lack effective utilization of the semantic information, spatial relationships, and physical constraints of environmental entities in complex industrial scenarios, resulting in insufficient stability of pose estimation, poor physical consistency, and high deployment costs for new workpieces under conditions of occlusion, reflection, weak texture, and background interference.

[0181] Specifically, such as Figure 2As shown, the workpiece six-DOF pose estimation based on continuous semantic topological field feedback constraints can be divided into two stages: scene continuous representation and continuous semantic topological field construction, and online pose estimation and feedback constraint optimization. The scene continuous representation and continuous semantic topological field construction stage includes the following steps: 1. Data acquisition, i.e., acquiring the target workpiece reference image, scene multi-view video data, and the currently observed image or depth image; 2. Real-scale 3D model construction, i.e., through 3D reconstruction, CAD matching, template fitting, and scale consistency correction, outputting a real physical scale 3D model of the workpiece; 3. Semantically enhanced continuous field representation construction, i.e., jointly representing spatial density, color information, and semantic features, outputting a semantically enhanced continuous 3D model. 4. Cross-view consistency optimization, which ensures consistency from three aspects: color, density, and semantic features, and improves the stability of the continuous field; 5. Semantic entity instance and topology graph construction, which extracts semantic entity instances and constructs an entity space relationship graph. For example, based on the target workpiece, workbench, fixture, material frame and adjacent workpieces contained in the semantic entity instance, and combined with the support relationship, adjacency relationship, occlusion relationship, non-collision relationship and relative orientation relationship between semantic entity instances, a semantic topology graph is output; 6. Continuous semantic topology field generation, which performs continuous field processing on discrete relationship edges, generates topological association strength distribution, and outputs a continuous semantic topology field; Enter the online pose estimation and feedback constraint optimization stage. The online pose estimation and feedback constraint optimization stage includes the following steps: 7. Current observation input, i.e., inputting the real-time image and depth information of the current scene to be measured; 8. Target region extraction, i.e., performing target workpiece detection, selecting the target workpiece in the image, and then obtaining the corresponding region mask, contour and key points, and outputting the target observation region; 9. Initial six-degree-of-freedom pose solution, i.e., can be obtained through PnP, ICP, template matching, feature matching, edge matching, depth point cloud registration, learned pose regression or a combination of multiple methods; the initial six-degree-of-freedom pose is used to provide initial values ​​for subsequent joint pose optimization, and does not limit the pose initialization method of this invention; 10. Parametric spatial constraint generation, i.e., outputting parametric spatial constraints including support constraints, adjacency constraints, occlusion constraints, non-collision constraints and relative orientation constraints. The above two stages, combined with feedback loop optimization and application steps, complete the six-DOF pose estimation of the workpiece. The feedback loop optimization and application steps include: 11. Joint pose optimization and feedback update, i.e., combining visual observation consistency, topological consistency cost, space occupancy conflict cost, and geometric relationship cost to conduct feedback loop optimization, which can update relational edge weights, continuous semantic topological fields, and spatial constraints based on topological consistency deviations; 12. Outputting the six-DOF pose and application, outputting the optimized six-DOF pose, which includes rotation and translation parameters, and can be applied to robot grasping, assembly positioning, and inspection and measurement. Figure 2The blue solid arrows represent data flow, the green solid arrows represent semantic or model information flow, and the red dashed arrows represent feedback constraint update flow. Its core is to transform the spatial relationship of semantic entities into a continuous semantic topological field and map it into parameterized spatial constraints to participate in the six-degree-of-freedom pose optimization of the workpiece. During the optimization process, the topological field and spatial constraints are updated based on the topological consistency deviation feedback, thereby improving the accuracy, stability and physical consistency of pose estimation.

[0182] like Figure 5 As shown, in practical applications, this invention comprises three main parts: an input unit, a core processing unit, and an output and application unit. The input content of the input unit includes three types of inputs: 1. a reference image of the target workpiece; 2. multi-view video data of the scene; and 3. the currently observed image or depth image.

[0183] The core processing units include: 1. A data acquisition module, used to acquire reference image data of the target workpiece, multi-view video data of the scene to be estimated, and the current observation image of the workpiece. 2. A 3D model construction module, used to construct a 3D model of the target workpiece based on the reference image data, and to perform scale consistency correction on the 3D model of the target workpiece according to the known actual size parameters of the target workpiece to obtain a 3D model of the workpiece with real physical scale. 3. A continuous field construction module, used to construct a semantically enhanced continuous field representation model based on the multi-view video data of the scene to be estimated, and to optimize the continuous field representation model through cross-view consistency constraints so that it represents the spatial density, color radiation information, and semantic feature information of the scene in a unified 3D spatial coordinate system. 4. A semantic topological field generation module, used to extract semantic entity instances based on the semantic feature information in the continuous field representation model, construct a semantic topological graph representing the spatial relationship between semantic entity instances, and map the discrete relation edges in the semantic topological graph to a continuous topological association strength distribution in 3D space to generate a continuous semantic topological field. 5. Spatial Constraint Generation Module: This module maps the continuous semantic topological field into a parameterized spatial constraint representation for workpiece six-DOF pose optimization, and generates a continuous spatial occupancy representation based on spatial density response, semantic entity response, and continuous semantic topological field intensity. 6. Pose Initialization Module: This module solves for the initial six-DOF pose of the target workpiece based on the current observed image of the workpiece and a 3D model of the target workpiece with real physical scale, and transforms the 3D model of the target workpiece into the scene space according to the initial six-DOF pose. 7. Feedback Optimization Module: This module constructs a joint pose optimization objective based on visual reprojection error, semantic topological consistency cost, spatial occupancy conflict cost, and geometric relationship cost. During pose optimization, it updates the relation edge weights, continuous semantic topological field, and parameterized spatial constraint representation in the semantic topological graph based on the topological consistency deviation caused by changes in the spatial relationship between the target workpiece and environmental entities, allowing the updated spatial constraints to participate in the next round of pose optimization. 8. Pose Output Module: This module outputs the optimized six-DOF pose of the target workpiece after a preset stopping condition is met. Outputs and applications include: 1. Outputting the optimized six-DOF pose of the target workpiece; 2. Using the six-DOF pose of the target workpiece for the grasping or handling planning of industrial robots, i.e., realizing the motion control of industrial robots; 3. Using it for assembly positioning or inspection and measurement, i.e., for workpiece state recognition. Figure 5 The blue solid arrows represent the data flow, the green solid arrows represent the model or semantic information flow, and the red dashed arrows represent the feedback update flow, which is used to update the relation edge weights, continuous semantic topology fields, and spatial constraints. That is, through continuous field construction, semantic topology field generation, spatial constraint mapping, and feedback optimization, stable estimation of the six-degree-of-freedom pose of a workpiece in complex industrial scenarios is achieved.

[0184] This invention can be applied to industrial robot grasping scenarios, specifically where an industrial robot needs to grasp a target workpiece from a worktable. The worktable contains fixtures, adjacent workpieces, and partial obstructions. The target workpiece may be partially obstructed by the fixtures or adjacent workpieces, resulting in incomplete edges in a single-frame image. The initial pose obtained directly based on image features, template matching, or point cloud registration is prone to shifting, and situations may arise where the target workpiece penetrates the worktable, interferes with the fixtures, or is suspended relative to the support surface.

[0185] When using the method of this embodiment, multi-view RGB-D video data including the worktable, fixture, adjacent workpieces and target workpiece are acquired by a fixed camera, a robotic arm end-effector camera or an RGB-D camera. The video data includes continuous RGB image sequences, depth image sequences, camera intrinsic and extrinsic parameters and spatial pose information at the corresponding viewpoints. The acquired video data is processed by camera calibration, time synchronization, depth denoising and spatial coordinate unification, and the color information, depth information and spatial pose information at different viewpoints are mapped to a unified three-dimensional coordinate system.

[0186] Based on multi-view observation data in a unified spatial coordinate system, point cloud fusion and spatial sampling are performed on RGB-D data to construct a semantically enhanced continuous field representation model. The spatial density response, color response, and semantic feature response of the scene are obtained through spatial sampling. Furthermore, environmental entity instances are extracted based on semantic response, spatial connectivity, and three-dimensional spatial adjacency relationships, and a semantic topology graph containing support, adjacency, occlusion, and non-collision relationships is constructed.

[0187] In actual grasping tasks, the consistency of the spatial relationship between the target workpiece pose and environmental entities is detected during the optimization process. This identifies physical inconsistencies such as spatial penetration, suspension, abnormal occlusion order, and relative orientation deviation. Based on the detection results, the continuous semantic topology field feedback constraints are adjusted. Specifically, the semantic entity instance corresponding to the target workpiece is used as the target node. In the semantic topology graph, a support relationship is formed between the target workpiece and the worktable, an adjacency relationship and a non-collision constraint relationship is formed between the target workpiece and the fixture, and an occlusion relationship and a relative orientation relationship are formed between the target workpiece and adjacent workpieces or occluders.

[0188] Furthermore, based on the relation edges, relation edge weights, relation type encodings, and spatial influence functions in the semantic topology graph, a continuous semantic topology field is generated, and this continuous semantic topology field is mapped to parameterized spatial constraints for six-degree-of-freedom pose optimization of the target workpiece. The parameterized spatial constraints include at least one of the following: support constraints, adjacency constraints, occlusion constraints, non-collision constraints, and relative orientation constraints.

[0189] During pose optimization, the cost of support constraints and space occupation conflicts increases when the initial pose causes the bottom of the target workpiece to be lower than the surface of the worktable; the cost of non-collision constraints increases when the initial pose causes spatial penetration between the target workpiece and the fixture or insufficient safety clearance; and the cost of semantic topological consistency increases when the initial pose causes the occlusion order between the target workpiece and occluders to be inconsistent with the multi-view observation results. Through joint optimization, the pose of the target workpiece is adjusted to a position that simultaneously satisfies the current image observation, support relationship, non-collision constraint relationship, occlusion relationship, and relative orientation relationship.

[0190] During the feedback iteration process, if a semantic relation experiences a significant topological consistency deviation due to occlusion, reflection, segmentation errors, or instance misidentification (e.g., misidentifying a fixture as an adjacent workpiece or misjudging a temporary occluder as a stable environmental entity), the weight of the corresponding relation edge is reduced based on the topological consistency deviation. A continuous semantic topological field and parameterized spatial constraints are then regenerated based on the updated relation edge weights, thereby mitigating the impact of unstable semantic relations on pose optimization results. Conversely, if a semantic relation remains consistent throughout multi-view observations and continuous iterations, the weight of the corresponding relation edge is increased to enhance the constraining effect of stable spatial relations on the target workpiece pose optimization.

[0191] The final output of the target workpiece's six-degree-of-freedom pose can simultaneously satisfy visual observation consistency, semantic topological consistency, and spatial physical constraints, thereby enhancing the stability and spatial physical consistency of the industrial robot's grasping and positioning in scenarios with occlusion, weak texture, reflection, or complex background interference.

[0192] To verify the effectiveness of the method in this embodiment, multi-view RGB-D operational data including the target workpiece, worktable, fixture, and occlusions were collected in an industrial robot grasping scenario. A visual pose estimation method without continuous semantic topological field constraints and a pose estimation method incorporating the method of this embodiment were compared. The physical consistency of the method was verified by statistically analyzing spatial penetration, suspension, consistency of occlusion order, and consistency of relative orientation during the pose estimation process.

[0193] In one implementation, 50 sets of industrial robot grasping tasks were selected as test samples, including different target workpiece poses, different occlusion levels, and different environmental entity layouts. Pose estimation was performed using both the method without continuous semantic topological field constraints and the method of this embodiment, and the consistency of the spatial relationship between the target workpiece and environmental entities after optimization was statistically analyzed. The test data included RGB-D data collected under different types of industrial workpieces, different occlusion levels, and different environmental entity layouts, where the target workpiece occlusion ratio covered no occlusion, partial occlusion, and multi-entity occlusion scenarios. The test results are shown in Table 1 below:

[0194] Table 1. Test results of pose estimation using different methods.

[0195] The test results above show that after introducing continuous semantic topological field feedback constraints, the spatial relationship anomalies generated during the target workpiece pose optimization process are significantly reduced.

[0196] Experimental results show that after introducing continuous semantic topological field feedback constraints, the spatial relationship between environmental entities can be used to constrain the pose optimization process, and the interference of unstable semantic relationships on the pose optimization process can be suppressed by dynamically adjusting the weight of relation edges. At the same time, under the condition that the target workpiece has occlusion, weak texture, reflection and complex background interference, the occurrence of spatial penetration, suspension and abnormal occlusion order is reduced, and the stability and physical consistency of the six-degree-of-freedom pose estimation results are improved.

[0197] Furthermore, since this embodiment constructs a continuous semantic topological field through multi-view observation and uses semantic relationship constraints to assist in pose optimization, it reduces the need to rebuild a dedicated pose optimization model for different new workpieces. It only needs to establish corresponding semantic entity relationship constraints based on actual observation data, thereby reducing the dependence on dedicated training data for specific workpieces and improving the application adaptability in scenarios with new workpieces or small batches of workpieces.

[0198] To facilitate implementation, this embodiment provides optional settings for some key parameters. Each parameter can be adjusted according to the scene scale, target workpiece size, camera accuracy, occlusion level, and industrial operation accuracy requirements. Preferably, before setting each weight coefficient, the corresponding loss or constraint terms are normalized.

[0199] When constructing a semantically enhanced continuous field representation model, the weights of the color response term in the cross-view consistency loss can be set from 0.5 to 2.0, the weights of the spatial density term can be set from 0.1 to 1.0, and the weights of the semantic feature term can be set from 0.5 to 3.0. When there is reflection, illumination variation, or weak texture interference, the weight of the color response term can be reduced, while the weights of the spatial density term or the semantic feature term can be increased.

[0200] The spatial influence range parameter of relation edges when generating continuous semantic topological fields. It can be determined based on semantic entity scale, inter-entity distance, and relationship type. In one implementation, Set to 0.1 to 1.5 times the diagonal length of the bounding box of the relevant semantic entity. The influence range of support relationships is located near the support contact area, the influence range of adjacency relationships and non-collision constraint relationships is located near the entity boundary area, and the influence range of occlusion relationships is determined by the overlapping area of ​​projections along the observation direction.

[0201] When forming a continuous spatial occupancy representation, the occupancy probability threshold used to determine the occupancy area of ​​an environmental entity can be set to 0.3 to 0.8 and adjusted according to sensor noise, occupancy level, and safety clearance requirements.

[0202] When constructing the joint pose optimization objective, the weights for topology consistency cost can be set from 0.1 to 5.0, space occupancy conflict cost can be set from 0.5 to 10.0, and geometric relation cost can be set from 0.5 to 10.0. When the target workpiece is severely occluded or the visual observation is incomplete, the relative weights of topology consistency cost and space occupancy conflict cost can be increased.

[0203] During the feedback update process, the bias suppression coefficient can be set to 0.1 to 5.0, and the observation enhancement coefficient can be set to 0.01 to 1.0. The updated relation edge weights can be normalized to 0 to 1, or truncated to a preset interval. .

[0204] The stopping conditions for feedback iteration can be set according to the job accuracy. In one implementation, the upper limit of the number of iterations is set to 5 to 100, the stopping threshold for pose translation is set to 0.1 mm to 5 mm, and the stopping threshold for pose rotation is set to 0.05° to 2°. When the change in pose parameters, topology consistency deviation, or joint pose optimization target change is less than the corresponding threshold, the iteration stops and the target workpiece's six-degree-of-freedom pose is output. The topology consistency deviation threshold is determined based on the number of semantic entity relations, the stability of relation edge weights, and the complexity of the scene, and is preferably set to 0.01-0.1; the joint pose optimization target change threshold is determined based on the initial value of the optimization objective function, the pose estimation accuracy requirements, and the optimization convergence stability, and is preferably set to 0.001-0.01.

[0205] Without departing from the core concept of constructing a continuous semantic topological field, mapping the continuous semantic topological field to spatial constraints in the six-degree-of-freedom pose optimization of the target workpiece, and updating the topological consistency deviation, the present invention can adopt the following optional implementation methods.

[0206] The continuous field representation model can be implemented using neural radiation field, three-dimensional Gaussian representation, sparse voxel field, hash grid feature field, three-plane feature field, or a combination of the above methods, as long as it can express the spatial density, color information and semantic feature information of the scene in a unified three-dimensional space.

[0207] Semantic features can be obtained from semantic segmentation models, instance segmentation models, open-vocabulary visual models, multimodal visual language models, or semantic models specific to industrial scenarios. Alternatively, they can be determined by manually pre-defined semantic categories and visual feature matching results. Semantic entity instance extraction can be achieved using threshold segmentation, 3D connected component analysis, clustering, instance segmentation, or a combination of these methods.

[0208] The continuous influence function in the continuous semantic topological field can be implemented using Gaussian function, radial basis function, distance transformation function, truncated symbolic distance function, voxel interpolation function or neural implicit function, to map the discrete relation edges in the semantic topological graph to the continuous topological association strength distribution in three-dimensional space.

[0209] Parametric spatial constraints can be implemented using explicit geometric constraints, energy function constraints, probabilistic constraints, differentiable optimization constraints, or a combination of the above. Parametric spatial constraints are used to characterize at least one of the following relationships between the target workpiece and the environmental entity: support, adjacency, occlusion, non-collision, and relative orientation.

[0210] Feedback updates can be implemented using exponential decay updates, moving average updates, Bayesian updates, confidence reweighting, or learned weight updates. The objects of feedback updates include at least one of the following: relation edge weights in the semantic topology graph, continuous semantic topology fields, or parameterized spatial constraints. The update is based on the topological consistency deviation between the spatial relationships of the target workpiece in its current pose and the reference topology state. The updated spatial constraints participate in the next round of pose optimization.

[0211] Pose optimization can be achieved using gradient descent, the Levenberg-Marquardt algorithm, graph optimization, nonlinear least squares, sampling optimization, learned optimizers, or a combination of these methods. The optimization objective should include at least a visual observation consistency term and a spatial constraint term generated from a continuous semantic topological field.

[0212] Through the above optional implementation methods, the present invention can realize the continuous field representation, spatially constrained mapping, and feedback pose optimization of environmental semantic topological relationships without relying on a specific continuous field model, a specific semantic model, or a specific optimization algorithm.

[0213] The technical effects of this invention include at least one of the following: 1. Unifying the expression of multi-view geometric information, color information, and semantic information in a continuous three-dimensional space, improving the completeness of environmental perception in complex industrial scenarios; 2. Transforming discrete semantic relationships such as support, adjacency, occlusion, non-collision, and relative orientation into a continuous semantic topological field, enabling environmental spatial relationships to participate in the six-degree-of-freedom pose optimization of the target workpiece; 3. Generating parameterized spatial constraints through the continuous semantic topological field, ensuring that the pose estimation results simultaneously satisfy visual observation consistency and scene spatial constraints, reducing physical inconsistencies such as spatial penetration, suspension, and incorrect occlusion order; 4. In the pose optimization process... 5. Based on the topological consistency deviation, the relation edge weights, continuous semantic topological fields, or parameterized spatial constraints are dynamically updated to suppress the influence of unstable semantic associations on the pose optimization process; 6. When the target workpiece is occluded, has weak texture, reflects light, or is subject to background interference, the continuous semantic topological field feedback constraints enhance the role of environmental spatial relationships in pose optimization, improving the stability and physical consistency of the workpiece's six-degree-of-freedom pose estimation; 7. By utilizing the semantic entity relationships obtained from multi-view observations to construct a continuous semantic topological field, the dependence on specific workpiece-specific training data is reduced, improving the application adaptability in scenarios with new workpieces or small batches of workpieces.

[0214] This invention can combine or adjust the continuous field model, semantic entity extraction method, topological relationship type, spatial constraint form, and feedback optimization method according to specific industrial scenarios, sensor configurations, target workpiece types, and operational accuracy requirements.

[0215] In summary, this invention includes: acquiring a reference image of the target workpiece, multi-view video data of the scene to be estimated, and the currently observed image; constructing a 3D model of the target workpiece with realistic physical scale; constructing a semantically enhanced continuous field representation model based on the multi-view video data to obtain the spatial density, color information, and semantic feature information of the scene; extracting semantic entity instances based on the semantic feature information, constructing a semantic topology graph representing the support, adjacency, occlusion, non-collision, or relative orientation relationships between entities, and converting the semantic topology graph into a continuous semantic topology field; mapping the continuous semantic topology field to parameterized spatial constraints, and solving the initial six-degree-of-freedom pose by combining the currently observed image and the 3D model of the target workpiece; performing joint optimization based on visual observation consistency, semantic topology consistency, spatial occupancy conflict, and geometric relationship constraints, and updating the continuous semantic topology field and spatial constraints based on topology consistency deviation feedback, outputting the optimized six-degree-of-freedom pose of the workpiece. This invention can improve the accuracy and physical consistency of workpiece pose estimation in scenes with occlusion, weak texture, and complex spatial relationships.

[0216] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for estimating the pose of a workpiece with six degrees of freedom based on feedback constraints of continuous semantic topological fields, characterized in that, The method includes: Acquire reference image data of the target workpiece, construct a measurable 3D model of the target workpiece based on the reference image data, and construct an enhanced continuous field representation model based on multi-view observation data; Extract a set of semantic entity instances based on the enhanced continuous field representation model, and construct a semantic topology graph based on the set of semantic entity instances; The relation edges in the semantic topology graph are mapped to a continuous topological association strength distribution in three-dimensional space. A continuous semantic topology field is generated based on the continuous topological association strength distribution, and a topological consistency constraint is applied to the continuous semantic topology field to generate a continuous space occupancy representation. The initial six-degree-of-freedom pose is solved based on the current observed image and the measurable 3D model of the target workpiece. A joint pose optimization objective is constructed by combining the continuous semantic topological field and the continuous spatial occupancy representation. The initial six-degree-of-freedom pose is optimized based on the joint pose optimization objective to obtain the optimized six-degree-of-freedom pose of the target workpiece.

2. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 1, characterized in that, The construction of a measurable 3D model of the target workpiece based on reference image data, and the construction of an enhanced continuous field representation model based on multi-view observation data, include: Based on the reference image data, a three-dimensional model of the target workpiece is constructed using three-dimensional geometric reconstruction. The three-dimensional model of the target workpiece is then corrected for scale consistency based on the actual size parameters of the target workpiece to obtain the corrected three-dimensional model of the target workpiece. The corrected 3D model of the target workpiece is defined in the target workpiece coordinate system and mapped to the unified scene coordinate system through pose transformation to obtain a measurable 3D model of the target workpiece. Acquire multi-view observation data of the scene to be estimated, construct a continuous field representation model based on the multi-view observation data, and optimize the continuous field representation model to obtain an enhanced continuous field representation model.

3. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 2, characterized in that, The continuous field representation model constructed based on multi-view observation data, and the continuous field representation model optimized to obtain the enhanced continuous field representation model, includes: Based on multi-view observation data, a continuous field representation model is constructed using parameterized approximation operations of a three-dimensional continuous field; The continuous field representation model includes: spatial density field, color radiation field, and semantic feature field; By utilizing cross-view consistency loss, the continuous field representation model is optimized for cross-view consistency, resulting in an enhanced continuous field representation model.

4. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 1, characterized in that, The step of extracting a set of semantic entity instances based on the enhanced continuous field representation model and constructing a semantic topology graph based on the set of semantic entity instances includes: The three-dimensional semantic feature vector output by the enhanced continuous field representation model is normalized to obtain the normalized semantic feature vector. The normalized semantic feature vector is compared with the preset category prototype feature vector, text encoding feature vector or the output feature of the classification network to obtain the semantic similarity. The semantic similarity is compared with a preset matching threshold to obtain the semantic category matching result. The spatial sampling points are then filtered based on the semantic category matching result to obtain a three-dimensional sampling point set. Three-dimensional connected component clustering is performed on the three-dimensional sampling point set to obtain a set of semantic entity instances, and the spatial relationships between semantic entity instances are determined based on the set of semantic entity instances; Each semantic entity instance is treated as a semantic entity instance node, and the set of relationship edges between semantic entity instances is determined based on spatial relationships. A semantic topology graph is then constructed based on the semantic entity instance nodes and the set of relationship edges.

5. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 4, characterized in that, The step of performing three-dimensional connected component clustering on the three-dimensional sampling point set to obtain a set of semantic entity instances, and determining the spatial relationships between semantic entity instances based on the set of semantic entity instances, includes: Obtain the spatial distance between three-dimensional sampling points in the three-dimensional sampling point set, and connect three-dimensional sampling points whose spatial distance is less than the preset neighborhood radius to obtain the sampling point adjacency relationship; Based on the adjacency relationship of sampling points, connected component analysis is performed using depth-first search or breadth-first search to obtain several connected regions. Each connected region is then treated as a semantic entity instance, resulting in a set of semantic entity instances. Determine the 3D scene representation elements of each semantic entity instance in the semantic entity instance set, and calculate the instance center, spatial boundary, and occupied area based on the 3D scene representation elements; Based on instance center, spatial boundary, and occupied area, and combined with preset relationship determination rules, the spatial relationship between semantic entity instances is determined.

6. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 5, characterized in that, The spatial relationships include at least one of the following: support relationship, adjacency relationship, occlusion relationship, non-collision constraint relationship, and relative orientation relationship.

7. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 1, characterized in that, The process of mapping relation edges in the semantic topology graph to a continuous topological association strength distribution in three-dimensional space, generating a continuous semantic topology field based on the continuous topological association strength distribution, and applying topological consistency constraints to the continuous semantic topology field to generate a continuous space occupancy representation includes: Determine the spatial center of action of relation edges in the semantic topology graph, and substitute the spatial center of action, spatial influence range parameters and spatial location into the continuous influence function to calculate the influence intensity, thereby obtaining the continuous topology association intensity distribution; Based on the weights of relation edges and the encoding of relation types in the semantic topology graph, and combined with the spatial field superposition and aggregation of the continuous topology association strength distribution, a continuous semantic topology field is obtained. The continuous semantic topological field is converted into a scalar topological intensity, and then weighted and fused with spatial density and semantic entity response values ​​of semantic entity instances to obtain a continuous spatial occupancy representation.

8. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 1, characterized in that, The initial six-DOF pose is solved based on the current observed image and the measurable 3D model of the target workpiece. A joint pose optimization objective is constructed by combining continuous semantic topological field and continuous spatial occupancy representation. The initial six-DOF pose is then optimized based on the joint pose optimization objective to obtain the optimized target workpiece six-DOF pose, including: Acquire the current observation image of the workpiece to be tested, and input the current observation image into the measurable three-dimensional model of the target workpiece to solve the initial six-degree-of-freedom pose of the target workpiece; The continuous semantic topological field is mapped to a parameterized spatial constraint representation for pose optimization, and the spatial constraint cost between the target workpiece and the environment entity is calculated based on the parameterized spatial constraint representation. The six-degree-of-freedom pose of the target workpiece is used as the optimization variable, and a joint pose optimization objective is constructed by combining continuous space occupancy representation and space constraint cost. The topology consistency deviation is calculated based on the initial six-DOF pose. The parameterized spatial constraint representation is updated according to the topology consistency deviation to obtain the updated parameterized spatial constraint representation. The initial six-DOF pose is iteratively optimized based on the updated parameterized spatial constraint representation and the joint pose optimization objective to obtain the optimized target workpiece six-DOF pose.

9. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 8, characterized in that, The process of mapping a continuous semantic topological field to a parameterized spatial constraint representation for pose optimization includes: Based on the continuous semantic topological field, the relation edges, relation type codes, relation edge weights, and spatial influence functions of the target workpiece are extracted, and the original feature set of topological association is obtained by summarizing them. Based on the original feature set of topological association, the constraint objects between the target workpiece and the environmental entity are determined, and the constraint parameters corresponding to the constraint objects are extracted. A parameterized spatial constraint representation is generated based on the constraint parameters. The constraint objects include at least one of the following: supporting entities, adjacent entities, occluding entities, non-collision entities, and relative orientation reference entities; The constraint parameters include at least one of the following: support surface height, support surface normal, adjacent boundary distance, occlusion order quantity, safety distance threshold, relative direction vector, and relational edge weight.

10. The workpiece six-degree-of-freedom pose estimation method based on continuous semantic topological field feedback constraints according to claim 8, characterized in that, The process involves calculating the topology consistency deviation based on the initial six-DOF pose, updating the parameterized spatial constraint representation based on the topology consistency deviation, obtaining the updated parameterized spatial constraint representation, and iteratively optimizing the initial six-DOF pose based on the updated parameterized spatial constraint representation and the joint pose optimization objective. The optimized target workpiece six-DOF pose includes: The measurable 3D model of the target workpiece is transformed into the scene coordinate system according to the initial six-degree-of-freedom pose, and the current spatial relationship between the target workpiece and the environmental entities is calculated. The current spatial relationship is compared with the reference topology state to obtain the topology consistency deviation. The relation edge weights in the semantic topology graph are updated based on the topology consistency deviation, and the updated relation edge weights are normalized to obtain the updated relation edge weights. The continuous semantic topology is updated based on the updated relation edge weights to obtain the updated continuous semantic topology. The parameterized space constraint representation is then updated using the updated continuous semantic topology to obtain the updated parameterized space constraint representation. The joint pose optimization objective is recalculated based on the updated parameterized space constraint representation to obtain the updated objective result. The initial six-DOF pose is then iteratively optimized based on the updated objective result until convergence, resulting in the optimized target workpiece six-DOF pose.

Images