Filtering method for optimal estimation of motion state of unmanned aerial vehicle under accelerometer fault

Abstract

The invention provides a filtering method for optimal estimation of a motion state of an unmanned aerial vehicle under an accelerometer fault, and relates to the technical field of unmanned aerial vehicle flight control. The method comprises the following steps: establishing a kinematic model and a kinetic model of a target unmanned aerial vehicle; modeling an accelerometer fault into a random walk model, wherein values of diagonal elements of a covariance matrix of the random walk model reflect the speed of fault change; on the basis of the kinematic model and the dynamical model, considering the fault of the accelerometer and the measurement noise of the inertial measurement unit, and establishing a continuous time linear state filtering model; identifying a covariance matrix of the random walk model, designing a two-step Kalman filter, and taking the information of a fault-free filter as the input of a fault filter; and a final state estimation value and a final fault estimation value are obtained through output of the two-step Kalman filter. The method has good stability for the aircraft with the characteristics of strong nonlinearity and high dimension.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) flight control, and particularly to a filtering method for optimal estimation of the motion state of a UAV under an accelerometer fault. Background Art

[0002] In practical applications, unmanned aerial vehicles are affected by many external factors, such as bird strikes, lightning, strong winds, heavy rains, hailstorms, icing, etc., resulting in faults in the vehicle sensors. When the damage is severe, catastrophic consequences will occur. The accelerometer is a key sensor of an unmanned aerial vehicle. If its fault is not identified in time, it will cause the flight control computer to calculate incorrect flight control instructions, resulting in incorrect motion state parameters being input into the controller, greatly reducing the control performance of the system. In severe cases, it will cause structural damage to the vehicle or the vehicle to enter an extreme flight state such as a stall.

[0003] In many engineering fields such as aircraft attitude control and fault diagnosis, the state estimation of the system is a key link. In state estimation methods, the Extended Kalman filter (EKF) is a non-linear filtering method that effectively solves practical non-linear problems. However, when EKF is used for state estimation, there are obvious problems such as large computational complexity and poor robustness. In particular, the truncation error caused by model linearization will reduce the filtering accuracy of EKF. For this reason, many new non-linear filtering algorithms have been proposed, such as the Unscented Kalman filter, the Central Difference Kalman filter, and the Particle filter. These traditional algorithms can achieve the state estimation of an aircraft in the presence of noise. However, when there is a sensor fault, their state estimation is often inaccurate, resulting in a large fault amplitude or even the unavailability of state information. The mainstream Optimal Two-Step Kalman Filter (OTSKF) can ensure the optimality of the filter state estimation in the presence of random deviations. However, when identifying an accelerometer fault, it is difficult to match the covariance matrix of the random walk model with the real fault for initialization; and when the dynamic change of model uncertainty is relatively fast, the estimation effect based on the OTSUKF estimation algorithm is not very stable. Summary of the Invention

[0004] Object of the Invention: Aiming at the deficiencies of the prior art, the present invention proposes a filtering method for optimal estimation of the motion state of a UAV under an accelerometer fault, so as to more accurately and effectively achieve the optimal estimation of the motion state of a UAV under an accelerometer fault, thereby solving the above problems proposed by the prior art.

[0005] To achieve the above object, the present invention proposes a filtering method for optimal estimation of the motion state of a UAV under an accelerometer fault, and the method includes the following steps:

[0006] Considering the uncertain factors included in the aerodynamic parameters, the accelerometer is used to measure the parameter specific force to replace the aerodynamic force, and the kinematic and dynamic models of the aircraft are established.

[0007] The accelerometer fault is modeled as a random walk model, and the values of the diagonal elements of the covariance matrix of the random walk model reflect the speed of fault change.

[0008] Considering the accelerometer fault and the IMU (Inertial Measurement Unit) measurement noise, the system equation of the filter is established; combining the aircraft motion and attitude information output by the three-antenna GNSS (Global Navigation Satellite System), the observation equation of the filter is established.

[0009] A state space model is established, a continuous-time linear state filtering model is obtained by using the linearization method, and then a discrete-time state filtering model is obtained by using the discretization method.

[0010] According to the information covariance matching technology, the covariance matrix of the random walk fault model is identified online, an unbiased KF and a bias filter are designed, and the estimated values of the two filters are fused through the coupling matrix to obtain the final state estimate and fault estimate.

[0011] Combined with the CKF method, the ATSCKF model is obtained, and the optimal estimate of the aircraft state under the accelerometer fault is obtained.

[0012] In a further embodiment, the kinematic model expression of the target UAV is as follows:

[0013]

[0014] In the formula, φ, θ, ψ represent the attitude angles of the aircraft, which are the roll angle, pitch angle and yaw angle respectively; p, q, r represent the three-axis angular rates of the aircraft, which are the roll angular rate, pitch angular rate and yaw angular rate respectively; cosθ is the cosine value of the pitch angle of the aircraft, representing the projection operator of the longitudinal axis of the airframe on the horizontal plane of the ground system; cosφ is the cosine value of the roll angle of the aircraft, representing the projection operator of the vertical axis of the airframe on the vertical plane containing the longitudinal axis of the airframe; sinθ is the sine value of the pitch angle of the aircraft, representing the projection scale of the longitudinal axis of the airframe on the horizontal plane of the ground system; sinφ is the sine value of the roll angle of the aircraft, representing the projection scale of the vertical axis of the airframe on the vertical plane containing the longitudinal axis of the airframe.

[0015] In a further embodiment, the dynamic model expression of the target UAV is as follows:

[0016]

[0017] In the formula, Denote the accelerometer measurement parameter specific force; Denoted as on the three output axes of the accelerometer u, v, w denote the projections of the ground speed on the body frame; m denotes the mass of the aircraft; g denotes the acceleration due to gravity; T denotes the engine thrust.

[0018] In a further embodiment, the expression of the random walk model is as follows:

[0019]

[0020] E[ω ε i ω ε jT = Q f k δ ij ε = X,Y,Z

[0021] In the formula, f ε denotes the accelerometer fault; Q f is the covariance matrix of the random walk model; δ ij is the Kronecker function; ω ε is white noise and ω ε ~(0,Q f ); X, Y, Z respectively represent the three output axes of the accelerometer;

[0022] Q f The values of the diagonal elements reflect the speed of fault change, and the larger the value, the faster the fault change is characterized; Q f The values of the non-diagonal elements reflect the linear correlation degree of the fault changes of different sensors, and the larger the value, the greater the correlation degree is characterized.

[0023] In a further embodiment, considering the fault and measurement noise of the accelerometer, the accelerometer measurement parameter specific force is rewritten as follows:

[0024]

[0025] Considering the measurement noise of the gyroscope, the three-axis angular rate is expressed as follows:

[0026]

[0027] In the formula, a xm , a ym , a zm are respectively the measurement values of the accelerometer on the three axes; f X , f Y , f Z are respectively the faults of the accelerometer on the three axes; ω ax, ω ay , ω az are the measurement noises of the accelerometer in the three axes respectively; p m , q m , r m are the measured values of the gyroscope in the three axes respectively; ω p , ω q , ω r are the measurement noises of the gyroscope in the three axes respectively.

[0028] In a further embodiment, the expression of the continuous-time linear state filtering model is as follows:

[0029]

[0030] where

[0031] In the formula, F(t) is the Jacobian matrix of the partial derivative of the non-linear equation with respect to the accelerometer fault vector, and G(t) is the Jacobian matrix of the partial derivative with respect to the process noise vector.

[0032] In a further embodiment, a two-step Kalman filter is designed, and the innovation r k of the fault-free filter is used as the input of the fault filter. The definition of the innovation r k is:

[0033]

[0034] In the formula, A k-1 is the system matrix, and H k is the observation matrix.

[0035] In a further embodiment, when there is a fault in the system, an adaptive fault filter is designed, where the fault system transition matrix is the identity matrix, and the observation matrix of the fault is and this matrix is a non-singular matrix, and the estimation of the covariance matrix of the fault model is obtained:

[0036]

[0037] In the formula, pinv represents the inverse matrix, represents the fault error covariance matrix;

[0038] Considering the influence of the fault noise on the fault-free filter, the following compensation is made for the one-step prediction of the fault-free filter:

[0039]

[0040] In the formula, represents the one-step prediction state estimation of the fault-free filter, represents the optimal compensation matrix of the one-step prediction state estimation, represents the one-step prediction error covariance matrix of the fault-free filter, represents the optimal compensation matrix of the one-step prediction error covariance matrix.

[0041] In a further embodiment, the two-step Kalman filter calculates through the coupling matrix V k and the output of the fault filter The innovation of the fault-free filter will be used as the input of the fault filter, as follows:

[0042]

[0043] wherein, inno_ represents the innovation, z k+1|k+1 represents the observation value of the fault-free filter, represents the observation estimate value of the one-step prediction of the fault-free filter.

[0044] In a further embodiment, the innovation, gain and update formula of the fault-free filter are as follows:

[0045]

[0046] wherein, inno_ represents the innovation, H k+1 represents the observation matrix, represents the one-step prediction state estimate value, U k+1 represents the fusion matrix, f k+1|k represents the fault one-step prediction, represents the Kalman gain, represents the fault one-step prediction error covariance matrix, f k+1|k+1 represents the fault estimate, represents the fault error covariance matrix, and I represents the identity matrix.

[0047] Beneficial effects: The method proposed by the present invention has good stability for aircraft with strong nonlinearity and high dimensionality characteristics; when the dynamic change of model uncertainty is relatively fast, it still has good identification effect; under acceleration faults, it has high state estimation accuracy, good fault identification effect and strong robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a schematic diagram of the solution of the present invention.

[0049] Figure 2 is a flowchart of the algorithm of the present invention.

[0050] Figure 3 is a comparison chart of the ground speed estimation of ATSCKF1 and OTSCKF1.

[0051] Figure 4 This is a comparison chart of the acceleration sensor fault estimation for ATSCKF1 and OTSCKF1.

[0052] Figure 5 This is a comparison chart of the ground speed estimation for ATSCKF2 and OTSCKF2.

[0053] Figure 6 This is a comparison chart of the acceleration sensor fault estimation for ATSCKF2 and OTSCKF2. Detailed implementation manners

[0054] In the following description, numerous specific details are given to provide a more thorough understanding of the present invention. However, it will be apparent to one skilled in the art that the present invention may be practiced without one or more of these details. In other instances, some well-known technical features are not described in order to avoid obscuring the present invention.

[0055] Taking a certain type of fixed-wing unmanned aerial vehicle as an example, the schematic diagram of the solution is as Figure 1 shown, and the flowchart of a filtering method for the optimal estimation of the motion state of an unmanned aerial vehicle under an acceleration sensor fault is as Figure 2 shown, and specifically includes the following operating steps:

[0056] Step 1: Considering the uncertain factors included in the aerodynamic parameters, in order to improve the accuracy of modeling, the specific force measured by the acceleration sensor is used to replace the aerodynamic force, and the kinematic and dynamic models of the aircraft are established.

[0057] The kinematic model of the aircraft is:

[0058]

[0059] Where: φ, θ, ψ represent the attitude angles of the aircraft, which are the roll angle, pitch angle, and yaw angle respectively. p, q, r represent the three-axis angular rates of the aircraft, which are the roll angular rate, pitch angular rate, and yaw angular rate respectively. Among them, cosθ is the cosine value of the pitch angle of the aircraft, representing the projection operator of the longitudinal axis of the airframe on the horizontal plane of the ground system; cosφ is the cosine value of the roll angle of the aircraft, representing the projection operator of the vertical axis of the airframe on the vertical plane containing the longitudinal axis of the airframe; sinθ is the sine value of the pitch angle of the aircraft, representing the projection scale of the longitudinal axis of the airframe on the horizontal plane of the ground system; sinφ is the sine value of the roll angle of the aircraft, representing the projection scale of the vertical axis of the airframe on the vertical plane containing the longitudinal axis of the airframe.

[0060] The dynamic model of the aircraft is:

[0061]

[0062] Where: specific force The representations on the three output axes of the accelerometer are u, v, and w represent the projections of the ground speed on the body frame, m represents the mass of the aircraft, g represents the acceleration due to gravity, T represents the engine thrust, and X, Y, and Z are the projections of the aerodynamic forces acting on the aircraft on the body frame.

[0063] Step 2: In the Kalman filter framework, to avoid losing the sensitivity to faults, the faults are modeled as a random walk model:

[0064]

[0065] where: f ε is the accelerometer fault, Q f is the covariance matrix of the random walk model, δ ij is the Kronecker function, ω is white noise and ω ε ~(0, Q f ), and X, Y, and Z represent the three output axes of the accelerometer respectively.

[0066] The values of the diagonal elements of Q f reflect the speed of fault change. The larger the value, the faster the fault changes. The values of the non-diagonal elements of Q f reflect the linear correlation degree of fault changes of different sensors. The larger the value, the greater the correlation degree.

[0067] Step 3: Considering the faults of the accelerometer and the IMU measurement noise, establish the system equation of the filter; combining the flight motion and attitude information output by the three-antenna GNSS, establish the observation equation of the filter.

[0068] First, considering the faults of the accelerometer and the measurement noise, the specific force can be rewritten as follows:

[0069]

[0070] where a xm , a ym , and a zm are the measured values of the accelerometer on the three axes respectively, f X , f Y , and f Z are the faults of the accelerometer on the three axes respectively, and ω ax , ω ay , and ω az are the measurement noises of the accelerometer on the three axes respectively.

[0071] Considering the measurement noise of the gyroscope, the angular rates of the three axes can be expressed as follows:

[0072]

[0073] where p m , q m , r m are the measured values of the gyroscope on the three axes respectively, and ω p , ω q , ω r are the measurement noises of the gyroscope on the three axes respectively.

[0074] Substituting equations (5) and (6) into equations (1) and (2), the system equation of the filter is obtained as follows:

[0075]

[0076] Secondly, combining the aircraft motion and attitude information output by the three-antenna GNSS, the observation equation of the filter is obtained as follows:

[0077]

[0078] where u Gm , v Gm , w Gm are the aircraft motion information on the three axes of the navigation coordinate system output by GNSS, φ m , θ m , ψ m are the attitude information output by GNSS, ν u , ν v , ν w are the measurement noises of the motion information output by GNSS, v φ , v φ , ν ψ are the measurement noises of the attitude information output by GNSS, and S att is the direction cosine matrix, which describes the coordinate transformation relationship between the body and the navigation system, and its specific definition is:

[0079]

[0080] where sinψ is the sine value of the yaw angle of the aircraft, indicating the projection scale of the longitudinal axis of the body in the vertical plane of the ground system.

[0081] Step 4: Establish a state-space model, obtain a continuous-time linear state filtering model using a linearization method, and then use a discretization method to obtain a discrete-time state filtering model.

[0082] First, define the state vector x, input vector u, observation vector z, accelerometer fault vector f, process noise vector ω, and observation noise vector v of the system model as follows:

[0083] x = [u v w φ θ ψ] T (10)

[0084] u = [axm a xm a xm p m q m r m T (11)

[0085] z = [u Gm v Gm w Gm φ m θ m ψ m T (12)

[0086] f = [f X f Y f Z 0 0 0] T (13)

[0087] ω = [ω ax ω ay ω az ω p ω q ω r T (14)

[0088] v = [v u v v v w v φ v θ v ψ T (15)

[0089] Obtain the continuous-time state filtering model:

[0090]

[0091] where x represents the state vector, u represents the input vector, f represents the accelerometer fault vector, ω represents the process noise vector, z represents the observation vector, v represents the observation noise vector, and f(·) and h(·) represent nonlinear functions.

[0092] Secondly, adopt the linearization method to convert the nonlinear noise into additive noise, and the continuous-time linear state filtering model can be obtained:

[0093]

[0094] where

[0095]

[0096] ​​​​Where \(F(t)\) is the partial derivative Jacobian matrix of the non - linear equation with respect to the accelerometer fault vector, and \(G(t)\) is the partial derivative Jacobian matrix with respect to the process noise vector.

[0097] During the use of the cubature Kalman filter, it is assumed that the noise in Equation (17) follows a Gaussian distribution with zero mean, that is

[0098]

[0099] where \(Q\) represents the process noise intensity matrix, \(R\) represents the measurement noise intensity matrix, \(\delta(t - \tau)\) is the Dirac function, \(E[\cdot]\) represents the mathematical expectation, and \(T\) is the transpose of the matrix.

[0100] Finally, discretizing Equation (17) gives the discrete - time state filtering model:

[0101]

[0102] where \(A\) represents the system matrix, \(B\) represents the input matrix, and \(F\) represents the fault distribution matrix.

[0103] Discretizing Equation (20) gives the discretized noise intensity matrix:

[0104]

[0105] where \(Q\) k represents the discretized process noise intensity matrix, \(R\) k represents the discretized measurement noise intensity matrix, and \(G\) represents the process noise distribution matrix. \(\delta\) ij represents the Dirichlet function.

[0106] Step 5: According to the information covariance matching technology, online identify the covariance matrix of the random - walk fault model, design an unbiased - influence Kalman filter and a bias filter, and fuse the estimated values of the two filters through a coupling matrix to obtain the final state estimate and fault estimate.

[0107] First, considering the linear discrete model in Equation (21), if there is no fault in the system, the definition of the innovation \(r\) k is:

[0108]

[0109] where \(A\) k-1 is the system matrix and \(H\) k is the observation matrix.

[0110] Processing the innovations of the filter for the most recent \(m\) steps can obtain an estimate of the innovation covariance, as follows:

[0111]

[0112] where covr k represents the covariance of the innovation.

[0113] According to the model information, the innovation covariance is as follows:

[0114]

[0115] where the one-step prediction error covariance matrix is defined as:

[0116]

[0117] Substituting formula (26) into (25), we get:

[0118]

[0119] By combining equations (24) and (27), the estimated covariance matrix of the process noise is obtained as:

[0120]

[0121] Secondly, when there is a fault in the system, an adaptive fault filter is designed, where the fault system transition matrix is the identity matrix, and the observation matrix of the fault is and this matrix is non-singular, obtaining the estimate of the covariance matrix of the fault model:

[0122]

[0123] where pinv represents the inverse matrix, represents the fault error covariance matrix.

[0124] Considering the influence of the fault noise on the fault-free filter, the following compensation is made to the one-step prediction of the fault-free filter:

[0125]

[0126] where represents the one-step prediction state estimate of the fault-free filter, represents the optimal compensation matrix of the one-step prediction state estimate, represents the one-step prediction error covariance matrix of the fault-free filter, represents the optimal compensation matrix of the one-step prediction error covariance matrix.

[0127] The two-step Kalman filter calculates through the coupling matrix V k and the output of the fault filter The calculation result ensures the optimality of the state estimate. The innovation of the fault-free filter will be used as the input of the fault filter, as follows:

[0128]

[0129] In the formula, inno_ represents the innovation, and z k+1|k+1 represents the observation value of the fault-free filter, and represents the observation estimation value of the one-step prediction of the fault-free filter.

[0130] The innovation, gain, and update formula of the fault-free filter are as follows:

[0131]

[0132] In the formula, inno_ represents the innovation, and H k+1 represents the observation matrix, represents the one-step prediction state estimation value, and U k+1 represents the fusion matrix, and f k+1|k represents the one-step prediction of the fault, represents the Kalman gain, represents the one-step prediction error covariance matrix of the fault, and f k+1|k+1 represents the fault estimation, represents the fault error covariance matrix, and I represents the identity matrix.

[0133] Therefore, the input of the fault filter is the innovation of the fault-free filter, and S k+1 can be regarded as the observation matrix of the fault filter, and the output of the fault filter can realize the fault identification of the IMU.

[0134] Step 6: Combine the adaptive method with the two-step filtering idea and the CKF method to obtain the ATSCKF algorithm. Its implementation pseudocode is shown in Table 1, and the optimal estimation V of the aircraft state under the accelerometer fault is output k+1 , f k+1|k+1 ,

[0135] Table 1 ATSCKF implementation pseudocode

[0136]

[0137] To verify the estimation performance and robustness of the proposed algorithm, the present invention conducts simulation experiments to compare the final state estimation results obtained by ATSCKF and OTSCKF. The sampling time is taken as 0.01 s, the simulation time is taken as 60 s, and the time and type of fault injection are shown in Table 2. Step faults (1, 1.5, -0.5), sine fault Si = sin(0.5π·Δt), ramp fault (0.1Δt, 0.15Δt), and cosine fault Co = cos(0.5π·Δt) information are randomly injected into the three axes of the accelerometer at four moments: 5 - 10 s, 15 - 25 s, 35 - 45 s, and 50 - 55 s. The accelerometer faults are in units of m / s.

[0138] Table 2 Accelerometer fault injection time and type

[0139]

[0140] At the same time, two different fault models are adopted to initialize the covariance matrix. The covariance matrices of the first group and the second group are respectively initialized as diagonal matrices with diagonal elements of 10 -6 and 10 -1 to verify the sensitivity of the proposed algorithm ATSCKF to the covariance matrix of the fault model and the matching of the fault dynamic characteristics.

[0141] Effect evaluation of the present invention: See Figures 3 to 6 . Figure 3 . In (a), it is the estimated value of the ground speed projected onto the body frame by ATSCKF1; in (b), it is the estimated value of the ground speed projected onto the body frame by OTSCKF1; in (c), it is the estimated error of the ground speed projected onto the body frame by ATSCKF1; in (d), it is the estimated error of the ground speed projected onto the body frame by OTSCKF1. Figure 4 . In (a), it is the estimated value of the accelerometer fault by ATSCKF1; in (b), it is the estimated value of the accelerometer fault by OTSCKF1; in (c), it is the estimated error of the accelerometer fault by ATSCKF1; in (d), it is the estimated error of the accelerometer fault by OTSCKF1. Figure 5 . In (a), it is the estimated value of the ground speed projected onto the body frame by ATSCKF2; in (b), it is the estimated value of the ground speed projected onto the body frame by OTSCKF2; in (c), it is the estimated error of the ground speed projected onto the body frame by ATSCKF2; in (d), it is the estimated error of the ground speed projected onto the body frame by OTSCKF2. Figure 6 . In (a), it is the estimated value of the accelerometer fault by ATSCKF2; in (b), it is the estimated value of the accelerometer fault by OTSCKF2; in (c), it is the estimated error of the accelerometer fault by ATSCKF2; in (d), it is the estimated error of the accelerometer fault by OTSCKF2.

[0142] It can be seen from the figure that the estimation accuracy of the ATSCKF for the motion state of the aircraft is better than that of the OTSCKF under different initial fault covariances. Moreover, when the fault model covariance is initialized to the first group of covariance matrices, this advantage is particularly obvious. This reflects that the state estimation performance of the OTSCKF is greatly affected by the initial value of the fault model covariance, while the proposed ATSCKF is basically not affected by it. Both the ATSCKF and the OTSCKF can accurately estimate the accelerometer fault information, and the ATSCKF has a significant improvement in the effect of accelerometer fault identification compared with the OTSCKF under different initial fault covariances. At the same time, under the two groups of different initial covariance matrices, the fault identification performance of the OTSCKF varies greatly, while the proposed ATSCKF shows stability.

[0143] To verify the accuracy of the proposed algorithm for estimating the motion state of the aircraft under accelerometer faults and estimating accelerometer faults, the root mean square error (RMSE) is used as the evaluation index:

[0144]

[0145] In the formula, represents the state estimation value, x represents the true state value, and N represents the state dimension.

[0146] The root mean square errors of the algorithms for estimating the motion state of the accelerometer-fault aircraft under different covariance matrix initializations are shown in Table 3; the root mean square errors of each algorithm for estimating accelerometer faults are shown in Table 4. Among them, OTSCKF 1 and OTSCKF 2 are the OTSCKF algorithms when the covariance matrix of the fault model is initialized to the first group and the second group of covariance matrices, respectively; ATSCKF 1 and ATSCKF 2 are the ATSCKF algorithms when the covariance matrix of the fault model is initialized to the first group and the second group of covariance matrices, respectively.

[0147] Table 3 Accuracy of each algorithm for motion state estimation

[0148]

[0149] As can be seen from Table 3, the mean square errors of both groups of ATSCKF and OTSCKF are lower than those of the standard CKF, that is, the accuracy of state estimation of the two algorithms is higher than that of the standard CKF; the mean square error of the first group of ATSCKF is much lower than that of the first group of OTSCKF, that is, the accuracy of state estimation of the first group of ATSCKF is much higher than that of the first group of OTSCKF; the mean square error of the second group of ATSCKF is similar to that of the second group of OTSCKF, and the algorithm accuracies of the two are not much different. From the above results, it can be seen that the ATSCKF algorithm has better state estimation performance and stability compared with the OTSCKF algorithm.

[0150] Table 4 Accuracy of each algorithm for accelerometer fault estimation

[0151]

[0152] As can be seen from Table 4, the mean square errors of both groups of ATSCKF are much lower than those of OTSCKF, that is, the accuracy of fault identification of the ATSCKF algorithm is much higher than that of OTSCKF; the mean square error of the first group of OTSCKF changes greatly compared with that of the second group, while the mean square error of the first group of ATSCKF remains basically unchanged compared with that of the second group. From the above results, it can be seen that the ATSCKF algorithm has better fault identification effect and stronger robustness compared with the OTSCKF algorithm.

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

Claims

1. A filtering method for optimal estimation of the motion state of a drone under accelerometer failure, characterized in that: The steps include: Establish kinematic and dynamic models of the target UAV; The accelerometer fault is modeled as a random walk model, and the values ​​of the diagonal elements of the covariance matrix of the random walk model reflect the speed of the fault change; On the basis of the kinematic model and the dynamic model, a continuous-time linear state filter model is established by considering the failure of the accelerometer and the measurement noise of the inertial measurement unit; Identify the covariance matrix of the random walk model, design a two-step Kalman filter, and use the new information of the fault-free filter as the input of the fault filter; The final state estimation and fault estimation are obtained using the two-step Kalman filter output.

2. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1 is characterized in that: The kinematic model expression of the target UAV is as follows: Wherein, φ, θ, ψ represent the attitude angle of the aircraft, which are roll angle, pitch angle and yaw angle respectively; p, q, r represent the three-axis angular rate of the aircraft, which are roll angular rate, pitch angular rate and yaw angular rate respectively; cosθ is the cosine value of the pitch angle of the aircraft, which represents the projection operator of the longitudinal axis of the fuselage on the horizontal plane of the ground system; cosφ is the cosine value of the roll angle of the aircraft, which represents the projection operator of the vertical axis of the fuselage on the vertical plane containing the longitudinal axis of the fuselage; sinθ is the sine value of the pitch angle of the aircraft, which represents the projection scale of the longitudinal axis of the fuselage on the horizontal plane of the ground system; sinφ is the sine value of the roll angle of the aircraft, which represents the projection scale of the vertical axis of the fuselage on the vertical plane containing the longitudinal axis of the fuselage.

3. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1, characterized in that: The dynamic model expression of the target UAV is as follows: In the formula, Indicates the accelerometer measurement parameter specific force; The expression on the three output axes of the accelerometer is u, v, w represent the projection of ground speed on the aircraft system; m represents the mass of the aircraft; g represents the acceleration due to gravity; and T represents the engine thrust.

4. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1, characterized in that: The accelerometer fault is modeled as a random walk model, and the expression is as follows: E[oh ε i oh ε jT ]=Q f k d ij e=X,Y,Z In the formula, f ε Indicates accelerometer failure; Q f is the covariance matrix of the random walk model; ε ij is the Kronecker function; ω ε is white noise and ω ε ~(0,Q f ); X, Y, Z represent the three output axes of the accelerometer respectively; Q f The value of the diagonal element reflects the speed of fault change. The larger the value, the faster the fault changes. f The values ​​of the off-diagonal elements reflect the degree of linear correlation between the fault changes of different sensors, and the larger the value, the greater the degree of correlation.

5. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 3, characterized in that: Considering the fault and measurement noise of the accelerometer, the accelerometer measurement parameters are more Rewrite as follows: Taking into account the measurement noise of the gyroscope, the three-axis angular rate is expressed as follows: In the formula, a xm 、a ym 、a zm are the measured values ​​of the accelerometer on the three axes; f X 、f Y 、f Z are the faults of the accelerometer in three axes respectively; ω ax ,ω ay ,ω az are the measurement noise of the accelerometer in three axes; p m ,q m 、r m are the measured values ​​of the gyroscope on three axes respectively; ω p ,ω q ,ω r are the measurement noise of the gyroscope in three axes.

6. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1 or 5, characterized in that: The continuous-time linear state filter model expression is as follows: in Where F(t) is the Jacobian matrix of the partial derivatives of the nonlinear equation with respect to the accelerometer fault vector, and G(t) is the Jacobian matrix of the partial derivatives with respect to the process noise vector.

7. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1 or 5, characterized in that: Design a two-step Kalman filter to transform the new information r of the fault-free filter k As the input of the fault filter, the new information r k The definition is: In the formula, A k-1 is the system matrix, H k is the observation matrix.

8. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 7, characterized in that: When there is a fault in the system, an adaptive fault filter is designed, where the fault system transfer matrix is ​​the unit matrix and the fault observation matrix is And the matrix is ​​a non-singular matrix, and the estimation of the covariance matrix of the fault model is obtained: Where pinv represents the inverse matrix, represents the fault error covariance matrix; Considering the influence of the fault noise on the non-fault filter, the one-step prediction of the non-fault filter is compensated as follows: In the formula, represents the one-step predictive state estimate of the fault-free filter, represents the optimal compensation matrix for one-step prediction state estimation, represents the one-step prediction error covariance matrix of the fault-free filter, represents the optimal compensation matrix for the one-step forecast error covariance matrix.

9. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 1, 7 or 8, characterized in that: The two-step Kalman filter is implemented by coupling matrix V k and the output of the fault filter to calculate The innovation of the non-fault filter will be used as the input of the fault filter as follows: In the formula, inno_ represents new information, z k+1|k+1 represents the observed value of the fault-free filter, represents the observed estimate of the one-step forecast of the fault-free filter.

10. The filtering method for optimal estimation of the motion state of a drone under accelerometer failure according to claim 9, characterized in that: The innovation and gain and update formulas for the fault-free filter are shown below: In the formula, inno_ represents new information, H k+1 represents the observation matrix, represents the one-step forecast state estimate, U k+1 represents the fusion matrix, f k+1|k represents one-step prediction of faults, represents the Kalman gain, represents the covariance matrix of the fault one-step prediction error, f k+1|k+1 represents the fault estimate, represents the fault error covariance matrix, and I represents the identity matrix.

Images