CN113432612B - Navigation method, device and system for flying object - Google Patents

Navigation method, device and system for flying object Download PDF

Info

Publication number
CN113432612B
CN113432612B CN202110686092.3A CN202110686092A CN113432612B CN 113432612 B CN113432612 B CN 113432612B CN 202110686092 A CN202110686092 A CN 202110686092A CN 113432612 B CN113432612 B CN 113432612B
Authority
CN
China
Prior art keywords
time
noise
matrix
error
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110686092.3A
Other languages
Chinese (zh)
Other versions
CN113432612A (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 Information Science and Technology University
Original Assignee
Beijing Information Science and Technology University
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 Information Science and Technology University filed Critical Beijing Information Science and Technology University
Priority to CN202110686092.3A priority Critical patent/CN113432612B/en
Publication of CN113432612A publication Critical patent/CN113432612A/en
Application granted granted Critical
Publication of CN113432612B publication Critical patent/CN113432612B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/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/18—Stabilised platforms, e.g. by gyroscope
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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

Abstract

The invention discloses a navigation method, a navigation device and a navigation system of a flight object. Wherein, the method comprises the following steps: acquiring the motion state information of the flying object; estimating at least one of system noise and measurement noise based on a pre-constructed integrated navigation model and the motion state information; modifying inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and navigating the flight volume based on the modified inertial navigation data. The invention solves the technical problem of poor real-time performance caused by the fact that the Jacobian matrix needs to be calculated every time during navigation.

Description

Navigation method, device and system of flying object
Technical Field
The invention relates to the field of computer navigation, in particular to a navigation method, a navigation device and a navigation system of a flight object.
Background
The high-speed spinning flying body has certain difficulty in the process of guidance. The rocket projectile has a large rolling angle, so that the satellite has high difficulty in capturing signals; the transverse roll angle is too large in the flying process of the rocket projectile, a large dynamic measurement range is needed, and the requirement on the precision of a gyroscope is high.
With the combined navigation scheme, EKF filtering methods are typically used. In the process of constructing the model, because the system is a nonlinear system, linearization is needed, but the Jacobian matrix operation is used, so that the real-time performance is influenced to a certain extent; in addition, in the process of using the EKF filtering algorithm, the system noise matrix Q and the measurement noise matrix R cannot be estimated in real time, and the filtering result has an error because the values are fixed in the EKF application process.
In view of the above problems, no effective solution has been proposed.
Disclosure of Invention
The embodiment of the invention provides a navigation method, a navigation device and a navigation system of a flight object, which at least solve the technical problem of poor real-time performance caused by the fact that a Jacobian matrix needs to be calculated every time during navigation.
According to an aspect of an embodiment of the present invention, there is provided a navigation method for a flying object, including: acquiring the motion state information of the flying object; estimating at least one of system noise and measurement noise based on a pre-constructed integrated navigation model and the motion state information; modifying inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and navigating the flight volume based on the modified inertial navigation data.
According to another aspect of the embodiments of the present invention, there is also provided a navigation device of a flying object, including: an acquisition module configured to acquire motion state information of the flying object; an estimation module configured to estimate at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the motion state information; a navigation module configured to modify inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and navigate the flight volume based on the modified inertial navigation data.
According to another aspect of the embodiments of the present invention, there is also provided a navigation system of a flying object, including: a flying body; and a navigation device as described above configured to navigate the flying object.
According to another aspect of embodiments of the present invention, there is also provided a computer-readable storage medium having stored thereon a program which, when executed, causes a processor to perform the method as described above.
In the embodiment of the invention, the motion state information of the flying object is acquired; estimating at least one of system noise and measurement noise based on a pre-constructed integrated navigation model and the motion state information; and based on at least one of the estimated system noise and the measured noise, correcting inertial navigation data of an inertial navigation system, and navigating the flying object based on the corrected inertial navigation data, so that the technical effect of improving navigation real-time performance is realized, and the technical problem of poor real-time performance caused by the fact that a Jacobian matrix needs to be calculated every time during navigation is solved.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the invention and together with the description serve to explain the invention and do not constitute a limitation of the invention. In the drawings:
FIG. 1 is a flow chart of a AEKF-based high-speed spin flying-body integrated navigation method according to an embodiment of the invention;
FIG. 2 is a flow chart of a method of navigating a flight volume according to an embodiment of the invention;
FIG. 3 is a schematic diagram of a navigation device according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a navigation system according to an embodiment of the present invention;
FIG. 5 is a schematic structural diagram of another navigation system according to an embodiment of the present invention;
fig. 6 is a schematic structural diagram of a computer device according to an embodiment of the present invention.
DETAILED DESCRIPTION OF EMBODIMENT (S) OF INVENTION
In order to make the technical solutions of the present invention better understood, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be noted that the terms "first", "second", and the like in the description and claims of the present invention and in the drawings described above are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those illustrated or described herein. Furthermore, the terms "comprises," "comprising," and any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
Example 1
According to an embodiment of the present invention, a flow chart of a high-speed spin flight body integrated navigation method based on an AEKF (adaptive kalman filter algorithm) is provided, as shown in fig. 1, the method includes:
step S101, initializing the system and setting relevant navigation parameters.
And step S102, performing initial alignment, and acquiring the initial speed, position and posture of the flying object.
And step S103, establishing a combined navigation model.
1) State quantity construction
In order to improve the real-time performance of the combined navigation, taylor series expansion is carried out on the nonlinear functions of a system equation and a measurement equation, only linear terms are reserved, and a linear model is obtained.
Different from the traditional EKF (extended Kalman Filter) modeling method, the Jacobian matrix can be avoided to be calculated every time, and the error quantity is directly selected as the state variable. For example, the following error amounts of the system are selected as the state variables: position error Δ P, velocity error Δ v,Attitude error
Figure GDA0003163277490000031
The accelerometer zero offset error delta a and the gyro zero offset error delta omega are established, and a state equation is as follows:
Figure GDA0003163277490000041
delta P in the formula (1) k And Δ P k+1 Position errors at the time k and the time k +1 respectively; Δ V k And Δ V k+1 The speed errors at the time k and the time k +1 respectively;
Figure GDA0003163277490000042
and
Figure GDA0003163277490000043
attitude errors at the time k and the time k +1 respectively; delta a k And Δ a k+1 Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω k And Δ ω k+1 The gyroscope zero offset errors at the time k and the time k +1 respectively; b is 1 、B 2 The proportional coefficients of the accelerometer zero offset error and the gyroscope zero offset error; w k·a 、W k·ω The random system dynamic noises are respectively the errors of an accelerometer and a gyroscope at the moment k, and the mean value and the variance of the random system dynamic noises meet the requirements
Figure GDA0003163277490000044
Is a zero mean white noise sequence; k denotes the time of day, at is the time,
Figure GDA0003163277490000045
for the variation matrix, n denotes a navigation coordinate system and b denotes a carrier coordinate system. St is an antisymmetric matrix.
Writing the above equation as a standard equation of state X k+1 ＝f[X k ，k]+W k The form is as follows:
Figure GDA0003163277490000046
2) Observed quantity construction
Establishing a measurement equation Z k+1 ＝h[X k+1 ，k+1]Wherein the observed quantity is the position and the speed of the carrier, and is specifically represented by formula (3):
Figure GDA0003163277490000047
wherein Z is k+1 Is observed at the time k +1, h is the observation matrix, X k+1 And k is the state vector of the system at the moment of k +1, and k is the current calculation moment. The measured system noise, mean and variance in the formula (3) satisfy
Figure GDA0003163277490000051
Is a zero mean white noise sequence; i is a 3x3 identity matrix.
And step S104, calculating the SINS attitude.
SINS (strapdown inertial navigation system) attitude calculation. After the acceleration and the angular velocity of the carrier are obtained through the accelerometer and the gyroscope, the acceleration and the angular velocity are transformed to a navigation coordinate system from a carrier coordinate system through coordinate transformation. And the current position, speed and attitude information of the carrier is calculated through data obtained by self-contained deviation correction of an accelerometer and a gyroscope.
Step S105, judging whether the GPS information is updated or not, and entering a combined navigation stage.
1) Judging whether the GPS data is updated or not, and entering integrated navigation at the updating moment;
2) If the GPS data is not updated, navigation is carried out by using inertial navigation data of the inertial navigation system, and next GPS data updating is waited;
3) If the GPS data is updated, firstly, the coordinate transformation is carried out on the GPS data so as to ensure that the GPS data and the inertial navigation data are in the same coordinate system.
And step S106, combining navigation stages.
The filtering method adopted in this embodiment is an AEKF (adaptive extended kalman filter) method, and estimates system noise and measurement noise to obtain a dynamically changing noise matrix, thereby improving the accuracy of the integrated navigation. This introduces an adaptation due to the noise-agnostic nature, which has an impact on the algorithm of the EKF.
1) The AEKF algorithm is specifically as follows:
and (3) performing state prediction:
Figure GDA0003163277490000052
Figure GDA0003163277490000053
wherein the content of the first and second substances,
Figure GDA0003163277490000054
for the purpose of a one-step prediction of the state quantity of the system,
Figure GDA0003163277490000055
is the system state quantity at the last moment, phi k.k-1 For the one-step prediction matrix of the state transition,
Figure GDA0003163277490000056
is the mean value of the system noise, k is the current calculation time, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA0003163277490000061
transpose matrix for one-step prediction of state transitions, P k-1 Is the system covariance matrix, f, of the last moment k-1 The array is driven for the system noise,
Figure GDA0003163277490000062
the covariance matrix of the system noise at the previous time instant,
Figure GDA0003163277490000063
is the transpose matrix of the system noise driving matrix, and T is time.
The residual is expressed as:
Figure GDA0003163277490000064
wherein epsilon k Is the residual at time k, Z k Is the observed quantity at time K, H is the observation matrix,
Figure GDA0003163277490000065
for the purpose of a one-step prediction of the state quantity of the system,
Figure GDA0003163277490000066
the mean of the noise is measured for the system.
And (3) carrying out measurement updating:
Figure GDA0003163277490000067
Figure GDA0003163277490000068
P k ＝[I-K k H k ]P k，k-1 (9)
in the above formula, K k To extend the Kalman gain, P k.k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA0003163277490000069
for transposing the observation matrix, H k In order to observe the matrix, the system is,
Figure GDA00031632774900000610
for the system state quantity at the moment k,
Figure GDA00031632774900000611
for one-step prediction of the state quantity of the system,. Epsilon k Is a residual, P k Is the system covariance matrix at time k, I is the identity matrix,
Figure GDA00031632774900000612
a covariance matrix of the noise is measured for the system.
The adaptive estimator is:
Figure GDA00031632774900000613
Figure GDA00031632774900000614
Figure GDA00031632774900000615
Figure GDA00031632774900000616
wherein, among others,
Figure GDA00031632774900000617
respectively a covariance matrix of the measurement noise at the time k and a mean value of the measurement noise;
Figure GDA00031632774900000618
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure GDA00031632774900000619
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA00031632774900000620
in order to be a transpose of the observation matrix,
Figure GDA0003163277490000071
is the mean value of the measured noise at time k-1, d k ＝(1-b)/(1-b k+1 ) (0 < b < 1) is a forgetting factor which acts as a boostThe effect on the new data pair filtering.
And the AEKF algorithm is used for estimating the noise on line, so that the problem that the EKF cannot estimate the noise is solved.
2) Algorithm improvement
The system noise covariance and the measurement noise covariance are adjusted simultaneously by using the AKEF, but in the practical application process, the two can not be estimated simultaneously. Requires a known matrix
Figure GDA0003163277490000072
Or a matrix
Figure GDA0003163277490000073
And then the other one is estimated. If in the covariance matrix of the collaborative noise
Figure GDA0003163277490000074
And measure covariance
Figure GDA0003163277490000075
When the unknown signals are not known, the matrix is updated at the same time, so that filtering divergence is easily caused. Therefore, typically only the measurement noise is estimated.
In this embodiment, the embodiment of the AEKF algorithm is further improved, and a coefficient is added to a measurement noise estimation formula of the adaptive device, specifically:
Figure GDA0003163277490000076
Figure GDA0003163277490000077
wherein the content of the first and second substances,
Figure GDA0003163277490000078
respectively, covariance matrix of measurement noise at time k, mean value of measurement noise, d k ＝(1-b)/(1-b k+1 ) (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA0003163277490000079
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is determined,
Figure GDA00031632774900000710
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A one-step prediction matrix for the covariance of the system,
Figure GDA00031632774900000711
for the transpose of the observation matrix, b is the coefficient, the value range is (0, 1), k represents the time,
Figure GDA00031632774900000712
is the mean value of the measurement noise at time k-1, Z k For systematic measurement, H k In order to observe the matrix, the system,
Figure GDA00031632774900000713
and predicting the state quantity of the system in one step.
By improving the AEKF algorithm, the navigation precision is improved.
3) And correcting inertial navigation data according to the satellite data, updating a parameter state covariance matrix P, a Kalman gain K and a measurement noise R matrix in the AEKF, and navigating the flying object based on the corrected inertial navigation data.
Example 2
According to an embodiment of the present invention, there is provided a flowchart of another navigation method for a flying object, as shown in fig. 2, the method includes:
step S202, obtaining the motion state information of the flying object.
For example, the speed, attitude, position information, etc. of the flying object are acquired by a GPS system, and/or the speed, attitude, position information, etc. of the flying object are acquired by an inertial navigation system.
And step S204, estimating at least one of system noise and measurement noise based on a pre-constructed combined navigation model and the motion state information.
The integrated navigation model is constructed by the following method: constructing a state quantity based on the error quantity of the inertial navigation system; and constructing observed quantity based on the measurement parameters of the GPS system.
In one exemplary embodiment, the state quantities are constructed by the following method: obtaining the error amount from the inertial navigation system, wherein the error amount comprises a position error delta P, a speed error delta v and an attitude error
Figure GDA0003163277490000081
The accelerometer zero offset error delta a and the gyro zero offset error delta omega; based on these error amounts described above, a state equation is established as the state amount. And constructing a measurement equation based on the measurement parameters of the GPS system. The constructed measurement equation and state equation are the combined navigation model.
Step S206, inertial navigation data of the inertial navigation system is corrected based on at least one of the estimated system noise and the measured noise, and the flying object is navigated based on the corrected inertial navigation data.
After the integrated navigation model is constructed, at least one of system noise and measurement noise is estimated based on a pre-constructed integrated navigation model and the motion state information. For example, a state prediction is performed on the state quantity; measuring and updating the observed quantity; estimating at least one of the system noise and the metrology noise with an adaptive estimator based on the predicted state quantities and the updated observations.
Example 3
According to an embodiment of the present disclosure, there is also provided a navigation device for implementing the methods in embodiment 1 and embodiment 2 described above, as shown in fig. 3, the navigation device 300 including:
an obtaining module 32 configured to obtain motion state information of the flying object.
An estimation module 34 configured to estimate at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the motion state information;
a navigation module 36 configured to modify inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and to navigate the flight volume based on the modified inertial navigation data.
In an embodiment of the present disclosure, the navigation device 300 further includes a model building module configured to build a state quantity based on the error quantity of the inertial navigation system, and build an observed quantity based on the measurement parameters of the GPS system to build a combined navigation model.
For example, the error amount is obtained from the inertial navigation system, wherein the error amount comprises a position error Δ P, a velocity error Δ v, and an attitude error
Figure GDA0003163277490000091
The accelerometer zero offset error delta a and the gyro zero offset error delta omega; based on the error amount, the following equation of state is established as the state amount:
Figure GDA0003163277490000092
wherein, Δ P k And Δ P k+1 Position errors at the time k and the time k +1 respectively; Δ V k And Δ V k+1 The speed errors at the time k and the time k +1 respectively;
Figure GDA0003163277490000093
and
Figure GDA0003163277490000094
attitude errors at the time k and the time k +1 respectively; Δ a k And Δ a k+1 Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω k And Δ ω k+1 The gyroscope zero offset errors at the time k and the time k +1 respectively; b is 1 Zero offset error for accelerometer; b is 2 The scale factor is the gyro zero offset error; w is a k·a Random system dynamic noise for accelerometer error at time k; w k·ω As randomness of gyro errorSystem dynamic noise, k denotes time of day, Δ t is time, st is an antisymmetric matrix,
Figure GDA0003163277490000095
for the variation matrix, n denotes a navigation coordinate system and b denotes a carrier coordinate system.
For example, the following measurement equation is constructed based on the measurement parameters of the GPS system as the observed quantity:
Figure GDA0003163277490000096
wherein Z is k+1 Is observed at the time k +1, h is the observation matrix, X k+1 Is the state vector of the system at the moment k +1, I is the identity matrix, Δ P k+1 The position error at the time k + 1; Δ V k+1 The velocity error at time k + 1.
The navigation device 300 further comprises a determining module for determining whether to enter the integrated navigation. For example, whether the data of the GPS system is updated is judged; under the condition of updating the data of the GPS system, entering a state of combined navigation of the GPS and the inertial navigation system; and under the condition that the data of the GPS system is not updated, navigating by using the data of the inertial navigation system, waiting for the updating of the GPS data, and entering a state of combined navigation of the GPS and the inertial navigation system after the updating of the GPS data.
The estimation module 36 of the navigation device 300 further comprises a state prediction unit, a measurement update unit and an adaptive estimation unit, wherein the state prediction unit is configured to perform state prediction on the state quantity; the measurement unit is configured to perform measurement updating on the observed quantity; the adaptive estimation unit is configured to estimate at least one of the system noise and the measurement noise using an adaptive estimator based on the predicted state quantity and the updated observed quantity.
In one exemplary embodiment, the state prediction unit performs state prediction on the state quantity based on the following formula:
Figure GDA0003163277490000101
Figure GDA0003163277490000102
wherein the content of the first and second substances,
Figure GDA0003163277490000103
for the purpose of a one-step prediction of the state quantity of the system,
Figure GDA0003163277490000104
is the system state quantity at the last moment, phi k.k-1 For the one-step prediction matrix of the state transition,
Figure GDA0003163277490000105
is the mean value of the system noise, k is the current calculation time, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA0003163277490000106
transpose matrix for one-step prediction of state transitions, P k-1 Is the covariance matrix of the system at the previous moment, Γ k-1 The array is driven for the system noise,
Figure GDA0003163277490000107
the covariance matrix of the system noise at the previous time instant,
Figure GDA0003163277490000108
is the transpose of the system noise driven matrix.
In an exemplary embodiment, the measurement update unit is configured to perform measurement update on the observed quantity based on the following formula:
Figure GDA0003163277490000109
Figure GDA00031632774900001010
P k ＝[I-K k H k ]P k，k-1
wherein, K k To extend the Kalman gain, P k.k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA0003163277490000111
for transposing the observation matrix, H k In order to observe the matrix, the system,
Figure GDA0003163277490000112
for the system state quantity at the moment k,
Figure GDA0003163277490000113
for one-step prediction of the state quantity of the system,. Epsilon k Is a residual, P k Is the system covariance matrix at time k, I is the identity matrix,
Figure GDA0003163277490000114
a covariance matrix of the noise is measured for the system.
In an exemplary embodiment, the estimation module 34 includes a metrology noise estimation unit and a system noise estimation unit.
The measurement noise estimation unit may estimate the measurement noise based on, for example, the following formula:
Figure GDA0003163277490000115
Figure GDA0003163277490000116
wherein the content of the first and second substances,
Figure GDA0003163277490000117
respectively a covariance matrix of the measurement noise at the moment k and a mean value of the measurement noise; d k ＝(1-b)/(1-b k+1 ) And (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA0003163277490000118
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure GDA0003163277490000119
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA00031632774900001110
in order to be a transpose of the observation matrix,
Figure GDA00031632774900001111
is the average of the measured noise at time k-1.
In other embodiments, the measurement noise estimation unit may estimate the measurement noise based on the following formula, for example:
Figure GDA00031632774900001112
Figure GDA00031632774900001113
wherein the content of the first and second substances,
Figure GDA00031632774900001114
respectively, covariance matrix of measurement noise at time k, mean value of measurement noise, d k ＝(1-b)/(1-b k+1 ) (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA00031632774900001115
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure GDA00031632774900001116
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A one-step prediction matrix for the covariance of the system,
Figure GDA00031632774900001117
for the transposition of the observation matrix, b is a coefficient with a value range of (0, 1), k represents the time,
Figure GDA00031632774900001118
is the mean value of the measurement noise at time k-1, Z k For systematic measurement, H k In order to observe the matrix, the system is,
Figure GDA00031632774900001119
and predicting the state quantity of the system in one step.
The system noise estimation unit may estimate the system noise based on, for example, the following formula:
Figure GDA0003163277490000121
Figure GDA0003163277490000122
wherein the content of the first and second substances,
Figure GDA0003163277490000123
and
Figure GDA0003163277490000124
respectively, covariance matrix of system noise at time k, mean of system noise, d k ＝(1-b)/(1-b k+1 ) And (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA0003163277490000125
is the covariance matrix of the system noise at time K-1, K k For extending the Kalman gain, ε k Which is a residual error, is calculated,
Figure GDA0003163277490000126
is a transposed matrix of the residual error,
Figure GDA0003163277490000127
transposed matrix for extended Kalman gain, P k Is a covariance matrix of the system at time K, F k，k-1 For one-step prediction matrices of system state transitions, P k-1 Is the system covariance matrix at time k-1,
Figure GDA0003163277490000128
a transpose of the one-step prediction matrix for the system state transition,
Figure GDA0003163277490000129
and
Figure GDA00031632774900001210
the mean values of the system noise at time k and at time k-1 respectively,
Figure GDA00031632774900001211
is the system state quantity at time k, F k，k-1 A matrix is predicted for one step of the system state transition,
Figure GDA00031632774900001212
the system state quantity at the moment k-1, k the current calculation moment and T the time.
And finally, correcting inertial navigation data according to the satellite data, updating a parameter state covariance matrix P, a Kalman gain K and a measurement noise R matrix in the AEKF, and navigating the flight object based on the corrected inertial navigation data.
Example 4
There is also provided a server for implementing the above navigation, as shown in fig. 4, the server including a navigation system 400 including the navigation device 300, the inertial sensor 42, and the database 44 as described above according to the embodiment of the present disclosure.
The inertial sensor 42 is carried by a carrier, such as a flying object, and is configured to acquire motion state information of the carrier.
The navigation device 300 is configured to acquire motion state information of the carrier acquired by the inertial sensor 42, estimate at least one of system noise and measurement noise based on a pre-constructed combined navigation model and the motion state information, correct inertial navigation data of the inertial navigation system based on the estimated at least one of the system noise and the measurement noise, and navigate the flight object based on the corrected inertial navigation data.
The inertial sensor 42 and the navigation device 300 may be connected by a network, such as a wireless network constructed using wireless technologies such as WiFi, 4G, 5G, etc.
Optionally, the specific examples in this embodiment may refer to the examples described in embodiment 1, embodiment 2, and embodiment 3, which are not described herein again.
Example 5
According to an embodiment of the present disclosure, there is also provided another server for implementing the above navigation, as shown in fig. 5, which includes a navigation system including an SINS system 52, a GPS system 54, a signal synthesis point 56, and an AEKF filter 58.
The SINS system 52 obtains the acceleration and angular velocity of the flying object through the accelerometer and the gyroscope, and then transforms the acceleration and angular velocity from the carrier coordinate system to the navigation coordinate system through coordinate transformation. And the current position, speed and attitude information of the flying object is calculated through the data obtained by correcting the self-contained deviation of the accelerometer and the gyroscope. The GPS system 54 also acquires position and velocity information of the flying object.
The motion state information collected by the SINS system 52 and the GPS system 54 is input to the signal combining point 56. The signal synthesis point 56 judges whether the GPS data is updated or not, and indicates to enter the integrated navigation at the updating time; if the GPS data is not updated, navigation is carried out by using inertial navigation data of an inertial navigation system, and the next GPS data update is indicated to be waited; if the GPS data is updated, firstly, the coordinate transformation is carried out on the GPS data so as to ensure that the GPS data and the inertial navigation data are in the same coordinate system.
The AEKF filter 58 is configured to estimate at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the motion state information, and to modify inertial navigation data of the inertial navigation system based on the estimated at least one of system noise and metrology noise, and to navigate the flight volume based on the modified inertial navigation data.
In the present embodiment, the processor 56 and the AEKF filter 58 correspond to the navigation device in embodiment 4.
Example 6
Embodiments of the present disclosure also provide a storage medium. Alternatively, in the present embodiment, the storage medium may implement the methods described in embodiments 1 and 2 above.
Optionally, in this embodiment, the storage medium may be located in at least one of a plurality of network devices in a network of the inertial navigation system.
Optionally, in this embodiment, the storage medium may include, but is not limited to: a U-disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a removable hard disk, a magnetic or optical disk, and other various media capable of storing program codes.
Alternatively, in the present embodiment, the processor executes the methods in embodiments 1 and 2 described above according to the program code stored in the storage medium.
Optionally, for a specific example in this embodiment, reference may be made to the examples described in embodiment 1 and embodiment 2, and this embodiment is not described herein again.
Example 7
Referring now to FIG. 6, shown is a block diagram of a computer device 800 suitable for use in implementing embodiments of the present disclosure. The computer device shown in fig. 6 is only an example and should not bring any limitation to the function and scope of use of the embodiments of the present disclosure.
As shown in fig. 6, the computer apparatus 800 includes a Central Processing Unit (CPU) 801 which can perform various appropriate actions and processes in accordance with a program stored in a Read Only Memory (ROM) 802 or a program loaded from a storage section 808 into a Random Access Memory (RAM) 803. In the RAM803, various programs and data necessary for the operation of the apparatus 800 are also stored. The CPU801, ROM802, and RAM803 are connected to each other via a bus 804. An input/output (I/O) interface 805 is also connected to bus 804.
The following components are connected to the I/O interface 805: an input portion 806 including a keyboard, a mouse, and the like; an output section 807 including components such as a Cathode Ray Tube (CRT), a Liquid Crystal Display (LCD), and a speaker; a storage portion 808 including a hard disk and the like; and a communication section 809 including a network interface card such as a LAN card, a modem, or the like. The communication section 809 performs communication processing via a network such as the internet. A drive 810 is also connected to the I/O interface 805 as necessary. A removable medium 811 such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, or the like is mounted on the drive 810 as necessary, so that the computer program read out therefrom is mounted on the storage section 808 as necessary.
According to an embodiment of the present disclosure, the processes described above with reference to the flowcharts may be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program embodied on a computer readable medium, the computer program comprising program code for performing the method illustrated by the flow chart. In such an embodiment, the computer program may be downloaded and installed from a network via the communication section 809 and/or installed from the removable medium 811. The computer program performs the above-described functions defined in the method of the present disclosure when executed by the Central Processing Unit (CPU) 801. It should be noted that the computer storage media of the present disclosure can be computer readable signal media or computer readable storage media or any combination of the two. A computer readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples of the computer readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the present disclosure, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In contrast, in the present disclosure, a computer-readable signal medium may include a propagated data signal with computer-readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A computer readable signal medium may be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, fiber optic cable, RF, etc., or any suitable combination of the foregoing.
The flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
The modules or units described in the embodiments of the present disclosure may be implemented by software or hardware. The modules or units described may also be provided in a processor, the names of which in some cases do not constitute a limitation of the module or unit itself.
The method provides a new model construction, and solves the problem of poor real-time performance caused by the fact that the Jacobian matrix needs to be calculated every time in the traditional EKF model construction; and the AEKF algorithm is used for estimating the noise on line, so that the problem that the EKF cannot estimate the noise is solved, and the AEKF algorithm is improved, so that the navigation precision is improved, and the beneficial effects of real-time performance enhancement, navigation system error reduction and precision improvement are achieved.
The disclosed embodiments may also be configured to:
(1) A method of navigating a flying object, comprising:
acquiring motion state information of the flying object;
estimating at least one of system noise and measurement noise based on a pre-constructed integrated navigation model and the motion state information;
modifying inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and navigating the flight volume based on the modified inertial navigation data.
(2) The navigation method of item 1, wherein the integrated navigation model is constructed by:
constructing a state quantity based on an error quantity of the inertial navigation system;
and constructing observed quantity based on the measurement parameters of the GPS system.
(3) The navigation method according to item 2, wherein constructing the state quantity based on the error quantity of the inertial navigation system includes:
obtaining the error amount from the inertial navigation system, wherein the error amount comprises a position error delta P, a speed error delta v and an attitude error
Figure GDA0003163277490000161
The accelerometer zero offset error delta a and the gyro zero offset error delta omega;
based on the error amount, the following state equation is established as the state amount:
Figure GDA0003163277490000171
ΔP k and Δ P k+1 Position errors at the time k and the time k +1 respectively; Δ V k And Δ V k+1 The speed errors at the time k and the time k +1 respectively;
Figure GDA0003163277490000172
and
Figure GDA0003163277490000173
attitude errors at the time k and the time k +1 respectively; Δ a k And Δ a k+1 Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω k And Δ ω k+1 The gyroscope zero offset errors at the time k and the time k +1 respectively; b 1 Zero offset error for accelerometer; b is 2 The scale factor is the gyro zero offset error; w k·a Random system dynamic noise for accelerometer error at time k; w k·ω Random system dynamic noise, which is the gyro error, k represents the time of day, Δ t is the time, st is the antisymmetric matrix,
Figure GDA0003163277490000175
for the variation matrix, n denotes a navigation coordinate system and b denotes a carrier coordinate system.
(4) The navigation method of item 2, wherein constructing the observations based on the metrology parameters of the GPS system comprises: constructing the following measurement equation based on the measurement parameters of the GPS system as the observed quantity:
Figure GDA0003163277490000174
wherein Z is k+1 Is observed at the time k +1, h is an observation matrix, X k+1 Is the state vector of the system at the time k +1, I is the identity matrix, Δ P k+1 Is the position error at time k + 1; Δ V k+1 The velocity error at time k + 1.
(5) The navigation method of any of clauses 1-4, wherein prior to modifying inertial navigation data of an inertial navigation system, the method further comprises:
judging whether the data of the GPS system is updated;
under the condition of updating the data of the GPS system, entering a state of integrated navigation of the GPS and the inertial navigation system;
and under the condition that the data of the GPS system is not updated, navigating by using the data of the inertial navigation system, waiting for the updating of the GPS data, and entering a state of combined navigation of the GPS and the inertial navigation system after the updating of the GPS data.
(6) The navigation method according to any one of items 2 to 5, wherein estimating at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the kinematic state information comprises:
performing state prediction on the state quantity;
measuring and updating the observed quantity;
estimating at least one of the system noise and the metrology noise with an adaptive estimator based on the predicted state quantities and the updated observations.
(7) The navigation method according to item 6, wherein the state predicting the state quantity includes: performing state prediction on the state quantity based on the following formula:
Figure GDA0003163277490000181
Figure GDA0003163277490000182
wherein the content of the first and second substances,
Figure GDA0003163277490000183
for the purpose of a one-step prediction of the state quantity of the system,
Figure GDA0003163277490000184
is the system state quantity at the last moment, phi k.k-1 For the one-step prediction matrix of the state transition,
Figure GDA0003163277490000185
is the mean value of the system noise, k is the current calculation time, P k，k-1 A one-step prediction matrix for the covariance of the system,
Figure GDA0003163277490000186
transpose matrix for one-step prediction of state transitions, P k-1 Is the covariance matrix of the system at the previous moment, Γ k-1 The array is driven for the system noise,
Figure GDA0003163277490000187
the covariance matrix of the system noise at the previous time instant,
Figure GDA0003163277490000188
is the transpose of the system noise driver matrix.
(8) The navigation method of item 7, wherein measurably updating the observations comprises: and updating the measurement of the observed quantity based on the following formula:
Figure GDA0003163277490000189
Figure GDA00031632774900001810
P k ＝[I-K k H k ]P k，k-1
wherein, K k To extend the Kalman gain, P k.k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA0003163277490000191
for transposing the observation matrix, H k In order to observe the matrix, the system,
Figure GDA0003163277490000192
for the system state quantity at the moment k,
Figure GDA0003163277490000193
for one-step prediction of the state quantities of the system, epsilon k Is a residual, P k Is the system covariance matrix at time k, I is the identity matrix,
Figure GDA0003163277490000194
a covariance matrix of the noise is measured for the system.
(9) The navigation method of any of items 6 to 8, wherein the metrology noise is estimated based on the following formula:
Figure GDA0003163277490000195
Figure GDA0003163277490000196
wherein the content of the first and second substances,
Figure GDA0003163277490000197
respectively a covariance matrix of the measurement noise at the moment k and a mean value of the measurement noise; d is a radical of k ＝(1-b)/(1-b k+1 ) And (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA0003163277490000198
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure GDA0003163277490000199
is a transposition of the residual errorMatrix, T being the transposed sign of the matrix, H k To observe the matrix, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure GDA00031632774900001910
in order to be a transpose of the observation matrix,
Figure GDA00031632774900001911
is the average of the measured noise at time k-1.
(10) The navigation method of any one of items 6 to 8, wherein the metric noise is estimated based on the following formula:
Figure GDA00031632774900001912
Figure GDA00031632774900001913
wherein the content of the first and second substances,
Figure GDA00031632774900001914
the covariance matrix of the measurement noise at time k, the mean of the measurement noise, d k ＝(1-b)/(1-b k+1 ) And (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA00031632774900001915
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is determined,
Figure GDA00031632774900001916
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A one-step prediction matrix for the covariance of the system,
Figure GDA00031632774900001917
for the transposition of the observation matrix, b is a coefficient with a value range of (0, 1), k represents the time,
Figure GDA00031632774900001918
is the mean value of the measurement noise at time k-1, Z k For systematic measurement, H k In order to observe the matrix, the system,
Figure GDA00031632774900001919
and predicting the state quantity of the system in one step.
(11) The navigation method of any of items 6 to 8, wherein the system noise is estimated based on the following formula:
Figure GDA0003163277490000201
Figure GDA0003163277490000202
wherein, the first and the second end of the pipe are connected with each other,
Figure GDA0003163277490000203
and
Figure GDA0003163277490000204
respectively, covariance matrix of system noise at time k, mean of system noise, d k ＝(1-b)/(1-b k+1 ) (b is more than 0 and less than 1) is a forgetting factor,
Figure GDA0003163277490000205
is the covariance matrix of the system noise at time K-1, K k For extending the Kalman gain, ε k Which is a residual error, is determined,
Figure GDA0003163277490000206
is a transposed matrix of the residual error,
Figure GDA0003163277490000207
transposed matrix, P, for extended Kalman gain k Is a covariance matrix of the system at time K, F k，k-1 One-step prediction for system state transitionsMatrix, P k-1 Is the system covariance matrix at time k-1,
Figure GDA0003163277490000208
a transpose of the one-step prediction matrix for the system state transition,
Figure GDA0003163277490000209
and
Figure GDA00031632774900002010
the mean values of the system noise at time k and at time k-1 respectively,
Figure GDA00031632774900002011
is the system state quantity at time k, F k，k-1 A one-step prediction matrix for the system state transition,
Figure GDA00031632774900002012
the system state quantity at the moment k-1, k the current calculation moment and T the time.
(12) A navigation device of a flying object, comprising:
an acquisition module configured to acquire motion state information of the flying object;
an estimation module configured to estimate at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the motion state information;
a navigation module configured to modify inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and to navigate the flight volume based on the modified inertial navigation data
(13) A navigation system for a flying object, comprising:
a flying body; and
the navigation device of item 12, configured to navigate the flying object.
(14) A computer readable storage medium having stored thereon a program which, when executed, causes a processor to perform the method of any of items 1 to 11.
The above-mentioned serial numbers of the embodiments of the present disclosure are for description only and do not represent the merits of the embodiments.
The integrated unit in the above embodiments, if implemented in the form of a software functional unit and sold or used as a separate product, may be stored in the above computer-readable storage medium. Based on such understanding, the technical solutions of the present disclosure may be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for causing one or more computer devices (which may be personal computers, servers, network devices, or the like) to execute all or part of the steps of the methods according to the embodiments of the present disclosure.
In the above embodiments of the present disclosure, the description of each embodiment is focused on, and for parts which are not described in detail in a certain embodiment, reference may be made to the description of other embodiments.
In the several embodiments provided in the present application, it should be understood that the disclosed client may be implemented in other ways. The above-described apparatus embodiments are merely illustrative, and for example, the division of the units is merely a logical division, and other divisions may be realized in practice, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, units or modules, and may be in an electrical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment.
In addition, functional units in the embodiments of the present disclosure may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit may be implemented in the form of hardware, or may also be implemented in the form of a software functional unit.
The foregoing is illustrative of the preferred embodiments of the present disclosure, and it will be appreciated by those skilled in the art that various modifications and adaptations can be made without departing from the principles of the disclosure, and such modifications and adaptations are intended to be within the scope of the disclosure.

Claims (8)

1. A method of navigating a flying object, comprising:
acquiring the motion state information of the flying object;
estimating at least one of system noise and measurement noise based on a pre-constructed integrated navigation model and the motion state information;
modifying inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measurement noise, and navigating the flying object based on the modified inertial navigation data;
wherein the integrated navigation model is constructed by the following method: constructing a state quantity based on the error quantity of the inertial navigation system, and establishing a state equation; constructing observed quantity based on measurement parameters of a GPS system, and establishing a measurement equation;
the method comprises the following steps of constructing a state quantity based on an error quantity of the inertial navigation system, and establishing a state equation, wherein the method comprises the following steps: obtaining the error amount from the inertial navigation system, wherein the error amount comprises a position error delta P, a speed error delta v and an attitude error
Figure FDA0003760094860000011
The accelerometer zero offset error delta a and the gyro zero offset error delta omega; selecting the error quantity as the state quantity, and establishing the following state equation:
Figure FDA0003760094860000012
ΔP k and Δ P k+1 Position errors at the time k and the time k +1 respectively; Δ V k And Δ V k+1 The speed errors at the time k and the time k +1 respectively;
Figure FDA0003760094860000013
and
Figure FDA0003760094860000014
attitude errors at the time k and the time k +1 respectively; Δ a k And Δ a k+1 Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω k And Δ ω k+1 The gyroscope zero offset errors at the time k and the time k +1 respectively; b 1 A proportionality coefficient for accelerometer zero offset error; b is 2 A proportionality coefficient of a gyro zero offset error; w k·a Random system dynamic noise for accelerometer error at time k; w is a group of k·ω Random system dynamic noise which is gyro error at the moment k, k represents the moment, deltat is time, st is an antisymmetric matrix,
Figure FDA0003760094860000021
n represents a navigation coordinate system and b represents a carrier coordinate system for the change matrix;
wherein, an AEKF algorithm is adopted to estimate the measurement noise based on the following formula:
Figure FDA0003760094860000022
Figure FDA0003760094860000023
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003760094860000024
respectively, covariance matrix of measurement noise at time k, mean value of measurement noise, d k ＝(1-b)/(1-b k+1 ) In order to be a forgetting factor,
Figure FDA0003760094860000025
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure FDA0003760094860000026
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure FDA0003760094860000027
for the transpose of the observation matrix, b is the coefficient, the value range is (0, 1), k represents the time,
Figure FDA0003760094860000028
is the mean value of the measurement noise at time k-1, Z k For systematic measurement, H k In order to observe the matrix, the system is,
Figure FDA0003760094860000029
is a one-step prediction of the system state quantity.
2. The navigation method of claim 1, wherein constructing the observations based on the metrology parameters of the GPS system comprises: the following measurement equation is constructed based on the measurement parameters of the GPS system:
Figure FDA00037600948600000210
wherein, Z k+1 Is observed at the time k +1, h is the observation matrix, X k+1 Is the state vector of the system at time k +1, l is the identity matrix, Δ P k+1 Position error at time k +1；ΔV k+1 The velocity error at time k + 1.
3. The navigation method of any one of claims 1-2, wherein prior to modifying inertial navigation data of the inertial navigation system, the method further comprises:
judging whether the data of the GPS system is updated;
under the condition of updating the data of the GPS system, entering a state of integrated navigation of the GPS and the inertial navigation system;
and under the condition that the data of the GPS system is not updated, navigating by using the data of the inertial navigation system, waiting for the updating of the GPS data, and entering a state of combined navigation of the GPS and the inertial navigation system after the updating of the GPS data.
4. The navigation method of claim 3, wherein estimating at least one of system noise and metrology noise based on a pre-constructed combined navigation model and the motion state information comprises:
performing state prediction on the state quantity;
measuring and updating the observed quantity;
estimating at least one of the system noise and the metrology noise with an adaptive estimator based on the predicted state quantities and the updated observations.
5. The navigation method of claim 4, wherein state predicting the state quantity comprises: performing state prediction on the state quantity based on the following formula:
Figure FDA0003760094860000031
Figure FDA0003760094860000032
wherein the content of the first and second substances,
Figure FDA0003760094860000033
for the purpose of a one-step prediction of the state quantity of the system,
Figure FDA0003760094860000034
is the system state quantity at the last moment, phi k，k-1 For the one-step prediction matrix of the state transition,
Figure FDA0003760094860000035
is the mean value of the system noise, k is the current calculation time, P k，k-1 A one-step prediction matrix for the covariance of the system,
Figure FDA0003760094860000036
transpose of one-step prediction matrix for state transition, P k-1 Is the system covariance matrix of the last moment, Γ k-1 The array is driven for the system noise,
Figure FDA0003760094860000037
the covariance matrix of the system noise at the previous time instant,
Figure FDA0003760094860000038
is the transpose of the system noise driver matrix.
6. A navigation device of a flying object, comprising:
an acquisition module configured to acquire motion state information of the flying object;
an estimation module configured to estimate at least one of system noise and metrology noise based on a pre-constructed integrated navigation model and the motion state information;
a navigation module configured to modify inertial navigation data of an inertial navigation system based on at least one of the estimated system noise and the measured noise, and to navigate the flight volume based on the modified inertial navigation data;
wherein the integrated navigation model is constructed by the following method: constructing a state quantity based on the error quantity of the inertial navigation system, and establishing a state equation; constructing observed quantity based on measurement parameters of a GPS system, and establishing a measurement equation;
constructing a state quantity based on the error quantity of the inertial navigation system, and establishing a state equation, wherein the method comprises the following steps: obtaining the error amount from the inertial navigation system, wherein the error amount comprises a position error delta P, a speed error delta v and an attitude error
Figure FDA0003760094860000041
The accelerometer zero offset error delta a and the gyro zero offset error delta omega; selecting the error quantity as the state quantity, and establishing the following state equation:
Figure FDA0003760094860000042
ΔP k and Δ P k+1 Position errors at the time k and the time k +1 respectively; Δ V k And Δ V k+1 The speed errors at the time k and the time k +1 respectively;
Figure FDA0003760094860000043
and
Figure FDA0003760094860000044
attitude errors at the time k and the time k +1 respectively; Δ a k And Δ a k+1 Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω k And Δ ω k+1 The gyroscope zero offset errors at the time k and the time k +1 respectively; b is 1 A proportionality coefficient for accelerometer zero offset error; b is 2 The scale factor is the gyro zero offset error; w k·a Random system dynamic noise for accelerometer error at time k; w k·ω Random system dynamic noise that is the gyro error at time k, k representing time, Δ t time, st the antisymmetric matrix,
Figure FDA0003760094860000045
n represents a navigation coordinate system and b represents a carrier coordinate system for the change matrix;
the AEKF algorithm is adopted, and the measurement noise is estimated on the basis of the following formula:
Figure FDA0003760094860000051
Figure FDA0003760094860000052
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003760094860000053
respectively, covariance matrix of measurement noise at time k, mean value of measurement noise, d k ＝(1-b)/(1-b k+1 ) In order to be a forgetting factor,
Figure FDA0003760094860000054
covariance matrix of measurement noise at time k-1, ε k Which is a residual error, is calculated,
Figure FDA0003760094860000055
is the transposed matrix of the residual, T is the matrix transposed sign, H k To observe the matrix, P k，k-1 A matrix is predicted for one step of the covariance of the system,
Figure FDA0003760094860000056
for the transpose of the observation matrix, b is the coefficient, the value range is (0, 1), k represents the time,
Figure FDA0003760094860000057
is the mean value of the measurement noise at time k-1, Z k For systematic measurement, H k In order to observe the matrix, the system,
Figure FDA0003760094860000058
is a one-step prediction of the system state quantity.
7. A navigation system for a flying object, comprising:
a flying body; and
the navigation device of claim 6, configured to navigate the flying object.
8. A computer-readable storage medium, on which a program is stored which, when executed, causes a processor to carry out the method of any one of claims 1 to 5.
CN202110686092.3A 2021-06-21 2021-06-21 Navigation method, device and system for flying object Active CN113432612B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110686092.3A CN113432612B (en) 2021-06-21 2021-06-21 Navigation method, device and system for flying object

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110686092.3A CN113432612B (en) 2021-06-21 2021-06-21 Navigation method, device and system for flying object

Publications (2)

Publication Number Publication Date
CN113432612A CN113432612A (en) 2021-09-24
CN113432612B true CN113432612B (en) 2022-10-14

Family

ID=77756880

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110686092.3A Active CN113432612B (en) 2021-06-21 2021-06-21 Navigation method, device and system for flying object

Country Status (1)

Country Link
CN (1) CN113432612B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111797B (en) * 2021-11-30 2024-02-20 北京信息科技大学 Kalman filter, IP core and navigation chip based on FPGA
CN114111773B (en) * 2021-11-30 2023-11-17 北京信息科技大学 Combined navigation method, device, system and storage medium

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6417802B1 (en) * 2000-04-26 2002-07-09 Litton Systems, Inc. Integrated inertial/GPS navigation system
CN105043385B (en) * 2015-06-05 2018-10-26 北京信息科技大学 A kind of method for adaptive kalman filtering of pedestrian's Camera calibration
CN106950586A (en) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN107621264B (en) * 2017-09-30 2021-01-19 华南理工大学 Self-adaptive Kalman filtering method of vehicle-mounted micro-inertia/satellite integrated navigation system
CN110926465A (en) * 2019-12-11 2020-03-27 哈尔滨工程大学 MEMS/GPS loose combination navigation method

Also Published As

Publication number Publication date
CN113432612A (en) 2021-09-24

Similar Documents

Publication Publication Date Title
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
CN113432612B (en) Navigation method, device and system for flying object
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
JP4789216B2 (en) Improved GPS cumulative delta distance processing method for navigation applications
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
CN110887480B (en) Flight attitude estimation method and system based on MEMS sensor
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN111780756A (en) Vehicle dead reckoning method, device, equipment and storage medium
CN112683269B (en) MARG attitude calculation method with motion acceleration compensation
CN110595434B (en) Quaternion fusion attitude estimation method based on MEMS sensor
CN113960622A (en) Real-time positioning method and device fusing laser radar and IMU sensor information
JP2002323322A (en) Device and method for estimating attitude using inertial measurement device and program
CN111832690A (en) Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN111044082A (en) Gyro error parameter on-orbit rapid calibration method based on star sensor assistance
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
CN109459769B (en) Autonomous positioning method and system
Kim et al. Drift error analysis caused by RLG dither axis bending
CN115727871A (en) Track quality detection method and device, electronic equipment and storage medium
CN115667845A (en) Navigation assistance method for a mobile carrier
CN116539029B (en) Base positioning method and device of underwater movable base, storage medium and equipment
CN114279426B (en) Six-axis optimized magnetometer online calibration method
CN113686334B (en) Method for improving on-orbit combined filtering precision of star sensor and gyroscope
CN113188566B (en) Airborne distributed POS data fusion method
CN115164886B (en) Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation 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