Multi-sensor fusion method of inertial navigation equipment

Abstract

The invention discloses a multi-sensor fusion method of inertial navigation equipment, which belongs to the technical field of inertial positioning, and comprises the steps of sensor data acquisition and synchronization, MCU and GPU data synchronization, particle filter algorithm data fusion and PCA (Principal Component Analysis) weft descending, and solves the problem that the influence of time delay on the particle filter algorithm is effectively reduced by adopting an MCU and GPU cooperative work mode. According to the method, all stages of particle filtering can be efficiently processed, the overall processing capacity of the system is remarkably improved, the influence of time delay on a particle filtering algorithm is effectively reduced, and the positioning precision is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of inertial positioning, and in particular relates to a multi-sensor fusion method of an inertial navigation device. Background Art

[0002] Particle filtering is widely used in positioning systems because it can handle nonlinear and non-Gaussian problems. Sensor data fusion technology combines data from multiple sensors (such as IMU, vision, GNSS, etc.) to improve the accuracy and reliability of estimation. In the prior art, IMU and vision sensors usually provide location information through time synchronization and fusion, while GNSS is used to provide global positioning.

[0003] Deficiencies of existing technology:

[0004] Insufficient time synchronization accuracy: Sensor data synchronization in existing technologies mostly relies on simple calibration of external hardware crystal oscillators or clock chips. The accuracy of time synchronization is often difficult to meet the requirements of high-precision positioning, resulting in increased position estimation errors.

[0005] Low computational efficiency: The particle filter algorithm itself has a large amount of computation, especially in the scenario of multi-sensor data fusion. Traditional processing methods may not be able to fully utilize hardware acceleration, resulting in computational delays.

[0006] Poor real-time performance: Currently, many sensor data fusion systems still have certain response delays in application scenarios with high real-time requirements, such as autonomous driving and robot navigation, and it is difficult to meet the stringent real-time requirements.

[0007] Complexity of fusion algorithms: Existing sensor data fusion algorithms often have difficulty processing large-scale, high-dimensional data. Traditional algorithms have high computational complexity when faced with multi-dimensional data and are difficult to adapt to modern high-performance systems.

[0008] The accuracy of the final result in the system using the particle filter algorithm is limited: In the current sensor data fusion system, it is impossible to eliminate the data deviation caused by hardware clock drift or other delays, and the accuracy of the fusion result is limited. Summary of the invention

[0009] The purpose of the present invention is to provide a multi-sensor fusion method for an inertial navigation device, which solves the technical problem of using an MCU and a GPU to work together to effectively reduce the influence of time delay on a particle filter algorithm and improve positioning accuracy.

[0010] To achieve the above purpose, the present invention adopts the following technical inventions:

[0011] A multi-sensor fusion method for an inertial navigation device comprises the following steps:

[0012] Step 1: MCU collects sensor data from IMU sensor, vision sensor and GNSS receiver, including IMU data, vision data and GNSS data;

[0013] Calibrate the local clock of the MCU using the timestamp of the GNSS data;

[0014] Use the timestamp of the GNSS data to adjust the time of the IMU data and vision data so that they are aligned with the local clock of the MCU;

[0015] Step 2: The MCU timestamps all sensor data and passes the timestamped sensor data to the GPU. The GPU uses the timestamp to perform data interpolation on the sensor data and handle data latency.

[0016] Step 3: Use the particle filter algorithm to fuse the sensor data. Specifically, the MCU is responsible for initializing the particles and completing the prediction step; the GPU is responsible for accelerating the update and resampling steps;

[0017] Step 4: GPU uses principal component analysis to reduce the dimension of the data generated by the particle filter algorithm;

[0018] Step 5: MCU outputs the particle weight, position and attitude state after fusion processing.

[0019] Preferably, when executing step 1, the following steps are specifically included:

[0020] Step 1-1: MCU collects IMU data, visual data and GNSS data from IMU sensor, visual sensor and GNSS receiver respectively;

[0021] Step 1-2: MCU synchronizes the MCU clock to the time of GNSS data through the timestamp attached to the GNSS data;

[0022] Step 1-3: Synchronize the IMU data and vision data with the MCU’s clock via timestamps.

[0023] Preferably, when executing step 2, the following steps are specifically included:

[0024] Step 2-1: Before transmitting the sensor data to the GPU, the MCU timestamps each data frame of the sensor data;

[0025] Step 2-2: The GPU interpolates the sensor data and performs delay compensation.

[0026] Preferably, when executing step 3, the particle filter algorithm includes a particle initialization step, a prediction step, an accelerated update step and a resampling step:

[0027] Initialize particle step: MCU initializes the particle set, each particle represents a possible state;

[0028] Prediction step: MCU updates the particle state according to the acceleration and angular velocity in the IMU data. The prediction formula is as follows:

[0029] x k =f(x k-1 ,u k )+w k ;

[0030] Among them, x k is the state vector at the kth moment, which includes position, velocity, and attitude; f(x k-1 ,u k ) is the state transition model, u k is the control input, including acceleration and angular velocity; w k is the process noise;

[0031] Accelerated update step: The GPU updates the weight of each particle based on the visual data. The formula for calculating particle weight is as follows:

[0032]

[0033] Among them, z k is the observation value at the kth moment, specifically the image feature in the visual data; is the predicted value of the state of particle i, h is the function mapping from the particle state to the observation space; R -1 It is the inverse of the observation noise covariance matrix, which is used to measure the accuracy of the observation. The smaller the value, the smaller the noise of the observation data and the greater the weight. is the weight of the i-th particle at the k-th moment; represents the transpose of the difference between the observed value and the predicted value of the particle state, representing the residual; T represents the transpose operation;

[0034] Resampling step: The GPU resamples the particle set according to the particle weights and selects particles whose weights reach the expected value for further state estimation.

[0035] Preferably, the algorithm formula of the principal component analysis method is as follows:

[0036] A=YV;

[0037] Among them, A is the data after dimensionality reduction, Y is the original data matrix, V is the transformation matrix of the principal component analysis method, which is composed of the eigenvectors of the data covariance matrix; the original data matrix includes sensor data.

[0038] The multi-sensor fusion method of an inertial navigation device described in the present invention solves the technical problem of using the collaborative working mode of MCU and GPU to effectively reduce the influence of time delay on the particle filter algorithm and improve the positioning accuracy. The present invention can efficiently process each stage of particle filtering by maximizing the respective advantages of MCU and GPU. The collaborative design of MCU and GPU hardware significantly improves the overall processing power of the system. By using GNSS timestamp to calibrate the MCU clock, IMU and visual data are synchronized with MCU time, which can effectively reduce the influence of time delay on the particle filter algorithm and improve positioning accuracy. In addition, the GPU performs interpolation processing according to the timestamp, further improving the timeliness and consistency of the data. By introducing GPU to accelerate the weight calculation and resampling of particle filtering, the calculation speed can be greatly improved, the delay can be reduced, and it is suitable for systems with high real-time requirements. PCA dimensionality reduction is performed by GPU to reduce the processing complexity of high-dimensional data and significantly improve data processing efficiency. PCA can remove redundant information, so that the system can focus on important features and improve computing efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is the main flow chart of the present invention;

[0040] Figure 2 is a flow chart of the particle filtering algorithm of the present invention;

[0041] Figure 3 is a flow chart of synchronizing MCU clock, IMU data and visual data through GNSS data of the present invention;

[0042] Figure 4 It is a flow chart of data synchronization between the GPU and the MCU of the present invention. DETAILED DESCRIPTION

[0043] Depend on Figure 1-Figure 4 A multi-sensor fusion method for an inertial navigation device is shown, comprising the following steps:

[0044] Step 1: MCU collects sensor data from IMU sensor, vision sensor and GNSS receiver, including IMU data, vision data and GNSS data;

[0045] Calibrate the local clock of the MCU using the timestamp of the GNSS data;

[0046] Use the timestamp of the GNSS data to adjust the time of the IMU data and vision data so that they are aligned with the local clock of the MCU;

[0047] When executing step 1, the specific steps include:

[0048] Step 1-1: MCU collects IMU data, visual data and GNSS data from IMU sensor, visual sensor and GNSS receiver respectively;

[0049] Step 1-2: MCU synchronizes the MCU clock to the time of GNSS data through the timestamp attached to the GNSS data;

[0050] In this embodiment, the specific steps of steps 1-2 are as follows:

[0051] Step 1-2-1: Get the current timestamp t from the GNSS receiver GNSS ;

[0052] Step 1-2-2: Set the MCU clock t MCU Calibrated to the timestamp t of the GNSS data GNSS :

[0053] Calculate the synchronization deviation between MCU and GNSS Δt = t GNSS -t MCU , then update the MCU clock:

[0054]

[0055] The updated value of the MCU clock. The updated timestamp is still recorded as t in the program. MCU ;

[0056] Step 1-3: Synchronize the IMU data and vision data with the MCU’s clock via timestamps.

[0057] Step 1-3-1: Synchronize IMU data and visual data. Assume that IMU data and visual data are respectively accompanied by timestamps t IMU and t VIS ;

[0058] Similar to the principle of step 1-2-2, convert the timestamps of IMU data and visual data to the timestamps of MCU clock:

[0059] IMU data synchronization:

[0060] Calculate the synchronization error between IMU and MCU Then update the timestamp of the IMU data:

[0061]

[0062] The updated timestamp is still recorded as t in the program. IMU ;

[0063] Visual Data Synchronization:

[0064] Calculate the synchronization deviation between the vision sensor and the MCU Then update the timestamp of the visual data:

[0065]

[0066] The updated timestamp is still recorded as t in the program. VIS ;

[0067] Step 2: The MCU timestamps all sensor data and passes the timestamped sensor data to the GPU. The GPU uses the timestamp to perform data interpolation on the sensor data and handle data latency.

[0068] When executing step 2, the specific steps include:

[0069] Step 2-1: Before transmitting the sensor data to the GPU, the MCU timestamps each data frame of the sensor data;

[0070] Step 2-2: The GPU interpolates the sensor data and performs delay compensation.

[0071] The GPU interpolates according to the following formula:

[0072]

[0073] Among them, t GPU is the local time of the GPU, d sensor is the sensor data, d sensor (t next ) and d sensor (t prev ) is the timestamp t next and t prev The corresponding data, t next and t prev Representing the next moment and the previous moment respectively, the interpolation process enables the GPU to use appropriate data for processing at different timestamps.

[0074] Step 3: Use the particle filter algorithm to fuse the sensor data. Specifically, the MCU is responsible for initializing the particles and completing the prediction step; the GPU is responsible for accelerating the update and resampling steps;

[0075] When executing step 3, the particle filter algorithm includes initializing particles, predicting, accelerating, updating, and resampling steps:

[0076] Initialize particle step: MCU initializes the particle set, each particle represents a possible state;

[0077] Prediction step: MCU updates the particle state according to the acceleration and angular velocity in the IMU data. The prediction formula is as follows:

[0078] x k =f(x k-1 ,u k )+w k ;

[0079] Among them, x k is the state vector at the kth moment, which includes position, velocity, and attitude; f(x k-1 ,u k ) is the state transition model, u k is the control input, including acceleration and angular velocity; w k is the process noise;

[0080] Accelerated update step: The GPU updates the weight of each particle based on the visual data. The formula for calculating particle weight is as follows:

[0081]

[0082] Among them, z k is the observation value at the kth moment, specifically the image feature in the visual data; is the predicted value of the state of particle i, h is the function mapping from the particle state to the observation space; R -1 It is the inverse of the observation noise covariance matrix, which is used to measure the accuracy of the observation. The smaller the value, the smaller the noise of the observation data and the greater the weight. is the weight of the i-th particle at the k-th moment; represents the transpose of the difference between the observed value and the predicted value of the particle state, representing the residual; T represents the transpose operation;

[0083] Resampling step: The GPU resamples the particle set according to the particle weight, and selects particles whose weight reaches the expected value for further state estimation. In this embodiment, particles with high weight are selected for further state estimation.

[0084] The particle weight is calculated based on the difference between the particle's predicted state and the actual observed data. The larger the weight value, the closer the particle's state is to the actual observed data, and vice versa. The exponential part of the formula for calculating particle weights represents the relationship between the residual and the observation noise, that is, the smaller the residual, the greater the weight.

[0085] Step 4: GPU uses principal component analysis (PCA) to reduce the dimension of the data generated by the particle filter algorithm;

[0086] The algorithm formula of the principal component analysis (PCA) is as follows:

[0087] A=YV;

[0088] Among them, A is the data after dimensionality reduction, Y is the original data matrix, and V is the transformation matrix of the principal component analysis method, which consists of the eigenvectors of the data covariance matrix;

[0089] In this embodiment, Y represents the raw data set of all sensors, each row represents a data sample, and each column represents a feature (such as the feature of position, speed, IMU data, and visual data). For example, assuming there are 4 samples (data points) and each sample has 5 features, then Y is a 4×5 matrix.

[0090] The raw data matrix includes sensor data:

[0091] IMU data: including acceleration, angular velocity, etc.;

[0092] Visual data: including image features (such as corners, textures, depth information, etc.);

[0093] GNSS data: including longitude, latitude, altitude, etc.

[0094] In this embodiment, the sensor data may be pre-processed (eg, normalized, denoised, time aligned, etc.) before forming the matrix Y.

[0095] V consists of the eigenvalues ​​and eigenvectors of the data covariance matrix. Each eigenvector is a new axis in the original data space, and all eigenvectors form a new coordinate system.

[0096] Step 5: MCU outputs the particle weight, position and attitude state after fusion processing.

[0097] The MCU of the present invention serves as the main control unit, responsible for controlling the overall workflow of the system and managing external sensor data collection, while the GPU has a powerful parallel computing capability, suitable for accelerating complex computing tasks (such as weight calculation and resampling in particle filtering). Through the synchronization of the MCU and the GPU, the respective hardware advantages can be fully utilized to optimize data processing efficiency.

[0098] Through precise timestamp synchronization, the MCU and GPU can eliminate data deviations caused by hardware clock drift or other delays, providing more accurate fusion results. Especially when multiple sensor data (IMU, vision, GNSS) are involved, precise synchronization of data is a prerequisite for ensuring the effectiveness of the fusion algorithm.

[0099] In dynamic environments, such as mobile robots or self-driving cars, time synchronization is particularly critical. The present invention uses the synchronization mechanism of MCU and GPU to effectively synchronize the timing of different sensors in high-speed motion, ensuring real-time and high-precision state estimation.

[0100] The multi-sensor fusion method of an inertial navigation device described in the present invention solves the technical problem of using the collaborative working mode of MCU and GPU to effectively reduce the influence of time delay on the particle filter algorithm and improve the positioning accuracy. The present invention can efficiently process each stage of particle filtering by maximizing the respective advantages of MCU and GPU. The collaborative design of MCU and GPU hardware significantly improves the overall processing power of the system. By using GNSS timestamp to calibrate the MCU clock, IMU and visual data are synchronized with MCU time, which can effectively reduce the influence of time delay on the particle filter algorithm and improve positioning accuracy. In addition, the GPU performs interpolation processing according to the timestamp, further improving the timeliness and consistency of the data. By introducing GPU to accelerate the weight calculation and resampling of particle filtering, the calculation speed can be greatly improved, the delay can be reduced, and it is suitable for systems with high real-time requirements. PCA dimensionality reduction is performed by GPU to reduce the processing complexity of high-dimensional data and significantly improve data processing efficiency. PCA can remove redundant information, so that the system can focus on important features and improve computing efficiency.

Claims

1. A multi-sensor fusion method for an inertial navigation device, characterized in that: The steps include: Step 1: MCU collects sensor data from IMU sensor, vision sensor and GNSS receiver, including IMU data, vision data and GNSS data; Calibrate the local clock of the MCU using the timestamp of the GNSS data; Use the timestamp of the GNSS data to adjust the time of the IMU data and vision data so that they are aligned with the local clock of the MCU; Step 2: The MCU timestamps all sensor data and passes the timestamped sensor data to the GPU. The GPU uses the timestamp to perform data interpolation on the sensor data and handle data latency. Step 3: Use the particle filter algorithm to fuse the sensor data. Specifically, the MCU is responsible for initializing the particles and completing the prediction step; the GPU is responsible for accelerating the update and resampling steps; Step 4: GPU uses principal component analysis to reduce the dimension of the data generated by the particle filter algorithm; Step 5: MCU outputs the particle weight, position and attitude state after fusion processing.

2. The multi-sensor fusion method of an inertial navigation device as claimed in claim 1, characterized in that: When executing step 1, the specific steps include: Step 1-1: MCU collects IMU data, visual data and GNSS data from IMU sensor, visual sensor and GNSS receiver respectively; Step 1-2: MCU synchronizes the MCU clock to the time of GNSS data through the timestamp attached to the GNSS data; Step 1-3: Synchronize the IMU data and vision data with the MCU’s clock via timestamps.

3. The multi-sensor fusion method of an inertial navigation device as claimed in claim 1, characterized in that: When executing step 2, the specific steps include: Step 2-1: Before transmitting the sensor data to the GPU, the MCU timestamps each data frame of the sensor data; Step 2-2: The GPU interpolates the sensor data and performs delay compensation.

4. The multi-sensor fusion method of an inertial navigation device as claimed in claim 1, characterized in that: When executing step 3, the particle filter algorithm includes initializing particles, predicting, accelerating, updating, and resampling steps: Initialize particle step: MCU initializes the particle set, each particle represents a possible state; Prediction step: MCU updates the particle state according to the acceleration and angular velocity in the IMU data. The prediction formula is as follows: x k =f(x k-1 ,u k )+w k ; Among them, x k is the state vector at the kth moment, which includes position, velocity, and attitude; f(x k-1 ,u k ) is the state transition model, u k is the control input, including acceleration and angular velocity; w k is the process noise; Accelerated update step: The GPU updates the weight of each particle based on the visual data. The formula for calculating particle weight is as follows: Among them, z k is the observation value at the kth moment, specifically the image feature in the visual data; is the predicted value of the state of particle i, h is the function mapping from the particle state to the observation space; R -1 It is the inverse of the observation noise covariance matrix, which is used to measure the accuracy of the observation. The smaller the value, the smaller the noise of the observation data and the greater the weight. is the weight of the i-th particle at the k-th moment; represents the transpose of the difference between the observed value and the predicted value of the particle state, representing the residual; T represents the transpose operation; Resampling step: The GPU resamples the particle set according to the particle weights and selects particles whose weights reach the expected value for further state estimation.

5. The multi-sensor fusion method of an inertial navigation device as claimed in claim 1, characterized in that: The algorithm formula of the principal component analysis method is as follows: A=YV; Among them, A is the data after dimensionality reduction, Y is the original data matrix, V is the transformation matrix of the principal component analysis method, which is composed of the eigenvectors of the data covariance matrix; the original data matrix includes sensor data.

Images