Unmanned aerial vehicle autonomous navigation method based on fusion of health assessment and hierarchical fault tolerance

Abstract

The present application relates to the technical field of unmanned aerial vehicle autonomous navigation, and discloses an unmanned aerial vehicle autonomous navigation method based on measurement health assessment and hierarchical fault tolerance fusion, comprising the following steps: S1, multi-source navigation measurement data acquisition and preprocessing; S2, state propagation and measurement prediction based on inertial navigation; S3, measurement health evaluation index construction and health degree calculation; S4, measurement health state grading determination; S5, differential fault tolerance fusion processing; S6, abnormal measurement recovery control and closed-loop optimization. Through the technical scheme of constructing a four-dimensional measurement health evaluation index system including residual amplitude, residual change rate, time continuity and cross-sensor consistency, and calculating the comprehensive health degree through a weighted summation algorithm, the technical effect of comprehensively and accurately evaluating the measurement state of each sensor is achieved, and the problems of being unable to identify potential abnormalities such as slow drift and short-term interruption of the measurement, and one-sided and inaccurate measurement health assessment are solved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for unmanned aerial vehicles (UAVs), specifically to an autonomous navigation method for UAVs based on the fusion of measurement health assessment and hierarchical fault tolerance. Background Technology

[0002] Autonomous navigation systems for unmanned aerial vehicles (UAVs) are the core support for UAVs to achieve autonomous flight and complete various tasks. They primarily collect measurement data through multi-source navigation sensors, which are then fused and processed to output navigation parameters such as the UAV's position, velocity, and attitude, providing precise data for flight control. Currently, commonly used multi-source navigation sensors for UAVs include inertial measurement units (IMUs), GNSS modules, barometers, magnetic sensors, visual sensors, and optical flow sensors. These sensors work together to meet the navigation needs of different flight scenarios and are widely used in low-altitude reconnaissance, material transportation, surveying and exploration, and other fields. As UAV applications extend to complex environments such as urban low-altitude areas and mountainous canyons, higher demands are placed on the reliability of navigation systems and the accuracy of measurement status assessment.

[0003] In existing UAV autonomous navigation technologies, the evaluation of the measurement status of multi-source navigation sensors often adopts a single-residual discrimination method. This involves calculating the single-residual between the measured data and the predicted data and comparing it with a fixed threshold to determine whether the measurement is abnormal. However, this discrimination method, which relies solely on instantaneous amplitude, has inherent drawbacks: First, it cannot identify slow drifts in measured values, such as gradual deviations caused by sensor temperature drift or aging. Such drifts may always be less than the threshold in the single residual and thus be misjudged as normal. Second, it is difficult to detect short-term interruptions or intermittent failures, such as packet loss caused by signal blockage. After the interruption is recovered, the single residual may quickly return to normal, but data loss has already occurred during that period. Third, it lacks cross-validation of multi-source information. Anomalies of a single sensor may go undetected because the residual does not exceed the limit, leading to false alarms or missed detections.

[0004] The aforementioned deficiencies lead to biased and inaccurate measurement health assessments, which in turn affect the reliability of subsequent navigation fusion. If abnormal measurements are not identified and handled promptly and appropriately, they will directly contaminate the fusion results, reducing navigation accuracy and stability. Furthermore, existing methods lack a smooth transition mechanism for measurements after anomaly recovery, which can easily cause abrupt changes in the fusion output, further exacerbating system fluctuations. Therefore, how to achieve accurate assessment, hierarchical fault tolerance, and smooth recovery of multi-source sensor measurements in complex environments has become a critical issue that urgently needs to be addressed in UAV autonomous navigation technology. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides an autonomous navigation method for unmanned aerial vehicles (UAVs) based on measurement health assessment and hierarchical fault tolerance fusion. This method solves the technical problems caused by relying solely on single residuals to determine measurement status in existing technologies, such as slow drift misses, short-term interruption failures, and lack of cross-validation. It achieves accurate assessment, hierarchical handling, smooth recovery, and closed-loop optimization of measurement status from multiple sources of sensors.

[0006] To achieve the above objectives, the present invention provides the following technical solution: an autonomous navigation method for unmanned aerial vehicles (UAVs) based on measurement health assessment and hierarchical fault tolerance fusion, characterized by comprising the following steps:

[0007] An autonomous navigation method for unmanned aerial vehicles (UAVs) based on measurement health assessment and hierarchical fault tolerance fusion includes the following steps:

[0008] S1, Multi-source navigation measurement data acquisition and preprocessing: Acquire measurement data from UAV multi-source navigation sensors, and obtain preprocessed measurement data after denoising, synchronization alignment and extreme outlier filtering;

[0009] S2, State propagation and measurement prediction based on inertial navigation: The inertial navigation algorithm is used to propagate the state of the inertial measurement unit data after S1 preprocessing to obtain the navigation state estimate and error covariance matrix, and the measurement prediction value is calculated by combining the measurement models of each auxiliary sensor.

[0010] S3, Construction of Measurement Health Evaluation Indicators and Calculation of Health: Based on the navigation state estimate and measurement prediction values ​​in S2 and the preprocessed measurement data in S1, a four-dimensional measurement health evaluation index system including residual amplitude, residual change rate, time continuity, and cross-sensor consistency is constructed, and a multi-index fusion algorithm is used to calculate the comprehensive health of each auxiliary sensor measurement.

[0011] S4, Health Status Grading Determination: Based on the comprehensive health status in S3, three threshold levels are set to divide the measurement status into three levels: normal, suspicious, and faulty.

[0012] S5, Differentiated fault-tolerant fusion processing: Based on the classification results of S4, normal measurements are fused with conventional fusion weights, suspicious measurements are fused with reduced weights based on comprehensive health, and fault measurements are isolated and replaced for updates, thus obtaining the navigation state fusion result;

[0013] S6, Abnormal Measurement Recovery Control and Closed-Loop Optimization: Monitor abnormal measurements and identify the recovery status through continuous compliance judgment, gradually restore its fusion weight, and feed back the fusion result of S5 to S2 as the initial value for the state propagation of the next cycle. Feed back the recovery control result to S3 for adaptive adjustment of the evaluation index weight and judgment threshold to achieve closed-loop optimization.

[0014] Furthermore, in S3, constructing a four-dimensional health evaluation index system and calculating the comprehensive health score includes:

[0015] Calculate the residual magnitude index: ,in These are actual measured values. This is a predicted value based on measurement.

[0016] Calculate the residual change rate index: ,in Let x be the residual amplitude of the k-th sampling period. The sampling period;

[0017] Calculate the time continuity index: ,in The number of valid measurements within a preset time period. The total number of samples; the time continuity index is also used to characterize the frequency and duration of measurement interruptions;

[0018] Calculate cross-sensor consistency metrics: ,in The values ​​are associated with sensor measurements, where the associated sensors are either similar sensors or dissimilar sensors with physical coupling. This represents the maximum permissible consistency deviation.

[0019] After normalizing the four indicators, a weighted summation is used to calculate the overall health score:

[0020] ;

[0021] in , , , Let be the weighting coefficient, satisfying .

[0022] Furthermore, in S4, the setting of the three-level thresholds includes:

[0023] The first threshold α and the second threshold β are adaptively calibrated based on the sensor type and navigation accuracy requirements to meet the following conditions. ;

[0024] When overall health When, it is judged as a normal state; when When, it is judged as a suspicious state; when When this occurs, it is determined to be a fault state.

[0025] Furthermore, the three-level threshold is based on the real-time noise variance of the measurement data. Perform adaptive adjustments:

[0026] ;

[0027] in , As the initial threshold, , This is for adjusting the coefficient.

[0028] Furthermore, in S5, the weighted fusion of suspicious measurements includes:

[0029] Based on overall health Determine the fusion weights ;

[0030] Adjust the measurement matrix and measurement noise covariance matrix: , Substituting this into the Kalman filter update equation, we can achieve weighted fusion of suspicious measurements.

[0031] Furthermore, in S5, the alternative update of the fault measurement includes:

[0032] Alternative estimates are calculated using interpolation forecasting: ,in , The values ​​are the fused measurements from the two sampling periods prior to the fault.

[0033] The alternative estimates are used only to maintain the continuity of navigation output and are not involved in the fusion calculation.

[0034] Furthermore, in S6, the continuous compliance determination and recovery status identification includes:

[0035] Set the preset number of sampling periods When the overall health status of abnormal measurements is continuous Each sampling period satisfies At that time, it was determined that the measurement had returned to normal.

[0036] Furthermore, in S6, the progressive recovery fusion weights include:

[0037] Set preset recovery period The fusion weights for the restored measurements are calculated using a linearly increasing formula:

[0038] ;

[0039] in To restore the initial weights, For regular fusion weights, To determine when things return to normal.

[0040] Furthermore, the time continuity index is also used to characterize the frequency and duration of measurement interruptions. When the number of consecutive invalid measurements exceeds a preset threshold, it is determined to be an interruption event. The time continuity index is attenuated and corrected based on the interruption frequency and the duration of a single interruption.

[0041] ;

[0042] in The interrupt frequency per unit time. This is the attenuation coefficient.

[0043] This invention provides an autonomous navigation method for unmanned aerial vehicles (UAVs) based on the fusion of measurement health assessment and hierarchical fault tolerance. It has the following beneficial effects:

[0044] 1. This invention constructs a four-dimensional measurement health evaluation index system encompassing residual amplitude, residual change rate, temporal continuity, and cross-sensor consistency. It comprehensively characterizes the sensor measurement status from four perspectives: instantaneous deviation, trend change, temporal stability, and multi-source consistency. Specifically, the residual change rate effectively captures slow drifts that traditional methods cannot identify; temporal continuity accurately detects short-term interruptions and intermittent failures; and cross-sensor consistency eliminates false alarms or missed detections from a single sensor through cross-validation of multi-source information. Compared to existing one-sided evaluation methods that rely solely on single residual judgments, this invention achieves multi-dimensional and comprehensive perception of the measurement status, significantly improving the accuracy and reliability of health assessment.

[0045] 2. This invention classifies measurement states into three levels—normal, suspicious, and faulty—based on comprehensive health status and implements a differentiated fusion strategy: normal measurements use conventional fusion weights to fully leverage their high accuracy; suspicious measurements are adaptively weighted according to health status, suppressing interference while retaining valid information; faulty measurements are isolated and replaced with alternative estimates to maintain the continuity of navigation output. Compared to existing methods that use fixed thresholds to eliminate anomalies or uniform weight fusion, this invention achieves an optimal balance between accuracy and robustness through hierarchical processing, effectively curbing the contamination of navigation solutions by abnormal measurements.

[0046] 3. This invention introduces a continuous compliance judgment mechanism for abnormal measurement recovery, avoiding false recovery caused by single-point fluctuations. It employs a progressive recovery mechanism, gradually increasing the fusion weights within a preset recovery period, eliminating the impact of sudden weight changes during recovery on the navigation solution in traditional methods. Simultaneously, the fusion results and recovery control results are fed back to the inertial navigation state propagation and health calculation steps, respectively, enabling dynamic correction of the initial state propagation value and adaptive adjustment of the health assessment index weights and thresholds. This forms a closed-loop system of "assessment-grading-fusion-recovery-optimization," giving the navigation system dynamic adaptive capabilities and significantly improving navigation stability and environmental adaptability in complex dynamic scenarios. Attached Figure Description

[0047] Figure 1 This is a flowchart illustrating the steps of the UAV autonomous navigation method based on the fusion of measurement health assessment and hierarchical fault tolerance according to the present invention. Detailed Implementation

[0048] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0049] Please see the appendix Figure 1 This invention provides an autonomous navigation method for unmanned aerial vehicles (UAVs) based on measurement health assessment and hierarchical fault tolerance fusion, comprising the following steps:

[0050] S1, Multi-source navigation measurement data acquisition and preprocessing: Acquire measurement data from UAV multi-source navigation sensors, and obtain preprocessed measurement data after denoising, synchronization alignment and extreme outlier filtering.

[0051] Furthermore, the preprocessed measurement data includes the following steps:

[0052] The inertial measurement unit (IMU), GNSS module, barometer, magnetometer, vision sensor, and optical flow sensor onboard the UAV are used as multi-source navigation sensors. The IMU collects angular rate and linear acceleration data from the UAV; the GNSS module collects position and velocity data; the barometer collects altitude data; the magnetometer collects attitude angle data; the vision sensor collects visual feature point data; and the optical flow sensor collects relative velocity data. Noise reduction, synchronization alignment, and extreme outlier filtering are performed on each type of measurement data to remove extreme outliers caused by sensor malfunctions. The pre-processed multi-source navigation measurement data is then output.

[0053] Specifically, the denoising process employs the Kalman filter algorithm. Its core principle is to suppress inherent sensor noise through an iterative process of state estimation and observation updates, providing high-precision foundational data for subsequent inertial navigation state propagation and measurement prediction, thus ensuring the accuracy of state propagation in S2. The Kalman filter state update formula is:

[0054] ;

[0055] in, The filtered state estimate at time k is... The state prediction value at time k is based on the state at time k-1. The Kalman gain at time k, The actual measurement value of the sensor at time k. Let k be the measurement matrix at time k.

[0056] Synchronization alignment employs timestamp interpolation, its core function being to resolve the time discrepancy issue between data from sensors with different sampling frequencies. This prevents time mismatch from causing excessive deviations between subsequent measurement predictions and actual measurements. The principle is to align all data to a unified time node using linear interpolation based on the timestamps of each sensor's data. Specifically, using the IMU data timestamp as a benchmark, linear interpolation is performed on the measurement data from other sensors to ensure that all sensor data have corresponding measurement values ​​at the same time point, providing time consistency assurance for subsequent fusion calculations.

[0057] The 3σ criterion is used for screening extreme outliers, and the formula is as follows:

[0058] ;

[0059] in The mean of the measurement data. The standard deviation of the measurement data is defined as the standard deviation. Data exceeding this range is considered extreme anomalies and is removed. This effectively prevents abnormal data caused by sensor failure from entering the subsequent processing flow, ensuring the reliability of the preprocessed data and providing effective data support for the state propagation of S2 and the health calculation of S3.

[0060] S2, based on state propagation and measurement prediction of inertial navigation, uses an inertial navigation algorithm to propagate the state of the inertial measurement unit data preprocessed in S1, obtains the navigation state estimate and error covariance matrix, and calculates the measurement prediction value by combining the measurement models of each auxiliary sensor.

[0061] Furthermore, the calculation of the predicted measurement values ​​based on the measurement models of each auxiliary sensor includes the following steps:

[0062] The inertial measurement unit (IMU) data after S1 preprocessing is extracted and input into the inertial navigation algorithm. The inertial navigation algorithm is used to perform state propagation calculations on the IMU data, outputting the real-time navigation state estimate of the UAV and the corresponding error covariance matrix. The navigation state estimate includes position, velocity, and attitude angle. The navigation state estimate is extracted and combined with the measurement models of GNSS, barometer, magnetic sensor, visual sensor, and optical flow sensor. Based on the measurement dimensions and physical characteristics of each auxiliary sensor, the measurement prediction values ​​of each auxiliary sensor are calculated.

[0063] Specifically, the inertial navigation algorithm adopts the strapdown inertial navigation algorithm, whose core principle is based on Newton's laws of motion. By integrating the IMU angular rate and linear acceleration data after S1 preprocessing, the real-time propagation of the UAV's navigation status is realized, providing a benchmark for subsequent measurement prediction and health assessment.

[0064] The position propagation formula is:

[0065] ;

[0066] The velocity propagation formula is:

[0067] ;

[0068] The formula for attitude angle propagation is:

[0069] ;

[0070] in, Let k be the position vector at time k (unit: m). Let k be the velocity vector at time k (unit: m / s). Let be the attitude angle vector at time k (unit: rad). This is the linear acceleration vector at time k-1 (unit: m / s²). This is the angular velocity vector at time k-1 (unit: rad / s). The sampling period is given in seconds. The propagation formula for the error covariance matrix is:

[0071] ;

[0072] in, Let k be the prediction error covariance matrix. Here is the state transition matrix. Let K be the error covariance matrix after filtering at time k-1. This is the process noise covariance matrix. The state transition matrix is ​​also shown. Based on the UAV kinematic model, the process noise covariance matrix is ​​constructed. Calibration is performed based on the noise characteristics of the IMU.

[0073] Each auxiliary sensor measurement model is tailored to its physical characteristics and corresponds one-to-one with the data types collected by each sensor in S1. The GNSS measurement model is as follows:

[0074] ;

[0075] in, This represents the actual GNSS measurement location. To estimate the position for navigation status, The noise is GNSS measurement noise, and its noise variance is estimated in real time based on the GNSS signal quality.

[0076] The barometric measurement model is as follows:

[0077] ;

[0078] in, The actual height measured by the barometer. Estimate altitude for navigation status. Noise from barometer measurement.

[0079] The magnetic sensor measurement model is as follows:

[0080] ;

[0081] in, For the actual measurement of attitude angle by the magnetic sensor, Estimate attitude angles for navigation state. To measure noise for magnetic sensors.

[0082] The visual sensor measurement model is constructed based on the projection relationship between the pixel coordinates of visual feature points and their three-dimensional spatial coordinates, while the optical flow sensor measurement model is constructed based on the relationship between pixel motion velocity and UAV motion velocity. The predicted values ​​calculated by the measurement models will serve as the core benchmark for health assessment in S3, used for comparison with the actual measurement data after preprocessing in S1, calculating residuals and various health evaluation indicators, and providing key data support for health calculation in S3.

[0083] S3, Measurement and Health Evaluation Index Construction and Health Calculation: Based on the navigation status estimate, measurement prediction value and preprocessed measurement data of S2, a four-dimensional measurement and health evaluation index system is constructed, and a multi-index fusion algorithm is used to calculate the comprehensive health of each auxiliary sensor measurement.

[0084] Furthermore, the calculation of the comprehensive health score measured by each auxiliary sensor using a multi-index fusion algorithm (preferably the weighted summation method in this embodiment) includes the following steps:

[0085] Obtain the navigation state estimate and predicted measurements from each auxiliary sensor output by S2, as well as the preprocessed actual measurement data from each auxiliary sensor output by S1. Construct a measurement health evaluation index system comprising four core dimensions: residual amplitude, residual rate of change, temporal continuity, and cross-sensor consistency, and calculate the value of each index. Normalize the four evaluation indices and adaptively adjust the weights of each index according to sensor type and navigation scenario requirements. Calculate the normalized four evaluation indices using a weighted summation algorithm, and output the comprehensive health score of each auxiliary sensor measurement. The comprehensive health score ranges from [value missing in original text]. .

[0086] The four-dimensional health evaluation index system includes: residual amplitude, residual change rate, temporal continuity, and cross-sensor consistency. The calculation of each index value involves the following steps:

[0087] Calculate the residual amplitude index: Calculate the residual between the actual measured value and the corresponding predicted value of each auxiliary sensor, and take the absolute value of the residual as the residual amplitude. The calculation formula is:

[0088] ;

[0089] in, Let be the residual amplitude of the i-th sensor. This represents the actual measured value of the i-th sensor S1 after preprocessing. Let be the predicted measurement value output by the i-th sensor S2. The normalization formula is:

[0090] ;

[0091] Let be the maximum permissible residual of the i-th sensor. This value can be calibrated according to the sensor's measurement range, accuracy class, and actual application scenario. After normalization, its value range is as follows: This is used to quantify the degree of deviation between the actual measurement and the predicted value. When the value is close to 0, it indicates that the measured value is highly consistent with the predicted value; when it is close to 1, it indicates that the deviation is close to the maximum allowable range.

[0092] Calculate the residual change rate index: Calculate the change in residual amplitude between two adjacent sampling periods to obtain the residual change rate. The calculation formula is:

[0093] ;

[0094] in, Let be the residual change rate of the i-th sensor. Let x be the residual amplitude of the k-th sampling period. The residual amplitude for the (k-1)th sampling period. The sampling period is the same as that in S2. The normalization formula is:

[0095] ;

[0096] This represents the maximum permissible residual change rate of the i-th sensor. This value reflects the maximum allowable rate of change in the sensor measurement per unit time, and its normalized value range is [value missing]. This indicator is used to capture abrupt changes in measurement bias. It can effectively identify slow drift phenomena that traditional single-residual discrimination cannot detect. This is because slow drift causes the residual change rate to remain positive but with a small amplitude. In a single residual, it may always be below the threshold and be misjudged as normal. The residual change rate indicator can detect such anomalies in a timely manner through the cumulative change trend.

[0097] Calculate the time continuity index: This involves statistically analyzing the duration of continuous effective measurements from each auxiliary sensor, as well as the frequency and duration of measurement interruptions. The basic formula for calculating the time continuity index is:

[0098] ;

[0099] in Let i be the time continuity index of the i-th sensor. The number of valid measurements within a preset time period. This represents the total number of samples within a preset time period. To further characterize the interruption features, when the number of consecutive invalid measurements exceeds a preset threshold (set to 3 times in this embodiment), it is determined to be an interruption event. The time continuity index is attenuated and corrected based on the interruption frequency and the duration of a single interruption. The correction formula is as follows:

[0100] ;

[0101] in The interrupt frequency per unit time. This is the attenuation coefficient (0 < λ < 1, taken as 0.5 in this embodiment). This index itself is already at... Within a certain range, this metric is used to measure the temporal stability of measurement data. When measurement interruptions are frequent or prolonged, this metric decreases accordingly, reflecting the instability of the sensor data. This metric can effectively detect short-term interruptions and intermittent failures that are difficult to identify using traditional methods.

[0102] Calculate the cross-sensor consistency index: Select measurement data from sensors of the same type or related to each other, and calculate their data consistency deviation. The related sensors are either similar sensors (e.g., comparison between dual-antenna GNSS sensors) or dissimilar sensors with physical coupling (e.g., position information calculated by GNSS and inertial navigation, attitude angle information calculated by a magnetic sensor and inertial navigation, altitude information calculated by a barometer and GNSS, etc.). The calculation formula is:

[0103] ; ;

[0104] in, The measurement deviation between the i-th sensor and the associated sensor j is... The actual measurement value after preprocessing of the associated sensor j S1 This represents the maximum permissible consistency deviation between the i-th sensor and its associated sensor j. This value is calibrated based on the physical coupling relationship between the sensors and the system accuracy requirements. As a cross-sensor consistency metric, this metric itself is already at... Within this range, no additional normalization is required; this is used to verify measurement reliability through multi-sensor comparison. When multiple correlated sensors exist, the smallest consistency index can be selected as the final index. Alternatively, a weighted average can be calculated for each consistency index, with the weights allocated based on the reliability of each associated sensor.

[0105] The comprehensive health score calculation formula (multi-indicator fusion algorithm) is as follows:

[0106] ;

[0107] in, The overall health status of the i-th sensor. , , , The weights of the four indicators are respectively, satisfying... The weights can be adaptively adjusted based on sensor type and navigation scenario requirements. For example, in scenarios with rapid dynamic changes, the weight of the residual change rate index can be appropriately increased. To enhance sensitivity to mutational anomalies; in scenarios with frequent signal obstruction, the weight of temporal continuity indicators can be increased. In scenarios with redundant multi-sensor configurations, the weight of cross-sensor consistency metrics can be increased. . The range of values ​​is The closer the value is to 1, the healthier the measurement status; the closer it is to 0, the worse the measurement status. This calculation result will directly serve as the core basis for determining the measurement health status in S4, achieving accurate quantitative grading of the measurement status.

[0108] S4 measures the health status classification and judgment. Based on the comprehensive health status of S3, three threshold levels are set to divide the measurement status into three levels: normal, suspicious, and faulty.

[0109] Furthermore, the division of measurement states includes the following steps:

[0110] Obtain the overall health status of each auxiliary sensor measured by S3; adaptively calibrate the three-level health status judgment thresholds according to sensor type and navigation accuracy requirements to ensure that the three thresholds are met. The measurement status is classified according to the comparison between the overall health status and the threshold: when the overall health status is ≥ α, it is judged as normal status; when β ≤ overall health status < α, it is judged as suspicious status; when the overall health status < β, it is judged as fault status; and the measurement status classification results of each auxiliary sensor are output.

[0111] Specifically, the calibration principle of the three-level threshold is based on the accuracy of each sensor in S1, navigation scenario requirements, and engineering practice experience. This achieves a refined division of measurement states, avoiding the coarseness of traditional binary judgments, and providing a clear hierarchical basis for the differentiated fault-tolerant fusion of S5. Specifically, the first threshold α ranges from 0.7 to 0.9, and the second threshold β ranges from 0.3 to 0.5, satisfying… α and β are both dimensionless coefficients.

[0112] For example, in urban low-altitude navigation scenarios, GNSS sensor calibration is α=0.8, β=0.4, when... When, it is judged as a normal state; when When, it is judged as a suspicious state; when When this occurs, it is determined to be a fault state.

[0113] Adaptive threshold calibration can be achieved through a combination of offline training and online adjustment. During the offline training phase, sensor measurement data from different scenarios are collected, and the initial threshold is determined through statistical analysis. , During the online adjustment phase, the noise variance of the measurement data is used as a basis. The dynamic threshold adjustment formula is as follows:

[0114] ;

[0115] in, , As the initial threshold, The real-time noise variance of the measurement data after S1 preprocessing. , To adjust the coefficient (in this embodiment) Take a value of 0.1 to 0.3. (Take a value of 0.1~0.3). When the noise variance increases, the α threshold is appropriately increased and the β threshold is appropriately decreased to reduce the interference of noise on the health status determination; when the noise variance decreases, the threshold reverts to its initial value. This allows the classification determination to adapt to the measurement characteristics under different scenarios and improves the accuracy of the determination.

[0116] The classification results output in this step will directly determine the fusion method of each sensor measurement in S5, realize the logic of corresponding handling according to classification, and provide the core decision-making basis for the differentiated fault-tolerant fusion of S5.

[0117] S5, differentiated fault-tolerant fusion processing, based on the hierarchical results of S4, performs routine fusion of normal measurements, downweighted fusion of suspicious measurements, isolation and replacement updates of faulty measurements to obtain the navigation state fusion result.

[0118] Furthermore, obtaining the navigation state fusion result includes the following steps:

[0119] The system acquires the measurement state classification results of S4, the inertial navigation state estimate of S2, and the preprocessed measurement data of S1. For measurements determined to be in a normal state, conventional fusion weights are used to fuse them with the inertial navigation state estimate, updating the UAV's navigation state estimate and error covariance matrix. For measurements determined to be in a suspicious state, the fusion weights are adaptively adjusted based on the overall health, reducing their fusion weights before fusing them with the inertial navigation state estimate. For measurements determined to be in a fault state, isolation processing is performed to prevent them from participating in navigation fusion. Simultaneously, based on the inertial navigation state propagation results and the fusion results of other normal or suspicious measurements, the navigation parameters corresponding to the faulty measurements are estimated by substitution. A Kalman filter-like algorithm is used to complete the fusion processing, updating the error covariance matrix in real time, and outputting the UAV navigation state fusion result.

[0120] The fusion process, which employs a Kalman filter-like algorithm to update the error covariance matrix in real time, includes the following steps:

[0121] Kalman filtering algorithms are selected, including one or more of Extended Kalman Filter (EKF) and Cubature Kalman Filter (CKF). The fusion results of normal measurements, the weighted fusion results of suspicious measurements, and the substitute estimation results of faulty measurements are input into the selected Kalman filtering algorithm. The input data are fused and calculated using the Kalman filtering algorithm, and the error covariance matrix is ​​updated in real time. The fused UAV navigation state estimate and the updated error covariance matrix are output.

[0122] Specifically, the core principle of differentiated fusion is to assign different fusion weights to measurements of different health states based on the measurement status classification results of S4, taking into account both navigation accuracy and output continuity. Its fusion logic is closely related to the output data of S1, S2, S3, and S4, and at the same time provides fusion result support for the closed-loop optimization of S6.

[0123] The fusion logic for each state is as follows:

[0124] Normal Measurement Fusion: Conventional Fusion Weights The fusion formula follows the Kalman filter update equation:

[0125] ;

[0126] ;

[0127] in, Let k be the estimated navigation state after fusion. The predicted state value at time k is output by S2. The Kalman gain at time k, These are the normal measurement values ​​after S1 preprocessing. For the measurement matrix, It is the identity matrix. Let k be the error covariance matrix after filtering. This represents the prediction error covariance matrix of the S2 output. Kalman gain. The calculation formula is:

[0128] ;

[0129] in This is to measure the noise covariance matrix. This approach fully utilizes the high precision of normal measurements to improve navigation accuracy.

[0130] Suspicious measurement fusion: The fusion weight is positively correlated with the overall health score calculated by S3, as shown in the formula:

[0131] ;

[0132] in, For suspicious measurement fusion weights, This represents the overall health status of suspicious measurements. The measurement matrix will be used during fusion. Covariance matrix of measurement noise Adjusted by weight, i.e.:

[0133] ;

[0134] Substituting this into the Kalman filter update equation, we achieve weighted fusion of suspicious measurements. When When the weights approach the α threshold, the weights are close to 1, and the correction effect of the measurements on the fusion results is close to normal; when When the weights approach the β threshold, their correction effect is significantly reduced, but some valid information is still retained. This approach reduces the anomalous impact of suspicious measurements while preserving their valid information, avoiding the information loss caused by simply eliminating suspicious measurements in traditional methods.

[0135] Fault Measurement Replacement Update: Replacement estimation is performed using interpolation prediction method, with the following formula:

[0136] ;

[0137] in, This is a substitute estimate for fault measurement. , These are the fused measurement values ​​from the two sampling periods prior to the fault. The substitute values ​​are used only to maintain navigation output continuity and are not involved in the fusion calculation. This interpolation method is based on the principle of linear extrapolation, providing continuous measurement estimates during a fault and avoiding navigation output interruptions due to missing measurements.

[0138] To more clearly illustrate the multi-sensor collaborative fusion process, we will take GNSS / INS integrated navigation as an example:

[0139] When the GNSS measurement health status is determined to be normal, the position and velocity information are fused with the INS solution using conventional fusion weights and Kalman filtering to update the navigation status.

[0140] When the health status of GNSS measurements is deemed questionable, it is based on its overall health status. Calculate the weighted fusion weight By adjusting the measurement matrix and measurement noise covariance matrix and then inputting them into the Kalman filter equation, the correction effect of GNSS on the navigation solution can be reduced.

[0141] When GNSS measurements fail, the GNSS is isolated and not involved in the fusion calculation. Simultaneously, an interpolation prediction method is used to calculate a replacement estimate to maintain navigation output continuity. In this situation, the navigation system relies solely on INS measurements and other normal measurements such as those from the barometer and magnetic sensor for fusion updates, ensuring uninterrupted navigation output.

[0142] Among Kalman filter algorithms, the Extended Kalman Filter (EKF) is suitable for nonlinear navigation systems. Its core is to linearize the nonlinear measurement model through Taylor expansion. The Cumulative Kalman Filter (CKF) improves the accuracy of nonlinear fusion by approximating the probability density function through capacitive points, and can be selected according to the complexity of the navigation system. The error covariance matrix update can reflect the uncertainty of the navigation state in real time. The output navigation state fusion result will be fed back to S6 as the core input for closed-loop optimization, and at the same time provide a benchmark for the abnormal measurement recovery control of S6.

[0143] S6, Abnormal Measurement Recovery Control and Closed-Loop Optimization, monitors abnormal measurements and identifies the recovery status through continuous compliance judgment, gradually restores its fusion, and feeds back the fusion result and recovery control result to S2 and S3 respectively to achieve closed-loop optimization.

[0144] Furthermore, monitoring abnormal measurements and identifying the recovery status through continuous compliance determination, and gradually restoring its fusion includes the following steps:

[0145] Continuous monitoring is performed on sensor measurements that are identified as suspicious or faulty by S4 to obtain their overall health status in real time; a preset number of sampling cycles is set. A continuous compliance judgment mechanism is adopted, which determines compliance when a certain abnormal measurement of overall health score is continuously... When a sampling period reaches the normal state threshold, the measurement is determined to have returned to normal; for measurements determined to have returned to normal, a preset recovery period is set. A gradual recovery mechanism is adopted to gradually increase the fusion weight within a preset recovery period; measurements that have returned to normal are reintegrated into the navigation fusion process.

[0146] Achieving closed-loop optimization includes the following steps:

[0147] Extract the UAV navigation state fusion result output from S5 and feed it back to S2 as the initial value for the inertial navigation state propagation in the next sampling period; extract the abnormal measurement recovery control result from S6 and feed it back to S3; based on the recovery control result, adaptively adjust the weight and judgment threshold of the measurement health evaluation index in S3; realize the organic coupling of measurement health assessment, state classification, graded handling, fault-tolerant fusion and recovery control.

[0148] Specifically, the core principle of anomaly measurement recovery and closed-loop optimization is to achieve dynamic self-adaptation of the navigation system. By iteratively correcting the parameters of the preceding steps through feedback, the navigation stability in complex scenarios is improved. Each link is closely connected with the S1-S5 process to form a complete closed loop.

[0149] Continuous compliance determination: Preset number of sampling periods (In this embodiment, 3 to 5 sampling periods are used), and the determination formula is:

[0150] ;

[0151] in, Let α be the overall health status measured abnormally at time t, and α be the normal state threshold set by S4. Only continuous Only when the health status meets the standard for each sampling period is it determined to have returned to normal, avoiding false recovery caused by single-point fluctuations and ensuring the reliability of the recovery determination. For example, when In this case, the health status needs to reach above the α threshold for three consecutive sampling periods before it can be judged as having returned to normal.

[0152] Gradual recovery mechanism: preset recovery period (Unit: sampling periods, 5-10 sampling periods in this embodiment), the formula for progressively increasing the fusion weight is:

[0153] ;

[0154] in, The fusion weights for restoring measurements at time t. To restore the initial weights (0.5~0.7 in this embodiment), For regular fusion weights, To determine the point at which normalcy is restored. During the recovery period. Internally, the fusion weights from linearly increasing to Afterward, the normal fusion weights are maintained. This avoids navigation solution shocks caused by sudden weight changes after measurement recovery, ensuring output smoothness. The recovered measurements will then re-participate in the S5 fusion process.

[0155] Closed-loop optimization feedback:

[0156] The fusion result is fed back to S2: it can correct the initial error of inertial navigation state propagation, as shown in the formula:

[0157] ;

[0158] in To correct the predicted state at time k, This represents the fused state at time k output by S5. By feeding the fused result back to the initial value of state propagation, the accumulation of inertial navigation integration error can be effectively suppressed, thus improving the measurement prediction accuracy of S2.

[0159] The recovery control result is fed back to S3: used for adaptive adjustment of the evaluation index weights and judgment thresholds. In this embodiment, the specific rules for adaptive adjustment are as follows:

[0160] Weighting adjustment: After the measurement recovers from the fault state to the normal state, the weight of the time continuity index should be appropriately increased in the initial stage. and cross-sensor consistency index weights Reduce the weight of the residual change rate index Prioritize verifying the stability and consistency of measurements. As the sampling period increases after recovery, gradually restore to the initial weight configuration.

[0161] Threshold adjustment: Based on the change in noise variance of the post-recovery measurements, the α and β thresholds are dynamically fine-tuned to make the health assessment more suitable for the post-recovery measurement characteristics.

[0162] This feedback mechanism enables the health assessment module to dynamically adjust its assessment strategy based on the measured recovery status, thereby improving the accuracy of assessments during the recovery phase.

[0163] The entire closed-loop process achieves organic coupling of steps S1 to S6 through iterative feedback, adjustment, and optimization, continuously corrects the prediction accuracy of S2 and the evaluation accuracy of S3, thereby improving the reliability of S4 hierarchical judgment and S5 fusion processing, forming a complete dynamic optimization system.

[0164] To verify the effectiveness of the technical solution of this invention, a low-altitude flight simulation scenario in a certain city was implemented, setting up short-term GNSS signal interruption and slow drift faults. The traditional single-result residual discrimination method was compared with this method. The simulation parameters were set as follows: UAV flight altitude 50-100m, flight speed 8-12m / s, GNSS sampling frequency 5Hz, IMU sampling frequency 100Hz, simulation duration 300 seconds. A short-term GNSS signal interruption (intermittent loss) was set from 60 to 90 seconds, and a slow drift (drift rate 0.5m / s) in GNSS position measurement was set from 150 to 200 seconds.

[0165] Simulation results show that:

[0166] During GNSS outages, traditional methods fail to identify measurement loss caused by the outage due to single residual discrimination, misjudging invalid measurements as normal and causing navigation errors to diverge rapidly. This method identifies outages in a timely manner through time continuity indicators (c_i decreases significantly within 3 sampling periods after the outage), and isolates GNSS during the fault period, using INS and other sensors such as barometers to maintain navigation output. The rate of navigation error accumulation is reduced by about 35% compared to traditional methods.

[0167] In GNSS slow drift scenarios, traditional methods fail to identify drift because the single residual is always below the threshold, resulting in contamination of the navigation solution. This method effectively captures the drift trend through the residual change rate index (the residual change rate is continuously positive and slowly increases), identifies GNSS as a suspicious state and performs weighted fusion, effectively suppressing the impact of drift contamination on the navigation solution.

[0168] During the GNSS recovery phase, traditional methods cause sudden changes in fusion weights during recovery, resulting in significant jumps in navigation output. This method avoids false recovery by using continuous compliance judgment (N_judge=3) and uses a gradual recovery mechanism (N_rec=8) to ensure a smooth transition of fusion weights, resulting in no sudden changes in navigation output and significantly improved output continuity.

[0169] The simulation results above verify the significant effect of this method on improving navigation accuracy and robustness in complex environments.

[0170] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for autonomous navigation of unmanned aerial vehicles (UAVs) based on the fusion of measurement health assessment and hierarchical fault tolerance, characterized in that, Includes the following steps: S1, Multi-source navigation measurement data acquisition and preprocessing: Acquire measurement data from UAV multi-source navigation sensors, and obtain preprocessed measurement data after denoising, synchronization alignment and extreme outlier filtering; S2, State propagation and measurement prediction based on inertial navigation: The inertial navigation algorithm is used to propagate the state of the inertial measurement unit data after S1 preprocessing to obtain the navigation state estimate and error covariance matrix, and the measurement prediction value is calculated by combining the measurement models of each auxiliary sensor. S3, Construction of Measurement Health Evaluation Indicators and Calculation of Health: Based on the navigation state estimate and measurement prediction values ​​in S2 and the preprocessed measurement data in S1, a four-dimensional measurement health evaluation index system including residual amplitude, residual change rate, time continuity, and cross-sensor consistency is constructed, and a multi-index fusion algorithm is used to calculate the comprehensive health of each auxiliary sensor measurement. The construction of a four-dimensional health evaluation index system and the calculation of comprehensive health include: Calculate the residual magnitude index: ,in These are actual measured values. This is a predicted value based on measurement. Calculate the residual change rate index: ,in Let x be the residual amplitude of the k-th sampling period. The sampling period; Calculate the time continuity index: ,in The number of valid measurements within a preset time period. The total number of samples; the time continuity index is also used to characterize the frequency and duration of measurement interruptions; Calculate cross-sensor consistency metrics: ,in The values ​​are associated with sensor measurements, where the associated sensors are either similar sensors or dissimilar sensors with physical coupling. This represents the maximum permissible consistency deviation. After normalizing the four indicators, a weighted summation is used to calculate the overall health score: ； in, The normalized residual magnitude index, The normalized residual change rate index This refers to the time continuity index. This refers to the cross-sensor consistency index; , , , Let be the weighting coefficient, satisfying ; S4, Measurement of Health Status Grading: Based on the comprehensive health level in S3, a three-level threshold is set to classify the measurement status into three levels: normal, suspicious, and faulty. The setting of the three-level threshold includes: The first threshold α and the second threshold β are adaptively calibrated based on the sensor type and navigation accuracy requirements to meet the following conditions. ; When overall health When, it is judged as a normal state; when When, it is judged as a suspicious state; when When this occurs, it is determined to be a fault state; S5, Differentiated fault-tolerant fusion processing: Based on the classification results of S4, normal measurements are fused with conventional fusion weights, suspicious measurements are fused with reduced weights based on comprehensive health, and fault measurements are isolated and replaced for updates, thus obtaining the navigation state fusion result; S6, Abnormal Measurement Recovery Control and Closed-Loop Optimization: Monitor abnormal measurements and identify the recovery status through continuous compliance judgment, gradually restore its fusion weight, and feed back the fusion result of S5 to S2 as the initial value for the state propagation of the next cycle. Feed back the recovery control result to S3 for adaptive adjustment of the evaluation index weight and judgment threshold to achieve closed-loop optimization. The continuous compliance determination and recovery status identification includes: Set the preset number of sampling periods When the overall health status of abnormal measurements is continuous Each sampling period satisfies At that time, it was determined that the measurement had returned to normal; among them, The overall health status measured abnormally at time t. The first threshold; Progressive recovery fusion weights include: Set preset recovery period The fusion weights for the restored measurements are calculated using a linearly increasing formula: ； in, The fusion weights for restoring measurements at time t. At the current sampling time, To restore the initial weights, For regular fusion weights, To preset the recovery period, To determine when things return to normal.

2. The UAV autonomous navigation method based on measurement health assessment and hierarchical fault tolerance fusion as described in claim 1, characterized in that, The three-level threshold is based on the real-time noise variance of the measurement data. Perform adaptive adjustments: ； in , As the initial threshold, , This is for adjusting the coefficient.

3. The UAV autonomous navigation method based on measurement health assessment and hierarchical fault tolerance fusion as described in claim 1, characterized in that, In S5, the weighted fusion of suspicious measurements includes: Based on overall health Determine the fusion weights ; Adjust the measurement matrix and measurement noise covariance matrix: , Substituting this into the Kalman filter update equation, we can achieve weighted fusion of suspicious measurements.

4. The UAV autonomous navigation method based on measurement health assessment and hierarchical fault tolerance fusion as described in claim 1, characterized in that, In S5, the alternative update for the fault measurement includes: Alternative estimates are calculated using interpolation forecasting: ,in , The values ​​are the fused measurements from the two sampling periods prior to the fault. The alternative estimates are used only to maintain the continuity of navigation output and are not involved in the fusion calculation.

5. The UAV autonomous navigation method based on measurement health assessment and hierarchical fault tolerance fusion according to claim 1, characterized in that, The time continuity index is also used to characterize the frequency and duration of measurement interruptions. When the number of consecutive invalid measurements exceeds a preset threshold, it is determined to be an interruption event. The time continuity index is attenuated and corrected based on the interruption frequency and the duration of a single interruption. ； in The interrupt frequency per unit time. This is the attenuation coefficient.

Images