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 PDF

Info

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
Application number
CN202210892728.4A
Other languages
Chinese (zh)
Other versions
CN115218927A (en
Inventor
韩晓佳
谢安桓
严旭飞
王晓波
胡易人
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202210892728.4A priority Critical patent/CN115218927B/en
Publication of CN115218927A publication Critical patent/CN115218927A/en
Application granted granted Critical
Publication of CN115218927B publication Critical patent/CN115218927B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering
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:
Figure 179213DEST_PATH_IMAGE001
Figure 951997DEST_PATH_IMAGE002
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
Figure 492700DEST_PATH_IMAGE003
Wherein the content of the first and second substances,
Figure 667329DEST_PATH_IMAGE004
representing a Kalman filtering posterior estimation of the unmanned aerial vehicle state variable at the moment k,
Figure 268075DEST_PATH_IMAGE005
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:
Figure 26690DEST_PATH_IMAGE006
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
Figure 371083DEST_PATH_IMAGE007
Figure 400219DEST_PATH_IMAGE008
Wherein e (k + 1) is an innovation of the quadratic Kalman filtering,
Figure 437446DEST_PATH_IMAGE009
representing unmanned aerial vehiclesThe second kalman filtering posteriori estimation of the state variables at time k,
Figure 919242DEST_PATH_IMAGE010
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:
Figure 67327DEST_PATH_IMAGE011
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:
Figure 950969DEST_PATH_IMAGE012
Figure 159097DEST_PATH_IMAGE013
wherein b is a forgetting factor.
Further, the calculation formula of the residual r (k + 1) of the step five is as follows:
Figure 128190DEST_PATH_IMAGE014
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:
Figure 814386DEST_PATH_IMAGE015
wherein v is E Is the inertial velocity in the earth's fixed coordinate system,
Figure 818114DEST_PATH_IMAGE016
. 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.
Figure 698608DEST_PATH_IMAGE017
Wherein, the first and the second end of the pipe are connected with each other,
Figure 154997DEST_PATH_IMAGE018
representing the angular rate in the coordinate system of the fuselage,
Figure 644884DEST_PATH_IMAGE019
。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.
Figure 237539DEST_PATH_IMAGE020
Is the rolling torque, and the rolling speed is,
Figure 787470DEST_PATH_IMAGE021
is the pitch torque of the motor, and,
Figure 731155DEST_PATH_IMAGE022
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:
Figure 24733DEST_PATH_IMAGE023
wherein p is E Is the position of the inertia, and,
Figure 737474DEST_PATH_IMAGE024
in (1). This equation represents the relationship between position and velocity in the earth's fixed coordinate system.
Figure 192726DEST_PATH_IMAGE025
Wherein v is B Is the speed of the multi-rotor drone relative to the x, y and z axes of the fuselage,
Figure 358128DEST_PATH_IMAGE026
。
Figure 455397DEST_PATH_IMAGE027
wherein the content of the first and second substances,
Figure 786759DEST_PATH_IMAGE028
representing the euler angles in the coordinate system of the fuselage,
Figure 412912DEST_PATH_IMAGE029
。
Figure 331190DEST_PATH_IMAGE030
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:
Figure 966571DEST_PATH_IMAGE031
Figure 388325DEST_PATH_IMAGE032
Figure 450959DEST_PATH_IMAGE033
Figure 590953DEST_PATH_IMAGE034
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:
Figure 30025DEST_PATH_IMAGE035
Figure 571864DEST_PATH_IMAGE036
after linearization, discretization of the model is performed:
Figure 805400DEST_PATH_IMAGE037
Figure 934155DEST_PATH_IMAGE038
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:
Figure 911338DEST_PATH_IMAGE039
wherein
Figure 307684DEST_PATH_IMAGE040
And
Figure 712121DEST_PATH_IMAGE041
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.
Figure 826707DEST_PATH_IMAGE042
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).
Figure 342002DEST_PATH_IMAGE043
Wherein, K 1 (k) Is the kalman gain. R is the variance of the measurement noise v (k).
Figure 858434DEST_PATH_IMAGE044
Wherein the equation represents the state variable x 1 (k + 1) Kalman FilterThe device estimates that the number of the first and second signals,
Figure 433772DEST_PATH_IMAGE045
is an innovation of the primary kalman filter. The posterior covariance calculation formula of the primary Kalman filtering is as follows:
Figure 35655DEST_PATH_IMAGE046
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:
Figure 354641DEST_PATH_IMAGE047
wherein
Figure 460000DEST_PATH_IMAGE048
And
Figure 993791DEST_PATH_IMAGE049
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.
Figure 817391DEST_PATH_IMAGE050
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).
Figure 940067DEST_PATH_IMAGE051
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.
Figure 165512DEST_PATH_IMAGE052
Wherein the equations represent state variables
Figure 82653DEST_PATH_IMAGE053
The second-order kalman filter estimation of (1),
Figure 659128DEST_PATH_IMAGE054
is an innovation of the quadratic kalman filter. The a posteriori covariance of the quadratic kalman filter is as follows:
Figure 319916DEST_PATH_IMAGE055
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:
Figure 134288DEST_PATH_IMAGE056
Figure 753488DEST_PATH_IMAGE057
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:
Figure 286101DEST_PATH_IMAGE058
wherein
Figure 16160DEST_PATH_IMAGE059
Is the filtered data after the primary kalman filter,
Figure 186503DEST_PATH_IMAGE060
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:
Figure 976605DEST_PATH_IMAGE061
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.
Figure 262092DEST_PATH_IMAGE062
Figure 264683DEST_PATH_IMAGE063
Figure 319227DEST_PATH_IMAGE064
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.
Figure 14651DEST_PATH_IMAGE065
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:
Figure DEST_PATH_IMAGE001
Figure 610662DEST_PATH_IMAGE002
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
Figure DEST_PATH_IMAGE003
Wherein the content of the first and second substances,
Figure 399014DEST_PATH_IMAGE004
representing a Kalman filtering posterior estimation of the unmanned aerial vehicle state variable at the moment k,
Figure DEST_PATH_IMAGE005
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:
Figure 869178DEST_PATH_IMAGE006
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
Figure DEST_PATH_IMAGE007
Figure 81854DEST_PATH_IMAGE008
Wherein e (k + 1) is an innovation of the quadratic Kalman filtering,
Figure DEST_PATH_IMAGE009
indicating state variable of drone at time kThe a-posteriori estimation by the quadratic kalman filter,
Figure 755412DEST_PATH_IMAGE010
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:
Figure DEST_PATH_IMAGE011
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:
Figure 344525DEST_PATH_IMAGE012
Figure DEST_PATH_IMAGE013
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:
Figure 394828DEST_PATH_IMAGE014
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.
CN202210892728.4A 2022-07-27 2022-07-27 Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering Active CN115218927B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
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
Lombaerts et al. Online aerodynamic model structure selection and parameter estimation for fault tolerant control
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
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
CN110763253A (en) SVR-based integrated navigation system fault diagnosis method
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
CN110553642A (en) Method for improving inertial guidance precision
EP3350604B1 (en) High-performance inertial measurements using a redundant array of inexpensive inertial sensors
Pourtakdoust et al. An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS
Caliskan et al. Innovation sequence application to aircraft sensor fault detection: comparison of checking covariance matrix algorithms
CN113483759A (en) Integrity protection level calculation method for GNSS, INS and Vision integrated navigation system
Rudin et al. A sensor fault detection for aircraft using a single Kalman filter and hidden Markov models
Okatan et al. Kalman filter innovation sequence based fault detection in LEO satellite attitude determination and control system
Samy et al. Fault detection and flight data measurement: Demonstrated on unmanned air vehicles using neural networks
CN110108301B (en) Robust alignment method for moving base for modulus detection
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
CN111678515A (en) Device state estimation method and device, electronic device and storage medium
Shi et al. Research on Fault Diagnosis of Three Degrees of Freedom Gyroscope Redundant System

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