CN115218927B - Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering - Google Patents

Abstract

The invention discloses a fault detection method for an IMU sensor of an unmanned aerial vehicle based on a secondary Kalman filter. The method includes five steps of UAV modeling, model linearization and discretization, primary Kalman filter denoising, secondary Kalman filter detrending and residual detection. The present invention comprehensively considers the characteristic that the fault amplitude of the sensor in the UAV is small and is difficult to detect due to the influence of noise and feedback control. On the basis of the original Kalman filter, the detection method of the second Kalman filter is introduced to realize the detection The effective detection of sensor faults in man-machine can improve the detection rate of UAV sensor faults, so that sensor faults can be found in time, and UAV accidents and losses caused by sensor faults can be avoided.

Description

Fault detection method of UAV IMU sensor based on quadratic Kalman filter

technical field

The invention relates to the safety and reliability technology of unmanned aerial vehicles, in particular to a fault detection method for IMU sensors of unmanned aerial vehicles based on secondary Kalman filtering.

Background technique

In recent years, drones have developed rapidly in applications such as search and rescue, aerial photography, agriculture, and surveying and mapping. During the use of the drone, every part of the drone will inevitably fail. In practice, drones are usually equipped with a low-cost, lightweight MEMS inertial measurement unit (Inertial Measurement Unit, IMU), including a three-axis gyroscope, accelerometer, and magnetometer. These sensors play a vital role in most UAV navigation and control applications. However, the mechanical parts and electronic components of MEMS usually undergo a gradual degradation process (for example, the value of microresistors may change slowly), and IMUs are prone to failure due to component damage, vibration, and temperature effects. If the failure is not detected in time, it will directly affect the safety and reliability of the drone. Therefore, fault detection has important research significance and value, and fault detection is the first line of defense to improve safety and reliability.

Sensor faults can be divided into sudden faults and slow faults. Abrupt faults can usually be modeled as step signals, while slowly changing faults (slowly developing), such as sensor drift, can be represented as ramp signals. Sensor drift is when the difference between the sensor output and the actual value of the process variable deviates linearly over time. Utilizing redundant information from sensors to perform detection functions is the basis for any sensor fault detection. Different detection methods have been developed for different types of redundant information. For example, hardware redundancy and resolution redundancy. Hardware redundancy is based on multiple physical IMUs. The disadvantages of this method are high cost, increased maintenance and space occupation. However, the parsing redundancy approach does not increase the hardware cost. Analytical redundancy analyzes estimate the output of a sensor from other related measurements in the system. Analytical redundancy-based fault detection methods can be broadly classified into data-driven and model-based methods. Among the model-based fault detection methods, particle filtering is a method for fault detection and isolation of UAV IMU sensors. However, particle filtering has higher computational complexity compared to Kalman filtering. H∞ is also a commonly used fault detection method. Unlike the Kalman filter which is based on the state space equation, the H∞ filter is based on the transfer function. Compared with Kalman filtering, H∞ filtering does not need to assume the statistical characteristics of noise. However, the H∞ filter cannot guarantee that the variance of the estimation error is minimal or within a limited range. Compared with model-based fault detection methods, data-driven fault detection methods, such as machine learning, require large datasets. In actual use, processing a large number of data sets takes a long time to calculate, and this type of method is not interpretable.

The application research of Kalman filter in UAV sensor fault detection includes the application of Kalman filter, the research and application of extended Kalman filter algorithm, the research and application of adaptive Kalman filter algorithm and the combination of Kalman filter and other methods Applications. Kalman filter extension algorithms include extended Kalman filter, unscented Kalman filter, volumetric Kalman filter and federated Kalman filter. The extended Kalman filter solves nonlinear problems through the gradual extension of the linear algorithm; the unscented Kalman filter is mainly based on the unscented transformation, which requires the square root of the covariance matrix; the volumetric Kalman filter uses the cubic volume rule, and the numerical calculation of Integral of any nonlinear function multiplied by a Gaussian density in a linear Bayesian filter. Regardless of the approach used, accurate knowledge of the a priori statistical properties of the process and measurement noise is required. In actual UAV testing, the prior statistics of process noise and measurement noise are unknown or inaccurate. Although the prior statistical information of process noise and measurement noise is known accurately, in the actual operating environment, the system is affected by internal and external uncertain factors, and the statistical characteristics of process noise and measurement noise are easy to change, which has a strong time-varying characteristics. Based on this, in order to make the estimation of the Kalman filter more accurate, a noise estimation module can usually 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 changes. An adaptive and robust Unscented Kalman Filter based on Variational Bayesian is proposed, where the idea of Variational Bayesian approximation is to minimize the Kullback-Leibler between the true distribution and the approximation ( KL) difference. Unlike the variational Bayesian approximation, the Sage-Husa adaptive Kalman filter does not need to take probability density into account. It also enables real-time estimation of noise statistics. However, this method cannot guarantee the positive definiteness of the measurement noise covariance matrix and the semi-positive definiteness of the process noise covariance matrix, and is prone to divergence.

In an active fault-tolerant flight control system for UAV sensor/actuator failures, Kalman filters are used to detect sensor/actuator failures, and then isolate the sensors based on a robust two-stage Kalman filter-based fault isolation scheme and actuator failure. In a two-stage Kalman filter based wind speed and UAV motion parameter estimation algorithm, in the first stage, wind speed is estimated using a wind speed estimation algorithm with the help of GPS and dynamic pressure measurements. Among them, the extended Kalman filter method is applied to the sensor fault detection of UAV system. In the second stage, to estimate the state parameters of the UAV, the wind speed estimated in the first stage Kalman filter is used.

Contents of the invention

Aiming at the problems of low fault detection rate, small sensor fault amplitude, and difficult detection characteristics affected by noise and feedback control in the existing UAV state estimation, the present invention proposes a quadratic Kalman filter-based For the fault detection method of human-machine IMU sensor, first, it is necessary to establish the kinematics and dynamics model of the UAV. The model is then linearized and discretized for subsequent quadratic Kalman filtering. In this method, the first Kalman filter is used for denoising, the second Kalman filter is used for detrending, and residuals 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. This method was proposed and applied to UAVs for the first time. In the simulation platform, faults are injected into the pitch rate to simulate IMU sensor faults. The simulation results confirm the effectiveness of the method in the detection of sudden faults and slowly changing faults of IMU sensors. The results show that the fault detection rate of the quadratic Kalman filter is higher than that of the conventional Kalman filter. In addition, this method also provides important information and basis for fault-tolerant control of UAVs. The main innovations of the present invention are as follows: (1) A quadratic Kalman filter-based UAV sensor fault detection method is proposed, which is designed under the framework of denoising and detrending. The first Kalman filter is used for denoising, the second Kalman filter is used for detrending and computing residuals for detection. This is the first method to use the technology to detect sensor failures in drones. (2) In the second Kalman filter, an improved Sage-Husa adaptive Kalman filter is proposed, which can automatically estimate the noise variance R of the measurement data. Compared with traditional Kalman filtering, this method has the advantages of self-adaptation, divergence avoidance and high detection rate.

The purpose of the present invention is achieved through the following technical solutions:

A kind of UAV IMU sensor fault detection method based on quadratic Kalman filter, comprises the following steps:

Step 1: Establish the kinematics and dynamics model of the UAV, and obtain the state space equation of the UAV;

Step 2: linearizing and discretizing the state space equation of the unmanned aerial vehicle to obtain the state space equation after linearization and discretization;

Step 3: Based on the linearized and discretized state space equations, use Kalman filter to denoise the signals collected by the IMU sensor under normal conditions, and obtain the denoised IMU sensor signal;

Step 4: Based on the linearized and discretized state-space equations, the Kalman filter is used again to filter the denoised IMU sensor signal, and the signal output by the second Kalman filter is used for detrending;

Step 5: Calculate the difference between the denoised IMU sensor signal and the prior estimate of the quadratic Kalman filter IMU sensor signal, the difference conforms to the Gaussian distribution, and the integer multiple of the standard deviation of the difference is taken as the IMU sensor fault detection threshold;

Step 6: Repeat steps 3 and 4 for the IMU sensor signal collected online, and calculate the residual error of the IMU sensor signal collected online. When the residual error of the IMU sensor signal collected online is greater than or equal to the IMU sensor fault detection threshold, it indicates There is a fault in the IMU sensor; otherwise, the IMU sensor is normal.

Further, the linearized and discretized state space equation obtained in the second step is:

Among them, x(k+1) is the state variable of the UAV, w(k) is the process noise, v(k) is the measurement noise; A is the state transition matrix, B is the input matrix; C is the output matrix, and C represents The relationship between the measured value vector z(k) and the state variable x(k); z(k) represents the measured value vector at time k obtained from the UAV sensor;

In the step 3, the state variable posterior estimation of a Kalman filter is

表示无人机状态变量在k时刻的一次卡尔曼滤波后验估计，

表示无人机状态变量在k+1时刻的一次卡尔曼滤波先验估计；K₁(k)表示在k时刻的一次卡 尔曼滤波卡尔曼增益： in,

Represents a Kalman filter posterior estimation of the state variable of the UAV at time k,

Represents a Kalman filter prior estimation of the UAV state variable at time k+1; K ₁ (k) represents a Kalman filter Kalman gain at time k:

P ₁ (k+1|k) represents the state error covariance matrix of a Kalman filter prior estimation of the state variable at time k+1; R represents the covariance matrix of the measurement noise v(k).

Further, in the step 4, the state variable posterior estimation of the second Kalman filter

表示无人机状态变量在k时刻的二次 卡尔曼滤波后验估计，

表示无人机状态变量在k+1时刻的二次卡尔曼滤波先验估 计；K₂(k)为在k时刻的二次卡尔曼滤波卡尔曼增益： Among them, e(k+1) is the innovation of the second Kalman filter,

Represents the quadratic Kalman filter posterior estimation of the state variable of the UAV at time k,

Represents the quadratic Kalman filter a priori estimate of the state variable of the UAV at time k+1; K ₂ (k) is the Kalman gain of the quadratic Kalman filter at time k:

P ₂ (k+1|k) represents the state error covariance matrix of the prior estimation of the quadratic Kalman filter of the state variable at time k+1.

Further, in the quadratic Kalman filter, the improved Sage-Husa adaptive Kalman filter method is used to automatically estimate the measurement noise variance R, that is, the estimation process of R is as follows:

Among them, b is the forgetting factor.

Further, the calculation formula of the residual r(k+1) in the step five is as follows:

In step six, calculate the residual error of the IMU sensor signal collected online, when the residual error of the IMU sensor signal collected online is greater than or equal to the fault detection threshold of the IMU sensor, it indicates that the IMU sensor has a fault; otherwise, the IMU sensor is normal.

A secondary Kalman filter-based UAV IMU sensor fault detection device includes one or more processors for implementing a secondary Kalman filter-based UAV IMU sensor fault detection method.

A computer-readable storage medium, on which a program is stored, and when the program is executed by a processor, a fault detection method for an IMU sensor of an unmanned aerial vehicle based on a secondary Kalman filter is realized.

The beneficial effects of the present invention are as follows:

The IMU sensor fault detection method based on the quadratic Kalman filter in the state estimation of the UAV proposed by the present invention comprehensively considers the characteristics that the fault amplitude of the UAV sensor is small, and it is difficult to detect due to the influence of noise and feedback control. On the basis of the single Kalman filter, additionally introduce the second Kalman filter, denoise through the first Kalman filter, detrend through the second Kalman filter, and finally get the residual and offline data training through the Kalman filter twice Compared with the threshold value, the detection of sensor failure is realized. And in order to achieve self-adaptation, the improved Sage-Husa adaptive Kalman filter is used for secondary filtering to avoid divergence and improve prediction accuracy at the same time. The method of the invention can improve the detection rate of the UAV sensor failure, so that the sensor failure can be found in time, and the UAV accident and loss caused by the sensor failure can be avoided.

Description of drawings

Fig. 1 Schematic diagram of UAV IMU sensor fault detection based on quadratic Kalman filter.

Fig. 2 is a diagram of fault detection results of IMU pitch angular velocity mutation based on the quadratic Kalman filter.

Fig. 3 is based on the fault detection results of IMU pitch angle velocity slowly changing based on the quadratic Kalman filter.

Fig. 4 is based on a Kalman filter IMU pitch angular velocity mutation detection results diagram.

Figure 5 is a diagram of the fault detection results of IMU pitch angle velocity slowly changing based on a Kalman filter.

Fig. 6 is a schematic diagram of a UAV IMU sensor fault detection system based on the quadratic Kalman filter.

detailed description

Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numerals in different drawings refer to the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with aspects of the present application as recited in the appended claims.

The terminology used in this application is for the purpose of describing particular embodiments only, and is not intended to limit the application. As used in this application and the appended claims, the singular forms "a", "the", and "the" are intended to include the plural forms as well, unless the context clearly dictates otherwise. It should also be understood that the term "and/or" as used herein refers to and includes any and all possible combinations of one or more of the associated listed items.

It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, the information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of the present application, first information may also be called second information, and similarly, second information may also be called first information. Depending on the context, the word "if" as used herein may be interpreted as "at" or "when" or "in response to a determination."

The invention proposes a UAV fault detection method based on the quadratic Kalman filter, which is a model-based fault detection method. Different from the definition of other quadratic Kalman filters: mainly for Kalman filters whose measurement equation is quadratic. The method proposed in the present invention is named after the algorithm designed under the framework of denoising and detrending. Among them, the first Kalman filter is used to reduce the measurement noise in the denoising process, where the variance of the process noise and the measurement noise is a fixed value. In the detrending part, considering that the measurement data has been denoised, the improved Sage-Husa adaptive Kalman filtering method is used in the detrending part to automatically estimate the measurement noise variance. Finally, the Kalman filter algorithm acts continuously on the measured data, so the method proposed by the present invention is called quadratic Kalman filter.

The present invention is used for the secondary Kalman filtering method of the UAV IMU sensor fault detection. First, the kinematics and dynamics model of the UAV needs to be established, and then the model is linearized and discretized for subsequent secondary Kalman filtering. For the working condition of large signal noise, the first Kalman filter is used for noise reduction, and the second Kalman filter is used for detection. In the simulation platform, the pitch rate of the IMU is injected into faults (mutant faults and slow-change faults) respectively. The simulation results show that the fault detection rate of the quadratic Kalman filter is higher than that of the fault detection method based on the Kalman filter.

1. Establish the kinematics and dynamics model of the UAV, and obtain the state space equation of the UAV;

The rigid body model of multi-rotor UAV flight control includes dynamic model and kinematics model. Dynamics involves motion and force, and is related to the mass and moment of inertia of the object. The input of the multi-rotor UAV dynamic model is force and moment, and the output is velocity and angular velocity. Finally, translational acceleration can be obtained by Newton's second law of motion, and angular acceleration can be obtained by Euler's equation. Therefore, this modeling method is also called the Newton-Euler equation, and the dynamic model of the multi-rotor UAV is as follows:

。U是升力，m是多旋翼无人机的 质量。R_EB是从机身坐标系到地球固定坐标系的旋转矩阵，g是重力加速度。该方程表明，多旋 翼无人机在地球固定坐标系下，基于牛顿第二运动定律，在推力U和重力的作用下产生的加 速度。 where v _E is the inertial velocity in the earth-fixed coordinate system,

. U is the lift force and m is the mass of the multi-rotor drone. R _EB is the rotation matrix from the fuselage frame to the Earth-fixed frame, and g is the gravitational acceleration. This equation shows the acceleration of the multi-rotor UAV under the action of thrust U and gravity based on Newton's second law of motion in the fixed coordinate system of the earth.

表示机身坐标系中的角速率，

。J_x、J_y和J_z是多旋翼无人机绕 机身x、y和z轴的惯性矩。

是滚动扭矩，

是俯仰扭矩，

是偏航扭矩，它们都在机身坐标 系中。该方程表明，多旋翼无人机在机身坐标系下根据欧拉方程产生的角加速度。 in,

represents the angular rate in the body coordinate system,

. J _x , J _y and J _z are the moments of inertia of the multi-rotor UAV about the x, y and z axes of the fuselage.

is the rolling torque,

is the pitching torque,

are the yaw torques, and they are all in the fuselage coordinate system. This equation shows the angular acceleration of the multi-rotor UAV in the fuselage coordinate system according to the Euler equation.

In a rigid body kinematic model, kinematics studies the relationship between variables such as position, velocity, attitude, and angular velocity. The input of multi-rotor UAV kinematics model is velocity and angular velocity, and the corresponding output is position and attitude. The kinematics model of the multi-rotor UAV is as follows:

中。该方程表示地球固定坐标系中位置和速度之间的 关系。 where p _E is the inertial position,

middle. This equation expresses the relationship between position and velocity in an Earth-fixed coordinate system.

。 where v _B is the velocity of the multi-rotor UAV relative to the x, y and z axes of the fuselage,

.

表示机身坐标系中的欧拉角，

。

表示角速率与欧拉角速 率之间的旋转矩阵。基于以上方程，非线性系统和测量模型如下所示： in,

Indicates the Euler angles in the fuselage coordinate system,

.

Represents the rotation matrix between angular rate and 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 measured data vector, w(t) is the process noise, and v(t) is the measurement noise.

2. Discretization and linearization of UAV state space equations

Since the state space equation composed of the kinematics and dynamics model of the UAV is nonlinear, in order to use the Kalman filter algorithm, it is necessary to linearize the model first. The linearized equation is as follows:

After linearization, discretize the model:

Among them, x(k+1) is the state variable of the UAV, w(k) is the process noise, v(k) is the measurement noise; A is the state transition matrix, B is the input matrix; C is the output matrix, and C represents The relationship between the measured value vector z(k) and the state variable x(k); z(k) represents the measured value vector obtained from the UAV sensor at time k.

So far, the linearization and discretization of the multi-rotor UAV model have been completed. Next, the fault detection method of the multi-rotor UAV IMU sensor is introduced.

3. Based on the linearized and discretized state space equations, the Kalman filter is used to denoise the signals collected by the IMU sensor under normal conditions, and the denoised IMU sensor signal is obtained

In the quadratic Kalman filter algorithm, the primary Kalman filter is used for denoising to improve the fault detection rate. The invention compares the quadratic Kalman filter and the Kalman filter in experiments, and proves the superiority of the design of the quadratic Kalman filter method. Kalman filtering is known as an optimal linear estimator, which minimizes the mean square error and can provide recursive computation of state variables. Based on the state-space equation and the measurement equation, the estimation process is as follows:

和

是一次卡尔曼滤波器估计器中状态变量x₁(k)的先验和后验 最小均方误差估计。方程表示状态变量x₁(k)的一次卡尔曼滤波器先验估计。 in

and

is the prior and posterior minimum mean square error estimate of the state variable x ₁ (k) in a Kalman filter estimator. The equation represents a Kalman filter prior estimation of the state variable x ₁ (k).

where P ₁ (k+1|k) and P ₁ (k) are the corresponding prior and posterior state error covariances in the first-order Kalman filter. Q is the variance of the system noise w(k). The equation represents the calculation process of the prior covariance P ₁ (k+1|k) in the Kalman filter.

where K ₁ (k) is the Kalman gain. R is the variance of the measurement noise v(k).

是一次卡尔曼滤波器的新息。一次卡尔曼滤波的后验协方差计算 公式为： where the equation represents a Kalman filter estimation of the state variable x ₁ (k+1),

is a Kalman filter innovation. The formula for calculating the posterior covariance of a Kalman filter is:

4. Quadratic Kalman filter to detrend

According to the result output of the primary Kalman filter algorithm, the trend analysis is performed using the secondary Kalman filter algorithm to obtain the residual error of the data. Sensor failures are detected by comparing residuals to thresholds. Similarly, based on the state space equation and the measurement equation, the estimation process is as follows:

和

是二次卡尔曼滤波估计器中状态变量x₂(k)的先验和后验最 小均方误差估计。该方程表示状态变量x₂(k)的二次卡尔曼滤波器先验估计。 in

and

is the prior and posterior minimum mean square error estimate of the state variable x ₂ (k) in the quadratic Kalman filter estimator. This equation represents a quadratic Kalman filter prior estimate of the state variable x ₂ (k).

where P ₂ (k+1|k) and P ₂ (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). This equation represents the calculation process of the prior covariance P ₂ (k+1|k) in the quadratic Kalman filter.

Among them, K ₂ (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.

的二次卡尔曼滤波器估计，

是二次卡尔曼滤波器的新息。以下为二次卡尔曼滤波器的后验协 方差： where the equation represents the state variable

The quadratic Kalman filter estimate of ,

is the innovation of the quadratic Kalman filter. The following is the posterior covariance of the quadratic Kalman filter:

In the quadratic Kalman filter, the present invention proposes an improved Sage-Husa adaptive Kalman filter method to automatically estimate the measurement noise variance R, and the estimation process of R is as follows:

Among them, b is the forgetting factor.

The above process is the calculation process of the quadratic Kalman filter, where the formula for calculating the residual r(k+1) of the quadratic Kalman filter is as follows:

是一次卡尔曼滤波器后的滤波数据，

是二次卡尔曼滤波后 的状态变量的先验估计，C为输出矩阵。 in

is the filtered data after a Kalman filter,

is the prior estimate of the state variable after the quadratic Kalman filter, and C is the output matrix.

5. Residual detection process based on quadratic Kalman filter

The present invention assumes that the residual follows a Gaussian distribution. Thresholds are calculated as integer multiples of the standard deviation of the normal residuals within the confidence interval. The detection process is as follows:

There are three indicators to evaluate the effect of the fault detection method: detection delay (detection delay DD), fault detection rate (fault detection rate FDR) and false alarm rate (false alarm rate FAR). DD represents the time required for the fault detection method to detect the fault after the fault occurs. FDR represents the ratio of the number of faulty samples that can be correctly detected during the detection process. FAR refers to the rate at which anomalies are detected in normal samples.

In the above equation, DFN is the number of correctly detected faults. TFN is the total number of failure samples. DAN is the number of anomalies detected in normal data. TNN is the total number of normal samples. The specific algorithm flow is shown in Figure 1.

Corresponding to the above-mentioned embodiment of the fault detection method for the UAV IMU sensor based on the quadratic Kalman filter, the present invention also provides an embodiment of the fault detection system for the UAV IMU sensor based on the quadratic Kalman filter.

As shown in Fig. 2, it is the IMU pitch rate sudden change fault detection result based on the secondary Kalman filter. The fault is injected at the 30th second, and the IMU pitch rate sudden change fault is simulated as a step signal, and the amplitude of the fault signal is 0.175rad ( 10 degrees). Detection index: FDR is 98.63%, FAR is 0.60%, DD is 0.00s. Figure 3 shows the detection results of the IMU pitch rate slow-change fault based on the second Kalman filter. The fault is injected at the 30th second, and the IMU pitch rate slow-change fault is simulated as a ramp signal, and the fault change rate is 0.0058 rad/s ( 0.3325deg/s), the maximum amplitude of the fault signal is 0.175rad (10deg). Detection indicators: FDR is 47.62%, FAR is 2.75%, and DD is 1.87s. Figure 4 shows the IMU pitch rate mutation fault detection results based on a Kalman filter. The fault is injected at the 30th moment, and the IMU pitch rate mutation fault is simulated as a step signal, and the amplitude of the fault signal is 0.175rad (10deg). Detection indicators: FDR is 4.80%, FAR is 0.15%, and DD is 0.24s. Figure 5 shows the detection results of the IMU pitch rate slowly changing fault based on a Kalman filter. The fault is injected at the time of 30s, and the IMU pitch rate slowly changing fault is simulated as a ramp signal, and the fault change rate is 0.0058rad/s (0.3325 deg/s), the maximum amplitude of the fault signal is 0.175rad (10deg). Detection indicators: FDR is 0.70%, FAR is 0.00%, and DD is 12.41s. Through the comparison of experimental results, it is proved that the quadratic Kalman filter algorithm has better detection performance.

Referring to Fig. 6, a UAV IMU sensor fault detection system based on a secondary Kalman filter provided by an embodiment of the present invention includes one or more processors for implementing the secondary Kalman filter based on the above-mentioned embodiment A fault detection method for UAV IMU sensors.

以 软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备 的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层 面而言，如图6所示，为本发明基于二次卡尔曼滤波的无人机IMU传感器故障检测系统所在 任意具备数据处理能力的设备的一种硬件结构图，除了图6所示的处理器、内存、网络接口、 以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该 任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。 The embodiment of the UAV IMU sensor fault detection system based on the quadratic Kalman filter of the present invention can be applied to any device with data processing capabilities, and the arbitrary devices with data processing capabilities can be devices or devices such as computers. The device embodiments can be implemented by software, or by hardware or a combination of software and hardware.

Taking software implementation as an example, as a device in a logical sense, it is formed by reading the corresponding computer program instructions in the non-volatile memory into the memory for operation by the processor of any device capable of data processing. From the hardware level, as shown in Figure 6, it is a hardware structure diagram of any device with data processing capability where the UAV IMU sensor fault detection system based on the secondary Kalman filter of the present invention is located, except as shown in Figure 6 In addition to the processor, memory, network interface, and non-volatile memory, any device with data processing capabilities in which the device in the embodiment is usually based on the actual function of any device with data processing capabilities may also include other hardware , which will not be repeated here.

For the implementation process of the functions and effects of each unit in the above device, please refer to the implementation process of the corresponding steps in the above method for details, and will not be repeated here.

As for the device embodiment, since it basically corresponds to the method embodiment, for related parts, please refer to the part description of the method embodiment. The device embodiments described above are only illustrative, and the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in One place, or it can be distributed to multiple network elements. Part or all of the modules can be selected according to actual needs to achieve the purpose of the solution of the present invention. It can be understood and implemented by those skilled in the art without creative effort.

The embodiment of the present invention also provides a computer-readable storage medium, on which a program is stored. When the program is executed by a processor, the method for detecting the fault of the UAV IMU sensor based on the secondary Kalman filter in the above-mentioned embodiment is implemented.

The computer-readable storage medium may be an internal storage unit of any device capable of data processing described in any of the foregoing embodiments, such as a hard disk or a memory. The computer-readable storage medium may also be an external storage device, such as a plug-in hard disk, a smart memory card (SmartMedia card, SMC), an SD card, a flash memory card (Flash card) and the like equipped on the device. Further, the computer-readable storage medium may also include both an internal storage unit of any device capable of data processing and an external storage device. The computer-readable storage medium is used to store the computing program and other programs and data required by any device capable of data processing, and may also be used to temporarily store outputted or to-be-outputted data.

Other embodiments of the present application will readily occur to those skilled in the art from consideration of the specification and practice of the disclosure herein. This application is intended to cover any modification, use or adaptation of the application, these modifications, uses or adaptations follow the general principles of the application and include common knowledge or conventional technical means in the technical field not disclosed in the application . The specification and examples are to be considered exemplary only, with a true scope and spirit of the application indicated by the appended claims.

It should be understood that the present application is not limited to the precise constructions which have been described above and shown in the accompanying drawings, and 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 ₁ (k) Representing the primary kalman filter kalman gain at time k:

P ₁ (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 ₂ (k) For the quadratic kalman filter kalman gain at time k:

P ₂ (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.

Images