CN110849360A - Distributed relative navigation method for multi-machine cooperative formation flight - Google Patents
Distributed relative navigation method for multi-machine cooperative formation flight Download PDFInfo
- Publication number
- CN110849360A CN110849360A CN201911166367.XA CN201911166367A CN110849360A CN 110849360 A CN110849360 A CN 110849360A CN 201911166367 A CN201911166367 A CN 201911166367A CN 110849360 A CN110849360 A CN 110849360A
- Authority
- CN
- China
- Prior art keywords
- navigation system
- data
- gps
- plane
- ins
- 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.)
- Granted
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 34
- 238000000034 method Methods 0.000 title claims abstract description 23
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 238000012545 processing Methods 0.000 claims abstract description 14
- 238000012937 correction Methods 0.000 claims abstract description 8
- 238000005259 measurement Methods 0.000 claims description 49
- 239000011159 matrix material Substances 0.000 claims description 41
- 230000008569 process Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 6
- 230000010006 flight Effects 0.000 claims 2
- 238000002360 preparation method Methods 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 14
- 238000012423 maintenance Methods 0.000 abstract description 4
- 230000002776 aggregation Effects 0.000 abstract description 3
- 238000004220 aggregation Methods 0.000 abstract description 3
- 230000005284 excitation Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000009897 systematic effect Effects 0.000 description 2
- 239000007983 Tris buffer Substances 0.000 description 1
- 150000001875 compounds Chemical class 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
- 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
- 239000000243 solution Substances 0.000 description 1
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
- 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)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention belongs to the technical field of multi-aircraft collaborative formation flying, and provides a distributed relative navigation method for the collaborative formation flying of a plurality of airplanes, wherein an MEMS-SINS navigation system and a GPS navigation system of a long plane respectively output INS data and GPS data of the long plane, the INS data and the GPS data of the long plane are subjected to Kalman filtering processing to obtain INS output data of the long plane after GPS correction, and the INS output data is fed back to an assistant plane through a data chain; an MEMS-SINS navigation system and a GPS navigation system of a wing plane respectively output INS data and GPS data of the wing plane, and the INS data and the GPS data of the wing plane are subjected to Kalman filtering treatment to obtain INS output data of the wing plane after GPS correction; and (4) carrying out relative navigation calculation by utilizing INS output data of the fans and the wing machines after GPS correction to obtain the relative distance and the relative position of the fans and the wing machines. The obtained relative distance and relative position information can be provided for a multi-machine cooperative formation flying system in real time, and more accurate data sources are provided for 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 carry out certain formation arrangement and task allocation for adapting to task requirements, and the formation arrangement and the task allocation comprise the aspects of flight path planning, formation design, relative navigation, formation control and formation reconstruction. In the flight of the fleet cooperative formation, accurate estimation of the relative position among 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 concerned as an important way for acquiring the relative and absolute information of the members in the flight of the formation.
The GPS is a satellite navigation positioning system developed by the U.S. department of defense for sea, land and air, has the advantages of globality, all-weather, three-dimensional positioning and the like, but has poor reliability in a dynamic environment, is easy to be blocked by a ground object to interrupt positioning, has low data acquisition frequency and is a non-autonomous system. The SINS (strapdown inertial navigation system) is a common inertial navigation method at present, it uses inertial element to measure the acceleration of moving carrier, and calculates the navigation parameter, it is completely autonomous, and is not affected by the interference of external environment, and has no signal loss, and it is multifunctional, its navigation accuracy mainly depends on gyroscope and accelerometer, but the traditional inertial device is generally large in volume and high in cost, and the MEMS inertial sensor has the characteristics of small volume, low cost and light weight, but its system accuracy is low, if it is directly used, the system error is large, so it can not work alone.
Modern navigation technology has been developed into an integrated navigation system composed of multiple types of sensors, and the integrated navigation system overcomes the defects of uncertainty and unreliability of a single sensor by the cooperation of multiple types of sensors and the application of a data fusion algorithm.
Disclosure of Invention
The invention adopts a distributed filter structure, both the fans and the wings adopt a GPS/INS combined navigation system, and obtains INS output data respectively corrected by the GPS by carrying out Kalman filtering treatment on the INS data and the GPS data of the fans and the wings, and then calculates the relative distance and the relative position of the fans and the wings, thereby improving the accuracy of the respective output data.
The principle of the invention is as follows: performing Kalman filtering on INS data and GPS data of the lead aircraft to obtain INS output data of the lead aircraft corrected by the GPS, and transmitting the INS output of the lead aircraft corrected to a wing aircraft through a data link; similarly, the INS data and the GPS data of the wing plane are subjected to Kalman filtering processing to obtain INS output data of the wing plane corrected by the GPS; the relative distance and the relative position of the long plane and the wing plane are calculated by utilizing the INS output data of the long plane corrected by the GPS and the INS output data of the wing plane corrected by the GPS. The obtained relative distance and relative position information can be provided for a multi-machine cooperative formation flying system in real time, and more accurate data sources are provided for functions of formation aggregation, formation maintenance, reconstruction and the like.
The technical scheme of the invention is a distributed relative navigation method facing a plurality of airplanes to collaboratively form a formation for flying, wherein the airplanes comprise at least one leader plane and at least one bureaucratic plane, and all adopt a combined navigation system to navigate; 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 plane respectively output INS data and GPS data of the long plane, and the INS data and the GPS data of the long plane are subjected to Kalman filtering processing to obtain INS output data of the long plane after GPS correction and are fed back to a wing plane through a data chain;
the MEMS-SINS navigation system and the GPS navigation system of the wing plane respectively output INS data and GPS data of the wing plane, and carry out Kalman filtering processing on the INS data and the GPS data of the wing plane to obtain INS output data of the wing plane corrected by the GPS;
utilizing INS output data of the farm aircraft corrected by the GPS and INS output data of the wing aircraft corrected by the GPS to carry out relative navigation calculation to obtain the relative distance and the relative position of the farm aircraft and the wing aircraft;
and providing the relative distance and the relative position of the director and the bureaucratic plane to a multi-plane cooperative formation flight system in real time.
Further, the INS data and the GPS data of the fans and the fans 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 integrated navigation system, selecting respective state quantities of the MEMS-SINS navigation system and the GPS navigation system according to the error models, not determining a state quantity matrix of the integrated navigation system, and calculating to obtain state equations of the integrated navigation system of the Youji and the Liao-plane according to the state quantity matrix;
obtaining a quantity measurement matrix of the integrated navigation system of each of the lead aircraft and the wing aircraft according to INS data and GPS data of the integrated navigation system of the lead aircraft and the wing aircraft, and calculating a measurement equation of the integrated navigation system of each of the lead aircraft and the wing aircraft according to the quantity measurement matrix;
discretizing the state equation and the measurement equation of the integrated navigation system of each of the permanent aircraft and the wing aircraft to obtain a discretized equation, estimating various error states of the integrated navigation system by using a Kalman filtering algorithm in discrete time, and correcting INS data by using the estimated values of the error states to obtain corrected INS output data of each of the permanent aircraft and the wing aircraft.
Further, the selection of the state quantity at least comprises one or more of a mathematical platform misalignment angle, speed errors of the east direction, the north direction and the sky direction of the carrier, latitude errors, longitude errors and altitude errors, gyroscope constant drift errors, gyroscope related errors and accelerometer system errors.
Further, the state equation is
Wherein, X (t) represents a navigation system state vector under the inertial navigation system at the moment t; w (t) represents a noise vector at time t; f (t) represents a state transition matrix at the time t; g (t) represents the system noise transfer at the time tA matrix;
representing the state derivative vector at time t.
Further, the INS data of the long machine comprises 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;
correspondingly and respectively subtracting the speed and the position of the long machine output by the MEMS-SINS navigation system of the long machine from the speed and the position of the long machine output by the GPS navigation system of the long machine to obtain a measurement matrix of the combined navigation system of the long machine;
the INS data of a wing plane comprise the speed and position of the wing plane output by the MEMS-SINS navigation system; the GPS data of a wing plane comprises the speed and position of the wing plane output by the GPS navigation system;
the speed and the position of the wing plane output by the MEMS-SINS navigation system of the wing plane are respectively subtracted from the speed and the position of the wing plane output by the GPS navigation system of the wing plane, so as to obtain a measurement matrix of the combined navigation system of the wing plane.
Furthermore, the measurement equation of the lead plane and the wing plane is as follows,
Z(t)＝H(t)·X(t)+V(t)
wherein H (t) represents the measurement matrix of the integrated navigation system at time t; x (t) represents a state quantity at t time; v (t) represents the measured noise vector at time t; z (t) represents the measurement vector at time t.
Furthermore, the state equation and the measurement equation of the combined navigation system of the fans and the wings are discretized, and the discretized equation is that,
wherein, XkRepresenting an n-dimensional state vector of the integrated navigation system at time k; zkRepresenting an m-dimensional measurement vector at time k; phik,k-1Is systematic from time k-1 to time kA state transition matrix; hkA measurement matrix representing time k; wk-1The system noise vector at time denoted as k-1; gamma-shapedk-1A system noise matrix which represents the degree that each system noise from the k-1 moment to the k moment respectively influences each state at the k moment; vkThe noise vector is measured for m dimensions at time k.
Further, the Kalman filtering algorithm processing step further comprises state further prediction, state estimation, filtering gain, one-step prediction mean square error and estimation 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 combined navigation has good stability, can reduce errors of position and speed, and has higher navigation precision;
through the introduction of a data chain, the relative navigation between the longplane/bureaucratic planes is realized.
Drawings
FIG. 1 is a schematic diagram of an INS/GPS integrated navigation system with a fixed plane and a fixed plane;
FIG. 2 is a schematic diagram of a distributed relative navigation system according to an embodiment;
fig. 3 shows two calculation loops of kalman filtering according to the present embodiment.
Detailed Description
The present embodiment provides a distributed relative navigation system facing a plurality of airplanes for collaborative formation flying, where the plurality of airplanes includes at least one lead plane and at least one bureaucratic plane, and all use a combined navigation system to navigate.
FIG. 2 is an INS/GPS integrated navigation system of a leader or a leader; 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 the MEMS sensor, and initial reference position and speed information of the MEMS-SINS navigation system is obtained through initial alignment of the MEMS sensor. In the system starting stage, initial alignment of the attitude angle is carried out through the magnetic compass, the real-time attitude angle of the system is obtained through recursion of gyro data through a quaternion method, double integration is carried out on acceleration information to time, and the absolute position and absolute speed information of the carrier are obtained. And the MEMS-SINS navigation system performs inertial navigation solution on the data information acquired by the MEMS sensor to obtain INS data output by the MEMS-SINS navigation system. The INS data comprise the speed and position of the carrier (tractor or wing) output by the MEMS-SINS navigation system.
The GPS navigation system adopts a GPS receiver and then carries out GPS signal extraction to obtain GPS data output by the GPS navigation system. The GPS data include the speed and position of the carrier (a lead or a wing) 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 Youji or the Liao-plane) and GPS data output by the GPS navigation system, and Kalman filtering processing is carried out by a Kalman filter, so as to obtain INS output data of the carrier (the Youji or the Liao-plane) after GPS correction. And (3) subtracting the INS output data of the lead aircraft after GPS correction and the INS output data of the wing aircraft after GPS correction to obtain the relative distance and the relative position of the lead aircraft and the wing aircraft. The relative distance and the relative position of the captain aircraft and the bureaucratic aircraft are provided for a multi-aircraft cooperative formation flight system in real time, so that a more accurate data source is provided for functions of formation aggregation, formation maintenance, reconstruction and the like.
FIG. 2 is a schematic diagram of the distributed relative navigation system according to the embodiment. As shown in fig. 2, the INS output data of the long plane corrected by the GPS is obtained and fed back to the wing plane via a data link. And performing relative navigation calculation by utilizing INS output data of the lead aircraft corrected by the GPS and INS output data of the wing aircraft corrected by the GPS to obtain the relative distance and the relative position of the lead aircraft and the wing aircraft.
Further, in this embodiment, a navigation computer is used to perform calculation processing on the INS data output by the MEMS-SINS navigation system of a carrier (a lead plane or a wing plane) and the GPS data output by the GPS navigation system, and the specific processing steps mainly include the following steps:
step 1: obtaining 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 combined navigation system adopts a common reference system as a north-pointing direction, and the sensor error model adopts a northeast geographical coordinate system. And a sensor error model of the system can be deduced according to the principle of the strapdown inertial navigation algorithm.
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; and b is an inertial navigation carrier coordinate system.
In the northeast geographic coordinate system, when the flying height h and the earth are considered to be a spheroid, there are
In the formula:
E. n, U represent east, north, and day, respectively;
vE、vN、vUis a component of the flight velocity;
φE、φN、φUis the triaxial component of the mathematical platform error angle;
εE、εN、εUis the three-axis component of gyro drift;
radius of the earth Re＝6378245m；
Radius of meridian RM＝Re(1-2f+3f sin2L)；
Radius R of mortise and unitary ringN＝Re(1+f sin2L)；
The global oblateness f is 1/298.257;
angular velocity of rotation omega of the earthie＝7.292×10-5rad/s；
δ represents the error sign; l represents latitude.
In the northeast geographic coordinate system, when considering the flight height h and the earth as a spheroid:
in the formula (I), the compound is shown in the specification,
three-axis component of accelerometer error, fE、fU、fNThe three-axis component of the specific force. When the height channel is not considered, then there is vU,δvUIs zero.
In the northeast geographic coordinate system, when the flying height h and the earth are considered as a rotational ellipsoid, the position error of the inertial navigation output is expressed as
The inertial instrument errors include installation errors, scale coefficient errors, and random errors. For simplicity, only random errors are considered.
The gyro drift in the formula (1) is a gyro drift along a geographical coordinate system of east, north and sky. In the strap-down inertial navigation system, the gyro drift in the formula (1) is required to be equivalent gyro drift converted from the slave system to the geographic system.
Take the gyro drift as
ε＝εb+εr+wg(4)
In the formula, epsilonbIs a random constant; epsilonrIs a first order Markov process; w is agIs white gyroscope noise.
The gyro drift error models in three axial directions are the same and are all
In the formula, TrIs the correlation time; w is arGyroscope markov noise.
Considering the first-order Markov process, and assuming that the error models of the three axial accelerometers are the same, all the error models are
In the formula, TaIs a first order Markov correlation time;is the accelerometer error;
is the accelerometer error derivative; w is aaAccelerometer markov noise.
(2) Selecting the state quantity of the integrated navigation system and calculating to obtain a state equation
And (3) selecting the 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 airborne vehicle is selected as
In the formula, each physical quantity means a mathematical platform misalignment angle, speed errors of a carrier in east, north and sky directions, a latitude error, a longitude error, an altitude error, a gyroscope constant drift error, a gyroscope related error and an accelerometer system error; the subscripts x, y and z of the gyroscope constant drift error, the gyroscope related error and the accelerometer system error respectively represent three directions of coordinate axes. In the present embodiment, the navigation coordinate system, i.e., the components in the northeast geographic coordinate system (i.e., the previous east, north, and sky directions) are used.
The system noise is expressed as
W＝[wgxwgywgzwrxwrywrzwaxwaywaz]T
Wherein the meaning of each physical quantity is: gyroscope white noise, gyroscope markov noise, and accelerometer markov noise.
Obtaining a state equation of the inertial navigation system in a navigation coordinate system:
wherein, X (t) represents a navigation system state vector of an inertial navigation system (MEMS-SINS navigation system) at the time t; w (t) represents a system noise vector at the time t; f (t) represents a system state transition matrix at the time t; g (t) represents a system noise transfer matrix at the time 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 attitude transition matrixDerived from attitude quaternion or attitude angle in the trajectory signal, i.e.
A system state transition matrix F (t), wherein F and F (t) both represent an inertial navigation system state transition matrix, and the expression of F is as follows:
wherein, FS、FM、FNAre all 9x9 square matrixes, and
step 2: obtaining measurement equation of MEMS-SINS navigation system and GPS navigation system
Obtaining a measurement matrix of the integrated navigation system of the aircraft according to the difference between the speed and the position output by the MEMS-SINS navigation system of the aircraft and the speed and the position information output by the GPS navigation system of the aircraft, and calculating a measurement equation of the integrated navigation system of the aircraft, wherein the measurement equation is expressed as
In the formula, L, lambda and h are respectively the position and the speed in three directions of northeast; the superscript INS and GPS represent different navigation systems.
The measurement equation of the integrated navigation system is
Z(t)＝H(t)·X(t)+V(t) (10)
Wherein H (t) represents a measurement matrix at time t of the continuous system; x (t) represents the state quantity of the continuous system at the time t; v (t) represents the measured noise vector at the time t of the continuous system; z (t) represents the measurement vector at time t of the continuous system.
H is the measurement matrix, and H (1,7) is 1, H (2,8) is 1, H (3,9) is 1, and the remaining elements in the H matrix are all zero.
And step 3: kalman filtering algorithm processing for combined navigation system
Discretizing the state equation and the measurement equation of the integrated navigation system of the respective long and wing aircrafts to obtain a discretized equation, estimating various error states of the integrated navigation system by using a discrete time Kalman filtering algorithm, and correcting INS data by using the estimated values of the error states to obtain corrected INS output data of the respective long and wing aircrafts. The embodiment specifically includes the following contents:
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 required, and therefore the state information is estimated by applying a discrete time Kalman filtering algorithm.
Discretizing the equation of state (7) and the equation of measurement (10) can obtain
In the formula, XkAn n-dimensional state vector representing the system at time k, which is also the desired estimated vector; zkRepresenting an m-dimensional measurement vector at time k; phik,k-1The system state transition matrix is from the time k-1 to the time k; hkA measurement matrix representing time k; wk-1A system noise vector representing a pseudo k-1 time instant; gamma-shapedk-1A system noise matrix which represents the degree that each system noise from the k-1 moment to the k moment respectively influences each state at the k moment; vkThe noise vector is measured for m dimensions at time k. And is
In the formula, T is an iteration period, and n is limited in actual calculation.
The system noise and the metrology noise in the state equation and the metrology equation have the following properties:
E[Wk]＝0，
E[Vk]＝0，
Cov[Wk,Vj]＝E[WkVj T]＝0
in the formula, QkFor systematic noise sequences WkThe variance matrix of (2); rkFor measuring noise sequences VkThe variance matrix of (2).
Discrete quantity Qk、RkAnd a continuous quantity Q (t),The relationship of 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 comprises the following steps: on the basis of some measured output quantities of the navigation system, various error states of the system are estimated by Kalman filtering, and the system is corrected by the estimated values of the error states, so that the purpose of system combination is achieved. And correcting the INS data by using the estimated value of the error state to obtain corrected INS output data of the longplane and the wing plane respectively.
Filtering is to extract a desired signal from a plurality of signals mixed together, and kalman filtering estimates the desired signal from a measurement related to the extracted signal by an algorithm. Where the estimated signal is a random response caused by a white noise excitation, the transfer structure between the excitation source and the response (system equation) is known, as is the functional relationship between the quantity measurement and the estimated quantity (metrology equation). The following information is utilized in the estimation process: system equation, measurement equation, statistical characteristic of white noise excitation, and statistical characteristic of measurement error. Because the information used is a quantity in the time domain, the kalman filter is designed in the time domain and is suitable for multidimensional cases.
Kalman filtering has the following characteristics:
(1) the object of the Kalman filtering process is a random signal;
(2) the processed signals have no useful and interference components, and the purpose of filtering is to estimate all the processed signals;
(3) the white noise excitation and the measurement noise of the system are not opposite to each other to be filtered, and the statistical characteristics of the white noise excitation and the measurement noise are just information to be utilized in the estimation process.
The kalman filter is essentially a set of recursive algorithms implemented by a digital computer, and each recursive cycle includes two processes, namely, time update and measurement update of the estimated quantity. And the time updating is determined by the measurement updating result of the last step and the prior information during the design of the Kalman filter, and the measurement updating is determined according to the measurement value obtained in real time on the basis of the time updating. Thus the measurements can be viewed as inputs to the kalman filter, the estimates can be viewed as outputs, and the inputs and outputs are linked by a time update and measurement update algorithm.
Assume initial state X of the system0Is also a normal random vector with mean and covariance of
The basic steps of the discrete Kalman filtering algorithm are as follows:
state one-step prediction
State estimation
Filter gain
Kk＝Pk·Hk T·Rk -1(17)
One-step prediction of mean square error
Pk|k-1＝Φk,k-1·Pk-1·Φk,k-1 T+Γk-1·Qk·Γk-1 T(18)
Estimating mean square error
Pk＝(I-Kk·Hk)Pk|k-1(19)
As long as the initial value of the filter is given
And P0According to the measurement of time kkThen the state estimation at the k moment can be obtained by recursion calculation
The calculation process of the discrete kalman filter algorithm can be represented by fig. 3.
It is evident from fig. 3 that kalman filtering has two computation loops: a gain calculation loop and a filter calculation loop. Wherein the gain calculation loop is an independent calculation loop and the filter calculation loop is dependent on the gain calculation loop.
Claims (8)
1. A distributed relative navigation method facing the cooperative formation flight of a plurality of airplanes, wherein the airplanes comprise at least one longplane and at least one bureaucratic plane, and all adopt an integrated navigation system to carry out navigation; it is characterized in that the preparation method is characterized in that,
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 plane respectively output INS data and GPS data of the long plane, and the INS data and the GPS data of the long plane are subjected to Kalman filtering processing to obtain INS output data of the long plane after GPS correction and are fed back to a wing plane through a data chain;
the MEMS-SINS navigation system and the GPS navigation system of the wing plane respectively output INS data and GPS data of the wing plane, and carry out Kalman filtering processing on the INS data and the GPS data of the wing plane to obtain INS output data of the wing plane corrected by the GPS;
the INS output data of the lead aircraft corrected by the GPS and the INS output data of the wing aircraft corrected by the GPS are subtracted to obtain the relative distance and the relative position of the lead aircraft and the wing aircraft;
and providing the relative distance and the relative position of the director and the bureaucratic plane to a multi-plane cooperative formation flight system in real time.
2. A distributed relative navigation process oriented to the cooperative formation of flights of a plurality of airplanes as claimed in claim 1, wherein the INS data and GPS data of the franchises and the bureaucratics are kalman filtered, and the processing steps each include:
establishing respective error models of an MEMS-SINS navigation system and a GPS navigation system in the integrated navigation system, selecting state quantities according to the error models and determining a state quantity matrix of the integrated navigation system, and calculating to obtain state equations of respective integrated navigation systems of Youji and Lijian according to the state quantity matrix;
obtaining a quantity measurement matrix of the integrated navigation system of the lead aircraft and the wing aircraft according to INS data and GPS data of the integrated navigation system of the lead aircraft and the wing aircraft, and calculating a measurement equation of the integrated navigation system of the lead aircraft and the wing aircraft according to the quantity measurement matrix;
discretizing the state equation and the measurement equation of the integrated navigation system of the respective long and wing aircrafts to obtain a discretized equation, estimating various error states of the integrated navigation system by using a discrete time Kalman filtering algorithm, and correcting INS data by using the estimated values of the error states to obtain corrected INS output data of the respective long and wing aircrafts.
3. The distributed relative navigation method for collaborative formation of flight for multiple aircraft according to claim 2,
the selected state quantity at least comprises one or more of a mathematical platform misalignment angle, speed errors of the east direction, the north direction and the sky direction of the carrier, latitude errors, longitude errors and altitude errors, gyroscope constant drift errors, gyroscope related errors and accelerometer system errors.
4. The method of claim 3, wherein the equation of state is
Wherein, X (t) represents a navigation system state vector under the inertial navigation system at the moment t; w (t) represents a noise vector at time t; f (t) represents a state transition matrix at the time t; g (t) represents a system noise transfer matrix at the time t;
representing the state derivative vector at time t.
5. The distributed relative navigation method for collaborative formation of flight for multiple aircraft according to claim 2,
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;
correspondingly and respectively subtracting the speed and the position of the long machine output by the MEMS-SINS navigation system of the long machine from the speed and the position of the long machine output by the GPS navigation system of the long machine to obtain a measurement matrix of the combined navigation system of the long machine;
the INS data of a wing plane comprise the speed and position of the wing plane output by the MEMS-SINS navigation system; the GPS data of a wing plane comprises the speed and position of the wing plane output by the GPS navigation system;
the speed and the position of a wing plane output by the MEMS-SINS navigation system of the wing plane are subtracted from the speed and the position of a wing plane output by the GPS navigation system of the wing plane, respectively, to obtain a measurement matrix of a combined navigation system of the wing plane.
6. A distributed relative navigation process oriented to the collaborative formation of flights of a plurality of aircraft according to claim 5, wherein the measured equations of the lead plane and the bureaucratic plane are expressed as,
Z(t)＝H(t)·X(t)+V(t)
wherein, H (t) represents the measurement matrix of the integrated navigation system at the time t; x (t) represents a state quantity at t time; v (t) represents the measured noise vector at time t; z (t) represents the measurement vector at time t.
7. The distributed relative navigation method oriented to the cooperative formation flying of multiple airplanes as recited in claim 2, wherein the state equation and the measurement equation of the integrated navigation system of the franchise and the bureaucratic respectively are discretized, and the equation after discretization is expressed as,
wherein, XkRepresenting an n-dimensional state vector of the integrated navigation system at time k; zkRepresenting an m-dimensional measurement vector at time k; phik,k-1The system state transition matrix is from the time k-1 to the time k; hkA measurement matrix representing time k; wk-1The system noise vector at time denoted as k-1; gamma-shapedk-1A system noise matrix which represents the degree that each system noise from the k-1 moment to the k moment respectively influences each state at the k moment; vkThe noise vector is measured for m dimensions at time k.
8. The method of claim 2, wherein the kalman filter algorithm comprises one-step prediction of state, state estimation, filter gain, one-step prediction mean square error, and 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 true CN110849360A (en) | 2020-02-28 |
| CN110849360B 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) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111708377A (en) * | 2020-06-21 | 2020-09-25 | 西北工业大学 | Flight control method based on inertial navigation/flight control system information fusion |
| CN112783203A (en) * | 2020-12-28 | 2021-05-11 | 西北工业大学 | Multi-sensor-based control system and method for unmanned aerial vehicle formation maintenance |
| CN113885577A (en) * | 2021-10-29 | 2022-01-04 | 西北工业大学 | Anti-collision control method, system and device for airplane multi-machine intensive formation |
| CN115597608A (en) * | 2022-12-14 | 2023-01-13 | 湖南警察学院（Cn） | Multi-unmanned aerial vehicle relative positioning method and device, computer equipment and medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080082266A1 (en) * | 2006-09-29 | 2008-04-03 | Honeywell International Inc. | Multipath Modeling For Deep Integration |
| 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 |
-
2019
- 2019-11-25 CN CN201911166367.XA patent/CN110849360B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080082266A1 (en) * | 2006-09-29 | 2008-04-03 | Honeywell International Inc. | Multipath Modeling For Deep Integration |
| 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 |
|---|
| 袁杰波;杨峰;张共愿;梁彦;: "无人机编队飞行导航方法及其仿真研究" * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111708377A (en) * | 2020-06-21 | 2020-09-25 | 西北工业大学 | Flight control method based on inertial navigation/flight control system information fusion |
| CN111708377B (en) * | 2020-06-21 | 2022-10-25 | 西北工业大学 | Flight control method based on inertial navigation/flight control system information fusion |
| CN112783203A (en) * | 2020-12-28 | 2021-05-11 | 西北工业大学 | Multi-sensor-based control system and method for unmanned aerial vehicle formation maintenance |
| CN113885577A (en) * | 2021-10-29 | 2022-01-04 | 西北工业大学 | Anti-collision control method, system and device for airplane multi-machine intensive formation |
| CN113885577B (en) * | 2021-10-29 | 2023-11-28 | 西北工业大学 | Anti-collision control method, system and device for multi-aircraft dense formation of aircraft |
| CN115597608A (en) * | 2022-12-14 | 2023-01-13 | 湖南警察学院（Cn） | Multi-unmanned aerial vehicle relative positioning method and device, computer equipment and medium |
| CN115597608B (en) * | 2022-12-14 | 2023-03-10 | 湖南警察学院 | Multi-unmanned aerial vehicle relative positioning method and device, computer equipment and medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110849360B (en) | 2023-08-01 |
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 | |
| CN110095800B (en) | Multi-source fusion self-adaptive fault-tolerant federal filtering integrated navigation method | |
| CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| CN106990426B (en) | Navigation method and navigation device | |
| CN106500693B (en) | A kind of AHRS algorithm based on adaptive extended kalman filtering | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN105737828B (en) | A kind of Combinated navigation method of the joint entropy Extended Kalman filter based on strong tracking | |
| Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
| CN102538792B (en) | Filtering method for position attitude system | |
| WO2020114301A1 (en) | Magnetic-side roll angular velocity information-based rotary shell flight posture high-precision estimation method | |
| CN109000642A (en) | A kind of improved strong tracking volume Kalman filtering Combinated navigation method | |
| CN106980133A (en) | The GPS INS Combinated navigation methods and system for being compensated and being corrected using neural network algorithm | |
| CN106767787A (en) | A kind of close coupling GNSS/INS combined navigation devices | |
| CN111121766B (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN106989761B (en) | A kind of spacecraft Guidance instrumentation on-orbit calibration method based on adaptive-filtering | |
| CN109708663B (en) | Star sensor online calibration method based on aerospace plane SINS assistance | |
| CN104655152A (en) | Onboard distributed type POS real-time transmission alignment method based on federal filtering | |
| CN104698486A (en) | Real-time navigation method of data processing computer system for distributed POS | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN104833375B (en) | A kind of IMU Two position methods by star sensor | |
| CN113340298A (en) | Inertial navigation and dual-antenna GNSS external reference calibration method | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |