CN112484731A - High-precision real-time flight navigation calculation method - Google Patents

High-precision real-time flight navigation calculation method Download PDF

Info

Publication number
CN112484731A
CN112484731A CN202011359908.3A CN202011359908A CN112484731A CN 112484731 A CN112484731 A CN 112484731A CN 202011359908 A CN202011359908 A CN 202011359908A CN 112484731 A CN112484731 A CN 112484731A
Authority
CN
China
Prior art keywords
navigation
time
gnss
calculation
result
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
Application number
CN202011359908.3A
Other languages
Chinese (zh)
Other versions
CN112484731B (en
Inventor
董春杨
方海红
王君
鞠晓燕
杨宇
彭杰
秦卓
张瑞鹏
苏峰
孟晨光
王菁华
边梦琦
程光耀
窦宇飞
付思帅
凌咸庆
李德标
宋景亮
王晨
王东东
王洁
王玥兮
张超
张竑颉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Aerospace Changzheng Aircraft Institute
Original Assignee
Beijing Aerospace Changzheng Aircraft Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Aerospace Changzheng Aircraft Institute filed Critical Beijing Aerospace Changzheng Aircraft Institute
Priority to CN202011359908.3A priority Critical patent/CN112484731B/en
Publication of CN112484731A publication Critical patent/CN112484731A/en
Application granted granted Critical
Publication of CN112484731B publication Critical patent/CN112484731B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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
    • 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

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)
  • Navigation (AREA)

Abstract

A high-precision real-time flight navigation calculation method utilizes a high-speed processor to sequentially execute SINS navigation calculation, inertial navigation extrapolation, SINS navigation calculation and integrated navigation Kalman filtering after receiving INS navigation data and GNSS information. In a typical working scene of the working of the invention, the SINS navigation is resolved for 0.5ms, the time spent by the GNSS for extrapolating the SINS navigation is less than 0.01ms, the time spent by the GNSS for extrapolating the SINS navigation can be ignored, and the combined navigation Kalman filtering is 1.6 ms. Therefore, the total time consumption is about 0.5 × 2+1.6 ═ 2.6ms, and the system real-time performance requirement that the combined navigation calculation period is greater than 3.25ms can be met by considering 80% of the time margin. The invention fully utilizes double CPU resources, can realize strict real-time navigation computation performance requirements, and can realize accurate integrated navigation.

Description

High-precision real-time flight navigation calculation method
Technical Field
The invention relates to a high-precision real-time flight navigation calculation method, and belongs to the technical field of control navigation calculation and attitude measurement.
Background
The GNSS (global navigation satellite system) can realize the measurement of position, speed and time, but cannot stably provide correct navigation information in an application scene that the working electromagnetic environment of an aircraft is complex or satellite signals are affected; an INS (inertial navigation system) can realize the measurement of position, speed and attitude, but as a navigation mode of integral calculation, there is an inherent defect that errors accumulate with time; and for complex attitude motions of the spinning aircraft, the sensor performance of the INS may be reduced.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the invention provides a high-precision real-time flight navigation calculation method for real-time accurate navigation calculation, and provides a scheme for performing combined navigation calculation by using position and speed information output by a Global Navigation Satellite System (GNSS) and speed and acceleration information output by an Inertial Navigation System (INS).
The technical scheme of the invention is as follows:
a high-precision real-time flight navigation calculation method comprises the following steps:
step 1.1, obtaining an initial time t0The reentry vehicle attitude initial value of (a) comprises: initial pitch angle
Figure RE-GDA0002886246560000011
Initial heading angle psi0And initial roll angle gamma0；
Step 1.2,Determining an initial value of the aircraft position as binding data, wherein the binding data comprises: initial latitude L0Initial longitude λ0Initial height value h0(ii) a Initial value v of velocityN,0,vU,0,vE,0；
Step 1.3, determining an attitude quaternion Q (t) according to the attitude initial value obtained in the step 1.10) Attitude transformation matrix from missile coordinate system to navigation coordinate system
Figure RE-GDA0002886246560000012
Step 1.4, using the speed and acceleration information output by the INS inertial navigation system, and the initial value of the attitude, the initial value of the speed and the initial value of the position, executing navigation calculation according to a navigation calculation period to obtain an attitude quaternion Q (t) of the reentry vehicle1)＝[q1,0 q1,1 q1,2 q1,3]TVelocity vN,k,vU,k,vE,kAnd location information latitude LkLongitude λ, longitudekHeight value hk；
Step 1.5, using the inertial navigation extrapolation method, correcting the navigation calculation result by using the receiver data, and obtaining the attitude quaternion, speed and position information latitude, longitude and altitude values of the reentry vehicle consistent with the receiver as the navigation calculation result (L)k,λk,hk,vN,k,vU,k,vE,k,qk,0,qk,1,qk,2,qk,3)；
Step 1.6, judging the reliability of the GNSS navigation signal by combining the PPS pulse synchronous signal state, selecting position and speed measurement data provided by the GNSS, and carrying out delay compensation on position and speed information of the GNSS receiver according to the GNSS data delay time provided by the PPS pulse synchronous signal to obtain a position compensation value and a speed information compensation value of the GNSS receiver after delay compensation;
step 1.7, result (L) of navigation solution using step 1.5k,λk,hk,vN,k,vU,k,vE,k,qk,0,qk,1,qk,2,qk,3) Position (L) of the GNSS receiver compensated with the delay of step 1.6Gk、λGk、hGk) Velocity information (v)GN,k、 vGU,k、vGE,k) Performing combined navigation filtering on the compensation value to obtain position, speed and attitude information of the filtered aircraft as a combined navigation result;
and step 1.8, respectively feeding back the inertial navigation extrapolation result obtained in the step 1.5 and the combined navigation result obtained in the step 1.7 to a navigation resolving module.
A dual-CPU runtime platform, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method according to claim 1 when executing the computer program, and specifically:
after receiving the GNSS data at the time k, firstly performing inertial navigation extrapolation correction on the GNSS data by using navigation resolving data according to the step 1.5, replacing an SINS navigation resolving result at the time k-1 with the GNSS data, and performing navigation resolving at the time k; meanwhile, an inertial navigation extrapolation result is transmitted to integrated navigation Kalman filtering;
performing linear extrapolation processing on the SINS navigation resolving result at the k-1 moment;
and after receiving inertial navigation extrapolation data, the k-moment integrated navigation Kalman filter uses a linear extrapolation result of the k-moment SINS navigation calculation result at the k-1 moment, calculates an integrated navigation Kalman filter result by using the method in the step 1.7, and feeds the calculation result back to the k +1 moment SINS navigation calculation module.
Compared with the prior art, the invention has the beneficial effects that:
1) the invention uses the inertial navigation extrapolation method, uses GNSS data to compensate INS (inertial navigation system) as a navigation mode of integral calculation, and has the inherent defect that errors are accumulated along with time;
2) the method judges the reliability of the GNSS (global navigation satellite system) signal by combining the PPS signal state, selects and uses the position and speed measurement data of the GNSS, and uses the position and speed measurement result of the receiver as much as possible to carry out combined navigation calculation under the condition of stably providing correct navigation information in the flight process of the aircraft;
3) the invention realizes the rapid combination of navigation calculation by using the double CPU system and reasonably distributing the calculation modules, thereby meeting the performance requirement of the system.
Drawings
FIG. 1 illustrates a GNSS data compensation method.
FIG. 2 is a block diagram of a navigation computation flow.
FIG. 3 Dual CPU navigation computation scheme.
FIG. 4 is a dual CPU navigation high speed computing scheme.
Detailed Description
In order to realize high-precision real-time control navigation or attitude angle measurement of an aircraft, position and speed information output by a GNSS (global navigation satellite system) and speed and acceleration information output by an INS (inertial navigation system) need to be comprehensively processed, and by means of combined navigation calculation, the advantages of the two systems are complemented, continuous, all-weather and high-precision navigation is realized, and further, the performance which is better than that of any navigation equipment used independently in the aspects of precision and reliability can be obtained.
In order to realize the real-time integrated navigation computation, a high-precision real-time processing system is designed, double CPU resources are fully utilized, the strict real-time navigation computation performance requirement can be realized, and the purpose of accurate integrated navigation is realized.
The invention is described in further detail below with reference to the figures and the detailed description. The invention discloses a high-precision real-time navigation calculation method, which comprises the following steps:
step 1, design of integrated navigation calculation method
Step 1.1, obtaining an initial time t0The initial value of the attitude of the reentry vehicle, the initial pitch angle
Figure RE-GDA0002886246560000041
Initial heading angle psi0And initial roll angle gamma0。
As is known, an INS (inertial navigation System) outputs a frame of aircraft angular velocity (ω) every 2msx，k、ωy，k、ωz，k) And acceleration (n)x，k、ny，k、nz，k) The method accumulates 250 frames of INS data within 1 minute, and averages the acceleration information to obtain the average value of the acceleration
Figure RE-GDA0002886246560000042
And calculating to obtain the initial attitude of the aircraft by using the average value of the acceleration, wherein the calculation method comprises the following steps:
Figure RE-GDA0002886246560000043
Figure RE-GDA0002886246560000044
ψ0＝0
step 1.2, determining the initial value of the position as the initial latitude L of binding0Initial longitude λ0Initial height value h0Initial value of velocity vN,0＝0，vU,0＝0，vE,0＝0。
Step 1.3, determining other representation forms of the initial attitude according to the attitude initial value obtained in the step 1, including an attitude quaternion Q (t)0) And attitude transformation matrix
Figure RE-GDA0002886246560000045
The relationship between the attitude transformation matrix and the attitude initial value is as follows:
Figure RE-GDA0002886246560000046
the attitude quaternion is represented using the attitude transformation matrix as follows:
Q(t0)＝[q0,0 q0,1 q0,2 q0,3]Twherein
Figure RE-GDA0002886246560000047
Step 1.4, using the speed and acceleration information output by the INS (inertial navigation system) and initial values of the attitude, the speed and the position, executing navigation calculation every 2ms to obtain the attitude Q (t) of the reentry vehiclek)＝[qk，0，qk，1，qk，2，qk，3]TVelocity vN，k,vU，k,vE，kAnd location information latitude LkLongitude λ, longitudekHeight value hk。
Step 1.4.1, calculating attitude quaternion Q (t)k)
Q(tk)＝[qk,0，qk,1，qk,2，qk,3]T；
Figure RE-GDA0002886246560000051
In the formula (I), the compound is shown in the specification,
Figure RE-GDA0002886246560000052
is the attitude quaternion at the kth time
Figure RE-GDA0002886246560000053
Is the attitude quaternion at the k-1 th moment
While
Figure RE-GDA0002886246560000054
Wherein
Figure RE-GDA0002886246560000055
Figure RE-GDA0002886246560000056
Figure RE-GDA0002886246560000057
And
Figure RE-GDA0002886246560000058
the calculation method is as follows for the projection of the angular velocity of rotation around the earth caused by the angular velocity of rotation of the earth and the carrier velocity of the navigation coordinate system on the navigation system.
Figure RE-GDA0002886246560000059
Figure RE-GDA0002886246560000061
Wherein
Figure RE-GDA0002886246560000062
Re6378137.0m is the earth's semi-major axis,
Figure RE-GDA0002886246560000063
the eccentricity of the earth is defined as f 1/298.257223563, and the oblateness of the earth is defined as f.
In the above calculation method, k is the calculation period, k-1 is the last calculation period, the first calculation period uses the initial value, TsTo resolve the cycle duration, it is 2ms in the system of the present invention.
Step 1.4.2, speed calculation
The differential equation for velocity is solved as:
Figure RE-GDA0002886246560000064
wherein the content of the first and second substances,
Figure RE-GDA0002886246560000065
the speed of the aircraft under the geographic coordinate system at the kth moment is respectively
North, sky, east speed;
Figure RE-GDA0002886246560000066
the speed of the aircraft at the k-1 th time in the geographic coordinate system is respectively the speed of the north, the speed of the sky and the speed of the east;
Figure RE-GDA0002886246560000067
an attitude transformation matrix;
Figure RE-GDA0002886246560000068
the specific forces output by the inertia measurement assembly under the coordinate system of the aircraft at the moment k are the specific forces in the forward direction, the upward direction and the right direction respectively. The scale factor of the data output by the inertial measurement unit is g09.80665, the formula:
Figure RE-GDA0002886246560000069
g0＝9.78049m/s2is the ground surface gravitational acceleration.
Figure RE-GDA0002886246560000071
Is the gravity acceleration vector under the geographic coordinate system:
Figure RE-GDA0002886246560000072
Figure RE-GDA0002886246560000073
Figure RE-GDA0002886246560000074
as the detrimental acceleration term:
Figure RE-GDA0002886246560000075
recording the cumulative increment of speed
Figure RE-GDA0002886246560000076
Step 1.4.3, position calculation
The differential equation for position is solved as:
Figure RE-GDA0002886246560000077
wherein L isk-1、λk-1、hk-1Resolving latitude, precision and height data obtained at the k-1 moment; v. ofN,k、 vU,k、vE,kCalculating the north, the sky and the east speeds of the geographic coordinate system at the moment k; t issThe cycle is solved for navigation.
Step 1.5, using inertial navigation extrapolation method, using INS information to correct GNSS receiver data
Because the GNSS data has time delay due to the calculation of the receiver, the time delay needs to be compensated for realizing high-precision real-time navigation calculation. The aircraft has no larger maneuver in the flight process, and can receive better effect by adopting a simple method, namely, the received GNSS information is directly subjected to extrapolation delay compensation by directly utilizing the INS information at the current latest moment. As shown in fig. 1.
The PPS signals are hardware synchronization signals, and the GNSS data delay specific time can be obtained by acquiring the PPS signals
The GNSS data is compensated as follows:
Figure RE-GDA0002886246560000081
wherein:
Figure RE-GDA0002886246560000082
m is the compensation period calculated by the GNSS data delay for the velocity update amount at the k-th time of the INS.
Figure RE-GDA0002886246560000083
For the position update amount at the k-th time of the INS, m is the compensation period calculated by the GNSS data delay.
Step 1.6, judging the reliability of the GNSS (global navigation satellite system) signal by combining the PPS signal state, and selecting the position and speed measurement data of the GNSS;
because the electromagnetic environment of the aircraft is complex or the application scenario of the affected satellite signal may not stably provide correct navigation information, in order to implement high-precision navigation calculation, the following judgment needs to be performed according to the PPS signal state and the GNSS data information, and the GDOP threshold value G is determined according to the GNSS signal characteristics0And continuous reliable duty cycle Gt:
GNSS positioning and GDOP<G0When, if
Firstly, a PPS signal comes, and inertial navigation extrapolation and integrated navigation processing are carried out;
② the PPS signal is not coming
a) If the arrival time of the last PPS signal is greater than Gt, the delta V is not usedfk，ΔPfkPerforming time delay compensation, and directly using GNSS information to perform inertial navigation extrapolation calculation and integrated navigation calculation;
b) if the time from the last PPS signal to the coming time is less than Gt, inertial navigation extrapolation calculation and integrated navigation calculation are carried out without using GNSS information;
when GNSS is not positioning, over-limit or without GNSS data
And the inertial navigation extrapolation calculation and the integrated navigation calculation are carried out without using GNSS information.
Step 1.7, result (L) of solution by navigationk,λk,hk,vN,k,vU,k,vE,k,qk,0,qk,1,qk,2,qk,3) With position (L) of the time-delay compensated GNSS receiverGk、λGk、hGk) Velocity information (v)GN,k、vGU,k、vGE,k) And carrying out combined navigation filtering on the compensation value to obtain the filtered position, speed and attitude information.
1.7.1 Kalman Filter initialization
And the integrated navigation adopts Kalman filtering. The combined navigation selects 15-order state variables which are respectively 3 position error variables, 3 speed error variables, 3 attitude error variables, 3 gyro drift variables and 3 accelerometer zero-offset variables, namely:
Figure RE-GDA0002886246560000091
the observed quantity selects 6-order variables which are respectively 3 position errors and 3 speed errors, namely:
Figure RE-GDA0002886246560000092
the state equation of the combined navigation basic equation is as follows:
Figure RE-GDA0002886246560000093
the measurement equation is as follows:
Zk＝Hk·Xk+Vk
in the equation of state and measurement equation, WkExciting a noise sequence for the system, VkFor measuring noise sequences, while WkAnd VkThe following conditions are satisfied: e [ W ]k]＝0，Cov[Wk,Wj]＝E[WkWj T]＝Qkδkj；E[Vk]＝0，
Cov[Vk,Vj]＝E[VkVj T]＝Rkδkj；Cov[Wk,Vj]＝E[WkVj T]＝0。QkThe variance matrix of the system noise sequence is a non-negative array; rkThe variance matrix of the measured noise sequence is a positive definite matrix. Driving the matrix Γ due to noisek-1Posture of heel only
Figure RE-GDA0002886246560000094
Correlation, assuming that the gyro drift noise variance matrix under the aircraft coordinate system is QbThe elements are usually equal, and there is variance transformation relation
Figure RE-GDA0002886246560000095
For QkOther terms have similar relationships to noise, so Γ is omitted from the calculationk-1. Measurement array Hk＝[I6×606×9]。
2 calculation of
Figure RE-GDA0002886246560000096
And calculating according to the navigation calculation result at the moment k.
Figure RE-GDA0002886246560000101
Figure RE-GDA0002886246560000102
Figure RE-GDA0002886246560000103
Wherein, ω isie＝7.292115×10-5rad/s is the angular velocity of rotation of the earth, Re6378137.0m is the earth's semi-major axis.
Figure RE-GDA0002886246560000104
Figure RE-GDA0002886246560000105
Initiating value
h、L0From binding data, calculate Rm、RnThe value used L is also the binding value L0
g0＝9.80665
Initial values of system state variables:
X0＝015×1
mean square error initial value of system state estimation error:
Figure RE-GDA0002886246560000106
diag represents a diagonal matrix, and the remaining elements except the diagonal are 0. The units of the parameters in P0 are as follows:
diag[m m m m/s m/s m/s rad rad rad rad/s rad/s rad/s m/s2 m/s2 m/s2]2
initial value of the variance matrix of the system noise sequence:
Figure RE-GDA0002886246560000111
Q0the unit of the middle parameter is as follows:
diag[m m m m/s m/s m/s rad rad rad rad/s rad/s rad/s m/s2 m/s2 m/s2]2
initial value of the variance matrix of the measured noise sequence:
Figure RE-GDA0002886246560000112
R0the unit of the middle parameter is as follows: diag [ mm m m/s m/s m/s]2
Note: p0、Q0A diagonal matrix of 15 x 15; r0Is a 6 x 6 diagonal matrix. P0、Q0、R0The initial value is selected according to indexes of the inertial measurement unit and the receiver, different equipment initial values are selected differently, and the empirical value is obtained through multiple tests.
1.7.2 Kalman Filter processing
In the k filtering period, the selection of the state quantity is XkAnd the quantity is measured and calculated as Zk。
The one-step transfer matrix for solving the error differential equation is Ak,k-1Wherein A is a 15 x 15 dimensional matrix, and the non-zero term is calculated as follows, wherein Vx，Vy，VzAre each vNf,k，vUf,k，vEf,k：
Figure RE-GDA0002886246560000113
Figure RE-GDA0002886246560000114
Figure RE-GDA0002886246560000115
Figure RE-GDA0002886246560000116
Figure RE-GDA0002886246560000117
Ak,k-1(3,5)＝1
Figure RE-GDA0002886246560000118
Figure RE-GDA0002886246560000121
Figure RE-GDA0002886246560000122
Figure RE-GDA0002886246560000123
Figure RE-GDA0002886246560000124
Ak,k-1(4,8)＝-fE
Ak,k-1(4,9)＝fU
Figure RE-GDA0002886246560000125
Figure RE-GDA0002886246560000126
Figure RE-GDA0002886246560000127
Ak,k-1(5,1)＝-2ωieVzsin(Lk)
Figure RE-GDA0002886246560000128
Figure RE-GDA0002886246560000129
Figure RE-GDA00028862465600001210
Ak,k-1(5,7)＝fE
Ak,k-1(5,9)＝-fN
Figure RE-GDA00028862465600001211
Figure RE-GDA00028862465600001212
Figure RE-GDA00028862465600001213
Figure RE-GDA00028862465600001214
Figure RE-GDA00028862465600001215
Figure RE-GDA0002886246560000131
Figure RE-GDA0002886246560000132
Figure RE-GDA0002886246560000133
Ak,k-1(6,7)＝-fU
Ak,k-1(6,8)＝fN
Figure RE-GDA0002886246560000134
Figure RE-GDA0002886246560000135
Figure RE-GDA0002886246560000136
Ak,k-1(7,1)＝-ωiesin(Lk)
Figure RE-GDA0002886246560000137
Figure RE-GDA0002886246560000138
Figure RE-GDA0002886246560000139
Figure RE-GDA00028862465600001310
Figure RE-GDA00028862465600001311
Figure RE-GDA00028862465600001312
Figure RE-GDA00028862465600001313
Figure RE-GDA00028862465600001314
Figure RE-GDA00028862465600001315
Figure RE-GDA00028862465600001316
Ak,k-1(8,7)＝-Ak,k-1(7,8)
Ak,k-1(8,9)＝-Ak,k-1(9,8)
Figure RE-GDA0002886246560000141
Figure RE-GDA0002886246560000142
Figure RE-GDA0002886246560000143
Figure RE-GDA0002886246560000144
Figure RE-GDA0002886246560000145
Figure RE-GDA0002886246560000146
Figure RE-GDA0002886246560000147
Figure RE-GDA0002886246560000148
Figure RE-GDA0002886246560000149
Figure RE-GDA00028862465600001410
When the filtering is updated, A needs to be updatedk,k-1Discretizing to obtain Fk,k-1(ii) a W is to bekDiscretizing to obtain Qk. The discretization process is as follows:
Figure RE-GDA00028862465600001411
wherein Q0Is the initial value of the state error, Ts1Is 100ms for the filtering period (i.e., GNSS data update time).
Solving a one-step prediction mean square error matrix:
according to the state equation, the system one-step prediction mean square error equation is as follows:
Figure RE-GDA00028862465600001412
filter gain equation:
Kk＝Pk/k-1Hk T(HkPk/k-1Hk T+Rk)-1
one-step prediction equation of state:
Figure RE-GDA00028862465600001413
the optimal state estimation equation:
Figure RE-GDA0002886246560000151
estimating a mean square error equation:
Figure RE-GDA0002886246560000152
by using XkAnd correcting a filtering result:
Figure RE-GDA0002886246560000153
Figure RE-GDA0002886246560000154
Figure RE-GDA0002886246560000155
step 1.8, respectively feeding back the inertial navigation extrapolation result and the integrated navigation result to a navigation resolving module
After the INS navigation calculation, the inertial navigation extrapolation and the integrated navigation calculation at the time k, inertial navigation extrapolation and integrated navigation results need to be fed back to the INS navigation calculation respectively, so as to make up the inherent defect that errors accumulate along with time when the INS (inertial navigation system) is used as a navigation mode of integral calculation.
The flow chart of the high-precision navigation calculation method is shown in FIG. 2 in combination with the description of the steps 1.1-1.7.
Step 2, determining real-time calculation method of combined navigation algorithm
The method comprises the following steps: algorithm module serial operation
And utilizing a high-speed processor to sequentially execute SINS navigation calculation, inertial navigation extrapolation, SINS navigation calculation and integrated navigation Kalman filtering after receiving the INS navigation data and the GNSS information.
In a typical working scene of the operation of the invention, the SINS navigation is resolved for 0.5ms, the time spent by the GNSS for extrapolating the SINS navigation is less than 0.01ms, the time spent by the GNSS for extrapolating the SINS navigation can be ignored, and the combined navigation Kalman filtering is 1.6ms, so that the operation is independent due to different navigation resolving inputs in the two times in the figure 2. Therefore, the total time consumption is about 0.5 × 2+1.6 ═ 2.6ms, and the system real-time performance requirement that the combined navigation calculation period is greater than 3.25ms can be met by considering 80% of the time margin.
The method 2 comprises the following steps: operating platform by using double CPUs
In a typical scenario of the present invention, two CPUs can run independently and simultaneously, and the two CPUs are used to run simultaneously, so as to construct a workflow as shown in fig. 3.
In this case, the navigation calculation time is mainly concentrated on the CPU1, and the total time is about 0.5+1.6 — 2.1ms, and the system real-time performance requirement that the combined navigation calculation period is greater than 2.625ms can be satisfied in consideration of 80% of the time margin.
The method 3 comprises the following steps: parallel work + data compensation
In a typical working scenario of the present invention, however, the real-time performance of the system requires that all the combined navigation calculations be completed within 2 ms.
According to the time measurement, the integrated navigation kalman filter calculation is a main time unit, but in combination with processing resource analysis, the total processing resource of the dual CPUs is 4ms in natural timing time of 2ms, and the available processing resource is 4 × 0.8-3.2 ms by calculating 80% of the time margin, and since 3.2ms >2.625ms, in principle, the dual CPUs can support real-time navigation calculation.
The calculation division of work and the redistribution of the processing flow of the dual CPUs are performed according to the processing time of each module as shown in fig. 4.
After the GNSS data is received at the time k, firstly, the navigation resolving data is used for inertial navigation extrapolation correction of the GNSS data according to the description of the step 1.5, and the GNSS data is used for replacing the SINS navigation resolving result at the time k-1 to perform navigation resolving at the time k. Meanwhile, an inertial navigation extrapolation result is transmitted to integrated navigation Kalman filtering;
and 2.2, performing linear extrapolation processing on the SINS navigation calculation result at the k-1 moment. Because the 2ms time is short and the navigation calculation change is not large, a value which is close to the actual navigation calculation can be obtained by linear extrapolation, so that the whole system can ensure high precision to the maximum extent on the basis of meeting the real-time performance;
and 2.3, after receiving the inertial navigation extrapolation data, using the SINS navigation calculation result at the moment k-1 to calculate a combined navigation Kalman filtering result by using the method in the step 1.7, and feeding the calculation result back to the SINS navigation calculation module at the moment k +1.
For simple description, the INS data updating period and the navigation resolving period are set to be 2ms, and if the target processor has strong computing capability or the system performance requirement is loose, the high-precision real-time navigation calculation can be realized by using the methods 1 and 2; if the target processor has general computing power and has more rigorous system performance requirements, the method 3 can be used for realizing high-precision real-time navigation calculation under the resource operation condition.
Those skilled in the art will appreciate that the details of the invention not described in detail in the specification are within the skill of those skilled in the art.

Claims (2)

1. A high-precision real-time flight navigation calculation method is characterized by comprising the following steps:
step 1.1, obtaining an initial time t0The reentry vehicle attitude initial value of (a) comprises: initial pitch angle
Figure FDA0002803683520000012
Initial heading angle psi0And initial roll angle gamma0；
Step 1.2, determining an initial value of the aircraft position as binding data, wherein the binding data comprises: initial latitude L0Initial longitude λ0Initial height value h0(ii) a Initial value v of velocityN,0,vU,0,vE,0；
Step 1.3, determining an attitude quaternion Q (t) according to the attitude initial value obtained in the step 1.10) Attitude transformation matrix from missile coordinate system to navigation coordinate system
Figure FDA0002803683520000011
Step 1.4, using the speed and acceleration information output by the INS inertial navigation system, and the initial value of the attitude, the initial value of the speed and the initial value of the position, executing navigation calculation according to a navigation calculation period to obtain an attitude quaternion Q (t) of the reentry vehicle1)＝[q1,0q1,1 q1,2 q1,3]TVelocity vN,k,vU,k,vE,kAnd location information latitude LkLongitude λ, longitudekHeight value hk；
Step 1.5, using the inertial navigation extrapolation method, correcting the navigation calculation result by using the receiver data, and obtaining the attitude quaternion, speed and position information latitude, longitude and altitude values of the reentry vehicle consistent with the receiver as the navigation calculation result (L)k,λk,hk,vN,k,vU,k,vE,k,qk,0,qk,1,qk,2,qk,3)；
Step 1.6, judging the reliability of the GNSS navigation signal by combining the PPS pulse synchronous signal state, selecting position and speed measurement data provided by the GNSS, and carrying out delay compensation on position and speed information of the GNSS receiver according to the GNSS data delay time provided by the PPS pulse synchronous signal to obtain a position compensation value and a speed information compensation value of the GNSS receiver after delay compensation;
step 1.7, result (L) of navigation solution using step 1.5k,λk,hk,vN,k,vU,k,vE,k,qk,0,qk,1,qk,2,qk,3) Position (L) of the GNSS receiver compensated with the delay of step 1.6Gk、λGk、hGk) Velocity information (v)GN,k、vGU,k、vGE,k) Performing combined navigation filtering on the compensation value to obtain position, speed and attitude information of the filtered aircraft as a combined navigation result;
and step 1.8, respectively feeding back the inertial navigation extrapolation result obtained in the step 1.5 and the combined navigation result obtained in the step 1.7 to a navigation resolving module.
2. A dual CPU execution platform comprising a memory, a processor, and a computer program stored in said memory and executable on said processor, wherein: the steps of the method according to claim 1 are implemented when the processor executes the computer program, in particular:
after receiving the GNSS data at the time k, firstly performing inertial navigation extrapolation correction on the GNSS data by using navigation resolving data according to the step 1.5, replacing an SINS navigation resolving result at the time k-1 with the GNSS data, and performing navigation resolving at the time k; meanwhile, an inertial navigation extrapolation result is transmitted to integrated navigation Kalman filtering;
performing linear extrapolation processing on the SINS navigation resolving result at the k-1 moment;
and after receiving inertial navigation extrapolation data, the k-moment integrated navigation Kalman filter uses a linear extrapolation result of the k-moment SINS navigation calculation result at the k-1 moment, calculates an integrated navigation Kalman filter result by using the method in the step 1.7, and feeds the calculation result back to the k +1 moment SINS navigation calculation module.
CN202011359908.3A 2020-11-27 2020-11-27 High-precision real-time flight navigation calculation method Active CN112484731B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011359908.3A CN112484731B (en) 2020-11-27 2020-11-27 High-precision real-time flight navigation calculation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011359908.3A CN112484731B (en) 2020-11-27 2020-11-27 High-precision real-time flight navigation calculation method

Publications (2)

Publication Number Publication Date
CN112484731A true CN112484731A (en) 2021-03-12
CN112484731B CN112484731B (en) 2023-05-09

Family

ID=74936127

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011359908.3A Active CN112484731B (en) 2020-11-27 2020-11-27 High-precision real-time flight navigation calculation method

Country Status (1)

Country Link
CN (1) CN112484731B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114295122A (en) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 SINS _ GNSS time synchronization method and system for embedded system

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
CN201497509U (en) * 2009-06-12 2010-06-02 西安星展测控科技有限公司 Double-antenna GPS/INS combined navigator
EP2574880A2 (en) * 2011-09-30 2013-04-03 Maishi Electronic (Shanghai) Ltd. A method, apparatus and system with error correction for an inertial navigation system
CN104181574A (en) * 2013-05-25 2014-12-03 成都国星通信有限公司 Strapdown inertial navigation system/global navigation satellite system combined based navigation filter system and method
CN111947681A (en) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 Filtering correction method for GNSS and inertial navigation combined navigation position output

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
CN201497509U (en) * 2009-06-12 2010-06-02 西安星展测控科技有限公司 Double-antenna GPS/INS combined navigator
EP2574880A2 (en) * 2011-09-30 2013-04-03 Maishi Electronic (Shanghai) Ltd. A method, apparatus and system with error correction for an inertial navigation system
CN104181574A (en) * 2013-05-25 2014-12-03 成都国星通信有限公司 Strapdown inertial navigation system/global navigation satellite system combined based navigation filter system and method
CN111947681A (en) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 Filtering correction method for GNSS and inertial navigation combined navigation position output

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
雷浩然: ""弹载SINS/GNSS组合导航系统研究"", 《中国优秀硕士学位论文全文数据库工程科技II辑》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114295122A (en) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 SINS _ GNSS time synchronization method and system for embedded system

Also Published As

Publication number Publication date
CN112484731B (en) 2023-05-09

Similar Documents

Publication Publication Date Title
CN108535755B (en) GNSS/IMU vehicle-mounted real-time integrated navigation method based on MEMS
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
CA3003298C (en) Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
US7328104B2 (en) Systems and methods for improved inertial navigation
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
CN108594283B (en) Free installation method of GNSS/MEMS inertial integrated navigation system
CN106767797B (en) inertial/GPS combined navigation method based on dual quaternion
JP4199553B2 (en) Hybrid navigation device
US20040133346A1 (en) Attitude change kalman filter measurement apparatus and method
CN110440830B (en) Self-alignment method of vehicle-mounted strapdown inertial navigation system under movable base
KR101294623B1 (en) Enhanced ground based precise attitude determination method of imaging satellite
CN109459776B (en) GNSS/INS deep integrated navigation method based on GNSS signal discontinuous tracking
CN1910428A (en) System and method for using multiple aiding sensors in a deeply integrated navigation system
CN111965685B (en) Low-orbit satellite/inertia combined navigation positioning method based on Doppler information
CN102538792A (en) Filtering method for position attitude system
Hansen et al. Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN112146683B (en) Inertial measurement unit calibration parameter adjusting method and device and electronic equipment
CN113783652A (en) Data synchronization method and device of combined navigation system
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN112484731B (en) High-precision real-time flight navigation calculation method
Kumar Integration of inertial navigation system and global positioning system using kalman filtering
CN113566850A (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN116817905A (en) Wearable multi-MEMS quasi-real-time collaborative navigation system and method

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