Multi-sensor fusion based intelligent state estimation method for high-speed industrial unit trajectory tracking

Abstract

The application discloses a kind of multi-sensor fusion's high-speed industrial unit track tracking intelligent state estimation method, belong to industrial motion control technical field.The method is by constructing heterogeneous multi-source observation framework and quality evaluation mechanism, noise time sequence characteristics and system dynamic characteristics of observation deviation are respectively extracted using double-flow gate timing memory network, and based on observation quality dynamic distribution weight is fused;Through lightweight deep neural network, adaptive Kalman gain matrix is generated, intelligent fusion and high-precision state estimation of multi-source data are realized;Further introduce the robust weight matrix based on Huber loss function, update abnormal threshold by combining the median absolute deviation of sliding window, effectively suppress outlier observation;Through confidence evaluation and saturation limiting mechanism, state estimation quality is assessed and guaranteed in real time.The application significantly improves the estimation accuracy, robustness and system stability of high-speed industrial unit under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of high-speed industrial unit trajectory tracking and control technology, specifically to a multi-sensor fusion intelligent state estimation method for high-speed industrial unit trajectory tracking. Background Technology

[0002] In modern industrial automation systems, high-speed motion units are widely used in intelligent logistics, automated warehousing, and flexible manufacturing. These motion units require precise trajectory tracking capabilities to ensure accurate following of predetermined paths even at high speeds, enabling them to complete complex transportation and positioning tasks. Accurate state estimation is fundamental to achieving high-performance trajectory tracking control, directly impacting the system's positioning accuracy, response speed, and operational safety.

[0003] Traditional state estimation methods primarily rely on a single sensor, such as machine vision or inertial measurement units (IMUs). Single-sensor methods have significant limitations: machine vision sensors experience a sharp performance decline under complex environments such as varying lighting conditions, occlusion, and missing ground texture; IMUs suffer from cumulative drift errors, leading to significant deviations after prolonged operation; and motion encoders are susceptible to ground slippage and wheel wear, resulting in inaccurate velocity estimation. These problems are particularly pronounced in high-speed motion scenarios, leading to insufficient estimation accuracy for longitudinal position errors, lateral position errors, orientation angle errors, and velocity errors, severely impacting trajectory tracking performance.

[0004] Existing multi-sensor fusion methods mostly employ traditional Kalman filtering or extended Kalman filtering, fusing multi-source information through a fixed Kalman gain matrix. However, the observation noise characteristics in high-speed industrial environments are complex and variable, and the reliability of different sensors varies significantly under different operating conditions. For example, visual sensors are more accurate in well-lit areas, while IMUs are more reliable in low-light areas. Fixed-gain fusion strategies cannot adaptively adjust the weights of each sensor, making it difficult to maintain optimal fusion results when sensor performance fluctuates. Furthermore, traditional Kalman filtering assumes that the noise is Gaussian white noise, lacking robustness to outliers such as those caused by temporary sensor failures, which can easily lead to estimation divergence.

[0005] In recent years, deep learning technology has demonstrated powerful capabilities in time-series data processing. Existing methods often use deep learning as an independent end-to-end state estimator, replacing traditional filtering frameworks. However, these methods have two main drawbacks: first, they lack physical model constraints and interpretability, making it difficult to guarantee generalization performance in scenarios exceeding the training distribution; second, they require large amounts of labeled data for training, resulting in high data acquisition costs in industrial applications. Summary of the Invention

[0006] The technical problem to be solved by this invention is: addressing the technical challenges of unstable single-sensor data, severe noise interference, insufficient adaptive capability of fixed-gain Kalman filtering, and poor robustness in state estimation during trajectory tracking of high-speed industrial units, this invention proposes a multi-sensor fusion-based intelligent state estimation method for trajectory tracking of high-speed industrial units. This method achieves accurate, fast, robust, low-computational-cost, and highly reliable estimation of the multi-dimensional motion state of high-speed industrial units, thereby improving the trajectory tracking performance, control accuracy, and overall anti-interference capability of high-speed industrial units under complex operating conditions.

[0007] To solve the above problems, the present invention adopts the following technical solution:

[0008] This invention proposes a multi-sensor fusion intelligent state estimation method for high-speed industrial unit trajectory tracking, specifically including the following steps:

[0009] S1. Acquire spatial position observation data, attitude angle observation data, and velocity observation data of the high-speed industrial unit through machine vision sensors, inertial measurement units, and motion encoders; establish unified raw heterogeneous multi-source observation data using heterogeneous multi-source observation modules.

[0010] S2. The data processing module performs preprocessing and quality assessment on the original heterogeneous multi-source observation data, including time synchronization alignment, observation quality coefficient calculation, and data standardization, to obtain cleaned heterogeneous multi-source observation data.

[0011] S3. Define the extended state vectors of the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit. Calculate the extended state of the high-speed industrial unit through the extended state prediction module to obtain the heterogeneous multi-source observation deviation vector.

[0012] S4. Obtain the noise temporal features of machine vision sensors, inertial measurement units and motion encoders in the heterogeneous multi-source observation deviation vector through the dual-stream temporal feature extraction module, and dynamically learn the system dynamic features of their longitudinal position, lateral position, orientation angle and velocity changes. Combine the two features according to the dual-path dynamic weight allocation mechanism of visual quality perception, and output the fused temporal feature vector.

[0013] S5. Input the fused temporal feature vector into the lightweight deep neural network operation module and output the adaptive multi-source fusion gain matrix;

[0014] S6. Through the state update module, the heterogeneous multi-source observation deviation vector is weighted and fused using the adaptive multi-source fusion gain matrix, and combined with the robust weight matrix of the Hu Bo loss, the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit are updated.

[0015] S7. Through the output saturation limiting module, saturation limiting is applied to the longitudinal position error, lateral position error, directional angle error, longitudinal velocity error, lateral velocity error, and directional angular velocity error of the high-speed industrial unit after the update state. The confidence evaluation index is calculated, and the state estimation result is output to the controller.

[0016] Preferably, step S1 specifically includes:

[0017] S101. Establish a unified heterogeneous multi-source observation framework, expressed as:

[0018] ;

[0019] in, Let k be the original heterogeneous multi-source observation data vector at time k. This is the trajectory tracking error state vector, which includes longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error. To control the input vector; For observation functions; , , These are the sub-observation functions for the machine vision sensor, inertial measurement unit, and motion encoder, respectively. Image data acquired by machine vision sensors; The attitude angle data measured by the inertial measurement unit; Angular velocity observation data obtained from the motion encoder; , , This is the corresponding observation noise vector;

[0020] S102. Generate a raw multi-source observation dataset containing spatial position, attitude angle, and velocity information.

[0021] Preferably, step S2 specifically includes:

[0022] S201. Linear interpolation is used to synchronize and align the data of machine vision sensors, inertial measurement units, and motion encoders in time, eliminating differences in sensor sampling times.

[0023] S202, Observation quality coefficient of computer vision sensor:

[0024] ;

[0025] in, The observation quality coefficient; This represents the number of feature points detected. This represents the maximum number of feature points. The standard deviation of image contrast; Maximum contrast; , These are the weighting coefficients;

[0026] S203, Heterogeneous multi-source observation data Standardization and normalization processes were performed to obtain cleaned observation data. .

[0027] Preferably, step S3 includes:

[0028] S301. Define the trajectory tracking extended error state vector:

[0029] ;

[0030] in, , These are longitudinal position error and lateral position error, respectively. This refers to the direction angle error; , , These are longitudinal velocity error, lateral velocity error, and directional angular velocity error, respectively.

[0031] S302. Establish the nonlinear state evolution equation:

[0032] ;

[0033] in, Here is the state transition matrix. To control the input matrix, It is a higher-order nonlinear coupling term. This is process noise;

[0034] S303, Perform state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of high-speed industrial units:

[0035] ;

[0036] ;

[0037] in, For prior state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error, Let be the prior error covariance matrix. Let be the posterior error covariance matrix of the previous time step. The process noise covariance matrix;

[0038] S304. Calculate the heterogeneous multi-source observation bias vector:

[0039] ;

[0040] in, This is the heterogeneous multi-source observation bias vector.

[0041] Preferably, step S4 specifically includes:

[0042] S401. Extract heterogeneous multi-source observation bias sequences The noise temporal characteristics are analyzed, and the observation bias includes measurement biases such as longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error. A gated temporal memory network is used to calculate the hidden states of the noise temporal characteristics. :

[0043] ;

[0044] in, For the mapping function of the heterogeneous noise temporal feature extraction unit, For network parameters;

[0045] S402. Extract prior state prediction sequences through the dynamic law learning unit of the motion unit. The system's dynamic characteristics are analyzed. The motion unit dynamic law learning unit uses a gated temporal memory network to model the dynamic evolution law of the motion unit. This prior state prediction includes predicted values ​​of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit. The hidden state of the system's dynamic characteristics is calculated. :

[0046] ;

[0047] in, For the mapping function of the learning unit of the dynamic law of the motion unit, For network parameters;

[0048] S403, Based on the observation quality coefficient Dynamic calculation of fusion weights and ;

[0049] S404, Timing characteristics of fused noise With system dynamic characteristics Generates fused temporal feature vectors for adaptive estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angle velocity error. :

[0050]

[0051] Preferably, step S5 specifically includes:

[0052] S501. Establish an adaptive multi-source fusion gain matrix generation network based on the fusion temporal feature vector. Calculate the adaptive multi-source fusion gain matrix for longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error of a high-speed industrial cell. :

[0053] ;

[0054] in, For gain generation neural networks, For network parameters;

[0055] S502. Construct the hierarchical structure of the gain generation network, which includes a fully connected layer, a ReLU activation function, a Dropout random deactivation layer, and a Tanh output layer.

[0056] S503, constrain the adaptive multi-source fusion gain matrix by using a structural constraint mask matrix.

[0057] Preferably, step S6 specifically includes:

[0058] S601, updating the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of a high-speed industrial unit based on an adaptive multi-source fusion gain matrix:

[0059] ;

[0060] in, For the posterior state estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angular velocity error; These are the predicted values ​​of the prior states; This is the robust weight matrix;

[0061] S602. Calculate robust weights using the Hu Bo loss function:

[0062] ;

[0063] in, The robust weight of the i-th state component to the j-th observation component; This represents the j-th heterogeneous multi-source observation bias component; The anomaly detection limit value for the j-th observation channel;

[0064] S603. Update the anomaly detection limit value by the absolute deviation of the middle value in the sliding window. ;

[0065] S604. Update the posterior error covariance matrix:

[0066] ;

[0067] in, The posterior error covariance matrix; It is the identity matrix; The observation matrix; To observe the noise covariance matrix.

[0068] Preferably, step S7 specifically includes:

[0069] S701, apply saturation limiting to the estimated values ​​of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error;

[0070] S702. Calculate the confidence evaluation index for state estimation:

[0071] ;

[0072] in, As a confidence level evaluation index; , Calculate the weight parameters for the confidence level;

[0073] S703, Execution Validity Assurance Determination:

[0074] ;

[0075] in, This is the upper bound of the magnitude of the state estimation vector; This is the confidence level limit; when the validity guarantee conditions are not met, the standby state estimation strategy is activated.

[0076] Preferably, the confidence level limit value Set it to 0.7.

[0077] Preferably, in step S603, the length of the sliding window used to update the anomaly detection limit value is 50.

[0078] The present invention adopts the above technical solution and has the following technical effects compared with the prior art:

[0079] (1) This invention establishes a unified heterogeneous multi-source observation framework and dynamically adjusts the fusion weight ratio of each sensor according to the observation quality coefficient of the machine vision sensor, thereby achieving adaptive adjustment of sensor weights. This method can automatically adjust the weight allocation according to the reliability differences of machine vision sensors, inertial measurement units, and motion encoders under different working conditions, improving the state estimation accuracy and robustness of high-speed industrial units under complex working conditions such as changes in lighting, occlusion, missing ground texture, and ground slippage, and solving the problem of poor adaptability of traditional fixed weight methods to environmental changes.

[0080] (2) This invention uses a heterogeneous sensor dual-stream time-series feature extraction network to learn the noise time-series characteristics of observation bias and the dynamic evolution law of the system, respectively. It then uses a dynamic weight allocation mechanism based on observation quality for adaptive fusion, and maps the fused features into an adaptive Kalman gain matrix through a deep neural network. This method achieves deep learning and real-time adaptation of observation noise characteristics and system dynamic laws, fully utilizing the time-series correlation between observation bias and state evolution, and overcoming the limitations of traditional fixed-gain Kalman filtering. Compared with traditional methods, this invention significantly improves the state estimation accuracy, temporal coherence, and anti-interference capability of high-speed industrial units at a lower computational cost, making it suitable for high-speed industrial applications.

[0081] (3) This invention uses a robust weighting matrix and the Hu Bo loss function to weight the heterogeneous multi-source observation bias, and uses the absolute deviation of the middle value in the sliding window to update the anomaly detection threshold in real time, thereby realizing automatic identification and weight suppression of outlier observations. This method reduces the interference of outlier observations on state estimation, ensures the continuity of state estimation of high-speed industrial units under sensor partial failure conditions, and improves the reliability and fault tolerance of the system under harsh conditions.

[0082] (4) This invention integrates the information from the observation deviation vector and the adaptive multi-source fusion gain matrix using a confidence evaluation index, combined with saturation limiting constraints and an effectiveness guarantee mechanism, to achieve real-time quantitative evaluation of the estimation quality of longitudinal position error, lateral position error, azimuth angle error, longitudinal velocity error, lateral velocity error, and azimuth angle velocity error. This method ensures that the output state estimation meets the confidence and amplitude constraints, providing a high-precision and high-reliability state input for the subsequent trajectory tracking controller, improving the trajectory tracking control performance and system stability of the high-speed industrial unit, and meeting the stringent requirements for state estimation quality in high-speed industrial applications. Attached Figure Description

[0083] Figure 1 This is a system topology diagram of the high-speed industrial unit involved in the present invention.

[0084] Figure 2This is a schematic diagram comparing the changes in position and angle estimation errors of the high-speed industrial unit involved in the embodiment over time.

[0085] Figure 3 This is a schematic diagram comparing the trajectory tracking performance of high-speed industrial units involved in the embodiments.

[0086] Figure 4 This is a schematic diagram illustrating the anti-interference performance of the method of the present invention as described in the embodiments.

[0087] Figure 5 The flowchart of the intelligent state estimation method for high-speed industrial unit trajectory tracking based on multi-sensor fusion, which is the subject of this invention, is shown in the present invention. Detailed Implementation

[0088] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0089] Example 1: To verify the effectiveness of the multi-sensor fusion-based intelligent state estimation method for high-speed industrial cell trajectory tracking of the present invention, a comparative experiment was conducted on a wheeled mobile robot platform. Since the state estimation accuracy cannot be directly measured, this example uses the estimated state as input to the controller, with trajectory tracking accuracy as an indirect evaluation index: accurate state estimation enables the controller to obtain reliable feedback information, thereby achieving high-precision trajectory tracking; while state estimation errors are directly transmitted to the control loop, leading to a decrease in trajectory tracking accuracy. The experiment used an extended Kalman filter (EKF) as the benchmark method for comparison.

[0090] This invention is applicable to various industrial motion scenarios, including intelligent logistics, automated warehousing, and flexible manufacturing, and can achieve high-precision state estimation under complex conditions such as changes in lighting, missing ground textures, and partial sensor malfunctions. Specific embodiments of this invention include a heterogeneous multi-source data acquisition module, a data processing module, an extended state prediction module, a dual-stream temporal feature extraction module, a lightweight deep neural network computation module, a state update module, and an output saturation limiting module.

[0091] A specific embodiment of the present invention is a state estimation method for a high-speed industrial cell, in the form of a high-speed industrial inspection mobile robot. The system topology is as follows: Figure 1 As shown, the system includes a high-speed industrial unit body, machine vision sensors, an inertial measurement unit, and a motion encoder. To verify the quality of the state estimation, a non-singular terminal sliding mode controller was used as the verification tool. This controller uses the state estimation results as feedback input to drive the robot to track a reference trajectory. The experimental parameters are set as shown in Table 1.

[0092] Table 1

[0093]

[0094] The experimental trajectory adopts an "8-shaped" reference trajectory, which includes various motion modes such as straight lines, turns, acceleration, and deceleration, and can comprehensively examine the performance of the state estimation algorithm under complex working conditions.

[0095] The heterogeneous multi-source data acquisition module of a specific embodiment of the present invention acquires two-dimensional planar position observations of a high-speed industrial unit through a machine vision sensor, acquires orientation angle observations through an inertial measurement unit, and acquires angular velocity observations through a motion encoder. (Reference) Figure 5 This includes the following steps:

[0096] S101. Establish a unified heterogeneous multi-source observation framework, expressed as:

[0097] (1)

[0098] in, Let k be the original heterogeneous multi-source observation data vector at time k. This is the trajectory tracking error state vector, which includes longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error. To control the input vector; For observation functions; , , These are the sub-observation functions for the machine vision sensor, inertial measurement unit, and motion encoder, respectively. Image data acquired by machine vision sensors; The attitude angle data measured by the inertial measurement unit; Angular velocity observation data obtained from the motion encoder; , , This is the corresponding observation noise vector;

[0099] S102. Position observation is provided by machine vision sensors, orientation angle observation is provided by inertial measurement units, and angular velocity observation is provided by motion encoders. The data from these three types of sensors together constitute the original multi-source observation dataset containing spatial position, attitude angle, and velocity information.

[0100] The data preprocessing and quality assessment module in a specific embodiment of the present invention performs spatiotemporal alignment, quality assessment, and standardization on the raw heterogeneous multi-source observation data, providing high-quality input for subsequent state estimation. This includes the following steps:

[0101] S201. Since the three types of sensors have different sampling frequencies, linear interpolation is used to synchronize and align the data of the machine vision sensor, inertial measurement unit and motion encoder in time to eliminate the difference in sensor sampling time.

[0102] S202, Observation quality coefficient of computer vision sensor:

[0103] (2)

[0104] in, The observation quality coefficient; This represents the number of feature points detected. The maximum number of feature points is set to 500 in this embodiment; The standard deviation of image contrast; Set the maximum contrast to 80. , These are weighting coefficients, set to 0.6 and 0.4 in this embodiment. The observation quality coefficient is used to evaluate the reliability of the visual sensor at the current moment. When the lighting conditions are good and feature points are abundant, Approaching 1; when illumination changes or feature points are missing, decline.

[0105] S203, Heterogeneous multi-source observation data Standardization and normalization processes were performed to obtain cleaned observation data. This data is used for subsequent state estimation. Standardization eliminates the dimensional differences between different sensors, and normalization maps the data to the [-1, 1] interval, improving numerical stability.

[0106] The extended state prediction module of a specific embodiment of the present invention, based on a high-speed industrial unit kinematic model, predicts the state at the next moment and calculates the observation deviation. It includes the following steps:

[0107] S301. Define the trajectory tracking extended error state vector:

[0108] (3)

[0109] in, , These are longitudinal position error and lateral position error, respectively. This refers to the direction angle error; , , These are longitudinal velocity error, lateral velocity error, and angular velocity error. The extended state vector includes six dimensions, encompassing position error and velocity error, comprehensively describing the trajectory tracking error state of the high-speed industrial unit.

[0110] S302. Establish the nonlinear state evolution equation:

[0111] (4)

[0112] in, Here is the state transition matrix. To control the input matrix, It is a higher-order nonlinear coupling term. This represents process noise. The state evolution equation describes the change of the error state of the high-speed industrial unit over time, where the nonlinear coupling term characterizes the robot's kinematic constraints and dynamic characteristics.

[0113] S303, Perform state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of high-speed industrial units:

[0114] (5)

[0115] (6)

[0116] in, For prior state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error, Let be the prior error covariance matrix. Let be the posterior error covariance matrix of the previous time step. Let be the process noise covariance matrix. Based on the posterior estimate of the previous time step and the current control input, predict the state at the current time step, providing prior information for subsequent Kalman updates.

[0117] S304. Calculate the heterogeneous multi-source observation bias vector:

[0118] (7)

[0119] in, This is a heterogeneous multi-source observation bias vector, reflecting the difference between actual and predicted observations. Observation bias is the core information for Kalman filter state updates, encompassing the combined effects of sensor noise and model uncertainty.

[0120] The dual-stream temporal feature extraction module of a specific embodiment of the present invention extracts a fused temporal feature vector through deep temporal modeling of heterogeneous sensor observation sequences and system state prediction sequences. This includes the following steps:

[0121] S401. The heterogeneous noise temporal feature extraction module extracts the noise temporal features of machine vision sensors, inertial measurement units, and motion encoders from the heterogeneous multi-source observation bias vector of high-speed industrial units. A GRU network is used. Encode the observation bias sequence:

[0122] (8)

[0123] in, To observe the parameters of the encoder GRU, This is the extracted noise temporal feature vector. Observation encoders can learn the noise characteristics of different sensors under different operating conditions, such as the noise patterns of vision sensors when illumination changes, the drift characteristics of inertial measurement units, and encoder slippage noise.

[0124] S402: The system dynamics of a high-speed industrial cell are learned through dynamic learning of its longitudinal position, lateral position, orientation angle, and velocity variations. A GRU network is employed. Encode the prior state prediction sequence:

[0125] (9)

[0126] in, These are the parameters of the state encoder GRU. This is the extracted system dynamic feature vector. The state encoder can learn the motion patterns of high-speed industrial units, such as typical action modes like acceleration, turning, and deceleration, providing motion priors for adaptive adjustment of the gain matrix.

[0127] S403, Based on the observation quality coefficient Dynamic calculation of fusion weights and In this embodiment, the sum of the two weights is 1.

[0128] S404, Timing characteristics of fused noise With system dynamic characteristics Generates fused temporal feature vectors for adaptive estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angle velocity error. :

[0129]

[0130] in, , For the Sigmoid function, , In this embodiment, the parameters are learnable. = 2.5, = -1.2. When visual quality is high, Increased visual quality depends more on observational information; when visual quality deteriorates, The reduction in reliance on motion model predictions is significant. This adaptive fusion mechanism ensures optimal feature representations are obtained under various operating conditions.

[0131] The lightweight deep neural network computing module of a specific embodiment of the present invention maps fused temporal features into an adaptive multi-source fusion gain matrix using a lightweight deep neural network. It includes the following steps:

[0132] S501. Input the fused temporal feature vector into the lightweight deep neural network operation module, and output the adaptive multi-source fusion gain matrix:

[0133] (11)

[0134] in, For gain generation neural networks, Network parameters. Adaptive multi-source fusion gain matrix. The dimension is 6×m, where 6 is the state dimension and m is the observation dimension.

[0135] S502. Construct the hierarchical structure of the gain generation network. The specific network structure is as follows: input layer (fusion feature dimension 128), fully connected layer 1 (256 neurons), ReLU, Dropout (inactivation rate 0.3), fully connected layer 2 (128 neurons), ReLU, output layer (6×m neurons), Tanh. The Dropout layer prevents overfitting, and the Tanh output layer limits the gain value to the [-1,1] range to improve numerical stability.

[0136] S503. The adaptive multi-source fusion gain matrix is ​​constrained by the structural constraint mask matrix. The mask matrix M is multiplied element-wise with the gain matrix to ensure that the gain matrix meets the theoretical requirements of Kalman filtering.

[0137] The state update module in a specific embodiment of the present invention updates the posterior state estimate and covariance matrix based on an adaptive gain matrix and a robust weighting strategy. This includes the following steps:

[0138] S601: Utilizes an adaptive multi-source fusion gain matrix to weight and fuse heterogeneous multi-source observation bias vectors, and combines this with a robust weight matrix to update the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit.

[0139] (12)

[0140] in, For the posterior state estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angular velocity error; These are the predicted values ​​of the prior states; This is the robust weight matrix;

[0141] S602. Calculate robust weights using the Hu Bo loss function:

[0142] (13)

[0143] in, The robust weight of the i-th state component to the j-th observation component; This represents the j-th heterogeneous multi-source observation bias component; Let be the anomaly detection limit value for the j-th observation channel; when the observation deviation is less than the limit value, the robust weight is 1 to make full use of the observation information; when the observation deviation exceeds the limit value, the robust weight decreases inversely to suppress the impact of abnormal observations.

[0144] S603. Update the anomaly detection limit value by the absolute deviation of the middle value in the sliding window. A sliding window of length 50 is used to calculate the median absolute deviation (MAD) of the observation bias, and the anomaly detection threshold is dynamically adjusted to adapt to changes in noise levels under different operating conditions.

[0145] S604. Update the posterior error covariance matrix:

[0146] (14)

[0147] in, The posterior error covariance matrix; It is the identity matrix; The observation matrix; The observation noise covariance matrix is ​​used. The posterior covariance matrix quantifies the uncertainty of the state estimate, providing a basis for prediction at the next time step.

[0148] The confidence assessment and output module of a specific embodiment of the present invention constrains and assesses the quality of the state estimation results to ensure the reliability of the output. It includes the following steps:

[0149] S701 applies saturation limiting to the estimated values ​​of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error. Based on the physical constraints of the high-speed industrial unit, the position error is limited to ±2m, the velocity error to ±3m / s, and the angular error to ±π rad to prevent estimation divergence.

[0150] S702. Calculate the confidence evaluation index for state estimation:

[0151] (15)

[0152] in, As a confidence level evaluation index; , The weighting parameters are calculated for the confidence level. The confidence level takes into account both the magnitude of the observation bias and the norm of the gain matrix. When the observation bias is small and the gain is moderate, the confidence level is high. When the observation is abnormal or the gain is too large, the confidence level decreases.

[0153] S703, Execution Validity Assurance Determination:

[0154] ;

[0155] in, This is the upper bound of the magnitude of the state estimation vector; This is the confidence level limit; when the validity guarantee conditions are not met, the standby state estimation strategy is activated.

[0156] when Furthermore, when all state components satisfy the magnitude constraints, the output state estimation result is obtained. The system will then switch to the high-speed industrial unit drive controller to complete subsequent control; otherwise, it will issue an early warning signal and use the state estimate from the previous moment or activate the backup state estimation scheme to ensure system safety.

[0157] The traditional Extended Kalman Filter (EKF) was chosen as the baseline method for state estimation. Under the same experimental conditions, the performance of the proposed method and the EKF was compared. The experimental results are shown in Table 2.

[0158] Table 2

[0159]

[0160] Experimental results show that, compared with the traditional EKF method, the method of the present invention reduces the total MSE by 53.5%, the longitudinal position error MSE by 54.7%, the lateral position error MSE by 54.8%, and the orientation angle error MSE by 42.1%. Furthermore, the inference time of the method of the present invention is only 0.04 seconds, meeting the requirements for real-time control.

[0161] See appendix Figure 2 This paper demonstrates the changes in position and angle estimation errors of the two methods over time during "figure-eight" trajectory tracking. It can be seen that the estimation error of the method proposed in this invention remains at a low level throughout the entire trajectory process, with a position error not exceeding 0.1 m and an angle error not exceeding 0.4 rad. In contrast, the error of the EKF method accumulates over time, exceeding 0.25 m in position and 0.5 rad in angle at the later stage of the trajectory, i.e., after 60 time steps, showing a significant increase in error. This performance advantage stems from the fact that the dual-stream temporal feature extraction module of the method proposed in this invention can quickly and continuously process the observation sequence and state dynamics separately, while simultaneously learning temporal dependencies through feature fusion to achieve accurate, fast, and continuously stable state estimation.

[0162] See appendix Figure 3 This paper compares the trajectory tracking performance of a cascaded high-speed industrial unit drive controller based on the method of this invention with that based on an extended Kalman filter algorithm. Under high-speed motion conditions, the position tracking error does not exceed ±2.75 cm, and the attitude tracking error is constrained to within ±0.015 rad. Table 3 provides the quantitative indicators of the trajectory tracking control performance.

[0163] Table 3

[0164]

[0165] See appendix Figure 4 To evaluate the algorithm's robustness, random high-frequency perturbations of ±0.4 m / s were introduced within a 20-50 second time period. Experimental results show that under perturbation conditions, the MSE of the proposed method increased by only 8.5%, from -8.12 dB to -7.43 dB; while the MSE of EKF increased by 23.7%, from -4.89 dB to -3.73 dB. This robustness advantage is attributed to the adaptive mechanism of the proposed method, which allows the network to effectively handle the effects of noise and perturbations by dynamically adjusting the Kalman gain, while effectively suppressing error accumulation.

[0166] The present invention proposes a multi-sensor fusion-based intelligent state estimation method for trajectory tracking of high-speed industrial units. The effectiveness of the method has been verified on a high-speed industrial inspection mobile robot system platform.

[0167] (1) Compared with the traditional EKF method, the total MSE is reduced by 53.5%, the longitudinal position error MSE is reduced by 54.7%, the lateral position error MSE is reduced by 54.8%, and the orientation angle error MSE is reduced by 42.1%, achieving high-precision state estimation;

[0168] (2) The reasoning time is only 0.04 seconds, which meets the real-time requirement of a 20 ms control cycle, and the calculation delay does not exceed 5 ms, thus achieving the requirement of accurate and fast state estimation;

[0169] (3) Under the condition of ± 0.4 m / s random disturbance, the MSE increases by only 8.5%, demonstrating excellent robustness and anti-interference ability;

[0170] The method of this invention significantly improves the state estimation accuracy, calculation speed, anti-interference and stability of trajectory tracking of high-speed industrial units by using techniques such as heterogeneous multi-source sensor fusion, dual-stream time-series feature extraction, adaptive gain generation and robust state update, providing a stable and effective prerequisite for the precise control of high-speed industrial units under complex working conditions.

[0171] The specific implementation schemes described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific implementation schemes of the present invention and are not intended to limit the scope of the present invention. Any equivalent changes and modifications made by those skilled in the art without departing from the concept and principles of the present invention should fall within the scope of protection of the present invention.

Claims

1. A multi-sensor fusion intelligent state estimation method for high-speed industrial cell trajectory tracking, characterized in that, Specifically, the following steps are included: S1. Acquire spatial position observation data, attitude angle observation data, and velocity observation data of the high-speed industrial unit through machine vision sensors, inertial measurement units, and motion encoders; establish unified raw heterogeneous multi-source observation data using heterogeneous multi-source observation modules. S2. The data processing module performs preprocessing and quality assessment on the original heterogeneous multi-source observation data, including time synchronization alignment, observation quality coefficient calculation, and data standardization, to obtain cleaned heterogeneous multi-source observation data. S3. Define the extended state vectors of the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit. Calculate the extended state of the high-speed industrial unit through the extended state prediction module to obtain the heterogeneous multi-source observation deviation vector. S4. Obtain the noise temporal features of machine vision sensors, inertial measurement units and motion encoders in the heterogeneous multi-source observation deviation vector through the dual-stream temporal feature extraction module, and dynamically learn the system dynamic features of their longitudinal position, lateral position, orientation angle and velocity changes. Combine the two features according to the dual-path dynamic weight allocation mechanism of visual quality perception, and output the fused temporal feature vector. S5. Input the fused temporal feature vector into the lightweight deep neural network operation module and output the adaptive multi-source fusion gain matrix; S6. Through the state update module, the heterogeneous multi-source observation deviation vector is weighted and fused using the adaptive multi-source fusion gain matrix, and combined with the robust weight matrix of the Hu Bo loss, the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit are updated. S7. Through the output saturation limiting module, saturation limiting is applied to the longitudinal position error, lateral position error, directional angle error, longitudinal velocity error, lateral velocity error, and directional angular velocity error of the high-speed industrial unit after the update state. The confidence evaluation index is calculated, and the state estimation result is output to the controller.

2. The method according to claim 1, characterized in that, Step S1 specifically includes: S101. Establish a unified heterogeneous multi-source observation framework, expressed as: ； in, Let k be the original heterogeneous multi-source observation data vector at time k. This is the trajectory tracking error state vector, which includes longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error. To control the input vector; For observation functions; , , These are the sub-observation functions for the machine vision sensor, inertial measurement unit, and motion encoder, respectively. Image data acquired by machine vision sensors; The attitude angle data measured by the inertial measurement unit; Angular velocity observation data obtained from the motion encoder; , , This is the corresponding observation noise vector; S102. Generate a raw multi-source observation dataset containing spatial position, attitude angle, and velocity information.

3. The method according to claim 1, characterized in that, Step S2 specifically includes: S201. Linear interpolation is used to synchronize and align the data of machine vision sensors, inertial measurement units, and motion encoders in time, eliminating differences in sensor sampling times. S202, Observation quality coefficient of computer vision sensor: ； in, The observation quality coefficient; This represents the number of feature points detected. The maximum number of feature points; The standard deviation of image contrast; Maximum contrast; , These are the weighting coefficients; S203, Heterogeneous multi-source observation data Standardization and normalization processes were performed to obtain cleaned observation data. .

4. The method according to claim 1, characterized in that, Step S3 includes: S301. Define the trajectory tracking extended error state vector: ； in, , These are longitudinal position error and lateral position error, respectively. This refers to the direction angle error; , , These are longitudinal velocity error, lateral velocity error, and directional angular velocity error, respectively. S302. Establish the nonlinear state evolution equation: ； in, Here is the state transition matrix. To control the input matrix, It is a higher-order nonlinear coupling term. This is process noise; S303, Perform state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of high-speed industrial units: ； ； in, For prior state prediction of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error, Let be the prior error covariance matrix. Let be the posterior error covariance matrix of the previous time step. The process noise covariance matrix; S304. Calculate the heterogeneous multi-source observation bias vector: ； in, This is the heterogeneous multi-source observation bias vector.

5. The method according to claim 1, characterized in that, Step S4 specifically includes: S401. Extract heterogeneous multi-source observation bias sequences The noise temporal characteristics are analyzed, and the observation bias includes measurement biases such as longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error. A gated temporal memory network is used to calculate the hidden states of the noise temporal characteristics. : ； in, For the mapping function of the heterogeneous noise temporal feature extraction unit, For network parameters; S402. Extract prior state prediction sequences through the dynamic law learning unit of the motion unit. The system's dynamic characteristics are analyzed. The motion unit dynamic law learning unit uses a gated temporal memory network to model the dynamic evolution law of the motion unit. This prior state prediction includes predicted values ​​of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of the high-speed industrial unit. The hidden state of the system's dynamic characteristics is calculated. : ； in, For the mapping function of the learning unit of the dynamic law of the motion unit, For network parameters; S403, Based on the observation quality coefficient Dynamic calculation of fusion weights and ; S404, Timing characteristics of fused noise With system dynamic characteristics Generates fused temporal feature vectors for adaptive estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angle velocity error. :

6. The method according to claim 1, characterized in that, Step S5 specifically includes: S501. Establish an adaptive multi-source fusion gain matrix generation network based on the fusion temporal feature vector. Calculate the adaptive multi-source fusion gain matrix for longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth angular velocity error of a high-speed industrial cell. : ； in, For gain generation neural networks, For network parameters; S502. Construct the hierarchical structure of the gain generation network, which includes a fully connected layer, a ReLU activation function, a Dropout random deactivation layer, and a Tanh output layer. S503, constrain the adaptive multi-source fusion gain matrix by using a structural constraint mask matrix.

7. The method according to claim 1, characterized in that, Step S6 specifically includes: S601, updating the longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error of a high-speed industrial unit based on an adaptive multi-source fusion gain matrix: ； in, For the posterior state estimation of longitudinal position error, lateral position error, orientation angle error, longitudinal velocity error, lateral velocity error, and orientation angular velocity error; These are the predicted values ​​of the prior states; This is the robust weight matrix; S602. Calculate robust weights using the Hu Bo loss function: ； in, The robust weight of the i-th state component to the j-th observation component; This represents the j-th heterogeneous multi-source observation bias component; The anomaly detection limit value for the j-th observation channel; S603. Update the anomaly detection limit value by the absolute deviation of the middle value in the sliding window. ; S604. Update the posterior error covariance matrix: ； in, The posterior error covariance matrix; It is the identity matrix; The observation matrix; To observe the noise covariance matrix.

8. The method according to claim 1, characterized in that, Step S7 specifically includes: S701, apply saturation limiting to the estimated values ​​of longitudinal position error, lateral position error, azimuth error, longitudinal velocity error, lateral velocity error, and azimuth velocity error; S702. Calculate the confidence evaluation index for state estimation: ； in, As a confidence level evaluation index; , Calculate the weight parameters for the confidence level; S703, Execution Validity Assurance Determination: ； in, This is the upper bound of the magnitude of the state estimation vector; This is the confidence level limit; when the validity guarantee conditions are not met, the standby state estimation strategy is activated.

Images