CN110426032B - Analytical redundant aircraft fault-tolerant navigation estimation method - Google Patents

Analytical redundant aircraft fault-tolerant navigation estimation method Download PDF

Info

Publication number
CN110426032B
CN110426032B CN201910693594.1A CN201910693594A CN110426032B CN 110426032 B CN110426032 B CN 110426032B CN 201910693594 A CN201910693594 A CN 201910693594A CN 110426032 B CN110426032 B CN 110426032B
Authority
CN
China
Prior art keywords
imu
moment
fault
time
angular
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
CN201910693594.1A
Other languages
Chinese (zh)
Other versions
CN110426032A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910693594.1A priority Critical patent/CN110426032B/en
Publication of CN110426032A publication Critical patent/CN110426032A/en
Application granted granted Critical
Publication of CN110426032B publication Critical patent/CN110426032B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations

Abstract

The invention discloses an analytical redundant aircraft fault-tolerant navigation estimation method. Firstly, estimating acceleration and angular acceleration information of the aircraft by using a dynamic model of the rotor aircraft; secondly, constructing an angular accelerometer/dynamic model/IMU fault detection filter and an IMU/magnetic sensor/GPS/barometer fault detection filter; then, according to the outputs of the two fault detection filters, a fault positioning strategy is constructed to realize the fault positioning of the navigation sensor; and finally, isolating the fault sensor according to the fault detection result, and completing navigation information calculation. According to the invention, the dynamic model and the angular accelerometer are introduced to realize fault detection of the angular accelerometer, the dynamic model, the IMU and the measuring sensor, and meanwhile, the dynamic model can be used for completely replacing the fault IMU to carry out navigation calculation, so that the fault tolerance of the navigation system is improved.

Description

Analytical redundant aircraft fault-tolerant navigation estimation method
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to an aircraft fault-tolerant navigation estimation method.
Background
The airborne navigation sensor is a data source for navigation calculation of the aircraft, and the calculated navigation information is the basis for stable flight of the aircraft. Currently, commonly used onboard navigation sensors include inertial sensors, IMU, and measurement sensors, magnetic sensors, GPS, barometers, and the like. However, in some special environments, the performance of the onboard navigation sensor is reduced or even fails, for example, the performance of the IMU is damaged by sound wave interference, the GPS signal is lost due to urban occlusion, and the like. If a faulty sensor is used to participate in the navigation solution, the aircraft can be out of control and even crash.
Disclosure of Invention
In order to solve the technical problems mentioned in the background art, the invention provides an analytical redundant aircraft fault-tolerant navigation estimation method, which improves the fault-tolerant performance of an aircraft navigation system.
In order to achieve the technical purpose, the technical scheme of the invention is as follows:
an analytical redundant aircraft fault-tolerant navigation estimation method comprises the following steps:
the method comprises the following steps: periodically reading the outputs of a rotor wing rotating speed measuring system, an angular accelerometer, an IMU, a magnetic sensor, a GPS and an air pressure meter of a carrier at the moment k, and estimating the motion acceleration information and the angular acceleration information of the rotor wing aircraft through a dynamic model of the rotor wing aircraft;
step two: utilizing the data output of the airborne sensor in the step one to construct an angular accelerometer/dynamic model/IMU fault detection filter S1 and an IMU/magnetic sensor/GPS/barometer fault detection filter S2; s1 includes three parallel sub-detectors, angular accelerometer/dynamical model sub-detector D1, angular accelerometer/IMU sub-detector D2 and dynamical model/IMU sub-detector D3; s2 includes three parallel sub-detectors, IMU/magnetic sensor sub-detector D4, IMU/GPS sub-detector D5 and IMU/barometer sub-detector D6;
step three: according to the fault detection results of the sub-detectors in the S1, a fault positioning strategy is constructed, fault positioning of the diagonal accelerometer, the dynamic model and the IMU is achieved, and positioning results are output; according to the fault location result of S1 and the fault detection results of each sub-detector in S2, a global fault location strategy is constructed, and a fault sensor is located;
step four: according to the fault positioning result obtained in the step three, a fault isolation strategy is made, a fault sensor is isolated from state filtering estimation, and navigation state estimation is completed;
when the airborne sensor has no fault or a dynamic model has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input, and the gyroscope, the magnetic sensor, the GPS and the barometer in the IMU are used as measuring sensors;
when the IMU fails, an angular accelerometer and a aerodynamic model of a dynamic model are used as state equation input, and a magnetic sensor, a GPS and a barometer are used as measuring sensors;
when the angular accelerometer has a fault, using a pneumatic moment model of a dynamic model and an accelerometer in the IMU as state equation input, and using a gyroscope, a magnetic sensor, a GPS and a barometer in the IMU as measuring sensors;
when the GPS or the barometer or the magnetic sensor has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input to isolate the fault sensor so as not to participate in global fusion filtering;
step five: and correcting the state quantities in S1 and S2 at the time k according to the state estimation result obtained in step four.
Further, in step one, the kinematic acceleration information and the angular acceleration information of the rotorcraft are estimated by:
Figure BDA0002148634710000021
Figure BDA0002148634710000022
fmz＝kT0+kT1(ω1 2+ω2 2+ω3 2+ω4 2)
Figure BDA0002148634710000023
Figure BDA0002148634710000024
Figure BDA0002148634710000025
wherein f ismx、fmy、fmzThe acceleration of the x axis, the y axis and the z axis of the body system is obtained through calculation of a dynamic model;
Figure BDA0002148634710000026
the angular acceleration, omega, of the x, y and z axes of the body system is calculated by a dynamic modelmzAngular velocity for the z-axis; k is a radical ofHx0、kHx1、kHx2、kHx3、kHy0、kHy1、kHy2、kHy3、kT0、kT1、kx0、kx1、kx2、kx3、ky0、ky1、ky2、ky3、kz0、kz1、kz2、kz3The parameters of the dynamic model of the rotor craft are all constant values;
Figure BDA0002148634710000027
the components of the linear velocity of the machine body system relative to the navigation system on the x and y axes of the machine system; beta is ax、βyThe angular acceleration of the machine body system x and y axes is obtained through an angular accelerometer; omegaiThe number of revolutions of the ith rotor, i ═ 1,2,3,4,
Figure BDA00021486347100000212
angular acceleration which is the ith rotor speed; f. ofax、fayThe acceleration of the body system in x and y axes is obtained by an accelerometer in the IMU.
Further, in step two, the method for constructing the angular accelerometer/dynamical model sub-detector D1 is as follows:
1. calculating a z-axis angular acceleration residual using the angular accelerometer output and the kinetic model output:
Figure BDA0002148634710000028
in the above formula, r1(k) Calculating a z-axis angular acceleration residual error for the angular accelerometer and the dynamic model; beta is az(k) Obtaining the z-axis angular acceleration of the machine system at the moment k through the output of the z-axis angular accelerometer;
Figure BDA0002148634710000029
the angular acceleration of the z axis of the organism system is calculated through a dynamic model; abs (, denotes the absolute value;
2. judging whether the sub detector D1 fails:
Figure BDA00021486347100000210
in the above formula, τ1A fault determination threshold; t is1Is a fault detection result; if T is11, then the angular accelerometer or the dynamical model fails; if T is1If the angular accelerometer and the dynamic model are 0, the angular accelerometer and the dynamic model are not in fault;
the method of constructing the angular accelerometer/IMU sub-detector D2 is as follows:
1. calculating x, y, z axis angular velocities using angular accelerometer outputs:
Figure BDA00021486347100000211
in the above formula, ωx(k)、ωy(k)、ωz(k) The angular velocity of the x, y and z axes of the body system at the moment k is output beta through the angular accelerometerx、βy、βzObtaining an integral; omegax(k-1)、ωy(k-1)、ωz(k-1) is the angular speed of the x, y and z axes of the organism system at the time of k-1, and is obtained through the output of a Federal Kalman filter at the time of k-1; Δ T is the discrete sampling time;
2. calculating the statistical parameters of the sub-detector D2:
r2(k)＝[ωgx(k) ωgy(k) ωgz(k)]T-[ωx(k) ωy(k) ωz(k)]T
A2(k)＝H2(k)P2(k|k-1)H2(k)+R2(k)
λ2＝r2(k)A2(k)-1r2(k)
P2(k|k-1)＝F2(k)P2(k-1)F2 T(k)+G2(k-1)Q2(k-1)G2 T(k-1)
in the above formula, r2(k) Outputting calculated x, y and z axis angular velocity residual errors for the IMU and the angular accelerometer at the moment k; omegagx(k)、ωgy(k)、ωgz(k) The angular velocities of the x, y and z axes of the organism system are obtained through gyro output; a. the2(k) Outputting the calculated residual variance for the IMU and the angular accelerometer at the moment k; h2(k)＝[I3×3]Is a matrix of k time measurement coefficients, I3×3Is a 3 × 3 identity matrix; p2(k | k-1) is a one-step prediction mean square error matrix at the time k; r2(k)＝diag([εωgx(k) εωgy(k) εωgz(k)]2) Measuring the noise matrix, epsilon, for time kωgx(k)、εωgy(k)、εωgz(k) Gyro noise of x, y and z axes of the organism system; lambda [ alpha ]2(k) Outputting the calculated statistical parameters for the IMU and the angular accelerometer at the moment k; f2(k)＝[I3×3]Detecting a state coefficient matrix of the system for the IMU and the angular accelerometer at the moment k; p2(k-1) is a mean square error matrix at the time of k-1; g2(k-1)＝[ΔT*I3×3]Detecting a state noise coefficient matrix of the system for the IMU and the angular accelerometer at the moment k-1; q2(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1)]2) The IMU and angular accelerometer detect the state noise, ε, of the system at time k-1βx(k-1)、εβy(k-1)、εβz(k-1) is the time k-1X, y, z axis angular accelerometer noise of (a); superscript T denotes transpose;
3. judging whether the sub detector D2 fails:
Figure BDA0002148634710000031
in the above formula, τ2A fault determination threshold; t is2Is a fault detection result; if T is2If 1, then the angular accelerometer or IMU fails; if T is3If 0, the angular accelerometer and the IMU are not in fault;
the method for constructing the kinetic model/IMU subdetector D3 is as follows:
1. calculating z-axis angular velocity using the kinetic model output:
Figure BDA0002148634710000032
in the above formula, ωmz(k-1) is the angular velocity at the k-1 moment and is obtained through the Federal Kalman filter output at the k-1 moment; omegamz(k) Angular velocity at time k;
Figure BDA0002148634710000033
the angular acceleration at the moment k is obtained through a dynamic model; Δ T is the discrete sampling time;
2. the residuals and statistical parameters of sub-detector D3 are calculated:
r3a(k)＝abs(faz(k)-fmz(k))
r3b(k)＝ωgz(k)-ωmz(k)
A3b(k)＝H3b(k)P3b(k|k-1)H3b(k)+R3b(k)
λ3b＝r3b(k)A3b(k)-1r3b(k)
P3b(k|k-1)＝F3b(k)P3b(k-1)F3b T(k)+G3b(k-1)Q3b(k-1)G3b T(k-1)
in the above formula, r3a(k) Calculating a z-axis acceleration residual error for the IMU at the moment k and the dynamic model; f. ofaz(k) The acceleration of the z axis of the machine system at the moment k is obtained through an accelerometer; r is3b(k) Calculating a z-axis angular velocity residual error for the IMU at the moment k and the dynamic model; a. the3b(k) Calculating a z-axis angular velocity residual variance for the IMU at the moment k and the dynamic model; h3b(k) 1 is a k moment measurement coefficient matrix; p3b(k | k-1) is a one-step prediction mean square error matrix at the time k; r3b(k)＝diag([εωgz(k)]2) Measuring a noise matrix for time k; lambda [ alpha ]3b(k) Calculating a z-axis angular velocity statistical parameter for the IMU at the moment k and the dynamic model;
Figure BDA0002148634710000041
a state coefficient matrix of a z-axis gyroscope and a dynamic model detection system at the moment k; p3b(k-1) is a mean square error matrix at the time of k-1; g3b(k-1) ═ 1 is a state noise coefficient matrix of the z-axis gyroscope and the dynamic model detection system at the moment of k-1; q3b(k-1)＝diag([εωmz(k-1)]2) For the state noise, ε, of the z-axis gyro and the dynamical model detection system at time k-1ωmz(k-1) obtaining the z-axis angular velocity noise of the machine system through a dynamic model at the moment of k-1;
3. judging whether the sub detector D3 fails:
Figure BDA0002148634710000042
in the above formula, τ3a、τ3bA fault determination threshold; t is3a、T3bIs a fault detection result; if T is3a(k) 1 or T3b(k) 1, IMU or kinetic model failure; if T is3a(k) 0 and T3b(k) 0, the IMU and the kinetic model are fault free.
Further, in step two, the state predictions of the sub-detectors D4, D5, D6 at time k are calculated:
q0(k)＝q0(k-1)-0.5ωgx(k)q1(k-1)-0.5ωgy(k)q2(k-1)-0.5ωgz(k)q3(k-1)
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure BDA0002148634710000043
Figure BDA0002148634710000044
Figure BDA0002148634710000045
Figure BDA0002148634710000046
Figure BDA0002148634710000047
Figure BDA0002148634710000048
in the above formula, q0(k)、q1(k)、q2(k)、q3(k) To pass through a state equationObtaining a k time quaternion; q. q.s0(k-1)、q1(k-1)、q2(k-1)、q3(k-1) is a quaternion at the moment k-1 and is obtained through the output of a federal Kalman filter at the moment k-1;
Figure BDA0002148634710000049
the component of the linear velocity of the body system relative to the navigation system at the moment k, which is obtained through a state equation, on the z axis of the body system is obtained;
Figure BDA00021486347100000410
the component of the linear velocity of the body system relative to the navigation system at the moment k-1 on the z axis of the body system is obtained through the Federal Kalman filter output at the moment k-1; g is the acceleration of gravity; pN(k)、PE(k)、PD(k) The north, east and earth positions at the k moment are obtained through a state equation; pN(k-1)、PE(k-1)、PD(k-1) is north, east and earth positions at the moment k-1 and is obtained through the Federal Kalman filter output at the moment k-1;
constructing IMU/magnetic sensor sub-detector D4:
1. calculating the statistical parameters of the sub-detector D4:
heading is calculated using state prediction at time k:
Figure BDA0002148634710000051
Figure BDA0002148634710000052
A4(k)＝H4(k)P4(k|k-1)H4(k)+R4(k)
λ4(k)＝r4(k)A4(k)-1r4(k)
P4(k|k-1)＝F4(k)P4(k-1)F4 T(k)+G4(k-1)Q4(k-1)G4 T(k-1)
Figure BDA0002148634710000053
Figure BDA0002148634710000054
Figure BDA0002148634710000055
Figure BDA0002148634710000056
Figure BDA0002148634710000057
Figure BDA0002148634710000058
Figure BDA0002148634710000059
Figure BDA00021486347100000510
Q4(k-1)＝diag([εωgx(k-1) εωgy(k-1) εωgz(k-1) εvbx(k-1) εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2)
in the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000061
the course angle at the k moment is obtained through state prediction calculation; r is4(k) Is k atThe heading angle residual error of the D4 detector; psim(k) The heading output by the magnetic sensor at the moment k; a. the4(k) The residual variance of the D4 detector at time k; h4(k) A measurement coefficient matrix of a D4 detector at the time k; p4(k | k-1) is a one-step predicted mean square error from time k-1 to time k; r4(k)＝diag([εψm(k)]2) Is the noise matrix at time k, epsilonψm(k) The high noise of the magnetic sensor at time k; lambda [ alpha ]4(k) The statistical parameters at the k moment; 0i×jIs a zero matrix of i x j; i isi×jIs an identity matrix of i x j; f4(k) A state coefficient matrix of the IMU and the magnetic sensor detector at the moment k; p4(k-1) is a mean square error matrix at the time of k-1; g4(k-1) is a state noise coefficient matrix of the IMU and the magnetic sensor detector at the time of k-1; q4(k-1) is the state noise of the IMU and the magnetic sensor detector at time k-1, ∈vbx(k-1)、εvby(k-1)、εvbz(k-1) x, y, z axis velocity noise at time k; epsilonpn(k-1)、εpe(k-1)、εpd(k-1) is the north, east, and ground position noise at time k-1;
3. determine if the sub-detector D4 is malfunctioning
Figure BDA0002148634710000062
In the above formula, τ4A fault determination threshold; t is4Is a fault detection result; if T is41, IMU or magnetic sensor failure; if T is40, the IMU and magnetic sensor are fault-free;
constructing an IMU/GPS sub-detector D5:
1. calculating the statistical parameters of the sub-detector D5:
east speed was calculated using state prediction at time k:
Figure BDA0002148634710000063
calculating the northbound speed using state prediction at time k:
Figure BDA0002148634710000064
Figure BDA0002148634710000065
A5(k)＝H5(k)P5(k|k-1)H5(k)+R5(k)
λ5(k)＝r5(k)A5(k)-1r5(k)
H5(k)＝[Υ2×4 Ω2×3 02×3]
Figure BDA0002148634710000066
Figure BDA0002148634710000067
in the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000069
the east speed prediction and the north speed at the k moment are obtained through state prediction calculation; r is5(k) Detecting the northbound speed and the eastern speed residual errors of the system for the IMU and the GPS at the moment k; vNG(k)、VEG(k) The north speed and the east speed output by the GPS at the moment k; a. the5(k) Residual variance of the IMU and the GPS detection system at the moment k; h5(k) Measuring coefficient matrixes of an IMU and a GPS detection system at the moment k; p5(k|k-1)＝P4(k | k-1) is a one-step predicted mean square error from time k-1 to time k;
Figure BDA00021486347100000712
is the noise matrix at time k and,
Figure BDA00021486347100000713
the north and east speed noises of the GPS at the time k; lambda [ alpha ]5(k) The statistical parameters at the k moment;
3. determine if detector D5 is malfunctioning:
Figure BDA0002148634710000071
in the above formula, τ5A fault determination threshold; t is5Is a fault detection result; if T is51, IMU or GPS failure; if T is50, IMU and GPS are fault-free;
constructing IMU/barometer sub-detector D6:
1. calculate statistical parameters for detector D6:
height is calculated using state prediction at time k:
Figure BDA0002148634710000072
Figure BDA0002148634710000073
A6(k)＝H6(k)P6(k|k-1)H6(k)+R6(k)
λ6(k)＝r6(k)A6(k)-1r6(k)
in the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000074
the height at the k moment is obtained through state prediction calculation; r is6(k) The height residual error of the IMU and the barometer detection system at the moment k is obtained; h isb(k) The height output by the barometer at time k; a. the6(k) The residual variance of the IMU and the barometer detection system at the moment k; h6(k) 1 is a measurement coefficient matrix of the IMU and the barometer detection system at the moment k; p6(k|k-1)＝P4(k | k-1) from time k-1 to time kPredicting the mean square error in one step; r6(k)＝diag([εhb(k)]2) Is the noise matrix at time k, epsilonhb(k) Altitude noise of the barometer at time k; lambda [ alpha ]6(k) The statistical parameters at the k moment;
3. judging whether the sub detector D6 fails:
Figure BDA0002148634710000075
in the above formula, τ6A fault determination threshold; t is6Is a fault detection result; if T is61, IMU or barometer failure; if T is60, the IMU and barometer are not faulty.
Further, in step three, a fault location strategy is first constructed according to S1:
Figure BDA0002148634710000076
Figure BDA0002148634710000077
Figure BDA0002148634710000078
then, a global fault location strategy is constructed according to S1 and S2:
Figure BDA0002148634710000079
Figure BDA00021486347100000710
Figure BDA00021486347100000711
in the above formula, FGACC、FDM、FIMU、Fmag、Fgps、FbaroFault positioning functions of the angular accelerometer, the dynamic model, the IMU, the magnetic sensor, the GPS and the barometer are respectively adopted, the fault positioning function is equal to 1, which indicates that the corresponding sensor has a fault, and the fault positioning function is equal to 0, which indicates that the corresponding sensor has no fault; "|" represents the logical operator "or"; "&"represents the logical operator" and "; "-" denotes the logical operator "NOT".
Further, in step four, the fault conditions are classified into the following 7 conditions:
case 1: the airborne sensor has no fault:
(1) and (3) calculating a state and mean square error predicted value at the k moment:
Figure BDA0002148634710000081
Figure BDA0002148634710000082
Figure BDA0002148634710000083
Figure BDA0002148634710000084
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure BDA0002148634710000085
Figure BDA0002148634710000086
Figure BDA0002148634710000087
Figure BDA0002148634710000088
Figure BDA0002148634710000089
Figure BDA00021486347100000810
P(k|k-1)＝F(k)P(k-1)FT(k)+G(k)Q(k)GT(k)
Figure BDA00021486347100000811
Figure BDA00021486347100000812
Q(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1) εωx(k-1) εωy(k-1) εωz(k-1) εvbx(k-1) εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2)
in the above formula, the first and second carbon atoms are,
Figure BDA00021486347100000813
the components of the angular speed of the machine system relative to the navigation system at the moment k on the x, y and z axes of the machine system; p (k | k-1) is a one-step prediction mean square error from the k-1 moment to the k moment; p (k-1) is the mean square error at the time of k-1; f (k) is a state coefficient matrix at the time k; g (k-1) is a noise coefficient matrix at the k-1 moment; q (k-1) is a noise coefficient matrix at the k-1 moment; epsilonωx(k-1)、εωy(k-1)、εωz(k-1) is the angular velocity noise of x, y and z axes at the time of k-1;
(2) calculating gain K of extended Kalman filter of ith sub-filter at moment KCi：
KCi(k)＝P(k|k-1)HCi(k)T[HCi(k)P(k|k-1)HCi(k)T+RCi(k)]-1
Figure BDA0002148634710000091
Figure BDA0002148634710000092
Figure BDA0002148634710000093
In the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000096
GPS ground direction velocity noise for time k;
Figure BDA0002148634710000097
GPS ground direction velocity noise for time k;
Figure BDA0002148634710000098
the noise of the east and north positions of the GPS at the time k;
(3) calculating the state estimation value X of the extended Kalman filter of the ith sub-filter at the moment kCi(k) And mean square error PCi(k)：
XCi(k)＝X(k|k-1)+KCi(k)[YCi(k)-hCi(XCi(k|k-1))]
PCi(k)＝[I-KCi(k)HCi(k)]P(k|k-1)
Figure BDA0002148634710000094
YCi(k) Measure for the ith sub-filter quantity:
Figure BDA0002148634710000095
Figure BDA0002148634710000101
in the above formula, VDG(k)、PNG(k)、PEG(k) The GPS ground speed, the north position and the east position at the moment k; h isbaro(k) The height of the barometer at time k;
(4) computing a global fusion state estimate X for a sub-filter at time kg(k) And mean square error estimate Pg(k)：
Xg(k)＝Pg(k)[PC1(k)-1XC1(k)+PC2(k)-1XC2(k)+PC3(k)-1XC3(k)+PC4(k)-1XC4(k)]-1
Pg(k)＝[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
X(k)＝Xg(k)
P(k)＝4Pg(k)
In the above formula, X (k), P (k) are the state reset and the mean square error reset of each sub-filter at the time k;
case 2: upon failure of the IMU, the IMU is isolated and the velocity differential equation using the accelerometer in the IMU as input is modified as follows:
Figure BDA0002148634710000102
Figure BDA0002148634710000103
Figure BDA0002148634710000104
a sub-filter using a gyroscope in the IMU as measurement information is cut off and does not participate in fusion filtering; modifying the state estimation correlation matrix based on case 1 according to the sensor isolation condition;
case 3: when the dynamic model fails, the filtering updating process is the same as that in the case 1;
case 4: upon failure of the angular accelerometer, the angular accelerometer is isolated and the differential equation of angular velocity using the angular accelerometer as input is modified as follows:
Figure BDA0002148634710000105
Figure BDA0002148634710000106
Figure BDA0002148634710000107
the measurement update process is the same as case 1;
case 5: when the magnetic sensor fails, the magnetic sensor is isolated and redundant course information is used for doingMeasuring the quantity; in the process of updating measurement, Y in the step (3) in the case 1 is usedC2(k)＝ψm(k) Modifying the data output by the redundant course sensor;
case 6: when the GPS fails, the GPS is isolated, and redundant speed position information is used as measurement; in the process of measurement updating, Y in the step (3) in the case 1 is usedC3(k)＝[VNG(k) VEG(k) VDG(k) PNG(k) PEG(k)]TModifying the data output by the redundant speed measurement sensor;
case 7: when the barometer is in fault, the barometer is isolated, and redundant height information is used as measurement; in the process of measurement updating, Y in the step (3) in the case 1 is usedC4(k)＝hbaro(k) Modified to use the data output by the redundant height measurement sensor.
Adopt the beneficial effect that above-mentioned technical scheme brought:
according to the invention, by introducing the dynamic model and the angular accelerometer, fault detection of the angular accelerometer, the dynamic model, the IMU and the measuring sensor can be realized, and meanwhile, the dynamic model can be used for completely replacing the fault IMU to carry out navigation calculation, so that the fault tolerance of the navigation system is improved. And when the IMU fails, the output of the angular accelerometer can be used as the input of the dynamic model, so that the estimation precision of the dynamic model under the large attitude angle change is improved.
Drawings
FIG. 1 is an overall fault tolerant block diagram of the present invention;
FIG. 2 is a fault diagnosis block diagram of the present invention;
FIG. 3 is a block diagram of filtering in the present invention;
FIG. 4 is a flow chart of the present invention;
FIG. 5 is a diagram of the fault detection results of the present invention when an IMU fails;
FIG. 6 is a diagram of x-axis angular velocity estimation results when an IMU fails according to the present invention;
FIG. 7 is a diagram of the y-axis angular velocity estimation result of the present invention upon IMU failure;
FIG. 8 is a graph of the z-axis angular velocity estimation result of the present invention at IMU failure;
FIG. 9 is a graph of the roll angle estimation result of the IMU failure of the present invention;
FIG. 10 is a graph of pitch angle estimation results for an IMU failure according to the present invention;
FIG. 11 is a chart of the course angle estimation result of the present invention upon IMU failure;
FIG. 12 is a graph of x-axis velocity estimates for an IMU fault of the present invention;
FIG. 13 is a graph of the y-axis velocity estimation results of the present invention upon IMU failure.
Detailed Description
The technical scheme of the invention is explained in detail in the following with the accompanying drawings.
The invention designs an analytical redundant aircraft fault-tolerant navigation estimation method, as shown in figure 1, comprising the following steps:
the method comprises the following steps: periodically reading the outputs of a rotor wing rotating speed measuring system, an angular accelerometer, an IMU, a magnetic sensor, a GPS and an air pressure meter of a carrier at the moment k, and estimating the motion acceleration information and the angular acceleration information of the rotor wing aircraft through a dynamic model of the rotor wing aircraft;
step two: utilizing the data output of the airborne sensor in the step one to construct an angular accelerometer/dynamic model/IMU fault detection filter S1 and an IMU/magnetic sensor/GPS/barometer fault detection filter S2; s1 includes three parallel sub-detectors, angular accelerometer/dynamical model sub-detector D1, angular accelerometer/IMU sub-detector D2 and dynamical model/IMU sub-detector D3; s2 includes three parallel sub-detectors, IMU/magnetic sensor sub-detector D4, IMU/GPS sub-detector D5 and IMU/barometer sub-detector D6;
step three: according to the fault detection results of the sub-detectors in the S1, a fault positioning strategy is constructed, fault positioning of the diagonal accelerometer, the dynamic model and the IMU is achieved, and positioning results are output; according to the fault location result of S1 and the fault detection results of each sub-detector in S2, a global fault location strategy is constructed, and a fault sensor is located;
step four: according to the fault positioning result obtained in the step three, a fault isolation strategy is made, a fault sensor is isolated from state filtering estimation, and navigation state estimation is completed;
when the airborne sensor has no fault or a dynamic model has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input, and the gyroscope, the magnetic sensor, the GPS and the barometer in the IMU are used as measuring sensors;
when the IMU fails, an angular accelerometer and a aerodynamic model of a dynamic model are used as state equation input, and a magnetic sensor, a GPS and a barometer are used as measuring sensors;
when the angular accelerometer has a fault, using a pneumatic moment model of a dynamic model and an accelerometer in the IMU as state equation input, and using a gyroscope, a magnetic sensor, a GPS and a barometer in the IMU as measuring sensors;
when the GPS or the barometer or the magnetic sensor has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input to isolate the fault sensor so as not to participate in global fusion filtering;
step five: and correcting the state quantities in S1 and S2 at the time k according to the state estimation result obtained in step four.
In this embodiment, the first step is implemented by the following preferred scheme:
in step one, estimating the motion acceleration information and the angular acceleration information of the rotorcraft by:
Figure BDA0002148634710000121
Figure BDA0002148634710000122
fmz＝kT0+kT1(ω1 2+ω2 2+ω3 2+ω4 2)
Figure BDA0002148634710000123
Figure BDA0002148634710000124
Figure BDA0002148634710000125
wherein f ismx、fmy、fmzThe acceleration of the x axis, the y axis and the z axis of the body system is obtained through calculation of a dynamic model;
Figure BDA0002148634710000126
the angular acceleration, omega, of the x, y and z axes of the body system is calculated by a dynamic modelmzAngular velocity for the z-axis; k is a radical ofHx0、kHx1、kHx2、kHx3、kHy0、kHy1、kHy2、kHy3、kT0、kT1、kx0、kx1、kx2、kx3、ky0、ky1、ky2、ky3、kz0、kz1、kz2、kz3The parameters of the dynamic model of the rotor craft are all constant values;
Figure BDA0002148634710000127
the components of the linear velocity of the machine body system relative to the navigation system on the x and y axes of the machine system; beta is ax、βyThe angular acceleration of the machine body system x and y axes is obtained through an angular accelerometer; omegaiThe number of revolutions of the ith rotor, i ═ 1,2,3,4,
Figure BDA0002148634710000128
angular acceleration which is the ith rotor speed; f. ofax、fayThe acceleration of the body system in x and y axes is obtained by an accelerometer in the IMU.
In this embodiment, the second step is implemented by using the following preferred scheme:
in step two, the method for constructing the angular accelerometer/dynamical model sub-detector D1 is as follows:
1. calculating a z-axis angular acceleration residual using the angular accelerometer output and the kinetic model output:
Figure BDA0002148634710000129
in the above formula, r1(k) Calculating a z-axis angular acceleration residual error for the angular accelerometer and the dynamic model; beta is az(k) Obtaining the z-axis angular acceleration of the machine system at the moment k through the output of the z-axis angular accelerometer;
Figure BDA00021486347100001210
the angular acceleration of the z axis of the organism system is calculated through a dynamic model; abs (, denotes the absolute value;
2. judging whether the sub detector D1 fails:
Figure BDA00021486347100001211
in the above formula, τ1A fault determination threshold; t is1Is a fault detection result; if T is11, then the angular accelerometer or the dynamical model fails; if T is1If the angular accelerometer and the dynamic model are 0, the angular accelerometer and the dynamic model are not in fault;
the method of constructing the angular accelerometer/IMU sub-detector D2 is as follows:
1. calculating x, y, z axis angular velocities using angular accelerometer outputs:
Figure BDA0002148634710000131
in the above formula, ωx(k)、ωy(k)、ωz(k) The angular velocity of the x, y and z axes of the body system at the moment k is output beta through the angular accelerometerx、βy、βzObtaining an integral; omegax(k-1)、ωy(k-1)、ωz(k-1) is the angular speed of the x, y and z axes of the organism system at the time of k-1, and is obtained through the output of a Federal Kalman filter at the time of k-1; Δ T is the discrete sampling time;
2. calculating the statistical parameters of the sub-detector D2:
r2(k)＝[ωgx(k) ωgy(k) ωgz(k)]T-[ωx(k) ωy(k) ωz(k)]T
A2(k)＝H2(k)P2(k|k-1)H2(k)+R2(k)
λ2＝r2(k)A2(k)-1r2(k)
P2(k|k-1)＝F2(k)P2(k-1)F2 T(k)+G2(k-1)Q2(k-1)G2 T(k-1)
in the above formula, r2(k) Outputting calculated x, y and z axis angular velocity residual errors for the IMU and the angular accelerometer at the moment k; omegagx(k)、ωgy(k)、ωgz(k) The angular velocities of the x, y and z axes of the organism system are obtained through gyro output; a. the2(k) Outputting the calculated residual variance for the IMU and the angular accelerometer at the moment k; h2(k)＝[I3×3]Is a matrix of k time measurement coefficients, I3×3Is a 3 × 3 identity matrix; p2(k | k-1) is a one-step prediction mean square error matrix at the time k; r2(k)＝diag([εωgx(k) εωgy(k) εωgz(k)]2) Measuring the noise matrix, epsilon, for time kωgx(k)、εωgy(k)、εωgz(k) Gyro noise of x, y and z axes of the organism system; lambda [ alpha ]2(k) Outputting the calculated statistical parameters for the IMU and the angular accelerometer at the moment k; f2(k)＝[I3×3]Detecting a state coefficient matrix of the system for the IMU and the angular accelerometer at the moment k; p2(k-1) is a mean square error matrix at the time of k-1; g2(k-1)＝[ΔT*I3×3]Detecting a state noise coefficient matrix of the system for the IMU and the angular accelerometer at the moment k-1; q2(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1)]2) The IMU and angular accelerometer detect the state noise, ε, of the system at time k-1βx(k-1)、εβy(k-1)、εβz(k-1) x, y, z axis angular accelerometer noise at time k-1; superscript T denotes transpose;
3. judging whether the sub detector D2 fails:
Figure BDA0002148634710000132
in the above formula, τ2A fault determination threshold; t is2Is a fault detection result; if T is2If 1, then the angular accelerometer or IMU fails; if T is3If 0, the angular accelerometer and the IMU are not in fault;
the method for constructing the kinetic model/IMU subdetector D3 is as follows:
1. calculating z-axis angular velocity using the kinetic model output:
Figure BDA0002148634710000133
in the above formula, ωmz(k-1) is the angular velocity at the k-1 moment and is obtained through the Federal Kalman filter output at the k-1 moment; omegamz(k) Angular velocity at time k;
Figure BDA0002148634710000134
the angular acceleration at the moment k is obtained through a dynamic model; Δ T is the discrete sampling time;
2. the residuals and statistical parameters of sub-detector D3 are calculated:
r3a(k)＝abs(faz(k)-fmz(k))
r3b(k)＝ωgz(k)-ωmz(k)
A3b(k)＝H3b(k)P3b(k|k-1)H3b(k)+R3b(k)
λ3b＝r3b(k)A3b(k)-1r3b(k)
P3b(k|k-1)＝F3b(k)P3b(k-1)F3b T(k)+G3b(k-1)Q3b(k-1)G3b T(k-1)
in the above formula, r3a(k) Calculating a z-axis acceleration residual error for the IMU at the moment k and the dynamic model; f. ofaz(k) The acceleration of the z axis of the machine system at the moment k is obtained through an accelerometer; r is3b(k) Calculating a z-axis angular velocity residual error for the IMU at the moment k and the dynamic model; a. the3b(k) Calculating a z-axis angular velocity residual variance for the IMU at the moment k and the dynamic model; h3b(k) 1 is a k moment measurement coefficient matrix; p3b(k | k-1) is a one-step prediction mean square error matrix at the time k; r3b(k)＝diag([εωgz(k)]2) Measuring a noise matrix for time k; lambda [ alpha ]3b(k) Calculating a z-axis angular velocity statistical parameter for the IMU at the moment k and the dynamic model;
Figure BDA0002148634710000141
a state coefficient matrix of a z-axis gyroscope and a dynamic model detection system at the moment k; p3b(k-1) is a mean square error matrix at the time of k-1; g3b(k-1) ═ 1 is a state noise coefficient matrix of the z-axis gyroscope and the dynamic model detection system at the moment of k-1; q3b(k-1)＝diag([εωmz(k-1)]2) For the state noise, ε, of the z-axis gyro and the dynamical model detection system at time k-1ωmz(k-1) obtaining the z-axis angular velocity noise of the machine system through a dynamic model at the moment of k-1;
3. judging whether the sub detector D3 fails:
Figure BDA0002148634710000142
in the above formula, τ3a、τ3bA fault determination threshold; t is3a、T3bIs a fault detection result; if T is3a(k) 1 or T3b(k) 1, IMU or powerLearning a model fault; if T is3a(k) 0 and T3b(k) 0, the IMU and the kinetic model are fault free.
In step two, the state predictions of the sub-detectors D4, D5, D6 at time k are calculated:
q0(k)＝q0(k-1)-0.5ωgx(k)q1(k-1)-0.5ωgy(k)q2(k-1)-0.5ωgz(k)q3(k-1)
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure BDA0002148634710000143
Figure BDA0002148634710000144
Figure BDA0002148634710000145
Figure BDA0002148634710000146
Figure BDA0002148634710000147
Figure BDA0002148634710000148
in the above formula, q0(k)、q1(k)、q2(k)、q3(k) Is a k-time quaternion obtained through a state equation; q. q.s0(k-1)、q1(k-1)、q2(k-1)、q3(k-1) is a quaternion at the moment k-1 and is obtained through the output of a federal Kalman filter at the moment k-1;
Figure BDA0002148634710000151
the component of the linear velocity of the body system relative to the navigation system at the moment k, which is obtained through a state equation, on the z axis of the body system is obtained;
Figure BDA0002148634710000152
the component of the linear velocity of the body system relative to the navigation system at the moment k-1 on the z axis of the body system is obtained through the Federal Kalman filter output at the moment k-1; g is the acceleration of gravity; pN(k)、PE(k)、PD(k) The north, east and earth positions at the k moment are obtained through a state equation; pN(k-1)、PE(k-1)、PD(k-1) is north, east and earth positions at the moment k-1 and is obtained through the Federal Kalman filter output at the moment k-1;
constructing IMU/magnetic sensor sub-detector D4:
1. calculating the statistical parameters of the sub-detector D4:
heading is calculated using state prediction at time k:
Figure BDA0002148634710000153
Figure BDA0002148634710000154
A4(k)＝H4(k)P4(k|k-1)H4(k)+R4(k)
λ4(k)＝r4(k)A4(k)-1r4(k)
P4(k|k-1)＝F4(k)P4(k-1)F4 T(k)+G4(k-1)Q4(k-1)G4 T(k-1)
Figure BDA0002148634710000155
Figure BDA0002148634710000156
Figure BDA0002148634710000157
Figure BDA0002148634710000158
Figure BDA0002148634710000159
Figure BDA00021486347100001510
Figure BDA0002148634710000161
Figure BDA0002148634710000162
Q4(k-1)＝diag([εωgx(k-1) εωgy(k-1) εωgz(k-1) εvbx(k-1)εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2)
in the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000163
the course angle at the k moment is obtained through state prediction calculation; r is4(k) Is the heading angle residual of the D4 detector at time k; psim(k) The heading output by the magnetic sensor at the moment k; a. the4(k) The residual variance of the D4 detector at time k; h4(k) A measurement coefficient matrix of a D4 detector at the time k; p4(k | k-1) is a one-step predicted mean square error from time k-1 to time k; r4(k)＝diag([εψm(k)]2) Is the noise matrix at time k, epsilonψm(k) The high noise of the magnetic sensor at time k; lambda [ alpha ]4(k) The statistical parameters at the k moment; 0i×jIs a zero matrix of i x j; i isi×jIs an identity matrix of i x j; f4(k) A state coefficient matrix of the IMU and the magnetic sensor detector at the moment k; p4(k-1) is a mean square error matrix at the time of k-1; g4(k-1) is a state noise coefficient matrix of the IMU and the magnetic sensor detector at the time of k-1; q4(k-1) is the state noise of the IMU and the magnetic sensor detector at time k-1, ∈vbx(k-1)、εvby(k-1)、εvbz(k-1) x, y, z axis velocity noise at time k; epsilonpn(k-1)、εpe(k-1)、εpd(k-1) is the north, east, and ground position noise at time k-1;
4. determine if the sub-detector D4 is malfunctioning
Figure BDA0002148634710000164
In the above formula, τ4A fault determination threshold; t is4Is a fault detection result; if T is41, IMU or magnetic sensor failure; if T is40, the IMU and magnetic sensor are fault-free;
constructing an IMU/GPS sub-detector D5:
1. calculating the statistical parameters of the sub-detector D5:
east speed was calculated using state prediction at time k:
Figure BDA0002148634710000165
calculating the northbound speed using state prediction at time k:
Figure BDA0002148634710000166
Figure BDA0002148634710000167
A5(k)＝H5(k)P5(k|k-1)H5(k)+R5(k)
λ5(k)＝r5(k)A5(k)-1r5(k)
H5(k)＝[Υ2×4 Ω2×3 02×3]
Figure BDA0002148634710000171
Figure BDA0002148634710000172
in the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000174
the east speed prediction and the north speed at the k moment are obtained through state prediction calculation; r is5(k) Detecting the northbound speed and the eastern speed residual errors of the system for the IMU and the GPS at the moment k; vNG(k)、VEG(k) The north speed and the east speed output by the GPS at the moment k; a. the5(k) Residual variance of the IMU and the GPS detection system at the moment k; h5(k) Measuring coefficient matrixes of an IMU and a GPS detection system at the moment k; p5(k|k-1)＝P4(k | k-1) is a one-step predicted mean square error from time k-1 to time k;
Figure BDA0002148634710000175
is the noise matrix at time k and,
Figure BDA0002148634710000176
the north and east speed noises of the GPS at the time k; lambda [ alpha ]5(k) The statistical parameters at the k moment;
4. determine if detector D5 is malfunctioning:
Figure BDA0002148634710000177
in the above formula, τ5A fault determination threshold; t is5Is a fault detection result; if T is51, IMU or GPS failure; if T is50, IMU and GPS are fault-free;
constructing IMU/barometer sub-detector D6:
1. calculate statistical parameters for detector D6:
height is calculated using state prediction at time k:
Figure BDA0002148634710000178
Figure BDA0002148634710000179
A6(k)＝H6(k)P6(k|k-1)H6(k)+R6(k)
λ6(k)＝r6(k)A6(k)-1r6(k)
in the above formula, the first and second carbon atoms are,
Figure BDA00021486347100001710
the height at the k moment is obtained through state prediction calculation; r is6(k) The height residual error of the IMU and the barometer detection system at the moment k is obtained; h isb(k) As barometer output at time kThe height of (d); a. the6(k) The residual variance of the IMU and the barometer detection system at the moment k; h6(k) 1 is a measurement coefficient matrix of the IMU and the barometer detection system at the moment k; p6(k|k-1)＝P4(k | k-1) is a one-step predicted mean square error from time k-1 to time k; r6(k)＝diag([εhb(k)]2) Is the noise matrix at time k, epsilonhb(k) Altitude noise of the barometer at time k; lambda [ alpha ]6(k) The statistical parameters at the k moment;
4. judging whether the sub detector D6 fails:
Figure BDA00021486347100001711
in the above formula, τ6A fault determination threshold; t is6Is a fault detection result; if T is61, IMU or barometer failure; if T is60, the IMU and barometer are not faulty.
In this embodiment, the step three is implemented by using the following preferred scheme:
in step three, a fault location strategy is first constructed according to S1:
Figure BDA0002148634710000181
Figure BDA0002148634710000182
Figure BDA0002148634710000183
then, a global fault location strategy is constructed according to S1 and S2:
Figure BDA0002148634710000184
Figure BDA0002148634710000185
Figure BDA0002148634710000186
in the above formula, FGACC、FDM、FIMU、Fmag、Fgps、FbaroFault positioning functions of the angular accelerometer, the dynamic model, the IMU, the magnetic sensor, the GPS and the barometer are respectively adopted, the fault positioning function is equal to 1, which indicates that the corresponding sensor has a fault, and the fault positioning function is equal to 0, which indicates that the corresponding sensor has no fault; "|" represents the logical operator "or"; "&"represents the logical operator" and "; "-" denotes the logical operator "NOT".
The above-described failure diagnosis process is shown in fig. 2.
In this embodiment, the step four is implemented by using the following preferred scheme:
in step four, the fault conditions are classified into the following 7 conditions:
case 1: the airborne sensor has no fault:
(1) and (3) calculating a state and mean square error predicted value at the k moment:
Figure BDA0002148634710000187
Figure BDA0002148634710000188
Figure BDA0002148634710000189
Figure BDA00021486347100001810
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure BDA00021486347100001811
Figure BDA00021486347100001812
Figure BDA00021486347100001813
Figure BDA00021486347100001814
Figure BDA00021486347100001815
Figure BDA00021486347100001816
P(k|k-1)＝F(k)P(k-1)FT(k)+G(k)Q(k)GT(k)
Figure BDA0002148634710000191
Figure BDA0002148634710000192
Q(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1) εωx(k-1) εωy(k-1) εωz(k-1) εvbx(k-1) εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2) In the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000193
the components of the angular speed of the machine system relative to the navigation system at the moment k on the x, y and z axes of the machine system; p (k | k-1) is a one-step prediction mean square error from the k-1 moment to the k moment; p (k-1) is the mean square error at the time of k-1; f (k) is a state coefficient matrix at the time k; g (k-1) is a noise coefficient matrix at the k-1 moment; q (k-1) is a noise coefficient matrix at the k-1 moment; epsilonωx(k-1)、εωy(k-1)、εωz(k-1) is the angular velocity noise of x, y and z axes at the time of k-1;
(2) calculating gain K of extended Kalman filter of ith sub-filter at moment KCi：
KCi(k)＝P(k|k-1)HCi(k)T[HCi(k)P(k|k-1)HCi(k)T+RCi(k)]-1
Figure BDA0002148634710000194
Figure BDA0002148634710000195
Figure BDA0002148634710000196
In the above formula, the first and second carbon atoms are,
Figure BDA0002148634710000197
GPS ground direction velocity noise for time k;
Figure BDA0002148634710000198
GPS ground direction velocity noise for time k;
Figure BDA0002148634710000199
the noise of the east and north positions of the GPS at the time k;
(3) calculating the state estimation value X of the extended Kalman filter of the ith sub-filter at the moment kCi(k) And mean square error PCi(k)：
XCi(k)＝X(k|k-1)+KCi(k)[YCi(k)-hCi(XCi(k|k-1))]
PCi(k)＝[I-KCi(k)HCi(k)]P(k|k-1)
Figure BDA0002148634710000201
YCi(k) Measure for the ith sub-filter quantity:
Figure BDA0002148634710000202
Figure BDA0002148634710000203
in the above formula, VDG(k)、PNG(k)、PEG(k) The GPS ground speed, the north position and the east position at the moment k; h isbaro(k) The height of the barometer at time k;
(4) computing a global fusion state estimate X for a sub-filter at time kg(k) And mean square error estimate Pg(k)：
Xg(k)＝Pg(k)[PC1(k)-1XC1(k)+PC2(k)-1XC2(k)+PC3(k)-1XC3(k)+PC4(k)-1XC4(k)]-1
Pg(k)＝[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
X(k)＝Xg(k)
P(k)＝4Pg(k)
In the above formula, X (k), P (k) are the state reset and the mean square error reset of each sub-filter at the time k;
case 2: upon failure of the IMU, the IMU is isolated and the velocity differential equation using the IMU accelerometer as input is modified as follows:
Figure BDA0002148634710000204
Figure BDA0002148634710000205
Figure BDA0002148634710000206
a sub-filter using a gyroscope in the IMU as measurement information is cut off and does not participate in fusion filtering; modifying the state estimation correlation matrix based on case 1 according to the sensor isolation condition;
case 3: when the dynamic model fails, the filtering updating process is the same as that in the case 1;
case 4: upon failure of the angular accelerometer, the angular accelerometer is isolated and the differential equation of angular velocity using the angular accelerometer as input is modified as follows:
Figure BDA0002148634710000207
Figure BDA0002148634710000211
Figure BDA0002148634710000212
the measurement update process is the same as case 1;
case 5: when the magnetic sensor fails, the magnetic sensor is isolated, and redundant course information is used as measurement; the measurement updating process is modified on the basis of the case 1 according to the measurement equation, and Y in the step (3) in the case 1 is usedC2(k)＝ψm(k) Modifying the data output by the redundant course sensor;
case 6: when the GPS fails, the GPS is isolated, and redundant speed position information is used as measurement; the measurement updating process is modified on the basis of the case 1 according to the measurement equation, and Y in the step (3) in the case 1 is obtainedC3(k)＝[VNG(k) VEG(k) VDG(k) PNG(k) PEG(k)]TModifying the data output by the redundant speed measurement sensor;
case 7: when the barometer is in fault, the barometer is isolated, and redundant height information is used as measurement; the measurement updating process is modified on the basis of the case 1 according to the measurement equation, and Y in the step (3) in the case 1 is obtainedC4(k)＝hbaro(k) Modified to use the data output by the redundant height measurement sensor.
The filtering process described above is illustrated in fig. 3.
In this embodiment, the step five is implemented by adopting the following preferred scheme:
in step five, the angular velocity of the angular accelerometer/IMU sub-detector D2, the angular velocity of the dynamical model/IMU sub-detector D3, and the quaternion, linear velocity, altitude of the IMU/magnetic sensor/GPS/barometer fault detection filter S2 are updated with the state estimation results in step four.
The whole process is shown in fig. 4.
The following describes the effect of the present invention through a specific simulation example:
the fault-tolerant estimation performance after the method is used is verified in a mode of performing semi-physical simulation verification on actual flight data. Verifying the fault detection performance of the carrier, and acquiring data of the aircraft under different maneuvering motions: hover, roll motion, side-fly motion along a pitch axis, hover, pitch motion, side-fly motion along a pitch axis, rotation of a slow heading axis, rotation of a fast heading axis. Because the used unmanned aerial vehicle lacks an angular accelerometer sensor, the output of the angular accelerometer is simulated by using the gyroscopic data differentiation, and a constant simulation fault is injected into different sensor data for fault detection and verification.
Fig. 5 shows the fault detection result after the method of the present invention is used when the zero offset fault is injected into the IMU data. As can be seen from the figure, the detection system can timely and accurately detect the IMU fault.
FIG. 6 is an x-axis angular velocity estimation result after the method of the present invention is employed when injecting a zero-offset fault in IMU data. As can be seen in the figure, the angular velocity estimation error is less than 10 degrees/second.
FIG. 7 shows the y-axis angular velocity estimation result after the method of the present invention is applied when a zero offset fault is injected into IMU data. As can be seen in the figure, the angular velocity estimation error is less than 10 degrees/second.
FIG. 8 is a z-axis angular velocity estimation result after the method of the present invention is employed when a zero offset fault is injected in IMU data. As can be seen in the figure, the angular velocity estimation error is less than 2 degrees/second.
FIG. 9 shows the roll angle estimation result after the method of the present invention is applied when a zero-offset fault is injected into IMU data. As can be seen in the figure, the roll angle estimation error is less than 5 degrees.
Fig. 10 shows the estimation result of the pitch angle after the method of the present invention is used when the zero offset fault is injected into the IMU data. As can be seen in the figure, the pitch angle estimation error is less than 5 degrees.
FIG. 11 is a result of course angle estimation after the method of the present invention is applied when a zero-offset fault is injected into IMU data. As can be seen from the figure, the heading angle estimation error is less than 1 degree.
FIG. 12 is an x-axis velocity estimation result after the method of the present invention is employed when injecting a zero-offset fault in IMU data. As can be seen in the figure, the x-axis velocity estimation error is less than 0.2 m/s.
FIG. 13 is a y-axis velocity estimation result after the method of the present invention is employed when injecting a zero-offset fault in IMU data. As can be seen in the figure, the y-axis velocity estimation error is less than 0.2 m/s.

Claims (5)

1. An analytical redundant aircraft fault-tolerant navigation estimation method is characterized by comprising the following steps:
the method comprises the following steps: periodically reading the outputs of a rotor wing rotating speed measuring system, an angular accelerometer, an IMU, a magnetic sensor, a GPS and an air pressure meter of a carrier at the moment k, and estimating the motion acceleration information and the angular acceleration information of the rotor wing aircraft through a dynamic model of the rotor wing aircraft;
step two: utilizing the data output of the airborne sensor in the step one to construct an angular accelerometer/dynamic model/IMU fault detection filter S1 and an IMU/magnetic sensor/GPS/barometer fault detection filter S2; s1 includes three parallel sub-detectors, angular accelerometer/dynamical model sub-detector D1, angular accelerometer/IMU sub-detector D2 and dynamical model/IMU sub-detector D3; s2 includes three parallel sub-detectors, IMU/magnetic sensor sub-detector D4, IMU/GPS sub-detector D5 and IMU/barometer sub-detector D6;
the method of constructing the angular accelerometer/dynamical model sub-detector D1 is as follows:
calculating a z-axis angular acceleration residual using the angular accelerometer output and the kinetic model output:
Figure FDA0002974155430000011
in the above formula, r1(k) Z-axis angular acceleration calculated for angular accelerometer and dynamical modelMeasuring residual errors; beta is az(k) Obtaining the z-axis angular acceleration of the machine system at the moment k through the output of the z-axis angular accelerometer;
Figure FDA0002974155430000012
the angular acceleration of the z axis of the organism system is calculated through a dynamic model; abs (, denotes the absolute value;
judging whether the sub detector D1 fails:
Figure FDA0002974155430000013
in the above formula, τ1A fault determination threshold; t is1Is a fault detection result; if T is11, then the angular accelerometer or the dynamical model fails; if T is1If the angular accelerometer and the dynamic model are 0, the angular accelerometer and the dynamic model are not in fault;
the method of constructing the angular accelerometer/IMU sub-detector D2 is as follows:
calculating x, y, z axis angular velocities using angular accelerometer outputs:
Figure FDA0002974155430000021
in the above formula, ωx(k)、ωy(k)、ωz(k) The angular velocity of the x, y and z axes of the body system at the moment k is output beta through the angular accelerometerx、βy、βzObtaining an integral; omegax(k-1)、ωy(k-1)、ωz(k-1) is the angular speed of the x, y and z axes of the organism system at the time of k-1, and is obtained through the output of a Federal Kalman filter at the time of k-1; Δ T is the discrete sampling time;
calculating the statistical parameters of the sub-detector D2:
r2(k)＝[ωgx(k) ωgy(k) ωgz(k)]T-[ωx(k) ωy(k) ωz(k)]T
A2(k)＝H2(k)P2(k|k-1)H2(k)+R2(k)
λ2(k)＝r2(k)A2(k)-1r2(k)
P2(k|k-1)＝F2(k)P2(k-1)F2 T(k)+G2(k-1)Q2(k-1)G2 T(k-1)
in the above formula, r2(k) Outputting calculated x, y and z axis angular velocity residual errors for the IMU and the angular accelerometer at the moment k; omegagx(k)、ωgy(k)、ωgz(k) The angular velocities of the x, y and z axes of the organism system are obtained through gyro output; a. the2(k) Outputting the calculated residual variance for the IMU and the angular accelerometer at the moment k; h2(k)＝[I3×3]Is a matrix of k time measurement coefficients, I3×3Is a 3 × 3 identity matrix; p2(k | k-1) is a one-step prediction mean square error matrix at the time k; r2(k)＝diag([εωgx(k) εωgy(k) εωgz(k)]2) Measuring the noise matrix, epsilon, for time kωgx(k)、εωgy(k)、εωgz(k) Gyro noise of x, y and z axes of the organism system; lambda [ alpha ]2(k) Outputting the calculated statistical parameters for the IMU and the angular accelerometer at the moment k; f2(k)＝[I3×3]Detecting a state coefficient matrix of the system for the IMU and the angular accelerometer at the moment k; p2(k-1) is a mean square error matrix at the time of k-1; g2(k-1)＝[ΔT*I3×3]Detecting a state noise coefficient matrix of the system for the IMU and the angular accelerometer at the moment k-1; q2(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1)]2) The IMU and angular accelerometer detect the state noise, ε, of the system at time k-1βx(k-1)、εβy(k-1)、εβz(k-1) x, y, z axis angular accelerometer noise at time k-1; superscript T denotes transpose;
judging whether the sub detector D2 fails:
Figure FDA0002974155430000031
in the above formula, τ2A fault determination threshold; t is2Is a fault detection result; if T is2If 1, then the angular accelerometer or IMU fails; if T is2If 0, the angular accelerometer and the IMU are not in fault;
the method for constructing the kinetic model/IMU subdetector D3 is as follows:
calculating z-axis angular velocity using the kinetic model output:
Figure FDA0002974155430000032
in the above formula, ωmz(k-1) is the angular velocity at the k-1 moment and is obtained through the Federal Kalman filter output at the k-1 moment; omegamz(k) Angular velocity at time k;
Figure FDA0002974155430000033
the angular acceleration at the moment k is obtained through a dynamic model;
the residuals and statistical parameters of sub-detector D3 are calculated:
r3a(k)＝abs(faz(k)-fmz(k))
r3b(k)＝ωgz(k)-ωmz(k)
A3b(k)＝H3b(k)P3b(k|k-1)H3b(k)+R3b(k)
λ3b＝r3b(k)A3b(k)-1r3b(k)
P3b(k|k-1)＝F3b(k)P3b(k-1)F3b T(k)+G3b(k-1)Q3b(k-1)G3b T(k-1)
in the above formula, r3a(k) Calculating a z-axis acceleration residual error for the IMU at the moment k and the dynamic model; f. ofaz(k) The acceleration of the z axis of the machine system at the moment k is obtained through an accelerometer; f. ofmz(k) Calculating the acceleration of the z axis of the organism system at the moment k through a dynamic model; r is3b(k) Calculating a z-axis angular velocity residual error for the IMU at the moment k and the dynamic model; a. the3b(k) Calculating a z-axis angular velocity residual variance for the IMU at the moment k and the dynamic model; h3b(k) 1 is a k moment measurement coefficient matrix; p3b(k | k-1) is a one-step prediction mean square error matrix at the time k; r3b(k)＝diag([εωgz(k)]2) Measuring a noise matrix for time k; lambda [ alpha ]3b(k) Calculating a z-axis angular velocity statistical parameter for the IMU at the moment k and the dynamic model;
Figure FDA0002974155430000041
a state coefficient matrix of a z-axis gyroscope and a dynamic model detection system at the moment k; p3b(k-1) is a mean square error matrix at the time of k-1; g3b(k-1) ═ 1 is a state noise coefficient matrix of the z-axis gyroscope and the dynamic model detection system at the moment of k-1; q3b(k-1)＝diag([εωmz(k-1)]2) For the state noise, ε, of the z-axis gyro and the dynamical model detection system at time k-1ωmz(k-1) obtaining the z-axis angular velocity noise of the machine system through a dynamic model at the moment of k-1;
judging whether the sub detector D3 fails:
Figure FDA0002974155430000042
in the above formula, τ3a、τ3bA fault determination threshold; t is3a、T3bIs a fault detection result; if T is3a1 or T3b1, IMU or kinetic model failure; if T is3a0 and T3b0, the IMU and the kinetic model are fault-free;
step three: according to the fault detection results of the sub-detectors in the S1, a fault positioning strategy is constructed, fault positioning of the diagonal accelerometer, the dynamic model and the IMU is achieved, and positioning results are output; according to the fault location result of S1 and the fault detection results of each sub-detector in S2, a global fault location strategy is constructed, and a fault sensor is located;
step four: according to the fault positioning result obtained in the step three, a fault isolation strategy is made, a fault sensor is isolated from state filtering estimation, and navigation state estimation is completed;
when the airborne sensor has no fault or a dynamic model has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input, and the gyroscope, the magnetic sensor, the GPS and the barometer in the IMU are used as measuring sensors;
when the IMU fails, an angular accelerometer and a aerodynamic model of a dynamic model are used as state equation input, and a magnetic sensor, a GPS and a barometer are used as measuring sensors;
when the angular accelerometer has a fault, using a pneumatic moment model of a dynamic model and an accelerometer in the IMU as state equation input, and using a gyroscope, a magnetic sensor, a GPS and a barometer in the IMU as measuring sensors;
when the GPS or the barometer or the magnetic sensor has a fault, the angular accelerometer and the accelerometer of the IMU are used as state equation input to isolate the fault sensor so as not to participate in global fusion filtering;
step five: and correcting the state quantities in S1 and S2 at the time k according to the state estimation result obtained in step four.
2. The analytically redundant, aircraft fault-tolerant navigation estimation method of claim 1, wherein in step one, the kinematic acceleration information and the angular acceleration information of the rotorcraft are estimated by:
Figure FDA0002974155430000051
Figure FDA0002974155430000052
fmz＝kT0+kT1(ω1 2+ω2 2+ω3 2+ω4 2)
Figure FDA0002974155430000053
Figure FDA0002974155430000054
Figure FDA0002974155430000055
wherein f ismx、fmy、fmzThe acceleration of the x axis, the y axis and the z axis of the body system is obtained through calculation of a dynamic model;
Figure FDA0002974155430000056
the angular acceleration, omega, of the x, y and z axes of the body system is calculated by a dynamic modelmzAngular velocity for the z-axis; k is a radical ofHx0、kHx1、kHx2、kHx3、kHy0、kHy1、kHy2、kHy3、kT0、kT1、kx0、kx1、kx2、kx3、ky0、ky1、ky2、ky3、kz0、kz1、kz2、kz3The parameters of the dynamic model of the rotor craft are all constant values;
Figure FDA0002974155430000057
the components of the linear velocity of the machine body system relative to the navigation system on the x and y axes of the machine system; beta is ax、βyThe angular acceleration of the machine body system x and y axes is obtained through an angular accelerometer; omegaiThe number of revolutions of the ith rotor, i ═ 1,2,3,4,
Figure FDA0002974155430000058
angular acceleration for the ith rotor speedDegree; f. ofax、fayThe acceleration of the body system in x and y axes is obtained by an accelerometer in the IMU.
3. The analytical redundant aircraft fault-tolerant navigation estimation method according to claim 1, wherein in step two, the state prediction of the k-time sub-detectors D4, D5, D6 is calculated:
Figure FDA0002974155430000061
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure FDA0002974155430000062
Figure FDA0002974155430000063
Figure FDA0002974155430000064
Figure FDA0002974155430000065
Figure FDA0002974155430000066
Figure FDA0002974155430000067
in the above formula, q0(k)、q1(k)、q2(k)、q3(k) Is a k-time quaternion obtained through a state equation; q. q.s0(k-1)、q1(k-1)、q2(k-1)、q3(k-1) is a quaternion at the moment k-1 and is obtained through the output of a federal Kalman filter at the moment k-1;
Figure FDA0002974155430000068
the component of the linear velocity of the body system relative to the navigation system at the moment k, which is obtained through a state equation, on the z axis of the body system is obtained;
Figure FDA0002974155430000069
the component of the linear velocity of the body system relative to the navigation system at the moment k-1 on the z axis of the body system is obtained through the Federal Kalman filter output at the moment k-1; g is the acceleration of gravity; pN(k)、PE(k)、PD(k) The north, east and earth positions at the k moment are obtained through a state equation; pN(k-1)、PE(k-1)、PD(k-1) is north, east and earth positions at the moment k-1 and is obtained through the Federal Kalman filter output at the moment k-1;
constructing IMU/magnetic sensor sub-detector D4:
calculating the statistical parameters of the sub-detector D4:
heading is calculated using state prediction at time k:
Figure FDA0002974155430000071
Figure FDA0002974155430000072
A4(k)＝H4(k)P4(k|k-1)H4(k)+R4(k)
λ4(k)＝r4(k)A4(k)-1r4(k)
P4(k|k-1)＝F4(k)P4(k-1)F4 T(k)+G4(k-1)Q4(k-1)G4 T(k-1)
Figure FDA0002974155430000073
Figure FDA0002974155430000074
Figure FDA0002974155430000075
Figure FDA0002974155430000076
Figure FDA0002974155430000077
Figure FDA0002974155430000078
Figure FDA0002974155430000079
Figure FDA0002974155430000081
Q4(k-1)＝diag([εωgx(k-1) εωgy(k-1) εωgz(k-1) εvbx(k-1) εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2)
in the above formula, the first and second carbon atoms are,
Figure FDA0002974155430000082
the course angle at the k moment is obtained through state prediction calculation; r is4(k) Is the heading angle residual of the D4 detector at time k; psim(k) The heading output by the magnetic sensor at the moment k; a. the4(k) The residual variance of the D4 detector at time k; h4(k) A measurement coefficient matrix of a D4 detector at the time k; p4(k | k-1) is a one-step predicted mean square error from time k-1 to time k; r4(k)＝diag([εψm(k)]2) Is the noise matrix at time k, epsilonψm(k) The high noise of the magnetic sensor at time k; lambda [ alpha ]4(k) The statistical parameters at the k moment; 0i×jIs a zero matrix of i x j; i isi×jIs an identity matrix of i x j; f4(k) A state coefficient matrix of the IMU and the magnetic sensor detector at the moment k; p4(k-1) is a mean square error matrix at the time of k-1; g4(k-1) is a state noise coefficient matrix of the IMU and the magnetic sensor detector at the time of k-1; q4(k-1) is the state noise of the IMU and the magnetic sensor detector at time k-1, ∈vbx(k-1)、εvby(k-1)、εvbz(k-1) x, y, z axis velocity noise at time k; epsilonpn(k-1)、εpe(k-1)、εpd(k-1) is the north, east, and ground position noise at time k-1;
determine if the sub-detector D4 is malfunctioning
Figure FDA0002974155430000083
In the above formula, τ4A fault determination threshold; t is4Is a fault detection result; if T is41, IMU or magnetic sensor failure; if T is40, the IMU and magnetic sensor are fault-free;
constructing an IMU/GPS sub-detector D5:
calculating the statistical parameters of the sub-detector D5:
east speed was calculated using state prediction at time k:
Figure FDA0002974155430000091
calculating the northbound speed using state prediction at time k:
Figure FDA0002974155430000092
Figure FDA0002974155430000093
A5(k)＝H5(k)P5(k|k-1)H5(k)+R5(k)
λ5(k)＝r5(k)A5(k)-1r5(k)
H5(k)＝[Υ2×4 Ω2×3 02×3]
Figure FDA0002974155430000094
Figure FDA0002974155430000095
in the above formula, the first and second carbon atoms are,
Figure FDA0002974155430000096
the east speed prediction and the north speed at the k moment are obtained through state prediction calculation; r is5(k) Detecting the northbound speed and the eastern speed residual errors of the system for the IMU and the GPS at the moment k; vNG(k)、VEG(k) The north speed and the east speed output by the GPS at the moment k; a. the5(k) Residual variance of the IMU and the GPS detection system at the moment k; h5(k) Measuring coefficient matrixes of an IMU and a GPS detection system at the moment k; p5(k|k-1)＝P4(k | k-1) is a one-step predicted mean square error from time k-1 to time k;
Figure FDA0002974155430000097
is the noise matrix at time k and,
Figure FDA0002974155430000098
the north and east speed noises of the GPS at the time k; lambda [ alpha ]5(k) The statistical parameters at the k moment;
determine if detector D5 is malfunctioning:
Figure FDA0002974155430000099
in the above formula, τ5A fault determination threshold; t is5Is a fault detection result; if T is51, IMU or GPS failure; if T is50, IMU and GPS are fault-free;
constructing IMU/barometer sub-detector D6:
calculate statistical parameters for detector D6:
height is calculated using state prediction at time k:
Figure FDA0002974155430000101
Figure FDA0002974155430000102
A6(k)＝H6(k)P6(k|k-1)H6(k)+R6(k)
λ6(k)＝r6(k)A6(k)-1r6(k)
in the above formula, the first and second carbon atoms are,
Figure FDA0002974155430000103
the height at the k moment is obtained through state prediction calculation; r is6(k) The height residual error of the IMU and the barometer detection system at the moment k is obtained; h isb(k) The height output by the barometer at time k; a. the6(k) The residual variance of the IMU and the barometer detection system at the moment k; h6(k) 1 is a measurement coefficient matrix of the IMU and the barometer detection system at the moment k; p6(k|k-1)＝P4(k | k-1) is a one-step predicted mean square error from time k-1 to time k; r6(k)＝diag([εhb(k)]2) Is the noise matrix at time k, epsilonhb(k) Altitude noise of the barometer at time k; lambda [ alpha ]6(k) The statistical parameters at the k moment;
judging whether the sub detector D6 fails:
Figure FDA0002974155430000104
in the above formula, τ6A fault determination threshold; t is6Is a fault detection result; if T is61, IMU or barometer failure; if T is60, the IMU and barometer are not faulty.
4. The analytical redundant aircraft fault-tolerant navigation estimation method of claim 3, wherein in step three, a fault location strategy is first constructed according to S1:
Figure FDA0002974155430000105
Figure FDA0002974155430000106
Figure FDA0002974155430000107
then, a global fault location strategy is constructed according to S1 and S2:
Figure FDA0002974155430000111
Figure FDA0002974155430000112
Figure FDA0002974155430000113
in the above formula, FGACC、FDM、FIMU、Fmag、Fgps、FbaroFault positioning functions of the angular accelerometer, the dynamic model, the IMU, the magnetic sensor, the GPS and the barometer are respectively adopted, the fault positioning function is equal to 1, which indicates that the corresponding sensor has a fault, and the fault positioning function is equal to 0, which indicates that the corresponding sensor has no fault; "|" represents the logical operator "or"; "&"represents the logical operator" and "; "-" denotes the logical operator "NOT".
5. The analytically redundant aircraft fault-tolerant navigation estimation method of claim 3, wherein in step four, fault conditions are classified into the following 7 conditions:
case 1: the airborne sensor has no fault:
(1) and (3) calculating a state and mean square error predicted value at the k moment:
Figure FDA0002974155430000114
Figure FDA0002974155430000115
Figure FDA0002974155430000116
Figure FDA0002974155430000117
q1(k)＝0.5ωgx(k)q0(k-1)+q1(k-1)+0.5ωgz(k)q2(k-1)-0.5ωgy(k)q3(k-1)
q2(k)＝0.5ωgy(k)q0(k-1)-0.5ωgz(k)q1(k-1)+q2(k-1)+0.5ωgx(k)q3(k-1)
q3(k)＝0.5ωgz(k)q0(k-1)+0.5ωgy(k)q1(k-1)-0.5ωgx(k)q2(k-1)+q3(k-1)
Figure FDA0002974155430000118
Figure FDA0002974155430000119
Figure FDA00029741554300001110
Figure FDA0002974155430000121
Figure FDA0002974155430000122
Figure FDA0002974155430000123
P(k|k-1)＝F(k)P(k-1)FT(k)+G(k)Q(k)GT(k)
Figure FDA0002974155430000124
Figure FDA0002974155430000125
Q(k-1)＝diag([εβx(k-1) εβy(k-1) εβz(k-1) εωx(k-1) εωy(k-1) εωz(k-1) εvbx(k-1) εvby(k-1) εvbz(k-1) εpn(k-1) εpe(k-1) εpd(k-1)]2)
in the above formula, the first and second carbon atoms are,
Figure FDA0002974155430000126
the components of the angular speed of the machine system relative to the navigation system at the moment k on the x, y and z axes of the machine system; p (k | k-1) is a one-step prediction mean square error from the k-1 moment to the k moment; p (k-1) is the mean square error at the time of k-1; f (k) is a state coefficient matrix at the time k; g (k-1) is a noise coefficient matrix at the k-1 moment; q (k-1) is a noise coefficient matrix at the k-1 moment; epsilonωx(k-1)、εωy(k-1)、εωz(k-1) is the angular velocity noise of x, y and z axes at the time of k-1;
(2) calculate the ith time of kGain K of extended Kalman filter of sub-filterCi：
KCi(k)＝P(k|k-1)HCi(k)T[HCi(k)P(k|k-1)HCi(k)T+RCi(k)]-1
Figure FDA0002974155430000131
Figure FDA0002974155430000132
Figure FDA0002974155430000133
In the above formula, the first and second carbon atoms are,
Figure FDA0002974155430000134
GPS ground direction velocity noise for time k;
Figure FDA0002974155430000135
the noise of the east and north positions of the GPS at the time k;
(3) calculating the state estimation value X of the extended Kalman filter of the ith sub-filter at the moment kCi(k) And mean square error PCi(k)：
XCi(k)＝X(k|k-1)+KCi(k)[YCi(k)-hCi(XCi(k|k-1))]
PCi(k)＝[I-KCi(k)HCi(k)]P(k|k-1)
Figure FDA0002974155430000136
YCi(k) Measure for the ith sub-filter quantity:
Figure FDA0002974155430000141
Figure FDA0002974155430000142
in the above formula, VDG(k)、PNG(k)、PEG(k) The GPS ground speed, the north position and the east position at the moment k; h isbaro(k) The height of the barometer at time k;
(4) computing a global fusion state estimate X for a sub-filter at time kg(k) And mean square error estimate Pg(k)：
Xg(k)＝Pg(k)[PC1(k)-1XC1(k)+PC2(k)-1XC2(k)+PC3(k)-1XC3(k)+PC4(k)-1XC4(k)]-1
Pg(k)＝[PC1(k)-1+PC2(k)-1+PC3(k)-1+PC4(k)-1]-1
X(k)＝Xg(k)
P(k)＝4Pg(k)
In the above formula, X (k), P (k) are the state reset and the mean square error reset of each sub-filter at the time k;
case 2: upon failure of the IMU, the IMU is isolated and the velocity differential equation using the accelerometer in the IMU as input is modified as follows:
Figure FDA0002974155430000143
Figure FDA0002974155430000144
Figure FDA0002974155430000145
a sub-filter using a gyroscope in the IMU as measurement information is cut off and does not participate in fusion filtering; modifying the state estimation correlation matrix based on case 1 according to the sensor isolation condition;
case 3: when the dynamic model fails, the filtering updating process is the same as that in the case 1;
case 4: upon failure of the angular accelerometer, the angular accelerometer is isolated and the differential equation of angular velocity using the angular accelerometer as input is modified as follows:
Figure FDA0002974155430000151
Figure FDA0002974155430000152
Figure FDA0002974155430000153
the measurement update process is the same as case 1;
case 5: when the magnetic sensor fails, the magnetic sensor is isolated, and redundant course information is used as measurement; in the process of updating measurement, Y in the step (3) in the case 1 is usedC2(k)＝ψm(k) Modifying the data output by the redundant course sensor;
case 6: when the GPS fails, the GPS is isolated, and redundant speed position information is used as measurement; in the process of measurement updating, Y in the step (3) in the case 1 is usedC3(k)＝[VNG(k) VEG(k) VDG(k) PNG(k) PEG(k)]TModifying the data output by the redundant speed measurement sensor;
case 7: when the barometer fails, the barometer is isolated, using redundancyAs a measure of the height information; in the process of measurement updating, Y in the step (3) in the case 1 is usedC4(k)＝hbaro(k) Modified to use the data output by the redundant height measurement sensor.
CN201910693594.1A 2019-07-30 2019-07-30 Analytical redundant aircraft fault-tolerant navigation estimation method Active CN110426032B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910693594.1A CN110426032B (en) 2019-07-30 2019-07-30 Analytical redundant aircraft fault-tolerant navigation estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910693594.1A CN110426032B (en) 2019-07-30 2019-07-30 Analytical redundant aircraft fault-tolerant navigation estimation method

Publications (2)

Publication Number Publication Date
CN110426032A CN110426032A (en) 2019-11-08
CN110426032B true CN110426032B (en) 2021-05-28

Family

ID=68413116

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910693594.1A Active CN110426032B (en) 2019-07-30 2019-07-30 Analytical redundant aircraft fault-tolerant navigation estimation method

Country Status (1)

Country Link
CN (1) CN110426032B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112947508B (en) * 2019-12-10 2023-12-05 广州极飞科技股份有限公司 Fault cause determining method and device
CN111024124B (en) * 2019-12-25 2023-11-07 南京航空航天大学 Combined navigation fault diagnosis method for multi-sensor information fusion
CN111880435B (en) * 2020-07-27 2024-03-26 中国工程物理研究院总体工程研究所 Motion perception considered G value compensation control method for continuous load simulator
CN112698663B (en) * 2020-12-04 2023-05-09 一飞(海南)科技有限公司 Cluster performance fault processing method and system, unmanned aerial vehicle, ground station and terminal
CN112629521A (en) * 2020-12-13 2021-04-09 西北工业大学 Modeling method for dual-redundancy combined navigation system of rotor aircraft
CN113821059B (en) * 2021-11-24 2022-02-18 中航金城无人系统有限公司 Multi-rotor unmanned aerial vehicle sensor fault safety flight control system and method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015180171A1 (en) * 2014-05-30 2015-12-03 SZ DJI Technology Co., Ltd. Aircraft attitude control methods
CN107421534A (en) * 2017-04-26 2017-12-01 哈尔滨工程大学 A kind of redundance type SINS multiple faults partition method
CN108981709A (en) * 2018-08-02 2018-12-11 南京航空航天大学 Quadrotor roll angle, the fault-tolerant estimation method of pitch angle based on moment model auxiliary
CN109238307A (en) * 2018-08-30 2019-01-18 衡阳市衡山科学城科技创新研究院有限公司 A kind of flight failure detection method and device based on more used group information auxiliary
CN109612459A (en) * 2018-11-15 2019-04-12 南京航空航天大学 The fault-tolerant air navigation aid of quadrotor inertial sensor based on kinetic model

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9688416B2 (en) * 2014-10-20 2017-06-27 Honeywell International Inc System and method for isolating attitude failures in aircraft
CN108168509B (en) * 2017-12-06 2019-08-13 南京航空航天大学 A kind of quadrotor Error Tolerance estimation method of lift model auxiliary

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015180171A1 (en) * 2014-05-30 2015-12-03 SZ DJI Technology Co., Ltd. Aircraft attitude control methods
CN107421534A (en) * 2017-04-26 2017-12-01 哈尔滨工程大学 A kind of redundance type SINS multiple faults partition method
CN108981709A (en) * 2018-08-02 2018-12-11 南京航空航天大学 Quadrotor roll angle, the fault-tolerant estimation method of pitch angle based on moment model auxiliary
CN109238307A (en) * 2018-08-30 2019-01-18 衡阳市衡山科学城科技创新研究院有限公司 A kind of flight failure detection method and device based on more used group information auxiliary
CN109612459A (en) * 2018-11-15 2019-04-12 南京航空航天大学 The fault-tolerant air navigation aid of quadrotor inertial sensor based on kinetic model

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A fault-tolerant attitude estimation method for quadrotors based on analytical redundancy;Shichao Liu 等;《Aerospace Science and Technology》;20190712;第93卷;第1-10页 *
Design and Analysis of a Fault-Tolerant Coplanar Gyro-Free Inertial Measurement Unit;Tsung-Lin Chen;《Journal of Microelectromechanical Systems 》;20080207;第17卷(第01期);第201-212页 *
非线性系统闭式被控对象故障诊断方法研究;黄俊杰;《中国博士学位论文全文数据库 工程科技Ⅱ辑》;20160415;第C031-4页 *

Also Published As

Publication number Publication date
CN110426032A (en) 2019-11-08

Similar Documents

Publication Publication Date Title
CN110426032B (en) Analytical redundant aircraft fault-tolerant navigation estimation method
CN110207697B (en) Inertial navigation resolving method based on angular accelerometer/gyroscope/accelerometer
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
US6498996B1 (en) Vibration compensation for sensors
CN108981709B (en) Four-rotor-wing roll angle and pitch angle fault-tolerant estimation method based on moment model assistance
CN108981708B (en) Fault-tolerant integrated navigation method for four-rotor torque model/course gyroscope/magnetic sensor
RU2236697C2 (en) Reserve heading and spatial attitude indication system
EP3392613A2 (en) Air data attitude reference system
Youn et al. Combined quaternion-based error state Kalman filtering and smooth variable structure filtering for robust attitude estimation
JPH06102053A (en) Trouble-permission inertial navigation system
CN112945225A (en) Attitude calculation system and method based on extended Kalman filtering
CN109612459B (en) Four-rotor aircraft inertial sensor fault-tolerant navigation method based on dynamic model
CN108592911B (en) Four-rotor aircraft dynamic model/airborne sensor combined navigation method
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN111121766A (en) Astronomical and inertial integrated navigation method based on starlight vector
Liu et al. A fault-tolerant attitude estimation method for quadrotors based on analytical redundancy
CN110567457A (en) Inertial navigation self-detection system based on redundancy
CN108759814B (en) Method for estimating transverse rolling axis angular velocity and pitching axis angular velocity of four-rotor aircraft
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN108693372B (en) Course axis angular velocity estimation method of four-rotor aircraft
Kopecki et al. Algorithms of measurement system for a micro UAV
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN110567456B (en) BDS/INS combined train positioning method based on robust Kalman filtering
Dolega et al. Possibilities of using software redundancy in low cost aeronautical control systems
CN113932803B (en) Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft

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