Robust adaptive filtering prediction method, device and storage medium

Abstract

The present invention belongs to the field of visual servo technology, and in particular relates to a robust adaptive filtering prediction method, device and storage medium, comprising the following steps: measuring motion state information of an inertial stabilization platform; converting the rotation angle of the inertial stabilization platform into pixel coordinates in an image plane; measuring the inherent delay time Δ of a visual sensor; and t , the measurement data of the inertial sensor is converted into △t Delay processing is performed to calibrate the time consistency of the two data under the same time reference; the target state vector is defined as a six-dimensional vector, and the system state equation and measurement equation are established based on the Singer model; robust adaptive filtering processing is performed; and time lag compensation is performed. This can improve the tracking accuracy and stability of the visual servo system; through the collaborative work of the inertial measurement unit and the visual sensor, combined with the robust adaptive filtering algorithm and time lag compensation technology, high-precision prediction and tracking of non-cooperative targets can be achieved.

Description

Technical Field

[0001] The present invention belongs to the field of visual servo technology, and in particular relates to a robust adaptive filtering prediction method with time lag compensation and noise resistance, its application in a visual servo system, a computer device for executing the filtering prediction method, and a non-transient computer-readable storage medium. Background Art

[0002] A visual servo system uses visual sensors to obtain target position information and then uses this information to control mechanical motion for precise tracking. This system has widespread applications in industrial automation, robotics, and drone navigation. However, practical applications of visual servo systems face two major challenges: measurement lag and noise interference.

[0003] Measurement lag primarily stems from the image acquisition and processing process of vision sensors. Due to the large amount of image data and the complex feature extraction and target recognition algorithms required, there is a certain time delay between the actual change in the target's position and the system acquiring the corresponding measurement information. This time lag causes the system to make control decisions based on outdated information, thereby reducing tracking accuracy and even causing system instability. Noise interference is another key issue. Vision sensor measurement results are easily affected by factors such as ambient light fluctuations, image noise, and sensor accuracy limitations, resulting in inaccurate measurement data. Furthermore, the random motion characteristics of the target (such as random variations in acceleration) can also introduce process noise. This noise interference further reduces the tracking accuracy and robustness of the system.

[0004] Traditional filtering methods, such as Kalman filtering, have certain limitations when dealing with time lag and noise and require a priori values ​​of noise. Specifically, Kalman filtering assumes that both process noise and measurement noise are Gaussian white noise and that the system model is precisely known. However, in practical applications, these assumptions are often difficult to meet. In particular, when the target motion is complex or the environmental noise is large, traditional filtering methods find it difficult to maintain stable tracking performance. Summary of the Invention

[0005] In view of this, the present invention aims to provide a robust adaptive filtering prediction method that can effectively compensate for time lag and resist noise interference, so as to improve the tracking accuracy and stability of the visual servo system; through the collaborative work of the inertial measurement unit and the visual sensor, combined with the robust adaptive filtering algorithm and time lag compensation technology, high-precision prediction and tracking of non-cooperative targets can be achieved.

[0006] To achieve the above object, the technical solution created by the present invention is implemented as follows:

[0007] The present invention provides a robust adaptive filtering prediction method with time lag compensation and noise resistance, the method comprising the steps of:

[0008] S1. The inertial stabilization platform includes an inertial measurement unit, the inertial measurement unit being used to measure the motion state information of the inertial stabilization platform;

[0009] S2. The rotation angle of the inertial stabilization platform is converted into pixel coordinates in the image plane by geometric relationships;

[0010] S3. Measuring the intrinsic delay time of the visual sensor in the visual servo system △t , the measurement data of the inertial sensor is converted into △t Performing delay processing so that the inertial data and the visual data are on the same time basis, and calibrating the time consistency of the inertial data and the visual data using a timestamp alignment algorithm;

[0011] S4. Define the target state vector as a six-dimensional vector and establish the system state equation and measurement equation based on the Singer model;

[0012] S5. Perform robust adaptive filtering;

[0013] S6. According to the inherent delay time of the visual sensor △t and filter period T s , calculate the number of prediction steps N = ceil ( △t / T s );

[0014] In getting k Momentary state estimation After that, execute N Step prediction is performed to obtain the target position information after compensating the time lag;

[0015] A robust adaptive Kalman filter predictor is used to perform multi-step prediction, and the state estimation value at the current moment is advanced by the corresponding time step to compensate for the time delay in the visual servo system.

[0016] Furthermore, the motion state information includes acceleration and angular velocity;

[0017] The formula for the geometric relationship is ;

[0018] in, θ x ,θ yare the rotation angles of the inertial stabilization platform on the x-axis and y-axis of the field of view, respectively; u,v are the mobile end pixels on the x-axis and y-axis of the field of view, λ is the camera focal length.

[0019] Furthermore, the six-dimensional vector includes a target position, a target velocity, and a target acceleration; the target state vector is:

[0020] ;

[0021] in, u T ,v T Target location , is the target speed, is the target acceleration;

[0022] The system state equation is x T ( k +1)= Fx T ( k )+ w k , the measurement equation is z ( k )= Hx T ( k )+ v k ; The covariance matrix is:

[0023] ;

[0024] in, x T ( k ) is the target state vector at time k, x T ( k +1) is the target state vector at time k+1, F is the state transition matrix, w k is the process noise, is the inverse of the maneuvering time constant, σ a 2 is the maneuver acceleration variance, H is the measurement matrix, v k To measure noise, Represents the noise weight of the Singer model process after integral discretization.

[0025] Furthermore, the iterative formula of the robust adaptive filtering process is:

[0026] ;

[0027] in, is the Kalman gain, Estimate the variance of the previous state.

[0028] Furthermore, the robust adaptive filtering process comprises the steps of:

[0029] S51. Calculate prediction residuals ;in, ( k +1) is the target state vector at the next moment predicted based on the current state, z ( k ) is the sensor measurement value at the current moment;

[0030] S52. Dynamically adjust the adaptation factor based on the statistics of the prediction residual α T ;

[0031] S53. Calculate the diagonal weight matrix using the IGG-III weight function .

[0032] Furthermore, in the diagonal weight matrix ω ( i ) is calculated as:

[0033] in, k 0 and k 1 is a constant, is the standardized residual.

[0034] Furthermore, the adaptation factor α T The calculation formula is:

[0035] ;

[0036] Where c is the preset threshold, represents the normalized statistic of the residual, ; is the covariance matrix of the predicted state vector, R is the measurement noise covariance matrix.

[0037] The present invention also provides a visual servo system, which can implement the adaptive filtering prediction method with time lag compensation and noise resistance described in the present invention.

[0038] The present invention also provides a computer device, comprising:

[0039] at least one processor; and

[0040] a memory communicatively connected to the at least one processor; wherein,

[0041] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned robust adaptive filtering prediction method with time lag compensation and noise resistance of the present invention.

[0042] The present invention also provides a non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to enable the computer to execute the above-mentioned robust adaptive filtering prediction method with time lag compensation and noise resistance of the present invention.

[0043] Compared with the prior art, the present invention can achieve the following beneficial effects:

[0044] The present invention provides a robust adaptive filtering prediction method with time-lag compensation and noise resistance. By combining time-lag compensation technology and a robust adaptive filtering algorithm, it effectively addresses the issues of reduced tracking accuracy and system stability caused by time-lag and noise in visual servo systems. By compensating for visual sensor delays through a prediction algorithm, the system ensures that control decisions are based on real-time information, significantly improving the system's real-time and control performance. Furthermore, the robust adaptive filtering algorithm, combined with the IGG-III weighting function, effectively resists interference from measurement noise and abnormal data, enhancing the system's robustness and stability.

[0045] In addition, by comprehensively considering the position, velocity and acceleration of the target and fully describing the target's motion state, the tracking accuracy of the visual servo system is further improved. The design of the dynamic adjustment adaptation factor enables the system to adapt to the target's maneuverability and environmental changes, ensuring that high-performance tracking capabilities can be maintained under complex working conditions.

[0046] The robust adaptive filtering prediction method with time lag compensation and noise resistance provided by the present invention is suitable for visual servo systems with high-precision tracking and stable control requirements, and has important application value in the fields of non-cooperative target tracking and robot visual control. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The accompanying drawings, which constitute part of the present invention, are intended to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are intended to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0048] Figure 1This is a schematic diagram of the overall process of the robust adaptive filtering prediction method with time lag compensation and noise resistance provided in the embodiment of the present invention;

[0049] Figure 2 A schematic diagram showing a comparison of the predicted curves of the motion trajectory of a non-cooperative target in Example 1 of the present invention;

[0050] Figure 3 A schematic diagram showing a comparison of non-cooperative target motion tracking errors in Example 2 of the present invention;

[0051] Figure 4 A schematic diagram of a computer device for executing a robust adaptive filtering prediction method with time lag compensation and noise resistance according to an embodiment of the present invention.

[0052] Description of reference numerals:

[0053] 12. Computer device; 14. External device; 16. Processing unit; 18. Bus; 20. Network adapter; 22. I / O interface; 24. Display; 28. System memory; 30. RAM; 32. Cache; 34. Storage system; 40. Program / utility; 42. Program module. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and do not constitute a limitation of the present invention.

[0055] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.

[0056] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. In addition, the terms "first", "second" and the like are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, features defined as "first", "second" and the like may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0057] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "installed," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; and they can refer to internal connections between two components. Those skilled in the art can understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0058] In a specific embodiment of the present invention, a robust adaptive filtering prediction method with time lag compensation and noise resistance is provided, and the robust adaptive filtering prediction method with time lag compensation and noise resistance comprises the steps of:

[0059] S1. An inertial stabilization platform includes an inertial measurement unit (IMU) configured to measure motion state information of the inertial stabilization platform. The motion state information includes acceleration (angular acceleration) and angular velocity. By measuring the angular velocity and angular acceleration, an angle can be obtained, and the angle can be converted into information on a visual image plane.

[0060] S2. Convert the angle information into image information. Specifically, the rotation angle of the inertial stabilization platform is converted into pixel coordinates in the image plane through geometric relationships. By converting the angle information of the inertial sensor itself into image plane information of its own motion, and then superimposing the image plane information of the error between itself and the target (which can be measured by a visual sensor), information on the target motion trajectory can be obtained.

[0061] Specifically, the formula of the geometric relationship is (1);

[0062] in, θ x ,θ y are the rotation angles of the inertial stabilization platform on the x-axis and y-axis of the field of view, respectively; u,v are the mobile end pixels on the x-axis and y-axis of the field of view respectively; λ is the camera focal length, that is, the camera focal length in the visual servoing system.

[0063] This conversion process provides the basis for predicting the subsequent motion trajectory of non-cooperative targets. In specific implementation, the pixel position of the target on the image plane is calculated by accurately measuring the rotation angle of the inertial stabilization platform and combining it with the camera parameters, thereby realizing the mapping from physical space to image space.

[0064] S3. Since the measurement delay of the inertial sensor is negligible, while there is a significant delay in the visual servoing system, the present invention ensures the temporal consistency of the two data by delaying the inertial sensor measurement data; specifically, the inherent delay time of the visual sensor in the visual servoing system is measured. △t , the measurement data of the inertial sensor is converted into △t Performing delay processing so that the inertial data and the visual data are on the same time basis, and using a timestamp alignment algorithm to calibrate the temporal consistency of the inertial data and the visual data to ensure the accuracy of subsequent processing;

[0065] S4. Define the target state vector as a six-dimensional vector. Based on the Singer model, establish the system state equation and measurement equation. Since the target motion is unknown, a kinematic model is established to describe the target's motion trajectory. The Singer model assumes that the target's acceleration is uniformly distributed within a certain range and is suitable for describing the motion of targets with random maneuvering characteristics.

[0066] Specifically, the six-dimensional vector includes target position, target velocity and target acceleration; the target state vector is shown in formula (2):

[0067] (2);

[0068] in, u T ,v T Target location , is the target speed, is the target acceleration;

[0069] The system state equation is shown in formula (3): x T ( k +1)= Fx T ( k )+ w k (3);

[0070] The measurement equation is shown in formula (4): z ( k )= Hx T ( k )+ v k (4);

[0071] The covariance matrix is ​​shown in formula (5):

[0072] (5);

[0073] in,x T ( k ) is the target state vector at time k, x T ( k +1) is the target state vector at time k+1, F is the state transfer matrix, whose elements are determined by the parameters in the Singer model and reflect the transfer law of the target motion state at different times; w k is the process noise; It is the inverse of the maneuvering time constant, which represents the speed of target acceleration change; σ a 2 is the maneuvering acceleration variance, reflecting the fluctuation amplitude of target acceleration; H is the measurement matrix, the measurement matrix H Determined based on the sensor's measurement method and the definition of the target state vector, it is usually used to extract position information from the state vector. Here, the sensor's measurement data is the result of a fusion of inertial sensor data and visual sensor data. v k To measure noise, including sensor errors and environmental interference, Represents the noise weight of the Singer model process after integral discretization.

[0074] S5. Perform robust adaptive filtering; specifically, the robust adaptive filtering comprises the steps of:

[0075] S51. Calculate the prediction residual according to formula (6);

[0076] (6);

[0077] in, ( k +1) is the target state vector at the next moment predicted based on the current state, z ( k ) is the sensor measurement value at the current moment, specifically the measurement value after the data of the inertial sensor and the visual sensor are fused; the size of the residual directly reflects the accuracy of the prediction model and the reliability of the sensor data;

[0078] S52. Dynamically adjust the adaptation factor based on the statistics of the prediction residual α T Specifically, the accuracy of the model is evaluated by calculating the prediction residuals, and the adaptation factor is dynamically adjusted based on the residual statistics; the adaptation factor α T The calculation formula is shown in formula (7):

[0079] (7) ;

[0080] Where c is the preset threshold, which is usually selected between 1 and 2.5 based on experience; represents the normalized statistic of the residual, ;in, is the covariance matrix of the predicted state vector, R is the measurement noise covariance matrix.

[0081] S53. Calculate the diagonal weight matrix using the IGG-III weight function ; In the diagonal weight matrix ω ( i ) is calculated as shown in formula (8):

[0082] (8);

[0083] in, k 0 and k 1 is a constant, generally selected in the range of 1.0 to 2.5 and 3.5 to 8.0, respectively, to determine the interval of weight change. is the standardized residual; in this way, when the residual is small, it is given a larger weight, and the measurement data is considered to be more reliable; when the residual is large, the weight is gradually reduced, or even a very small weight is given to reduce the impact of abnormal data on the filtering results.

[0084] In a specific implementation, the complete iterative formula of the robust adaptive filtering process is shown in formula (9):

[0085] (9);

[0086] in, is the Kalman gain, Estimate the variance of the previous state.

[0087] S6. According to the inherent delay time of the visual sensor △t and filter period T s , the formula for calculating the number of prediction steps is N = ceil ( △t / T s )(10);

[0088] In getting k Momentary state estimation After that, execute N Step prediction,

[0089] (11);

[0090] The target position information after compensation is obtained, where F is as follows:

[0091] .

[0092] A robust adaptive Kalman filter predictor is used to perform multi-step prediction, and the state estimation value at the current moment is pushed forward by the corresponding time step to compensate for the time delay in the visual servo system. Specifically, after processing the previous steps, six-dimensional target motion information has been obtained, but this motion information contains delay. At this time, multi-step prediction is to predict the motion state at the current moment based on the delayed motion state. Generally, the sampling period is short, assuming it is 2 milliseconds, with a delay of 100 milliseconds, and the specific multi-step prediction is 50 steps. By using a robust adaptive Kalman filter predictor for multi-step prediction, the information lag caused by the time delay can be effectively compensated, and the real-time and control performance of the visual servo system can be improved.

[0093] In a specific embodiment, in the robust adaptive filtering prediction method with time lag compensation and noise resistance provided by the present invention, the specific implementation steps of S6 are as follows:

[0094] (1) Monitor the delay time △t of the visual sensor in real time and dynamically adjust the number of prediction steps N according to the filtering period Ts.

[0095] (2) A robust adaptive Kalman filter predictor is used to perform multi-step predictions, pushing the current state estimate forward by the corresponding time step to obtain the target position information after compensating for the time lag.

[0096] (3) Feedback the compensated target position information to the control system to ensure that the system makes control decisions based on real-time information, thereby improving tracking accuracy and system stability.

[0097] The robust adaptive filtering prediction method with time lag compensation and noise resistance provided in the specific embodiment of the present invention is suitable for visual servo systems with high-precision tracking and stable control requirements, and has important application value in the fields of non-cooperative target tracking and robot visual control.

[0098] The specific embodiment of the present invention also provides an application of a robust adaptive filtering prediction method with time lag compensation and noise resistance in a visual servo system, and specifically provides a visual servo system, which can implement the robust adaptive filtering prediction method with time lag compensation and noise resistance described in the present invention.

[0099] A specific embodiment of the present invention further provides a computer device, comprising:

[0100] at least one processor; and

[0101] a memory communicatively connected to the at least one processor; wherein,

[0102] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned robust adaptive filtering prediction method with time lag compensation and noise resistance of the present invention.

[0103] A specific embodiment of the present invention further provides a non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to enable the computer to execute the above-mentioned robust adaptive filtering prediction method with time lag compensation and noise resistance of the present invention.

[0104] The present invention provides a robust adaptive filtering prediction method with time-lag compensation and noise resistance. By combining time-lag compensation technology and a robust adaptive filtering algorithm, it effectively addresses the issues of reduced tracking accuracy and system stability caused by time-lag and noise in visual servo systems. By compensating for visual sensor delays through a prediction algorithm, the system ensures that control decisions are based on real-time information, significantly improving the system's real-time and control performance. Furthermore, the robust adaptive filtering algorithm, combined with the IGG-III weighting function, effectively resists interference from measurement noise and abnormal data, enhancing the system's robustness and stability.

[0105] In addition, by comprehensively considering the position, velocity and acceleration of the target and fully describing the target's motion state, the tracking accuracy of the visual servo system is further improved. The design of the dynamic adjustment adaptation factor enables the system to adapt to the target's maneuverability and environmental changes, ensuring that high-performance tracking capabilities can be maintained under complex working conditions.

[0106] The robust adaptive filtering prediction method with time lag compensation and noise resistance provided by the present invention is suitable for visual servo systems with high-precision tracking and stable control requirements, and has important application value in the fields of non-cooperative target tracking and robot visual control.

[0107] The present invention will be described in detail below with reference to the accompanying drawings and in combination with embodiments.

[0108] Select an inertial measurement unit with high precision and fast response speed, and integrate it tightly with the inertial stabilization platform to accurately measure the acceleration, angular velocity and other motion state information of the target object; specifically, Figure 1 FIG. 1 is a schematic diagram of the overall flow of the method for robust adaptive filtering prediction with time lag compensation and noise resistance according to a specific embodiment of the present invention. As can be seen from the figure, the method for robust adaptive filtering prediction with time lag compensation and noise resistance includes the following steps:

[0109] S1. The inertial stabilization platform includes an inertial measurement unit (IMU), which is used to measure the motion state information of the inertial stabilization platform. Specifically, the IMU can be integrated with the inertial stabilization platform to measure the motion state information of the inertial stabilization platform. The motion state information includes acceleration and angular velocity, and angle information is obtained through acceleration and angular velocity.

[0110] S2. Convert the angle information into image information using the following formula (1):

[0111] (1);

[0112] in, θ x ,θ y are the rotation angles of the inertial stabilization platform on the x-axis and y-axis of the field of view, respectively; u,v are the mobile end pixels on the x-axis and y-axis of the field of view, λ is the focal length of the camera; by calculating the moving pixels of the inertial stabilization platform itself, the motion trajectory of the non-cooperative target is not prepared.

[0113] S3. Artificially delay the inertial sensor measurement data according to a fixed delay time to ensure data synchronization and time consistency between the two. Specifically, the inertial sensor is artificially delayed according to the fixed delay time. The visual servo system determines relative deviations through visual sensors, and the extension of image acquisition and processing time will introduce significant delay to the system. In contrast, the measurement delay of the inertial measurement unit in the inertial stabilization platform is negligible.

[0114] S4. Define the target state vector and establish the target kinematic model; define the state vector of the moving target as a six-dimensional vector containing the target position, target velocity and target acceleration, that is,

[0115] (2);

[0116] in, u T ,v T Target location , is the target speed, is the target acceleration; this definition can comprehensively describe the target's motion state and provide sufficient information for subsequent state estimation and prediction. Specifically, the position component directly gives the target's specific coordinates in space and is a key parameter for the servo system to locate and track; the velocity component can help the system understand the target's motion trend and predict its approximate position at the next moment; the acceleration component reflects the changes in the target's motion state and is important for handling complex movements such as rapid changes in direction, acceleration, or deceleration of maneuvering targets; by comprehensively considering these three levels of motion characteristics, the state vector can more accurately characterize the target's dynamic behavior and improve the system's adaptability to target motion and tracking accuracy.

[0117] The Singer model, assuming that the target's acceleration is uniformly distributed within a certain range, is suitable for describing the motion of targets with random maneuvering characteristics. In practical applications, many targets do not move with completely uniform speed or acceleration, but rather exhibit a certain degree of random variation. The Singer model can well fit the motion patterns of such targets, providing a reasonable theoretical basis for establishing the system's equation of state.

[0118] Based on the Singer model, the state equation and measurement equation of the system are established.

[0119] The state equation describes the evolution of the target state vector over time, and its specific form is shown in formula (3): x T ( k +1)= Fx T ( k )+ w k (3);

[0120] The measurement equation is shown in formula (4): z ( k )= Hx T ( k )+ v k (4);

[0121] in, x T ( k ) is the target state vector at time k, x T ( k +1) is the target state vector at time k+1, Fis the state transfer matrix, whose elements are determined by the parameters in the Singer model and reflect the transfer law of the target motion state between different moments; is the process noise, which is used to simulate the random factors in the target motion, such as the random change of acceleration, etc. Its covariance matrix formula is shown in (5):

[0122] (5);

[0123] α is the inverse of the maneuvering time constant, which represents the speed of target acceleration change; σ a 2 is the maneuvering acceleration variance, which reflects the fluctuation amplitude of acceleration, Represents the noise weight of the Singer model process after integral discretization.

[0124] The measurement equation establishes the relationship between the sensor measurement value and the target's true state; H The measurement matrix is ​​determined by the sensor's measurement method and the definition of the target state vector. It is usually used to extract the position information corresponding to the sensor measurement from the state vector. v k Measurement noise includes the influence of sensor errors, environmental interference and other factors on the measurement results.

[0125] S5. Perform robust adaptive filtering. First, the complete iterative formula for robust adaptive filtering is given:

[0126] (9);

[0127] in, is the Kalman gain, Estimate the variance of the previous state.

[0128] Calculating the prediction residual is a key step in the filtering prediction process. The residual reflects the difference between the predicted value and the actual measured value, that is,

[0129] (6);

[0130] in, ( k +1) is the target state vector at the next moment predicted based on the current state, z ( k ) is the sensor measurement value at the current moment; the size of the residual directly reflects the accuracy of the prediction model and the reliability of the sensor data, and provides a basis for the calculation of the subsequent adaptation factor and robust factor.

[0131] The adaptation factor adjusts the data weighting during the filtering process. When the residual statistics exceed a preset threshold, the influence of the predicted value on the filtering and decision-making process is reduced, enhancing the system's adaptability to target mobility and environmental changes. By properly setting the adaptation factor, the filtering algorithm can maintain good performance under different operating conditions, avoiding problems such as filter divergence caused by large deviations between the model and the actual situation.

[0132] The adaptation factor is determined based on the statistics of the residual. The specific formula is:

[0133] (7);

[0134] in, c It is a preset threshold, usually selected between 1 and 2.5 based on experience, used to determine whether the residual is within an acceptable range; The normalized statistic representing the residuals normalizes the residual vector with the trace of its covariance matrix, eliminating the effects of dimension and magnitude, so that residuals of different dimensions can be uniformly compared and evaluated.

[0135] ,in, is the covariance matrix of the predicted state vector, R The covariance matrix of the measurement noise jointly determines the covariance structure of the residuals.

[0136] In order to further improve the filtering algorithm's resistance to measurement noise and abnormal data, the IGG-III weight function is used to calculate the diagonal weight matrix. ; In the diagonal weight matrix ω ( i ) is calculated as shown in formula (8):

[0137] (8);

[0138] in, k 0 and k 1 is a constant, generally selected in the range of 1.0 to 2.5 and 3.5 to 8.0, respectively, to determine the interval of weight change. is the standardized residual, indicating the i The ratio of a residual component to the corresponding standard deviation; in this way, when the residual is small, it is given a larger weight, and the measurement data is considered to be more reliable; when the residual is large, the weight is gradually reduced, or even a very small weight is given to reduce the impact of abnormal data on the filtering results, thereby improving the robustness and stability of the system.

[0139] S6. Non-cooperative target position time lag compensation processing.

[0140] In visual servo systems, time lag primarily stems from the image acquisition and processing of visual sensors. Due to the large amount of image data and the complex computations required for feature extraction and target recognition, there is a delay between the actual target position change and the system acquiring the corresponding measurement information. This time lag can cause the system to make control decisions based on outdated information, reducing tracking accuracy and even causing system instability.

[0141] According to the delay time of the visual sensor △t and filter period T s , the formula for calculating the number of prediction steps is N = ceil ( △t / T s )(10);

[0142] In getting k Momentary state estimation After that, execute N Step prediction,

[0143] (11);

[0144] The target position information after compensation is obtained, where F is as follows:

[0145] .

[0146] After obtaining the target motion state estimate at each time node, the robust adaptive Kalman filter predictor is used to perform additional prediction steps to determine the position of the moving target, thereby compensating for the time lag in the system. Specifically, based on the estimated value of the time lag, the number of prediction steps required is determined, and then the state estimate at the current moment is converted to the state estimate at the current moment using the recursive formula of the filter predictor.

[0147] Advance the corresponding time step forward to obtain the target position information after compensating the time lag; this method can effectively make up for the information lag caused by time lag, enable the system to respond to the target's motion changes more promptly and accurately, and improve the real-time and control performance of the visual servo system.

[0148] According to the above conditions and methods, two groups of comparative analysis and verification are performed using the robust adaptive filtering prediction method provided by the embodiment of the present invention and the traditional Kalman filtering method.

[0149] Example 1

[0150] The motion trajectory of the non-cooperative target is filtered and predicted by the robust adaptive filtering prediction method, and compared with the real curve. The results are as follows: Figure 2As shown in the figure, the three lines from left to right in the left box represent the predicted trajectory, the true trajectory and the measured trajectory respectively; in the right box, the three lines from top to bottom represent the predicted trajectory, the true trajectory and the measured trajectory respectively; it can be seen from the figure that the predicted trajectory is closer to the true trajectory, overcoming the influence of time lag.

[0151] Example 2

[0152] The non-cooperative target is tracked by the robust adaptive wave prediction method, and the tracking errors of the traditional method and the method of the present invention are compared. The results are as follows: Figure 3 As shown in the figure, it can be seen that the tracking effect of the prediction method proposed in the embodiment of the present invention is significantly better than that of the traditional method.

[0153] Accordingly, according to embodiments of the present invention, the present invention further provides a visual servoing system, a computer device, a readable storage medium, and a computer program product.

[0154] Figure 4 FIG. 1 is a structural diagram of a computer device 12 provided in an embodiment of the present invention. Figure 4 A block diagram of an exemplary computer device 12 suitable for use in implementing embodiments of the present invention is shown. Figure 4 The computer device 12 shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present invention.

[0155] like Figure 4 As shown, computer device 12 is represented in the form of a general-purpose computing device. Computer device 12 is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. Electronic devices can also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present invention described and / or claimed herein.

[0156] Components of computer device 12 may include, but are not limited to, one or more processors or processing units 16 , a system memory 28 , and a bus 18 that connects various system components, including system memory 28 and processing unit 16 .

[0157] Bus 18 represents one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, an accelerated graphics port, a processor, or a local bus using any of a variety of bus architectures. Examples of these architectures include, but are not limited to, an Industry Standard Architecture (ISA) bus, a Micro Channel Architecture (MAC) bus, an Enhanced ISA bus, a Video Electronics Standards Association (VESA) local bus, and a Peripheral Component Interconnect (PCI) bus.

[0158] The computer device 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by the computer device 12, including volatile and non-volatile media, removable and non-removable media.

[0159] System memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and / or cache memory (cache) 32. Computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, storage system 34 may be configured to read and write non-removable, non-volatile magnetic media ( Figure 4 Not shown, often called a "hard drive"). Although Figure 4 Not shown, a magnetic disk drive for reading and writing to a removable non-volatile magnetic disk (e.g., a "floppy disk"), and an optical disk drive for reading and writing to a removable non-volatile optical disk (e.g., a CD-ROM, DVD-ROM, or other optical media) may be provided. In these cases, each drive may be connected to bus 18 via one or more data media interfaces. Memory 28 may include at least one program product having a set (e.g., at least one) of program modules configured to perform the functions of various embodiments of the present invention.

[0160] A program / utility 40 having a set (at least one) of program modules 42 may be stored, for example, in memory 28. Such program modules 42 include, but are not limited to, an operating system, one or more application programs, other program modules, and program data, each of which, or some combination thereof, may include an implementation of a network environment. Program modules 42 generally implement the functions and / or methods of the embodiments described herein.

[0161] The computer device 12 can also communicate with one or more external devices 14 (e.g., a keyboard, pointing device, display 24, etc.), one or more devices that enable a user to interact with the computer device 12, and / or any device that enables the computer device 12 to communicate with one or more other computing devices (e.g., a network card, a modem, etc.). Such communication can occur via an input / output (I / O) interface 22. Furthermore, the computer device 12 can communicate with one or more networks (e.g., a local area network (LAN), a wide area network (WAN), and / or a public network such as the Internet) via a network adapter 20. As shown, the network adapter 20 communicates with the other modules of the computer device 12 via a bus 18. It should be understood that, although not shown, other hardware and / or software modules can be used in conjunction with the computer device 12, including but not limited to microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0162] The processing unit 16 executes various functional applications and data processing by running programs stored in the system memory 28, such as implementing the robust adaptive filtering prediction method with time lag compensation and noise resistance provided by the embodiment of the present invention.

[0163] An embodiment of the present invention also provides a non-transitory computer-readable storage medium storing computer instructions, on which a computer program is stored, wherein when the program is executed by a processor, the adaptive filtering prediction method with time lag compensation and noise resistance provided in all the inventive embodiments of the present application is provided.

[0164] The computer storage medium of the embodiment of the present invention can adopt any combination of one or more computer-readable media. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this document, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, apparatus or device.

[0165] A computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, which carries computer-readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0166] The program code that comprises on the computer-readable medium can be transmitted with any appropriate medium, includes but not limited to wireless, electric wire, optical cable, RF etc., or above-mentioned any suitable combination.Can write the computer program code that is used to carry out the operation of the present invention with one or more programming languages ​​or its combination, described programming language comprises object-oriented programming language such as Java, Smalltalk, C++, also comprises conventional procedural programming language--such as " C " language or similar programming language.Program code can be carried out on user's computer completely, partly on user's computer, carry out as an independent software package, partly on user's computer partly on remote computer, or carry out completely on remote computer or server.In the situation that relates to remote computer, remote computer can comprise local area network (LAN) or wide area network (WAN) to be connected to user's computer by the network of any kind, perhaps, can be connected to external computer (for example, utilize Internet service provider to come to connect by Internet).

[0167] An embodiment of the present invention further provides a computer program product, comprising a computer program, which, when executed by a processor, implements the above-mentioned robust adaptive filtering prediction method with time lag compensation and noise resistance.

[0168] It should be understood that the various forms of the processes shown above can be used to reorder, add, or delete steps. For example, the steps described in the present disclosure can be performed in parallel, sequentially, or in a different order, as long as the desired results of the technical solutions disclosed in the present disclosure can be achieved. This is not limited herein.

[0169] The above specific embodiments do not limit the scope of protection of the present invention. Those skilled in the art will appreciate that various modifications, combinations, sub-combinations, and substitutions may be made based on design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention are intended to be included within the scope of protection of the present invention.

Claims

1. A robust adaptive filtering prediction method with time lag compensation and noise resistance, characterized by: The method for predicting with adaptive filtering and time-delay compensation and noise resistance comprises the following steps: S1. The inertial stabilization platform includes an inertial measurement unit, the inertial measurement unit being used to measure the motion state information of the inertial stabilization platform; S2. The rotation angle of the inertial stabilization platform is converted into pixel coordinates in the image plane by geometric relationships; S3. Measuring the intrinsic delay time of the visual sensor in the visual servo system △t , the measurement data of the inertial sensor is converted into △t Performing delay processing so that the inertial data and the visual data are on the same time basis, and calibrating the time consistency of the inertial data and the visual data using a timestamp alignment algorithm; S4. Define the target state vector as a six-dimensional vector and establish the system state equation and measurement equation based on the Singer model; S5. Perform robust adaptive filtering; the iterative formula for the robust adaptive filtering is: ； in, is the target state vector of the next moment predicted based on the current state, is the Kalman gain, is the variance of the previous state estimate, is the covariance matrix of the predicted state vector, R is the measurement noise covariance matrix, z ( k ) is the sensor measurement value at the current moment, α T is the adaptation factor, is a diagonal weight matrix, H is the measurement matrix, F is the state transfer matrix; The steps of the robust adaptive filtering process include: S51. Calculate prediction residuals ; S52. Dynamically adjust the adaptation factor based on the statistics of the prediction residual α T ; S53. Calculate the diagonal weight matrix using the IGG-III weight function ; S6. According to the inherent delay time of the visual sensor △ t and filter period T s , calculate the number of prediction steps N = ceil (△ t / T s ); In getting k Momentary state estimation After that, execute N Step prediction is performed to obtain the target position information after compensating the time lag; A robust adaptive Kalman filter predictor is used to perform multi-step prediction, and the state estimation value at the current moment is advanced by the corresponding time step to compensate for the time delay in the visual servo system.

2. The robust adaptive filtering prediction method with time lag compensation and noise resistance according to claim 1, characterized in that: The motion state information includes acceleration and angular velocity; The formula for the geometric relationship is ; in, θ x ,θ y are the rotation angles of the inertial stabilization platform on the x-axis and y-axis of the field of view, respectively; u,v are the mobile end pixels on the x-axis and y-axis of the field of view, λ is the camera focal length.

3. The robust adaptive filtering prediction method with time lag compensation and noise resistance according to claim 1, characterized in that: The six-dimensional vector includes the target position, target velocity and target acceleration; the target state vector is: ； in, u T ,v T is the target location, , is the target speed, is the target acceleration; The system state equation is x T ( k +1)= Fx T ( k )+ w k , the measurement equation is z ( k )= Hx T ( k )+ v k ; The covariance matrix is: ； in, x T ( k ) is the target state vector at time k, x T ( k +1) is the target state vector at time k+1, F is the state transition matrix, w k is the process noise, is the inverse of the maneuvering time constant, σ a 2 is the maneuver acceleration variance, H is the measurement matrix, v k To measure noise, Represents the noise weight of the Singer model process after integral discretization.

4. The robust adaptive filtering prediction method with time lag compensation and noise resistance according to claim 1, characterized in that: The diagonal weight matrix ω ( i ) is calculated as: ； in, k 0 and k 1 is a constant, is the standardized residual.

5. The robust adaptive filtering prediction method with time lag compensation and noise resistance according to claim 1, characterized in that: The adaptation factor α T The calculation formula is: ； Where c is the preset threshold, represents the normalized statistic of the residual, .

6. A visual servo system, characterized in that: The visual servo system can implement the robust adaptive filtering prediction method with time lag compensation and noise resistance according to any one of claims 1 to 5.

7. A computer device, characterized in that: include: at least one processor; as well as a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the robust adaptive filtering prediction method with time-lag compensation and noise resistance as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium storing computer instructions, characterized in that: The computer instructions are used to enable the computer to execute the robust adaptive filtering prediction method with time lag compensation and noise resistance according to any one of claims 1 to 5.

Images