CN115218927B - Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering - Google Patents
Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering Download PDFInfo
- Publication number
- CN115218927B CN115218927B CN202210892728.4A CN202210892728A CN115218927B CN 115218927 B CN115218927 B CN 115218927B CN 202210892728 A CN202210892728 A CN 202210892728A CN 115218927 B CN115218927 B CN 115218927B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- kalman filtering
- aerial vehicle
- imu sensor
- kalman
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 101
- 238000001514 detection method Methods 0.000 title claims abstract description 81
- 238000000034 method Methods 0.000 claims abstract description 63
- 238000005259 measurement Methods 0.000 claims description 35
- 230000008569 process Effects 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000003044 adaptive effect Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000007704 transition Effects 0.000 claims description 3
- TYJJADVDDVDEDZ-UHFFFAOYSA-M potassium hydrogencarbonate Chemical compound [K+].OC([O-])=O TYJJADVDDVDEDZ-UHFFFAOYSA-M 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 description 15
- 230000008859 change Effects 0.000 description 10
- 238000012545 processing Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000011160 research Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 230000009471 action Effects 0.000 description 2
- 238000002955 isolation Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 241001269235 Danis Species 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000009530 blood pressure measurement Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000002277 temperature effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
The invention discloses an unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering. The method comprises five steps of unmanned aerial vehicle modeling, model linearization and discretization, primary Kalman filtering denoising, secondary Kalman filtering destrending and residual error detection. The invention comprehensively considers the characteristics that the fault amplitude of the sensor in the unmanned aerial vehicle is small and is difficult to detect due to the influence of noise and feedback control, and introduces a secondary Kalman filtering detection method on the basis of the original Kalman filtering, thereby realizing the effective detection of the fault of the sensor in the unmanned aerial vehicle, improving the detection rate of the fault of the sensor of the unmanned aerial vehicle, finding the fault of the sensor in time and avoiding the accident and loss of the unmanned aerial vehicle caused by the fault of the sensor.
Description
Technical Field
The invention relates to the safety and reliability technology of an unmanned aerial vehicle, in particular to an unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering.
Background
In recent years, unmanned aerial vehicles have been rapidly developed for search and rescue, aerial photography, agriculture, surveying and mapping, and other applications. During the use of the drone, each component of the drone inevitably fails. In practice, drones are typically equipped with low-cost, lightweight micro-electromechanical system Inertial Measurement Units (IMUs), including three-axis gyroscopes, accelerometers, and magnetometers. These sensors play a crucial role in most drone navigation and control applications. However, the mechanical components and electronic components of the mems typically undergo a gradual degradation process (e.g., the values of the micro-resistors may change slowly), and the IMU is susceptible to failure due to component damage, vibration, and temperature effects. If the trouble is not detected in time, will directly influence unmanned aerial vehicle's complete machine security and reliability. Therefore, fault detection has important research significance and value, and is the first line of defense for improving safety and reliability.
Sensor failures can be classified into catastrophic failures and creep failures. Abrupt faults can be modeled as a step signal in general, while gradual faults (slow development) such as drift of the sensor can be represented by a ramp signal. Sensor drift refers to the situation where the difference between the sensor output and the actual value of the process variable deviates linearly with time. The use of redundant information from the sensors to perform the sensing function is the basis for any sensor failure detection. Different detection methods are developed for different types of redundant information. For example, hardware redundancy and analytic redundancy. The hardware redundancy is based on multiple physical inertial measurement units. Disadvantages of this approach are high cost, increased maintenance and space usage, etc. However, resolving redundancy methods does not increase hardware costs. Analytical redundancy the output of the sensor is estimated from other relevant measurements in the system. Analytical redundancy-based fault detection methods can be broadly divided into data-driven-based methods and model-based methods. In a model-based fault detection approach, particle filtering is one approach for unmanned aerial vehicle IMU sensor fault detection and isolation. However, particle filtering has a higher computational complexity compared to kalman filtering. H ∞ is also a commonly used fault detection method, unlike the kalman filter based on the state space equation, which is based on a transfer function. In contrast to kalman filtering, H ∞ filtering does not require the statistical nature of the assumed noise. However, the H ∞ filter cannot guarantee that the variance of the estimation error is minimal or within a limited range. Data-driven fault detection methods require a large data set, such as machine learning, compared to model-based fault detection methods. In practical use, a large number of data sets are processed, the calculation time is long, and the method is not interpretable.
The application research of the Kalman filtering in the unmanned aerial vehicle sensor fault detection comprises the application of Kalman filtering, the research and application of Kalman filtering extended algorithm, the research and application of adaptive Kalman filtering algorithm and the application of Kalman filtering combined with other methods. The Kalman filter extension algorithm comprises an extended Kalman filter, an unscented Kalman filter, a volumetric Kalman filter and a Federal Kalman filter. The extended Kalman filter solves the nonlinear problem through gradual extension of a linear algorithm; the unscented kalman filter is mainly based on an unscented transformation, where the square root of the covariance matrix is required; the cubature kalman filter numerically calculates the integral of the product of any nonlinear function in a nonlinear bayesian filter and the gaussian density using cubic volume rules. Regardless of the method used, accurate knowledge of the a priori statistical properties of the process noise and the measurement noise is required. In actual drone testing, the prior statistics of process noise and measurement noise are unknown or inaccurate. Although the a priori statistical information of the process noise and the measurement noise is accurately known, in an actual operating environment, the system is influenced by internal and external uncertain factors, and the statistical characteristics of the process noise and the measurement noise are easy to change and have strong time-varying characteristics. Based on this, in order to make the kalman filter estimation more accurate, a noise estimation module may be added to the kalman filter. Therefore, the adaptive kalman filter algorithm does not require accurate noise prior statistics and has the ability to adapt to noise variations. An unscented Kalman filter based on variational Bayes with adaptivity and robustness is proposed, where the idea of variational Bayes approximation is to minimize the Kullback-Leibler (KL) difference between the true distribution and the approximation. Unlike variational bayes approximation, the Sage-Husa adaptive kalman filter does not need to consider probability density. It may also make real-time estimates of noise statistics. However, this method cannot guarantee the positivity of the measurement noise covariance matrix and the semi-positivity of the process noise covariance matrix, and is easily diverged.
In an active fault-tolerant flight control system for unmanned aerial vehicle sensor/actuator faults, a kalman filter is used to detect the sensor/actuator faults and then isolate the sensor and actuator faults based on a fault isolation scheme of a robust two-stage kalman filter. In a wind speed and unmanned aerial vehicle motion parameter estimation algorithm based on a two-stage Kalman filter, in a first stage, a wind speed estimation algorithm is used for estimating wind speed by means of a global positioning system and dynamic pressure measurement. Wherein the extended kalman filtering method is applied to sensor fault detection of an unmanned aerial vehicle system. In the second stage, to estimate the state parameters of the drone, the wind speed estimated in the first stage kalman filter is used.
Disclosure of Invention
Aiming at the problems of low fault detection rate, small sensor fault amplitude, difficult detection due to the influence of noise and feedback control and the like in the existing unmanned aerial vehicle state estimation, the invention provides an unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering. The model is then linearized and discretized for subsequent quadratic kalman filtering. In the method, a first Kalman filter is used for denoising, a second Kalman filter is used for trend removing, and residual errors are calculated for detection. It is worth mentioning that the second kalman filter is a modified Sage-Husa adaptive kalman filter, which can automatically estimate the noise variance R of the measurement data. The method is first proposed and applied to unmanned aerial vehicles. In the simulation platform, a fault is injected in the pitch angle rate to simulate IMU sensor faults. Simulation results prove the effectiveness of the method in detection of sudden faults and gradual faults of the IMU sensor. The result shows that the fault detection rate of the secondary Kalman filtering is higher than that of the conventional Kalman filtering. In addition, the method also provides important information and basis for the fault-tolerant control of the unmanned aerial vehicle. The main innovation points of the invention are as follows: (1) The method is designed under the frames of denoising and trend removing. The first Kalman filter is used for denoising, the second Kalman filter is used for removing trend, and residual errors are calculated for detection. This is the first method to detect drone sensor failure using this technique. (2) In the second kalman filter, a modified Sage-Husa adaptive kalman filter is proposed, which can automatically estimate the noise variance R of the measurement data. Compared with the traditional Kalman filtering, the method has the advantages of self-adaption, divergence avoidance and high detection rate.
The purpose of the invention is realized by the following technical scheme:
an unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering comprises the following steps:
the method comprises the following steps: establishing a kinematics and dynamics model of the unmanned aerial vehicle to obtain a state space equation of the unmanned aerial vehicle;
step two: linearizing and discretizing the state space equation of the unmanned aerial vehicle to obtain a linearized and discretized state space equation;
step three: denoising a signal acquired under a normal condition of the IMU sensor by adopting Kalman filtering based on a state space equation after linearization and discretization to obtain a denoised IMU sensor signal;
step four: filtering denoised IMU sensor signals by adopting Kalman filtering again based on a state space equation after linearization and discretization, and using signals output by secondary Kalman filtering for trend removing;
step five: calculating a difference value of the denoised IMU sensor signal and a quadratic Kalman filter IMU sensor signal prior estimation, wherein the difference value accords with Gaussian distribution, and an integral multiple of a standard variance of the difference value is taken as an IMU sensor fault detection threshold value;
step six: repeating the third step and the fourth step on the IMU sensor signal acquired on line, calculating the residual error of the IMU sensor signal acquired on line, and indicating that the IMU sensor has a fault when the residual error of the IMU sensor signal acquired on line is greater than or equal to the fault detection threshold value of the IMU sensor; otherwise, the IMU sensor is normal.
Further, the state space equation after linearization and discretization obtained in the second step is:
wherein x (k + 1) is the state variable of the unmanned aerial vehicle, w (k) is process noise, and v (k) is measurement noise; a is a state transition matrix, and B is an input matrix; c is an output matrix, C represents the relationship between the measurement vector z (k) and the state variable x (k); z (k) represents a measurement vector at time k obtained from the drone sensor;
in the third step, the state variable posterior estimation of one Kalman filtering is
Wherein,
representing a Kalman filtering posterior estimation of the unmanned aerial vehicle state variable at the moment k,
representing the primary Kalman filtering prior estimation of the state variable of the unmanned aerial vehicle at the moment of k + 1; k 1 (k) A kalman filter kalman gain at time k is represented:
P 1 (k +1 k) represents a state error covariance matrix of the state variable estimated in the moment of k +1 by Kalman filtering a priori; r denotes the covariance matrix of the measurement noise v (k).
Further, in the fourth step, the state variable posterior estimation of the second Kalman filtering
Wherein e (k + 1) is an innovation of the quadratic Kalman filtering,
representing unmanned aerial vehiclesThe second kalman filtering posteriori estimation of the state variables at time k,
representing the secondary Kalman filtering prior estimation of the state variable of the unmanned aerial vehicle at the moment of k + 1; k 2 (k) For the quadratic kalman filter kalman gain at time k:
P 2 (k +1 k) represents the state error covariance matrix of the quadratic Kalman filter prior estimation of the state variable at the moment k + 1.
Further, in a secondary Kalman filter, a modified Sage-Husa adaptive Kalman filtering method is adopted to automatically estimate a measurement noise variance R, namely the estimation process of R is as follows:
wherein b is a forgetting factor.
Further, the calculation formula of the residual r (k + 1) of the step five is as follows:
in the sixth step, residual errors of the IMU sensor signals acquired on line are calculated, and when the residual errors of the IMU sensor signals acquired on line are larger than or equal to the IMU sensor fault detection threshold value, the IMU sensor is indicated to have faults; otherwise, the IMU sensor is normal.
The utility model provides an unmanned aerial vehicle IMU sensor fault detection device based on secondary Kalman filtering, includes one or more treater for realize unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering.
A computer readable storage medium having stored thereon a program which, when executed by a processor, implements a method for fault detection of an IMU sensor of an unmanned aerial vehicle based on quadratic kalman filtering.
The invention has the following beneficial effects:
the fault detection method of the IMU sensor based on the secondary Kalman filtering in the unmanned aerial vehicle state estimation comprehensively considers the characteristics that the fault amplitude of the unmanned aerial vehicle sensor is small and difficult to detect due to the influence of noise and feedback control, introduces the secondary Kalman filtering on the basis of the primary Kalman filtering, removes noise through the primary Kalman filtering, removes the trend through the secondary Kalman filtering, and finally realizes the detection of the fault of the sensor by comparing the residual error of the secondary Kalman filtering with the threshold value obtained by off-line data training. In order to realize self-adaptation, the improved Sage-Husa self-adaptive Kalman filter is adopted for secondary filtering, so that divergence is avoided, and meanwhile, the prediction precision is improved. The method can improve the detection rate of the faults of the sensor of the unmanned aerial vehicle, so that the faults of the sensor can be found in time, and accidents and loss of the unmanned aerial vehicle caused by the faults of the sensor are avoided.
Drawings
FIG. 1 is a schematic diagram of fault detection of an unmanned aerial vehicle IMU sensor based on secondary Kalman filtering.
FIG. 2 is a diagram of IMU pitch angle speed sudden change fault detection results based on quadratic Kalman filtering.
FIG. 3 is a graph of IMU pitch angle rate ramp fault detection results based on secondary Kalman filtering.
FIG. 4 is a diagram of IMU pitch angle speed sudden change fault detection results based on primary Kalman filtering.
FIG. 5 is a diagram of IMU pitch angle rate ramp fault detection results based on primary Kalman filtering.
Fig. 6 is a schematic diagram of an unmanned aerial vehicle IMU sensor fault detection system based on quadratic kalman filtering.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. The following description refers to the accompanying drawings in which the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary examples do not represent all implementations consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the present application, as detailed in the appended claims.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this application and the appended claims, the singular forms "a", "an", and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.
It is to be understood that although the terms first, second, third, etc. may be used herein to describe various information, such information should not be limited to these terms. These terms are only used to distinguish one type of information from another. For example, first information may also be referred to as second information, and similarly, second information may also be referred to as first information, without departing from the scope of the present application. The word "if," as used herein, may be interpreted as "at \8230; \8230when" or "when 8230; \823030when" or "in response to a determination," depending on the context.
The invention provides an unmanned aerial vehicle fault detection method based on secondary Kalman filtering, which is a fault detection method based on a model. Unlike the definition of other quadratic kalman filters: the primary objective is to measure the equations as quadratic kalman filters. The method provided by the invention is named by an algorithm designed under a denoising and trend-removing framework. The first-time Kalman filter is used for reducing measurement noise in a denoising process, wherein the variance of the process noise and the measurement noise is a fixed value. In the detrending part, a modified Sage-Husa adaptive Kalman filtering method is used to automatically estimate the measurement noise variance in consideration that the measurement data has been denoised. Finally, the Kalman filtering algorithm continuously acts on the measured data, so the method provided by the invention is called secondary Kalman filtering.
The invention discloses a secondary Kalman filtering method for fault detection of an IMU sensor of an unmanned aerial vehicle. Firstly, a kinematics and dynamics model of the unmanned aerial vehicle needs to be established, and then linearization and discretization are carried out on the model so as to carry out subsequent secondary Kalman filtering. Aiming at the working condition of large signal noise, primary Kalman filtering is used for noise reduction, and secondary Kalman filtering is used for detection. In the simulation platform, IMU pitch angle velocities are injected into faults (abrupt fault and gradual fault) respectively. Simulation results show that the fault detection rate of the secondary Kalman filtering is higher than that of a Kalman filtering-based fault detection method.
1. Establishing a kinematics and dynamics model of the unmanned aerial vehicle to obtain a state space equation of the unmanned aerial vehicle;
the rigid body model of the flight control of the multi-rotor unmanned aerial vehicle comprises a dynamic model and a kinematic model. Dynamics relates to motion and force, and relates to mass and moment of inertia of an object, the input of a multi-rotor unmanned aerial vehicle dynamics model is force and moment, and the output is speed and angular velocity. Finally, the translational acceleration can be obtained by newton's second law of motion and the angular acceleration can be obtained by euler's equation. Therefore, this modeling method is also called newton-euler equation, and the dynamics model of the multi-rotor drone is as follows:
wherein v is E Is the inertial velocity in the earth's fixed coordinate system,
. U is the lift, and m is many rotor unmanned aerial vehicle's quality. R EB Is a rotation matrix from the body coordinate system to the earth fixed coordinate system, and g is the gravitational acceleration. The equation shows that the multi-rotor unmanned aerial vehicle is under the earth fixed coordinate system, based on Newton's second motion law, under the action of thrust U and gravityThe resulting acceleration.
Wherein,
representing the angular rate in the coordinate system of the fuselage,
。J x 、J y and J z Is the moment of inertia of the multi-rotor unmanned aerial vehicle about the x, y and z axes of the fuselage.
Is the rolling torque, and the rolling speed is,
is the pitch torque of the motor, and,
are yaw torques, which are all in the fuselage coordinate system. The equation shows that the multi-rotor unmanned aerial vehicle generates angular acceleration according to the Euler equation under the body coordinate system.
In the rigid body kinematics model, the kinematics studies the relationship between variables such as position, velocity, attitude, and angular velocity. The input of the multi-rotor unmanned aerial vehicle kinematics model is speed and angular velocity, and the corresponding output is position and attitude. The kinematics model of the multi-rotor unmanned aerial vehicle is as follows:
wherein p is E Is the position of the inertia, and,
in (1). This equation represents the relationship between position and velocity in the earth's fixed coordinate system.
Wherein v is B Is the speed of the multi-rotor drone relative to the x, y and z axes of the fuselage,
。
wherein,
representing the euler angles in the coordinate system of the fuselage,
。
representing a rotation matrix between the angular rate and the euler angular rate. Based on the above equations, the nonlinear system and measurement model are as follows:
where x (t) is the state vector, u (t) is the control input vector, z (t) is the measurement data vector, w (t) is the process noise, and v (t) is the measurement noise.
2. Unmanned aerial vehicle state space equation discretization and linearization
Because the state space equation formed by the kinematics and the dynamics model of the unmanned aerial vehicle is nonlinear, in order to utilize the Kalman filtering algorithm, the linearization of the model needs to be firstly carried out, and the equation after linearization is as follows:
after linearization, discretization of the model is performed:
wherein x (k + 1) is the state variable of the unmanned aerial vehicle, w (k) is process noise, and v (k) is measurement noise; a is a state transition matrix, and B is an input matrix; c is an output matrix, C represents the relationship between the measurement vector z (k) and the state variable x (k); z (k) represents the measurement vector obtained from the drone sensor at time k.
To date, linearization and discretization of multi-rotor drone models has been accomplished. Next, a fault detection method of the IMU sensor of the multi-rotor drone is described.
3. Denoising the signal acquired by the IMU sensor under the normal condition by adopting Kalman filtering based on the state space equation after linearization and discretization to obtain a denoised IMU sensor signal
In the secondary Kalman filtering algorithm, primary Kalman filtering is used for denoising so as to improve the fault detection rate. The invention carries out experimental comparison on the secondary Kalman filtering and the Kalman filter, and proves the superiority of the design of the secondary Kalman filtering method. Kalman filtering is known as an optimal linear estimator, which minimizes the mean square error and provides recursive computation of state variables. Based on the state space equation and the measurement equation, the estimation process is as follows:
wherein
And
is a state variable x in a primary Kalman filter estimator 1 (k) A priori and a posteriori minimum mean square error estimates. Equation representation state variable x 1 (k) A priori estimate of the kalman filter.
Wherein P is 1 (k +1 calucity k) and P 1 (k) Are the corresponding prior and a posteriori state error covariances in the primary kalman filter. Q is the variance of the system noise w (k). Equation representation of the prior covariance P in a first order Kalman filter 1 (k +1 purple k).
Wherein, K 1 (k) Is the kalman gain. R is the variance of the measurement noise v (k).
Wherein the equation represents the state variable x 1 (k + 1) Kalman FilterThe device estimates that the number of the first and second signals,
is an innovation of the primary kalman filter. The posterior covariance calculation formula of the primary Kalman filtering is as follows:
4. quadratic Kalman filtering de-trending
And outputting a result according to the primary Kalman filtering algorithm, and performing trend analysis by using a secondary Kalman filtering algorithm to obtain a residual error of the data. Sensor failure is detected by comparing the residual to a threshold. Similarly, based on the state space equation and the measurement equation, the estimation process is as follows:
wherein
And
is a state variable x in a quadratic Kalman filter estimator 2 (k) A priori and a posteriori minimum mean square error estimates. The equation represents the state variable x 2 (k) A priori estimate of the quadratic kalman filter.
Wherein P is 2 (k +1 is n) and P 2 (k) Are the corresponding prior and posterior state error covariances in the quadratic kalman filter. Q is the variance of the system noise w (k). The equation represents the prior covariance P in the quadratic Kalman filter 2 A calculation process of (k +1 k).
Wherein, K 2 (k) Is the kalman gain of the quadratic kalman filter. R is the variance of the measurement noise v (k) in the quadratic kalman filter.
Wherein the equations represent state variables
The second-order kalman filter estimation of (1),
is an innovation of the quadratic kalman filter. The a posteriori covariance of the quadratic kalman filter is as follows:
in a secondary Kalman filter, the invention provides an improved Sage-Husa adaptive Kalman filtering method for automatically estimating and measuring a noise variance R, wherein the estimation process of R is as follows:
wherein b is a forgetting factor.
The above process is a calculation process of a secondary kalman filter, wherein a residual r (k + 1) calculation formula of the secondary kalman filter is as follows:
wherein
Is the filtered data after the primary kalman filter,
is the prior estimation of the state variable after the secondary Kalman filtering, and C is an output matrix.
5. Residual error detection process based on secondary Kalman filtering
The present invention assumes that the residuals follow a gaussian distribution. Within the confidence interval, the threshold is calculated as an integer multiple of the normal residual standard deviation. The detection process is as follows:
the indexes for evaluating the effect of the fault detection method are three: detection Delay (DD), fault Detection Rate (FDR), and False Alarm Rate (FAR). DD represents the time required for the fault detection method to detect a fault after the fault occurs. FDR represents the ratio of the number of fault samples that can be correctly detected during the detection process. FAR refers to the rate at which abnormalities are detected in normal samples.
In the above-described equations, the process of the present invention,DFNis the number of correctly detected faults.TFNIs the total number of failed samples.DANIs to detect in the normal dataThe resulting iso-constants.TNNIs the total number of normal samples. The specific algorithm flow is shown in fig. 1.
Corresponding to the embodiment of the unmanned aerial vehicle IMU sensor fault detection method based on the secondary Kalman filtering, the invention also provides an embodiment of an unmanned aerial vehicle IMU sensor fault detection system based on the secondary Kalman filtering.
As shown in fig. 2, for the detection result of the IMU pitch angle velocity sudden change fault based on the secondary kalman filter, the fault is injected at the time of 30s, the IMU pitch angle velocity sudden change fault is simulated as a step signal, and the amplitude of the fault signal is 0.175rad (10 deg). Detection indexes are as follows: FDR 98.63%, FAR 0.60%, DD 0.00s. Fig. 3 shows the detection result of the IMU pitch angle rate ramp fault based on the secondary kalman filter, the fault is injected at the time of 30s, the IMU pitch angle rate ramp fault is simulated as a ramp signal, the fault change rate is 0.0058rad/s (0.3325 deg/s), and the maximum amplitude of the fault signal is 0.175rad (10 deg). Detection indexes are as follows: FDR was 47.62%, FAR was 2.75%, DD was 1.87s. Fig. 4 shows a detection result of the IMU pitch angle velocity sudden change fault based on the primary kalman filter, where the fault is injected at the time of 30s, and the IMU pitch angle velocity sudden change fault is simulated as a step signal, where the amplitude of the fault signal is 0.175rad (10 deg). Detection indexes are as follows: FDR 4.80%, FAR 0.15%, DD 0.24s. Fig. 5 shows the detection result of the IMU pitch angle rate ramp fault based on the primary kalman filter, in which the fault is injected at the time of 30s, and the IMU pitch angle rate ramp fault is modeled as a ramp signal, the fault change rate is 0.0058rad/s (0.3325 deg/s), and the maximum amplitude of the fault signal is 0.175rad (10 deg). Detection indexes are as follows: FDR 0.70%, FAR 0.00%, DD 12.41s. The secondary Kalman filtering algorithm is proved to have better detection performance through comparison of experimental results.
Referring to fig. 6, the fault detection system for the IMU sensor of the unmanned aerial vehicle based on the secondary kalman filter according to the embodiment of the present invention includes one or more processors, and is configured to implement the fault detection method for the IMU sensor of the unmanned aerial vehicle based on the secondary kalman filter in the above embodiment.
The invention is based on a quadratic Kalman filterEmbodiments of the wave drone IMU sensor fault detection system may be applied on any data processing capable device, which may be a device or location such as a computer. The device embodiments may be implemented by software, or by hardware, or by a combination of hardware and software.
The software implementation is taken as an example, and as a logical device, the device is formed by reading corresponding computer program instructions in the nonvolatile memory into the memory for running through the processor of any device with data processing capability. In terms of hardware, as shown in fig. 6, the present invention is a hardware structure diagram of any device with data processing capability where an unmanned aerial vehicle IMU sensor fault detection system based on secondary kalman filtering is located, and in addition to the processor, the memory, the network interface, and the nonvolatile memory shown in fig. 6, any device with data processing capability where an apparatus is located in an embodiment may also include other hardware generally according to the actual function of the any device with data processing capability, which is not described again.
The implementation process of the functions and actions of each unit in the above device is specifically described in the implementation process of the corresponding step in the above method, and is not described herein again.
For the device embodiments, since they substantially correspond to the method embodiments, reference may be made to the partial description of the method embodiments for relevant points. The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one position, or may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the invention. One of ordinary skill in the art can understand and implement without inventive effort.
The embodiment of the invention also provides a computer-readable storage medium, wherein a program is stored on the computer-readable storage medium, and when the program is executed by a processor, the method for detecting the fault of the IMU sensor of the unmanned aerial vehicle based on the quadratic Kalman filtering in the embodiment is realized.
The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any data processing capability device described in any of the foregoing embodiments. The computer readable storage medium may also be an external storage device such as a plug-in hard disk, a Smart Media Card (SMC), an SD card, a Flash memory card (Flash card), etc. provided on the device. Further, the computer readable storage medium may include both internal storage units and external storage devices of any data processing capable device. The computer readable storage medium is used to store the computer 20169the program and other programs and data required by the any data processing capable device, and may also be used to temporarily store data that has been or will be output.
Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the application being indicated by the following claims.
It will be understood that the present application is not limited to the precise arrangements described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof. The scope of the application is limited only by the appended claims.
Claims (5)
1. An unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering is characterized by comprising the following steps:
the method comprises the following steps: establishing a kinematics and dynamics model of the unmanned aerial vehicle to obtain a state space equation of the unmanned aerial vehicle;
step two: linearizing and discretizing the state space equation of the unmanned aerial vehicle to obtain a linearized and discretized state space equation;
step three: denoising a signal acquired under a normal condition of the IMU sensor by adopting Kalman filtering based on a state space equation after linearization and discretization to obtain a denoised IMU sensor signal;
step four: filtering denoised IMU sensor signals by Kalman filtering again based on a linearized and discretized state space equation, and using signals output by secondary Kalman filtering for trend removing;
step five: calculating a difference value of the denoised IMU sensor signal and a quadratic Kalman filter IMU sensor signal prior estimation, wherein the difference value accords with Gaussian distribution, and an integral multiple of a standard variance of the difference value is taken as an IMU sensor fault detection threshold value;
step six: repeating the third step and the fourth step on the IMU sensor signal acquired on line, calculating the residual error of the IMU sensor signal acquired on line, and when the residual error of the IMU sensor signal acquired on line is more than or equal to the IMU sensor fault detection threshold, indicating that the IMU sensor has a fault; otherwise, the IMU sensor is normal;
the state space equation after linearization and discretization obtained in the second step is as follows:
wherein x (k + 1) is the state variable of the unmanned aerial vehicle, w (k) is process noise, and v (k) is measurement noise; a is a state transition matrix, and B is an input matrix; c is an output matrix, C represents the relationship between the measurement vector z (k) and the state variable x (k); z (k) represents a measurement vector at time k obtained from the drone sensor;
in the third step, the state variable posterior estimation of one Kalman filtering is
Wherein,
representing a Kalman filtering posterior estimation of the unmanned aerial vehicle state variable at the moment k,
representing the primary Kalman filtering prior estimation of the state variable of the unmanned aerial vehicle at the moment of k + 1; k is 1 (k) Representing the primary kalman filter kalman gain at time k:
P 1 (k +1 purple k) represents a state error covariance matrix of the state variable estimated in the moment of k +1 by Kalman filtering in a priori mode; r represents a covariance matrix of the measurement noise v (k);
in the fourth step, the posterior estimation of the state variable of the second Kalman filtering
Wherein e (k + 1) is an innovation of the quadratic Kalman filtering,
indicating state variable of drone at time kThe a-posteriori estimation by the quadratic kalman filter,
representing the secondary Kalman filtering prior estimation of the state variable of the unmanned aerial vehicle at the moment of k + 1; k is 2 (k) For the quadratic kalman filter kalman gain at time k:
P 2 (k +1 k) denotes the state error covariance matrix of the quadratic kalman filter prior estimate of the state variable at time k + 1.
2. The unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering according to claim 1, characterized in that in the secondary Kalman filter, a modified Sage-Husa adaptive Kalman filtering method is adopted to automatically estimate the measurement noise variance R, that is, the estimation process of R is as follows:
wherein b is a forgetting factor.
3. The method for detecting faults of IMU (inertial measurement unit) sensors of unmanned aerial vehicles based on quadratic Kalman filtering according to claim 1, wherein the difference value of the step five is r (k + 1), and the calculation formula is as follows:
in the sixth step, residual errors of the IMU sensor signals acquired on line are calculated, and when the residual errors of the IMU sensor signals acquired on line are larger than or equal to the IMU sensor fault detection threshold value, the IMU sensor is indicated to have faults; otherwise, the IMU sensor is normal.
4. An unmanned aerial vehicle IMU sensor fault detection device based on secondary Kalman filtering is characterized by comprising one or more processors and used for realizing the unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering in any one of claims 1 to 3.
5. A computer-readable storage medium, on which a program is stored, which, when executed by a processor, implements the method for fault detection of an IMU sensor of an unmanned aerial vehicle based on quadratic kalman filtering according to any one of claims 1 to 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210892728.4A CN115218927B (en) | 2022-07-27 | 2022-07-27 | Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210892728.4A CN115218927B (en) | 2022-07-27 | 2022-07-27 | Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115218927A CN115218927A (en) | 2022-10-21 |
| CN115218927B true CN115218927B (en) | 2022-12-27 |
Family
ID=83613166
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210892728.4A Active CN115218927B (en) | 2022-07-27 | 2022-07-27 | Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115218927B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116700203B (en) * | 2023-05-04 | 2024-05-17 | 中国人民解放军国防科技大学 | Fault detection and isolation method for carrier rocket attitude control system |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6681182B2 (en) * | 2002-02-01 | 2004-01-20 | The Aerospace Corporation | Fault detection pseudo gyro |
| FR3034514B1 (en) * | 2015-04-01 | 2017-04-21 | Sagem Defense Securite | NAVIGATION TRACKING METHOD OF A MOBILE CARRIER WITH AN EXTENDED KALMAN FILTER |
| US20180307231A1 (en) * | 2017-04-19 | 2018-10-25 | 4D Tech Solutions, Inc. | Intelligent electronic speed controller (iesc) |
| CN108170127B (en) * | 2017-12-28 | 2019-07-26 | 山东科技大学 | A kind of fault detection method of UAV Flight Control System |
| CN109857094B (en) * | 2019-03-14 | 2020-06-02 | 杭州电子科技大学 | Two-stage Kalman filtering algorithm-based aeroengine fault diagnosis method |
| US20210270973A1 (en) * | 2020-02-28 | 2021-09-02 | The Mitre Corporation | System and methods for fault detection in kalman filter estimation |
| CN113654554B (en) * | 2021-08-13 | 2024-06-14 | 北京维思韦尔航空电子技术有限公司 | Fast self-adaptive dynamic inertial navigation real-time resolving denoising method |
-
2022
- 2022-07-27 CN CN202210892728.4A patent/CN115218927B/en active Active
Non-Patent Citations (1)
| Title |
|---|
| 基于ATSUKF的飞行器惯性测量单元的故障诊断;何启志等;《西北工业大学学报》;20200831(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115218927A (en) | 2022-10-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9728014B2 (en) | Sensor fault detection and diagnosis for autonomous systems | |
| Lu et al. | Adaptive three-step Kalman filter for air data sensor fault detection and diagnosis | |
| CN107421534B (en) | Redundant strapdown inertial navigation system multi-fault isolation method | |
| CN108196532B (en) | Fault detection and separation method for longitudinal flight control system of unmanned aerial vehicle based on nonlinear adaptive observer | |
| Youn et al. | Combined quaternion-based error state Kalman filtering and smooth variable structure filtering for robust attitude estimation | |
| CN110196049A (en) | The detection of four gyro redundance type Strapdown Inertial Navigation System hard faults and partition method under a kind of dynamic environment | |
| CN111024124B (en) | Combined navigation fault diagnosis method for multi-sensor information fusion | |
| Hamadi et al. | Data fusion fault tolerant strategy for a quadrotor UAV under sensors and software faults | |
| CN102435763A (en) | Measuring method for attitude angular velocity of spacecraft based on star sensor | |
| D'Amato et al. | Fault tolerant low cost IMUS for UAVs | |
| Han et al. | Quadratic-Kalman-filter-based sensor fault detection approach for unmanned aerial vehicles | |
| CN110763253A (en) | SVR-based integrated navigation system fault diagnosis method | |
| CN115218927B (en) | Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering | |
| CN113128035B (en) | Civil aircraft flight control sensor signal reconstruction fault-tolerant control method | |
| CN110553642A (en) | Method for improving inertial guidance precision | |
| CN113483759B (en) | Integrity protection level calculation method for GNSS, INS, vision integrated navigation system | |
| Pourtakdoust et al. | An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS | |
| Rudin et al. | A sensor fault detection for aircraft using a single Kalman filter and hidden Markov models | |
| Caliskan et al. | Innovation sequence application to aircraft sensor fault detection: comparison of checking covariance matrix algorithms | |
| Samy et al. | Fault detection and flight data measurement: Demonstrated on unmanned air vehicles using neural networks | |
| Okatan et al. | Kalman filter innovation sequence based fault detection in LEO satellite attitude determination and control system | |
| CN110108301B (en) | Robust alignment method for moving base for modulus detection | |
| Mirzaei et al. | Attitude determination improvement in accelerated motions for maneuvering underwater vehicles | |
| CN112629521A (en) | Modeling method for dual-redundancy combined navigation system of rotor aircraft | |
| Zhang et al. | Using simulation to design an MPC policy for field navigation using GPS sensing |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |