CN-122015808-A - Multi-sensor height fusion estimation method and system

CN122015808ACN 122015808 ACN122015808 ACN 122015808ACN-122015808-A

Abstract

The invention discloses a multi-sensor height fusion estimation method and system, and relates to the technical field of unmanned aerial vehicle navigation and positioning. The method comprises the steps of preprocessing original data of an IMU, a GNSS and a barometer, constructing a speed reconstruction value by utilizing a high derivative, constructing a high reconstruction value by utilizing a speed integral, constructing short-time high confidence reference data by utilizing an IMU acceleration integral and a barometer height difference as a judgment basis, performing comparison sequence fault diagnosis on the GNSS data, executing a hierarchical restoration strategy, outputting a GNSS effective height, adjusting a state estimation filter weight according to a confidence level by taking the GNSS effective height as a reference, estimating and correcting deviation of the barometer, and performing navigation calculation by combining the IMU data and second-order kinematic extrapolation compensation. The invention effectively solves the problems of low accuracy, discontinuous data and hysteresis of the height estimation under the condition of low-cost hardware.

Inventors

CHEN ZHAOBING
JIANG YUANQING
Lian Aoxiang
LI ZEQING

Assignees

杭州云箭智融信息技术有限公司

Dates

Publication Date: 20260512
Application Date: 20260228

Claims (9)

1. The multi-sensor height fusion estimation method is characterized by comprising the following steps of: acquiring original observation data of an IMU, a GNSS and a barometer, and preprocessing to obtain preprocessed IMU acceleration data, GNSS height data, GNSS speed data and barometer height data; The method comprises the steps of constructing a speed reconstruction value by utilizing the derivative of the GNSS altitude data and constructing the altitude reconstruction value by utilizing the integral of the GNSS speed data; The pre-processed IMU acceleration and barometer height data are used as judgment basis to respectively construct short-time high confidence reference data of speed and height, and the GNSS data are subjected to sequential fault diagnosis and repair, wherein when an original observed value is diagnosed to be abnormal based on a comparison result of the judgment basis, the original observed value is subjected to reconstruction repair by using a corresponding reconstruction value, and GNSS effective height is output through self-adaptive switching according to a GNSS positioning mode; constructing a state estimation filter, taking the GNSS effective height as a reference observation source, and carrying out deviation estimation and correction on the barometer height data to obtain corrected barometer height; and adaptively selecting the GNSS effective height or the corrected barometer height as a fusion observation value, inputting the fusion observation value into a navigation fusion algorithm for resolving by combining the preprocessed IMU acceleration data, and outputting the final fusion height.
2. The multi-sensor highly fusion estimation method of claim 1, wherein preprocessing raw observation data comprises: Zero offset error compensation and scale factor error compensation are carried out on IMU original observation data based on preset parameters; Performing real-time temperature compensation on the IMU and the barometer according to the temperature characteristic curve of the sensor; and integrating the coordinate systems of the sensors into a flight control coordinate system, performing downsampling by adopting an equal interval averaging method, and outputting processed acceleration data, GNSS height data, GNSS speed data and barometer height data.
3. The multi-sensor altitude fusion estimation method according to claim 2, wherein the constructing short-time high confidence reference data of speed and altitude by using the preprocessed IMU acceleration and barometer altitude data respectively includes: performing time integration by utilizing the preprocessed IMU acceleration to construct a short-time reference speed; And constructing short-time reference height variation by utilizing the differential value of the barometer height data.
4. The multi-sensor altitude fusion estimation method of claim 3, wherein said sequential fault diagnosis and repair of GNSS data is performed according to the following logic: The method comprises the steps of reconstructing and repairing speed, namely constructing a speed reconstruction value by using a time derivative of GNSS altitude data, obtaining an average speed by using a GNSS original speed in GNSS original observation data, respectively performing difference comparison on the average speed and the speed reconstruction value and the short-time reference speed by using the short-time reference speed as a judgment standard, and judging that the GNSS original speed has faults and resetting the GNSS original speed by using the speed reconstruction value when the deviation between the average speed and the reference speed is larger than a first preset threshold value and the deviation between the speed reconstruction value and the short-time reference speed is smaller than a second preset threshold value, so as to obtain the repaired GNSS speed; the method comprises the steps of reconstructing and repairing the height, namely performing time integration by using the repaired GNSS speed to construct a height reconstruction value increment, calculating the height increment by using the GNSS original height in GNSS original observed data, performing difference comparison on the height increment and the height integration increment respectively with the short-time reference height variation by using the short-time reference height variation as a judgment standard, and executing a hierarchical repairing strategy according to the comparison result to obtain the repaired GNSS height.
5. The multi-sensor highly fusion estimation method according to claim 4, wherein the performing a hierarchical repair strategy according to the comparison result comprises: when the deviation between the height increment and the short-time reference height variation is smaller than a third preset threshold value, judging that the GNSS original height is normal, and directly outputting the GNSS original height as the repaired GNSS height; When the deviation between the height increment and the short-time reference height variation is larger than a third preset threshold value and the deviation between the height integral increment and the short-time reference height variation is smaller than a fourth preset threshold value, judging that the GNSS original height has serious faults, resetting the current GNSS height state of the system to a reconstruction value corresponding to the height integral increment, recording the difference value between the original height and the reconstruction value as a net difference, and continuously outputting the GNSS original height after subtracting the net difference as the repaired GNSS height in a subsequent observation period until the next fault reset or system reset; And when the two conditions are not met or the deviation is in the middle range, adopting a complementary filtering algorithm to fuse the reconstruction value corresponding to the height integral increment and the GNSS original height, and generating a smoothed height as the repaired GNSS height.
6. The multi-sensor altitude mixture fusion estimation method according to claim 1, wherein said outputting the GNSS active altitude through adaptive switching comprises: judging the current positioning mode of the GNSS in real time; if the GNSS is in the RTK mode, selecting and outputting the GNSS original altitude as the GNSS effective altitude; And if the GNSS is in the non-RTK mode, selecting and outputting the repaired GNSS height obtained through the sequential fault diagnosis and repair as the GNSS effective height.
7. The multi-sensor altitude fusion estimation method of claim 1, wherein said performing bias estimation and correction on barometer altitude data comprises: Setting a barometer deviation value as a state quantity to be estimated in the state estimation filter; Dividing the current calibration scene into different confidence levels according to the positioning mode of the current GNSS data, the observed noise covariance and the fault diagnosis result; Setting a corresponding observed noise covariance matrix parameter for each confidence level, adjusting the input weight of the state estimation filter by using the parameter, and updating the estimation result of the barometer deviation value by taking the GNSS effective height as an observed quantity; subtracting the estimated barometer deviation value from the preprocessed barometer height data to obtain the corrected barometer height.
8. The multi-sensor altitude fusion estimation method of claim 1, wherein the navigation fusion algorithm further comprises a dynamic delay compensation step comprising: collecting a fusion observation value sequence and a system prediction state sequence of the past N frames; Fitting a residual error between the fusion observed value sequence and a system prediction state sequence by using a least square method, and estimating a time delay error between the fusion observed value sequence and the system prediction state sequence in real time; Based on the time delay error, the preprocessed IMU acceleration is utilized to perform kinematic extrapolation compensation on a hysteresis calculation result output by a navigation fusion algorithm, and the fusion height at the current moment is generated.
9. A multi-sensor height fusion estimation system for implementing the method of any one of claims 1 to 8, the system comprising: The data acquisition and preprocessing module is configured to acquire and preprocess original observation data of the IMU, the GNSS and the barometer; The GNSS data processing module is configured to execute mutual reconstruction and restoration of data and comprises a speed reconstruction value constructed by utilizing the derivative of the GNSS height, a height reconstruction value constructed by utilizing the integral of the GNSS speed, short-time high-confidence reference data constructed by utilizing the preprocessed IMU acceleration and barometer height data and taking the three-party comparison logic as a judgment basis, and when the original observation value is diagnosed to be abnormal, resetting and restoring the original observation value by utilizing the corresponding reconstruction value, executing a hierarchical restoration strategy comprising direct output, resetting and deducting a net difference or complementary filtering according to the deviation degree, and adaptively switching and outputting the GNSS effective height of a single signal flow according to the GNSS positioning mode; The barometer data correction module is configured to construct a state estimation filter, take the GNSS effective height as a reference observation source, and perform deviation estimation and correction on the barometer height data to obtain corrected barometer height; and the height fusion estimation module is configured to adaptively select the GNSS effective height or the corrected barometer height as a fusion observation value, and combine the preprocessed IMU acceleration input navigation fusion algorithm to perform calculation, and output the final fusion height.

Description

Multi-sensor height fusion estimation method and system Technical Field The invention belongs to the technical field of unmanned aerial vehicle navigation positioning, and particularly relates to a multi-sensor height fusion estimation method and system. Background The unmanned aerial vehicle's navigation system provides necessary status data for the flight, and compared with other motion carriers, unmanned aerial vehicle has extremely high requirement to the navigation location of altitude direction (Z axle). Currently, sensors involved in fixed altitude in navigation systems mainly include Inertial Measurement Units (IMUs), global satellite navigation systems (GNSS), and barometers. In the prior art, each sensor has the limitations that GNSS can provide absolute positions, but is influenced by satellite geometric structures, vertical direction precision is poor, shielding and multipath effect interference are easy to occur, measured value jump or unreliability is caused, IMU integrates based on Newton's second law, although short-time precision is high and is not influenced by the outside, divergence errors accumulated over time exist, barometer can provide barometric pressure height, but is easy to be influenced by temperature and outside airflow, and zero offset drift which changes over time exists. Conventional altitude estimation strategies typically employ a simple GNSS/IMU/barometer combination scheme. In order to improve the precision, a part of schemes adopt multi-sensor depth fusion, so that mutual error compensation of GNSS and barometer is allowed, but the method is difficult to cope with severe abnormal observation of the sensor and is limited by inherent deviation of observed data. The other scheme is to add additional sensors such as laser radar, millimeter wave radar or binocular vision, and the like, and although the precision is improved, the load, the power consumption and the manufacturing cost of the unmanned aerial vehicle are obviously increased, and the method is not suitable for the miniature unmanned aerial vehicle sensitive to the cost and the weight. In addition, in the prior art, when processing GNSS faults, a simple threshold value is generally adopted to judge and reject abnormal data. This approach can cause the system to lose absolute observation updates during data culling, relying only on IMU integration, and easily causing state divergence. Therefore, how to mine potential association of each sensor data through an algorithm under low-cost hardware configuration to realize high-precision and high-reliability highly-fused estimation is a difficulty to be solved in the prior art. Disclosure of Invention The invention aims to provide a multi-sensor height fusion estimation method and a multi-sensor height fusion estimation system aiming at the defects of the prior art, and solves the problems of low precision, high hardware cost and insufficient reliability of the height fusion system in the prior art through a multi-observation source reconstruction, restoration and correction mechanism. The multi-sensor height fusion estimation method comprises the following steps: acquiring original observation data of an IMU, a GNSS and a barometer, and preprocessing to obtain preprocessed IMU acceleration data, GNSS height data, GNSS speed data and barometer height data; The method comprises the steps of constructing a speed reconstruction value by utilizing the derivative of the GNSS altitude data and constructing the altitude reconstruction value by utilizing the integral of the GNSS speed data; The pre-processed IMU acceleration and barometer height data are used as judgment basis to respectively construct short-time high confidence reference data of speed and height, and the GNSS data are subjected to sequential fault diagnosis and repair, wherein when an original observed value is diagnosed to be abnormal based on a comparison result of the judgment basis, the original observed value is subjected to reconstruction repair by using a corresponding reconstruction value, and GNSS effective height is output through self-adaptive switching according to a GNSS positioning mode; constructing a state estimation filter, taking the GNSS effective height as a reference observation source, and carrying out deviation estimation and correction on the barometer height data to obtain corrected barometer height; and adaptively selecting the GNSS effective height or the corrected barometer height as a fusion observation value, inputting the fusion observation value into a navigation fusion algorithm for resolving by combining the preprocessed IMU acceleration data, and outputting the final fusion height. The invention also provides a multi-sensor height fusion estimation system which comprises a data acquisition and preprocessing module, a GNSS data processing module, an air pressure count correction module and a height fusion estimation module and is used for executing the method. Compared with the