A method and system for three-dimensional visual positioning of a partially feature-occluded target object

Abstract

The application provides a three-dimensional visual positioning method and system for a target object with a local feature being blocked, and relates to the technical field of robot three-dimensional visual positioning and assembly. The method comprises the following steps: generating a blocking map by using a known clamp model; inputting the blocking map, a color map and a depth map into an improved FoundationPose network to output an initial pose candidate with a score under the constraint of a clamping range; extracting a visible point cloud and generating a local model point cloud based on the blocking map; performing iterative registration with the clamping range constraint in the visible area; calculating the minimum safety distance from the clamp after registration and performing distance increasing correction; generating a positioning quality value by comprehensively considering the registration error, the overlap rate and the network score to select the optimal pose; and finally generating an executable decision by combining the inverse kinematics accessibility of the robot. The application realizes robust, accurate and directly safe three-dimensional visual positioning by deeply integrating the known clamp geometric information into the whole process from initial estimation, fine registration to safety correction and execution decision.

Description

Technical Field

[0001] This invention relates to the field of robot three-dimensional vision positioning and assembly technology, and in particular to a three-dimensional vision positioning method and system for a target object whose local features are occluded. Background Technology

[0002] Robot assembly scenarios often rely on 3D vision to obtain the six-DOF pose of the target object, driving the end effector to complete grasping, insertion, and positioning. RGB-D cameras provide color and depth information, and combining camera intrinsic and extrinsic parameters can align the image with the 3D model. In actual production, the target object is often clamped in a fixture, and the fixture structure occludes local features, resulting in incomplete visible point clouds and increased interference risk. To ensure successful assembly, the system needs to locate within a limited clamping pose range and evaluate the positioning quality and robot reachability to avoid erroneous actions.

[0003] Current technologies generally employ RGB-D sensors and model-driven pose estimation: one approach focuses on geometric features, first using features such as normals and FPFH for coarse registration and RANSAC matching, then using ICP iterative optimization of the rigid body pose; another approach is learning-based, utilizing an end-to-end 6D pose network to directly regress the pose from color / depth input, or fusing multimodal features to improve robustness. To address occlusion and clutter, semantic or instance segmentation is often used to extract target point clouds, or occlusion enhancement is added during the training phase. At the robot execution layer, the estimated pose is typically transformed to the robot's base coordinate system, and accessibility is determined by combining inverse kinematics and joint range, and assembly trajectory is planned accordingly. Quality assessment is usually based on registration residuals, reprojection errors, or network confidence.

[0004] However, the above-mentioned scheme has a key deficiency in scenarios with partial occlusion caused by the fixture: it lacks effective utilization of known fixture geometry information and fails to integrate occlusion recognition, clamping process constraints (pose range, safety distance) with pose estimation, optimization, and execution decisions into a closed-loop process. This leads to erroneous matching due to interference from occluded areas during the pose estimation stage, and the inability to avoid potential interference with the fixture in a timely manner during the optimization stage, resulting in insufficient reliability and executability of the output pose. Summary of the Invention

[0005] To address the technical problem of insufficient reliability in pose estimation and execution decision-making under fixture occlusion scenarios mentioned above, this invention provides a three-dimensional visual positioning method and system for targets with locally occluded features by introducing a series of technical features, such as explicit occlusion modeling based on known fixture models, pose optimization with integrated clamping and safety constraints, and comprehensive quality assessment and execution judgment based on multiple indicators. This achieves stable, safe, and high-precision positioning that can directly drive robot movements in complex occlusion environments.

[0006] To achieve the above objectives, a first aspect of the present invention provides a three-dimensional visual localization method for a target object whose local features are occluded, comprising: Acquire color and depth images of the assembly scene containing the target object and fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; Based on the 3D model of the fixture and the extrinsic parameters of the camera robot, a fixture reference depth map is rendered and generated. The depth map is then compared with the fixture to generate a fixture occlusion map that identifies the area where the fixture occludes the target object. The color image, the depth map, and the fixture occlusion map are input into the FoundationPose network. Under the constraints of the clamping pose range, the network outputs an initial pose candidate set for the target object and an occlusion matching score corresponding to each candidate pose. Based on the fixture occlusion map, the visible point cloud of the target object is extracted from the depth map; and based on the initial pose candidate set and the fixture occlusion map, a local model point cloud of the target object corresponding to each candidate pose is generated. For each candidate pose, within the visible area defined by the fixture occlusion map, the corresponding local model point cloud is iteratively registered with the visible point cloud of the target object to obtain a finely registered pose. The minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose is calculated. If the minimum safe distance is less than the interference threshold, the finely registered pose is corrected by increasing the distance to obtain a corrected finely registered pose. Based on the corrected finely registered pose, the registration error and model overlap rate are calculated, and combined with the corresponding occlusion matching score, a positioning quality value is generated. The target six-degree-of-freedom pose is selected according to the quality threshold. The target six-DOF pose is transformed to the robot base coordinate system, the robot's working pose is calculated, and the executable determination result is generated by combining the positioning quality value and the robot's inverse kinematics reachability.

[0007] Further, based on the 3D model of the fixture and the extrinsic parameters of the camera robot, a fixture reference depth map is rendered and generated, and compared with the depth map to generate a fixture occlusion map identifying the area where the fixture occludes the target object, including: Based on the 3D model of the camera robot extrinsic gripper, the pose of the gripper in the camera coordinate system is determined. Based on the camera intrinsic parameters, the 3D model of the fixture is subjected to depth rendering to generate a fixture reference depth map that is aligned with the pixel coordinates of the depth map; The fixture reference depth map is compared pixel by pixel with the depth map. When the distance value of a pixel in the depth map is less than the theoretical distance value of the corresponding pixel in the fixture reference depth map, the pixel is marked as a fixture occlusion area, and a binarized fixture occlusion map is generated.

[0008] Furthermore, the FoundationPose network includes an occlusion cue fusion layer, a clamping constraint candidate layer, and a pose grading layer; The processing steps of the FoundationPose network include: Multi-scale features of the color image, depth image, and fixture occlusion image are extracted using independent encoders. In the occlusion fusion layer, channel weights are generated based on the features of the fixture occlusion map, and color features and depth features are weighted and fused pixel by pixel. In the clamping constraint candidate layer, the fused features are converted into the initial pose candidate set, and the clamping pose range mapping constraint is performed on the translation and rotation components of each candidate pose. In pose layering, each candidate pose is combined with fused features to output the occlusion matching score, which reflects the degree of matching between the pose and the input data in the visible region.

[0009] Further, the step of extracting the visible point cloud of the target object from the depth map based on the fixture occlusion map, and generating a local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map, includes: Based on the occlusion markers in the fixture occlusion map, the distance values ​​of the corresponding pixels in the depth map are set to invalid, thus obtaining a visible depth map; Using camera intrinsic parameters, each effective pixel in the visible depth map is back-projected onto the camera coordinate system to form a three-dimensional point set as the visible point cloud of the target object; For each candidate pose in the initial pose candidate set, the three-dimensional model of the target object is transformed according to its corresponding six degrees of freedom parameters, and surface sampling is performed to obtain the point cloud of the candidate model. Project each candidate model point cloud onto the image plane, and remove points that fall into the occlusion mark area according to the fixture occlusion map to obtain the local model point cloud of the target object that corresponds one-to-one with each candidate pose.

[0010] Furthermore, the iterative registration and distance correction includes: For each candidate pose, point pair matching is established between its local model point cloud and the visible point cloud of the target object, and an iterative nearest point algorithm is executed. After each iteration, the updated pose is mapped to the clamping pose range until the finely registered pose is obtained. Calculate the minimum safe distance between the target object and the surface of the fixture model under the fine registration pose. If the distance is less than the interference threshold, determine the distance increase direction based on the nearest point pair that generates the distance. Incrementally correct the translation component of the pose along this direction and re-ensure that the pose is within the clamping pose range. Iterate this process until the safe distance requirement is met to obtain the corrected fine registration pose.

[0011] Furthermore, the process of generating positioning quality values ​​and filtering target six-DOF poses, as well as generating executable decision results, includes: Based on the corrected fine registration pose, the average registration error between the local model point cloud and the visible point cloud of the target object is calculated as the registration error, and the proportion of model points falling within a certain neighborhood of the visible point cloud is calculated as the model overlap rate. By combining the corresponding occlusion matching score, the registration error and the model overlap rate are weighted and combined to generate the localization quality value; Based on the quality threshold, the occlusion matching score and localization quality value of the candidate poses are screened, and the corrected fine-fit pose with the best localization quality value is selected as the target six-degree-of-freedom pose. The target six-degree-of-freedom pose is transformed into the robot base coordinate system to obtain the target reference pose, which is then combined with the preset operation relative pose to obtain the robot operation pose; The robot's working pose is solved by inverse kinematics and joint range is checked. If the solution is successful, the joints are reachable, and the positioning quality value meets the quality threshold, it is determined to be executable; otherwise, it is determined to be unexecutable.

[0012] A second aspect of the present invention provides a three-dimensional visual positioning system for a target object whose local features are occluded, comprising: The data acquisition module is used to acquire color and depth maps of the assembly scene containing the target object and the fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; The occlusion perception and initial pose estimation module is used to render and generate a gripper reference depth map based on the gripper's 3D model and the camera robot extrinsic parameters, and compare it with the depth map to generate a gripper occlusion map that identifies the area where the gripper occludes the target object; the color image, the depth map, and the gripper occlusion map are input into the FoundationPose network, and the network outputs an initial pose candidate set of the target object and an occlusion matching score corresponding to each candidate pose under the constraints of the gripping pose range; The visible area point cloud processing module is used to extract the visible point cloud of the target object from the depth map according to the fixture occlusion map; and to generate the local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map. The pose correction and quality screening module is used to iteratively register the corresponding local model point cloud with the visible point cloud of the target object within the visible area defined by the fixture occlusion map for each candidate pose, thereby obtaining a finely registered pose; calculate the minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose; if the minimum safe distance is less than the interference threshold, then perform distance increment correction on the finely registered pose to obtain a corrected finely registered pose; based on the corrected finely registered pose, calculate the registration error and model overlap rate, and generate a positioning quality value by combining the corresponding occlusion matching score; and screen out the target six-degree-of-freedom pose according to the quality threshold. The decision output module is used to transform the target six-degree-of-freedom pose to the robot base coordinate system, calculate the robot's working pose, and generate an executable decision result by combining the positioning quality value and the robot's inverse kinematics reachability.

[0013] A third aspect of the present invention provides an electronic device including a memory, a processor, and a program stored in the memory and running on the processor, wherein the processor executes the program to implement the steps in the three-dimensional visual localization method for a target object with partially occluded features as described in the first aspect of the present invention.

[0014] A fourth aspect of the present invention provides a computer-readable storage medium having a program stored thereon that, when executed by a processor, implements the steps in the three-dimensional visual localization method for a target object with partially occluded features as described in the first aspect of the present invention.

[0015] A fifth aspect of the present invention provides a computer program product comprising software code, wherein the program in the software code performs the steps of the three-dimensional visual localization method for a target object with partially occluded features as described in the first aspect of the present invention.

[0016] Compared with the prior art, the three-dimensional visual localization method and system for targets with partially occluded features provided by the present invention have the following beneficial effects: (1) This invention utilizes a known fixture model to render a reference depth map and performs pixel-level comparison to generate a fixture occlusion map, thus achieving explicit and accurate modeling of the occlusion area. Using the known fixture geometry and camera extrinsic parameters, the theoretical depth of the fixture in each pixel of the image can be accurately calculated. After comparing with the measured depth, pixels closer to the measured object than the fixture are identified as visible areas of the target object, while those farther away are identified as occluded areas. This transforms prior fixture knowledge into a computable binary mask (occlusion map), providing reliable guidance for all subsequent steps and avoiding invalid or erroneous feature matching and point cloud association in the occluded area.

[0017] (2) This invention inputs the color image, depth image, and fixture occlusion image into the FoundationPose network, and outputs an initial pose candidate set with occlusion matching score under the constraint of the clamping pose range, realizing the fusion of occlusion information and compliance with process constraints in the perception stage. The FoundationPose network dynamically adjusts the weights of color and depth features using occlusion map features through the occlusion cue fusion layer, so that the network attention is focused on reliable visible areas and the interference of occluded areas is suppressed. Next, the clamping constraint candidate layer ensures that all initial poses generated by the network fall within the clamping pose range allowed by the process. Finally, the pose scoring layer combines the fusion features to score each candidate pose. This score (occlusion matching score) specifically measures the degree of matching between the pose hypothesis and the visible image area. This deeply embeds the clamping process constraints and occlusion prior knowledge into the neural network of pose estimation, ensuring the rationality and directionality of the initial estimation.

[0018] (3) This invention performs iterative registration within the visible area defined by the fixture occlusion map and performs distance correction based on the interference threshold, achieving the integration of pose refinement and safety assurance. First, point cloud extraction and registration are performed entirely within the visible area defined by the occlusion map, ensuring the accuracy of data association. Second, each iteration of the iterative nearest point (ICP) registration forces the pose to be mapped back to the clamping pose range, realizing real-time interaction between the optimization process and process constraints. Most importantly, the minimum safe distance to the fixture is calculated immediately after registration. If it is less than the interference threshold, it is not simply discarded, but the distance is corrected based on the nearest point direction, actively pushing the pose away to meet the safety margin while ensuring registration accuracy. This approach of embedding safety constraints into the optimization loop, unlike traditional post-collision detection, can produce a pose that is both accurate and interference-free.

[0019] (4) This invention combines occlusion matching score, registration error, and overlap rate to generate a localization quality value, and integrates the quality value with kinematic reachability for execution determination, thus realizing a reliable decision-making bridge from localization results to robot executable commands. The localization quality value is not a single indicator, but a weighted combination of network confidence (occlusion matching score), local geometric alignment accuracy (registration error), and data support (overlap rate), which can comprehensively reflect the reliability of pose estimation under visible conditions. This quality value is combined with the robot's inverse kinematics solution results for determination. Only poses that simultaneously meet the conditions of reliable estimation and joint reachability are determined to be executable. This unifies perceptual uncertainty and execution feasibility into a decision-making framework, greatly improving the success rate and safety of robot actions, and realizing a closed loop from localization to reliable execution. Attached Figure Description

[0020] The accompanying drawings, which form part of this disclosure, are used to provide a further understanding of this disclosure. The illustrative embodiments of this disclosure and their descriptions are used to explain this disclosure and do not constitute an undue limitation of this disclosure.

[0021] Figure 1 A flowchart of a three-dimensional visual localization method for a target object with partially occluded features provided in Embodiment 1 of the present invention; Figure 2 A flowchart illustrating the scene reference data formation process for the three-dimensional visual localization method for a target object with partially occluded features provided in Embodiment 1 of the present invention. Figure 3 The fixture occlusion diagram and FoundationPose network flowchart are provided in Embodiment 1 of the present invention for the three-dimensional visual localization method of a target object with locally occluded features. Figure 4 This is a flowchart illustrating the formation of visible point cloud and local model point cloud of a target object in a 3D visual localization method for a target object with occluded local features, as provided in Embodiment 1 of the present invention. Figure 5 This is a flowchart of the iterative nearest point cloud registration and interference threshold constraint process for the three-dimensional visual localization method of the target object with occluded local features provided in Embodiment 1 of the present invention. Figure 6 This is a flowchart of the robot's working pose and executable determination process for the three-dimensional visual localization method of a target object with partially occluded features provided in Embodiment 1 of the present invention. Figure 7 This is a flowchart illustrating the pose data encapsulation and robot controller execution process of the three-dimensional visual localization method for a target object with partially occluded features, as provided in Embodiment 1 of the present invention. Figure 8 The visible region localization multi-part map driven by the fixture occlusion map is provided in the three-dimensional visual localization method for a target object whose local features are occluded according to Embodiment 1 of the present invention. Figure 9 The robot's three-segment trajectory diagram for the three-dimensional visual localization method for a target object with partially occluded features provided in Embodiment 1 of the present invention; Figure 10 A schematic diagram of the FoundationPose network architecture for the 3D visual localization method for targets with partially occluded features provided in Embodiment 1 of the present invention. Figure 1 ; Figure 11 A schematic diagram of the FoundationPose network architecture for the 3D visual localization method for targets with partially occluded features provided in Embodiment 1 of the present invention. Figure 2 ; Figure 12 This is an architecture diagram of a three-dimensional visual positioning system for a target object whose local features are occluded, provided in Embodiment 2 of the present invention. Detailed Implementation

[0022] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used in this invention have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0023] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form as well. Furthermore, it should be understood that the terms “comprising” and “having”, and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0024] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0025] All data acquisition in this embodiment is carried out in accordance with laws and regulations and with user consent, and the data is used legally.

[0026] Example 1 like Figure 1 This embodiment provides a three-dimensional visual localization method for a target object whose local features are occluded, including: S1. Obtain color and depth maps of the assembly scene containing the target object and fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; S2. Based on the 3D model of the fixture and the camera robot extrinsic parameters, a fixture reference depth map is rendered and compared with the depth map to generate a fixture occlusion map that identifies the area where the fixture occludes the target object; the color image, the depth map, and the fixture occlusion map are input into the FoundationPose network, and the network outputs an initial pose candidate set of the target object and an occlusion matching score corresponding to each candidate pose under the constraints of the clamping pose range; S3. Based on the fixture occlusion map, extract the visible point cloud of the target object from the depth map; and based on the initial pose candidate set and the fixture occlusion map, generate the local model point cloud of the target object corresponding to each candidate pose. S4. For each candidate pose, within the visible area defined by the fixture occlusion map, iteratively register the corresponding local model point cloud with the visible point cloud of the target object to obtain a finely registered pose; calculate the minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose; if the minimum safe distance is less than the interference threshold, then perform distance increment correction on the finely registered pose to obtain a corrected finely registered pose; based on the corrected finely registered pose, calculate the registration error and model overlap rate, and combine them with the corresponding occlusion matching score to generate a positioning quality value; select the target six-degree-of-freedom pose according to the quality threshold; S5. Transform the target six-degree-of-freedom pose to the robot base coordinate system, calculate the robot's working pose, and combine the positioning quality value with the robot's inverse kinematics reachability to generate an executable determination result.

[0027] like Figure 2 In one specific embodiment, step S1 specifically includes: Step S1 is used to form scene reference data, so that the subsequent step S2 can place the 3D model of the fixture in the camera coordinate system according to the camera robot extrinsic parameters and generate a fixture reference depth map according to the camera view. Then, a pixel-level comparison is performed with the depth map to generate a fixture occlusion map. The FoundationPose network outputs an initial pose candidate set and occlusion matching score under the constraint of the clamping pose range. At the same time, steps S3 and S4 complete the extraction of visible point cloud of target object, local model point cloud cropping, iterative nearest point cloud registration and interference threshold determination under the same pixel coordinates and the same geometric scale. A depth camera is used to simultaneously acquire color and depth maps. The color map is denoted as... Depth map is denoted as ,in It is a three-channel image. This is a single-channel distance map, based on the camera calibration relationship. and Perform pixel coordinate alignment so that the two images point to the same spatial line of sight at the same pixel coordinates, and apply a depth scale factor to the pixel distance values ​​of the depth map. Perform unit conversion to ensure the distance unit matches the length unit of the target object's 3D model. Invalid distance values ​​are marked as invalid and removed in subsequent back projections. and Crop or scale to preset resolution and This is used as a FoundationPose network input, together with the fixture occlusion map, at the same size; Obtain camera internal parameters Camera internal parameters Includes focal length and principal point parameters, used for pixel projection during depth back-projection and fixture reference depth map rendering, and receives camera robot extrinsic parameters. Camera robot external parameters Used to describe the coordinate transformation relationship from the robot's base coordinate system to the camera coordinate system, and to... Performing a rigid body transformation inverse yields the coordinate transformation relationship from the camera coordinate system to the robot base coordinate system. When cropping or scaling the image in step S11, coordinate translation correction is performed on the principal point parameter based on the cropping offset, and proportional scaling correction is performed on the focal length parameter and principal point parameter based on the scaling ratio, so that the updated camera intrinsic parameters are corrected. Consistent with pixel coordinates at the preset resolution; Read the 3D model of the target object fixture. The 3D model of the target object fixture includes the 3D model of the target object. 3D model of the fixture ,right and Unify the model coordinate system orientation and length unit to ensure that subsequent pose transformation, depth rendering, point cloud pruning, and nearest distance search use the same geometric scale, and read the fixture installation pose definition. Fixture mounting pose definition Used to describe the 3D model of the clamp. The fixed mounting pose in the robot base coordinate system, the fixture mounting pose definition From the 3D model of the fixture The installation calibration data is obtained by binding the fixture, which consists of the three-dimensional coordinates of the fixture reference feature points in the robot base coordinate system and the three-dimensional model of the fixture. The three-dimensional coordinates of the corresponding feature points are obtained through rigid body registration. Read the clamping pose range, interference threshold, and mass threshold, and resolve the clamping pose range into a three-dimensional translation range. With three-dimensional rotation range 3D translation range Used to define the boundaries of pose translation components and the range of three-dimensional rotation. Used to define the boundaries of pose rotation components, the interference threshold is denoted as... Interference threshold This is used to subsequently calculate the minimum safe distance between the target object's 3D model and the fixture's 3D model and trigger pose correction. The mass threshold is denoted as... quality threshold Used for subsequent judgment of occlusion matching score and positioning quality value, , , , , , , , , , , and Combined into scene baseline data and the scene baseline data This data serves as input for subsequent fixture reference depth map generation, fixture occlusion map calculation, initial pose candidate set constraints, point cloud registration constraints, and robot operation pose calculation.

[0028] Specifically, the step of rendering a gripper reference depth map based on the 3D model of the gripper and the camera robot extrinsic parameters, and comparing it with the depth map to generate a gripper occlusion map identifying the area where the gripper occludes the target object, includes: Based on the 3D model of the camera robot extrinsic gripper, the pose of the gripper in the camera coordinate system is determined. Based on the camera intrinsic parameters, the 3D model of the fixture is subjected to depth rendering to generate a fixture reference depth map that is aligned with the pixel coordinates of the depth map; The fixture reference depth map is compared pixel by pixel with the depth map. When the distance value of a pixel in the depth map is less than the theoretical distance value of the corresponding pixel in the fixture reference depth map, the pixel is marked as a fixture occlusion area, and a binarized fixture occlusion map is generated.

[0029] like Figure 3 In one specific embodiment, step S2 specifically includes: Step S2 is used to generate a fixture occlusion map in the fixture occlusion scene and drive the FoundationPose network to output an initial pose candidate set and occlusion matching score. The scene baseline data is denoted as... Scenario baseline data Includes color images Depth map Camera internal parameters Camera robot external parameters 3D model of the fixture Fixture mounting posture definition And the clamping position range; according to and Determine the spatial position and orientation of the fixture's 3D model in the camera coordinate system, and the camera robot's extrinsic parameters. Define the coordinate transformation relationship from the robot's base coordinate system to the camera coordinate system, and define the fixture mounting pose. 3D model of the fixture The coordinate transformation relationship from the model coordinate system to the robot base coordinate system is determined by multiplying homogeneous coordinate transformation matrices. Right multiplication Obtain the three-dimensional model of the fixture The pose in the camera coordinate system is denoted as Based on camera intrinsic parameters With preset resolution , 3D model of the fixture For depth rendering, a method of per-face projection of triangular facets and depth buffering is used. The depth values ​​of the three vertices of the triangular facets in the camera coordinate system are interpolated pixel by pixel. The depth buffer is updated pixel by pixel, and the minimum distance value is retained as the theoretical distance of that pixel, thus obtaining the fixture reference depth map. Fixture reference depth map With depth map Using the same distance unit and the same pixel coordinate system, for the unfixed 3D model The pixels covered by the projection Write an invalid flag in it; Fixture reference depth map With depth map After aligning at the same pixel coordinates, perform a pixel-by-pixel comparison, reading the position of each pixel. The theoretical distance value and The distance value, when The distance value is less than When the theoretical distance is reached, the pixel is written into the fixture occlusion marker and output to the fixture occlusion map. Fixture occlusion diagram This is a single-channel marker image. The value of the fixture-occluded marker is one, and the value of the non-fixture-occluded marker is zero. When the pixel position... When the mark is invalid, the fixture will be obscured. Write a non-clamp occlusion mark to the corresponding pixel, when the pixel position is... When the mark is invalid, the fixture will be obscured. Write non-clamp occlusion markers to the corresponding pixels; Color image Depth map Obscuring diagram with clamp According to the preset resolution , The network input consists of, It is a three-channel image. This is a single-channel distance map. For a single-channel marker image, the clamping pose range is decomposed into a three-dimensional translation range. With three-dimensional rotation range and will and The candidate pose output constraints are provided as clamping constraint candidate layers to the FoundationPose network for performing range mapping on the initial pose candidate set. The FoundationPose network enters the feature extraction stage, using a color image encoder to... Perform three sets of convolutional layer operations, each set containing a 3x3 convolution and a downsampling convolution, outputting three-scale color feature maps, denoted as . , , The number of channels is 64, 128, and 256 respectively, and a depth encoder is used for... Perform the same convolutional layer group operation as the color image encoder, and output a three-scale depth map feature map, denoted as... , , The number of channels is 64, 128, and 256 respectively, and an obstruction encoder is used for... Perform two sets of convolutional layer operations and output three-scale occlusion feature maps, denoted as... , , The number of channels is 16, 32, and 64 respectively; The FoundationPose network enters the occlusion cue fusion layer, selecting the third-scale feature as the fusion benchmark. It uses channel weights driven by the clamp occlusion map to perform pixel-wise weighting on the color and depth feature maps, and then concatenates and fuses the weighted results using a 1:1 convolution. Subsequently, global average pooling is performed on the fused features to form a fused feature vector. The specific calculation is as follows: , , , , , ,

[0030] in, For occlusion feature map The intermediate feature map obtained by one-to-one convolution mapping, This is a one-to-one convolution operation, and its kernel parameters are learnable parameters. The sigmoid function performs an exponential operation on each input value and combines it with a constant to normalize the output value, ensuring it falls within the range of zero to one. This is the initial channel weight map after mapping. This is the normalized result of the channel weight map, normalized at each pixel position. This is performed on the channel dimension, causing the channel weights at the same pixel location to sum to one. and For channel indexing, Feature map of third-scale color image The number of channels is set to 256. This indicates that the product is multiplied pixel by pixel along each channel. This indicates splicing along the channel dimension. For weighted color feature maps, This is a weighted depth map feature map. To fuse feature maps, To fuse feature vectors, and The width and height of the third-scale feature map. and From preset resolution , Obtained through downsampling convolution. and For spatial location index; The FoundationPose network enters the clamping constraint candidate layer, where the feature vectors are fused. The input consists of two fully connected neurons. The first fully connected neuron outputs a 256-dimensional intermediate vector, denoted as... The second layer of fully connected neurons outputs a preset number of [number] neurons. The set of six-degree-of-freedom vectors is denoted as the initial pose candidate set. The preset quantity Configure network parameters such that each six-degree-of-freedom vector contains three-dimensional translation and three-dimensional rotation. The three-dimensional rotation is represented by Euler angles and arranged in a fixed-axis order. Each six-degree-of-freedom vector in the process performs a range mapping of the clamping pose range, and the three-dimensional translation is performed according to the three-dimensional translation range. Component-by-component limiting, with the three-dimensional rotation amount determined by the three-dimensional rotation range. Component-wise amplitude limiting to make the initial pose candidate set All candidate poses fall within the clamping pose range; The FoundationPose network performs pose layering on the initial pose candidate set. Each six-degree-of-freedom vector in the fusion feature vector The vectors are concatenated to form the scoring input vector, denoted as . Scoring input vector The six components of a six-degree-of-freedom vector and the fused feature vector The 256 components will be used to input each score into the vector. The input consists of two fully connected neurons. The first fully connected neuron outputs a 128-dimensional scoring feature vector, and the second fully connected neuron outputs the corresponding occlusion matching score, denoted as . All occlusion matching scores are combined into an occlusion matching score vector. Occlusion matching score vector With the initial pose candidate set One-to-one correspondence, and the initial pose candidate set Matching score vector with occlusion Output to subsequent point cloud processing and iterative point cloud registration process.

[0031] The fixture occlusion map generated in this step and the visible area localization process driven by it can be referred to. Figure 8 The diagram shown is shown in the image.

[0032] Specifically, the FoundationPose network includes an occlusion cue fusion layer, a clamping constraint candidate layer, and a pose grading layer; the processing steps of the FoundationPose network are as follows: Figure 10 and Figure 11 As shown: Multi-scale features of the color image, depth image, and fixture occlusion image are extracted using independent encoders. In the occlusion fusion layer, channel weights are generated based on the features of the fixture occlusion map, and color features and depth features are weighted and fused pixel by pixel. In the clamping constraint candidate layer, the fused features are converted into the initial pose candidate set, and the clamping pose range mapping constraint is performed on the translation and rotation components of each candidate pose. In pose layering, each candidate pose is combined with fused features to output the occlusion matching score, which reflects the degree of matching between the pose and the input data in the visible region.

[0033] In one specific embodiment, step S3 is as follows: like Figure 4 Step S3 is used to transfer the limitation of the visible area of ​​the target object by the fixture occlusion map to the point cloud level, forming a set of visible point clouds of the target object and local model point clouds. This enables the subsequent step S4 to establish point pair matching and perform iterative nearest point cloud registration within the visible surface range defined by the fixture occlusion map. The input of this step includes the fixture occlusion map. Depth map Camera internal parameters Initial pose candidate set 3D model of the target object The output includes the visible point cloud of the target object. With local model point cloud The visible point cloud of the target object With local model point cloud All are represented using three-dimensional points in the camera coordinate system; Obscuring the clamp With depth map Aligning with the same pixel coordinates, read the fixture occlusion marker and distance value pixel by pixel. For the pixel position where the fixture occlusion marker has a value of one, transfer the depth map... The corresponding distance value is written to an invalid flag, and the value of the fixture occlusion flag is set to zero and the depth map is... For pixel locations with valid distance values, retain the depth map. With the distance value unchanged, a visible depth map is obtained. Visible depth map With depth map Maintain the same resolution and the same distance unit to ensure that the geometric scale of subsequent back projections is consistent; According to the visible depth map With camera internal reference The effective pixels are back-projected onto the camera coordinate system to generate 3D points, and the camera intrinsics are then used. Includes focal length parameter and and principal point parameters and Pixel coordinates are adopted This indicates that the visible depth map Each valid pixel Read distance value As the depth coordinate component in the camera coordinate system, the pixel lateral offset Divide by focal length parameter and distance value Multiplying them yields the horizontal coordinate component in the camera coordinate system, and the vertical offset of the pixels is then calculated. Divide by focal length parameter and distance value Multiplying them yields the longitudinal coordinate components in the camera coordinate system, thus forming a 3D point in the camera coordinate system. 3D points Composed of horizontal coordinate components, vertical coordinate components, and depth coordinate components, it encompasses all three-dimensional points. By constructing a three-dimensional point set, the visible point cloud of the target object is obtained. ; For the initial pose candidate set Each six-DOF vector performs the target object's 3D model transformation and fixture occlusion clipping, initial pose candidate set The Middle Each six-degree-of-freedom vector is determined by a three-dimensional translation. With three-dimensional rotation Composition, three-dimensional rotation The three-axis Euler angles, fixed around the camera coordinate system, are sequentially combined according to a preset fixed axis order to represent the three-dimensional rotation. Convert to rotation matrix Rotation matrix Used to perform rigid body rotation on points in the model coordinate system, for the target 3D model. The surface triangular facets are used to calculate the facet area. Based on the facet area, an area accumulation sequence is constructed. For each sampling point, uniformly distributed sampling parameters are generated, and the selected triangular facets are determined in the area accumulation sequence. Then, sampling parameters for the centroid coordinates are generated, and sampling points in the model coordinate system are obtained by interpolation within the triangular facets. Sampling points For the point coordinates of the model point cloud, repeat sampling until the number of sampled points is obtained. Model point cloud ,in To preset the number of sampling points, the model point cloud is... Each sampling point in First, use a rotation matrix. Perform rotation, then superimpose three-dimensional translation. The transformation points in the camera coordinate system are obtained. Transformation points whose depth coordinate components are less than zero are discarded. For transformation points that are not discarded, the camera intrinsic parameters are used to determine the transformation points. Perform projection by multiplying the lateral coordinate components of the transformed point by the focal length parameter. Divide by the depth coordinate components and then add the principal point parameters. Obtain the horizontal pixel coordinates of the projection, and multiply the vertical coordinate components of the transformation point by the focal length parameter. Divide by the depth coordinate components and then add the principal point parameters. Obtain the vertical pixel coordinates of the projection. Convert the non-integer pixel coordinates obtained from the projection to integer pixel coordinates by rounding down. If the integer pixel coordinates fall within the preset resolution... and If the image is outside the range of the transformation point, then the transformation point is removed. If the clamp occlusion image corresponds to an integer pixel coordinate... If the fixture occlusion marker is removed, the transformation point is discarded, and the remaining transformation points are retained to form a local model point cloud. Repeat the above process for all candidate poses to obtain point clouds composed of multiple local models. The local model point cloud formed And preserve the local model point cloud With the initial pose candidate set One-to-one correspondence by index; Visible point cloud of the target object With local model point cloud Unified into a point cloud representation in the camera coordinate system, visible point cloud for the target object. With each local model point cloud All points are stored using three-dimensional coordinate values, and the distance unit is confirmed to be consistent with the three-dimensional model of the target object. The length units are consistent, making the local model point cloud Able to be visible point clouds of the target object We directly perform nearest neighbor retrieval and point pair matching, which serve as input for subsequent iterations of nearest point cloud registration.

[0034] Specifically, the step of extracting the visible point cloud of the target object from the depth map based on the fixture occlusion map, and generating a local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map, includes: Based on the occlusion markers in the fixture occlusion map, the distance values ​​of the corresponding pixels in the depth map are set to invalid, thus obtaining a visible depth map; Using camera intrinsic parameters, each effective pixel in the visible depth map is back-projected onto the camera coordinate system to form a three-dimensional point set as the visible point cloud of the target object; For each candidate pose in the initial pose candidate set, the three-dimensional model of the target object is transformed according to its corresponding six degrees of freedom parameters, and surface sampling is performed to obtain the point cloud of the candidate model. Project each candidate model point cloud onto the image plane, and remove points that fall into the occlusion mark area according to the fixture occlusion map to obtain the local model point cloud of the target object that corresponds one-to-one with each candidate pose.

[0035] In one specific embodiment, step S4 is as follows: like Figure 5 Step S4 is used to combine the initial pose candidate set output by the FoundationPose network with the occlusion matching score, and combine it with the visible point cloud range defined by the fixture occlusion map to complete the fine-match pose solution, pose correction under interference threshold constraints, and form a localization quality value to filter the target six-DOF pose. The input of this step includes the initial pose candidate set. Occlusion matching scoring vector Fixture Obstruction Diagram Visible point cloud of the target object Local model point cloud 3D model of the target object 3D model of the fixture 3D translation range 3D rotation range Interference threshold With quality threshold quality threshold Including occlusion matching score threshold With positioning quality threshold The output is the target six-DOF pose. The preset distance threshold is denoted as The preset iteration number threshold is denoted as The registration error convergence threshold is denoted as The preset correction number threshold is denoted as The preset neighborhood radius is denoted as The preset registration error threshold is denoted as ; By candidate index Traverse the initial pose candidate set Retrieve local model point cloud Local model point cloud with consistent index And read the occlusion matching score vector Occlusion matching score with consistent index , will the The six-degree-of-freedom vectors are analyzed to determine the initial pose values. initial pose value For rigid body transformation from model coordinate system to camera coordinate system, local model point cloud The point cloud obtained by sampling the 3D model of the target object using the six-degree-of-freedom vectors and removing fixture occlusion marks is a local model point cloud. Located in the camera coordinate system; In the clamp occlusion diagram Establish point pair matching on the visible point cloud of the defined target object and read camera intrinsic parameters. focal length parameter , With principal point parameters , The visible point cloud of the target object Each three-dimensional point in Projected to pixel coordinates ,in Depend on The horizontal coordinate components and the depth coordinate components are arranged according to and Calculations show that Depend on The longitudinal coordinate components and the depth coordinate components are arranged according to and The calculation shows that... Convert to integer pixel coordinates and The system queries the fixture occlusion marker. When the fixture occlusion marker is 1, the 3D point is removed, resulting in the visible point cloud of the removed target object. ,exist Construct a nearest neighbor retrieval structure on the local model point cloud. For each sampling point, the nearest neighbor point is retrieved to form a set of corresponding point pairs. And remove points whose point-to-point distance is greater than a preset distance threshold. Corresponding point pairs; Local model point cloud Visible point cloud of the target object Input the nearest point cloud registration to obtain the fine registration pose, and then adjust the initial pose value. Inverse the rigid body transformation to obtain The rotation part is taken as the transpose, and the translation part is taken as the negative rotation transpose multiplied by the translation. In the next iteration, the current pose is used. and The point cloud transformation is obtained by combining the results. Apply point cloud transformation to local model point cloud Obtain the transformed point cloud and the point cloud visible to the target object. Re-search for nearest neighbors and establish a set of corresponding point pairs. ,according to Calculate rigid body pose increment Rigid body pose increment By calculating the centroid of corresponding point pairs, constructing decentralized coordinates, solving the covariance matrix, and performing singular value decomposition on the covariance matrix to obtain the rotation and translation increments, left-multiplication is used to update the result. ,right Extract the 3D translation and 3D rotation quantities and perform clamping pose range mapping to ensure that the 3D translation quantity falls within the 3D translation range. And the three-dimensional rotation amount falls within the three-dimensional rotation range. ,statistics The average distance from midpoint to midpoint is used as the registration error. When the number of iterations reaches the preset iteration threshold Or the difference between two consecutive registration errors is less than the registration error convergence threshold. The iteration is terminated at time 1, and the finely aligned pose is obtained. ; According to the precise positioning Transform the 3D model of the target object and the three-dimensional model of the fixture Perform the nearest distance search on the 3D model of the fixture. Construct a hierarchical enclosing structure of triangular facets, and perform a minimum value search on the distances from points on the surface of the transformed target 3D model to points on the surface of the fixture 3D model and then to the triangular facets to obtain the minimum safe distance. And record the closest point pair that produces the minimum safe distance. ; When the minimum safe distance Less than the interference threshold Timely correction of fine-tuning pose, based on the nearest point Determine the pitch extension direction vector, which is taken from... point to The direction is normalized, and when the direction cannot be normalized, the generated direction is taken. The normal vector of the clamp triangular facet is used as the distance-increasing direction vector for precise alignment of the pose. The three-dimensional translation is adjusted by applying an increment correction along the increment direction vector, and the increment correction magnitude is taken as... It then performs clamping pose range mapping on the three-dimensional rotation, repeating the nearest distance search and distance increment correction until the minimum safe distance is not less than the interference threshold. Or the number of corrections reaches the preset correction threshold. The corrected fine-fit pose is obtained. ; Based on the corrected fine-fit alignment pose Re-establish corresponding point pairs and generate positioning quality values, using point cloud transformation. Applying to local model point clouds Obtain the corrected point cloud, and still record the corrected point cloud as... In the visible point cloud of the target object Reconstruct the set of corresponding point pairs and calculate the average point-to-point distance as the registration error. For the correction point cloud Each point in Find the nearest neighbor and calculate the nearest neighbor distance, based on the occlusion matching score. The positioning quality value is obtained by weighting the overlap rate term and the registration error term. The calculation is as follows: ; in, Candidate index The corresponding positioning quality value, Candidate index The corresponding occlusion matching score For point cloud transformation Corrected local model point cloud, Point cloud for local model The number of points, Point cloud for local model Three-dimensional points in To remove the visible point cloud of the target object after removing the fixture occlusion marks, To make the point cloud visible to the target object Interior and Point The nearest neighbor, The point-to-point distance between two 3D points. To preset the neighborhood radius, This is an indicator function; it takes a value of 1 if the condition is true and a value of 0 if the condition is false. For registration error, To preset the registration error threshold, The operation to obtain the maximum value is used to limit the lower bound of the registration error score to zero; Based on quality threshold Filter and determine the target six-DOF pose When occlusion matches the score Below the occlusion matching score threshold Candidate poses are discarded when the positioning quality value is... Below the positioning quality threshold Candidate poses are discarded, and the localization quality value is selected from the remaining candidate poses. Maximum correction of precise alignment pose As the target six-degree-of-freedom pose .

[0036] Specifically, the iterative registration and distance correction include: For each candidate pose, point pair matching is established between its local model point cloud and the visible point cloud of the target object, and an iterative nearest point algorithm is executed. After each iteration, the updated pose is mapped to the clamping pose range until the finely registered pose is obtained. Calculate the minimum safe distance between the target object and the surface of the fixture model under the fine registration pose. If the distance is less than the interference threshold, determine the distance increase direction based on the nearest point pair that generates the distance. Incrementally correct the translation component of the pose along this direction and re-ensure that the pose is within the clamping pose range. Iterate this process until the safe distance requirement is met to obtain the corrected fine registration pose.

[0037] In one specific embodiment, step S5 is as follows: like Figure 6 Step S5 is used to convert the target six-DOF pose into a robot-executable work pose, and combine the localization quality value with the robot joint reachability to generate an executable determination result. The inputs for this step include scene reference data. Target six-degree-of-freedom pose With positioning quality value The target six-DOF pose For the rigid body transformation of the target object's 3D model from the model coordinate system to the camera coordinate system, locate the mass value. The output of this step is the robot's working pose and the decision result for the execution capability, which is the positioning quality value corresponding to the target six-DOF pose. From scenario benchmark data Reading camera robot extrinsic parameters Camera robot external parameters To define the coordinate transformation relationship from the robot's base coordinate system to the camera coordinate system, for Inverse the rigid body transformation to obtain The The coordinate transformation relationship from the camera coordinate system to the robot base coordinate system is given. The inverse is obtained by taking the transpose of the rotational part and the negative of the translational part, which is the product of the transpose and rotation. Left multiplication Complete the coordinate system transformation to obtain the target object's reference pose. Target object reference pose This refers to the rigid body transformation from the target object's 3D model coordinate system to the robot's base coordinate system. From scenario benchmark data Reading the relative pose of the job relative pose of the operation The preset pose of the robot's end effector coordinate system to the target object's 3D model coordinate system, and the relative pose of the operation. The relative pose of the operation is represented by a homogeneous coordinate transformation matrix. Right multiply to target reference pose Perform pose composition to obtain the robot's working pose. Robot working pose For the target rigid body transformation from the robot end effector coordinate system to the robot base coordinate system; From scenario benchmark data Read the robot joint range, which is defined by the lower limit of each joint. and upper limit Composition, in which For joint indexing, the robot's working pose is... The target pose is obtained by performing a rigid body transformation and inversion. Target pose This is a target rigid body transformation from the robot's base coordinate system to the robot's end effector coordinate system, used for inverse kinematics solutions, to determine the target pose. Joint pose is obtained by performing inverse kinematics solution. Joint position The vector composed of the positional quantities of each joint, representing the joint pose. The Each component is denoted as The inverse kinematics solution adopts a numerical solution method based on forward kinematics and Jacobian matrix iteration, with a preset threshold for the number of iterations. Pose error threshold With step size coefficient Step size coefficient As a scalar used to scale joint increments, the median vector of the joint range is used as the initial joint pose. The positive kinematic pose corresponding to the current joint pose is calculated according to the iteration number and denoted as . Among them, the correct sports posture The target pose is obtained by multiplying the robot link parameters and joint poses through successive rigid body transformations. Calculate the pose error vector Pose error vector It consists of a combination of three-dimensional translation error and three-dimensional rotation error. The three-dimensional translation error is composed of... and The translational component difference is obtained, and the three-dimensional rotation error is obtained by converting the relative rotation matrix of the two rotation components into an axis-angle representation. The Jacobian matrix is ​​then calculated based on the current joint pose. Jacobian matrix The linear mapping matrix from joint velocity to end-effector velocity is obtained by using the method of... Performing singular value decomposition yields the pseudo-inverse of the Jacobian matrix. ,Will With pose error vector Multiply by the step size factor Obtain the joint increments, update the current joint pose using the joint increments, and if the pose error vector... The norm is not greater than the pose error threshold. If the inverse kinematics solution is successful, the iteration count is considered successful if the preset iteration count threshold is reached. And the pose error vector The norm is still greater than the pose error threshold. If the inverse kinematics solution fails, the joint pose obtained from the inverse kinematics solution will be determined as follows: Perform joint range determination when inverse kinematics solution fails or when any joint satisfies or Time-based joint reachability marker generation It is unreachable when the inverse kinematics solution is successful and all joints satisfy the condition. Joint reachable markers are generated at the time. Accessible; From scenario benchmark data Read quality threshold quality threshold Includes positioning quality threshold , will locate quality value With positioning quality threshold When comparing, Not less than And the joint can reach the marker. Generate an executable decision result when reachable. For it to be executable, when Less than Or joint reach mark Generate an executable decision result when unreachable. If the operation is not executable, output the robot's working pose. Executable determination result .

[0038] Specifically, the steps of generating positioning quality values ​​and filtering target six-DOF poses, and generating executable decision results, include: Based on the corrected fine registration pose, the average registration error between the local model point cloud and the visible point cloud of the target object is calculated as the registration error, and the proportion of model points falling within a certain neighborhood of the visible point cloud is calculated as the model overlap rate. By combining the corresponding occlusion matching score, the registration error and the model overlap rate are weighted and combined to generate the localization quality value; Based on the quality threshold, the occlusion matching score and localization quality value of the candidate poses are screened, and the corrected fine-fit pose with the best localization quality value is selected as the target six-degree-of-freedom pose. The target six-degree-of-freedom pose is transformed into the robot base coordinate system to obtain the target reference pose, which is then combined with the preset operation relative pose to obtain the robot operation pose; The robot's working pose is solved by inverse kinematics and joint range is checked. If the solution is successful, the joints are reachable, and the positioning quality value meets the quality threshold, it is determined to be executable; otherwise, it is determined to be unexecutable.

[0039] Furthermore, the method also includes: S6, outputting the robot's working pose and executable determination result to the robot controller, and the robot controller generating assembly action instructions based on the robot's working pose and executable determination result.

[0040] In one specific embodiment, step S6 is as follows: like Figure 7 Step S6 is used to convert the robot's working pose obtained in step S5. Executable determination result With positioning quality value The data is transmitted to the robot controller, which then generates assembly action commands or stop assembly commands, and the pose is determined. The target rigid body transformation from the robot end effector coordinate system to the robot base coordinate system is used to determine the executable result. The value can be either executable or non-executable, thus determining the quality value. The value ranges from zero to one; Will , and Encapsulated into pose data according to the communication format of the robot controller. The communication format is denoted as Communication format In scene benchmark data The communication format is given in the document. Includes data frame header, data length, data area and checksum area, for Read the translation components and encode them into a 3D translation vector. ,right Read the rotation components and convert them to quaternions ,Will Encoded as a single-byte tag, The encoding is a floating-point number marker. To ensure communication consistency, candidate sequence numbers are written to the data area in byte order. With timestamp Candidate number The timestamp is an incrementing integer. Using milliseconds as the counter, a cyclic redundancy check is performed on all bytes except the check area to obtain the check value. , check value Write the pose data to the verification area. Encapsulation, using a TCP-based long connection to encapsulate the pose data. The target address and port are sent to the robot controller, and the target address and port are determined by the scene reference data. Give, The robot controller receives pose data. Then, first check the verification value. Perform a consistency check; if the check passes, the result is parsed. , , and When the executable decision result When it is executable, and Converted to the target pose of the robot end effector used internally by the robot controller. Target pose of robot end effector It includes three-dimensional position and three-dimensional attitude quantities, with the three-dimensional attitude quantities consisting of quaternions. The result of the conversion is The robot controller determines the positioning quality value. Select the preset assembly speed parameters and assembly force parameters, and denot the preset positioning quality segment threshold as follows. and And satisfy The preset assembly speed parameter set is denoted as The preset set of assembly force parameters is denoted as ,when Select assembly speed parameters With assembly force parameters ,when Select assembly speed parameters With assembly force parameters ,when Select assembly speed parameters With assembly force parameters ,in , , , , , , , All are based on scenario benchmark data Preset, The robot controller is based on the target pose. Plan the approach, assembly, and departure trajectories, and read the preset approach distance. Preset assembly stroke Distance from preset ,in , , For scalars expressed in units of length, from The rotational component is used to extract the tool axis direction vector of the end effector and transform the direction vector to the robot base coordinate system, respectively. , , Multiplying the direction vector yields three displacement increments, which are then superimposed onto... Generate near-endpoint pose Assembly section end position and the final position of the departure segment The approach segment trajectory starts from the robot's current pose. Planning to The assembly section trajectory from Planning to And load assembly speed parameters and assembly force parameters, leaving the segment trajectory from Planning to The waypoint sequence of the three trajectories, the interpolation period, and the selected assembly speed and force parameters are encapsulated into assembly action commands. The data is then sent to the robot servo execution module. The robot controller plans the above three-segment trajectory diagram based on the target pose, as shown in the diagram. Figure 9 As shown.

[0041] When the executable determination result When the operation is deemed unexecutable, the robot controller generates a stop assembly command. Stop assembly command To prevent the execution of the assembly section trajectory, the robot controller uses scene reference data. Read the preset safe pose Move the robot's end effector from its current pose. Plan to move to the preset safe position And keep the assembly speed and assembly force parameters inactive until manual reset or new pose data is obtained. It was determined to be executable.

[0042] Example 2 like Figure 12 As shown, this embodiment provides a three-dimensional visual positioning system for a target object whose local features are occluded, including: The data acquisition module is used to acquire color and depth maps of the assembly scene containing the target object and the fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; The occlusion perception and initial pose estimation module is used to render and generate a gripper reference depth map based on the gripper's 3D model and the camera robot extrinsic parameters, and compare it with the depth map to generate a gripper occlusion map that identifies the area where the gripper occludes the target object; the color image, the depth map, and the gripper occlusion map are input into the FoundationPose network, and the network outputs an initial pose candidate set of the target object and an occlusion matching score corresponding to each candidate pose under the constraints of the gripping pose range; The visible area point cloud processing module is used to extract the visible point cloud of the target object from the depth map according to the fixture occlusion map; and to generate the local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map. The pose correction and quality screening module is used to iteratively register the corresponding local model point cloud with the visible point cloud of the target object within the visible area defined by the fixture occlusion map for each candidate pose, thereby obtaining a finely registered pose; calculate the minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose; if the minimum safe distance is less than the interference threshold, then perform distance increment correction on the finely registered pose to obtain a corrected finely registered pose; based on the corrected finely registered pose, calculate the registration error and model overlap rate, and generate a positioning quality value by combining the corresponding occlusion matching score; and screen out the target six-degree-of-freedom pose according to the quality threshold. The decision output module is used to transform the target six-degree-of-freedom pose to the robot base coordinate system, calculate the robot's working pose, and generate an executable decision result by combining the positioning quality value and the robot's inverse kinematics reachability.

[0043] Example 3 Embodiment 3 of the present invention provides an electronic device.

[0044] An electronic device includes a memory, a processor, and a program stored in the memory and running on the processor. When the processor executes the program, it implements the steps in the three-dimensional visual localization method for a target object with partially occluded features as described in Embodiment 1 of the present invention.

[0045] The detailed steps are the same as those of the three-dimensional visual localization method for targets with partially occluded features provided in Example 1, and will not be repeated here.

[0046] Example 4 Embodiment 4 of the present invention provides a computer-readable storage medium.

[0047] A computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the steps in the three-dimensional visual localization method for a target object with partially occluded features as described in Embodiment 1 of the present invention.

[0048] The detailed steps are the same as those of the three-dimensional visual localization method for targets with partially occluded features provided in Example 1, and will not be repeated here.

[0049] Example 5 Embodiment 5 of the present invention provides a computer program product.

[0050] A computer program product includes software code, wherein the program in the software code performs the steps in the three-dimensional visual localization method for a target object with partially occluded features as described in Embodiment 1 of the present invention.

[0051] The detailed steps are the same as those of the three-dimensional visual localization method for targets with partially occluded features provided in Example 1, and will not be repeated here.

[0052] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present invention can be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

[0053] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0054] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0055] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0056] The above description is merely a preferred embodiment of this practice and is not intended to limit the scope of this practice. Various modifications and variations can be made to this practice by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of this practice should be included within the protection scope of this practice.

Claims

1. A three-dimensional visual localization method for a target object whose local features are occluded, characterized in that, include: Acquire color and depth images of the assembly scene containing the target object and fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; Based on the 3D model of the fixture and the extrinsic parameters of the camera robot, a fixture reference depth map is rendered and generated. The depth map is then compared with the fixture to generate a fixture occlusion map that identifies the area where the fixture occludes the target object. The color image, the depth map, and the fixture occlusion map are input into the FoundationPose network. Under the constraints of the clamping pose range, the network outputs an initial pose candidate set for the target object and an occlusion matching score corresponding to each candidate pose. Based on the fixture occlusion map, the visible point cloud of the target object is extracted from the depth map; and based on the initial pose candidate set and the fixture occlusion map, a local model point cloud of the target object corresponding to each candidate pose is generated. For each candidate pose, within the visible area defined by the fixture occlusion map, the corresponding local model point cloud is iteratively registered with the visible point cloud of the target object to obtain a finely registered pose. The minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose is calculated. If the minimum safe distance is less than the interference threshold, the finely registered pose is corrected by increasing the distance to obtain a corrected finely registered pose. Based on the corrected finely registered pose, the registration error and model overlap rate are calculated, and combined with the corresponding occlusion matching score, a positioning quality value is generated. The target six-degree-of-freedom pose is selected according to the quality threshold. The target six-DOF pose is transformed to the robot base coordinate system, the robot's working pose is calculated, and the executable determination result is generated by combining the positioning quality value and the robot's inverse kinematics reachability.

2. The method as described in claim 1, characterized in that, The process of rendering a reference depth map of the gripper based on the 3D model of the gripper and the extrinsic parameters of the camera robot, and comparing it with the depth map to generate a gripper occlusion map that identifies the area of ​​the gripper occlusion of the target object, includes: Based on the 3D model of the camera robot extrinsic gripper, the pose of the gripper in the camera coordinate system is determined. Based on the camera intrinsic parameters, the 3D model of the fixture is subjected to depth rendering to generate a fixture reference depth map that is aligned with the pixel coordinates of the depth map; The fixture reference depth map is compared pixel by pixel with the depth map. When the distance value of a pixel in the depth map is less than the theoretical distance value of the corresponding pixel in the fixture reference depth map, the pixel is marked as a fixture occlusion area, and a binarized fixture occlusion map is generated.

3. The method as described in claim 1, characterized in that, The FoundationPose network includes an occlusion cue fusion layer, a clamping constraint candidate layer, and a pose layer. The processing steps of the FoundationPose network include: Multi-scale features of the color image, depth image, and fixture occlusion image are extracted using independent encoders. In the occlusion fusion layer, channel weights are generated based on the features of the fixture occlusion map, and color features and depth features are weighted and fused pixel by pixel. In the clamping constraint candidate layer, the fused features are converted into the initial pose candidate set, and the clamping pose range mapping constraint is performed on the translation and rotation components of each candidate pose. In pose layering, each candidate pose is combined with fused features to output the occlusion matching score, which reflects the degree of matching between the pose and the input data in the visible region.

4. The method as described in claim 1, characterized in that, The step of extracting the visible point cloud of the target object from the depth map based on the fixture occlusion map, and generating a local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map, includes: Based on the occlusion markers in the fixture occlusion map, the distance values ​​of the corresponding pixels in the depth map are set to invalid, thus obtaining a visible depth map; Using camera intrinsic parameters, each effective pixel in the visible depth map is back-projected onto the camera coordinate system to form a three-dimensional point set as the visible point cloud of the target object; For each candidate pose in the initial pose candidate set, the three-dimensional model of the target object is transformed according to its corresponding six degrees of freedom parameters, and surface sampling is performed to obtain the point cloud of the candidate model. Project each candidate model point cloud onto the image plane, and remove points that fall into the occlusion mark area according to the fixture occlusion map to obtain the local model point cloud of the target object that corresponds one-to-one with each candidate pose.

5. The method as described in claim 1, characterized in that, The iterative registration and distance correction process includes: For each candidate pose, point pair matching is established between its local model point cloud and the visible point cloud of the target object, and an iterative nearest point algorithm is executed. After each iteration, the updated pose is mapped to the clamping pose range until the finely registered pose is obtained. Calculate the minimum safe distance between the target object and the surface of the fixture model under the fine registration pose. If the distance is less than the interference threshold, determine the distance increase direction based on the nearest point pair that generates the distance. Incrementally correct the translation component of the pose along this direction and re-ensure that the pose is within the clamping pose range. Iterate this process until the safe distance requirement is met to obtain the corrected fine registration pose.

6. The method as described in claim 1, characterized in that, The process of generating positioning quality values ​​and filtering target six-DOF poses, as well as generating executable decision results, includes: Based on the corrected fine registration pose, the average registration error between the local model point cloud and the visible point cloud of the target object is calculated as the registration error, and the proportion of model points falling within a certain neighborhood of the visible point cloud is calculated as the model overlap rate. By combining the corresponding occlusion matching score, the registration error and the model overlap rate are weighted and combined to generate the localization quality value; Based on the quality threshold, the occlusion matching score and localization quality value of the candidate poses are screened, and the corrected fine-fit pose with the best localization quality value is selected as the target six-degree-of-freedom pose. The target six-degree-of-freedom pose is transformed into the robot base coordinate system to obtain the target reference pose, which is then combined with the preset operation relative pose to obtain the robot operation pose; The robot's working pose is solved by inverse kinematics and joint range is checked. If the solution is successful, the joints are reachable, and the positioning quality value meets the quality threshold, it is determined to be executable; otherwise, it is determined to be unexecutable.

7. A three-dimensional visual positioning system for a target object whose local features are occluded, characterized in that, include: The data acquisition module is used to acquire color and depth maps of the assembly scene containing the target object and the fixture, camera robot extrinsic parameters, 3D model of the target object, 3D model of the fixture, clamping pose range, interference threshold and mass threshold; The occlusion perception and initial pose estimation module is used to render and generate a gripper reference depth map based on the gripper's 3D model and the camera robot extrinsic parameters, and compare it with the depth map to generate a gripper occlusion map that identifies the area where the gripper occludes the target object; the color image, the depth map, and the gripper occlusion map are input into the FoundationPose network, and the network outputs an initial pose candidate set of the target object and an occlusion matching score corresponding to each candidate pose under the constraints of the gripping pose range; The visible area point cloud processing module is used to extract the visible point cloud of the target object from the depth map according to the fixture occlusion map; and to generate the local model point cloud of the target object corresponding to each candidate pose based on the initial pose candidate set and the fixture occlusion map. The pose correction and quality screening module is used to iteratively register the corresponding local model point cloud with the visible point cloud of the target object within the visible area defined by the fixture occlusion map for each candidate pose, thereby obtaining a finely registered pose; calculate the minimum safe distance between the 3D model of the target object and the 3D model of the fixture under the finely registered pose; if the minimum safe distance is less than the interference threshold, then perform distance increment correction on the finely registered pose to obtain a corrected finely registered pose; based on the corrected finely registered pose, calculate the registration error and model overlap rate, and generate a positioning quality value by combining the corresponding occlusion matching score; and screen out the target six-degree-of-freedom pose according to the quality threshold. The decision output module is used to transform the target six-degree-of-freedom pose to the robot base coordinate system, calculate the robot's working pose, and generate an executable decision result by combining the positioning quality value and the robot's inverse kinematics reachability.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the program, it implements the steps of the three-dimensional visual localization method for a target object whose local features are occluded as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the three-dimensional visual localization method for a target object whose local features are occluded as described in any one of claims 1 to 6.

10. A computer program product, comprising software code, characterized in that, The program in the software code executes the steps of the three-dimensional visual localization method for a target object whose local features are occluded as described in any one of claims 1 to 6.

Images