Robot trajectory planning and motion control method and system based on visual feedback

Abstract

The application discloses a robot trajectory planning and motion control method and system based on visual feedback, and relates to the technical field of visual servo control. The trajectory control data stream is synchronously collected, the visual target residual error, the servo execution residual error and the background fixed anchor residual error are solved in parallel, and the gated access control of the trajectory compensation link is implemented according to the background fixed anchor residual error. After access, the initial time sequence causal attribution of the deviation source is performed by capturing the abnormal trigger time sequence of the three types of residual errors. When it is determined that the concurrent fuzzy state or the spatial direction consistency is in the conflict interval, the bidirectional micro-exploration action within the safety range is issued to confirm the final deviation source. Then, according to the deviation source type and the continuous maintenance period, the hierarchical state switching with hysteresis condition is performed, and the corresponding motion control parameters are output. The application realizes the identification and differentiated control of the deviation source under the condition of multi-source deviation coupling, and effectively improves the robustness of the robot trajectory tracking control.

Description

Technical Field

[0001] This invention relates to the field of visual servo control technology, specifically to a method and system for robot trajectory planning and motion control based on visual feedback. Background Technology

[0002] In modern industrial automation, vision feedback-based robot servo grasping and precision alignment technologies have been widely applied. However, existing vision servo control systems face severe challenges from multi-source non-Gaussian noise and sudden interference in actual operation: vibrations of the rigid support of the camera itself can cause false drift in the panoramic image; sudden changes in lighting or partial occlusion in dynamic factory environments can cause nonlinear jumps in the center of gravity of target feature point extraction; at the same time, joint overload, backlash caused by physical wear, and sudden load changes in the robotic arm body due to long-term operation can lead to lag in the underlying servo execution.

[0003] Existing control methods typically employ passive error feedback, directly feeding the macroscopic spatial difference between the visually detected position and the robot's end effector position into the position adjuster. This renders the control system completely unable to distinguish whether the deviation is due to camera observation noise, the actual physical displacement of the workpiece, or an execution defect in the robotic arm itself. This ambiguity of causality easily triggers the robot control loop to make drastic, incorrect compensation movements for spurious noise, leading to sudden high-frequency oscillations, trajectory deviation, joint overload tripping, or even rigid and violent impacts with the machining table during the target approach phase. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a robot trajectory planning and motion control method based on visual feedback.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] In a first aspect, the present invention discloses a robot trajectory planning and motion control method based on visual feedback, comprising the following steps:

[0007] The trajectory control data stream is collected synchronously, including command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data.

[0008] The trajectory control data stream is solved in parallel, and the corresponding visual target residual data, servo execution residual data and background fixed anchor point residual data are output. Based on the background fixed anchor point residual data, the trajectory compensation link of the visual target residual data is subjected to gating admission control.

[0009] Capture the sequence of abnormal triggering times when three types of residual data exceed their respective preset thresholds, and determine the initial deviation source control data based on the causal order of the abnormal triggering time sequence on the discrete time axis;

[0010] When the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range, the current command trajectory point data is intercepted, and bidirectional micro-probing motion control data within a preset safety range is sent to the robot.

[0011] Capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after executing the bidirectional micro-probing action control data, and confirm the final deviation source control data;

[0012] Based on the final deviation source control data and its continuous holding period, the driving control decision finite state machine performs hierarchical state switching with hysteresis conditions between multiple control states, and outputs the motion control parameters corresponding to the switching state according to the preset control period.

[0013] Secondly, this invention discloses a robot trajectory planning and motion control system based on visual feedback, using the aforementioned robot trajectory planning and motion control method based on visual feedback, including:

[0014] The synchronous acquisition module is used to synchronously acquire trajectory control data stream, which includes command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data.

[0015] The calculation and admission module is used to calculate the trajectory control data stream in parallel, output the corresponding visual target residual data, servo execution residual data and background fixed anchor point residual data, and perform gating admission control of the trajectory compensation link based on the background fixed anchor point residual data.

[0016] The attribution module is used to capture the sequence of abnormal triggering times when three types of residual data exceed the corresponding preset thresholds, and to determine the initial deviation source control data based on the causal order of the abnormal triggering time sequence on the discrete time axis.

[0017] The discrimination and excitation module is used to intercept the current command trajectory point data and send bidirectional micro-probing motion control data within a preset safety range to the robot when the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range.

[0018] The differential confirmation module is used to capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after the execution of the bidirectional micro-probing action control data, and to confirm the final deviation source control data.

[0019] The decision module is used to drive the control decision finite state machine to perform hierarchical state switching with hysteresis conditions between multiple control states based on the final deviation source control data and its continuous holding period, and output the motion control parameters corresponding to the switching state according to the preset control period.

[0020] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0021] 1. This invention allows visual target residuals to enter the trajectory compensation link only when the visual reference system remains stable, thereby avoiding interference to trajectory planning caused by visual feedback data pollution due to unexpected displacement of the camera carrier or loss of anchor lock. Therefore, it effectively improves the signal validity discrimination capability of the trajectory compensation control system.

[0022] 2. This invention changes the traditional method of treating various deviations as a single entity for unified compensation. When the spatial direction consistency between the visual target residual and the servo execution residual is within a preset conflict range, this invention further determines that the deviation source is in a concurrent ambiguity state, thereby achieving the distinguishable identification of different physical causes at the control strategy level. Therefore, it provides a reliable decision-making basis for subsequent differentiated control.

[0023] 3. This invention achieves effective separation of different deviation sources, such as external target drift and camera carrier instability. Based on the final deviation source type and its continuous holding period, it can adaptively select differentiated control strategies such as replanning, deceleration compensation, or breakpoint holding under different deviation scenarios, avoiding the problem of insufficient adaptability of a single control mode under complex working conditions, thereby ensuring the closed-loop integrity and mission safety of the entire robot trajectory tracking control process. Attached Figure Description

[0024] The disclosure of this invention is illustrated with reference to the accompanying drawings. It should be understood that the drawings are for illustrative purposes only and are not intended to limit the scope of protection of this invention. In the drawings, the same reference numerals are used to refer to the same parts. Wherein:

[0025] Figure 1 This is a flowchart of the steps of the present invention;

[0026] Figure 2 This is a flowchart illustrating the working principle of the present invention;

[0027] Figure 3 This is a module connection diagram of the present invention. Detailed Implementation

[0028] It is readily understood that, based on the technical solution of this invention, those skilled in the art can propose various interchangeable structural methods and implementations without altering the essential spirit of the invention. Therefore, the following detailed embodiments and accompanying drawings are merely illustrative examples of the technical solution of this invention and should not be considered as the entirety of the invention or as limitations or restrictions on the technical solution of this invention.

[0029] In existing industrial robot vision servo control technologies, robots typically rely on the target workpiece position information fed back by the vision system to form closed-loop trajectory control. However, in real-world operating environments, the sources of deviations affecting trajectory tracking accuracy are multi-faceted: they may originate from the target workpiece's own pose drift, from the lag in the robot's underlying servo response, or from unexpected displacement of the camera mounting carrier caused by external environmental vibrations. Traditional control methods usually attribute these deviations uniformly to vision feedback errors for single compensation, making it difficult to distinguish the physical causes of the deviations at the control level. When the visual target residual and the servo execution residual are inconsistent in spatial direction, different deviation sources couple with each other, making it impossible for conventional error compensation models to accurately identify the source of the deviation, leading to incorrect trajectory compensation direction or even system oscillation. Existing technologies lack effective mechanisms for proactive identification and differentiated processing of deviation sources under multi-source deviation coupling conditions, making it difficult to guarantee the safety and robustness of robot trajectory tracking control under complex working conditions.

[0030] To address the aforementioned issues, the study discovered a causal relationship between the temporal triggering sequence of three types of data—visual target residual, servo execution residual, and background fixed anchor point residual—when trajectory tracking deviations occur in the system, and the physical cause of the deviation. By capturing the sequence of abnormal triggering times when the three types of residuals exceed preset thresholds, the source of the deviation can be initially attributed based on the temporal sequence. Further investigation revealed that when the directional consistency between the visual target residual and the servo execution residual is within a specific conflict range, a single attribution will result in concurrent ambiguity. In this case, applying a bidirectional micro-probing motion within a safe range to the robot reveals that the relative differential response between the target workpiece, the end effector marker, and the background fixed anchor point reflects the essential physical difference of the deviation source—when the target truly drifts, the end effector marker moves in tandem with the probing command while the anchor point remains stable; when the camera becomes unstable, the target and end effector visual positions shift synchronously, and the anchor point also changes accordingly. Therefore, a multi-level deviation source identification strategy based on the combination of time-series causal attribution and bidirectional micro-trial differential response is proposed. Based on the identified deviation source type and its continuous holding period, a hierarchical state switching with hysteresis condition is executed to form a closed-loop system for active deviation source identification and differentiated control.

[0031] After introducing the basic concept of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0032] Example 1:

[0033] like Figure 1 and Figure 2 As shown, the robot trajectory planning and motion control method based on visual feedback includes the following steps:

[0034] The trajectory control data stream is acquired synchronously. The trajectory control data stream includes command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data.

[0035] Parallel computation of trajectory control data stream, outputting corresponding visual target residual data, servo execution residual data and background fixed anchor point residual data, and gating access control of the trajectory compensation link based on the background fixed anchor point residual data;

[0036] Capture the abnormal trigger time sequence of three types of residual data exceeding their respective preset thresholds, and determine the initial deviation source control data based on the causal order of the abnormal trigger time sequence on the discrete time axis;

[0037] When the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range, the current command trajectory point data is intercepted, and bidirectional micro-probing motion control data within a preset safety range is sent to the robot. The preset safety range is the safety range of the Cartesian space motion envelope determined by the robot's joint speed limit and acceleration limit.

[0038] Capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after executing the bidirectional micro-probing motion control data, and confirm the final deviation source control data;

[0039] Based on the final deviation source control data and its continuous holding period, the driving control decision finite state machine performs hierarchical state switching with hysteresis conditions between multiple control states, and outputs the motion control parameters corresponding to the switching state according to a preset control cycle. The multiple control states include at least a normal operation state, a visual verification state, a local smoothing replanning state, a servo deceleration compensation state, and a breakpoint recovery state with data snapshots; wherein, the normal operation state is the default state when the system has no deviation source confirmation, and the visual verification state is a transitional suspension state between the normal operation state and the local smoothing replanning state.

[0040] The overall workflow of this application is as follows: At the beginning of each preset control cycle, the system activates the industrial camera to acquire images through a hardware synchronous triggering mechanism. Simultaneously, it reads the robot joint encoder values ​​and the planned trajectory data issued by the main control algorithm through the industrial communication bus to synchronously acquire a set of trajectory control data streams. These trajectory control data streams include: instruction trajectory point data serving as the trajectory planning reference; actual pose data of the robotic arm end effector derived from feedback from the robot's underlying encoder; and visual position data of the target workpiece, background fixed anchor points, and end effector markers acquired by vision sensors and transformed to the robot's base coordinate system.

[0041] After obtaining the aforementioned trajectory control data stream, the system performs parallel calculations of three types of residual data. Specifically, the system performs spatial difference calculations between the actual pose data and the command trajectory point data to generate servo execution residual data; it performs spatial difference calculations between the target workpiece visual position data and the desired position corresponding to the command trajectory point data to generate visual target residual data; and it performs spatial difference calculations between the background fixed anchor point visual position data and its pre-calibrated initial reference position to generate background fixed anchor point residual data.

[0042] Based on this, the system implements gating access control on the visual target residual data according to the numerical characteristics of the background fixed anchor point residual data: when the background fixed anchor point residual data meets the preset access conditions, the visual target residual data is allowed to enter the subsequent trajectory compensation link; when the access conditions are not met, the visual target residual data is blocked from entering the trajectory compensation link.

[0043] After obtaining the above trajectory control data stream and calculating the three types of residual data, the system first continuously monitors the numerical changes of the three types of residual data, records the sampling time corresponding to the first trigger of the threshold event of each type of residual data in real time, and generates an abnormal trigger time sequence (among which, although the threshold time of the background fixed anchor point residual is recorded, if the gating access fails, it is directly processed by the gating logic and does not enter the subsequent time sequence attribution process; if the gating access passes, the anchor point threshold time is masked, and only the vision and servo threshold times participate in the attribution).

[0044] Subsequently, the system implements gating access control based on the background fixed anchor point residual data:

[0045] 1. If the anchor point residual does not meet the admission conditions (i.e. the anchor point has exceeded the threshold), the system directly determines that the current deviation source is an external environmental interference attribute, immediately blocks the visual target residual data from entering the trajectory compensation link, and controls the robot to slow down to maintain operation, and no longer executes the subsequent time series attribution comparison or micro-trial process.

[0046] 2. If the gating access control passes (indicating that the anchor point residual has not exceeded the threshold and the visual reference is stable), the system retrieves the visual over-threshold time and the servo over-threshold time from the captured abnormal trigger time sequence, and performs initial attribution judgment based on the causal order of the two on the discrete time axis.

[0047] After obtaining the initial deviation source control data, the system further calculates the angle between the direction vectors of the visual target residual data and the servo execution residual data in three-dimensional space, generating a spatial direction consistency factor. When this spatial direction consistency factor falls within a preset conflict range, it indicates that the visual observation deviation direction and the servo execution deviation direction are physically opposed or independent in space. At this time, the system intercepts the instruction trajectory point data of the current control cycle to prevent it from entering the end effector, and instead sends bidirectional micro-probing motion control data with amplitude within a preset safety boundary to the robot, so as to actively stimulate the system to generate an observable physical response. Among them, bidirectional micro-probing motion control data refers to: when the system determines that the visual target residual and the servo execution residual are in a spatiotemporally concurrent ambiguity state, the motion control card generates bidirectional (two directions) micro-displacement excitation commands to replace the original command trajectory points of the current interpolation cycle, with amplitudes limited within the robot's kinematic safety envelope (i.e., not exceeding the joint velocity limit and acceleration limit); these commands are not used to drive the robot to complete a predetermined task, but to actively stimulate the differentiated physical responses between the target workpiece, the end effector, and the visual background anchor point, so that the system can confirm the physical nature of the deviation source by comparing the visual difference data before and after the excitation.

[0048] After the bidirectional micro-probing action is completed, the system re-acquires visual position data including the target workpiece, background fixed anchor point, and end marker point, and calculates the relative differential responses between the visual position data of the target workpiece, end marker point, and background fixed anchor point before and after the bidirectional micro-probing action. Based on the geometric distribution characteristics of this relative differential response, the system confirms the final deviation source control data.

[0049] Finally, the system counts the number of times the final deviation source control data appears in the continuous control cycle and generates a continuous holding cycle. Based on the type of the final deviation source control data and its continuous holding cycle, the system drives the control decision finite state machine to perform hierarchical state switching with hysteresis conditions. That is, state transition is only allowed when the continuous holding cycle reaches the corresponding threshold, so as to avoid high-frequency oscillation of the state at the critical point. After the state switching is completed, the system outputs the motion control parameters corresponding to the current switching state according to the preset control cycle, and drives the robot to perform the corresponding trajectory motion.

[0050] Based on the above working principle, this application ensures precise temporal alignment of robot kinematic data and visual data through a synchronous acquisition mechanism, eliminating timing errors caused by asynchronous acquisition; by parallel solving of three types of residual data and introducing gating access control based on background fixed anchor point residuals, only visual target residuals when the visual reference system is stable are allowed to enter the compensation link, avoiding erroneous compensation due to visual reference failure; by capturing the temporal causal sequence of three types of residual over-threshold events, the deviation source is initially attributed, realizing rapid separation of multi-source disturbances in the time domain; by determining spatial direction consistency, concurrent ambiguity states of visual and servo direction conflicts are identified, and bidirectional micro-probing actions are triggered, further utilizing the differential response before and after the probing to confirm the physical nature of the deviation source; by using hierarchical state switching with hysteresis conditions, state transition is performed only after confirming the type of deviation source and its duration, avoiding frequent switching of control strategies and improving the stability and adaptability of trajectory tracking control under different deviation scenarios.

[0051] Regarding the steps of synchronously acquiring trajectory control data streams described above, this application provides the following specific implementation method. Specifically, the system sends data reading commands to the robot controller within each preset control cycle (e.g., 1ms to 4ms) via an EtherCAT or Profinet industrial Ethernet bus to synchronously acquire the three-dimensional coordinates and attitude quaternions of the current cycle planning axis space issued by the main control algorithm, as command trajectory point data. Simultaneously, the system reads the values ​​of the absolute encoder registers deployed at the robot joints at high frequency via the bus, and calculates them using a forward kinematics model to generate the actual position coordinates and attitude data of the center point of the robotic arm flange end relative to the robot's base coordinate system, as actual pose data. To achieve microsecond-level time alignment between kinematic data and visual data, the system generates a hardware TTL pulse signal at the beginning of each interpolation cycle through crystal oscillator frequency division. This signal is physically connected to the external trigger GPIO interface of the industrial camera via a shielded twisted pair cable, thereby synchronously triggering the camera to expose and acquire the current frame image data. The current frame image data contains pixel areas of three types of features: the target workpiece, the background fixed anchor point, and the end marker point.

[0052] After obtaining the current frame image data, the system first calls the preset camera intrinsic parameter matrix to eliminate lens distortion and extracts the pixel centroid coordinates of each feature. Then, using the camera extrinsic parameter matrix obtained through offline calibration (e.g., pre-calibrated using the Zhang Zhengyou calibration method), it performs perspective projection coordinate transformation. The transformation relationship is as follows:

[0053] ;

[0054] in, These are the pixel coordinates of the feature in the current frame image data; This is the camera intrinsic parameter matrix, including focal length. and principal point coordinates ; and These are the 3×3 rotation matrix and the 3×1 translation vector between the camera coordinate system and the robot base coordinate system, respectively; The depth value of the feature in the camera coordinate system can be obtained from the depth channel of the RGB-D industrial camera or calculated from the parallax data of the binocular industrial camera; when the target workpiece, the background fixed anchor point and the end mark point are respectively on the calibrated working plane, it can also be determined according to the preset spatial height of the corresponding working plane. To calculate the generated 3D spatial position data in the robot's base coordinate system, and to further improve computational robustness, the system normalizes the projection results based on the maximum geometric boundary of the field of view, mapping the coordinate values ​​to the [-1,1] interval.

[0055] In one embodiment, the system sets up dual boundary interception checks before and after the coordinate transformation: real-time comparison of whether the pixel centroid coordinates before the perspective projection coordinate transformation exceed the effective pixel boundaries of the image sensor chip (e.g., and At the same time, it compares whether the transformed robot base coordinate system coordinate values ​​exceed the safe working envelope boundary of the robot's reachable space (e.g., , , If any coordinate component exceeds the preset valid range, it indicates that the target workpiece may have moved out of the camera's field of view, experienced severe occlusion, or had abnormal calibration parameters. The system then determines that the current visual channel status is invalid and modifies the channel status register flag in memory for subsequent gating and access control steps. If all coordinate components are within the preset valid range, the current visual channel status is determined to be valid, and the converted three types of visual position data are cached in system memory for use in subsequent steps.

[0056] Based on the aforementioned synchronous acquisition mechanism, this application ensures microsecond-level time alignment between robot kinematic data and visual data through hardware-level synchronous triggering, eliminating dynamic time warping errors and visual lag pseudo-errors introduced by data timing inconsistencies in traditional asynchronous acquisition methods. By establishing dual boundary checks before and after perspective projection coordinate transformation, outlier data caused by target loss, field of view out-of-bounds, or abnormal lighting can be identified and intercepted early in the acquisition stage, preventing outliers from entering the subsequent solution link pollution control loop, improving the reliability and cleanliness of front-end acquired data, and providing a high-quality input data foundation for subsequent parallel residual solution.

[0057] This application further proposes a gating access control step for the trajectory compensation link of visual target residual data based on background fixed anchor point residual data. In one embodiment, the system first reads the channel status register flag bit in memory. If the flag bit indicates that the current visual channel state is in a failed state, the system immediately blocks the transmission channel of visual target residual data to the trajectory compensation link, and simultaneously issues a deceleration and holding command to the robot's underlying joint servo driver. Specifically, the deceleration command triggers an exponential decay smooth deceleration curve, causing the robot to decrease its planned linear velocity from the previous moment of the current control cycle by a preset decay coefficient (e.g., 0.5 to 0.8), thereby maintaining the original trajectory and continuing to operate safely when the visual signal is unavailable.

[0058] If the current visual channel state is valid, the system further performs dual spatial and feature metric verification. Firstly, the system retrieves the visual position data of the background fixed anchor point corresponding to the current frame from the previous cache and performs spatial difference with its initial reference position to obtain the components of the three-dimensional axes of the background fixed anchor point residual data in the robot's base coordinate system. Then, the system calculates the Euclidean geometric modulus of this residual data using square root operations.

[0059] ;

[0060] in, The three-dimensional spatial coordinates of the anchor point fixed to the background of the current frame. Its initial reference position coordinates, It is the Euclidean geometric modulus value, used to characterize the absolute rigidity of the spatial drift of the camera carrier relative to the physical background reference.

[0061] On the other hand, the system calls a pre-installed image feature extractor in memory, such as a matching algorithm based on gradient orientation histogram or local binary pattern, to perform normalized cross-correlation matching between local feature regions in the current frame image data and pre-stored background anchor point templates. The number of feature points with a similarity greater than a preset matching threshold (e.g., 0.7) is counted as the number of successfully matched feature points. The preset matching threshold is pre-calibrated based on the texture richness of the anchor point template and the imaging signal-to-noise ratio of the industrial camera, with a typical value range of 0.6 to 0.8. In this embodiment, 0.7 is preferred.

[0062] After obtaining the two verification values ​​mentioned above, the system performs a composite gating decision: determining the Euclidean geometric modulus value. Whether the value is less than a preset rigid threshold, and whether the number of feature points is greater than or equal to a preset minimum number of valid feature points. Preferably, the preset rigid threshold is set to 0.5mm to 1.5mm, and this range is determined jointly based on the repeatability accuracy of the robot end effector and the camera pixel resolution. In one embodiment, this threshold is determined through the following calibration experiment: with the robot base stationary, visual position data of fixed background anchor points are continuously collected for a total of 1000 frames at a sampling rate of 10Hz, and the standard deviation of the anchor point position data in each axis of three-dimensional space is calculated. The final threshold is determined by multiplying the base value of the rigid threshold by a safety margin coefficient of 1.2; the preset minimum number of effective feature points is set to 4 to 8, which is based on the minimum geometric constraints required for solving the homography matrix in three-dimensional space or the perspective N-point pose.

[0063] If either of the above two conditions is not met, it indicates that the camera carrier and the background reference have undergone relative displacement, or that changes in ambient lighting or occlusion have caused the anchor point features to be lost. The system then updates the current visual channel status to a failed state through kernel instructions, blocks the trajectory compensation link entry, prevents visual target residual data from entering the compensation link, and simultaneously issues an exponential decay deceleration command to control the robot to safely decelerate and maintain the current trajectory until the visual channel is restored to effectiveness.

[0064] Based on the aforementioned gating access mechanism, this application sets up a dual physical check based on a fixed background anchor point before the visual target residual enters the compensation link. The visual compensation channel is only opened when the anchor point reference system maintains spatial rigidity and stability and has sufficient feature texture. This effectively avoids the visual field value contaminating the trajectory control closed loop due to camera carrier vibration, mounting base drift, or sudden changes in ambient light. When the check fails, the channel state is updated to failure and a coordinated deceleration and maintenance strategy is implemented to achieve a smooth transition from visual servoing to a pure kinematic safety mode, ensuring the robot's operational safety when visual signals are unavailable.

[0065] Furthermore, this application also performs a temporal continuity check on the visual position data of the target workpiece within adjacent control cycles as a secondary confirmation of the validity of the visual signal.

[0066] Specifically, after the system passes the gating admission judgment based on the background fixed anchor point residual, it immediately triggers the temporal transition check logic. It retrieves the target workpiece visual position data cached in the previous adjacent control cycle (denoted as the k-1 cycle) from the memory historical data buffer, and performs a difference operation with the target workpiece visual position data calculated in the current control cycle (denoted as the k cycle) to calculate the three-dimensional Euclidean distance between the two, generating target position transition term data. The calculation formula is as follows:

[0067] ;

[0068] in, The three-dimensional spatial coordinates of the target workpiece's visual position data corresponding to the k-th control cycle are: The three-dimensional spatial coordinates of the target workpiece's visual position data corresponding to the (k-1)th control cycle are: This refers to the generated target position transition term data, which is used to quantitatively characterize the instantaneous spatial displacement amplitude of the target workpiece within a single control cycle.

[0069] Obtain target position transition data Subsequently, the system verifies whether the value is less than the preset kinematic jump threshold using a kernel numerical comparator. Preferably, the preset kinematic jump threshold is set to 2mm to 5mm. This range is determined based on the system's preset control cycle (e.g., 2ms) and the maximum possible extreme speed of the workpiece in the physical world (or the maximum linear speed of the conveyor belt), through theoretical derivation and with a kinematic margin. The maximum allowable speed of the workpiece. This is a preset control cycle. In one embodiment, this threshold is determined by: based on the maximum linear speed of the conveyor belt. (e.g., 0.5 m / s) and the system's preset control cycle (e.g., 2ms) Calculate the theoretical maximum physical displacement (i.e., 1.0 mm), then multiplied by the dynamic coefficient determined based on the workpiece's inertial characteristics. (The preferred range is 2.0 to 5.0), that is .

[0070] If the value is less than the preset kinematic jump threshold, it indicates that the spatial position evolution of the target workpiece within adjacent control cycles conforms to the laws of physical motion. The system determines that the data is valid and allows it to proceed to the subsequent attribution process.

[0071] If the target location jump item data If the kinematic jump threshold is exceeded, it indicates that the target workpiece has experienced a physically impossible instantaneous spatial change within a single control cycle. This is typically caused by sporadic interference such as sudden strong light reflection from the environment, dust or foreign objects flying into the field of view, or mismatched image feature points. In this case, the system determines that the visual position data of the target workpiece has a discontinuous jump, immediately blocks the entry of the visual target residual data of the current control cycle into the trajectory compensation link, and simultaneously drives the control decision finite state machine to switch to the visual verification state. The visual verification state is defined internally as a transitional suspension state between the normal operation state and the local smoothing replanning state. In this state, the robot freezes its current adaptive fine-tuning action and maintains its original trajectory inertial gliding. The system then performs multi-frame median filtering verification on the visual signal over a subsequent preset number of control cycles to distinguish whether the current jump is a brief, sporadic interference or a genuine target change.

[0072] By checking the position transitions between adjacent cycles as described above, this application constructs a rationality verification defense line based on kinematic physical boundaries in the time domain dimension. This line can identify and filter out visual spike noise whose amplitude does not exceed the spatial range but whose transition rate far exceeds the physical motion limit. At the same time, by setting a visual verification state as a transition state when judging discontinuous jumps, the system avoids directly entering the local smoothing replanning process due to sporadic visual jumps in a single frame. This reduces the computational overhead of the controller, prevents sudden joint changes and servo oscillations of the robotic arm caused by high-frequency visual noise, and enhances the robustness and operational stability of the trajectory tracking control system.

[0073] Regarding the specific steps for determining the control data of the initial deviation source, this application further proposes that the system allocate a first-in-first-out circular queue of fixed depth N in memory as an abnormal time sliding window. Preferably, the depth N is set to 20 to 50 sampling points. In each control cycle, the system writes the visual target residual data and servo execution residual data at the current sampling time into this sliding window in real time. During this process, the system continuously monitors the real-time magnitude of the visual target residual data and servo execution residual data, and compares them with their respective preset thresholds. The preset visual threshold corresponding to the visual target residual data is preferably set to 1.0 mm, and the preset servo threshold corresponding to the servo execution residual data is preferably set to 0.8 mm.

[0074] When the magnitude of the visual target residual data or the servo execution residual data exceeds its corresponding preset threshold for the first time, the system captures the hardware high-precision timestamp of the current discrete sampling point and records it as the visual over-threshold moment and the servo over-threshold moment, respectively, forming an abnormal trigger moment sequence.

[0075] It should be noted that the threshold handling of the background fixed anchor point residual data has already been completed in the gating admission control step: if the anchor point residual does not meet the admission conditions, the system directly determines that the initial deviation source control data is an external environmental interference attribute at the gating layer, and will not enter the temporal causal attribution process of this step. Therefore, the abnormal trigger time sequence of this step only includes visual threshold and servo threshold times.

[0076] After obtaining the sequence of abnormal trigger times, the system compares the compensated visual over-threshold times using a temporal causal chain analyzer. Compensated servo overthreshold time The order of priority. This is an inherent latency parameter for the visual data link (e.g., 10ms). This is the inherent latency parameter of the servo encoder data link (e.g., 2ms).

[0077] If the compensated visual over-threshold moment occurs earliest, the initial deviation source control data is assigned to the external target drift attribute; if the compensated servo over-threshold moment occurs earliest, the initial deviation source control data is assigned to the underlying servo hysteresis attribute.

[0078] If the absolute value of the difference between the compensated visual over-threshold time and the compensated servo over-threshold time is less than the preset concurrent time difference threshold (preferably set to 2ms to 5ms), then the initial deviation source control data will be assigned a concurrent fuzzy attribute as a boundary condition for triggering subsequent spatial direction consistency determination and active micro-probing processes.

[0079] Based on the aforementioned temporal causal attribution mechanism, this application achieves the capture of abnormal outbreak moments with extremely low memory overhead by using a sliding window to maintain abnormal time. By establishing a rigid mapping relationship between the order of out-of-threshold moments and the attributes of the deviation source, it can physically separate visual drift, servo lag, and camera shake on the discrete time axis under complex conditions of multiple intertwined disturbances. At the same time, by setting a preset concurrent time difference threshold, it scientifically defines the distinguishable limit boundary in the temporal domain, providing an accurate initial attribution basis for subsequent spatial consistency determination and active stimulus confirmation.

[0080] This application further proposes a method for calculating spatial orientation consistency. After determining the initial source of deviation, the system further performs geometric verification on the directional relationship between the visual target residual and the servo execution residual in the spatial dimension. Specifically, the system first reads the visual target residual data and the servo execution residual data, and calculates their Euclidean geometric modulus respectively. When both Euclidean geometric moduli are greater than a preset minimum residual threshold, it indicates that the current residual signal contains a deterministic motion trend with sufficient amplitude, satisfying the validity condition for direction vector calculation; preferably, the preset minimum residual threshold is set to 0.1mm to 0.3mm, and this range is determined based on the system's inherent mechanical backlash and the random high-frequency white noise boundary determined by the camera pixel resolution. After satisfying the above conditions, the system extracts the components of the three-dimensional axes of the visual target residual data in the robot's base coordinate system as the first direction vector, denoted as... Simultaneously, the components of the servo execution residual data along the three-dimensional axes in the robot's base coordinate system are extracted as the second direction vector, denoted as... These two terms respectively characterize the direction of correction in visual observation and the direction of deviation in mechanical execution.

[0081] Subsequently, the system calculates the cosine of the angle between the first direction vector and the second direction vector to generate a spatial direction consistency factor, the calculation formula of which is as follows:

[0082] ;

[0083] in, This is the generated spatial orientation consistency factor, with a value range of [-1, 1], used to quantitatively describe the degree of spatial consistency between the visual correction direction and the mechanical execution direction. When the value approaches 1, the two directions are basically consistent; when When the value approaches -1, the two directions are basically opposite; when As the value approaches 0, the two directions become nearly orthogonal.

[0084] After obtaining the spatial orientation consistency factor, the system performs spatiotemporal fusion analysis: when When the system is in a preset reverse conflict interval (preferably set to [-1.0, -0.7], corresponding to a spatial angle between 135° and 180°, indicating a severe physical conflict between the visual correction direction and the mechanical execution direction), or in a preset orthogonal uncertainty interval (preferably set to [-0.3, 0.3], corresponding to a spatial angle between 72.5° and 107.5°, indicating that the two types of residual directions are nearly perpendicular and independent in space), and simultaneously satisfies that the absolute value of the difference between the visual over-threshold moment and the servo over-threshold moment in the abnormal triggering time sequence is less than a preset concurrent time difference threshold (preferably set to 2ms to 5ms), the system determines that the current deviation is a concurrent burst in the time domain and a directional conflict or independence in the spatial domain, that is, the previous temporal causal attribution has failed. The system then performs a coverage assignment, changing the initial deviation source control data to an indication of concurrent fuzzy attributes to trigger the subsequent active micro-probing confirmation process.

[0085] Based on the aforementioned spatial orientation consistency calculation mechanism, this application reduces the computational complexity of orientation determination by quantifying the three-dimensional spatial motion trend into a normalized cosine value of the included angle; by cross-validating spatial orientation conflict and orthogonality discrimination with temporal concurrency conditions, it identifies the spatiotemporal intertwined ambiguity that cannot be handled by the previous temporal attribution, providing clear and well-defined triggering conditions for subsequent active excitation confirmation, effectively avoiding the risk of misattribution caused by single-dimensional determination.

[0086] This application further proposes specific steps for confirming that the final deviation source control data is an external target drift attribute. In one embodiment, after the system completes the issuance and execution of the bidirectional micro-probing action, it re-acquires the current frame image data through an industrial camera, and according to the aforementioned perspective projection coordinate transformation method, parses the visual position data of the target workpiece after probing, the visual position data of the end marker point, and the visual position data of the background fixed anchor point from the current frame image data. The system then retrieves the end marker point visual position data cached before probing from the historical data buffer, calculates the three-dimensional Euclidean distance between the end marker point visual position data before and after probing, and uses it as the Euclidean distance variation of the end marker point visual position data to characterize the actual physical displacement response of the robotic arm end to the micro-probing excitation.

[0087] Based on this, the system constructs a composite verification network consisting of three parallel conditions to determine whether the current differential response matches the physical characteristics of the external target drift.

[0088] The first condition is to verify whether the deviation between the Euclidean distance variation of the visual position data of the end effector marker and the preset bidirectional position increment sequence is less than a preset fault tolerance threshold. The preset bidirectional position increment sequence is the theoretical mapping value in Cartesian space of the command displacement sent by the system to the joint servo actuator. The amplitude of this sequence is consistent with the amplitude of the aforementioned bidirectional micro-probing motion control data, both set to ±2mm in each direction. This amplitude is determined based on the allowable range of motion of the robot's end effector in Cartesian space (not exceeding the end effector linear displacement limit corresponding to the joint velocity limit of 120° / s and the acceleration limit of 300° / s²). The preset fault tolerance threshold is preferably set to 0.1mm to 0.3mm, determined jointly based on the repeatability accuracy of the servo system and the visual measurement noise. Passing this condition indicates that the underlying robotic arm and servo mechanism can accurately respond to control commands, and there are no underlying faults such as slippage, overload hysteresis, or transmission mechanism breakage.

[0089] The second condition is to verify whether the variation in the Euclidean distance of the visual position data of the fixed anchor points in the background is less than a preset anchor point stillness threshold. Preferably, the preset anchor point stillness threshold is set to 0.05mm to 0.15mm, derived from the limit measurement Gaussian noise boundary caused by the sub-pixel resolution of the camera lens. Passing this condition indicates that the physical background has not shifted in the image during the trial action, excluding interference from collisions, loosening, or carrier instability of the camera.

[0090] The third condition is: verify whether the relative displacement of the target workpiece relative to the fixed anchor point of the background exceeds the preset target drift threshold. Calculate the relative position vector of the target workpiece relative to the fixed anchor point of the background before and after the trial. ,like If the drift exceeds the preset target threshold (preferably set to 0.5mm to 1.0mm), it is determined that the target workpiece is undergoing deterministic drift in the external physical space that is not caused by the robotic arm.

[0091] The system determines that there is no lag at the bottom layer, no camera shake, and a real workpiece drift when the above three conditions are met simultaneously. It then clears the concurrent fuzzy state in the preceding control word, confirms the final deviation source control data as the external target drift attribute, and writes it into the input register of the control decision finite state machine to trigger the subsequent corresponding state switching and control parameter output.

[0092] Based on the above three-point mutual verification mechanism, this application transforms the spatiotemporally intertwined fuzzy state into an observable physical dynamic response by applying bidirectional micro-probing excitation within the safety envelope. Through triple parallel verification of the accuracy of the end command response, the absolute stillness of the background anchor point, and the target's relative anchor point displacement exceeding the threshold, the self-consistency of the deviation source diagnosis results is achieved from a physical and logical perspective. This solves the technical problem that traditional vision servo systems cannot distinguish between "real workpiece movement" and "abnormal robot arm response" in a static fuzzy state, and provides an accurate decision-making basis for subsequent graded state switching.

[0093] This application further proposes specific steps for confirming that the final deviation source control data is the camera carrier instability attribute. In one embodiment, after the system completes the issuance and execution of the bidirectional micro-probing action, it re-acquires the current frame image data through the industrial camera, and according to the aforementioned perspective projection coordinate transformation method, parses the visual position data of the target workpiece after the probing, the visual position data of the end marker point, and the visual position data of the background fixed anchor point from the current frame image data. The system then retrieves the corresponding data cached before the probing from the historical data buffer and performs dual-channel spatial distance calculation in parallel: calculating the three-dimensional spatial Euclidean distance between the visual position data of the target workpiece before and after the probing, as the first Euclidean distance variation; and simultaneously calculating the three-dimensional spatial Euclidean distance between the visual position data of the end marker point before and after the probing, as the second Euclidean distance variation. The calculation formulas for both are as follows:

[0094] ;

[0095] ;

[0096] in, and These are the three-dimensional spatial coordinates of the target workpiece's visual position data before and after the trial, respectively. This represents the first variation in Euclidean distance; and These are the three-dimensional spatial coordinates of the visual position data of the end marker points before and after the trial, respectively. This represents the second Euclidean distance variation. Both are used to characterize the temporal displacement amplitude of the target workpiece and the robotic arm end effector in the image space under micro-probing excitation.

[0097] After obtaining the two Euclidean distance variations mentioned above, the system further calculates the absolute value of the difference between the first and second Euclidean distance variations and verifies whether this difference is less than a preset synchronous displacement tolerance threshold. Simultaneously, it verifies whether the Euclidean distance variation of the visual position data of the background fixed anchor point exceeds a preset anchor point variation threshold. Preferably, the preset synchronous displacement tolerance threshold is set to 0.05mm to 0.2mm, based on the sub-pixel matching error of the image sensor in a homogeneous environment and the slight nonlinear divergence boundary of the inverse kinematics solution of the robotic arm. The preset anchor point variation threshold is set to 0.1mm to 0.3mm, its value derived from the inherent pixel jitter extreme value caused by vibrations in the environment. Preferably, the preset anchor point variation threshold is greater than the preset anchor point stillness threshold. When both of the above conditions are met simultaneously, it indicates that the "external workpiece," which is physically not in relative motion, and the "robotic arm end effector," which is undergoing active micro-probing motion, exhibit almost synchronous and equal displacement in the image. Furthermore, the Euclidean distance variation of the physically stationary background anchor point in the image abnormally exceeds the allowable noise range. The only technical fact that this set of mutually corroborating data points to in terms of physical logic is that the anomaly is not due to the external target or the underlying servo, but rather to the industrial camera itself, which serves as the observation benchmark, experiencing shaking, loosening, or a relative pose transition relative to the physical world. This causes all objects within the field of view to produce a highly consistent, overall pseudo-displacement. Based on this, the system determines that the camera carrier has undergone a relative pose change, removes the concurrent ambiguity of the initial deviation source control data, confirms that the final deviation source control data is the camera carrier's instability attribute, and writes it into the system's core control word.

[0098] Based on the aforementioned composite verification mechanism of dual-channel synchronous displacement verification and anchor point change verification, this application verifies the synchronization of the changes in the moving end effector and the stationary workpiece image under micro-trial excitation. Combined with the over-threshold pseudo-jump of the stationary background anchor point, the existing data stream of the system can confirm visual reference failure without the need for additional hardware sensors (such as accelerometers or gyroscopes). This mechanism avoids unauthorized trajectory compensation caused by camera shake, effectively improving the active fault tolerance and self-health diagnosis level of the robot vision servo system in harsh industrial scenarios such as high vibration and high impact.

[0099] Furthermore, the system employs a tiered state switching process with hysteresis based on the final deviation source control data and its continuous holding period. The control decision finite state machine includes at least five control states: normal operation state (S0), representing the default operating state when no deviation source is confirmed; visual verification state (S1), representing the transitional state where the system performs signal verification when a suspected discontinuous jump or external environmental interference is detected; local smoothing replanning state (S2), representing the state where the system recalculates the trajectory after confirming external target drift; servo deceleration compensation state (S3), representing the state where the system performs deceleration and closed-loop correction after confirming underlying servo hysteresis; and breakpoint recovery state with data snapshot (S4), representing the state where the system performs breakpoint freezing and data snapshot encapsulation after confirming camera carrier instability. The transition conditions between each state are as follows: S0→S1: Triggered by external environmental interference or position transition exceeding limits; S0→S2: External target drift attribute and the continuous period reaches the first preset period threshold; S0→S3: Low-level servo hysteresis attribute and the continuous period reaches the second preset period threshold; S0→S4: Triggered by camera carrier instability attribute; S1→S2: Verification confirms the target's real mutation and the continuous period reaches the first preset period threshold; S1→S0: Verification confirms the current disturbance is an occasional interference and eliminates it.

[0100] Specifically, the system maintains a counter accumulation subroutine to perform consistency verification on the attribute type of the confirmed final deviation source control data in each control cycle: if the attribute of the final deviation source control data in the current control cycle is the same as that in the previous control cycle, the counter increments by 1 to generate a continuous holding cycle; if the attribute changes, the counter is cleared and counting starts again.

[0101] Based on this, the system executes differentiated state switching logic according to different deviation source attributes. When the final deviation source control data indicates the external target drift attribute and the continuous holding period reaches the first preset period threshold, the system drives the control decision finite state machine to activate the local smooth replanning state. Preferably, the first preset period threshold is set to 5 to 15 control cycles (corresponding to a physical time window of 10ms to 30ms), based on the minimum dynamic response window required by the system and the effective refresh rate of the vision sensor. After activating this state, the system, based on the currently calculated target workpiece visual position data, recalculates the robot's trajectory flow data using a fifth-order polynomial or a cubic B-spline curve within the underlying joint velocity, acceleration, and jerk dynamics constraint framework to achieve smooth deviation correction.

[0102] When the final deviation source control data indicates a lag attribute in the underlying servo system and the continuous holding period reaches a second preset period threshold, the system drives the control decision finite state machine to activate the servo deceleration compensation state. Preferably, the second preset period threshold is set to 3 to 8 control cycles, based on the underlying current loop response delay and speed loop setup time. In this state, the system dynamically calculates the deceleration coefficient based on the modulus of the servo execution residual data. Specifically, the deceleration coefficient... Based on the servo execution residual modulus Compared with the preset maximum allowable residual The ratio (preferably set to 5.0 mm) is determined after normalization mapping, and its calculation method is as follows: The value ranges from 0.3 to 1.0, where The function represents taking the larger of the two values; at the same time, the system calculates the position closed-loop correction based on the components of the servo execution residual data in each axis of the Cartesian space to alleviate the drive burden of the underlying motor; the system also blocks the input of visual target residual data to the trajectory compensation link through hardware logic switches to avoid introducing aggressive visual disturbances that could lead to system instability and divergence when the hardware response is lagging.

[0103] When the final deviation source control data indicates external environmental interference, the system immediately performs protective operations in the current control cycle: blocking the visual target residual data from entering the trajectory compensation link and controlling the robot to enter the visual verification state; in this state, the system freezes the adaptive control quantity, maintains the original inertial trajectory to glide smoothly, and verifies the visual signal in the subsequent preset number of control cycles.

[0104] When the final deviation source control data indicates camera carrier instability, the system enters a high-level hardware and software fault protection mode, activating the breakpoint recovery state with data snapshots in the drive control decision finite state machine. The system locks the robot's actual pose data and freezes the current trajectory through the highest priority hardware interrupt in the kernel, while simultaneously issuing zero-speed hold commands to the motors. The system encapsulates and sends a snapshot of the current trajectory control data stream to an external control console. The snapshot includes at least the current control cycle and a preset number of joint currents, actual poses of each axis, visual coordinates, and state machine status words for offline fault tracing and system recovery.

[0105] Based on the aforementioned hierarchical state switching mechanism with hysteresis, this application effectively filters out frequent state switching caused by small residual oscillations by introducing first and second preset period thresholds as time-domain thresholds for state transition, ensuring the operational life and control continuity of the mechanical actuator. For four different deviation source attributes, progressive control strategies are configured, ranging from local trajectory replanning, servo deceleration compensation, visual verification transition to breakpoint freezing and data snapshot encapsulation. This achieves a complete hierarchical response from software algorithm adjustment to hardware safety protection, ensuring that the robot can obtain appropriate control parameter outputs under various deviation scenarios, and improving the adaptability and task safety of the trajectory tracking control system under complex working conditions.

[0106] The following is a specific embodiment of a robot trajectory planning and motion control method based on visual feedback:

[0107] A six-axis industrial robot performs a workpiece picking task at a conveyor belt tracking and gripping station. The conveyor belt runs continuously at a speed of 0.5 m / s. The workpiece is a metal shell with dimensions of 80 mm × 60 mm × 40 mm, randomly placed on the surface of the conveyor belt. The system's preset control cycle is 2 ms. The industrial camera has a resolution of 1920 × 1200 pixels and is mounted 1.2 m directly above the conveyor belt. The background fixing anchor points are four circular reflective markers with a diameter of 10 mm, symmetrically arranged at the four corners of the camera's field of view.

[0108] During a certain control cycle, the system reads the current command trajectory point via the EtherCAT bus. The actual pose fed back by the encoder is After perspective projection coordinate transformation, the visual position of the target workpiece is obtained as follows: The visual position of the fixed anchor point in the background is (Relative to the initial reference), the visual position of the end marker point is The system performs parallel computation of three types of residual data: the visual target residual is... (Mold length 4.58mm), servo execution residual is (Die length 3.46mm), residual die length of background fixed anchor point .because =0.37mm is less than the preset rigid threshold of 1.0mm, and the anchor point feature matching count is 6 (greater than the preset minimum number of valid feature points of 4), so the gating access control passes.

[0109] After gating access, the system detects the earliest occurrence of visual over-threshold ( (If the threshold is not exceeded), the initial deviation source control data is assigned the external target drift attribute. Further calculations are made of the spatial orientation consistency factor and the visual target residual direction vector. With servo execution residual direction vector cosine value of the angle between If the deviation is within the preset orthogonal uncertainty range [-0.3, 0.3], and the difference between the visual overthreshold moment and the servo overthreshold moment is 1.2ms, which is less than the preset concurrent time difference threshold of 3ms, the system changes the initial deviation source to a concurrent fuzzy attribute and triggers a bidirectional micro-probing action. The system intercepts the current command trajectory point and issues bidirectional micro-probing actions of ±2mm each. After execution, the data is re-acquired: the displacement of the end marker point is 1.95mm (0.05mm deviation from the preset increment of 2mm, less than the preset fault tolerance threshold of 0.2mm), the displacement of the background fixed anchor point is 0.06mm (less than the preset noise threshold of 0.10mm), and the relative displacement modulus of the target workpiece relative to the background fixed anchor point is 0.8mm, which is greater than the preset target drift threshold of 0.5mm. If all three conditions are met simultaneously, the final deviation source is confirmed as an external target drift attribute.

[0110] The system detects that this attribute has been confirmed for 12 consecutive control cycles, reaching the first preset cycle threshold (10 cycles), and drives the control decision finite state machine to switch to a locally smooth reprogramming state. This is based on the current visual position of the target workpiece. With a joint velocity limit of 120° / s and an acceleration limit of 300° / s 2 Under the dynamic constraints, the trajectory flow was recalculated using a fifth-order polynomial. The corrected trajectory enabled the robot to successfully track the deviated workpiece position and complete the grasping. The entire deviation identification and state switching process was completed within 30ms, and the robotic arm operated smoothly without oscillation or loss of control. This embodiment verifies the effectiveness of the present invention in identifying deviation sources and differentiating control in multi-source disturbance scenarios.

[0111] Example 2:

[0112] like Figure 3 As shown, the robot trajectory planning and motion control system based on visual feedback uses the aforementioned robot trajectory planning and motion control method based on visual feedback, including:

[0113] The synchronous acquisition module is used to synchronously acquire trajectory control data streams, which include command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data. Its hardware components include an industrial camera, an EtherCAT communication bus interface card, a hardware trigger pulse generation circuit, and an industrial control computer. The industrial camera is mounted above the robot's workspace. The hardware trigger pulse generation circuit is connected to the interpolation cycle clock source of the robot controller, used to generate TTL pulse signals at the beginning of each interpolation cycle to synchronously trigger the industrial camera's exposure.

[0114] The solution and admission module is used to solve the trajectory control data stream in parallel, outputting the corresponding visual target residual data, servo execution residual data, and background fixed anchor point residual data. Based on the background fixed anchor point residual data, it performs gating admission control on the trajectory compensation link of the visual target residual data. Its hardware consists of a GPU parallel computing unit and memory registers built into the industrial control computer. The GPU parallel computing unit is used to solve the three types of residual data in parallel, and the memory registers are used to cache the background fixed anchor point residual data and the visual channel status flag.

[0115] The attribution module is used to capture the sequence of abnormal trigger times when three types of residual data exceed the corresponding preset thresholds, and to determine the initial deviation source control data based on the causal order of the abnormal trigger time sequence on the discrete time axis. Its hardware consists of a real-time operating system timer running in the industrial control computer and a first-in-first-out circular queue memory. The timer is used to record the high-precision timestamps of each residual exceeding the threshold, and the circular queue memory is used to maintain the abnormal time sliding window.

[0116] The discrimination and excitation module is used to intercept the current command trajectory point data and send bidirectional micro-probing motion control data within a preset safety range to the robot when the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range. Its hardware consists of a motion control card that communicates with the robot's underlying joint servo driver. The motion control card is used to intercept the current command trajectory point data and send bidirectional micro-probing motion control data.

[0117] The differential confirmation module is used to capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after the execution of the bidirectional micro-probing action control data, and to confirm the final deviation source control data. Its hardware consists of an image preprocessing chip and a differential operation register built into the industrial control computer. The image preprocessing chip is used to parse the visual position data after the probe, and the differential operation register is used to calculate the change in Euclidean distance before and after the probe.

[0118] The decision module is used to drive the control decision finite state machine to perform hierarchical state switching with hysteresis conditions between multiple control states based on the final deviation source control data and its continuous holding period, and output the motion control parameters corresponding to the switched states according to a preset control cycle. Its hardware consists of an embedded controller and the control decision finite state machine running in it. The embedded controller is communicatively connected to the robot's underlying joint servo drivers and is used to output motion control parameters according to the preset control cycle.

[0119] Based on the above system, this application provides a specific application embodiment. In a workpiece servo gripping scenario, the target workpiece on the conveyor belt experiences positional shift due to external vibration during its movement. The system synchronously acquires trajectory control data streams in each interpolation cycle through a synchronous acquisition module; the calculation and admission module calculates three types of residual data in parallel, and after passing the background fixed anchor point residual verification, the visual target residual is allowed to enter the trajectory compensation link; the attribution module detects that the visual target residual first exceeds the threshold, and assigns the initial deviation source control data to the external target drift attribute; the discrimination and excitation module calculates the spatial direction consistency factor, determines that it is not in the conflict zone, and does not need to trigger micro-probing; the decision module counts that this attribute is maintained for 12 consecutive cycles to reach the first preset cycle threshold, drives the finite state machine to switch to the local smooth replanning state, recalculates the robot's running trajectory stream based on the current target workpiece visual position data, and guides the robotic arm to track the shifted workpiece position to complete the gripping.

[0120] Based on the aforementioned hardware configuration and system architecture, this application achieves end-to-end hardware-based closed-loop control by deploying each functional module on a specific hardware carrier, encompassing visual signal acquisition, parallel residual calculation, time-series attribution of deviation sources, spatial orientation discrimination, differential response confirmation, and hierarchical state decision-making. The modules communicate with each other through clearly defined hardware interfaces and data buses, ensuring the system's real-time response capability and engineering feasibility under high-frequency control cycles, thus providing a hardware foundation for reliable deployment in industrial scenarios.

[0121] The technical scope of this invention is not limited to the content described above. Those skilled in the art can make various modifications and variations to the above embodiments without departing from the technical concept of this invention, and all such modifications and variations should fall within the protection scope of this invention.

Claims

1. A robot trajectory planning and motion control method based on visual feedback, characterized in that: Includes the following steps: The trajectory control data stream is collected synchronously, including command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data. The trajectory control data stream is solved in parallel, and the corresponding visual target residual data, servo execution residual data and background fixed anchor point residual data are output. Based on the background fixed anchor point residual data, the trajectory compensation link of the visual target residual data is subjected to gating admission control. Capture the abnormal trigger time sequence of three types of residual data exceeding their respective preset thresholds, and determine the initial deviation source control data according to the causal order of the abnormal trigger time sequence on the discrete time axis; When the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range, the current command trajectory point data is intercepted, and bidirectional micro-probing motion control data within a preset safety range is sent to the robot. Capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after executing the bidirectional micro-probing action control data, and confirm the final deviation source control data; Based on the final deviation source control data and its continuous holding period, the driving control decision finite state machine performs hierarchical state switching with hysteresis conditions between multiple control states, and outputs the motion control parameters corresponding to the switching state according to the preset control period.

2. The robot trajectory planning and motion control method based on visual feedback according to claim 1, characterized in that: Synchronously acquire trajectory control data streams, including: The command trajectory point data and the actual pose data corresponding to the current interpolation cycle are periodically read through the robot communication bus. The industrial camera is activated by a hardware pulse trigger signal synchronized with the current interpolation cycle to acquire current frame image data including the target workpiece, background fixed anchor points, and end marker points; The current frame image data is input into a preset calibration matrix to perform perspective projection coordinate transformation, generating visual position data of the target workpiece, visual position data of the background fixed anchor point, and visual position data of the end marker point in the robot base coordinate system; If the target pixel region before the perspective projection coordinate transformation or the coordinate value in the robot base coordinate system after the transformation exceeds the preset valid range, the current vision channel state is determined to be in a failed state.

3. The robot trajectory planning and motion control method based on visual feedback according to claim 2, characterized in that: Gating and admission control of the trajectory compensation link for the visual target residual data based on the background fixed anchor point residual data includes: If the current visual channel is in a failed state, then the visual target residual data is blocked from entering the trajectory compensation link, and the robot is controlled to slow down to maintain the current trajectory. If the current visual channel state is valid, the Euclidean geometric modulus of the background fixed anchor point residual data is calculated, and the feature point count that successfully matches the preset anchor point template in the current frame image data is extracted. Determine whether the Euclidean geometric modulus value is less than a preset rigid threshold, and whether the feature point count is greater than or equal to a preset minimum number of valid feature points; If either of the two conditions is not met, the current visual channel state is updated to a failed state, blocking the visual target residual data from entering the trajectory compensation link, and controlling the robot to slow down to maintain the current trajectory.

4. The robot trajectory planning and motion control method based on visual feedback according to claim 3, characterized in that: It also includes position transition checks within adjacent control cycles, specifically including: Calculate the three-dimensional Euclidean distance between the visual position data of the target workpiece within two adjacent preset control cycles, and generate target position transition term data; Verify whether the target position transition data is less than the preset kinematic transition threshold; If the target position transition data exceeds the preset kinematic transition threshold, it is determined that the visual position data of the target workpiece has a discontinuous transition, blocking the visual target residual data from entering the trajectory compensation link, and driving the control decision finite state machine to switch to the visual verification state.

5. The robot trajectory planning and motion control method based on visual feedback according to claim 4, characterized in that, The specific steps for determining the initial deviation source control data include: An abnormal time window is maintained by sliding on the discrete time axis. When the visual target residual data or the servo execution residual data exceeds their respective preset thresholds, the visual over-threshold time and the servo over-threshold time corresponding to the current sampling point are recorded respectively to form the abnormal trigger time sequence. Compare the order of visual over-threshold moments and servo over-threshold moments on the discrete time axis in the abnormal triggering time sequence. If the visual over-threshold moment occurs earliest, assign the initial deviation source control data an external target drift attribute; if the servo over-threshold moment occurs earliest, assign the initial deviation source control data a low-level servo hysteresis attribute; if the difference between the visual over-threshold moment and the servo over-threshold moment is less than a preset concurrent time difference threshold, assign the initial deviation source control data a concurrent fuzzy attribute.

6. The robot trajectory planning and motion control method based on visual feedback according to claim 5, characterized in that, The calculation method for spatial orientation consistency includes: When the Euclidean geometric modulus of both the visual target residual data and the servo execution residual data is greater than the preset minimum residual threshold, the first direction vector corresponding to the visual target residual data is extracted, and the second direction vector corresponding to the servo execution residual data is extracted. Calculate the cosine of the angle between the first direction vector and the second direction vector to generate a spatial direction consistency factor; When the spatial direction consistency factor is in a preset reverse conflict interval or a preset orthogonal uncertainty interval, and the difference between the visual over-threshold moment and the servo over-threshold moment in the abnormal triggering time sequence is less than a preset concurrent time difference threshold, the initial deviation source control data is changed to an indication concurrent fuzzy attribute.

7. The robot trajectory planning and motion control method based on visual feedback according to claim 6, characterized in that, The specific steps for confirming the final deviation source control data include: Receive the current frame image data after executing the bidirectional micro-probing action control data, and parse out the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point after the probing. Calculate the Euclidean distance change of the visual position data of the end marker points before and after the trial; If the deviation between the Euclidean distance variation of the visual position data of the end marker point and the preset bidirectional position increment sequence is less than the preset fault tolerance threshold, and the Euclidean distance variation of the visual position data of the background fixed anchor point is less than the preset anchor point stationary threshold, and the magnitude of the relative displacement vector of the visual position data of the target workpiece relative to the visual position data of the background fixed anchor point is greater than the preset target drift threshold, the final deviation source control data is confirmed as an external target drift attribute.

8. The robot trajectory planning and motion control method based on visual feedback according to claim 2, characterized in that, The specific steps for confirming the final deviation source control data include: Receive the current frame image data after executing the bidirectional micro-probing action control data, and parse out the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point after the probing. Calculate the three-dimensional Euclidean distance between the visual position data of the target workpiece before and after the trial, as the first Euclidean distance variation; calculate the three-dimensional Euclidean distance between the visual position data of the end marker point before and after the trial, as the second Euclidean distance variation; calculate the difference between the second Euclidean distance variation and the preset bidirectional position increment sequence to generate the end response deviation; if the end response deviation is not less than the preset fault tolerance threshold, then terminate the camera instability attribute determination process. If the end response deviation is less than a preset fault tolerance threshold, and the difference between the first Euclidean distance variation and the second Euclidean distance variation is less than a preset synchronous displacement tolerance threshold, and the Euclidean distance variation of the background fixed anchor visual position data exceeds a preset anchor variation threshold, it is determined that the camera carrier has undergone a relative pose change, and the final deviation source control data is confirmed as the camera carrier instability attribute.

9. The robot trajectory planning and motion control method based on visual feedback according to claim 5, characterized in that, Based on the final deviation source control data and its continuous holding period, hierarchical state switching with hysteresis conditions is performed between multiple control states, including: The continuous holding period is generated by counting the number of times the same final deviation source control data occurs continuously within a continuous control cycle. When the final deviation source control data indicates the external target drift attribute and the continuous holding period reaches the first preset period threshold, the control decision finite state machine is driven to activate the local smooth replanning state, and the robot's running trajectory flow is recalculated under the underlying dynamic constraint framework based on the current visual position data of the target workpiece. When the final deviation source control data indicates the underlying servo hysteresis attribute and the continuous holding period reaches the second preset period threshold, the control decision finite state machine is driven to activate the servo deceleration compensation state, dynamically calculate the deceleration coefficient and position closed-loop correction amount based on the servo execution residual data, and block the input of the visual target residual data to the trajectory compensation link; When the final deviation source control data indicates external environmental interference attributes, the visual target residual data is blocked from entering the trajectory compensation link, and the robot is controlled to enter the visual verification state. When the final deviation source control data indicates camera carrier instability, the control decision finite state machine is driven to activate the breakpoint recovery state with data snapshot, lock the actual pose data of the current robot and freeze the current running trajectory, and at the same time, high-frequency encapsulation and sending the current trajectory control data stream snapshot to the external control console.

10. A robot trajectory planning and motion control system based on visual feedback, characterized in that: The robot trajectory planning and motion control method based on visual feedback as described in any one of claims 1 to 9 includes: The synchronous acquisition module is used to synchronously acquire trajectory control data stream, which includes command trajectory point data, actual pose data, target workpiece visual position data, background fixed anchor point visual position data, and end marker point visual position data. The calculation and admission module is used to calculate the trajectory control data stream in parallel, output the corresponding visual target residual data, servo execution residual data and background fixed anchor point residual data, and perform gating admission control of the trajectory compensation link based on the background fixed anchor point residual data. The attribution module is used to capture the sequence of abnormal triggering times when three types of residual data exceed the corresponding preset thresholds, and to determine the initial deviation source control data based on the causal order of the abnormal triggering time sequence on the discrete time axis. The discrimination and excitation module is used to intercept the current command trajectory point data and send bidirectional micro-probing motion control data within a preset safety range to the robot when the spatial direction consistency between the visual target residual data and the servo execution residual data is within a preset conflict range. The differential confirmation module is used to capture the relative differential response between the visual position data of the target workpiece, the visual position data of the end marker point, and the visual position data of the background fixed anchor point before and after the execution of the bidirectional micro-probing action control data, and to confirm the final deviation source control data. The decision module is used to drive the control decision finite state machine to perform hierarchical state switching with hysteresis conditions between multiple control states based on the final deviation source control data and its continuous holding period, and output the motion control parameters corresponding to the switching state according to the preset control period.

Images