CN110849360B - Distributed relative navigation method for multi-machine collaborative formation flight - Google Patents
Distributed relative navigation method for multi-machine collaborative formation flight Download PDFInfo
- Publication number
- CN110849360B CN110849360B CN201911166367.XA CN201911166367A CN110849360B CN 110849360 B CN110849360 B CN 110849360B CN 201911166367 A CN201911166367 A CN 201911166367A CN 110849360 B CN110849360 B CN 110849360B
- Authority
- CN
- China
- Prior art keywords
- navigation system
- gps
- data
- plane
- long
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 33
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 238000012937 correction Methods 0.000 claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 17
- 238000005259 measurement Methods 0.000 claims description 48
- 239000011159 matrix material Substances 0.000 claims description 38
- 230000007704 transition Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 230000010006 flight Effects 0.000 claims 3
- 238000004364 calculation method Methods 0.000 abstract description 14
- 238000012423 maintenance Methods 0.000 abstract description 4
- 230000002776 aggregation Effects 0.000 abstract description 2
- 238000004220 aggregation Methods 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 8
- 230000005284 excitation Effects 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention belongs to the technical field of multi-aircraft cooperative formation flight, and provides a distributed relative navigation method for multi-aircraft cooperative formation flight, wherein a long-range MEMS-SINS navigation system and a GPS navigation system respectively output long-range INS data and GPS data, and the long-range INS data and the GPS data are subjected to Kalman filtering processing to obtain long-range INS output data corrected by a GPS and fed back to a stand-alone aircraft through a data link; the system comprises a micro-electromechanical system (MEMS) -Strapdown Inertial Navigation System (SINS) and a Global Positioning System (GPS) of a wing plane, wherein the MEMS-SINS navigation system and the GPS navigation system respectively output INS data and GPS data of the wing plane, and perform Kalman filtering processing on the INS data and the GPS data of the wing plane to obtain INS output data of the wing plane after GPS correction; and (3) performing relative navigation calculation by using INS output data corrected by the GPS of the long plane and the bureau plane to obtain the relative distance and the relative position of the long plane and the bureau plane. The obtained relative distance and relative position information can be provided for a multi-machine collaborative formation flight system in real time, and more accurate data sources are provided for achieving functions of formation aggregation, formation maintenance, reconstruction and the like.
Description
Technical Field
The invention belongs to the technical field of multi-aircraft cooperative formation flight, and particularly relates to a distributed relative navigation method for multi-aircraft cooperative formation flight.
Technical Field
The multiple aircrafts perform certain formation arrangement and task allocation for adapting to task requirements, including track planning, formation design, relative navigation, formation control and formation reconstruction aspects. In the coordinated formation flight of the fleet, accurate estimation of the relative positions of the members and the relative and absolute positions of the members in real time on line is a prerequisite for formation control and formation maintenance, so that the navigation system is widely focused as an important way for acquiring the relative and absolute information of the members in the formation flight.
The GPS is a satellite navigation positioning system developed by the national defense department for sea, land and air, has the advantages of global property, all weather, three-dimensional positioning and the like, but has poor reliability in a dynamic environment, is easily blocked by terrain to be positioned and interrupted, has low data acquisition frequency and is a non-autonomous system. SINS (strapdown inertial navigation system) is a comparatively common inertial navigation method at present, which utilizes inertial elements to measure the acceleration of a moving carrier, calculates to obtain navigation parameters, is completely autonomous, is not influenced by the interference of external environment, has no signal loss and is multifunctional, the navigation precision mainly depends on a gyroscope and an accelerometer, but the traditional inertial device has the characteristics of larger general volume, higher cost, and MEMS inertial sensor has the characteristics of small volume, low cost and light weight, but the system precision is lower, and if the system error is very large, the system can not work independently.
Modern navigation technology has been developed into a combined navigation system composed of multiple sensors, and the combined navigation system overcomes the defects of uncertainty and unreliability of a single sensor by combining multiple sensors and applying a data fusion algorithm.
Disclosure of Invention
The invention adopts a distributed filtering structure, a long machine and a assistant machine, adopts a GPS/INS integrated navigation system, obtains INS output data after GPS correction by carrying out Kalman filtering processing on INS data and GPS data of the long machine and the assistant machine, then carries out calculation of relative distance and relative position of the long machine and the assistant machine, and improves accuracy of the respective output data.
The principle of the invention is as follows: carrying out Kalman filtering processing on INS data and GPS data of the long machine to obtain INS output data of the long machine after GPS correction, and transmitting the INS output of the long machine after correction to a wing machine through a data chain; similarly, carrying out Kalman filtering processing on the INS data and the GPS data of the plane to obtain INS output data of the plane after GPS correction; and calculating the relative distance and the relative position of the long machine and the wing machine by utilizing the INS output data corrected by the GPS and the INS output data corrected by the GPS. The obtained relative distance and relative position information can be provided for a multi-machine collaborative formation flight system in real time, and more accurate data sources are provided for achieving functions of formation aggregation, formation maintenance, reconstruction and the like.
The technical scheme of the invention is a distributed relative navigation method for the cooperative formation flight of a plurality of aircrafts, wherein the aircrafts comprise at least one long aircraft and at least one assistant aircraft, and all adopt an integrated navigation system for navigation; the distributed relative navigation system comprises an MEMS-SINS navigation system and a GPS navigation system;
the MEMS-SINS navigation system and the GPS navigation system of the long machine respectively output INS data and GPS data of the long machine, carry out Kalman filtering processing on the INS data and the GPS data of the long machine to obtain INS output data of the long machine after GPS correction, and feed back the INS output data to the auxiliary machine through a data link;
the MEMS-SINS navigation system and the GPS navigation system of the plane respectively output INS data and GPS data of the plane, carrying out Kalman filtering processing on the INS data and the GPS data of the bureau to obtain INS output data of the bureau after GPS correction;
performing relative navigation calculation by using the INS output data of the long machine corrected by the GPS and the INS output data of the bureau machine corrected by the GPS to obtain the relative distance and the relative position of the long machine and the bureau machine;
and providing the relative distance and the relative position of the long plane and the plane to the multi-plane cooperative formation flight system in real time.
Further, the INS data and the GPS data of the long machine and the bureau machine are processed by Kalman filtering, the processing steps comprise,
establishing respective error models of an MEMS-SINS navigation system and a GPS navigation system in the combined navigation system, selecting respective state quantity of the MEMS-SINS navigation system and the GPS navigation system according to the error models, and calculating to obtain a state equation of the combined navigation system of each of the long-distance computer and the auxiliary computer according to the state quantity matrix instead of determining the state quantity matrix of the combined navigation system;
according to INS data and GPS data of the combined navigation systems of the long plane and the bureau plane, acquiring measurement matrixes of the combined navigation systems of the long plane and the bureau plane respectively, and according to the measurement matrixes, calculating to acquire measurement equations of the combined navigation systems of the long plane and the bureau plane respectively;
discretizing a state equation and a measurement equation of each integrated navigation system of the long machine and the bureau, obtaining a discretization equation, estimating various error states of the integrated navigation system by using a Kalman filtering algorithm by using discrete time, and correcting INS data by using an estimated value of the error states to obtain corrected INS output data of each of the long machine and the bureau.
Further, the state quantity is selected at least including one or more of a mathematical platform misalignment angle, a carrier east, north and sky speed error, a carrier latitude error, a longitude error, an altitude error, a gyroscope constant drift error, a gyroscope related error and an accelerometer system error.
Further, the state equation is
Wherein X (t) represents a navigation system state vector at the moment of t of the inertial navigation system; w (t) represents the noise vector at time t; f (t) represents a state transition matrix at the moment t; g (t) represents a system noise transfer matrix at the moment t;representing the state derivative vector at time t.
Further, the INS data of the long machine comprise the speed and the position of the long machine output by the MEMS-SINS navigation system; the GPS data of the long machine comprises the speed and the position of the long machine output by a GPS navigation system;
respectively making differences between the speed and the position of the long machine output by the MEMS-SINS navigation system of the long machine and the speed and the position of the long machine output by the GPS navigation system of the long machine correspondingly to obtain a measurement matrix of the combined navigation system of the long machine;
the INS data of the plane comprises the speed and the position of the plane output by the MEMS-SINS navigation system; the GPS data of the plane comprises the speed and the position of the plane output by the GPS navigation system;
and respectively making difference between the speed and the position of the assistant machine output by the MEMS-SINS navigation system of the assistant machine and the speed and the position of the assistant machine output by the GPS navigation system of the assistant machine correspondingly to obtain a measurement matrix of the integrated navigation system of the assistant machine.
Further, the measurement equations of the plane and the plane are that,
Z(t)＝H(t)·X(t)+V(t)
wherein H (t) represents a measurement matrix of the integrated navigation system at the moment t; x (t) represents a state quantity at time t; v (t) represents the measured noise vector at time t; z (t) represents the measurement vector at time t.
Further, discretizing the state equation and the measurement equation of the combined navigation system of the plane and the plane, wherein the discretization equation is that,
wherein X is k An n-dimensional state vector representing the combined navigation system at the k moment; z is Z k Representing an m-dimensional metrology vector at time k; phi k,k-1 A system state transition matrix from k-1 time to k time; h k A measurement matrix representing the moment k; w (W) k-1 A system noise vector denoted as time k-1; Γ -shaped structure k-1 A system noise matrix which represents the degree to which each system noise from k-1 time to k time affects each state at k time; v (V) k The noise vector is measured for the m dimension at time k.
Further, the Kalman filtering algorithm processing step further comprises state one-step prediction, state estimation, filtering gain, one-step prediction mean square error and estimated mean square error.
The invention has the technical effects that:
the invention combines the GPS and the MEMS-SINS, can improve the navigation precision of the system and enhance the anti-interference capability of the distributed relative navigation system;
compared with the condition of only INS navigation, the INS/GPS integrated navigation has good stability, can reduce position and speed errors, and has higher navigation precision;
by introducing the data chain, the relative navigation between the long machine and the wing machine is realized.
Drawings
FIG. 1 is a schematic diagram of an INS/GPS integrated navigation system for a long plane and a assistant plane according to the present embodiment;
FIG. 2 is a schematic diagram of a distributed relative navigation system according to the present embodiment;
fig. 3 shows two calculation loops of the kalman filter of the present embodiment.
Detailed Description
In this embodiment, a distributed relative navigation system for a coordinated formation flight of a plurality of aircraft is provided, where the plurality of aircraft includes at least one long aircraft and at least one plane, and all adopt an integrated navigation system for navigation.
FIG. 2 is a combined INS/GPS navigation system for a plane and a plane of the present embodiment; as shown in fig. 1, the integrated navigation system of the present embodiment includes a MEMS-SINS navigation system, a GPS navigation system, a kalman filter, and a navigation computer.
The MEMS-SINS navigation system uses MEMS sensors, and initial reference position and speed information of the MEMS-SINS navigation system are obtained through initial alignment of the MEMS sensors. In the system starting stage, initial alignment of attitude angles is carried out through a magnetic compass, gyro data is recursively calculated through a quaternion method to obtain real-time attitude angles of the system, and acceleration information is subjected to double integration on time to obtain absolute position and absolute speed information of a carrier. And the MEMS-SINS navigation system performs inertial navigation calculation on the data information acquired by the MEMS sensor to obtain INS data output by the MEMS-SINS navigation system. The INS data includes the speed and position of the carrier (the chang or the co-plane) output by the MEMS-SINS navigation system.
The GPS navigation system adopts a GPS receiver, and then GPS signal extraction is carried out to obtain GPS data output by the GPS navigation system. The GPS data includes the carrier (bench or bureau) speed and position output by the GPS navigation system.
Then, the navigation computer is used for calculating INS data output by the MEMS-SINS navigation system of the carrier (the long machine or the wing machine) and GPS data output by the GPS navigation system, and a Kalman filter is used for carrying out Kalman filtering processing to obtain INS output data of the carrier (the long machine or the wing machine) after GPS correction. And (3) performing difference making on the INS output data of the long machine after the GPS correction and the INS output data of the bureau machine after the GPS correction to obtain the relative distance and the relative position of the long machine and the bureau machine. The relative distance and the relative position of the long plane and the plane are provided for the multi-plane collaborative formation flying system in real time, and more accurate data sources are provided for realizing functions of formation set, formation maintenance, reconstruction and the like.
Fig. 2 is a schematic diagram of a distributed relative navigation system according to the present embodiment. As shown in fig. 2, after the INS output data of the long machine after the GPS correction is obtained, the data is fed back to the wing machine through a data link. And performing relative navigation calculation by using the INS output data of the long machine after the GPS correction and the INS output data of the bureau machine after the GPS correction to obtain the relative distance and the relative position of the long machine and the bureau machine.
Further, in this embodiment, the navigation computer is used to calculate and process INS data output by the MEMS-SINS navigation system of the carrier (the long machine or the auxiliary machine) and GPS data output by the GPS navigation system, and the specific processing steps mainly include the following:
step 1: acquiring state equations of MEMS-SINS navigation system and GPS navigation system
(1) Establishing respective error models of MEMS-SINS navigation system and GPS navigation system
The integrated navigation system adopts a common reference system as a north-finger position, and the sensor error model uses a northeast geographic coordinate system. The sensor error model of the system can be deduced by the strapdown inertial navigation algorithm principle.
The coordinate system is defined as follows: i is an inertial coordinate system; n is a navigation coordinate system, and a northeast geographic coordinate system is adopted in the text; b is the inertial navigation carrier coordinate system.
In the northeast geographic coordinate system, when the fly height h and the earth are considered to be spheroids, there are
Wherein:
E. n, U each represents east, north, and day;
v E 、v N 、v U is a component of the flight speed;
φ E 、φ N 、φ U three-axis components of the error angle of the mathematical platform;
ε E 、ε N 、ε U a triaxial component of gyro drift;
radius R of earth e ＝6378245m；
Radius of meridian R M ＝R e (1-2f+3f sin 2 L)；
Radius R of mortise unitary circle N ＝R e (1+f sin 2 L)；
Earth flatness f=1/298.257;
angular velocity of earth rotation omega ie ＝7.292×10 -5 rad/s；
Delta represents an error symbol; l represents latitude.
In the northeast geographic coordinate system, when the fly height h and the earth are considered to be ellipsoids of revolution:
in the method, in the process of the invention,is the triaxial component of the accelerometer error, f E 、f U 、f N Is the triaxial component of the specific force. When the height channel is not considered, then v U ,δv U Zero.
In the northeast geographic coordinate system, when the fly height h and the earth are considered to be a ellipsoid of revolution, the position error of the inertial navigation output is expressed as
Inertial instrument errors include installation errors, scale factor errors, and random errors. For simplicity, only random errors are considered.
The gyro drift in the formula (1) is gyro drift along the geographic coordinate system of east, north and sky. In the strapdown inertial navigation system, the gyro drift in the formula (1) is required to be equivalent gyro drift converted from a body system to a geographic system.
Taking the drift of the gyroscope as
ε＝ε b +ε r +w g (4)
Wherein ε b Is a random constant; epsilon r Is a first order markov process; w (w) g Is gyroscope white noise.
Assuming that the three axial gyro drift error models are the same, the three axial gyro drift error models are all
Wherein T is r Is the correlation time; w (w) r Gyro markov noise.
Consider a first order Markov process and assume that the error models of the three axial accelerometers are the same, all
Wherein T is a Is a first order Markov correlation time;is accelerometer error; />Is the accelerometer error derivative; w (w) a Acceleration Ji Maer koff noise.
(2) The state quantity of the integrated navigation system is selected and calculated to obtain a state equation
And (3) selecting state quantity according to each error model of the inertial navigation system determined in the step (1), and determining a state quantity matrix.
In summary, the state quantity of the inertial navigation state model of the carrier is selected as
Wherein each physical quantity is defined as a misalignment angle of a mathematical platform, a speed error of a carrier in the east direction, the north direction and the sky direction, a latitude error, a longitude error, an altitude error, a constant drift error of a gyroscope, a related error of the gyroscope and a systematic error of an accelerometer; the gyroscope constant drift error, gyroscope related error and accelerometer system error are respectively indicated by x, y and z subscripts of the gyroscope constant drift error, the gyroscope related error and the accelerometer system error. In this embodiment, the components in the navigation coordinate system, that is, the northeast geographic coordinate system (that is, the northeast, north, and heaven directions previously described) are used.
The system noise is expressed as
W＝[w gx w gy w gz w rx w ry w rz w ax w ay w az ] T
Wherein each physical quantity means: gyro white noise, gyro markov noise, acceleration Ji Maer koff noise.
Obtaining a state equation of the inertial navigation system under a navigation coordinate system:
wherein X (t) represents a navigation system state vector of an inertial navigation system (MEMS-SINS navigation system) at the moment t; w (t) represents a system noise vector at the time t; f (t) represents a system state transition matrix at the moment t; g (t) represents a system noise transfer matrix at the moment t;representing the state derivative vector at time t.
The system noise transfer matrix is G (t), wherein G and G (t) both represent the noise transfer matrix, and the expression of G is
Wherein, the gesture transfer matrixObtained from attitude quaternions or attitude angles in the track signal, i.e.
A system state transition matrix F (t), wherein F and F (t) both represent inertial navigation system state transition matrices, and the expression of F is:
wherein F is S 、F M 、F N Are all 9x9 square arraysStep 2: acquiring measurement equations of MEMS-SINS navigation system and GPS navigation system
According to the difference between the speed and the position output by the MEMS-SINS navigation system of the carrier and the speed and the position information output by the GPS navigation system of the carrier, a measurement matrix of the combined navigation system of the carrier is obtained, and a measurement equation of the combined navigation system of the carrier is calculated and obtained, which is expressed as
Wherein L, lambda, h are the positions and speeds along the northeast three directions respectively; the superscript INS and GPS denote different navigation systems.
The measurement equation of the integrated navigation system is as follows
Z(t)＝H(t)·X(t)+V(t) (10)
Wherein H (t) represents a measurement matrix under the moment t of the continuous system; x (t) represents a state quantity at the time t of the continuous system; v (t) represents a measured noise vector at the moment of t of the continuous system; z (t) represents the measurement vector at time t of the continuous system.
H is a measurement matrix, and H (1, 7) =1, H (2, 8) =1, H (3, 9) =1, and the remaining elements in the H matrix are all zero.
Step 3: kalman filtering algorithm processing for integrated navigation system
Discretizing a state equation and a measurement equation of each combined navigation system of the long machine and the bureau, obtaining a discretization equation, estimating various error states of the combined navigation system by using a discrete time Kalman filtering algorithm, and correcting INS data by using an estimated value of the error states to obtain corrected INS output data of each long machine and each bureau. The embodiment specifically includes the following:
after the state equation and the measurement equation of the INS/GPS integrated navigation system in the continuous time domain are obtained, discretization processing is needed, so that a discrete time Kalman filtering algorithm is applied to estimate the state information.
Discretizing the equation of state (7) and the measurement equation (10) to obtain
Wherein X is k An n-dimensional state vector representing the system at time k is also the desired estimated vector; z is Z k Representing an m-dimensional metrology vector at time k; phi k,k-1 A system state transition matrix from k-1 time to k time; h k A measurement matrix representing the moment k; w (W) k-1 Representing a pseudo-k-1 momentIs a system noise vector of (1); Γ -shaped structure k-1 A system noise matrix which represents the degree to which each system noise from k-1 time to k time affects each state at k time; v (V) k The noise vector is measured for the m dimension at time k. And is also provided with
In the formula, T is an iteration period, and n is a finite term in actual calculation.
The system noise and the measurement noise in the state equation and the measurement equation have the following properties:
E[W k ]＝0，
E[V k ]＝0，
Cov[W k ,V j ]＝E[W k V j T ]＝0
in which Q k For the system noise sequence W k Is a variance matrix of (a); r is R k To measure the noise sequence V k Is a variance matrix of (a).
Discrete quantity Q k 、R k The relationship between the continuous quantities Q (t) and R (t) can be approximately expressed as
Kalman filters are commonly used in combination and navigation systems. The main method for applying the Kalman filtering technology in the integrated navigation system is as follows: based on some measured output quantity of the navigation system, kalman filtering is utilized to estimate various error states of the system, and the estimated value of the error states is utilized to correct the system, so as to achieve the aim of system combination. And correcting the INS data by using the estimated value of the error state to obtain corrected INS output data of each of the long machine and the auxiliary machine.
The filtering is to extract a desired signal from a plurality of signals mixed together, and the kalman filtering estimates the desired signal from a quantity measurement related to the extracted signal by an algorithm. Where the estimated signal is a random response caused by white noise excitation, the transfer structure (system equation) between the excitation source and the response is known, as is the functional relationship (measurement equation) between the quantity measurement and the estimated quantity. The following information is used in the estimation process: system equations, measurement equations, statistical properties of white noise excitation, statistical properties of measurement errors. Since the information used is a quantity in the time domain, the kalman filter is designed in the time domain and is suitable for multidimensional situations.
The kalman filter has the following characteristics:
(1) The object of the Kalman filtering process is a random signal;
(2) The processed signal has no useful and interference division, and the purpose of filtering is to estimate all the processed signals;
(3) White noise excitation and measurement noise of the system are not opposite directions needing to be filtered, and the statistical characteristics of the white noise excitation and measurement noise are information needed to be utilized in the estimation process.
Kalman filtering is essentially a set of recursive algorithms implemented by a digital computer, each recursive period comprising two processes, time update and metrology update, for an estimated quantity. The time update is determined by the measurement update result of the last step and the prior information when the Kalman filter is designed, and the measurement update is determined according to the measurement value obtained in real time on the basis of the time update. The quantity measurement can thus be seen as an input to the Kalman filter and the estimate as an output, the input and output being linked by a time update and a measurement update algorithm.
Assume an initial state X of the system 0 Is also a normal random vector, the mean and covariance of which are respectively
The basic steps of the discrete kalman filter algorithm are as follows:
state one-step prediction
State estimation
Filtering gain
K k ＝P k ·H k T ·R k -1 (17)
One-step prediction of mean square error
P k|k-1 ＝Φ k,k-1 ·P k-1 ·Φ k,k-1 T +Γ k-1 ·Q k ·Γ k-1 T (18)
Estimating mean square error
P k ＝(I-K k ·H k )P k|k-1 (19)
As long as the initial value of the filtering is givenAnd P 0 Based on measurements Z at time k k The state estimate +.k for time k can be calculated recursively>
The calculation of the discrete kalman filter algorithm can be represented by fig. 3.
The kalman filter has two computational loops, as evident from fig. 3: gain calculation loop and filtering calculation loop. Wherein the gain calculation loop is a stand-alone calculation loop and the filter calculation loop is dependent on the gain calculation loop.
Claims (7)
1. The distributed relative navigation method for the cooperative formation flight of a plurality of airplanes comprises at least one long airplane and at least one assistant airplane, and all the airplanes adopt an integrated navigation system for navigation; it is characterized in that the method comprises the steps of,
the integrated navigation system comprises an MEMS-SINS navigation system and a GPS navigation system;
the MEMS-SINS navigation system and the GPS navigation system of the long machine respectively output INS data and GPS data of the long machine, carry out Kalman filtering processing on the INS data and the GPS data of the long machine to obtain INS output data of the long machine after GPS correction, and feed back the INS output data to the auxiliary machine through a data link;
the MEMS-SINS navigation system and the GPS navigation system of the plane respectively output INS data and GPS data of the plane, carrying out Kalman filtering processing on the INS data and the GPS data of the bureau to obtain INS output data of the bureau after GPS correction;
the INS output data of the long machine after GPS correction and the INS output data of the machine after GPS correction are subjected to difference making to obtain the relative distance and the relative position of the long machine and the bureau;
providing the relative distance and the relative position of the long plane and the bureau plane to a multi-plane collaborative formation flight system in real time;
the method comprises the following steps of:
establishing respective error models of an MEMS-SINS navigation system and a GPS navigation system in the integrated navigation system, selecting state quantity according to the error models, determining a state quantity matrix of the integrated navigation system, and calculating to obtain a state equation of the integrated navigation system of each of the long plane and the auxiliary plane according to the state quantity matrix;
according to INS data and GPS data of the combined navigation systems of the long plane and the bureau plane, obtaining a measurement matrix of each combined navigation system of the long plane and the bureau plane, and according to the measurement matrix, calculating to obtain a measurement equation of each combined navigation system of the long plane and the bureau plane;
discretizing a state equation and a measurement equation of each combined navigation system of the long machine and the bureau, obtaining a discretization equation, estimating various error states of the combined navigation system by using a discrete time Kalman filtering algorithm, and correcting INS data by using an estimated value of the error states to obtain corrected INS output data of each long machine and each bureau.
2. The method of distributed relative navigation for coordinated formation of flights by multiple aircraft according to claim 1,
the selected state quantity at least comprises one or more of a mathematical platform misalignment angle, a carrier east, north and sky speed error, a carrier latitude error, a carrier longitude error, a carrier altitude error, a gyroscope constant drift error, a gyroscope related error and an accelerometer system error.
3. The method for distributed relative navigation for coordinated formation of flights to multiple aircraft according to claim 2, wherein the equation of state is
Wherein X (t) represents a navigation system state vector at the moment of t of the inertial navigation system; w (t) represents a noise vector at the time t; f (t) represents a state transition matrix at the moment t; g (t) represents a system noise transfer matrix at the moment t;representing the state derivative vector at time t.
4. The method of distributed relative navigation for coordinated formation of flights by multiple aircraft according to claim 1,
the INS data of the long machine comprise the speed and the position of the long machine output by the MEMS-SINS navigation system; the GPS data of the long machine comprises the speed and the position of the long machine output by a GPS navigation system;
respectively making differences between the speed and the position of the long machine output by the MEMS-SINS navigation system of the long machine and the speed and the position of the long machine output by the GPS navigation system of the long machine correspondingly to obtain a measurement matrix of the combined navigation system of the long machine;
the INS data of the plane comprises the speed and the position of the plane output by the MEMS-SINS navigation system; the GPS data of the plane comprises the speed and the position of the plane output by the GPS navigation system;
and respectively differencing the speed and the position of the wing plane output by the MEMS-SINS navigation system of the wing plane and the speed and the position of the wing plane output by the GPS navigation system of the wing plane correspondingly to obtain a measurement matrix of the integrated navigation system of the wing plane.
5. The method for distributed relative navigation for coordinated formation of flight by multiple aircraft according to claim 4, wherein the measurement equation expression of long aircraft and wing aircraft is as follows,
Z(t)＝H(t)·X(t)+V(t)
wherein H (t) represents a measurement matrix of the integrated navigation system at the moment t; x (t) represents a state quantity at time t; v (t) represents the measured noise vector at time t; z (t) represents the measurement vector at time t.
6. The distributed relative navigation method for the coordinated formation flying of a plurality of airplanes according to claim 1, wherein discretization processing is carried out on a state equation and a measurement equation of a combined navigation system of a long airplane and a wing airplane respectively, and the equation expression after the discretization processing is that,
wherein X is k An n-dimensional state vector representing the combined navigation system at the k moment; z is Z k Representing an m-dimensional metrology vector at time k; phi k,k-1 A system state transition matrix from k-1 time to k time; h k A measurement matrix representing the moment k; w (W) k-1 A system noise vector denoted as time k-1; Γ -shaped structure k-1 A system noise matrix which represents the degree to which each system noise from k-1 time to k time affects each state at k time; v (V) k The noise vector is measured for the m dimension at time k.
7. The method of claim 1, wherein the kalman filtering algorithm comprises a state one-step prediction, a state estimation, a filter gain, a one-step prediction mean square error, and an estimated mean square error.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911166367.XA CN110849360B (en) | 2019-11-25 | 2019-11-25 | Distributed relative navigation method for multi-machine collaborative formation flight |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911166367.XA CN110849360B (en) | 2019-11-25 | 2019-11-25 | Distributed relative navigation method for multi-machine collaborative formation flight |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110849360A CN110849360A (en) | 2020-02-28 |
| CN110849360B true CN110849360B (en) | 2023-08-01 |
Family
ID=69604328
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911166367.XA Active CN110849360B (en) | 2019-11-25 | 2019-11-25 | Distributed relative navigation method for multi-machine collaborative formation flight |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110849360B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111708377B (en) * | 2020-06-21 | 2022-10-25 | 西北工业大学 | Flight control method based on inertial navigation/flight control system information fusion |
| CN112783203B (en) * | 2020-12-28 | 2023-03-21 | 西北工业大学 | Multi-sensor-based control system and method for unmanned aerial vehicle formation maintenance |
| CN113885577B (en) * | 2021-10-29 | 2023-11-28 | 西北工业大学 | Anti-collision control method, system and device for multi-aircraft dense formation of aircraft |
| CN115597608B (en) * | 2022-12-14 | 2023-03-10 | 湖南警察学院 | Multi-unmanned aerial vehicle relative positioning method and device, computer equipment and medium |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109813311A (en) * | 2019-03-18 | 2019-05-28 | 南京航空航天大学 | A kind of unmanned plane formation collaborative navigation method |
| CN110262553A (en) * | 2019-06-27 | 2019-09-20 | 西北工业大学 | Fixed-wing UAV Formation Flight apparatus and method based on location information |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8600660B2 (en) * | 2006-09-29 | 2013-12-03 | Honeywell International Inc. | Multipath modeling for deep integration |
-
2019
- 2019-11-25 CN CN201911166367.XA patent/CN110849360B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109813311A (en) * | 2019-03-18 | 2019-05-28 | 南京航空航天大学 | A kind of unmanned plane formation collaborative navigation method |
| CN110262553A (en) * | 2019-06-27 | 2019-09-20 | 西北工业大学 | Fixed-wing UAV Formation Flight apparatus and method based on location information |
Non-Patent Citations (1)
| Title |
|---|
| 袁杰波 ; 杨峰 ; 张共愿 ; 梁彦 ; .无人机编队飞行导航方法及其仿真研究.计算机仿真.2011,(11),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110849360A (en) | 2020-02-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN108226980B (en) | Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit | |
| CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN110095800B (en) | Multi-source fusion self-adaptive fault-tolerant federal filtering integrated navigation method | |
| CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
| CN105737828B (en) | A kind of Combinated navigation method of the joint entropy Extended Kalman filter based on strong tracking | |
| CN106990426B (en) | Navigation method and navigation device | |
| CN102538792B (en) | Filtering method for position attitude system | |
| Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
| Mulder et al. | Non-linear aircraft flight path reconstruction review and new advances | |
| CN103697889B (en) | A kind of unmanned plane independent navigation and localization method based on multi-model Distributed filtering | |
| CN104698486B (en) | A kind of distribution POS data processing computer system real-time navigation methods | |
| CN113340298B (en) | Inertial navigation and dual-antenna GNSS external parameter calibration method | |
| CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
| CN109708663B (en) | Star sensor online calibration method based on aerospace plane SINS assistance | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN113252038A (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
| Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
| CN112525188B (en) | Combined navigation method based on federal filtering | |
| RU2654965C1 (en) | Integrated strap-down astro-inertial navigation system | |
| CN111982126A (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
| Al-Jlailaty et al. | Efficient attitude estimators: A tutorial and survey |
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 |