Distributed two-arm cooperative end position tracking method under information delay

Abstract

The invention discloses a distributed double-arm cooperative tail end position tracking method under information delay, and the method comprises the steps: employing an observation fusion structure based on double-layer Kalman filtering, and respectively building two independent Kalman filters for processing high-frequency joint data and low-frequency visual observation; and the jump caused by time misalignment is relieved through linear interpolation, when a certain type of observation is unreliable, the system automatically adjusts the confidence coefficient weight, and finally the fused end position estimation is output. Compared with the prior art, the adaptive capacity of the system in the face of information delay is further improved, high-precision end position estimation is achieved under the conditions of information delay and uncertain observation data, and the method can be deployed in task scenes with high requirements for response frequency, such as industrial sites.

Description

Technical Field

[0001] This invention relates to the field of robotic arm motion planning algorithms, and specifically to a distributed dual-arm collaborative end-effector position tracking method under information delay. Background Technology

[0002] In collaborative dual-arm operations, spatial position perception of the robotic arm's end effector is crucial for achieving high-precision control and stable collaboration. However, in real-world deployment environments, the inherent processing latency and communication uncertainties between the local vision system and remote sensing channels make it difficult for traditional state estimation methods based on a unified time axis to maintain good consistency and real-time performance. Existing research indicates that vision-guided end-effector position estimation typically requires multiple image processing stages, including feature extraction, matching, and pose optimization. This serial processing mechanism not only increases the overall perception latency but also makes visual observations susceptible to non-ideal environmental factors such as occlusion and lighting changes, leading to missing observations or state jumps. These studies demonstrate that the visual perception module, due to its complex processing flow, struggles to meet the demands of high real-time tasks. On the other hand, while the joint angle data transmitted by the remote robotic arm via network has a high update frequency, its integrity is still constrained by the quality of the communication link. Existing work indicates that most current state estimation methods are designed based on the assumption of synchronous sensor updates. However, when faced with issues such as sampling frequency differences, communication jitter, or packet loss, these methods often struggle to maintain estimation consistency. Some studies have attempted to introduce Bernoulli processes to model packet loss behavior and combine them with equivalent observation transformations to improve estimation continuity. These results further illustrate that, under conditions of high-speed motion or asynchronous multi-source data, traditional filtering frameworks are no longer adequate for the state estimation requirements of complex cooperative tasks. Other studies have taken a system-level approach, exploring how to model communication delays, bandwidth limitations, and packet loss characteristics in simulation platforms to evaluate the performance of multi-robot systems under non-ideal network conditions. These studies demonstrate that information delay is not only a key variable in theoretical research but also a real-world factor that must be considered in engineering practice. Especially in dual-arm systems, when there is significant time misalignment between visual and joint signals, the state estimation may still be biased or unstable without modeling for asynchronicity and uncertainty. In summary, the observation data of both the visual perception module and the remote joint channel are inevitably affected by information delay. The former is limited by the image processing flow, while the latter is constrained by communication quality; both constitute a key reality that must be addressed in the design of end-effector tracking algorithms in dual-arm systems. Summary of the Invention

[0003] Purpose of the invention: In order to solve the problems of misalignment of update frequencies of multiple source sensors and communication delay in dual-arm systems, we propose a distributed dual-arm cooperative end-effector position tracking method under information delay.

[0004] In this invention, a two-layer nested Kalman filter algorithm is established in a dual-arm cooperative environment. Depending on the length of the time window, one algorithm processes high-frequency remote joint data, while the other processes low-frequency local visual observations. At the end of each short time window, observation alignment is performed using linear interpolation. Considering the uncertainties in image processing and network transmission, the confidence levels of the two types of sensors are dynamically adjusted based on sensor data anomalies. Finally, the fused end-effector position estimate is output. A distributed dual-arm cooperative end-effector position tracking method under information delay specifically includes the following steps:

[0005] Step 1: Select two period time window lengths, the shorter period time window length... The sampling frequency should be consistent with that of the joint data to ensure that each time window contains at least one joint position data point, and the length of the long-period time window should be consistent. The sampling frequency of the camera and the calculation time of the target recognition algorithm should be taken into account to ensure that the data from each exposure can be calculated within a long period.

[0006] Step 2: Within each discrete time step, the Kalman filter maintains two time axes (shorter time step). and long-term step size ), and predict the state at two future time points, for any observed source , No. The starting point of each time step is The destination is In the Within the first time step, the outer Kalman filter predicts the first... and The state at time ; if at the ___ If no observation data arrives within a certain time step, then when the system time arrives... At time 1, the model-based prediction is used directly as the estimate; if at time 2... If observation data arrives within a time step, then it is necessary to... arrive Between observation updates The estimated values ​​of the beginning and end states of each time window.

[0007] Step 3, when the first At the end of the first time window, the outer Kalman filter... The prediction of the first and last states of the first time window is immediately locked, and the system time enters the first... After the first time window, the outer Kalman filter still needs to be adjusted for the second time window. The delayed observation data arriving within the current time window are processed to update the state at the end of the current time window, i.e., the [number]th [time window]. and Predicting the state at any given time.

[0008] Step 4, proceed to the next step During the first time window, the inner Kalman filter algorithm with varying periods obtains the locked outer Kalman filter pair. The prediction of the beginning and end states of the first time window is performed, accepting only data from observation sources of the corresponding frequency within the time window, and combining this with an independent "direct estimation" module for the first time window. The time window is adjusted accordingly.

[0009] Step 5: In the dynamic update module, when the short-period data window ends, find the long-period time window to which the current time belongs. Regardless of whether the observation at the end of the window has been corrected, use the beginning and end states of the time window as a reference to generate a reasonable state estimate of the long-term sensor through linear interpolation.

[0010] Step 6: Based on the changing trend of the quantization error covariance matrix, the filter... In the time window confidence level The weights of each sensor channel in the fusion process are dynamically adjusted, and long and short period data are fused according to the weights to finally obtain the end position information of the two arms.

[0011] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the aforementioned distributed dual-arm cooperative end-effector position tracking method under information delay.

[0012] A computer-readable storage medium storing computer instructions that, when executed by a processor, implement the aforementioned distributed dual-arm cooperative end-effector position tracking method under information delay.

[0013] Compared with the prior art, the advantages of the present invention are as follows:

[0014] To address the challenge of spatiotemporal registration of heterogeneous sensor observations in distributed scenarios due to information latency, a dual-layer Kalman filter-based observation fusion algorithm is proposed. This algorithm establishes two nested Kalman filter layers and designs time windows of varying lengths for sensors of different frequencies. One window handles high-frequency remote joint data, while the other handles low-frequency local visual observations. At the end of each short time window, observation alignment is performed using linear interpolation, outputting a fused end-effector position estimate. Considering the uncertainties in image processing and network transmission, the confidence levels of the two types of sensors are dynamically adjusted based on sensor data anomalies to enhance the system's adaptability to information latency.

[0015] In the simulation experiment, the anomalies of visual data and network latency were simulated through parameter settings: data with a time range of 4s to 9s were selected for target position perception research. The processing latency of visual data was set between 20ms and 80ms, the probability of target loss was set at 20%, and the maximum time of target loss was 0.5s. The maximum latency of joint data was set at 50ms. For joint data, the packet loss probability due to low signal strength was set at 1%, the packet loss probability due to channel interference was set at 5%, and the packet loss probability due to channel contention was set at 10%. The simulation results clearly show that, under the condition of visual observation anomalies, the joint information data of other robotic arms in the collaborative scenario can serve as a supplement during the target loss process. As can be seen from Table 1, under good network conditions, transmitting simple joint information can largely alleviate the pressure of visual data anomalies and make up for the target perception during the insufficient time of visual observation. Figure 12 Taking position tracking along the X-axis as an example, this paper analyzes the comparison of fusion results provided by weighted fusion, Kalman filter, and cooperative end-effector tracking framework. In the figure, Supervisor represents the actual position data obtained by the monitor. Due to data asynchrony and based on the principle of using visual information as the primary criterion, the position observations acquired from joint data are linearly interpolated to obtain the time point of visual data acquisition. WeightedAverage represents the result of weighted fusion, while KalmanFilter is the result of Kalman filtering using the weighted fusion observations under the velocity model. The figure shows that when visual data anomalies are severe, the perception of target position based on the above methods is significantly affected by the quality of visual information. However, using this cooperative end-effector tracking method, the period of the position tracking result remains at 30ms, effectively reducing the requirement for data density.

[0016] Then, through simulation experiments, the fusion error characteristics with and without the confidence reduction mechanism are compared. Key results are as follows: Figure 13 As shown. The experimental setup for comparison is completely identical: First, regarding the consistency of baseline performance, when When the coefficient of error is 1.0, the errors of both mechanisms are 9.69 mm, verifying the consistency of experimental conditions; secondly, regarding the error growth pattern, there is no confidence decrease mechanism: the error increases with... The decrease shows a quasi-linear growth, with a slope =6.2; Includes confidence decrease mechanism: in A nonlinear jump occurs when the confidence level is less than 0.7, and the slope of the confidence decrease region is [not specified]. =32.1, which is 5.2 times that of the non-confidence decline region; from the perspective of the impact of the confidence decline mechanism on the error, when At a confidence level of 0.6, the confidence descent mechanism reduced the fusion error from 30.27 mm to 10.07 mm, a decrease of 66.7%. Increasing the confidence descent also significantly improves system stability. Without the confidence descent mechanism... When the confidence level is 0.2, the error reaches 14.37 mm, which is still within the safe zone; the confidence level decrease mechanism is... When the confidence level is 0.2, the error is 45.43 mm, triggering the system degradation mode. The confidence reduction mechanism makes dangerous operating conditions explicit, avoiding the accumulation of implicit errors. This simulation verifies the effectiveness of setting the confidence reduction scheme. The scheme of setting confidence to amplify observation noise has two effects. First, when a single sensor does not observe valid data, the fusion observation error can be reduced by amplifying the observation noise. Second, when the confidence levels of two sensors decrease simultaneously, the trace of the error covariance matrix can be significantly amplified. By setting a reasonable safety threshold judgment mechanism, abnormal observation states can be detected early, thereby detecting end-effector tracking anomalies in robotic arm collaboration early.

[0017] In summary, this method provides a high-precision and highly adaptable solution. By introducing a prediction compensation mechanism, time window modeling, linear interpolation, and confidence adjustment, the system can maintain the continuity of end-point tracking even without a unified time reference. This achievement not only expands the application boundaries of Kalman filtering in asynchronous sensing environments but also provides an important perception fusion paradigm for subsequent research on end-point tracking in distributed systems. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the real-time end-point tracking algorithm of the present invention;

[0019] Figure 2 This is a schematic diagram of the recursive locking mechanism and prediction correction within the window in the method of the present invention;

[0020] Figure 3 This is a schematic diagram of the interpolation fusion strategy based on dual-period collaboration in the method of the present invention;

[0021] Figure 4 This is a schematic diagram of the overall framework of the observation fusion algorithm based on dynamic confidence in this invention.

[0022] Figure 5 The figures show the simulation results of position observation and estimation and velocity estimation using the method of this invention.

[0023] Figure 6 This is a simulation result of the robotic arm end-effector tracking effect under information delay and observation fusion using the method of this invention;

[0024] Figure 7 This is the time-lead distribution characteristic simulated by the method of this invention;

[0025] Figure 8 This is the tracking effect (X-axis) of the dual-layer Kalman filter observation fusion framework under extreme conditions simulated by the method of this invention.

[0026] Figure 9 This is the tracking effect (Y-axis) of the dual-layer Kalman filter observation fusion framework under extreme conditions simulated by the method of this invention.

[0027] Figure 10 This is the tracking effect (Z-axis) of the dual-layer Kalman filter observation fusion framework under extreme conditions simulated by the method of this invention.

[0028] Figure 11 This is a comparison diagram of visual observation anomalies simulated by the method of the present invention (the yellow part represents visual observation anomalies);

[0029] Figure 12 This is a comparison of the tracking performance of the algorithm simulated by the method of this invention with that of the basic method;

[0030] Figure 13 This is the surface of the fusion error standard deviation as a function of confidence level, simulated by the method of this invention. Detailed Implementation

[0031] The following description, with reference to the specification, illustrates a preferred embodiment of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0032] This invention provides a distributed dual-arm collaborative end-effector position tracking method under information delay. This invention applies the method to a dual-arm collaborative scenario. First, a two-layer nested Kalman filter algorithm is established. Depending on the length of the time window, one layer processes high-frequency remote joint data, and the other processes low-frequency local visual observations. Then, a fast approximation method using linear interpolation is employed to align observations at the end of each short time window. Finally, considering the uncertainties in image processing and network transmission, the confidence levels of the two types of sensors are dynamically adjusted based on sensor data anomalies. The final output is a fused end-effector position estimate. The specific steps are as follows:

[0033] Step 1: Select two period time window lengths, the shorter period time window length The sampling frequency should be consistent with that of the joint data to ensure that each time window contains at least one joint position data point, and the length of the long-period time window should be consistent. The sampling frequency of the camera and the calculation time of the target recognition algorithm should be taken into account to ensure that the data from each exposure can be calculated within a long period.

[0034] Step 2: Within each discrete time step, the Kalman filter maintains two time axes (shorter time step). and long-term step size (and predict the state at two future time points. For any observation source) , No. The starting point of each time step is The destination is In the first Within the first time step, the outer Kalman filter predicts the first... and The state at time 1. If at the 1st... If no observation data arrives within a certain time step, then when the system time arrives... At time 1, the model-based prediction is used directly as the estimate; if at time 2... If observation data arrives within a time step, then it is necessary to... arrive Between observation updates The estimated values ​​of the first and last states of each time window. The specific implementation process of step 2 is as follows:

[0035] Step 2-1: If the observed data has not yet arrived, directly predict the first [item / phase] based on the model. and The state at any given moment:

[0036]

[0037]

[0038] in, yes Posterior estimation at time, Based on Direct prediction by the time-of-time model Time-prior estimation, Based on Direct prediction by the time-of-time model Prior time estimates, the state transition matrix is ​​as follows

[0039]

[0040] in,

[0041]

[0042] Process noise matrix:

[0043]

[0044] in,

[0045]

[0046] in The variance of the unmodeled acceleration is represented by the following state prediction formula:

[0047]

[0048]

[0049] in, for The posterior covariance at time 1, Based on Direct prediction by the time-of-time model Prior covariance at time

[0050] If the next time window has not yet been entered, directly provide the state estimate of the end time stamp of the next time window.

[0051]

[0052] Step 2-2: If a time window is detected When there is valid observation data in memory, first process each observation... To predict its occurrence: calculate its time of occurrence. Relative to the starting point of the current time window Time difference

[0053]

[0054] in, Indicates the source of observation The The time difference between each observation data; To generate timestamps for the actual data, This is the time step index of the current filter. For filter The length of the time window. This bias quantifies the time of observation relative to the start of the current time window. The offset.

[0055]

[0056]

[0057] in, and Estimate the state at the start of the current time window respectively Covariance Through the state transition matrix Forward propagation to the observation time The result. This is the process noise covariance matrix. Further calculations can be performed for the observed source. No. Kalman gain matrix of each observation

[0058]

[0059] in, For the observation matrix, extract the position components from the full state vector; To observe the noise covariance matrix, when its trace This indicates that short-period observation sources have higher accuracy.

[0060] = +

[0061] The posterior covariance updated after a single observation should be expressed as:

[0062]

[0063] in, This represents the time-aligned prediction covariance; This represents the Kalman gain matrix of the observation; This represents the observation matrix corresponding to the observation source.

[0064] State update volume Driven by observation residuals, residual terms This reflects the deviation between the predicted and measured positions. The updated covariance matrix. Semidefiniteness is guaranteed by Schmidt orthogonalization. When multiple frames of valid observation data appear within a window, the modified covariance propagation formula should satisfy the information superposition principle when fusing multi-source observation data. The prediction covariance matrix for the next time step is defined as follows:

[0065]

[0066] in, express The prior error covariance matrix at time t; Indicates the source of observation No. The fusion weights of individual observations; Represents the state transition matrix function; express Time window length of the filter type; This indicates the deviation between the time of the observation and the start of the window; Indicates observation Updated posterior covariance; This represents the noise covariance for the entire window process. Dynamic weights need to comprehensively consider both observation timeliness and information reliability.

[0067]

[0068] in, This represents the normalized fusion weights, with a range of . ; This indicates the timeliness of the observation; a larger value means the observation is closer to the end of the window. The trace represents the information content of the covariance matrix; a larger trace indicates a more reliable estimate. express The effective number of observations for each observation source; the denominator iterates through all observation sources. and its observation sequence number For all The validity of each observation is summed to achieve normalization, ensuring... This design enables an adaptive balance between high-frequency but noisy short-period observations and low-frequency but low-noise long-period observations. Before the current window ends, a prediction of the start time of the next time window needs to be given based on all observations within the current window.

[0069]

[0070] in, Indicates passing through the first Effective observations within a time window State prediction at the start time of each window. In the recursion of the next window... That is, the state estimate of the starting point of the next window, denoted as .

[0071] Similarly, at the end of each window, all valid predictions within that window need to be merged, and the prediction for the end of the next time window needs to be calculated using the model. For long-term predictions, the recommended approach is segmented prediction based on observation point corrections, which is the process described above. Change to Calculate the predicted value at the end of the next window.

[0072]

[0073] in, The weight calculation in the middle also needs to be Change to The computational load of this scheme is equivalent to only calculating Twice that of the previous window. If the short period is set relatively short, theoretically there will be fewer effective observations within the window, and the above method is generally feasible for prediction. However, the computational capability for real-time processing of observation data needs to be evaluated. If the short period is set very short, making it unsuitable for real-time updates of the predicted values ​​at two different times, then at the end of the current window, based on the estimated value at the end of the current window, the state at the end of the next window can be directly predicted by the model.

[0074]

[0075] in, Based on the latest update status Two-step prediction, time span Ensure that the predicted values ​​align with the end of the next time window. This step provides a forward-looking state estimate for the system, ensuring the smooth implementation of the system's estimation-then-correction scheme, and advancing the calculation process to guarantee the system's real-time performance.

[0076] Step 3: When the first At the end of the first time window, the outer Kalman filter... The prediction of the first and last states of the first time window is immediately locked, and the system time enters the first... After the first time window, the outer Kalman filter still needs to be adjusted for the second time window. The delayed observation data arriving within the current time window are processed to update the state at the end of the current time window, i.e., the [number]th [time window]. and The specific implementation process of step 3 for state prediction at time point is as follows:

[0077] Step 3-1: For each newly arrived observation data, the observation data first needs to be time-aligned, and its timestamp... This indicates the actual time the data was generated. To align this observation data with the state prediction for the current time window, its time relative to the start of the time window is first calculated. The time difference, and then, based on the time difference, the state transition equation. Calculate the state prediction corresponding to the observation data The state transition matrix

[0078]

[0079] in, For discrete time steps,

[0080] Step 3-3: Linearize the propagation equation based on the Jacobian matrix. Update error covariance matrix ,in, The process noise covariance is defined as follows:

[0081]

[0082] in, This represents the variance of the unmodeled acceleration.

[0083] Steps 3-4: Calculate the Kalman gain equation Update Kalman gain Among them, the observation matrix With the observed covariance matrix Defined as follows

[0084]

[0085] Steps 3-5: Correct the equations based on the state estimation Update state estimation ,in, for Time-time observation vector.

[0086] Steps 3-6: Update the equation based on covariance Update error covariance matrix .

[0087] Steps 3-7: Within the current time window, all newly arriving observation data will be compared to the end of the time window, i.e. The state prediction at each time step has an impact, based on the updated state. According to the formula Recalculate the first Predicting the state at any given time.

[0088] Steps 3-8: According to the formula Synchronous update of error covariance matrix .

[0089] Steps 3-9: If in the... If multiple sets of observation data arrive within a given time window, a weighted fusion method is used to integrate the influence of observation data from all corresponding sources to obtain the result. Final state estimation at time:

[0090]

[0091] Among them, weight Based on the inverse calculation of the error covariance matrix:

[0092]

[0093] The final error covariance matrix is

[0094]

[0095] Step 4: Proceed to the next step During the first time window, the inner Kalman filter algorithm with varying periods obtains the locked outer Kalman filter pair. The prediction of the beginning and end states of the first time window is performed, accepting only data from observation sources of the corresponding frequency within the time window, and combining this with an independent "direct estimation" module for the first time window. The correction is performed within a time window. The specific implementation process of step 4 is as follows:

[0096] Step 4-1: Assume the observed values If it is a location measurement value, then according to the formula... Calculate the direct estimate .in, These are location measurements. This is the velocity value of the Kalman filter. From the observation timestamp The time window up to the end of the current time window.

[0097] Step 4-2: According to the formula Directly estimate the error covariance matrix .in, It is the observation noise covariance matrix. It is the error covariance matrix for velocity prediction.

[0098] Step 4-3: As Figure 3 As shown, according to the formula The direct estimates are weighted and fused with the predictions from the Kalman filter, where the weights are... Error covariance matrix based on direct estimation The error covariance matrix of the Kalman filter Perform the inverse operation:

[0099]

[0100] Step 5: In the dynamic update module, when the short-period data window ends, the long-period time window to which the current time belongs is found. Regardless of whether the observations at the end of the window have been corrected, a reasonable state estimate of the long-term sensor is generated using linear interpolation based on the beginning and end states of that time window. The specific implementation process of Step 5 is as follows:

[0101] Step 5-1: Immediately at the end of the short-period time window, obtain the position information of the beginning and end of the nearest time window of the long-period Kalman filter.

[0102] Step 5-2: Assume the state prediction time of the long-period Kalman filter is... and The short-cycle time point is According to the formula Calculate interpolation coefficients Among them, molecules The denominator represents the time shift of a short-period time point relative to the starting point of a long-period time point. It is a long-cycle time step.

[0103] Step 5-3: Based on interpolation coefficients According to the formula Interpolate the state prediction of the long-period Kalman filter to a short-period time point. .in, It is the state prediction of the long-period Kalman filter at the current time step. This is the latest correction for the long-period Kalman filter at the end of the current time step. If there are no valid observations at this time step, then... For long-period Kalman filters, state prediction for future time steps .

[0104] Step 5-4: Based on interpolation coefficients Without considering in-window corrections, it can also be done using the interpolation formula. Calculate the error covariance matrix. Where, It is the error covariance matrix of the long-period Kalman filter at the current time step. It is the error covariance matrix of the long-period Kalman filter for future time steps.

[0105] Step 5-5: After interpolation, the short-period and long-period state predictions can be weighted and fused to generate the final state estimate. Weights and The calculation formula is

[0106]

[0107] Step 6: Based on the changing trend of the quantization error covariance matrix, and using the filter... In the time window confidence level The weights of each sensor channel in the fusion process are dynamically adjusted, and long and short period data are fused according to the weights to finally obtain the end-effector position information of the two arms. The specific implementation process of step 6 is as follows:

[0108] Step 6-1: Calculate the filter In the time window confidence level Its update rules are as follows

[0109]

[0110] in, This represents the confidence decay factor, which controls the decay rate when there are no observations. This indicates the lower confidence level, preventing excessive decay.

[0111] Step 6-2: When confidence decreases, the object of adjustment is the observation noise covariance. rather than process noise covariance or state covariance The observed noise covariance directly reflects the reliability of the sensor, and adjustment... It has a clear physical meaning; through Adjustment can achieve adaptive adjustment of the fusion weights, avoiding direct manipulation. This introduces numerical stability issues; it is compatible with the standard Kalman filter framework and requires no modification to the state propagation equation in the prediction stage; therefore, when the confidence level decreases, the reduced estimation reliability is reflected by adjusting the observation noise covariance.

[0112]

[0113] in, This represents the amplification process of the observation noise covariance matrix within the window, and the adjusted observation noise covariance adjustment coefficient. It is negatively correlated with confidence level, enabling controlled amplification of noise level.

[0114] Step 6-3: When any filter experiences a decrease in confidence, the final fused location estimate should be obtained based on the confidence level and the observation matrix. The fusion weights should be adjusted according to whether the corresponding outer Kalman filter receives valid observation data.

[0115]

[0116] in, Represents the normalized fusion weights, satisfying ; The item reduces the weight contribution of the low-confidence filter; a reset is performed when valid observations are regained. ,in This indicates the filter The initial observation covariance matrix.

[0117] To verify the effectiveness and fault-tolerant performance of the distributed dual-manipulator collaborative end-effector tracking method described in this invention under complex working conditions, a verification experimental environment was constructed based on software simulation. The experiment used local socket communication to simulate a distributed real-time system structure, and introduced visual observation anomalies, communication delays, and data packet loss through software methods to simulate the complex operating environment that may occur during actual manipulator collaborative operations. The simulation experimental platform used MATLAB 2022a, and the hardware configuration was consistent with the aforementioned embodiments.

[0118] The experimental data comes from simulation results of the cooperative motion of two robotic arms, specifically including the target's trajectory in space, the joint positions of the robotic arms, and joint velocity information. All data undergoes unified preprocessing before entering the system, including timestamp annotation, random noise superposition, and data encapsulation, and is sent in chronological order to ensure that time alignment and prediction calculations can still be performed on multi-source information under asynchronous conditions.

[0119] During the simulation, a portion of the visual observation data was randomly removed to simulate visual observation anomalies caused by target occlusion, sudden changes in illumination, motion blur, and complex environments. These anomalies result in the loss of visual information within a continuous time window, causing a significant uneven distribution of visual data along the time axis, such as… Figure 11 As shown in Table 1, different levels of communication delay were introduced into the robotic arm joint data, and data packets were randomly dropped according to a set probability to simulate the effects of factors such as network congestion, channel interference, and communication competition. The packet loss events of different durations and their corresponding probabilities are shown in Table 1.

[0120] Table 1. Time distribution and corresponding probability of persistent packet loss

[0121]

[0122] Under conditions where information processing delay exists, the cooperative end-point tracking method of this invention performs joint estimation of the target's position, velocity, and acceleration state based on a two-layer Kalman filter structure. The position observation and state estimation results are as follows: Figure 5 As shown in the figure. Based on this, the system performs forward prediction of the target's current position within a fixed prediction time window and outputs a usable target position prediction result before the visual computation is completed. The target position prediction effect under the information delay condition is shown in the figure. Figure 6 As shown, the prediction results can be generated before the visual computation is completed, and are used to estimate the current target position.

[0123] To evaluate the effectiveness of this prediction mechanism in the temporal dimension, a statistical analysis was performed on the relationship between prediction time and visual computation completion time. The distribution characteristics of prediction lead time and its cumulative distribution function are as follows: Figure 7As shown, the results indicate that most predicted events have positive lead time, demonstrating that the method of the present invention can effectively mitigate the impact of visual processing delay on the real-time performance of the system.

[0124] The robustness of the proposed collaborative end-point tracking method was further verified under extreme conditions of severe visual observation anomalies and overlapping communication packet loss. Under these extreme conditions, the system obtains only a small amount of effective visual observation data, but it can still continuously estimate the target position based on historical motion states. Its tracking performance in the X, Y, and Z axes is shown below. Figure 8 , Figure 9 and Figure 10 As shown. Combined with Figure 11 It can be seen that even when visual observations are missing within a continuous time window, the system can still output smooth and continuous target position estimation results.

[0125] Further analysis reveals that when the target's movement speed is high or its direction of movement changes, continued reliance on visual extrapolation results will lead to a gradual accumulation of estimation errors. To address this, the method of this invention automatically reduces the confidence level of the visual prediction results as the extrapolation time window increases, causing the fusion result to gradually converge towards the joint prediction result. This characteristic... Figure 9 This is particularly evident in the magnified local results, which aligns with the design expectations of collaborative end-point tracking.

[0126] In the comparative experiment, the method of this invention was compared and analyzed with the weighted average fusion method and the traditional Kalman filter method. Its target position tracking performance in the X-axis direction is as follows: Figure 12 As shown in the figure. The results show that when visual data anomalies are severe, traditional methods are highly sensitive to the quality of visual information, and the tracking error increases significantly; while the method of this invention can maintain a stable output cycle under low data density conditions, thereby significantly reducing the dependence on the continuity of visual information.

[0127] Furthermore, the stability of multi-source information fusion was verified through numerical simulation by introducing a confidence-based observation fusion mechanism. The results of fusion error as a function of observation confidence are shown below. Figure 13 As shown in Tables 2 and 3, when the observation quality of a single sensor deteriorates or fails, the influence of abnormal observations on the fusion result can be effectively suppressed by amplifying the corresponding observation noise covariance to participate in the fusion calculation. When multiple sensors are simultaneously in a low-confidence state, the fusion error covariance increases significantly, enabling the system to promptly identify abnormal operating conditions and enter a safe state, thereby avoiding the accumulation of implicit errors.

[0128] Table 2 Comparison of theoretical fusion errors in single sensor failure scenarios (unit: mm)

[0129]

[0130] Table 3 Comparison of Fusion Errors under Key Operating Conditions (Unit: mm)

[0131]

[0132] The simulation experiments described above verify that the distributed dual-manipulator collaborative end-effector tracking method under information delay conditions described in this invention can still stably output timely target position estimation results under complex conditions such as visual anomalies, communication delays, and data packet loss, demonstrating good robustness and engineering application value.

Claims

1. A distributed dual-arm cooperative end-effector position tracking method under information delay, characterized in that, The method is as follows: a two-layer nested Kalman filter algorithm is established in the dual-arm writing environment. Depending on the length of the time window, one algorithm processes high-frequency remote joint data and the other processes low-frequency local visual observations. At the end of each short time window, the observations are aligned by linear interpolation. Considering the uncertainty of image processing and network transmission, the confidence of the two types of sensors is dynamically adjusted by combining the data anomalies of the sensors. Finally, the fused end position estimate is output.

2. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 1, characterized in that, A robotic arm end-effector tracking algorithm based on asynchronous data fusion of multi-sensor data using Kalman filtering is proposed. The problem of information loss in the collaborative operation of the robotic arm is identified, and a confidence-based sensor data fusion algorithm is proposed to effectively ensure the accuracy of end-effector position estimation even when local visual information is severely abnormal.

3. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 2, characterized in that, Specifically, the following steps are included: Step 1: Select two period time window lengths, the shorter period time window length... The sampling frequency should be consistent with that of the joint data to ensure that each time window contains at least one joint position data point, and the length of the long-period time window should be consistent. The camera's sampling frequency and the target recognition algorithm's computation time should be considered comprehensively to ensure that the data from each exposure can be processed within a long time period. Step 2: Within each discrete time step, the Kalman filter maintains two time axes (shorter time step). and long-term step length ), and predict the state at two future time points, for any observed source , No. The starting point of each time step is The destination is In the Within the first time step, the outer Kalman filter predicts the first... and The state at time ; if at the ___ If no observation data arrives within a certain time step, then when the system time arrives... At time 1, the model-based prediction is used directly as the estimate; if at time 2... If observation data arrives within a time step, then it is necessary to... arrive Between observation updates The estimated values ​​of the beginning and end states of each time window. Step 3, when the first At the end of the first time window, the outer Kalman filter... The prediction of the first and last states of the first time window is immediately locked, and the system time enters the first... After the first time window, the outer Kalman filter still needs to be adjusted for the second time window. The delayed observation data arriving within the current time window are processed to update the state at the end of the current time window, i.e., the [number]th [time window]. and Predicting the state at any given time. Step 4, proceed to the next step During the first time window, the inner Kalman filter algorithm with varying periods obtains the locked outer Kalman filter pair. The prediction of the beginning and end states of the first time window is performed, accepting only data from observation sources of the corresponding frequency within the time window, and combining this with an independent "direct estimation" module for the first time window. Adjustments are made within a time window. Step 5: In the dynamic update module, when the short-period data window ends, the long-period time window to which the current time belongs is found. Regardless of whether the observations at the end of the window have been corrected, a reasonable state estimate of the long-term sensor is generated using linear interpolation based on the state at the beginning and end of that time window. Step 6: Based on the changing trend of the quantization error covariance matrix, the filter... In the time window confidence level The weights of each sensor channel in the fusion process are dynamically adjusted, and long and short period data are fused according to the weights to finally obtain the end position information of the two arms.

4. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 3, characterized in that, The specific implementation process of step 2 is as follows: Step 2-1: If the observed data has not yet arrived, directly predict the first [item / phase] based on the model. and The state at any given moment: in, yes Posterior estimation at time, Based on Direct prediction by the time-of-time model Time-prior estimation, Based on Direct prediction by the time-of-time model Prior time estimates, the state transition matrix is ​​as follows in, Process noise matrix: in, in The variance of the unmodeled acceleration is represented by the following state prediction formula: in, for The posterior covariance at time 1, For based on Direct prediction by the time-of-time model Prior covariance at time If the next time window has not yet been entered, directly provide the state estimate of the end time stamp of the next time window. Step 2-2: If a time window is detected When there is valid observation data in memory, first process each observation... To predict its occurrence: calculate its time of occurrence. Relative to the starting point of the current time window Time difference, in, Indicates the source of observation The The time difference between each observation data; To generate timestamps for the actual data, This is the time step index of the current filter. For filter The length of the time window, this bias quantifies the time of observation relative to the start of the current time window. The offset, in, and Estimate the state at the start of the current time window respectively Covariance Through the state transition matrix Forward propagation to the observation time As a result, The process noise covariance matrix is ​​calculated for the observed source. No. Kalman gain matrix of each observation in, For the observation matrix, extract the position components from the full state vector; To observe the noise covariance matrix, when its trace This indicates that short-period observation sources have higher accuracy. = + The posterior covariance updated after a single observation should be expressed as: in, This represents the time-aligned prediction covariance; This represents the Kalman gain matrix of the observation; This represents the observation matrix corresponding to the observation source. State update volume Driven by observation residuals, residual terms The updated covariance matrix reflects the deviation between predicted and measured positions. By ensuring semi-positive definiteness through Schmidt orthogonalization, and considering multiple frames of valid observation data within a window, the modified covariance propagation formula should satisfy the information superposition principle when fusing multi-source observation data. The prediction covariance matrix for the next time step is defined as follows: in, express The prior error covariance matrix at time t; Indicates the source of observation No. The fusion weights of individual observations; Represents the state transition matrix function; express Time window length of the filter type; This indicates the deviation between the time of the observation and the start of the window; Indicates observation Updated posterior covariance; This represents the noise covariance of the entire window process. Dynamic weights need to comprehensively consider both observation timeliness and information reliability. in, This represents the normalized fusion weights, with a range of . ; This indicates the timeliness of the observation; a larger value means the observation is closer to the end of the window. The trace represents the information content of the covariance matrix; a larger trace indicates a more reliable estimate. express The effective number of observations for each observation source; the denominator iterates through all observation sources. and its observation sequence number For all The validity of each observation is summed to achieve normalization, ensuring... This design enables an adaptive balance between high-frequency but noisy short-period observations and low-frequency but low-noise long-period observations. Before the current window ends, a prediction of the start time of the next time window needs to be given based on all the observations within the current window. in, Indicates passing through the first Effective observations within a time window The state prediction at the starting time of each window is used in the recursion of the next window. That is, the state estimate of the starting point of the next window, denoted as , At the end of each window, all valid predictions within that window need to be merged, and the prediction for the end of the next time window needs to be calculated using the model. For long-term predictions, the recommended approach is segmented prediction based on observation point corrections, which is the process described above. Change to Calculate the predicted value at the end of the next window. in, The weight calculation in the middle also needs to be Change to The computational load of this scheme is equivalent to only calculating If the short period is set too short, theoretically there will be less effective observation data within the window, requiring an assessment of the computational capabilities for real-time processing of observation data. If the short period is set too short to be suitable for real-time updates of the predicted values ​​at two different times, then at the end of the current window, the state at the end of the next window can be predicted directly using the model based on the estimated value at the end of the current window. in, Based on the latest update status Two-step prediction, time span Ensuring that the predicted values ​​align with the end of the next time window provides a forward-looking state estimate for the system, ensuring the smooth implementation of the system's estimation-then-correction scheme and advancing the calculation process to guarantee the system's real-time performance.

5. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 4, characterized in that, The specific implementation process of step 3 is as follows: Step 3-1: For each newly arrived observation data, the observation data first needs to be time-aligned, and its timestamp... This indicates the actual time the data was generated. To align this observation data with the state prediction for the current time window, its time relative to the start of the time window is first calculated. The time difference, and then, based on the time difference, the state transition equation. Calculate the state prediction corresponding to the observation data The state transition matrix in, For discrete time steps, Step 3-3: Linearize the propagation equation based on the Jacobian matrix. Update error covariance matrix ,in, The process noise covariance is defined as follows: in, This represents the variance of the unmodeled acceleration. Steps 3-4: Calculate the Kalman gain equation Update Kalman gain Among them, the observation matrix With the observed covariance matrix Defined as follows Steps 3-5: Correct the equations based on the state estimation Update state estimation ,in, for Observation vector at time, Steps 3-6: Update the equation based on covariance Update error covariance matrix , Steps 3-7: Within the current time window, all newly arriving observation data will be compared to the end of the time window, i.e. The state prediction at each time step has an impact, based on the updated state. According to the formula Recalculate the first Predicting the state at any given time. Steps 3-8: According to the formula Synchronous update of error covariance matrix , Steps 3-9: If in the... If multiple sets of observation data arrive within a given time window, a weighted fusion method is used to integrate the influence of observation data from all corresponding sources to obtain the result. Final state estimation at time: Among them, weight Based on the inverse calculation of the error covariance matrix: The final error covariance matrix is 。 6. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 5, characterized in that, The specific implementation process of step 4 is as follows: Step 4-1: Assume the observed values If it is a location measurement value, then according to the formula... Calculate the direct estimate ,in, These are location measurements. This is the velocity value of the Kalman filter. From the observation timestamp The time window up to the end of the current time window, Step 4-2: According to the formula Directly estimate the error covariance matrix ,in, It is the observation noise covariance matrix. It is the error covariance matrix of velocity prediction. Step 4-3: According to the formula The direct estimates are weighted and fused with the predictions from the Kalman filter, where the weights are... Error covariance matrix based on direct estimation The error covariance matrix of the Kalman filter Perform the inverse operation: 。 7. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 6, characterized in that, The specific implementation process of step 5 is as follows: Step 5-1: Immediately at the end of the short-period time window, acquire the position information of the beginning and end of the nearest time window of the long-period Kalman filter. Step 5-2: Assume the state prediction time of the long-period Kalman filter is... and The short-cycle time point is According to the formula Calculate interpolation coefficients Among them, molecules The denominator represents the time shift of a short-period time point relative to the starting point of a long-period time point. It is a long-period time step. Step 5-3: Based on interpolation coefficients According to the formula Interpolate the state prediction of the long-period Kalman filter to a short-period time point. ,in, It is the state prediction of the long-period Kalman filter at the current time step. This is the latest correction for the long-period Kalman filter at the end of the current time step. If there are no valid observations at this time step, then... For long-period Kalman filters, state prediction for future time steps , Step 5-4: Based on interpolation coefficients Without considering in-window corrections, it can also be done using the interpolation formula. Calculate the error covariance matrix, where, It is the error covariance matrix of the long-period Kalman filter at the current time step. It is the error covariance matrix of the long-period Kalman filter for future time steps. Step 5-5: After interpolation, the short-period and long-period state predictions can be weighted and fused to generate the final state estimate. and The calculation formula is 。 8. The distributed dual-arm cooperative end-effector position tracking method under information delay according to claim 7, characterized in that, The specific implementation process of step 6 is as follows: Step 6-1: Calculate the filter In the time window confidence level Its update rules are as follows in, This represents the confidence decay factor, which controls the decay rate when there are no observations. This represents the lower confidence level to prevent excessive decay. Step 6-2: When confidence decreases, the object of adjustment is the observation noise covariance. rather than process noise covariance or state covariance The observed noise covariance directly reflects the reliability of the sensor, and adjustment It has a clear physical meaning; through Adjustment can achieve adaptive adjustment of the fusion weights, avoiding direct manipulation. This introduces numerical stability issues; it is compatible with the standard Kalman filter framework and requires no modification to the state propagation equation in the prediction stage; therefore, when the confidence level decreases, the reduced estimation reliability is reflected by adjusting the observation noise covariance. in, This represents the amplification process of the observation noise covariance matrix within the window, and the adjusted observation noise covariance adjustment coefficient. Negatively correlated with confidence level, achieving controlled amplification of noise level. Step 6-3: When any filter experiences a decrease in confidence, the final fused location estimate should be obtained based on the confidence level and the observation matrix. The fusion weights should be adjusted according to whether the corresponding outer Kalman filter receives valid observation data. in, Represents the normalized fusion weights, satisfying ; The item reduces the weight contribution of the low-confidence filter; a reset is performed when valid observations are regained. ,in This indicates the filter The initial observation covariance matrix.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements a distributed dual-arm cooperative end-effector position tracking method under information delay as described in any one of claims 1 to 8.

10. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instruction is executed by the processor, it implements a distributed dual-arm cooperative end-position tracking method under information delay as described in any one of claims 1-8.

Images