DNN-EKF-based multi-sensor heterogeneous data fusion method, program, equipment and storage medium

Abstract

The invention belongs to the technical field of sensor data fusion, and particularly relates to a DNN-EKF-based multi-sensor heterogeneous data fusion method, a DNN-EKF-based multi-sensor heterogeneous data fusion program, equipment and a storage medium. The method comprises the following steps: predicting a current carrier pose and noise covariance matrix according to a last multi-sensor heterogeneous data fusion result based on an EKF model; for each sensor, pre-training a DNN model, and obtaining a nonlinear error compensation item according to a residual error between carrier pose prediction and observation data of each sensor; and filtering observation data of each sensor by adopting extended Kalman filtering, and balancing a nonlinear error compensation item and an extended Kalman filtering result by adopting a learning weight. According to the method, through combination of DNN and extended Kalman filtering, IMU, VO and GPS heterogeneous data can be effectively fused, the problem of non-uniform data time of different sensors is solved, sensor noise can be dynamically adjusted, and the accuracy of a data fusion result is improved.

Description

Technical Field

[0001] This invention belongs to the field of sensor data fusion technology, specifically involving a method, program, device, and storage medium for multi-sensor heterogeneous data fusion based on DNN-EKF. Background Technology

[0002] Multi-sensor (IMU, VO, GPS) systems suffer from fusion error accumulation due to differences in sampling frequencies and heterogeneous data structures (IMU uses inertial data, VO uses image feature points, and GPS uses latitude and longitude coordinates). Traditional extended Kalman filters (EPFs) assume noise follows a Gaussian distribution. However, the nonlinear relationships involved in heterogeneous multi-sensor data (e.g., differences in observation models between different sensors) or the non-Gaussian nature of the noise can lead to large data fusion errors. Inconsistent data acquisition frequencies and transmission delays between different sensors can also significantly reduce the accuracy of data fusion due to misalignment in system state estimation. Furthermore, the noise characteristics of different sensors can vary significantly (e.g., GPS noise is high but low-frequency, while IMU noise is low but high-frequency). Traditional EPFs, assuming the noise is Gaussian white noise with a fixed covariance, struggle to dynamically adapt to the heterogeneity of multi-source noise. Summary of the Invention

[0003] The purpose of this invention is to solve the problems of spatiotemporal mismatch, data structure mismatch, and low data fusion accuracy of traditional extended Kalman filter methods for heterogeneous data from multiple sensors. It provides a multi-sensor heterogeneous data fusion method, program, device, and storage medium based on DNN-EKF, which can effectively fuse heterogeneous data from IMU, VO, and GPS, and solve the problem of inconsistent time between different sensor data. It can also dynamically adjust sensor noise and improve the accuracy of data fusion results.

[0004] The multi-sensor heterogeneous data fusion method based on DNN-EKF includes the following steps:

[0005] During movement, a carrier equipped with multiple sensors acquires observation data about the carrier's pose from each sensor.

[0006] Based on the EKF model, the carrier pose and noise covariance matrix is ​​predicted according to the previous multi-sensor heterogeneous data fusion results.

[0007] The residuals between the carrier pose prediction and the observation data of each sensor are calculated separately; for each sensor, a DNN model is pre-trained; the residuals of each sensor are input into the corresponding pre-trained DNN model to obtain the nonlinear error compensation term, and the filter noise covariance matrix is ​​calculated through the latent spatial features of the convolutional autoencoder of the DNN model.

[0008] Based on the carrier pose prediction, noise covariance matrix prediction, and filtered noise covariance matrix, extended Kalman filtering is performed on the observation data of each sensor to obtain the carrier pose filtering result and the noise covariance matrix filtering result.

[0009] For each sensor, the learning weights are used to balance the nonlinear error compensation term and the extended Kalman filter result to obtain the balance result of the carrier pose. The balance results of each sensor with respect to the carrier pose are weighted and fused to obtain the accurate estimation result of the carrier pose, thus realizing the fusion of heterogeneous data from multiple sensors.

[0010] Furthermore, the carrier is equipped with a GPS sensor, an IMU sensor, and a V0 sensor, and the observation data from the GPS sensor is Z. k1 =[x k1 ,y k1 ,z k1 ] T The observation data from the IMU sensor is Z. k2 =[φ k2 ,θ k2 ,ψ k2 ] T The observation data from the VO sensor is Z. k3 =[x k3 ,y k3 ,z k3 ,φ k3 ,θ k3 ,ψ k3 ] T Where k is the number of calculations for multi-sensor heterogeneous data fusion, and the time interval between two adjacent calculations is Δt; x, y, z are the three-dimensional position coordinates of the carrier; φ, ψ, θ are the three-dimensional attitude angles of the carrier, namely roll angle, pitch angle and heading angle.

[0011] Furthermore, the prediction of the carrier pose and noise covariance matrix based on the EKF model and the previous multi-sensor heterogeneous data fusion results is as follows:

[0012]

[0013] in, For this carrier pose prediction; This is the accurate estimation result of the carrier pose obtained from the previous multi-sensor heterogeneous data fusion; For this noise covariance matrix prediction; This is the accurate estimation result of the noise covariance matrix obtained from the previous multi-sensor heterogeneous data fusion; I is the identity matrix; Q is the process noise covariance matrix; X0 = [x0, y0, z0, φ0, ψ0, θ0] T Let v0 be the initial pose of the carrier and v0 be the initial velocity of the carrier.

[0014] Furthermore, the calculation of the residuals between the predicted carrier pose and the observation data from each sensor specifically involves:

[0015]

[0016] in, H3 = I;

[0017] Residual A k1 A k2 A k3 The nonlinear error compensation term Δx is obtained by inputting the corresponding pre-trained DNN model into the model. k1 Δx k2 Δx k3 The filter noise covariance matrix R is calculated using the latent spatial features of the convolutional autoencoder in the DNN model. k1 R k2 R k3 ;

[0018] R k1 =f1(W1Α k1 +b1), R k2 =f2(W2Α k2 +b2), R k3 =f3(W3Α k3 +b3)

[0019] Where f1(·), f2(·), and f3(·) are the activation functions of the convolutional autoencoders of the DNN models corresponding to the GPS sensor, IMU sensor, and VO sensor, respectively; W1, W2, and W3 are the corresponding weight matrices; and b1, b2, and b3 are the corresponding biases.

[0020] Furthermore, the specific training method for the DNN model corresponding to a certain type of sensor is as follows:

[0021] Simulate the motion of the carrier over a continuous period of time to obtain a training dataset. Each sample in the training dataset includes the carrier's true pose information y. kσ The sensor's observation data Z regarding the carrier pose kσ , will y kσ -Z kσ As input to the DNN model, the DNN model outputs a nonlinear error compensation term Δx. kσ Train the DNN model until y (k-1)σ +Δx kσ With y kσ The error meets the requirements:

[0022] ||y (k-1)σ +Δx kσ -y kσ ||≤ξ

[0023] Where ξ is the training error threshold of the DNN model; y kσ With Z kσ The dimensions are consistent.

[0024] Furthermore, the prediction based on carrier pose Noise covariance matrix prediction and the filter noise covariance matrix R k1 R k2 R k3 The GPS sensor observation information Z was analyzed respectively. k1 IMU sensor observation information Z k2 VO sensor observation information Z k3 Extended Kalman filtering is performed to obtain the carrier pose filtering result. Filtering results with noise covariance matrix Specifically:

[0025]

[0026] in,

[0027] Will Weighted fusion yields the noise covariance matrix.

[0028]

[0029] Where β1, β2, and β3 are weighting coefficients, and β1 + β2 + β3 = 1.

[0030] Furthermore, for each sensor, the balance result of the carrier pose is obtained by balancing the nonlinear error compensation term and the extended Kalman filter result using learned weights, specifically as follows:

[0031]

[0032]

[0033] Where γ1, γ2, and γ3 are learning weights, γ1∈(0,1), γ2∈(0,1), and γ3∈(0,1);

[0034] The balance results of each sensor with respect to the carrier pose Weighted fusion is performed to obtain an accurate estimate of the carrier pose.

[0035]

[0036] Where α1, α2, and α3 are weighting coefficients, and α1 + α2 + α3 = 1.

[0037] A computer device / apparatus / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-described multi-sensor heterogeneous data fusion method based on DNN-EKF.

[0038] A computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the above-described DNN-EKF-based multi-sensor heterogeneous data fusion method.

[0039] A computer program product includes a computer program / instructions that, when executed by a processor, implement the steps of the above-described DNN-EKF-based multi-sensor heterogeneous data fusion method.

[0040] The beneficial effects of this invention are as follows:

[0041] This invention proposes a novel solution to problems such as sensor noise, frequency inconsistency, and nonlinear system modeling in the data fusion process by combining DNN and extended Kalman filtering. This invention can highlight the advantages of high precision and high robustness in multi-source heterogeneous data fusion. It innovatively uses DNN and extended Kalman filtering to optimize data noise and solves the problem of large accumulation of fusion error caused by differences in sampling frequency and delay of multiple sensors. Attached Figure Description

[0042] Figure 1 This is a diagram of the overall architecture of the present invention.

[0043] Figure 2 This is a comparison chart of the errors between the traditional EKF method and the present invention.

[0044] Figure 3 This is a comparison chart of the data fusion calculation results between the traditional EKF method and the present invention. Detailed Implementation

[0045] The present invention will now be further described with reference to the accompanying drawings.

[0046] To address the problems of spatiotemporal mismatch, data structure mismatch, and low data fusion accuracy of traditional extended Kalman filter methods for heterogeneous multi-sensor data, this invention innovatively proposes a multi-sensor heterogeneous data fusion method based on DNN-EKF. This method can effectively fuse heterogeneous data from IMU, VO, and GPS, and solves the problem of inconsistent time signatures among different sensor data. It can also dynamically adjust sensor noise, improving the accuracy of the data fusion results. This invention includes the following steps:

[0047] Step 1: Before movement, the carrier equipped with GPS, IMU, and VO sensors acquires initial pose information X0 = [x0, y0, z0, φ0, ψ0, θ0]. T x, y, z are the three-dimensional position coordinates of the carrier, and φ, ψ, θ are the three-dimensional attitude angles of the carrier, namely roll angle, pitch angle and yaw angle; set the calculation time interval Δt, process noise covariance matrix Q, initial velocity of the carrier v0, and initialize the number of calculations k = 1;

[0048] Where Q is a 6×6 diagonal matrix, and the elements on the diagonal are the process noise covariances of each dimension of the pose information.

[0049] Step 2: Acquire GPS sensor observation data Z k1 =[x k1 ,y k1 ,z k1 ] T IMU sensor observation data Z k2 =[φ k2 ,θ k2 ,ψ k2 ] T VO sensor measurement data Z k3 =[x k3 ,y k3 ,z k3 ,φ k3 ,θ k3 ,ψ k3 ] T ;

[0050] Step 3: Based on the EKF model, use the pose information obtained from the previous data fusion. With noise covariance matrix Obtain the pose prediction Prediction with noise covariance matrix

[0051]

[0052] in, I represents the identity matrix;

[0053] Step 4: Calculate pose predictions separately Z with GPS sensor observation information k1 IMU sensor observation information Z k2 VO sensor observation information Z k3 residuals:

[0054]

[0055] H3 = I

[0056] Residual A k1 A k2 A k3 The nonlinear error compensation term Δx is obtained by inputting the corresponding pre-trained DNN model into the model. k1 Δx k2 Δx k3 The filter noise covariance matrix R is calculated using the latent spatial features of the convolutional autoencoder in the DNN model. k1 R k2 R k3 ;

[0057] R k1 =f1(W1Α k1 +b1), R k2 =f2(W2Α k2 +b2), R k3 =f3(W3Α k3 +b3)

[0058] Where f1(·), f2(·), and f3(·) are the activation functions of the convolutional autoencoders of the DNN models corresponding to the GPS sensor, IMU sensor, and VO sensor, respectively; W1, W2, and W3 are the corresponding weight matrices; and b1, b2, and b3 are the corresponding biases. The activation function can be the softmax function.

[0059] The specific training method for the DNN model corresponding to the GPS sensor is as follows:

[0060] Simulate the motion of the carrier over a continuous period of time to obtain a training dataset. Each sample in the training dataset includes the carrier's true pose information y. k1 =[x k ,y k ,z k ] T Z with GPS sensor observation information k1 =[x k1 ,y k1 ,z k1 ] T , will y k1 -Z k1As input to the DNN model, the DNN model outputs a nonlinear error compensation term Δx. k1 Train the DNN model until y k-1 +Δx k1 With y k The error meets the requirements, that is:

[0061] ||y k-1 +Δx k1 -y k ||≤ξ

[0062] Where ξ is the training error threshold of the DNN model;

[0063] The training methods for the DNN models corresponding to the IMU sensor and the VO sensor are the same as above;

[0064] Step 5: Predict based on pose Noise covariance matrix prediction Filter noise covariance matrix R k1 R k2 R k3 The GPS sensor observation information Z was analyzed respectively. k1 IMU sensor observation information Z k2 VO sensor observation information Z k3 Perform extended Kalman filtering to obtain the pose filtering result. Filtering results with noise covariance matrix Will Weighted fusion yields the noise covariance matrix.

[0065]

[0066] in, β1, β2, and β3 are weighting coefficients, and β1 + β2 + β3 = 1;

[0067] Step 6: Use a learned weight-balanced DNN compensation term and an extended Kalman filter to obtain the balanced pose prediction result. Will Weighted fusion yields pose information. Complete the fusion of heterogeneous data from multiple sensors;

[0068]

[0069]

[0070] Where γ1, γ2, and γ3 are learning weights, γ1∈(0,1), γ2∈(0,1), and γ3∈(0,1); α1, α2, and α3 are weight coefficients, and α1+α2+α3=1;

[0071] Step 7: If the carrier does not stop moving, let k = k + 1 and return to step 2.

[0072] To address the issues of heterogeneous data from multiple sensors and inconsistent sensor data timing, this invention proposes an innovative methodological architecture, which can be implemented using a fully domestically produced Rockchip RK3588 hardware platform with an integrated 6-TOPS NPU. By constructing corresponding measurement models for different sensors and setting inconsistent measurement transformation matrices, preliminary processing of heterogeneous data is completed. A DNN noise compensation module is introduced, and a dynamic noise adjustment scheme reduces the impact of noise from different sensors on data fusion errors. Finally, a coupled EKF and DNN joint update method improves the accuracy of heterogeneous multi-source sensor data fusion and reduces data errors.

[0073] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-sensor heterogeneous data fusion method based on DNN-EKF, characterized in that: During movement, a carrier equipped with multiple sensors acquires observation data about the carrier's pose from each sensor. Based on the EKF model, the carrier pose and noise covariance matrix is ​​predicted according to the previous multi-sensor heterogeneous data fusion results. The residuals between the carrier pose prediction and the observation data of each sensor are calculated separately; for each sensor, a DNN model is pre-trained; the residuals of each sensor are input into the corresponding pre-trained DNN model to obtain the nonlinear error compensation term, and the filter noise covariance matrix is ​​calculated through the latent spatial features of the convolutional autoencoder of the DNN model. Based on the carrier pose prediction, noise covariance matrix prediction, and filtered noise covariance matrix, extended Kalman filtering is performed on the observation data of each sensor to obtain the carrier pose filtering result and the noise covariance matrix filtering result. For each sensor, the learning weights are used to balance the nonlinear error compensation term and the extended Kalman filter result to obtain the balance result of the carrier pose. By weighted and fused the balance results of each sensor regarding the carrier pose, an accurate estimation result of the carrier pose is obtained, realizing the fusion of heterogeneous data from multiple sensors.

2. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 1, characterized in that: The carrier is equipped with a GPS sensor, an IMU sensor, and a V0 sensor. The observation data from the GPS sensor is Z. k1 =[x k1 ,y k1 ,z k1 ] T The observation data from the IMU sensor is Z. k2 =[φ k2 ,θ k2 ,ψ k2 ] T The observation data from the VO sensor is Z. k3 =[x k3 ,y k3 ,z k3 ,φ k3 ,θ k3 ,ψ k3 ] T Where k is the number of calculations for multi-sensor heterogeneous data fusion, and the time interval between two adjacent calculations is Δt; x, y, z are the three-dimensional position coordinates of the carrier; φ, ψ, θ are the three-dimensional attitude angles of the carrier, namely roll angle, pitch angle and heading angle.

3. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 2, characterized in that: The prediction of the carrier pose and noise covariance matrix based on the EKF model, according to the previous multi-sensor heterogeneous data fusion results, is as follows: in, For this carrier pose prediction; This is the accurate estimation result of the carrier pose obtained from the previous multi-sensor heterogeneous data fusion; For this noise covariance matrix prediction; This is the accurate estimation result of the noise covariance matrix obtained from the previous multi-sensor heterogeneous data fusion; I is the identity matrix; Q is the process noise covariance matrix; X0 = [x0, y0, z0, φ0, ψ0, θ0] T Let v0 be the initial pose of the carrier and v0 be the initial velocity of the carrier.

4. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 3, characterized in that: The calculation of the residuals between the predicted carrier pose and the observation data from each sensor is specifically as follows: in, H3 = I; Residual A k1 A k2 A k3 The nonlinear error compensation term Δx is obtained by inputting the corresponding pre-trained DNN model into the model. k1 Δx k2 Δx k3 The filter noise covariance matrix R is calculated using the latent spatial features of the convolutional autoencoder in the DNN model. k1 R k2 R k3 ; R k1 =f1(W1Α k1 +b1),R k2 =f2(W2Α k2 +b2),R k3 =f3(W3Α k3 +b3) Where f1(·), f2(·), and f3(·) are the activation functions of the convolutional autoencoders of the DNN models corresponding to the GPS sensor, IMU sensor, and VO sensor, respectively; W1, W2, and W3 are the corresponding weight matrices; and b1, b2, and b3 are the corresponding biases.

5. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 4, characterized in that: The specific training method for the DNN model corresponding to a certain type of sensor is as follows: Simulate the motion of the carrier over a continuous period of time to obtain a training dataset. Each sample in the training dataset includes the carrier's true pose information y. kσ The sensor's observation data Z regarding the carrier pose kσ , will y kσ -Z kσ As input to the DNN model, the DNN model outputs a nonlinear error compensation term Δx. kσ Train the DNN model until y (k-1)σ +Δx kσ With y kσ The error meets the requirements: ||y (k-1)σ +Δx kσ -y kσ ||≤ξ Where ξ is the training error threshold of the DNN model; y kσ With Z kσ The dimensions are consistent.

6. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 4, characterized in that: The prediction based on carrier pose Noise covariance matrix prediction and the filter noise covariance matrix R k1 R k2 R k3 The GPS sensor observation information Z was analyzed respectively. k1 IMU sensor observation information Z k2 VO sensor observation information Z k3 Extended Kalman filtering is performed to obtain the carrier pose filtering result. Filtering results with noise covariance matrix Specifically: in, Will Weighted fusion yields the noise covariance matrix. Where β1, β2, and β3 are weighting coefficients, and β1 + β2 + β3 = 1.

7. The multi-sensor heterogeneous data fusion method based on DNN-EKF according to claim 6, characterized in that: For each type of sensor, the balance result of the carrier pose is obtained by balancing the nonlinear error compensation term and the extended Kalman filter result using learned weights, specifically as follows: Where γ1, γ2, and γ3 are learning weights, γ1∈(0,1), γ2∈(0,1), and γ3∈(0,1); The balance results of each sensor with respect to the carrier pose Weighted fusion is performed to obtain an accurate estimate of the carrier pose. Where α1, α2, and α3 are weighting coefficients, and α1 + α2 + α3 = 1.

8. A computer device / equipment / system, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instructions stored thereon, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method according to any one of claims 1 to 7.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method according to any one of claims 1 to 7.

Images