Robot fine operation control system based on diffusion model and 3D vision

Abstract

The application discloses a robot fine operation control system based on a diffusion model and 3D vision, and particularly relates to the technical field of industrial robot fine operation control, and is used for solving the problem that the existing industrial robot is unstable in three-dimensional perception, action resolution is fixed and it is difficult to meet the space and contact constraints when the industrial robot performs fine operations such as screwing bottle caps and plugging connectors in a complex three-dimensional work scene, and the success rate and safety of the task are insufficient. By constructing a voxel feature field updated with time under the self-body coordinate system, an interactive complexity and physical constraint field is generated based on task coding, and adaptive resolution and action granularity scheduling, diffusion trajectory generation and closed-loop execution correction are linked to map the environment geometry, contact conditions and task stage to the end trajectory control, so that the fine operations such as screwing bottle caps, plugging connectors and folding flexible objects can still be stably completed under complex occlusion and contact constraints, and the safety and task success rate are considered.

Description

Technical Field

[0001] This invention relates to the field of precision operation control technology for industrial robots, specifically a precision operation control system for robots based on diffusion models and 3D vision. Background Technology

[0002] Currently, in delicate operation scenarios such as screwing on bottle caps, plugging and unplugging connectors, and folding flexible objects, industrial robots mostly rely on fixed-view RGB-D cameras or offline 3D models for environmental modeling. Generally, only static point clouds or coarse-grained occupancy grids are constructed, and traditional path planning and simple force control strategies are used to complete approach, alignment, and contact actions. These solutions often separate environmental perception, task constraints, and trajectory planning, rarely maintaining a 3D environment representation that updates over time in the robot's own coordinate system. They also lack structured summarization of information such as local surface morphology, occlusion stability, and historical task results, making it difficult to reflect subtle changes in the geometric state near the workstation under the production line cycle time.

[0003] On the other hand, existing methods mostly employ fixed resolution and uniform step size for end-effector trajectory generation in trajectory planning and control layers, typically considering only simple obstacle avoidance constraints or single force threshold protection. Even when some literature attempts to introduce force feedback or learning-based trajectory generation models, they mostly treat the end-effector pose sequence as an independent object for optimization, lacking consistency constraints with voxel-level environmental geometry, task-related contact constraints, and operational phase states. This leads to problems such as alignment deviation, contact slippage, and over-force shutdown in complex occlusion, narrow spaces, or strict contact conditions. Field engineers often need to compensate by relaxing the cycle time, using conservative limits, or repeated teaching.

[0004] In summary, existing technologies generally suffer from a core problem: in delicate operation stations with complex occlusions and strict contact constraints, there is a lack of an integrated control mechanism that can organically integrate the robot's own body coordinate system with the time-updated 3D geometric information, task-related interaction complexity and physical constraints, and operation stage recognition results into the end-effector trajectory generation and closed-loop error correction link. This makes it difficult to steadily improve the overall success rate of tasks such as unscrewing bottle caps, plugging and unplugging connectors, and folding flexible objects while ensuring safety boundaries. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a robot precision operation control system based on diffusion model and 3D vision to solve the problems mentioned in the background.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a robot precision operation control system based on diffusion model and 3D vision, comprising:

[0007] The Ego 3D perception module is used to collect RGB-D data from the fuselage viewpoint and construct a voxel feature field in its own body coordinate system based on the calibration matrix.

[0008] The interaction complexity and physical constraint construction module is used to calculate the interaction complexity index and contact constraint parameters at each location based on voxel features and task encoding, and generate the interaction constraint field.

[0009] The adaptive resolution and motion granularity scheduling module is used to simultaneously adjust the voxel resolution and end pose step size according to the interaction constraint field, so that high-complexity regions use high-resolution and fine-grained motion, and low-complexity regions use low-resolution and coarse-grained motion.

[0010] The diffusion trajectory generation module is used to generate end trajectory segments that satisfy the interaction constraint field on an adaptive action grid using a diffusion model, based on voxel features, interaction constraint field, and operation stage identifier.

[0011] The operation phase recognition module is used to identify the approach, alignment, contact, tightening, and exit stages based on the distance between the end and the target, torque changes, and image features, and output stage identifiers.

[0012] The closed-loop execution and error correction module is used to control joint movement based on trajectory segments, periodically collect visual and force feedback to update the interactive constraint field, and trigger local trajectory replanning when the end tracking error and contact deviation exceed a preset threshold.

[0013] Furthermore, the Ego 3D perception module includes:

[0014] The Ego 3D perception module constructs a voxel feature field in its own body coordinate system;

[0015] The RGB-D data and joint angles are time-aligned, and depth pixels are converted into 3D observation points and mapped to voxels using a calibration matrix.

[0016] Based on time window statistics, voxel occupancy flags and color statistics are updated and spatial filtering is performed.

[0017] Using an idempotent key obtained by combining the session identifier and the period sequence number, the voxel feature field snapshot, along with the calibration matrix version number, voxel resolution configuration version number, quality flag, and session identifier, is written to the shared memory buffer, and the writing is controlled by a mutex lock.

[0018] Furthermore, the interaction complexity and physical constraint building modules include:

[0019] The interaction complexity and physical constraint construction module is based on voxel feature fields and task encoding. Within the observation window, it calculates the interaction complexity index according to voxel occupancy status, surface shape changes, occlusion, and historical task results.

[0020] In its own coordinate system, an interaction constraint field snapshot is generated, which records the interaction complexity index and the set of contact constraint parameters. Using an idempotent key obtained by combining the session identifier and the cycle number, the interaction constraint field snapshot, along with the complexity field version number, the task parameter version number, and the quality flag, is written to a shared memory buffer, and the writing order is controlled by a mutex lock.

[0021] Furthermore, the interaction complexity and physical constraint construction module, based on the voxel geometric neighborhood, task encoding, and task parameter table, determines a set of contact constraint parameters for the voxel, consisting of the expected approach direction range, lateral offset limit, approach speed range, contact force range, and safe avoidance distance, and writes the contact constraint parameter set and interaction complexity index into the interaction constraint field snapshot.

[0022] When the quality indicator voxel feature field is in a low-quality state, an interactive constraint field snapshot is generated by selecting a preset set of safety contact constraint parameters based on the task parameter table.

[0023] Furthermore, the adaptive resolution and action granularity scheduling module includes:

[0024] The adaptive resolution and action granularity scheduling module divides voxels into low-complexity, medium-complexity, and high-complexity regions based on the interaction complexity index of voxels in the interaction constraint field and according to a preset complexity threshold. It also generates voxel query resolution and end pose step size for each region in combination with the operation stage identifier.

[0025] Using an idempotent key composed of a session identifier and a period number, an action-granularity configuration snapshot containing the session identifier, period number, resolution mapping rule version number, task parameter version number, spatial step size, attitude step size, and quality flag is written to shared memory.

[0026] Furthermore, the diffusion trajectory generation module includes:

[0027] The diffusion trajectory generation module reads snapshots of interaction constraint fields and action granularity configurations verified by version number and quality flag from shared memory according to session identifier and cycle number;

[0028] An adaptive motion grid is constructed based on the spatial step size and attitude step size in the snapshot of the motion granularity configuration, and an end trajectory fragment is generated by combining voxel features and interaction constraints.

[0029] Using an idempotent key obtained by combining the session identifier and the fragment number, a trajectory fragment message carrying the session identifier, fragment number, trajectory generation network version number, and configuration version number and quality flag related to trajectory generation is written to the trajectory buffer.

[0030] Furthermore, the diffusion trajectory generation module calls the trajectory generation network to calculate the position correction and attitude correction for the initial end pose sequence obtained by discretization on the adaptive motion grid according to a preset number of iterations;

[0031] After each iteration, constraint projection is performed on the trajectory points based on the occupancy state, interaction complexity index and contact constraint parameters in the interaction constraint field, so that the trajectory points are located in the free space region defined by unoccupied voxels and the displacement and attitude difference between adjacent trajectory points does not exceed the spatial step size and attitude step size in the action granularity configuration snapshot.

[0032] In the trajectory segment message, constraint violations are characterized by quality flags and error codes.

[0033] Furthermore, the operational phase recognition module includes:

[0034] The operation phase recognition module is used to calculate the distance between the end effector and the target, torque changes, and image features within the observation window based on the nominal pose of the end effector and the target area position, joint torque, end effector force sensing signal, and fuselage view image in the body coordinate system.

[0035] The operation phase state machine is driven to switch between the approach, alignment, contact, tightening and exit phases according to the preset phase judgment rules. It generates an operation phase state snapshot carrying session identifier, cycle number, phase identifier, task parameter version number and quality flag, and provides it to downstream modules through shared memory.

[0036] Furthermore, the closed-loop execution and error correction module includes:

[0037] The closed-loop execution and error correction module is used to convert the end target pose into joint angle target values ​​and send them to the control cabinet based on the end trajectory segment in the trajectory buffer and the self-body coordinate system calibration results;

[0038] In each control cycle, joint angles, force feedback, and visual feedback are collected to calculate end-point tracking error and contact deviation. The number of deviation cycles is counted within a short observation window. When the number of deviation cycles reaches a threshold, local trajectory replanning based on voxel feature fields and interactive constraint fields is triggered. If replanning fails, a safe shutdown is performed along a safe path.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] 1. By constructing a voxel feature field that updates over time in the body coordinate system, and generating interaction complexity and physical constraint fields based on voxel features and task encoding, and then combining adaptive resolution and action granularity scheduling, diffusion trajectory generation constrained by the interaction constraint field and operation phase recognition, as well as closed-loop execution and error correction modules, the environmental geometry, contact conditions and task stages are uniformly mapped into end-point trajectory control that meets safety constraints. This achieves the effect of stably completing fine operations such as unscrewing bottle caps, plugging and unplugging connectors and folding flexible objects under complex occlusion and contact constraint conditions, while taking into account both operational safety and task success rate.

[0041] 2. By introducing mechanisms such as time alignment, observation window statistics, quality flags, idempotent keys, version numbers, and evidence chain registration within each module, and by transmitting voxel feature fields, interaction constraint fields, action granularity configurations, diffusion trajectory fragments, operation stage states, and closed-loop execution states in an orderly manner through shared memory and control bus using structured messages, modular deployment, online degradation and safe shutdown, traceable operation process, and cross-workstation reuse and expansion are achieved without modifying the underlying robot control cabinet. This facilitates rapid debugging, troubleshooting, and long-term maintenance of the system under different robot models and task scenarios. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the robot precision operation control system based on diffusion model and 3D vision according to the present invention. Detailed Implementation

[0043] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0044] Example: Figure 1 A schematic diagram of the robot fine manipulation control system based on diffusion model and 3D vision of the present invention is given. The robot fine manipulation control system based on diffusion model and 3D vision includes:

[0045] The Ego 3D perception module is used to acquire RGB-D data from the fuselage viewpoint and construct a voxel feature field in its own body coordinate system based on the calibration matrix. Specifically, the implementation is as follows:

[0046] In a precision manipulation control system for robots used for screwing on bottle caps, plugging and unplugging connectors, and folding flexible objects, the Ego 3D perception module is deployed on an industrial computer near the robot control cabinet. It establishes wired communication with an RGB-D camera mounted on the robot's body and the robot controller via an industrial Ethernet network. The Ego 3D perception module refers to the software and hardware combination unit that acquires and maintains the voxel feature field of the surrounding 3D environment in the robot's own coordinate system. The RGB-D data from the robot's perspective refers to a set of color image frames and depth maps acquired by a color depth camera mounted near the end of the robot's wrist in each control cycle. Each pixel in the color image frame records the red, green, and blue brightness in grayscale values, while each pixel in the depth map frame records the distance from the camera's optical axis to the surface of the work scene in millimeters. The own coordinate system refers to a system with the robot's base fixed point as the origin and the direction along the production line conveyor belt as the first axis. A Cartesian coordinate system is established with the vertical direction as the second axis and the remaining horizontal direction as the third axis. This coordinate system remains unchanged regardless of the camera's installation position during robot operation. The calibration matrix refers to the set of rigid transformation parameters obtained through a one-time on-site calibration, used to transform 3D points in the camera coordinate system to the body coordinate system. It includes 3D rotation parameters and 3D translation parameters. The version is locked by number and the generation date is recorded for subsequent traceability. The voxel feature field refers to a 3D grid constructed within the workspace defined by the body coordinate system. Each grid cell corresponds to a voxel. Each voxel records features such as occupancy flag, surface reflection intensity, color statistics, number of observations, most recent observation timestamp, and quality flag. It is used to describe the geometric state and surface properties of the environment within a certain range around the robot. In this scenario, the quality flag refers to the state value used to characterize the reliability of the voxel feature field in the current control cycle. This state value is subsequently used by downstream modules as the basis for whether to accept the voxel feature field of that cycle.

[0047] In actual production line scenarios, the Ego 3D perception module is preferably deployed at single-station screw-on or plug-in stations, covering a 600-millimeter square area around the robot's end effector to meet the needs of most daily fine operation tasks. During the system initialization phase, maintenance personnel rigidly fix the depth camera to the robot's wrist flange, set the camera's field of view to cover the work tray area and the target object, and then use a calibration plate with known geometric dimensions for calibration. By using multiple sets of camera images and robot end-effector pose records, the rotational and translational relationships between the camera coordinate system and the robot's own coordinate system are calculated, and this relationship is stored in a read-only configuration file in the form of a calibration matrix. At the same time, a calibration matrix version number is generated and written into the version lock registration table, forming the first evidence chain record.

[0048] During system operation, every 20 milliseconds constitutes a control cycle. The Ego 3D perception module acquires one frame of color image and one frame of depth map from the camera according to the control cycle rhythm. Simultaneously, it obtains the current joint angle set, joint angle timestamps, and the current session identifier from the robot controller. The session identifier is a string used to mark the lifecycle of a fine-grained operation task; for example, a bottle cap tightening task uses the same session identifier from successful target detection to the cap tightening. The current joint angle set can be converted into the nominal pose of the end effector in its own body coordinate system within each control cycle using the robot's pre-configured kinematic parameter model. The module first aligns the camera frame and joint angle records acquired in the current cycle based on the timestamps. If the time difference between the color image and the depth map is within a preset tolerance range, such as no more than five milliseconds, the sampling for that cycle is considered valid. If it exceeds the tolerance, the visual frame for that cycle is discarded, and a visual delay alarm is recorded.

[0049] After time alignment, the Ego 3D perception module uses camera intrinsic parameters to convert the depth value and pixel row and column coordinates of each pixel into 3D points in the camera coordinate system. This conversion is performed on all pixels sequentially within a control cycle to obtain the set of 3D observation points for the current cycle. These 3D points are then transformed into the body coordinate system sequentially through the calibration matrix and the end pose relationship corresponding to the current joint angle, so that each observation point obtains a fixed spatial position in the body coordinate system, thereby eliminating the influence of camera viewpoint changes caused by robot posture changes.

[0050] If certain depth values ​​are found to be less than the minimum effective distance or greater than the maximum effective distance during this process, such as less than 250 mm or greater than 1200 mm, the corresponding points are marked as invalid measurements and ignored in subsequent calculations. At the same time, a depth effectiveness ratio statistic is generated within the module. In this scenario, the depth effectiveness ratio can be understood as the ratio of the number of pixels in the depth map within the effective distance range to the total number of pixels in the workspace within a control cycle, which is used for subsequent quality assessment. After completing the coordinate transformation, the Ego 3D perception module sets a cuboid workspace centered on the nominal working point of the robot's end effector in its own body coordinate system. The dimensions of this workspace along the conveyor belt direction, lateral direction, and vertical direction can be set to 600 mm, 400 mm, and 300 mm, respectively. Thus, a 3D grid is divided within this space using preset voxel side lengths. The voxel side length preferably adopts a hierarchical strategy, using 5 mm voxels in areas within 150 mm of the end effector and 1 cm voxels in more distant areas to balance accuracy and computational burden.

[0051] Within each control cycle, all 3D observation points within the workspace are assigned to corresponding voxels. The occupancy flag within a voxel is determined by the number of observation points and time window statistics. For example, if an observation point appears more than three times in a voxel within the last ten control cycles, it is considered that a solid surface exists. At the same time, the surface reflection intensity and color statistics are calculated based on the average brightness and color distribution of the colored pixels within the voxel. The color statistics are preferably obtained by using the arithmetic mean of the color value of the current cycle and the color values ​​of the previous nine cycles during each update. For voxels with insufficient observations, the characteristics of the previous cycle are maintained in the current cycle with an additional attenuation weight. The attenuation weight can be set to a fixed coefficient less than one, for example, 0.9. In each control cycle, the historical characteristics of the voxel are multiplied by this coefficient, so that the characteristics of voxels that have not been covered by new observations for a long time are gradually reduced until they are cleared.

[0052] To reduce the impact of depth sensor noise and dust scattering at the work site, the module applies a spatial filtering strategy to isolated voxels that are highly discontinuous with their surrounding voxels when constructing the voxel feature field. The spatial filtering strategy can be understood as counting the number of occupied voxels in a spherical neighborhood with a radius of 20 mm centered on the voxel. When there are no other occupied voxels in the spherical neighborhood besides the current voxel, the voxel is regarded as noise and its occupancy mark is removed. A smoother voxel distribution is obtained through this spatial comparison operation.

[0053] At the end of each control cycle, the Ego 3D perception module packages all voxel features in the current workspace, along with its own body coordinate system boundary information, calibration matrix version number, voxel resolution configuration version number, and session identifier, into a structured binary stream and stores it in a shared memory buffer. It also creates an index using the control cycle timestamp as the key, allowing subsequent interaction complexity and physical constraint construction modules to read it in chronological order. This buffer uses a circular storage strategy to retain the voxel feature field snapshot of the most recent two seconds to ensure that subsequent modules can still access it when they need to backtrack the observation window.

[0054] To ensure idempotency and sequential consistency, the Ego 3D perception module generates an idempotent key for each voxel feature field, which is obtained by concatenating the session identifier and the period number. Before writing to shared memory, it checks whether the idempotent key already exists. If it does, the writing is skipped to avoid duplication. If the current period number is behind the largest existing period number, an order disorder alarm is triggered and the result of that period is discarded, only forming an evidence chain record in the log. To avoid multiple threads writing to the shared memory buffer simultaneously in the same control cycle, the Ego 3D perception module preferably uses a mutex lock to control the writing process after generating the idempotent key. If it fails to acquire the mutex lock, it abandons the writing of the current period and re-attempts to generate a new idempotent key in the next control cycle, thus balancing sequential consistency and resource consumption.

[0055] The module and downstream modules use a lightweight communication method based on a publish-subscribe mechanism. The downstream module subscribes to the voxel feature field corresponding to a certain session identifier. When the voxel feature field of a new period is written to the shared buffer and the index is updated, the module notifies the downstream module that new content is available through a memory notification mechanism. The notification content only includes the session identifier, the period number and the address handle pointing to the shared buffer, thereby reducing network transmission overhead.

[0056] In terms of resource constraints, the Ego 3D perception module sets an upper limit on the computation delay of each frame of color image and depth map, which can be set to forty milliseconds. When the time difference between the camera completing sampling and the voxel feature field being written to the shared buffer exceeds this upper limit, the module discards the result of that cycle and generates a delay over-limit alarm, while keeping the voxel feature field of the previous cycle available in the buffer to ensure that the control system does not use expired visual information.

[0057] When multiple consecutive cycles result in delay exceeding limits or sampling failures, the module enters a degraded mode. The number of consecutive cycles can be set to ten control cycles. If more than five delay exceeding limits or sampling failures occur within ten consecutive control cycles, the quality flag is set to a low-quality state, and the system reports to the upper-level monitoring system via the control bus that only safe actions without delicate operations are permitted at the current workstation, such as stopping or slowly withdrawing from the workspace, to ensure that safety and compliance boundaries are not breached. In this scenario, the control bus can be an Ethernet-based field control network, and the upper-level monitoring system can be workstation monitoring software running on the production line monitoring server, used to receive quality flags and alarm information and restrict robot actions according to preset strategies.

[0058] In on-site testing, the Ego 3D perception module can assess geometric accuracy by comparing the projection of the voxel feature field with the position of the actual tooling reference ruler. For example, a reference block with a known planar position and height is placed in the robot's workspace, and the voxel index of the reference block in its own body coordinate system is recorded under several different robot postures. The average and maximum distance deviations between the center positions of these voxels and their theoretical positions are calculated. When the average deviation is kept within three millimeters and the maximum deviation is not more than five millimeters, the module can be considered to meet the geometric accuracy requirements of fine operation scenarios. In addition, by statistically analyzing the relationship between the success rate of one hundred bottle cap unscrewing or connector plugging / unplugging tasks and the aforementioned depth effectiveness ratio, the impact of the Ego 3D perception module on the overall task success rate can be examined. If the task success rate is significantly higher when the depth effectiveness ratio is higher than 90%, it can be proven that the integrity of the voxel feature field makes a significant contribution to fine operation.

[0059] Preferably, in a typical single-station bottle cap tightening scenario, the RGB-D camera resolution can be set to 640 x 480 pixels, the color and depth sampling frequency can be set to 30 frames per second, the control cycle can be set to 20 milliseconds, the workspace can be set to a cuboid with a length of 0.6 meters along the conveyor belt, 0.4 meters laterally, and a height of 0.3 meters, the voxel side length can be set to 5 millimeters within 150 millimeters of the end, 1 centimeter in the area between 150 and 300 millimeters from the end, and 2 centimeters in the area beyond 300 millimeters from the end, the time window length can be set to ten control cycles, the voxel occupancy determination threshold can be set to three, the lower limit of the effective depth distance can be set to 250 millimeters, and the upper limit can be set to 1,000 millimeters, the color statistics adopt the arithmetic mean of the most recent ten observations, the calibration matrix version number and the voxel resolution configuration version number are updated when the system changes the camera installation position or adjusts the workspace range, and new entries are formed in the version lock register, and each update records the modifier, time and reason.

[0060] For interface interaction, preferably, the Ego 3D perception module publishes the voxel feature field as a message containing a session identifier string field, a period sequence number integer field, a timestamp integer field, a calibration matrix version number string field, a voxel resolution configuration version number string field, a 3D raster size integer combination field, a voxel feature compressed byte stream field, and a quality flag integer field. If sampling is normal, the quality flag can be set to zero, indicating success. If the camera is offline, the quality flag can be set to 101. If the effective depth ratio is lower than a preset threshold, the quality flag can be set to 102. If the delay exceeds the limit, the quality flag can be set to 103. If the calibration matrix version is inconsistent with the downstream expected version, the quality flag can be set to 104. These minimum set fields can meet the needs of downstream modules to access the voxel feature field and determine quality.

[0061] In terms of equivalent replacement, the implementation of the Ego 3D perception module can be a software program set running on a general industrial computer or an embedded computing unit integrated into the robot controller. The RGB-D camera can be replaced with a laser scanner with a synchronous intensity channel. As long as the 3D geometry and surface properties of the environment can be reconstructed in voxel form within its own body coordinate system, and the principles of time alignment, coordinate transformation, voxel division, sliding window statistics, spatial filtering, version locking, and evidence chain recording are followed, those skilled in the art can reproduce the capabilities and scope of the Ego 3D perception module on different hardware platforms.

[0062] The interaction complexity and physical constraint construction module is used to calculate the interaction complexity index and contact constraint parameters at each location based on voxel features and task encoding, and generate the interaction constraint field. Specifically, the implementation is as follows:

[0063] The interaction complexity and physical constraint construction module is installed on the same industrial computer as the Ego 3D perception module. It establishes an access relationship with the Ego 3D perception module through intra-process shared memory and a communication relationship with the upper-level monitoring system and subsequent control modules through industrial Ethernet. In each control cycle, this module calculates the interaction complexity index and contact constraint parameters of each spatial position based on the voxel feature field and task code, and generates an interaction constraint field in its own body coordinate system. In this scenario, the interaction complexity and physical constraint construction module can be understood as a set of computing units. Around the voxel features around the nominal working point of the robot end effector, it provides a quantitative value of the operation difficulty and acceptable contact conditions for the local areas of interest to different task types. In each control cycle, it forms the interaction constraint field of the current cycle by summarizing and statistically mapping the voxel features over a period of time, which is then used by the subsequent action granularity scheduling and trajectory generation module to arrange the movement.

[0064] In this scenario, the interaction complexity index refers to a dimensionless value between zero and one. A larger value indicates that the operation requirements for the current task in the vicinity of that location are more refined and the risk is higher. This value is obtained by comparing and weighting voxel occupancy, surface shape changes, occlusion stability, geometric relationship with the target, and historical task results in each control cycle. In actual calculation, sub-indicators such as distance from the center of the task target area, local surface undulation, number of occlusions within the observation window, contact posture deviation, and historical failure rate can be normalized to sub-quantities between zero and one, and then these sub-quantities are weighted using pre-configured fixed weights. For example, the weights of the distance-related subquantity can be set to 0.3, the surface undulation subquantity to 0.2, the occlusion frequency subquantity to 0.2, the contact attitude deviation subquantity to 0.2, and the historical failure rate subquantity to 0.1. Within a control cycle, a weighted sum is calculated for each voxel according to this combination of weights, and the result is truncated to between zero and one, thus obtaining a unique interaction complexity index value. Contact constraint parameters in this scenario refer to a set of physical quantities related to the current task, including the expected approach direction range, the allowable lateral offset limit, the recommended approach velocity range, and the expected contact force range. Parameters such as safe avoidance distance are all specified in units. For example, the directional range is expressed in angles, the offset limit and avoidance distance in millimeters, the approach speed in millimeters per second, and the contact force in Newtons. These parameters are calculated within a control cycle by geometrically inferring the voxel's geometric neighborhood, task type, and environmental empty space, combined with a pre-calibrated task parameter table. The interaction constraint field in this scenario refers to the scene mapping obtained by recording the interaction complexity index and contact constraint parameter set for each voxel within the workspace defined by the body coordinate system. This mapping shares the same spatial partitioning and indexing method as the voxel feature field. The system includes a complexity field version number, a task parameter version number, and a quality flag. The complexity field version number identifies the calculation rules and weight combinations of the interaction complexity index and the contact constraint parameters. Each time the calculation rules are adjusted, the sub-index types are modified, or the weight ratio is changed, a new complexity field version number is generated and written to the version lock registration table. The registration table records the modification time, modifier, and modification reason to form a chain of evidence and ensure subsequent traceability. The task parameter version number identifies the version update status of each physical threshold and recommendation interval in the task parameter table. When the task parameter table is modified, a new version number is also generated and a record is added to the version lock registration table.

[0065] In this scenario, task coding refers to a set of identifiers used to describe the current fine-grained operation task category and key constraints. It includes at least a task type marker, target area description, target contact mode label, task priority, and task parameter version number. The task type marker can be any of the following categories: screwing on a bottle cap, plugging and unplugging a connector, or folding a flexible object. The target area description can be the spatial range near the edge of the target bottle opening, near the connector socket surface, or near the fold line of the fabric in the body coordinate system. The target contact mode label can distinguish between screwing, inserting, and folding. The task parameter version number corresponds one-to-one with the task parameter table pre-registered in the configuration file. By parsing the task coding, the interaction complexity and physical constraint construction module can determine the target area and physical boundary that should be referenced in the current cycle within each control cycle.

[0066] Within each control cycle, the interaction complexity and physical constraint construction module reads the voxel feature field snapshot written by the previous module from the shared memory buffer based on the current session identifier and cycle number. It then checks whether the quality flag, calibration matrix version number, and voxel resolution configuration version number attached to the snapshot meet expectations. If the quality flag is successful and the version number is consistent with the current module configuration, the voxel feature field for this cycle is considered valid. Otherwise, it preferably reverts to the voxel feature field that was confirmed to be valid in the previous cycle. At the same time, a quality degradation event is recorded in the quality log. When quality degradation occurs in multiple consecutive cycles, the interaction complexity and physical constraint construction module sets the quality flag of its own generated interaction constraint field to a low-quality state, and subsequent control modules can take conservative actions accordingly. To ensure time alignment, the interaction complexity and physical constraint construction module only uses voxel feature fields whose timestamp and control cycle time difference are within a preset tolerance range. The tolerance range can be consistent with the aforementioned Ego 3D perception module setting, for example, no more than five milliseconds. Voxel feature fields exceeding the tolerance are directly discarded and do not participate in the interaction complexity calculation for this cycle.

[0067] The interaction complexity and physical constraint construction module uses an observation window to statistically analyze the changes in voxel features over a recent period within each control cycle. The length of the observation window is preferably consistent with the window used by the Ego 3D perception module, for example, ten control cycles. Within this observation window, the number of times each voxel changes its occupancy status, the number of observations, the variation amplitude of surface reflection intensity and color statistics, and the variation of depth effectiveness ratio are collected to determine whether the voxel is in a stable and usable state in the recent period. For example, when a voxel is marked as occupied and has sufficient depth effective pixels in each cycle within the observation window, and the standard deviation of the surface color statistics is lower than a preset threshold, the surface corresponding to the voxel can be considered a stable structure, such as the edge of a bottle body, bottle cap, or a fixed socket surface. Such voxels have higher reliability in interaction complexity calculation, and the corresponding sub-indicators can be set to a stable flag close to zero or one during normalization.

[0068] For each voxel, the interaction complexity and physical constraint construction module first estimates the task relevance by the distance between the voxel center and the center of the target region in the task encoding. When the distance is less than the preset working radius, the voxel is considered to be directly related to the current task. The working radius can be given by the task parameter table. For example, in the bottle cap unscrewing task, the voxels within a range of several tens of millimeters around the bottle opening are considered to be directly related areas. Within the related areas, the operation difficulty is judged from the local geometry of the voxel. For example, the local surface undulation is statistically analyzed based on the height difference between adjacent voxels in the body coordinate system. The larger the undulation, the more irregular the surface, and the higher the difficulty of gripping and alignment. In actual calculation, the range and average gradient of the height values ​​of several neighboring voxels around a voxel can be statistically analyzed to measure whether there are steps, protrusions or grooves on the local surface. At the same time, combined with the observation number and depth effectiveness ratio provided by the Ego 3D perception module, if the voxel switches between occlusion and visibility multiple times within the observation window, it can be considered that there is frequent occlusion in the area. When the robotic arm approaches, it may be blocked by the body or other objects. The interaction complexity sub-index of such voxels can be improved during normalization.

[0069] The interaction complexity and physical constraint construction module also considers the relative posture conditions of voxels based on the target contact mode labels in the task code. For example, in the bottle cap tightening task, the angle between the normal of the surface where the voxel center is located and the expected tightening direction should be the focus. When this angle deviates from the pre-configured appropriate range, even if the geometry is stable, the operation complexity will increase, and a higher posture deviation sub-index needs to be given when calculating the interaction complexity. In the connector plugging and unplugging task, the allowable deviation of the insertion direction needs to be calculated based on the relationship between the socket opening direction and the voxel center position. When the allowable deviation angle window is narrow, the interaction complexity of the corresponding voxel should be increased to encourage subsequent trajectory generation to adopt finer-grained actions and strictly adhere to the alignment path. In the folding flexible object task, the interaction complexity and physical constraint construction module preferably focuses on the surface deformation of voxels near the folding line. By comparing the voxel height and color texture change range within the observation window, it is estimated whether the fabric has obvious wrinkles. The operation difficulty in wrinkled areas is higher than that in flat areas, and the interaction complexity sub-index can be given a higher value accordingly.

[0070] After combining the above sub-indicators into a single interaction complexity index with fixed weights, in order to facilitate unified use by subsequent modules, the interaction complexity index can be divided into three levels: low, medium, and high, based on a preset high complexity threshold. The high complexity threshold can be set to a decimal between zero and one and recorded in the task parameter table to distinguish between regions that require fine-grained operations and regions that allow coarse-grained operations. When subsequent modules read the interaction constraint field, they can directly select the appropriate action resolution based on this level.

[0071] Contact constraint parameters are determined within this module based on voxel feature fields, task encoding, and a pre-calibrated task parameter table. For example, the desired approach direction range can be determined by the nominal approach direction based on the target contact mode label in the task encoding, and then the allowable deflection angle range can be calculated based on the local surface normal and the geometry of the voxel neighborhood. When the local surface formed by adjacent voxels is close to a plane and there are no obvious protrusions around it, the approach direction range can be set wider; conversely, when the local surface has edges or is close to other objects, the approach direction range needs to be tightened. The allowable lateral offset limit can be determined based on the minimum distance between the voxel center and the nearest obstacle voxel, reserving a certain safety margin to ensure that the trajectory... Despite tracking errors and the compliance characteristics of the robotic arm, collisions are still avoided. The recommended approach speed range can be determined by combining the task type and the spatial density of the surrounding voxels. In areas with crowded environments and high interaction complexity, a lower speed is recommended to reduce control errors and contact impacts. The expected contact force range can be adjusted by referring to the target force values ​​for different task types and materials in the task parameter table and combining them with the force statistics collected from previous task execution records. For example, in the bottle cap unscrewing task, if the force history at a certain position in the historical task shows that excessive torque can easily cause stripping, the recommended maximum contact force can be slightly reduced based on the task parameter table, and a stricter contact constraint can be recorded in that area.

[0072] The existence of the observation window allows the interaction complexity and physical constraint building modules to adjust the level of trust in the current voxel based on the environmental stability over a recent period. For example, if a voxel is marked as unusable for more than half of the cycles within the observation window due to depth invalidity or missing observations, the interaction complexity index of that voxel in the current cycle can adopt a conservative strategy. This can be achieved by increasing the occlusion frequency sub-index and narrowing the allowable range of the contact constraint parameters to reflect the uncertainty of the region, thus avoiding excessive reliance on unreliable regions by subsequent control modules. Conversely, for voxels that are stable in the long term within the observation window, the contact constraints can be appropriately relaxed to improve overall operational efficiency.

[0073] The computation results of the interaction complexity and physical constraint construction module are organized into a new interaction constraint field snapshot at the end of each control cycle. In this scenario, the interaction constraint field snapshot includes session identifier, cycle number, timestamp, workspace boundary in the body coordinate system, complexity field version number, task parameter version number, quality flag, and a set of interaction complexity indicators and contact constraint parameters corresponding one-to-one with voxel indexes. This set is arranged in the order of row, column, and layer number of voxels in the 3D raster and stored in a dedicated area of ​​shared memory after compression. It uses the same idempotent key naming method as the aforementioned voxel feature field. Before writing to the interaction constraint field, the interaction complexity and physical constraint construction module also checks whether the idempotent key has already appeared and uses a mutex lock to ensure that only one thread writes to the area in the same control cycle. If the mutex lock is not acquired, the current cycle writing is abandoned and a new interaction constraint field is regenerated in the next control cycle.

[0074] To ensure time resource constraints, the total latency of the interaction complexity and physical constraint construction module from reading the voxel feature field snapshot to completing the writing of the interaction constraint field to shared memory is set to an upper limit, which can be set to 30 milliseconds. When the computation time exceeds this upper limit, the writing of the interaction constraint field in this cycle is canceled, and the quality flag is set to the specified error state. When the downstream module receives an interaction constraint field with the corresponding cycle number but whose quality flag is not successful, it should default to using the interaction constraint field that has been confirmed as valid in the previous cycle and record an event of using the old interaction constraint field in its own log. For the quality flag of the interaction constraint field, a minimum feasible encoding set can preferably be used. A quality flag of zero indicates that the interaction constraint field calculation in this cycle is successful. A quality flag of 201 indicates that the voxel feature field quality is insufficient to support the interaction constraint calculation. A quality flag of 202 indicates that the computation time of the interaction complexity and physical constraint construction module in this cycle exceeds the set upper limit. A quality flag of 203 indicates that the complexity field version number or task parameter version number is inconsistent with the current configuration. Other values ​​can be reserved for extended use.

[0075] The interaction complexity and physical constraint construction module is connected to the subsequent adaptive resolution and action granularity scheduling module through a shared memory handle and a lightweight notification mechanism. Downstream modules subscribe to the interaction constraint field corresponding to a specified session identifier. When the interaction constraint field of a new cycle is successfully written and the index is updated, the interaction complexity and physical constraint construction module sends a notification containing the session identifier, cycle number and quality flag to the downstream through memory notification. The downstream module determines whether to obtain the new interaction constraint field and access the compressed data in the shared memory through the same idempotent key. Before accessing, it checks whether the complexity field version number and the task parameter version number are consistent with its own configuration. If they are inconsistent, it is considered that the current configuration is inconsistent with the parameter version that generated the interaction constraint field. Optionally, it can fall back to the interaction constraint field of the previous cycle or enter the security degradation strategy.

[0076] The safety and compliance boundaries of the interaction complexity and physical constraint construction module can be constrained by the distribution of interaction complexity indicators and quality indicators. If most voxel complexity indicators in the workspace exceed the preset high complexity threshold within a control cycle, or if the quality indicator indicates that the quality of the voxel feature field and interaction constraint field is poor, the module will report to the upper monitoring system that the current workstation environment is not suitable for performing fine operations, and notify the subsequent control module through the pre-configured error code that only safe actions such as evacuation and stop are allowed to be performed, so as to prevent the forced execution of plugging or tightening tasks when the environment is severely obscured or the perception information is severely lacking.

[0077] In the on-site verification phase, the rationality of the module can be verified by comparing the relationship between the interaction complexity index and the actual task failure rate. For example, a certain number of bottle cap unscrewing tasks can be selected, and the interaction complexity index of several voxels around the bottle opening in each task can be statistically analyzed along with whether the task failed due to misalignment, stripped teeth, or other reasons. The correspondence between the segmented interaction complexity index and the failure probability can be plotted. When the failure probability of the sample segment with a higher interaction complexity index is significantly higher than that of the sample segment with a lower complexity index, it indicates that the index is positively correlated with the actual operational difficulty. At the same time, in the connector insertion and removal task, by recording the insertion success rate and the number of collisions during the insertion process under different combinations of contact constraint parameter settings, it is possible to infer whether the contact constraint parameters are set reasonably, thereby adjusting the offset limit and approach speed range in the task parameter table.

[0078] Preferably, in a typical single-station bottle cap tightening scenario, the interaction complexity index can be set to a decimal between zero and one. Below 0.3 is considered low complexity, suitable for larger action step sizes; the middle range is considered medium complexity; and above 0.7 is considered high complexity, requiring fine-grained operation. The expected deflection angle for the approach direction range can be set to ±15 degrees, the allowable lateral offset limit can be set between 2 mm and 4 mm, the recommended approach speed range can be set to 50 mm to 150 mm per second, the expected contact force range can be set to 0 to 20 Newtons, and the voxels within 20 mm of the bottle opening are within the observation window. If the color changes are stable and observed for ten consecutive control cycles, the interaction complexity index can converge to between 0.6 and 0.8, while the interaction complexity index of the conveyor belt surface and voxels far from the bottle can remain below 0.2. In a representative set of experiments, one hundred bottle cap twisting tasks can be selected for statistical analysis. When the control strategy is set strictly according to the contact constraint parameters provided by the interaction constraint field, the overall success rate of the task can be increased to over 95%. However, when the interaction complexity index is ignored and a coarse-grained strategy with a fixed approach direction and fixed speed is adopted, the success rate may only be around 75%.

[0079] In alternative approaches, the interaction complexity and physical constraint construction module can also be designed using a rule-based method. Instead of introducing multi-layer parameter functions for offline fitting, the correspondence between complexity indicators and geometric distance, surface undulation, and occlusion frequency under different task types can be clearly defined in the task parameter table. For example, it can be directly stipulated that the complexity of all voxels within the radius of the target region is high, that between the radius and twice the radius is medium, and that outside twice the radius is low. The complexity can be adjusted by increasing or decreasing it with fixed surface undulation thresholds and occlusion frequency thresholds. As long as the principles of observation window statistics, version locking, idempotent keys, and quality flags mentioned above are still followed, those skilled in the art can choose this simpler implementation form according to the actual working conditions, and achieve better overall results when computing resources are tight or the on-site task type is relatively simple.

[0080] The adaptive resolution and motion granularity scheduling module is used to simultaneously adjust the voxel resolution and end-effector pose step size according to the interaction constraint field, so that high-complexity regions use high-resolution and fine-grained motions, and low-complexity regions use low-resolution and coarse-grained motions. Specifically, it is implemented as follows:

[0081] The adaptive resolution and motion granularity scheduling module is installed on the same industrial computer as the aforementioned Ego 3D perception module and interaction complexity and physical constraint construction module. It accesses the voxel feature field and interaction constraint field via shared memory and is associated with the diffusion trajectory generation module via industrial Ethernet. Within each control cycle, this module simultaneously adjusts the voxel resolution and end-effector pose step size based on the interaction constraint field. This allows high-resolution and fine-grained motion to be used in high-interaction-complexity areas, while low-resolution and coarse-grained motion is used in low-interaction-complexity areas, thus balancing safety and efficiency within the same task session. The end-effector pose step size in this scenario refers to… The spatial displacement and attitude change of the robot's end effector in its nominal pose in its own body coordinate system within two adjacent control cycles are calculated in each control cycle by the difference between the target pose of the previous cycle and the target pose of the current cycle. The adaptive resolution in this scene refers to the same voxel feature field and interaction constraint field. When querying local environment information, different voxel spatial sampling intervals and neighborhood sizes are used so that high-complexity areas use more detailed voxel grids to participate in subsequent trajectory generation, while low-complexity areas use downsampled voxel information to participate in trajectory generation.

[0082] The adaptive resolution and action granularity scheduling module reads a snapshot of the interaction constraint field that was confirmed to be valid in the previous cycle from shared memory in each control cycle using the session identifier and cycle number. It checks whether the complexity field version number and the task parameter version number are consistent with the configuration of this module, and whether the quality flag of the interaction constraint field is successful. If the version number is consistent and the quality flag is successful, the interaction constraint field of the current cycle is adopted. Otherwise, it preferably rolls back to the interaction constraint field of the previous cycle and records a version or quality degradation event in the log of this module. At the same time, the current cycle can still continue to be executed, but the granularity is not relaxed. To ensure time alignment, the adaptive resolution and action granularity scheduling module only processes interaction constraint fields whose timestamp and the time difference of the current control cycle do not exceed the preset tolerance. The tolerance can be set to no more than five milliseconds. If the tolerance is exceeded, the scheduling of this cycle will keep the configuration of the previous cycle unchanged.

[0083] The module internally pre-configures hierarchical complexity thresholds and corresponding resolution level mapping relationships. The complexity thresholds are derived from offline calibration of typical workstations and a batch of experimental statistical results. Version lock registration is performed through the resolution mapping rule version number. When the threshold or mapping rule is adjusted, a new mapping rule version number is generated and the modification time and reason are recorded in the version lock registration table. During runtime, the adaptive resolution and action granularity scheduling module classifies voxels with complexity below the low complexity threshold into low complexity regions, voxels with complexity above the high complexity threshold into high complexity regions, and the rest into medium complexity regions, based on the interaction complexity index of each voxel in the interaction constraint field. A corresponding set of spatial mask regions is formed in its own body coordinate system. This set remains smooth within a short observation window, which can be set to ten control cycles. Within the observation window, the complexity level of a voxel is changed only when the classification results are consistent in most cycles, in order to avoid frequent resolution jumps due to single-cycle noise. For example, if the complexity index of a voxel exceeds the high complexity threshold more than seven times in the last ten control cycles, then the voxel is stably classified into the high complexity region in this cycle.

[0084] For voxel resolution, the adaptive resolution and action granularity scheduling module does not directly modify the basic voxel grid constructed by the Ego 3D perception module. Instead, when querying local environmental features, it sets different sampling step sizes and neighborhood ranges according to the complexity level. For low-complexity regions, representative voxels can be extracted in steps of multiple voxel side lengths as local environmental descriptions. The sampling step size can preferably be set to three to four times the basic voxel side length and the neighborhood radius can be set to three to four basic voxel side lengths. For medium-complexity regions, the sampling step size can be set to twice the basic voxel side length and the neighborhood radius can be set to two basic voxel side lengths. For high-complexity regions, voxels are traversed one by one according to the basic voxel side length and a smaller neighborhood is expanded. The neighborhood radius can be set to one basic voxel side length to increase the weight of environmental details in the trajectory generation stage. The sampling step size and neighborhood range are generated by the adaptive resolution and action granularity scheduling module as voxel query resolution parameters in each control cycle and are passed to the diffusion trajectory generation module together with the interaction constraint field.

[0085] For the end-effector pose step size, the adaptive resolution and motion granularity scheduling module selects the maximum allowable pose step size limit and the default pose step size value in this cycle based on the complexity level and contact constraint parameters of the set of voxels closest to the current nominal position of the end-effector in the task code and interaction constraint field, combined with the operation stage identifier. The operation stage identifier can be given by the previous level task state machine in this scenario, for example, indicating that the current stage is approaching, alignment, or contact, and is read by this module in each control cycle along with the interaction constraint field as part of the task code. The default pose step size value can be given by different spatial step size and attitude step size table entries corresponding to three levels of low complexity, medium complexity, and high complexity. The spatial step size and attitude step size tables are locked by the task parameter table version.

[0086] In each control cycle, this module first calculates the weighted average of the complexity levels of the voxel at the current nominal position of the end and its neighboring voxels. In this scenario, this weighted average can be obtained by selecting all voxels in the body coordinate system with the nominal position of the end as the center and a radius twice the side length of the basic voxel. The complexity index of these voxels is weighted and summed according to the reciprocal of the distance from the nominal position of the end, and then normalized according to the weighted sum. Repeating this calculation in each control cycle yields a unique neighborhood complexity representative value. When the weighted result falls in the high complexity range, the default pose step size for this cycle is set to the fine-grained step size corresponding to the high complexity level, and the upper limit of the maximum pose step size is tightened to prevent excessive movement in one step from crossing the safe zone. When the weighted result falls in the low complexity range, a larger default step size can be used to speed up navigation. This rule is still followed in the approach phase, while in the contact phase, the fine-grained step size corresponding to the high complexity is forcibly used, regardless of whether the local value of the interaction complexity is slightly lower, to ensure the stability of the contact process.

[0087] Before the end of each control cycle, the adaptive resolution and action granularity scheduling module organizes the voxel query resolution parameters and end-effector pose step size parameters effective in this cycle into an action granularity configuration snapshot. This snapshot includes session identifier, cycle number, timestamp, resolution mapping rule version number, task parameter version number, spatial step size value, pose step size value, and complexity level threshold image. It is stored in a designated area of ​​shared memory using the same idempotent key naming method as the aforementioned modules. During writing, a mutex lock is used to ensure that only one action granularity configuration snapshot is written in the same control cycle. If the mutex lock acquisition fails, the writing for the current cycle is abandoned, and the configuration is regenerated in the next control cycle based on the latest interaction constraint field. The action granularity configuration snapshot is accompanied by a quality flag. A quality flag of zero indicates that the action granularity configuration for this cycle is valid. A quality flag of 301 indicates that the interaction constraint field has not been updated and the configuration of the previous cycle is used. A quality flag of 302 indicates that the delay exceeds the preset limit. The delay limit can be set to 20 milliseconds, meaning that the total time from reading the interaction constraint field to writing the action granularity configuration snapshot does not exceed 20 milliseconds. When this value is exceeded, the configuration for this cycle is abandoned, and a delay over-limit event is recorded in the log.

[0088] The diffusion trajectory generation module subscribes to the action granularity configuration snapshot corresponding to the session identifier. After receiving the memory notification, it reads the latest snapshot according to the cycle number and checks whether the resolution mapping rule version number and the task parameter version number are consistent. If the versions are consistent and the quality flag is zero, the current configuration is used to generate an adaptive action grid. If the quality flag is 301, the granularity configuration applied in the previous cycle is used. If the quality flag is 302 or the version is inconsistent, a conservative strategy is triggered, allowing only slow movement or pausing in low-complexity areas at a step size not exceeding medium, to ensure that the safety and compliance boundaries are not breached.

[0089] The effectiveness of the adaptive resolution and action granularity scheduling module in the field can be evaluated by comparing whether granularity scheduling significantly reduces the number of collisions or contact failures in high-complexity areas under the same task conditions. For example, in a bottle cap twisting station, select 100 tasks, record the number of contact failures within 10 mm of the bottle opening without adaptive granularity, and then record the same number of failures with the adaptive granularity enabled. If the number of failures significantly decreases after enabling adaptive granularity without a significant change in overall completion time, the action granularity scheduling can be considered effective. Preferably, in typical scenarios, the low-complexity threshold can be set to 0.3, the high-complexity threshold can be set to 0.7, the spatial step size in low-complexity areas can be set to 5 mm to 10 mm per cycle, and the medium-complexity threshold can be set to... The spatial step size in the complex region can be set to 3 to 5 millimeters per cycle, and the spatial step size in the high-complexity region can be set to 1 to 3 millimeters per cycle. The corresponding attitude step size can be set to 1 degree, 0.5 degrees, and 0.2 degrees per cycle, respectively. When the voxel complexity is consistently between 0.6 and 0.8 within 20 millimeters of the bottle mouth, the actual step size of the end effector during the approach and alignment phases can converge to approximately 2 millimeters and 0.3 degrees, respectively. When the complexity in the conveyor belt region far from the bottle body is less than 0.2, the end effector can use a step size of approximately 8 millimeters for rapid navigation. Statistical analysis of 100 task results shows that without adaptive granularity, alignment failures and slippage near the bottle mouth occurred a total of 25 times. After enabling adaptive granularity, the number of failures decreased to no more than 5 times, and the overall average task completion time changed by less than 5%.

[0090] In alternative approaches, the adaptive resolution and action granularity scheduling module can also use segmented rules instead of fine-grained weighted averages to determine the step size. For example, the fixed step size and voxel query step size can be determined by directly looking up a table based on the complexity level of the voxel where the current end is located and the operation stage, without weighting the neighborhood complexity. As long as the principles of complexity threshold segmentation, observation window smoothing, version locking, idempotent keys, and quality flags are still followed, those skilled in the art can implement an equivalent granularity scheduling form in a computing resource-constrained environment.

[0091] The diffusion trajectory generation module is used to generate end trajectory segments that satisfy the interaction constraint field on an adaptive action grid using a diffusion model, based on voxel features, interaction constraint fields, and operation phase identifiers. Specifically, the implementation is as follows:

[0092] The diffusion trajectory generation module is deployed on the same industrial computer as the Ego 3D perception module and the adaptive resolution and motion granularity scheduling module. It accesses the voxel feature field, interaction constraint field, and motion granularity configuration snapshot via shared memory according to session identifier and cycle number. It communicates with the trajectory following unit in the robot control cabinet via industrial Ethernet. In each planning cycle, this module generates end-effector trajectory segments that satisfy interaction constraints on the adaptive motion grid and writes them to the shared buffer, based on voxel features, interaction constraint field, and operation stage identifier. In this scenario, the end-effector trajectory segment refers to a series of target pose sequences of the robot end effector in its own body coordinate system within several consecutive control cycles. Each target pose includes three-dimensional position coordinates and attitude angle description, accompanied by a local time interval, forming a motion planning unit of finite length. In this scenario, the adaptive motion grid refers to the set of grid nodes discretized around the current nominal end position according to the spatial step size and attitude step size given by the motion granularity configuration snapshot. These nodes are arranged according to the spatial step size along the transport direction, side, and vertical, and according to the attitude step size in the attitude space, so that each adjacent pose on the trajectory can find a corresponding node on the grid or be obtained by interpolation of adjacent nodes.

[0093] At the start of each planning cycle, the diffusion trajectory generation module reads the interaction constraint field snapshot written by the interaction complexity and physical constraint construction module and the action granularity configuration snapshot written by the adaptive resolution and action granularity scheduling module from shared memory in the most recent cycle. It also obtains the current operation stage identifier from the task code and checks whether the complexity field version number, resolution mapping rule version number, and task parameter version number are consistent with the current configuration of the diffusion trajectory generation module. It also checks whether the quality flags on the two snapshots are successful. If the version numbers are consistent and the quality flags are both successful, the scene information and granularity configuration of the current cycle are adopted. Otherwise, it preferably rolls back to the snapshot that has been confirmed as valid in the previous cycle and records a version or quality degradation event in the module log, thereby ensuring that trajectory planning is not based on unreliable environmental information. To maintain time alignment, the diffusion trajectory generation module only performs trajectory diffusion calculations when the time difference between the timestamp of the interaction constraint field and action granularity configuration snapshot and the current planning cycle does not exceed a preset tolerance. The tolerance is preferably no more than five milliseconds. If the tolerance is exceeded, the trajectory segment is not regenerated in this cycle, but the remaining part of the previous segment that was not executed is used.

[0094] The diffusion trajectory generation module constructs an adaptive action grid in its own body coordinate system, starting from the current nominal pose of the end effector, based on the spatial step size and attitude step size given by the snapshot of the action granularity configuration in each planning cycle. The grid range preferably covers the space that the end effector may reach in several subsequent control cycles. For example, it expands by several step size levels in each of the three spatial directions and several step size levels in each of the three attitude angle directions. In the voxel feature field, it queries the occupancy status, interaction complexity index and contact constraint parameters of the corresponding voxel for each grid node. When it finds that a node falls within an occupied voxel or the complexity of the adjacent voxels is too high and the current operation stage does not need to enter the area, the node is marked as an impassable node.

[0095] In the offline phase, the diffusion trajectory generation module collects successful task trajectories from the actual production line as samples. Combining these with corresponding voxel features, interaction complexity distribution, and task encoding, it constructs a trajectory sample set. Each trajectory sample is discretized into several continuous target pose points and accompanied by a task success marker. The trajectory generation network is trained on this sample set. When the network receives a randomly perturbed pose sequence along with its corresponding task encoding, operation stage identifier, local voxel feature summary, and interaction constraint summary, it provides a set of corrections for that pose sequence. This corrects the pose sequence to more closely approximate the statistical distribution of historical successful trajectories and satisfies spatial and contact constraints. The voxel feature summary and interaction constraint summary are obtained by extracting several key points at time intervals along the candidate trajectory path and statistically analyzing the average and maximum values ​​of the interaction complexity index, the reserved safety distance, and the recommended contact force range and approach speed range within a spherical neighborhood of each key point. After training, a fixed version number is assigned to the trajectory generation network, and the version number, training data source, and training parameters are recorded in the version lock registry for future traceability.

[0096] During online operation, the diffusion trajectory generation module first constructs a nominal trajectory as the initial trajectory for the diffusion process in each planning cycle. This nominal trajectory can be extended from the current end nominal pose to the target area in a straight line or a gentle curve for several control cycles according to the nominal approach direction and spatial step size given in the task parameter table. The trajectory is discretized into several pose points that match the adaptive action grid nodes. The target position, target attitude and local time interval are recorded at each pose point. Small random perturbations are superimposed on the entire trajectory to form an initial noise trajectory. The amplitude of the random perturbation can preferably vary with the operation stage and interaction complexity. For example, larger perturbations are allowed in the approach stage to explore multiple paths, while only small perturbations are allowed in the contact stage to maintain trajectory stability.

[0097] Subsequently, the diffusion trajectory generation module iteratively corrects the initial noise trajectory within a preset number of iterations. In each iteration, the current trajectory point sequence, task code, operation stage identifier, and voxel feature summary and interaction constraint summary obtained along the trajectory are encoded into a trajectory state vector and fed into the trajectory generation network to obtain the correction increment for the position and attitude of each trajectory point. The trajectory points are then updated at a fixed step size. After each update, constraint projection operations are performed on each trajectory point, i.e., checking whether the position of the trajectory point falls within the free space region corresponding to the passable grid node. If it deviates from the free space boundary, the position is pulled back to the nearest safe point. The distance between the trajectory point and the surrounding occupied voxels is checked to see if it is greater than the safe avoidance parameter in the contact constraint. The distance is set to a lower limit, and the displacement between trajectory points is checked to see if it exceeds the upper limit of the spatial step size in the action granularity configuration, and whether the attitude difference exceeds the upper limit of the attitude step size. When these conditions are not met, truncation or sliding correction is performed along the allowed direction, so that the trajectory gradually shrinks towards the feasible region that satisfies the interaction constraints after each iteration. When the operation phase is marked as the contact phase, the diffusion trajectory generation module also adds constraint projection for the contact attitude near the end of the trajectory. For example, the normal direction of several trajectory points at the end is compared with the expected approach direction range given in the interaction constraint field. When the included angle exceeds the allowed range, the attitude of the trajectory point is adjusted by the minimum angle, while limiting the offset between the end position point and the center of the target contact area to not exceed the allowed lateral offset limit.

[0098] The diffusion trajectory generation module maintains a constraint violation counter throughout the entire iteration process. After each iteration, it counts the number of constraint corrections that occur among all trajectory points on the trajectory. When the total number of violations does not exceed the preset upper limit at the end of the set iteration round and the trajectory end has entered the target area and the contact posture meets the requirements, the trajectory segment is marked as a valid trajectory segment. Otherwise, it is considered that no feasible trajectory has been found in this diffusion process, triggering a retreat strategy. The end trajectory segment is degenerated into a conservative trajectory that maintains its position within the current safe area or slightly retreats along the safe direction, and the reason for failure is recorded in the quality flag.

[0099] After each successful planning, the diffusion trajectory generation module writes trajectory segments to the trajectory buffer in shared memory as messages. These messages include a session identifier, segment number, start cycle number, number of trajectory points, sequentially arranged target pose sequence, corresponding time interval, trajectory generation network version number, resolution mapping rule version number, task parameter version number, quality flag, and error code. The segment number and session identifier combine to form an idempotent key. Before writing, the module checks if the same idempotent key already exists in the buffer; if it does, the writing is skipped to avoid duplication. During writing, a mutex lock controls that only one thread on the same session can write to the trajectory buffer within a given time period. If acquiring the lock fails, the planning results are only retained within the module and not written to shared memory. The system will retry generating new segments in the next planning cycle. The total time for trajectory generation, from reading scene information and executing several rounds of diffusion iterations to completing the writing, is set to an upper limit, preferably not exceeding 50 milliseconds. If the actual time exceeds the upper limit, the segment will not be written to shared memory, and the error code will be set to 402 to indicate a planning timeout. An error code of 0 indicates that the trajectory segment was successfully generated. An error code of 401 indicates that the upstream interaction constraint field or action granularity configuration quality is insufficient. An error code of 403 indicates that a trajectory that simultaneously satisfies spatial constraints and contact constraints cannot be found within a given iteration round. After receiving the trajectory segment message, the downstream trajectory following module decides whether to execute the segment based on the error code. When the error code is non-zero, only a backoff conservative trajectory or remaining stationary is executed.

[0100] The diffusion trajectory generation module can be evaluated on-site by comparing the number of collision alarms, alignment failures, and overall task completion time in areas with high interaction complexity before and after enabling diffusion trajectory generation. For example, in the bottle cap twisting station, two groups of 100 tasks each are selected. Without enabling the diffusion trajectory generation module and only approaching in a straight line according to the rules, there are 30 collision alarms and 20 alignment failures within 10 millimeters of the bottle opening. After enabling diffusion trajectory generation and keeping other parameters unchanged, the number of collision alarms drops to no more than 5, the number of alignment failures to no more than 5, and the average task completion time increases by no more than 10%.

[0101] Preferably, under typical working conditions, the diffusion iteration rounds can be set to five to ten rounds. The adaptive action grid space step size is one to three millimeters in the high complexity region, three to five millimeters in the medium complexity region, and five to ten millimeters in the low complexity region. The attitude step size is 0.2 to 0.5 degrees in the high complexity region, 0.5 to 1 degree in the medium complexity region, and 1 to 2 degrees in the low complexity region. The trajectory segment length can cover ten to twenty control cycles. When these values ​​are applied to the two tasks of unscrewing bottle caps and plugging and unplugging connectors, the overall task success rate can be increased to over 95% in the above one hundred task samples.

[0102] In an alternative approach, the diffusion trajectory generation module can also directly query the trajectory prototype closest to the current task code and operation stage identifier from the trajectory prototype library obtained from offline calibration without using multiple rounds of random perturbation correction. This trajectory prototype is then mapped onto the current adaptive action grid, and the position and attitude of the trajectory points are corrected through a one-time constraint projection operation. As long as the evidence chain traceability mechanism of version locking, idempotent key control, quality flags and error codes is still followed, those skilled in the art can achieve an end-point trajectory planning capability equivalent to that of the diffusion trajectory generation module in a field with limited computing resources.

[0103] The operation phase recognition module is used to identify the approach, alignment, contact, tightening, and exit stages based on the distance between the end and the target, torque changes, and image features, and output stage identifiers. Specifically, the implementation is as follows:

[0104] The operation phase recognition module is deployed on the same industrial computer as the Ego 3D perception module, interaction complexity and physical constraint construction module, adaptive resolution and motion granularity scheduling module, and diffusion trajectory generation module. It accesses the current end effector nominal pose, target area position in its own coordinate system, voxel feature field, and interaction constraint field snapshots via shared memory according to session identifier and cycle number. It calculates the torque of each joint using the joint angles and drive currents provided by the robot control cabinet, and collects triaxial force and torque signals from force sensors installed on the end effector or wrist. At the same time, it acquires time-aligned RGB image frames and depth frames from the body-view camera. In each control cycle, it identifies the current task as being in the approach, alignment, contact, tightening, or exit stage based on the distance between the end effector and the target, torque changes, and image features, and provides the corresponding operation stage identifier. In this scenario, the operation stage identifier refers to the discrete state value used to distinguish the five operation stages. It can be set to one of the five values: approach, alignment, contact, tightening, and exit, and is accompanied by a stage version number to support subsequent traceability.

[0105] In this scenario, the distance between the end effector and the target refers to the Euclidean distance between the end effector reference point and the center point of the target region in the task encoding in its own body coordinate system, in millimeters. It is calculated in each control cycle by the difference between the nominal pose of the end effector and the three-dimensional coordinates of the target region maintained by the Ego 3D perception module. Torque change refers to the change in joint torque or wrist torque over time within a certain observation window, including steady-state baseline, rate of change, and whether there are obvious abrupt changes, in Newton-meters or Newtons. Image features refer to a set of numerical features extracted from the RGB image and depth map from the fuselage perspective that are related to the contour of the target object, opening position, insertion alignment relationship, and local texture of the contact area. In this scenario, it can be obtained through a fixed image cropping and a multi-layer nonlinear mapping network. The network structure and weights are locked with a version number after offline training and registered in the version lock registration table. The typical length of the feature vector can be set to tens to hundreds of floating-point components.

[0106] In each control cycle, the operation phase recognition module first reads the voxel feature field and interactive constraint field snapshots that were confirmed to be valid in the previous cycle from the shared memory, reads the current end nominal pose and target area description from the trajectory buffer, and obtains the most recently acquired force data and RGB-D data from the force acquisition channel and camera buffer. The sampling rhythm is preferably consistent with the control cycle, for example, a complete acquisition is completed every ten milliseconds. During sampling, the timestamps are compared, and only samples with a difference between timestamps of different signals not exceeding the preset tolerance (e.g., five milliseconds) are included in the recognition calculation of this cycle. Samples exceeding the tolerance are marked as not participating in the calculation of this cycle and a time alignment deviation event is registered in the log.

[0107] To suppress noise, the phase recognition module preprocesses the distance between the end and the target, torque, and image features within a short observation window. The short observation window can be set to the most recent ten control cycles. Within this window, a moving average and median filter are performed on the distance sequence, the baseline mean, standard deviation, and the difference between adjacent cycles are calculated on the torque sequence, and the change amplitude of image features is calculated on a time scale and whether there is a significant mode switch, such as from "target not in view" to "target outline is stable and visible". These processes are repeated in each control cycle, so that subsequent stage judgments are based on trends over continuous time rather than relying on single-point noise.

[0108] The operation phase recognition module maintains an operation phase state machine internally. The current state of the state machine is stored in the session context area of ​​shared memory. The initial state is preferably set to "approach". In each control cycle, the module determines whether to switch phases based on the consistency of the current state with the distance, torque, image features and interaction constraints within the observation window. For example, in the approach phase, the module focuses on whether the distance between the end and the target continues to decrease and whether the target outline in the image features changes from invisible to visible. If the distance within the observation window remains greater than the preset approach threshold for several consecutive cycles and the distance change in a single cycle is significantly negative, the phase state remains approach. When the distance drops below the approach threshold and the image features indicate that the target outline stably appears near the center of the image, the number of times the above conditions are met in the most recent control cycles reaches the trigger number threshold. For example, if the conditions are met seven times in ten cycles, the phase state is switched to the alignment phase and the timestamp is updated.

[0109] During the alignment phase, the operation phase recognition module focuses on image features and the relative posture relationship between the end effector and the target. It compares the offset between the target opening position and the end effector projection position in the image, and determines the angle between the end effector posture and the desired alignment posture through the voxel feature field and the interaction constraint field. If the offset continues to decrease and the angle falls within the preset alignment tolerance range within the observation window, and the distance between the end effector and the target is still greater than the contact distance threshold and the torque change does not show a significant sudden increase, then the phase remains in alignment. When there are enough cycles within the observation window that meet the conditions of "distance close to the contact threshold, offset and posture error are all within the alignment tolerance", the phase state is switched to the contact phase.

[0110] During the contact phase, the operation phase recognition module focuses on monitoring whether the distance between the end effector and the target is close to zero and whether the force or torque in the force perception along the contact direction slowly increases from near zero. By estimating the slope of the force perception curve within the observation window, if the force or torque is detected to steadily increase from near zero and reach the contact confirmation threshold within several consecutive cycles, and the distance between the end effector and the target no longer changes significantly and the relative position of the target in the image features remains stable, then contact can be considered established. If the current task in the task encoding includes a tightening phase, then after several cycles of meeting the above conditions, the phase state is switched from contact to tightening; during the tightening phase, the operation phase... The position recognition module mainly relies on torque changes and the amount of rotation of the end about the tightening axis. By accumulating the rotation angle about the predetermined axis in the observation window and monitoring the increase of the torque of the corresponding joint, when the accumulated rotation angle reaches the tightening target angle and the torque is close to the target torque range given in the task parameter table, the stage state switches from tightening to exiting. In the exiting stage, it mainly relies on the continuous increase of the distance between the end and the target and the force gradually falling back to near zero. The target outline in the image features gradually moves away from the center of the field of view. When the above conditions are met for several consecutive cycles in the observation window, the stage state can be switched from exiting back to approaching so that the operation can be repeated for the next workpiece.

[0111] In each of the above-mentioned stage switching conditions, the specific distance threshold, torque threshold, offset tolerance, and trigger number threshold are all derived from the pre-configured task parameter table. The task parameter table is configured with different entries according to the task type and workstation. For example, different contact distance thresholds and tightening target torque values ​​can be used for bottle cap tightening tasks and connector plugging / unplugging tasks. The task parameter table version number is recorded in the version lock registration table. After each parameter adjustment, a new version number is generated and the reason for the adjustment and the experimental basis are recorded to form a chain of evidence.

[0112] To avoid stage jitter, the operation phase recognition module uses a threshold combination with hysteresis characteristics in the stage switching determination. For example, when switching from near to alignment, a smaller distance threshold and a stricter image alignment condition are used, while when switching from alignment back to near, a slightly larger distance back and increased offset are allowed. Different entry and exit thresholds are also used in the contact and tightening stages. The specific threshold difference is given in the task parameter table.

[0113] The identification results of the operation phase recognition module are organized into an operation phase state snapshot in each control cycle. The state snapshot includes session identifier, cycle number, current phase identifier, last phase switching timestamp, number of control cycles the current phase has lasted, version number of the task parameters used, state machine version number, quality flag, and possible error codes. When the quality flag is zero, it indicates that the phase recognition of this cycle has been successfully completed. When the quality flag is non-zero, it indicates that there is signal inconsistency or timeout in the phase recognition. The state machine version number is used to identify the version of the phase judgment rule. The version number is updated and registered each time the phase switching conditions are adjusted or a new phase is added.

[0114] The operation phase recognition module writes state snapshots to the operation phase channel via shared memory and establishes a one-to-many subscription relationship with the adaptive resolution and action granularity scheduling module and the diffusion trajectory generation module. Before writing, it checks whether the current period has already written a state snapshot using a combination of session identifier and period sequence number as an idempotent key. If a duplicate idempotent key is detected, the writing is skipped to prevent duplicate recognition results from overwriting the results of the previous thread. During writing, a mutex lock is used to ensure that the same session updates the state only once in the same period. If the lock acquisition fails, the recognition result is retained within the current period and the recognition is re-executed and the writing is attempted in the next period.

[0115] The time resource limit for recognition operation can be set to no more than ten milliseconds per cycle. When the time limit is exceeded due to excessive time spent on image feature extraction or force statistics in a certain cycle, the operation phase recognition module abandons the update phase state of this cycle, sets the quality flag to 502, and records the corresponding error code as "recognition timeout". If the downstream module finds that the quality flag of the current cycle is non-zero when reading the state snapshot, it will continue to use the state of the previous cycle and may optionally enter a conservative strategy, such as disabling tightening actions before confirming contact.

[0116] For error codes, a minimum feasible set can be defined. For example, error code 0 indicates that the stage identification is completed normally, error code 501 indicates that the distance between the end and the target, torque and image features are contradictory within the observation window and cannot be classified into any stage, error code 502 indicates that the identification operation timed out, and error code 503 indicates that the expected stage switch did not occur within the preset longest stage duration and higher-level monitoring intervention is required. The remaining code positions are reserved for expansion.

[0117] For on-site verification, the accuracy of recognition can be assessed by comparing the consistency between the manually annotated actual stage sequences and the stage identification sequences provided by the operation phase recognition module in a certain number of task sessions recorded at the bottle capping station. For example, in one hundred bottle capping tasks, the distance curve between the end and the target, the torque curve, and the image sequence are collected for each task. The on-site engineer marks the time periods of approach, alignment, contact, tightening, and exit on an offline tool. These manually marked segments are then aligned with the online recognition results. The recognition accuracy of each stage and the time deviation of the stage switching time are statistically analyzed. When the accuracy of the approach and exit stages reaches over 95%, the accuracy of the alignment, contact, and tightening stages reaches over 90%, and the average stage switching time deviation is within a few... When the time is in the range of 10 milliseconds, the operation phase recognition module can be considered to be at a usable level at this workstation. Preferably, in the case of tightening bottle caps, the proximity stage distance threshold can be set to 20 to 30 millimeters, the alignment stage offset tolerance can be set to 1 to 3 millimeters, the contact distance threshold can be set to 1 to 2 millimeters, the contact force threshold can be set to 2 to 5 Newtons, the tightening target torque can be set to 1 to 3 Newton-meters, the state switching trigger number threshold can be set to at least 7 cycles within the last 10 control cycles that meet the corresponding conditions, and the longest stage duration can be set to several seconds. Under the above parameter configuration and version locking, comparing the results of 100 trial runs, the number of premature tightening or misjudged contact caused by stage recognition errors does not exceed 5 times, and the overall task success rate reaches more than 95%.

[0118] In alternative approaches, the operation phase recognition module can also adopt a segmentation strategy based entirely on rules. That is, instead of introducing a multi-layer feature mapping network, it directly sets fixed thresholds and time limits for the distance curve between the end and the target, the joint torque curve, and simple image geometry. A set of conditions is used to determine the stage switching. As long as the above-mentioned observation window statistics, parameter version locking, idempotent key control, quality flags, and error code logging mechanisms are still followed, those skilled in the art can achieve operation phase recognition capabilities equivalent to the above implementation in workstations with limited computing resources or relatively simple task scenarios.

[0119] The closed-loop execution and error correction module is used to control joint movement based on trajectory segments, periodically collect visual and force feedback to update the interactive constraint field, and trigger local trajectory replanning when the end-effector tracking error and contact deviation exceed a preset threshold. Specifically, the implementation is as follows:

[0120] The closed-loop execution and error correction module is installed on the same industrial computer as the Ego 3D perception module, interaction complexity and physical constraint construction module, adaptive resolution and motion granularity scheduling module, diffusion trajectory generation module, and operation phase recognition module. It establishes a real-time control channel with the robot control cabinet via industrial Ethernet. Through shared memory, it accesses the end-effector trajectory segments in the trajectory buffer by session identifier and segment number, and accesses the latest voxel feature field, interaction constraint field, and operation phase state snapshot. Within each control cycle, this module drives joint movement based on the end-effector target pose given by the trajectory segment, periodically acquires visual and force feedback, calculates the end-effector tracking error and contact deviation, and determines the error within a short observation window. The statistical data triggers the refresh of the interactive constraint field and the replanning of the local trajectory, thus forming a closed-loop control. In this scenario, the closed-loop execution and error correction module can be understood as an intermediate layer connecting trajectory planning and joint driving. On the one hand, it converts the end trajectory fragment given by the diffusion trajectory generation module into the expected quantity of joint space point by point according to the current session context and sends it to the control cabinet. On the other hand, in each control cycle, it obtains the joint angle and joint velocity from the control cabinet, collects three-dimensional force and three-dimensional torque from the end or wrist force sensor, obtains time-aligned RGB-D frames from the body-view camera, and calculates the current actual pose of the end and the contact state between the end and the environment by combining the self-body coordinate system calibration information provided by the Ego three-dimensional perception module.

[0121] In this scenario, end-point tracking error refers to the positional and attitude deviations between the actual pose of the current end-point tracking device and the target pose at the corresponding moment in the trajectory segment, within the device's own coordinate system. The positional deviation is measured in millimeters, and the attitude deviation is measured in angles. The positional deviation can be obtained by the magnitude of the difference between the two three-dimensional coordinates, while the attitude deviation can be obtained by the absolute value of the difference between the two attitude angles or by measuring the angle between the end-point normal and the target normal. Contact deviation refers to the deviation between the actual force or torque along the task contact direction and the recommended range given by the contact constraint parameters in the interactive constraint field, measured in Newtons or Newton-meters. It is obtained in each control cycle by projecting the six-dimensional torque measured by the force sensor onto the task direction and comparing it with the force range interval in the contact constraint parameters.

[0122] At the start of each control cycle, the closed-loop execution and error correction module reads trajectory segment messages from the trajectory buffer based on the session identifier and the current execution segment number. It checks whether the trajectory generation network version number, resolution mapping rule version number, and task parameter version number are consistent with the module's configuration. It also checks whether the quality flag and error code indicate that the trajectory segment is valid. Only when the version number is consistent and the error code is zero is the segment registered as the current execution segment, and the segment's starting control cycle number and the number of trajectory points are recorded. To prevent the same segment from being executed repeatedly, the module maintains the current execution segment identifier using a combination of the session identifier and the segment number as an idempotent key. When the same idempotent key appears repeatedly in the trajectory buffer, it is considered a duplicate message and ignored.

[0123] During the execution phase, each control cycle calculates the target pose point corresponding to the trajectory segment according to the current control cycle number. Preferably, linear interpolation is performed between trajectory points based on the local time interval recorded in the trajectory segment to obtain the target pose corresponding to the current cycle. Then, the offline calibrated inverse kinematics routine is called to map the target pose to the joint angle target value and the joint velocity target value. This routine has been calibrated and assigned a version number through a series of reachability tests during system deployment. The geometric parameters and numerical strategies used are recorded in the version lock registration table. The inverse kinematics solution preferably adopts an iterative numerical solution method with good convergence. The joint angle estimate is updated step by step in the solution cycle corresponding to each target pose. The iteration stops when the position deviation and attitude deviation are both lower than the built-in threshold or the number of iterations reaches the upper limit. If the target pose falls in the reachable space of the robot arm and does not violate the joint position and velocity limits at the end of the iteration, the joint angle target value is output. Otherwise, the target pose is marked as unreachable and a protection action is triggered in this cycle.

[0124] The closed-loop execution and error correction module sends the target values ​​of joint angle and joint velocity, along with the session identifier and the idempotent key of the current trajectory segment, to the control cabinet. The control cabinet adjusts the drive current of each joint within the drive cycle based on these target values. At the end of the same control cycle, the closed-loop execution and error correction module reads the latest joint angle, joint velocity, end effector force, and corresponding timestamp. It compares the timestamp with the control cycle time and considers the cycle feedback valid only if the time difference does not exceed the preset tolerance (e.g., five milliseconds). The module then uses the calibration matrix provided by the Ego 3D perception module to calculate the joint angle into the actual end effector pose, and compares it with the target pose to obtain the position deviation and attitude deviation.

[0125] To suppress noise, the closed-loop execution and error correction module statistically analyzes the end-point tracking error and contact deviation within a short observation window. This short observation window is preferably consistent with the aforementioned module, for example, covering the most recent ten control cycles. It calculates the moving average and maximum value for the position deviation sequence, the maximum angular deviation for the attitude deviation sequence, and the average, maximum value, and rate of change for the contact deviation sequence. When the maximum value of the position deviation within the observation window exceeds the spatial deviation threshold set for the current position complexity level in the task parameter table, or the maximum value of the attitude deviation exceeds the attitude deviation threshold, or the maximum value of the contact deviation exceeds the force or torque deviation threshold, the closed-loop execution and error correction module marks this cycle as a deviation cycle and counts the number of deviation cycles within the observation window. When the number of deviation cycles reaches a preset trigger threshold within ten cycles (e.g., seven cycles out of ten exceed the threshold), a systematic deviation is considered to have occurred, requiring an update of the interactive constraint field and triggering local trajectory replanning.

[0126] During the update of the interactive constraint field, the closed-loop execution and error correction module sends an RGB-D sampling and voxel feature field reconstruction request to the Ego 3D perception module through an internal notification mechanism. After confirming that the new voxel feature field snapshot is written to shared memory, it guides the interactive complexity and physical constraint construction module to recalculate the interactive complexity index and contact constraint parameters based on the latest voxel feature field and task encoding. When the quality flag of the new interactive constraint field snapshot is zero and the version number is incremented, the closed-loop execution and error correction module updates the interactive constraint field handle it references and marks the old interactive constraint field as a historical version.

[0127] During local trajectory replanning, the closed-loop execution and error correction module constructs a local planning request message based on the current actual pose of the end effector, the remaining task stage, and the updated interaction constraint field. This message includes a session identifier, the idempotency key of the current execution segment, the current control cycle number, the local planning time domain length, and local safety area constraints. In this scenario, the local safety area constraint refers to the safe volume area defined by the safety distance information given by the interaction constraint field and the geometric boundary of the device body in the Ego self-body coordinate system. For example, it requires that the minimum distance between the end effector position and all occupied voxels is not less than the lower limit of the safe avoidance distance in the contact constraint parameters, and requires that the joint angle be kept within the safe range specified in the device installation instructions. This constraint is formed during the debugging phase by scanning the surrounding environment of the workstation and calibrating the safety distance, and is version-locked as part of the task parameter table.

[0128] The closed-loop execution and error correction module writes the local planning request message to the planning request channel subscribed to by the diffusion trajectory generation module, and assigns a new segment number and timeout timer to the request. While waiting for the new trajectory segment, the closed-loop execution and error correction module takes conservative actions according to the operation stage identifier. For example, in the approach stage, it moves slowly along the original direction at a reduced speed. In the contact or tightening stage, it slightly retreats along the safe avoidance direction allowed by the local safety area constraint and adjusts the joint stiffness to a compliant mode. In the exit stage, it continues to move away from the target at a limited speed.

[0129] If a new trajectory fragment written by the diffusion trajectory generation module is received within the preset replanning timeout period (e.g., within 50 milliseconds) and the error code is zero, the closed-loop execution and error correction module replaces the current execution fragment with the new fragment idempotent key and executes according to the new trajectory fragment from the current control cycle. Otherwise, when the replanning timeout or the error code indicates that a feasible trajectory cannot be found, the closed-loop execution and error correction module marks the current session state as requiring manual intervention, sets the error code to 602, and triggers the safety shutdown logic. In the safety shutdown logic, the robotic arm slowly retracts to the safe posture along the pre-taught safe path. During the system debugging phase, the engineer manually guides the robotic arm from the typical operating posture of each station to the predetermined safe standby posture through the teach pendant. During the teaching process, the joint angle sequence and key nodes are recorded, and the safe path entries corresponding to different stations are registered in the task parameter table and locked by the version number. During runtime, the closed-loop execution and error correction module selects the corresponding safe path according to the current station identifier and executes it segment by segment, and issues an alarm through the upper monitoring system.

[0130] The closed-loop execution and error correction module generates a snapshot of the closed-loop execution status in each control cycle. The snapshot includes the session identifier, the idempotency key of the current execution segment, the control cycle number, the statistical values ​​of the end position deviation and attitude deviation, the statistical value of the contact deviation, the number of deviation cycles within the observation window, the timestamp of the most recent interactive constraint field update, the result of the most recent local replanning, the module version number, the quality flag, and the error code. When the quality flag is zero, it indicates that the closed-loop execution link is running normally in that cycle. When the error code is zero, it indicates that no protection behavior has been triggered. Preferably, a minimum set of implementable error codes can be defined. For example, error code zero indicates no error, error code 601 indicates that the number of deviation cycles within the observation window exceeds the threshold and a replanning has been triggered, error code 602 indicates that the replanning timeout or replanning failure has triggered a safety shutdown, and error code 603 indicates that the feedback time alignment has exceeded the tolerance multiple times, triggering a sensing link fault. The remaining code bits are reserved for expansion.

[0131] To ensure time resources, the upper limit of error calculation and judgment time for the closed-loop execution and error correction module in each cycle can be set to within five milliseconds. If the time taken in a certain cycle exceeds the upper limit, a new replanning will not be triggered in this cycle. Instead, a timeout event will be recorded in the log, and the quality flag will be set to a non-zero value. Downstream modules can then choose to use the state of the previous cycle.

[0132] For on-site testing, one hundred task sessions can be selected at the bottle capping station. The number of alignment failures, contact slippage, and over-force shutdowns occurring at the end of the machine in high-complexity areas is recorded when the closed-loop execution and error correction module is not enabled. Then, with the module enabled and local trajectory replanning used, the relevant indicators and total task time are recorded with the same sample size. When enabling the module significantly reduces the number of alignment failures and slippages, and the increase in average task completion time is within an acceptable range, the closed-loop execution and error correction module can be considered to have achieved the expected effect. Preferably, under typical bottle capping conditions, the position deviation threshold can be set to one to two millimeters in high-complexity areas and two to three millimeters in medium-complexity areas. The low-complexity region has a tolerance of 3 to 5 millimeters. The attitude deviation threshold can be set to 0.5 degrees for the high-complexity region, 1 degree for the medium-complexity region, and 2 degrees for the low-complexity region. The contact force deviation threshold can be set to 20% to 30% of the upper limit of the recommended contact force range. The deviation period trigger number threshold can be set to at least seven times exceeding the threshold within ten control cycles. Under this parameter configuration, comparing the results of one hundred tasks, without enabling closed-loop error correction, the number of contact failures and over-force shutdowns due to deviation accumulation exceeded 20 times. After enabling closed-loop error correction and activating local trajectory replanning, the number of related failures decreased to no more than five times, and the average task completion time increased by no more than 10%.

[0133] In alternative approaches, the closed-loop execution and error correction module can also adopt more simplified rules, such as directly triggering a global shutdown without performing local trajectory replanning when the end position deviation and a single contact force threshold are exceeded, or forcibly refreshing the interactive constraint field at fixed time intervals without using an observation window to count the deviation period. As long as the principles of version locking, idempotent key control, quality flags and error code logging, and safe shutdown strategies are still followed, those skilled in the art can select a closed-loop execution and error correction scheme equivalent to the above implementation form according to the safety requirements and computing resources of the specific workstation.

[0134] All calculations involved in the embodiments are dimensionless numerical calculations, and the preset parameters and thresholds in the calculations are set by those skilled in the art according to the actual situation.

[0135] It should be noted that this invention can be deployed on the device itself to realize embedded applications, or it can run on a PC or other terminal with a user interface, thereby meeting various hardware environments and usage requirements.

[0136] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, as a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wireless or wired transmission; wired transmission methods include optical fiber, twisted pair, coaxial cable, etc.; wireless transmission includes infrared, microwave, etc. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center containing one or more sets of available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium. A semiconductor medium can be a solid-state drive.

[0137] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and modules described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0138] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or modules may be electrical, mechanical, or other forms.

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

[0140] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

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

[0142] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

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

Claims

1. A robot precision manipulation control system based on diffusion model and 3D vision, characterized in that, include: The Ego 3D perception module is used to collect RGB-D data from the fuselage viewpoint and construct a voxel feature field in its own body coordinate system based on the calibration matrix. The interaction complexity and physical constraint construction module is used to calculate the interaction complexity index and contact constraint parameters at each location based on voxel features and task encoding, and generate the interaction constraint field. The adaptive resolution and motion granularity scheduling module is used to simultaneously adjust the voxel resolution and end pose step size according to the interaction constraint field, so that high-complexity regions use high-resolution and fine-grained motion, and low-complexity regions use low-resolution and coarse-grained motion. The diffusion trajectory generation module is used to generate end trajectory segments that satisfy the interaction constraint field on an adaptive action grid using a diffusion model, based on voxel features, interaction constraint field, and operation stage identifier. The operation phase recognition module is used to identify the approach, alignment, contact, tightening, and exit stages based on the distance between the end and the target, torque changes, and image features, and output stage identifiers. The closed-loop execution and error correction module is used to control joint movement based on trajectory segments, periodically collect visual and force feedback to update the interactive constraint field, and trigger local trajectory replanning when the end tracking error and contact deviation exceed a preset threshold.

2. The robot precision operation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The Ego 3D perception module includes: The Ego 3D perception module constructs a voxel feature field in its own body coordinate system; The RGB-D data and joint angles are time-aligned, and the depth pixels are converted into three-dimensional observation points and mapped to voxels using a calibration matrix. Based on time window statistics, voxel occupancy flags and color statistics are updated and spatial filtering is performed. Using an idempotent key obtained by combining the session identifier and the period sequence number, the voxel feature field snapshot, along with the calibration matrix version number, voxel resolution configuration version number, quality flag, and session identifier, is written to the shared memory buffer, and the writing is controlled by a mutex lock.

3. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The interaction complexity and physical constraint building modules include: The interaction complexity and physical constraint construction module is based on voxel feature fields and task encoding. Within the observation window, it calculates the interaction complexity index according to voxel occupancy status, surface shape changes, occlusion, and historical task results. In its own coordinate system, an interaction constraint field snapshot is generated, which records the interaction complexity index and the set of contact constraint parameters. Using an idempotent key obtained by combining the session identifier and the cycle number, the interaction constraint field snapshot, along with the complexity field version number, the task parameter version number, and the quality flag, is written to a shared memory buffer, and the writing order is controlled by a mutex lock.

4. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 3, characterized in that: The interaction complexity and physical constraint construction module, based on the voxel geometric neighborhood, task encoding, and task parameter table, determines a set of contact constraint parameters for the voxel, consisting of the expected approach direction range, lateral offset limit, approach velocity range, contact force range, and safe avoidance distance. The contact constraint parameter set and the interaction complexity index are then written into the interaction constraint field snapshot. When the quality indicator voxel feature field is in a low-quality state, an interactive constraint field snapshot is generated by selecting a preset set of safety contact constraint parameters based on the task parameter table.

5. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The adaptive resolution and action granularity scheduling module includes: The adaptive resolution and action granularity scheduling module divides voxels into low-complexity, medium-complexity, and high-complexity regions based on the interaction complexity index of voxels in the interaction constraint field and according to a preset complexity threshold. It also generates voxel query resolution and end pose step size for each region in combination with the operation stage identifier. Using an idempotent key composed of a session identifier and a period number, an action-granularity configuration snapshot containing the session identifier, period number, resolution mapping rule version number, task parameter version number, spatial step size, attitude step size, and quality flag is written to shared memory.

6. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The diffusion trajectory generation module includes: The diffusion trajectory generation module reads snapshots of interaction constraint fields and action granularity configurations verified by version number and quality flag from shared memory according to session identifier and cycle number; An adaptive motion grid is constructed based on the spatial step size and attitude step size in the snapshot of the motion granularity configuration, and an end trajectory fragment is generated by combining voxel features and interaction constraints. Using an idempotent key obtained by combining the session identifier and the fragment number, a trajectory fragment message carrying the session identifier, fragment number, trajectory generation network version number, and configuration version number and quality flag related to trajectory generation is written to the trajectory buffer.

7. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 6, characterized in that: The diffusion trajectory generation module calls the trajectory generation network to calculate the position correction and attitude correction for the initial end pose sequence obtained by discretization on the adaptive motion grid according to a preset iteration cycle; After each iteration, constraint projection is performed on the trajectory points based on the occupancy state, interaction complexity index and contact constraint parameters in the interaction constraint field, so that the trajectory points are located in the free space region defined by unoccupied voxels and the displacement and attitude difference between adjacent trajectory points does not exceed the spatial step size and attitude step size in the action granularity configuration snapshot. In the trajectory segment message, constraint violations are characterized by quality flags and error codes.

8. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The operation phase recognition module includes: The operation phase recognition module is used to calculate the distance between the end effector and the target, torque changes, and image features within the observation window based on the nominal pose of the end effector and the target area position, joint torque, end effector force sensing signal, and fuselage view image in the body coordinate system. The operation phase state machine is driven to switch between the approach, alignment, contact, tightening and exit phases according to the preset phase judgment rules. It generates an operation phase state snapshot carrying session identifier, cycle number, phase identifier, task parameter version number and quality flag, and provides it to downstream modules through shared memory.

9. The robot precision manipulation control system based on diffusion model and 3D vision according to claim 1, characterized in that, The closed-loop execution and error correction module includes: The closed-loop execution and error correction module is used to convert the end target pose into joint angle target values ​​and send them to the control cabinet based on the end trajectory segment in the trajectory buffer and the self-body coordinate system calibration results; In each control cycle, joint angles, force feedback, and visual feedback are collected to calculate end-point tracking error and contact deviation. The number of deviation cycles is counted within a short observation window. When the number of deviation cycles reaches a threshold, local trajectory replanning based on voxel feature fields and interactive constraint fields is triggered. If replanning fails, a safe shutdown is performed along a safe path.

Images