CN111156986A - Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF - Google Patents

Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF Download PDF

Info

Publication number
CN111156986A
CN111156986A CN202010030024.7A CN202010030024A CN111156986A CN 111156986 A CN111156986 A CN 111156986A CN 202010030024 A CN202010030024 A CN 202010030024A CN 111156986 A CN111156986 A CN 111156986A
Authority
CN
China
Prior art keywords
spacecraft
speed
ukf
navigation
model
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
CN202010030024.7A
Other languages
Chinese (zh)
Other versions
CN111156986B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202010030024.7A priority Critical patent/CN111156986B/en
Publication of CN111156986A publication Critical patent/CN111156986A/en
Application granted granted Critical
Publication of CN111156986B publication Critical patent/CN111156986B/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/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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

The invention discloses a spectrum redshift autonomous integrated navigation method based on robust adaptive UKF, which is characterized in that a first speed is calculated by ephemeris information of a solar system celestial body, attitude information of a spacecraft measured by a star sensor and a spectrum redshift value measured in a space where the spacecraft is located; acquiring a second speed of the spacecraft through an inertial navigation system, and generating a speed difference; measuring a first horizontal position and a second horizontal position of the spacecraft through an inertial navigation system and a geomagnetic positioning system respectively, and generating a horizontal position difference; establishing a measurement model of the spectrum redshift/inertial navigation/geomagnetic integrated navigation system; carrying out optimal estimation through an interactive multi-model robust adaptive UKF to obtain an error estimation value; correcting navigation parameters of an inertial navigation system to obtain speed information, position information and attitude information of the spacecraft; the method can effectively reduce the navigation error caused by the non-linear characteristic and the uncertainty of the system model, and is suitable for the requirements of the spacecraft on the autonomy and high precision of the navigation system.

Description

Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF
[ technical field ] A method for producing a semiconductor device
The invention belongs to the technical field of navigation, guidance and control, and particularly relates to a spectrum red shift autonomous integrated navigation method based on interactive multi-model robust adaptive UKF.
[ background of the invention ]
The development of the spacecraft puts higher and higher requirements on the aspects of autonomy, positioning accuracy, reliability and the like of a navigation system, and a single navigation system is difficult to meet the requirements due to inherent defects of the single navigation system. The combined navigation technology is characterized in that two or more than two single navigation systems are combined to form a brand new navigation system, and aims to give full play to the advantages of the single navigation systems, so that the advantages of the navigation systems are complemented, the positioning accuracy of the navigation systems is improved, and the requirements of users on the performance of the navigation systems are better met.
Because the Inertial Navigation System (INS) has the advantages of comprehensive navigation information, simple structure, small volume, light weight, low cost, high reliability and the like, the current home and abroad common integrated navigation system takes the INS as a main navigation system and is assisted by subsystems such as satellite navigation, atmospheric data, Synthetic Aperture Radar (SAR) and the like to form the integrated navigation system.
However, with the continuous development of spacecraft application technologies, there are mature integrated navigation systems, such as an INS/GPS, an INS/atmospheric data, an INS/GNSS/CNS, and other integrated navigation systems. The traditional combined navigation method depends on frequent spacecraft-ground station communication and continuous manual operation, and when the communication between a spacecraft and a ground station is interrupted, navigation data can be lost, so that the navigation accuracy is reduced.
In addition, for the convenience of calculation, most of these navigation methods adopt a linear method to process the nonlinear characteristics in the navigation process, which results in the reduction of navigation accuracy, and when the system has model uncertainty such as abrupt change of model parameters, transient interference, unknown time variation of noise statistics, unknown drift, etc., the navigation accuracy of the system is seriously affected. The adaptive filtering is an important approach for solving the above method, however, the existing adaptive filtering has the defects of poor real-time performance, poor convergence, manual parameter selection by experience and the like. Therefore, the traditional combined navigation method cannot meet the increasingly high requirements of the spacecraft on the navigation system.
[ summary of the invention ]
The invention aims to provide a spacecraft spectrum red shift autonomous integrated navigation method based on robust adaptive UKF (unscented Kalman filter), so as to solve the problem of low navigation accuracy caused by the fact that the traditional navigation method is interrupted in ground communication and a linear method is used for processing nonlinear characteristics of a navigation system.
The invention adopts the following technical scheme: a spectrum red shift autonomous combined navigation method based on robust adaptive UKF comprises the following steps:
solving a first speed through ephemeris information of a solar system celestial body, attitude information of a spacecraft measured by a star sensor and a spectrum red shift value measured in a space where the spacecraft is located; the first speed is the speed of the spacecraft in a navigation coordinate system after coordinate transformation;
acquiring a second speed of the spacecraft through an inertial navigation system, and generating a speed difference according to the first speed and the second speed; measuring a first horizontal position and a second horizontal position of the spacecraft through an inertial navigation system and a geomagnetic positioning system respectively, and generating a horizontal position difference according to the first horizontal position and the second horizontal position;
establishing a measurement model of the spectrum redshift/inertial navigation/geomagnetic integrated navigation system according to the speed difference and the horizontal position difference;
optimally estimating a measurement model and a pre-constructed nonlinear error state model through an interactive multi-model robust adaptive UKF to obtain a navigation error estimation value;
and correcting the navigation parameters of the inertial navigation system through the error estimation value to obtain the speed information, the position information and the attitude information of the spacecraft.
Further, the measurement model specifically includes:
z(t)＝H(t)x(t)+v(t)，
wherein the content of the first and second substances,
Figure BDA0002363965150000031
vE、vNand vURespectively representing the east speed, the north speed and the sky speed of the spacecraft obtained by the inertial navigation system; v. ofSE、vSNAnd vSURepresenting the east speed, the north speed and the sky speed of the spacecraft obtained by the spectrum red shift navigation system; l and lambda are latitude and longitude information of the spacecraft obtained by the inertial navigation system respectively; l isDAnd λDRespectively obtaining latitude and longitude information of the spacecraft for a geomagnetic positioning system; h (t) ([ 0 ]5×4I5×505×6]5×15，
Figure BDA0002363965150000032
Wherein v is1(t) Gaussian white noise included in the velocity information of the spectral redshift measurement, v2(t) is Gaussian white noise contained in the horizontal position information measured by the geomagnetic positioning system.
Further, the optimal estimation of the measurement model and the pre-constructed nonlinear error state model through the interactive multi-model robust adaptive UKF comprises the following steps:
constructing a self-adaptive fading UKF sub-filter and an anti-difference UKF sub-filter;
the state prediction covariance matrix in the adaptive fading UKF sub-filter is as follows:
Figure BDA0002363965150000033
wherein λ iskFor adaptive fading factor, Pk/k-1Predicting a covariance matrix for the state in the UKF;
the innovation vector covariance matrix in the robust UKF sub-filter is:
Figure BDA0002363965150000034
wherein s iskIs used as an anti-difference factor,
Figure BDA0002363965150000035
Figure BDA0002363965150000036
by measuring model and notThe linear error state model is used as a system model, the speed difference and the horizontal position difference are used as measurement input values, meanwhile, the input values are filtered through a self-adaptive fading UKF sub-filter and an anti-difference UKF sub-filter, and corresponding state estimation is obtained respectively
Figure BDA0002363965150000037
And its error covariance matrix Pj,k；
Respectively state estimation
Figure BDA0002363965150000041
And its error covariance matrix Pj,kAnd giving weight, and carrying out weighted average to obtain a navigation error estimation value.
Further, the weight is calculated by the following formula:
Figure BDA0002363965150000042
wherein, muj,kIs the model probability, Λ, of the sub-filter j at time kj,kIs the likelihood function of the sub-filter j at time k, cj,k-1Is the normalization factor at time k-1.
Further, the construction method of the nonlinear error state model comprises the following steps:
selecting an east-north-sky (E, N, U) geographic coordinate system as a navigation coordinate system N, and establishing a nonlinear error state model according to an error equation of an inertial navigation system and an inertial device:
Figure BDA0002363965150000043
wherein, x (t) is system state vector, f (x (t)) is nonlinear state function of system, G (t) is noise driving matrix of system, w (t) is noise of system process;
Figure BDA0002363965150000044
q0、q1、q2、q3is an attitude error quaternion; delta VE、δVN、δVUThe speed errors in the east, north and sky directions; δ L, δ λ, δ h are latitude, longitude and altitude errors;
Figure BDA0002363965150000045
representing a random constant drift of the gyro;
Figure BDA0002363965150000046
is the random constant error of the accelerometer;
further, the noise driving matrix g (t) is specifically:
Figure BDA0002363965150000047
wherein the content of the first and second substances,
Figure BDA0002363965150000051
Figure BDA0002363965150000052
representing a transformation matrix between the body coordinate system b and the platform coordinate system c, 03×3All 0 matrices of 3 x 3 order are represented.
Further, the first speed calculation method is:
establishing the radial velocity v of the spacecraft relative to the reference celestial light sourcesr1,vr2,vr3Velocity vector v in inertial system with spacecraftpThe relation of (1):
Figure BDA0002363965150000053
in the formula, v1,v2,v3For each reference celestial body's velocity vector in the inertial frame, u1,u2,u3Unit vector v representing the vector of the position of each reference celestial body light source pointing to the spacecraft in the inertial coordinate systemr1,vr2,vr3The radial speed of the spacecraft relative to each reference celestial body light source;
solving velocity vector square of spacecraft in inertial coordinate systemHas a stroke vp＝f(v1,v2,v3,u1,u2,u3,vr1,vr2,vr3) And obtaining the first speed of the spacecraft through coordinate transformation.
The invention has the beneficial effects that: the invention can effectively reduce the navigation error caused by nonlinear characteristics through the interactive multi-model robust adaptive UKF, weakens the influence of uncertainty of the state model and abnormal measurement information on the state estimation at the current moment through improving the classic UKF filter, and is suitable for the requirements of the spacecraft on the autonomy and high precision of the navigation system.
[ description of the drawings ]
FIG. 1 is a flow chart of an embodiment of the present invention;
FIG. 2 is a schematic diagram of spectral redshift autonomous navigation in an embodiment of the present invention;
FIG. 3 is a flowchart of the interactive multi-model robust adaptive UKF in the embodiment of the present invention.
[ detailed description ] embodiments
The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
The embodiment of the invention discloses a spectrum red shift autonomous integrated navigation method based on robust adaptive UKF (unscented Kalman filter), as shown in FIG. 1, comprising the following steps:
solving a first speed through ephemeris information of a solar system celestial body, attitude information of a spacecraft measured by a star sensor and a spectrum red shift value measured in a space where the spacecraft is located; the first speed is the speed of the spacecraft in a navigation coordinate system after coordinate transformation; acquiring a second speed of the spacecraft through an inertial navigation system, and generating a speed difference according to the first speed and the second speed; measuring a first horizontal position and a second horizontal position of the spacecraft through an inertial navigation system and a geomagnetic positioning system respectively, and generating a horizontal position difference according to the first horizontal position and the second horizontal position; establishing a measurement model of the spectrum redshift/inertial navigation/geomagnetic integrated navigation system according to the speed difference and the horizontal position difference; carrying out optimal estimation on a measurement model and a pre-constructed nonlinear error state model through an interactive multi-model robust adaptive UKF to obtain an error estimation value; and correcting the navigation parameters of the inertial navigation system through the error estimation value to obtain the speed information, the position information and the attitude information of the spacecraft.
The invention can effectively reduce the navigation error caused by nonlinear characteristics through the interactive multi-model robust adaptive UKF, weakens the influence of uncertainty of the state model and abnormal measurement information on the state estimation at the current moment through improving the classic UKF filter, and is suitable for the requirements of the spacecraft on the autonomy and high precision of the navigation system.
In the embodiment, at least 3 non-collinear celestial body light sources are selected, and the geometric relationship of the celestial body is constructed by utilizing the position information of the selected celestial body light sources, so that a velocity vector equation is established. Meanwhile, in the embodiment, the attitude information of the spacecraft at the current moment can be acquired through the star sensor carried by the spacecraft.
The celestial body spectrum consists of continuous spectrum, spectral line and various noises, and the speed information of the celestial body relative to the spacecraft is stored. When the spacecraft flies away from the earth at a high speed, the spectrum is observed from the spacecraft, the observed spectral line is longer than the stationary spectral line, and the phenomenon that the spectral line moves a certain distance towards the red end is the red shift phenomenon.
Defining the spectral red shift value and denoted by z, the spectral red shift value is calculated by:
Figure BDA0002363965150000071
wherein λ represents an observed value of wavelength, λ0Representing the actual wavelength of the spectral line, f being the light source frequency received by the spacecraft; f. of0Is the natural frequency of the celestial spectrum.
As shown in fig. 2, which is a spectrum redshift autonomous navigation schematic diagram, assuming that the spacecraft can detect the spectrums of a plurality of celestial bodies in the inertial space flight process, the spectrum frequency received by the spacecraft has the following relationship with the natural frequency of the spectrum of the celestial body according to the doppler effect of the light wave:
Figure BDA0002363965150000072
in the formula, v represents a velocity vector of the spacecraft relative to the celestial body light source in an inertial coordinate system, c represents the light velocity in vacuum, and theta represents an included angle between the celestial body light source-spacecraft wave vector and the velocity vector v.
If the spacecraft can observe 3 or more non-collinear celestial body light sources in the flying process, the following can be observed:
Figure BDA0002363965150000073
in the formula, vr1,vr2,vr3Respectively representing the radial velocity, z, of the spacecraft relative to the respective reference celestial light source1,z2,z3Respectively representing the red shift values, v, of the spacecraft relative to the 3 non-collinear reference celestial light sourcespRepresenting the velocity vector, v, of the spacecraft in the inertial system1,v2,v3Respectively representing the velocity vector of each reference celestial body in the inertial system.
According to the vector relation between the space celestial body light sources shown in FIG. 2, vr1,vr2,vr3And vpSatisfy the following relationship
Figure BDA0002363965150000081
In the formula u1,u2,u3And the unit vector represents the position vector of each reference celestial body light source pointing to the spacecraft under the inertial coordinate system.
In conclusion, the velocity vector equation of the spacecraft in the inertial coordinate system is vp＝f(v1,v2,v3,u1,u2,u3,vr1,vr2,vr3) And solving the equation and carrying out coordinate transformation to obtain the first speed of the spacecraft.
After the first speed is obtained, the speed of the spacecraft measured in the system is extracted through the inertial navigation system, and the speed is the second speed. And subtracting the first speed and the second speed to obtain the speed difference between the first speed and the second speed. And then measuring a first horizontal position and a second horizontal position of the spacecraft by an inertial navigation and geomagnetic positioning system respectively, and generating a horizontal position difference according to the first horizontal position and the second horizontal position. And constructing a measurement model of the integrated navigation system according to the speed difference and the horizontal position difference.
Specifically, the measurement model is specifically:
z(t)＝H(t)x(t)+v(t)，
wherein the content of the first and second substances,
Figure BDA0002363965150000082
vE、vNand vURespectively representing the east speed, the north speed and the sky speed of the spacecraft obtained by the inertial navigation system; v. ofSE、vSNAnd vSURepresenting the east speed, the north speed and the sky speed of the spacecraft obtained by the spectrum red shift navigation system; l and lambda are latitude and longitude information of the spacecraft obtained by the inertial navigation system respectively; l isDAnd λDRespectively obtaining latitude and longitude information of the spacecraft for a geomagnetic positioning system; h (t) ([ 0 ]5×4I5×505×6]5×15，
Figure BDA0002363965150000083
Wherein v is1(t) Gaussian white noise included in the velocity information of the spectral redshift measurement, v2(t) is Gaussian white noise contained in the horizontal position information measured by the geomagnetic positioning system.
After the measurement model is built, navigation error estimation needs to be carried out according to the measurement model, at the moment, a nonlinear error state model of the integrated navigation system needs to be combined, and compared with a traditional linear model, the nonlinear error state model used in the method is more suitable for actual conditions, and the navigation precision of the system can be improved.
In this embodiment, the method for constructing the nonlinear error state model includes:
selecting an east-north-sky (E, N, U) geographic coordinate system as a navigation coordinate system N, and establishing a nonlinear error state model according to an error equation of an inertial navigation system and inertial devices (namely inertial detection hardware such as a gyroscope, an accelerometer and the like):
Figure BDA0002363965150000091
wherein, x (t) is system state vector, f (x (t)) is nonlinear state function of system, G (t) is noise driving matrix of system, w (t) is noise of system process;
Figure BDA0002363965150000092
q0、q1、q2、q3is an attitude error quaternion; delta VE、δVN、δVUThe speed errors in the east, north and sky directions; δ L, δ λ, δ h are latitude, longitude and altitude errors;
Figure BDA0002363965150000093
representing a random constant drift of the gyro;
Figure BDA0002363965150000094
is the random constant error of the accelerometer.
In this example:
Figure BDA0002363965150000095
in the formula (I), the compound is shown in the specification,
Figure BDA0002363965150000096
i is an identity matrix and is a matrix of the identity,
Figure BDA0002363965150000097
representing a transformation matrix between the navigation coordinate system n to the platform coordinate system c,
Figure BDA0002363965150000098
representing navigation seatsThe rotational angular velocity of the reference system n relative to the inertial system i,
Figure BDA0002363965150000099
representing a transformation matrix, epsilon, between a body coordinate system b and a platform coordinate system cbRepresenting the projection of the gyro error in the carrier system b,
Figure BDA0002363965150000101
representing a transformation matrix between the platform coordinate system c to the navigation coordinate system n, fbA measurement value representing an accelerometer of the inertial navigation system,
Figure BDA0002363965150000102
and
Figure BDA0002363965150000103
respectively representing rotational angular velocity and position angular velocity of the earth, δ V representing velocity error, δ P representing positioning error, VnRepresenting the speed of the spacecraft, M and N respectively representing a speed error coefficient matrix and a position error coefficient matrix, L being the latitude of the current position of the spacecraft, 03×1All 0 matrices of 3 x 1 order are represented.
The speed error coefficient matrix and the position error coefficient matrix are respectively:
Figure BDA0002363965150000104
Figure BDA0002363965150000105
wherein R isMAnd RNThe radius of curvature of the meridian circle of the earth and the radius of curvature of the prime circle, h is the height in the geographical coordinate system of the spacecraft, VERepresenting the eastern speed of the spacecraft.
In the nonlinear error state model, the noise driving matrix g (t) is specifically:
Figure BDA0002363965150000106
wherein, 03×3All 0 matrices of 3 x 3 order are represented.
After obtaining the measurement model and the nonlinear error state model, carrying out optimal estimation on the measurement model and the nonlinear error state model constructed in advance through the interactive multi-model robust adaptive UKF. Because a navigation computer carried by a spacecraft usually adopts discrete data processing, a nonlinear error state model of a spectrum redshift/inertial navigation/geomagnetic integrated navigation system and a measurement model of the integrated navigation system are subjected to discretization processing to obtain two discretized models:
Figure BDA0002363965150000111
wherein x isk∈RnAnd zk∈RmRespectively, a state vector and a measurement vector, R, of the system at time knReal vector space of n x 1, RmThe real vector spaces, respectively denoted m x 1, f (-) are respectively nonlinear functions, HkTo measure the matrix, wk∈RnAnd vk∈RmIs a zero mean white Gaussian noise sequence with mutual independence and the covariance is cov (w)k,wj)＝Qδkj，cov(vk,vj)＝RδkjQ is a non-negative definite matrix, R is a positive definite matrix, deltakjAs a function of Kronecker-delta.
After the dispersion is completed, as shown in fig. 3, the specific optimization method is as follows:
firstly, an adaptive evanescent UKF sub-filter and an anti-difference UKF sub-filter are constructed.
Considering a nonlinear system as shown in the above formula, assuming that the nonlinear system has a state model uncertainty, the core idea of an adaptive fading UKF Algorithm (AFUKF) is to introduce a time-varying adaptive fading factor to the state prediction covariance matrix of the classical UKF to reduce the influence of the state model uncertainty on the state estimation at the current time. The state prediction covariance matrix in the adaptive fading UKF sub-filter is as follows:
Figure BDA0002363965150000112
wherein λ iskFor adaptive fading factor, Pk/k-1Covariance matrices are predicted for states in the UKF.
In contrast, when a nonlinear system has uncertainty of a measurement model, the core idea of the robust UKF algorithm (RUKF) is to suppress the influence of abnormal measurement information on the current state estimation by introducing a time-varying robust factor to the innovation vector covariance matrix of the classical UKF. The innovation vector covariance matrix in the robust UKF sub-filter is:
Figure BDA0002363965150000121
wherein s iskIs used as an anti-difference factor,
Figure BDA0002363965150000122
is an innovation vector covariance matrix in UKF.
The innovation vector is recorded as:
Figure BDA0002363965150000123
in the formula (I), the compound is shown in the specification,
Figure BDA0002363965150000124
the system measurement prediction value is obtained.
When the system works under the optimal condition, the nonlinear system does not have any uncertainty, and the information vectors output by the classical UKF algorithm at different moments are kept orthogonal to satisfy the following relation
Figure BDA0002363965150000125
The above expression is called an innovation orthogonalization criterion, and the criterion requires that innovation vectors at different time points keep an orthogonal relation so as to ensure that all effective information in an innovation sequence can be extracted. The adaptive fading and robust factors in AFUKF and RUKF can be calculated by forcing the above equation to hold.
After the steps are finished, interactive multi-model robust adaptive UKF is carried out, a measurement model and a nonlinear error state model are used as system models, a speed difference and a horizontal position difference are used as measurement input values, meanwhile, the input values are filtered through a self-adaptive gradually-eliminating UKF sub-filter and a robust UKF sub-filter, and corresponding state estimation is obtained respectively
Figure BDA0002363965150000126
And its error covariance matrix Pj,k；
Specifically, system initialization is firstly performed, and initialization setting is respectively performed on the AFUKF, the RUKF and the multi-filter fusion process.
i) The two sub-filter AFUKF and RUKF algorithms are initialized by:
Figure BDA0002363965150000127
ii) presetting a Markov transfer matrix and an initial model probability to complete the initialization setting of the fusion process of the multi-filter,
Figure BDA0002363965150000131
in the formula, mijIs the Markov transition probability between sub-filter i and sub-filter j; mu.sj,0Is the initial model probability of the sub-filter j (j ═ 1, 2).
Then, parallel filtering is carried out, and AFUKF and RUKF algorithms are simultaneously executed by a parallel structure to obtain corresponding state estimation
Figure BDA0002363965150000132
And its error covariance matrix Pj,k。
(i) In the AFUKF algorithm, P in the classical UKF is comparedk/k-1Substitution as an improved state prediction covariance matrix
Figure BDA0002363965150000133
The other steps are the same as for classical UKF.
(ii) Unlike AFUKF, the RUKF algorithm is a covariance matrix of innovation in classical UKF
Figure BDA0002363965150000134
Is modified into
Figure BDA0002363965150000135
Thereafter, model probability updating, i.e. state estimation, respectively, is performed
Figure BDA0002363965150000136
And its error covariance matrix Pj,kWeights are assigned. In the interactive multi-model estimation method, the model probability of the sub-filter at time k can be updated according to the following formula:
Figure BDA0002363965150000137
wherein, cj,k-1Is the normalization factor, mu, of the last time instant (k-1)j,kThe model probability of the sub-filter j at the moment k is described, and the model probability describes the credibility of the sub-filter j at the current moment; lambdaj,kThe likelihood function for sub-filter j at time k can be found by:
Figure BDA0002363965150000138
in the formula (I), the compound is shown in the specification,
Figure BDA0002363965150000139
the innovation vector for sub-filter j at time k,
Figure BDA00023639651500001310
is the covariance matrix of the innovation vector of the sub-filter j at time k, and m is the measurement vector zkDimension (d) of (a). For the adaptive fading UKF and robust UKF algorithms, the innovation vector covariance matrices are expressed as
Figure BDA0002363965150000141
And
Figure BDA0002363965150000142
and then, carrying out input information interaction, wherein the input interaction is the most important step of the interactive multi-model estimation method, and the initial value of each sub-filter at the current moment is obtained by state estimation and Markov transition probability calculation at the previous moment, which is also called as the redistribution process of the overall state estimation information.
State estimation obtained by two sub-filters in input interaction process
Figure BDA0002363965150000143
And its error covariance matrix Pj,k(j ═ 1,2) will be reassigned as:
Figure BDA0002363965150000144
Figure BDA0002363965150000145
in the formula, gij,kFor mixing probabilities, can be obtained by
Figure BDA0002363965150000146
g1j,k+g 2j,k1, find outj,kFor updated model probabilities, mijIs a preset Markov transition probability, ci,kFor the normalization factor, the probability update of the model to be used at the next time is calculated by the formula:
Figure BDA0002363965150000147
then, estimating the state of redistribution
Figure BDA0002363965150000148
And its error covariance matrix
Figure BDA0002363965150000149
And sent to two sub-filters as the initial value of the filtering solution at the next moment.
And then, carrying out state fusion, namely carrying out weighted average to obtain an error estimation value.
Overall system state estimation
Figure BDA00023639651500001410
Estimating states for two sub-filters
Figure BDA00023639651500001411
And
Figure BDA00023639651500001412
model probability weighted fusion of (1):
Figure BDA00023639651500001413
with a corresponding error covariance matrix of
Figure BDA0002363965150000151
The spacecraft navigation system adopts an inertial navigation system as a main navigation system to complete the whole flight process, and high-precision speed information output by a spectrum red shift navigation system and high-performance horizontal position information output by a geomagnetic navigation system are used for inhibiting the accumulation of inertial navigation errors, so that the autonomy and the precision of the spacecraft navigation system are improved. In the method, firstly, based on a spectrum redshift speed measurement navigation principle, a spectrum redshift/inertial navigation/geomagnetic autonomous integrated navigation system principle is designed by utilizing spectrum redshift information of a natural celestial body of a solar system, and a nonlinear mathematical model of the integrated navigation is established. And then, optimally estimating the navigation error of the system by using an interactive multi-model robust adaptive UKF algorithm, and performing error correction on the navigation parameters of inertial navigation through feedback correction, thereby improving the navigation positioning precision of the spacecraft.

Claims (7)

1. A spectrum red shift autonomous combined navigation method based on robust adaptive UKF is characterized by comprising the following steps:
solving a first speed through ephemeris information of a solar system celestial body, attitude information of a spacecraft measured by a star sensor and a spectrum red shift value measured in a space where the spacecraft is located; the first speed is the speed of the spacecraft in a navigation coordinate system after coordinate transformation;
acquiring a second speed of the spacecraft through an inertial navigation system, and generating a speed difference according to the first speed and the second speed; measuring a first horizontal position and a second horizontal position of the spacecraft through an inertial navigation system and a geomagnetic positioning system respectively, and generating a horizontal position difference according to the first horizontal position and the second horizontal position;
establishing a measurement model of the spectrum redshift/inertial navigation/geomagnetic integrated navigation system according to the speed difference and the horizontal position difference;
optimally estimating the measurement model and a pre-constructed nonlinear error state model through an interactive multi-model robust adaptive UKF to obtain a navigation error estimation value;
and correcting the navigation parameters of the inertial navigation system through the error estimation value to obtain the speed information, the position information and the attitude information of the spacecraft.
2. The spectrum redshift autonomous integrated navigation method based on robust adaptive UKF as claimed in claim 1, wherein the measurement model is specifically:
z(t)＝H(t)x(t)+v(t)，
wherein the content of the first and second substances,
Figure FDA0002363965140000011
vE、vNand vURespectively representing the east speed, the north speed and the sky of the spacecraft obtained by the inertial navigation systemA forward speed; v. ofSE、vSNAnd vSURepresenting the east speed, the north speed and the sky speed of the spacecraft obtained by the spectrum red shift navigation system; l and lambda are latitude and longitude information of the spacecraft obtained by the inertial navigation system respectively; l isDAnd λDRespectively obtaining latitude and longitude information of the spacecraft for a geomagnetic positioning system; h (t) ([ 0 ]5×4I5×505×6]5×15，
Figure FDA0002363965140000021
Wherein v is1(t) Gaussian white noise included in the velocity information of the spectral redshift measurement, v2(t) is Gaussian white noise contained in the horizontal position information measured by the geomagnetic positioning system.
3. The spectrum redshift autonomous integrated navigation method based on robust adaptive UKF as claimed in claim 1, wherein the optimal estimation of the metrology model and the pre-constructed nonlinear error state model by the interactive multi-model robust adaptive UKF comprises:
constructing a self-adaptive fading UKF sub-filter and an anti-difference UKF sub-filter;
the state prediction covariance matrix in the adaptive fading UKF sub-filter is as follows:
Figure FDA0002363965140000022
wherein λ iskFor adaptive fading factor, Pk/k-1Predicting a covariance matrix for the state in the UKF;
the innovation vector covariance matrix in the robust UKF sub-filter is as follows:
Figure FDA0002363965140000023
wherein s iskIs used as an anti-difference factor,
Figure FDA0002363965140000024
an innovation vector covariance matrix in UKF;
taking the measurement model and the nonlinear error state model as system models, taking the speed difference and the horizontal position difference as measurement input values, and simultaneously filtering the input values through a self-adaptive evanescent UKF sub-filter and an anti-error UKF sub-filter to respectively obtain corresponding state estimation
Figure FDA0002363965140000025
And its error covariance matrix Pj,k；
Respectively for the state estimation
Figure FDA0002363965140000026
And its error covariance matrix Pj,kAnd giving weight, and carrying out weighted average to obtain the navigation error estimation value.
4. The spectrum redshift autonomous integrated navigation method based on robust adaptive UKF as claimed in claim 3, wherein the weight is calculated by the following formula:
Figure FDA0002363965140000031
wherein, muj,kIs the model probability, Λ, of the sub-filter j at time kj,kIs the likelihood function of the sub-filter j at time k, cj,k-1Is the normalization factor at time k-1.
5. The spectrum redshift autonomous integrated navigation method based on robust adaptive UKF as claimed in claim 1, wherein the construction method of the nonlinear error state model is as follows:
selecting an east-north-sky (E, N, U) geographic coordinate system as a navigation coordinate system N, and establishing a nonlinear error state model according to an error equation of the inertial navigation system and an inertial device:
Figure FDA0002363965140000032
wherein, x (t) is system state vector, f (x (t)) is nonlinear state function of system, G (t) is noise driving matrix of system, w (t) is noise of system process;
Figure FDA0002363965140000033
q0、q1、q2、q3is an attitude error quaternion; delta VE、δVN、δVUThe speed errors in the east, north and sky directions; δ L, δ λ, δ h are latitude, longitude and altitude errors;
Figure FDA0002363965140000034
representing a random constant drift of the gyro;
Figure FDA0002363965140000035
is the random constant error of the accelerometer.
6. The spectrum redshift autonomous combined navigation method based on robust adaptive UKF as claimed in claim 5, wherein the noise driving matrix G (t) is specifically:
Figure FDA0002363965140000036
wherein the content of the first and second substances,
Figure FDA0002363965140000041
Figure FDA0002363965140000042
representing a transformation matrix between the body coordinate system b and the platform coordinate system c, 03×3All 0 matrices of 3 x 3 order are represented.
7. The spectrum redshift autonomous integrated navigation method based on the interactive multi-model robust adaptive UKF as claimed in claim 2, wherein the first speed solution method is:
establishing the radial velocity v of the spacecraft relative to the reference celestial light sourcesr1,vr2,vr3Velocity vector v in inertial system with spacecraftpThe relation of (1):
Figure FDA0002363965140000043
in the formula, v1,v2,v3For each reference celestial body's velocity vector in the inertial frame, u1,u2,u3Unit vector v representing the vector of the position of each reference celestial body light source pointing to the spacecraft in the inertial coordinate systemr1,vr2,vr3The radial speed of the spacecraft relative to each reference celestial body light source;
solving the velocity vector equation of the spacecraft in the inertial coordinate system as vp＝f(v1,v2,v3,u1,u2,u3,vr1,vr2,vr3) And obtaining the first speed of the spacecraft through coordinate transformation.
CN202010030024.7A 2020-01-13 2020-01-13 Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF Active CN111156986B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010030024.7A CN111156986B (en) 2020-01-13 2020-01-13 Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010030024.7A CN111156986B (en) 2020-01-13 2020-01-13 Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF

Publications (2)

Publication Number Publication Date
CN111156986A true CN111156986A (en) 2020-05-15
CN111156986B CN111156986B (en) 2021-10-19

Family

ID=70562552

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010030024.7A Active CN111156986B (en) 2020-01-13 2020-01-13 Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF

Country Status (1)

Country Link
CN (1) CN111156986B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179347A (en) * 2020-09-18 2021-01-05 西北工业大学 Combined navigation method based on spectrum red shift error observation equation
CN112985394A (en) * 2021-05-12 2021-06-18 腾讯科技（深圳）有限公司 Positioning method and device, and storage medium
CN113295170A (en) * 2021-04-25 2021-08-24 西北工业大学 Strapdown inertial navigation/spectrum redshift autonomous integrated navigation system and filtering method
CN113324539A (en) * 2021-05-19 2021-08-31 西北工业大学深圳研究院 SINS/SRS/CNS multi-source fusion autonomous integrated navigation method

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103206955A (en) * 2013-03-06 2013-07-17 上海卫星工程研究所 Spectrum red shift antimonous navigation method for spacecraft
CN107389063A (en) * 2017-07-26 2017-11-24 重庆邮电大学 The indoor fusion and positioning method of high accuracy based on GSM/MEMS fusions

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103206955A (en) * 2013-03-06 2013-07-17 上海卫星工程研究所 Spectrum red shift antimonous navigation method for spacecraft
CN107389063A (en) * 2017-07-26 2017-11-24 重庆邮电大学 The indoor fusion and positioning method of high accuracy based on GSM/MEMS fusions

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
CHEN JIANG: "Adaptive Estimation of Multiple Fading Factors for GPS/INS Integrated Navigation Systems", 《SENSORS》 *
WENHUI WEI: "A SINS/SRS/GNS Autonomous Integrated Navigation System Based on Spectral Redshift Velocity Measurements", 《SENSOR》 *
汪云等: "基于多模型ＧＧＩＷ－ＣＰＨＤ滤波的群目标跟踪算法", 《华中科技大学学报（自然科学版）》 *
薛海建: "基于自适应多重渐消因子卡尔曼滤波的SINS初始对准方法", 《系统工程与电子技术》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179347A (en) * 2020-09-18 2021-01-05 西北工业大学 Combined navigation method based on spectrum red shift error observation equation
CN113295170A (en) * 2021-04-25 2021-08-24 西北工业大学 Strapdown inertial navigation/spectrum redshift autonomous integrated navigation system and filtering method
CN112985394A (en) * 2021-05-12 2021-06-18 腾讯科技（深圳）有限公司 Positioning method and device, and storage medium
CN112985394B (en) * 2021-05-12 2021-08-06 腾讯科技（深圳）有限公司 Positioning method and device, and storage medium
CN113324539A (en) * 2021-05-19 2021-08-31 西北工业大学深圳研究院 SINS/SRS/CNS multi-source fusion autonomous integrated navigation method

Also Published As

Publication number Publication date
CN111156986B (en) 2021-10-19

Similar Documents

Publication Publication Date Title
CN111156986B (en) Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
CN111721289B (en) Vehicle positioning method, device, equipment, storage medium and vehicle in automatic driving
CN108759833B (en) Intelligent vehicle positioning method based on prior map
CN110412635B (en) GNSS/SINS/visual tight combination method under environment beacon support
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN106871928B (en) Strap-down inertial navigation initial alignment method based on lie group filtering
CN101344391B (en) Lunar vehicle posture self-confirming method based on full-function sun-compass
CN110702143B (en) Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN109506660B (en) Attitude optimization resolving method for bionic navigation
CN111366148B (en) Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system
CN111238467A (en) Bionic polarized light assisted unmanned combat aircraft autonomous navigation method
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN107389069B (en) Ground attitude processing method based on bidirectional Kalman filtering
CN110567455A (en) tightly-combined navigation method for quadrature updating of volume Kalman filtering
CN111207773B (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN113252038A (en) Course planning terrain auxiliary navigation method based on particle swarm optimization
CN108981691B (en) Sky polarized light combined navigation on-line filtering and smoothing method
Yang et al. Multilayer low-cost sensor local-global filtering fusion integrated navigation of small UAV
Mostafa et al. Optical flow based approach for vision aided inertial navigation using regression trees
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN110388942B (en) Vehicle-mounted posture fine alignment system based on angle and speed increment
Cucci et al. On raw inertial measurements in dynamic networks
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor

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