CN115451955A - INS/GPS tightly-coupled navigation method and system based on distributed robust filtering - Google Patents
INS/GPS tightly-coupled navigation method and system based on distributed robust filtering Download PDFInfo
- Publication number
- CN115451955A CN115451955A CN202211200811.7A CN202211200811A CN115451955A CN 115451955 A CN115451955 A CN 115451955A CN 202211200811 A CN202211200811 A CN 202211200811A CN 115451955 A CN115451955 A CN 115451955A
- Authority
- CN
- China
- Prior art keywords
- gps
- ins
- state
- error
- navigation system
- 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.)
- Pending
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention provides an INS/GPS tight coupling navigation method and system based on distributed robust filtering, belonging to the technical field of integrated navigation systems, wherein the system based on the method consists of a strapdown inertial navigation system and a global satellite positioning system, the inertial navigation system acquires the three-dimensional position, attitude and speed information of a mobile carrier, the global satellite positioning system outputs pseudo-moment and pseudo-range rate measurement values, and a distributed robust filtering algorithm is used for state estimation on the basis; the filtering precision of the integrated navigation system can be improved on the premise of keeping the robustness of the integrated navigation system, and the integrated navigation system is simple and easy to implement and has wide application prospect.
Description
Technical Field
The invention relates to the technical field of integrated navigation systems, in particular to an INS/GPS tightly-coupled navigation method and system based on distributed robust filtering.
Background
The combined navigation system combines and optimizes the output information of a plurality of different navigation systems by a multi-sensor information fusion technology, thereby obtaining better performance compared with a single navigation system. The Inertial Navigation System (INS) and the global positioning satellite navigation system (GPS) have complementary characteristics, the updating speed of the inertial navigation system is high, but compared with the GPS navigation system, the inertial navigation system has the problem of navigation error accumulation, so that the INS/GPS tightly-coupled combined navigation system formed by combining the inertial navigation system and the GPS can make up for the deficiency, and provides more accurate information such as position, attitude and the like for a mobile carrier.
The traditional EKF algorithm is widely applied to INS/GPS integrated navigation, but in practical application, the problems of low positioning precision, large error of a linearization model, easy interference of uncertain noise and the like are faced. The algorithm such as UKF and PF has promoted certain filtering precision, but when the initial error of system is great or the appearance abnormal value of measurationing, the phenomenon that filtering is diverged is easily produced to UKF algorithm, and the operation cost of PF filter is higher. The traditional robust filtering algorithm can effectively inhibit abnormal values of the system by compensating uncertainty of system modeling, but the traditional robust filtering method tends to be conservative, so that although the anti-interference performance of the system can be improved, the filtering precision is also reduced.
Disclosure of Invention
The invention aims to provide an INS/GPS tightly-coupled navigation method and system based on distributed robust filtering, which can not only keep stronger robustness of the system, but also effectively improve filtering precision when an integrated navigation system faces unknown noise and modeling error, so as to solve at least one technical problem in the background technology.
In order to achieve the purpose, the invention adopts the following technical scheme:
in one aspect, the invention provides an INS/GPS tightly-coupled navigation method based on distributed robust filtering, which comprises the following steps:
according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measuring model of the INS/GPS tightly coupled navigation system is constructed;
and constructing a moment-based distributed robust Kalman filter according to the state space-measurement model, and carrying out navigation system information fusion by adopting parameters of the dynamic filter and inhibiting system initial errors to obtain optimal estimation values of position, speed and attitude.
Preferably, the method for constructing the state space-measurement model of the INS/GPS tightly-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo-moment and pseudo-range rate information output by the GPS integrated navigation system includes: defining a reference coordinate system required by an INS/GPS tightly-coupled navigation system; outputting real-time position, speed and attitude information of a mobile carrier according to the INS system and pseudo moment and pseudo range rate information output by the GPS integrated navigation system; and (3) constructing an INS/GPS tightly-coupled navigation system state space-measurement model.
Preferably, outputting the real-time position, speed and attitude information of the mobile carrier and the pseudo-moment and pseudo-range rate information output by the GPS integrated navigation system according to the INS system includes:
the INS system consists of an IMU and an inertial navigation resolving processing unit, and the angular velocity value output by the gyroscope and the specific force value output by the accelerometer are resolved through the data processing unit to obtain the current three-dimensional position (p) of the mobile carrier x ,p y ,p z ) Velocity (v) x ,v y ,v z ) Attitude information (ψ, θ, γ), wherein ψ represents a heading angle of the carrier, θ represents a pitch angle of the carrier, and γ represents a roll angle of the carrier; calculating the pseudo moment rho of the mth satellite according to the ephemeris data of the GPS system m And pseudorange rate ρ' m 。
Preferably, the building of the INS/GPS tightly coupled navigation system state-measurement model comprises:
firstly, selecting an INS/GPS tightly-coupled navigation system state error vector:
in the formula, the three-dimensional position error δ p k Three dimensional velocity error δ v k (ii) a The rotational motion of the L system relative to the B system is expressed by using a quaternion q, q k ＝[q 0 q 1 q 2 q 3 ]，δq k ＝[q 1 q 2 q 3 ]；
Is indicative of the drift error of the gyroscope,
representing a constant error of the accelerometer; δ s g,k ,δs a,k Respectively representing scale factor errors of a gyroscope and an accelerometer; c δ t k 、cδt′ k Respectively representing a clock bias error and a drift error of the GPS receiver;
after the state error equation of the nonlinear combined navigation system is subjected to first-order linearization and discretization through a Taylor formula, a state space error model is obtained:
δx k ＝(Ι+F k-1 ·T s )δx k-1 +Γ k w k-1
in the formula F k-1 Jacobian matrix, T, for a system at time k-1 s For sampling time intervals, Γ k For noise driving the matrix, w k-1 Is system process noise;
measurement error model:
δy k ＝H k δx k-1 +v k
in the formula H k Measuring the Jacobian matrix v for time k k To measure noise;
the measurement error model is developed to obtain:
in the formula, δ ρ m,k Representing the pseudo-moment error for the mth satellite at time k,
representing the pseudo-moment rate error, I, of the m-th satellite at time k 3×1 Is a unit vector of 3 rows and 1 column, (δ x) k ,δy k ,δz k ) Indicates the three-dimensional position of the vector in the E system, (. Delta.v.) x,k ,δv y,k ,δv z,k ) Representing the three-dimensional speed of the carrier in the E system, and c represents the speed of light;
preferably, a distributed robust kalman filter based on a moment is constructed according to a state space-measurement model, and a dynamic filter parameter is adopted to suppress a system initial error and perform navigation system information fusion to obtain an optimal estimation value of a position, a speed and a posture, wherein the method comprises the following steps:
recording the historical measurement value of k time as Y k-1 ＝[y 1 ,y 2 ,L,y k-1 ]Then the system state-measurement combined calibration distribution is P k [(x k ,y k )|Y k-1 ]The variable z under the framework of Gaussian filtering k ＝[x k ,y k ] T Obey mean value of
Variance of
Of Gaussian distribution, i.e. P k [z k |Y k-1 ]:
Constructing a distribution fuzzy set based on moment to describe the modeling uncertainty of an INS/GPS tightly coupled navigation system, and introducing a fuzzy set omega:
in the formula, gamma 2 、γ 3 For the filter uncertain parameters, c k For the state at time k-measuring the true value, S k Is a state-measured true covariance matrix, the fuzzy set represents k time, and the state-measured joint distribution is located in
A ball with a centre of sphere. According to the distribution robust estimation theory, when
Namely that
Then, the state estimation is optimal;
initializing filter parameters of the integrated navigation system: the process includes distributed robust filter parameter initialization and system state initialization, given an initial state estimate
Sum error covariance matrix P 0 Calculating a state vector dimension x _ dim of the combined navigation system;
calculating a state prior estimate and a prior error covariance matrix:
according to the system jacobian matrix:
calculating a system state transition matrix phi k,k-1 ＝Ι+F k-1 ·T s ；
Computing state-measurement prior error covariance matrix
In the formula (I), the compound is shown in the specification,
further, a state prior error covariance matrix P is obtained k/k-1 ：
Measurement updating: computing optimal state-measurement covariance
Design dynamic parameter gamma 2 Inhibiting the initial error of the INS/GPS tightly-coupled navigation system;
order to
Calculating the Kalman gain K according to k ：
Calculating the optimal estimated value of the system state
In the formula, e k In order to filter the information of interest,
y k the measured value at the time k is the measured value,
is a prior estimated value of the state at the moment k;
updating the state error covariance matrix P k ：
Preferably, the specific process of designing the dynamic filter parameters includes:
dynamic parameter gamma 2 By dynamic factors
The dynamic factor can increase the state posterior error covariance matrix P when the initial error of the integrated navigation system is larger k The robustness of the filter at the k +1 moment is improved, the dynamic factor tends to be zero along with the increase of the filtering times, and the dynamic parameter gamma at the moment 2 Tends to be stable, i.e. gamma 2 ≈γ；
In the formula, gamma 2 More than or equal to 1, alpha is an adjustable parameter, the value range is (alpha is more than 1), n GPS For the current GPS measurement update times, N is a positive integer and needs to be guaranteed
Gamma is a steady-state factor, and the value interval is (gamma is more than or equal to 1).
In a second aspect, the present invention provides an INS/GPS tightly coupled navigation system based on distributed robust filtering, comprising:
the building module is used for building a state space-measuring model of the INS/GPS tightly-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and pseudo moment and pseudo range rate information output by the GPS integrated navigation system;
and the calculation module is used for constructing a distribution robust Kalman filter based on the moment according to the state space-measurement model, inhibiting the initial error of the system by adopting the parameters of the dynamic filter, and carrying out information fusion on the navigation system to obtain the optimal estimation values of the position, the speed and the posture.
In a third aspect, the present invention provides a non-transitory computer readable storage medium for storing computer instructions which, when executed by a processor, implement a distributed robust filtering based INS/GPS tight-coupled navigation method as described above.
In a fourth aspect, the invention provides a computer program product comprising a computer program for implementing a distributed robust filtering based INS/GPS tight-coupled navigation method as described above when run on one or more processors.
In a fifth aspect, the present invention provides an electronic device comprising: a processor, a memory, and a computer program; wherein a processor is coupled to a memory in which a computer program is stored, wherein the processor executes the computer program stored by the memory when the electronic device is running to cause the electronic device to execute instructions to implement the distributed robust filtering based INS/GPS tight coupled navigation method as described above.
The invention has the beneficial effects that: compared with an unscented Kalman filtering method, the robustness of the system for processing abnormal values can be improved; compared with a particle filtering method, the required filtering time is shorter, and the real-time performance of the algorithm is higher; compared with the traditional robust filtering method, the filtering precision is higher, and higher filtering convergence speed can be realized when the method is used for dealing with larger system initial errors; the realization process is simple, the parameters required to be adjusted are few, and the applicability is wide.
Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the description below are only some embodiments of the present invention, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a block diagram of an INS/GPS tightly coupled navigation system according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a distributed robust filtering model according to an embodiment of the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by way of the drawings are illustrative only and are not to be construed as limiting the invention.
It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
It will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.
As used herein, the singular forms "a", "an", "the" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and/or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
For the purpose of facilitating an understanding of the present invention, the present invention will be further explained by way of specific embodiments with reference to the accompanying drawings, which are not intended to limit the present invention.
It will be appreciated by those skilled in the art that the drawings are merely schematic representations of embodiments and that the elements in the drawings are not necessarily required to practice the present invention.
Example 1
First, in this embodiment 1, an INS/GPS tightly coupled navigation system based on distributed robust filtering is provided, which includes:
the building module is used for building a state space-measuring model of the INS/GPS tightly-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and pseudo moment and pseudo range rate information output by the GPS integrated navigation system;
and the calculation module is used for constructing a distribution robust Kalman filter based on the moment according to the state space-measurement model, inhibiting the initial error of the system by adopting the parameters of the dynamic filter, and carrying out information fusion on the navigation system to obtain the optimal estimation values of the position, the speed and the posture.
In this embodiment 1, based on the system, an INS/GPS tight coupling navigation method based on distributed robust filtering is implemented, including:
utilizing a construction module to construct a state space-measuring model of the INS/GPS tightly-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and pseudo moment and pseudo range rate information output by the GPS integrated navigation system;
and constructing a distribution robust Kalman filter based on the moment according to the state space-measurement model by using a calculation module, inhibiting the initial error of the system by adopting the parameters of a dynamic filter, and fusing the information of the navigation system to obtain the optimal estimation values of the position, the speed and the attitude.
According to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measurement model of the INS/GPS tightly-coupled navigation system is constructed, and the method comprises the following steps: defining a reference coordinate system required by an INS/GPS tightly-coupled navigation system; outputting real-time position, speed and attitude information of the mobile carrier and pseudo moment and pseudo range rate information output by the GPS integrated navigation system according to the INS system; and (3) constructing an INS/GPS tightly-coupled navigation system state space-measurement model.
The method comprises the following steps of outputting real-time position, speed and attitude information of a mobile carrier and pseudo moment and pseudo range rate information output by a GPS integrated navigation system according to an INS system, wherein the real-time position, speed and attitude information of the mobile carrier and the pseudo moment and pseudo range rate information comprise:
the INS system consists of an IMU and an inertial navigation resolving processing unit, and the angular velocity value output by the gyroscope and the specific force value output by the accelerometer are resolved through the data processing unit to obtain the current three-dimensional position (p) of the mobile carrier x ,p y ,p z ) Velocity (v) x ,v y ,v z ) Attitude information (ψ, θ, γ), wherein ψ represents a heading angle of the carrier, θ represents a pitch angle of the carrier, and γ represents a roll angle of the carrier; calculating the pseudo moment rho of the mth satellite according to the ephemeris data of the GPS system m And pseudo range rate ρ' m 。
The method for constructing the INS/GPS tightly-coupled navigation system state-measurement model comprises the following steps:
firstly, selecting an INS/GPS tightly-coupled navigation system state error vector:
in the formula, the three-dimensional position error δ p k Three dimensional velocity error δ v k (ii) a The rotational motion of the L system relative to the B system is expressed by using a quaternion q, q k ＝[q 0 q 1 q 2 q 3 ]，δq k ＝[q 1 q 2 q 3 ]；
Is indicative of the drift error of the gyroscope,
representing a constant error of the accelerometer; δ s of g,k ,δs a,k Individual watchScale factor error of the gyroscope and the accelerometer; c δ t k 、cδt′ k Respectively representing a clock bias error and a drift error of the GPS receiver;
after the state error equation of the nonlinear combined navigation system is subjected to first-order linearization and discretization through a Taylor formula, a state space error model is obtained:
δx k ＝(Ι+F k-1 ·T s )δx k-1 +Γ k w k-1
in the formula F k-1 Jacobian matrix, T, for a system at time k-1 s For sampling time intervals, Γ k For noise driving the matrix, w k-1 Is system process noise;
measuring an error model:
δy k ＝H k δx k-1 +v k
in the formula H k For k time measurement of Jacobian matrix, v k To measure noise;
the measurement error model is developed to obtain:
in the formula, δ ρ m,k Representing the pseudo-moment error for the mth satellite at time k,
representing the pseudo-moment rate error, I, of the m-th satellite at time k 3×1 Is a unit vector of 3 rows and 1 column, (δ x) k ,δy k ,δz k ) (δ v) represents the three-dimensional position of the vector in line E x,k ,δv y,k ,δv z,k ) Representing the three-dimensional speed of the carrier in the E system, and c represents the speed of light;
according to a state space-measurement model, a distributed robust Kalman filter based on moments is constructed, dynamic filter parameters are adopted, system initial errors are suppressed, navigation system information fusion is carried out, and optimal estimated values of positions, speeds and postures are obtained, and the method comprises the following steps:
calendar capable of recording k timeThe history value is Y k-1 ＝[y 1 ,y 2 ,L,y k-1 ]Then the system status-measurement combined calibration distribution is P k [(x k ,y k )|Y k-1 ]The variable z under the framework of Gaussian filtering k ＝[x k ,y k ] T Obey mean value of
Variance of
Of Gaussian distribution, i.e. P k [z k |Y k-1 ]:
Constructing a distribution fuzzy set based on moments to describe the modeling uncertainty of the INS/GPS tightly-coupled navigation system, and introducing a fuzzy set omega:
in the formula, gamma 2 、γ 3 For filter uncertainty parameters, c k For the state at time k-measured true value, S k Is a state-metric true covariance matrix, the fuzzy set represents the k time, and the state-metric joint distribution is located at
A ball with a centre. According to the distribution robust estimation theory, when
Namely that
Then, the state estimation obtains the best;
initializing filter parameters of the integrated navigation system: the process includes distributed robust filter parameter initialization and system state initialization, given an initial state estimate
Sum error covariance matrix P 0 Calculating a state vector dimension x _ dim of the combined navigation system;
calculating a state prior estimate and a prior error covariance matrix:
from the system Jacobian matrix:
solving a system state transition matrix phi k,k-1 ＝Ι+F k-1 ·T s ；
Computing state-measurement prior error covariance matrix
In the formula (I), the compound is shown in the specification,
further, a state prior error covariance matrix P is obtained k/k-1 ：
Measurement updating: computing optimal state-measurement covariance
Design dynamic parameter gamma 2 Inhibiting the initial error of the INS/GPS tightly-coupled navigation system;
order to
Calculating the Kalman gain K according to k ：
Calculating the optimal estimated value of the system state
In the formula, e k In order to filter the information of interest,
y k the measured value at the time k is the measured value,
is a prior estimated value of the state at the moment k;
updating the state error covariance matrix P k ：
The specific process of dynamic filter parameter design comprises the following steps:
dynamic parameter gamma 2 By dynamic factors
And a steady-state factor gamma, when the initial error of the integrated navigation system is larger, the dynamic factor can increase the covariance matrix P of the state posterior error k The robustness of the filter at the k +1 moment is improved, and the dynamic factor tends to be zero along with the increase of the filtering timesAt this time, the dynamic parameter gamma 2 Tend to be stable, i.e. gamma 2 ≈γ；
In the formula, gamma 2 More than or equal to 1, alpha is an adjustable parameter, the value range is (alpha is more than 1), n GPS For the current GPS measurement update times, N is a positive integer and needs to be guaranteed
Gamma is a steady-state factor, and the value range is (gamma is more than or equal to 1).
Example 2
In this embodiment 2, a method and a system for SINS/GNSS tightly-coupled navigation based on distributed robust filtering are provided, and a framework thereof is shown in fig. 1, which includes the following steps:
and 2, constructing a moment-based distributed robust Kalman filter according to the system state-measurement model established in the step 1, inhibiting the initial error of the system by adopting the parameters of the dynamic filter, carrying out information fusion on the integrated navigation system, and finally obtaining the optimal estimation values of the position, the speed and the attitude of the system in the fusion process as shown in figure 2.
The method comprises the following steps that 1, a state space model and a measurement model of the INS/GPS tightly-coupled navigation system are constructed according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, and the specific process is as follows:
step 1.1: defining a reference coordinate system required by an INS/GPS tightly coupled navigation system:
vector coordinate system-system B: the origin of the coordinate system is located at the center of the moving body, X B The axis being directed in front of the carrier, Y B Point direction X B To the right of the axis, Z B On the shaftAnd passes through the origin; local navigation coordinate system-L system: x L With the axis pointing north, Y L The axis pointing east, Z L The axis is downward along the normal of the earth reference ellipsoid; earth-fixed coordinate system-E system: origin at the center of the earth, X E Axial direction to the book meridian, Y E The axis pointing east, Z E The shaft is an earth rotation shaft;
step 1.2: the method specifically comprises the following steps of outputting real-time position, speed and attitude information of a mobile carrier and pseudo-moment and pseudo-range rate information output by a GPS integrated navigation system according to an INS system, wherein the real-time position, speed and attitude information of the mobile carrier and the pseudo-moment and pseudo-range rate information output by the GPS integrated navigation system comprise the following steps:
the INS system consists of an IMU and an inertial navigation resolving processing unit, and the angular velocity value output by the gyroscope and the specific force value output by the accelerometer are resolved by the data processing unit to obtain the current three-dimensional position (p) of the mobile carrier x ,p y ,p z ) Velocity (v) x ,v y ,v z ) And attitude information (psi, theta, gamma), wherein psi represents the heading angle of the carrier, theta represents the pitch angle of the carrier, and gamma represents the roll angle of the carrier; calculating the pseudo moment rho of the mth satellite according to the ephemeris data of the GPS system m And pseudorange rate ρ' m ；
Step 1.3: the method comprises the following steps of constructing an INS/GPS tightly-coupled navigation system state-measurement model:
firstly, selecting an INS/GPS tightly-coupled navigation system state error vector:
in the formula, the three-dimensional position error δ p k Three dimensional velocity error δ v k (ii) a The rotational motion of the L system relative to the B system is expressed by using a quaternion q, q k ＝[q 0 q 1 q 2 q 3 ]，δq k ＝[q 1 q 2 q 3 ]；
Is indicative of the drift error of the gyroscope,
representing a constant error of the accelerometer; δ s g,k ,δs a,k Respectively representing scale factor errors of a gyroscope and an accelerometer; c δ t k 、cδt′ k Respectively representing a clock bias error and a drift error of the GPS receiver;
the state error model of the nonlinear combined navigation system is linearized by a Taylor formula to obtain a discretized state space error model:
δx k ＝(Ι+F k-1 ·T s )δx k-1 +Γ k w k-1 (1)
in the formula T s For sampling time intervals, F k-1 The Jacobian matrix, Γ, for a k-1 time system k For noise driving the matrix, w k-1 Is the system process noise;
measurement error model:
δy k ＝H k δx k-1 +v k (2)
in the formula H k Measuring the Jacobian matrix v for time k k To measure noise;
as shown in fig. 1, the measured error model is obtained by modeling the difference between the predicted pseudo-moment and pseudo-range rate of the INS system and the measured pseudo-moment and pseudo-range rate output by the GPS, and the model is developed to obtain:
in the formula, δ ρ m,k Representing the pseudo-moment error for the mth satellite at time k,
representing the pseudo-moment rate error, I, of the m-th satellite at time k 3×1 Is a unit vector of 3 rows and 1 column, (δ x) k ,δy k ,δz k ) Indicates the three-dimensional position of the vector in the E system, (. Delta.v.) x,k ,δv y,k ,δv z,k ) Representing the three-dimensional speed of the carrier in the E system, and c represents the speed of light;
the specific process of the step 2 comprises the following steps:
step 2.1: recording the historical measurement value of k time as Y k-1 ＝[y 1 ,y 2 ,L,y k-1 ]Then the system status-measurement combined calibration distribution is P k [(x k ,y k )|Y k-1 ]The variable z under the framework of Gaussian filtering k ＝[x k ,y k ] T Obey mean value of
Variance of
Of Gaussian distribution, i.e. P k [z k |Y k-1 ]:
Constructing a distribution fuzzy set based on moment to describe the modeling uncertainty of an INS/GPS tightly coupled navigation system, and introducing a fuzzy set omega:
in the formula, gamma 2 、γ 3 For filter uncertainty parameters, c k For the state at time k-measuring the true value, S k Is a state-metric true covariance matrix, the fuzzy set represents the k time, and the state-metric joint distribution is located at
A ball with a centre of sphere. According to the distribution robust estimation theory, when
Namely, it is
Then, the state estimation obtains the best;
step 2.2: as shown in fig. 2, the distributed robust filtering process first initializes filter parameters of the integrated navigation system: the process includes distributed robust filteringInitialization of filter parameters and initialization of system states, given initial state estimates
Sum error covariance matrix P 0 Calculating a state vector dimension x _ dim of the combined navigation system;
step 2.3: in the time updating step, a state prior estimation and a prior error covariance matrix are calculated:
from the system Jacobian matrix:
calculating a system state transition matrix phi k,k-1 ＝Ι+F k-1 ·T s (ii) a Computing state-measurement prior error covariance matrix
In the formula (I), the compound is shown in the specification,
further, a state prior error covariance matrix P is obtained k/k-1 ：
Step 2.4: measurement update
Computing optimal state-measurement covariance
Design dynamic parameter gamma 2 Inhibiting the initial error of the INS/GPS tightly-coupled navigation system;
order to
Calculating the Kalman gain K according to equation (10) k ：
Calculating the optimal estimated value of the system state
In the formula, e k In order to filter the information of interest,
y k the measured value at the time k is the measured value,
is a prior estimated value of the state at the moment k;
updating the state error covariance matrix P k ：
The specific process of the dynamic filter parameter design in step 2 includes:
dynamic parameter gamma 2 By dynamic factors
And the dynamic factor can be increased after the state when the initial error of the integrated navigation system is largerError covariance matrix P k The robustness of the filter at the k +1 moment is improved, the dynamic factor tends to be zero along with the increase of the filtering times, and the dynamic parameter gamma at the moment 2 Tend to be stable, i.e. gamma 2 ≈γ；
In the formula, gamma 2 More than or equal to 1, alpha is an adjustable parameter, the value range is (alpha is more than 1), n GPS For the current GPS measurement update times, N is a positive integer and needs to be guaranteed
Gamma is a steady-state factor, and the value range is (gamma is more than or equal to 1).
In summary, in this embodiment 2, when the navigation system faces a large modeling error or an abnormal value occurs in measurement, the conventional robust filtering method has a phenomenon of accuracy degradation, and in this case, a distributed robust filtering method is provided, in which an INS/GPS tightly coupled navigation system is used as a research object, and a distributed robust estimation theory is adopted to fuse and update the prior predicted value of the INS system and the measured value of the GPS system, so as to obtain high-accuracy state estimation information. Compared with an unscented Kalman filtering method, the robustness of the system for processing abnormal values can be improved; compared with a particle filtering method, the required filtering time is shorter, and the real-time performance of the algorithm is higher; compared with the traditional robust filtering method, the filtering precision is higher, and higher filtering convergence speed can be realized when the method is used for dealing with larger system initial errors; the realization process is simple, the parameters required to be adjusted are few, and the applicability is wide.
Example 3
Embodiment 3 of the present invention provides a non-transitory computer-readable storage medium, where the non-transitory computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, the non-transitory computer-readable storage medium implements an INS/GPS tight-coupled navigation method based on distributed robust filtering, where the method includes:
according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measurement model of the INS/GPS tightly-coupled navigation system is constructed;
and constructing a distribution robust Kalman filter based on the moment according to a state space-measurement model, and adopting parameters of a dynamic filter to inhibit the initial error of the system to perform information fusion of the navigation system to obtain the optimal estimation values of the position, the speed and the attitude.
Example 4
An embodiment 4 of the present invention provides a computer program (product), including a computer program, for implementing an INS/GPS tight-coupled navigation method based on distributed robust filtering when the computer program runs on one or more processors, the method including:
according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measurement model of the INS/GPS tightly-coupled navigation system is constructed;
and constructing a moment-based distributed robust Kalman filter according to the state space-measurement model, and carrying out navigation system information fusion by adopting parameters of the dynamic filter and inhibiting system initial errors to obtain optimal estimation values of position, speed and attitude.
Example 5
An embodiment 5 of the present invention provides an electronic device, including: a processor, a memory, and a computer program; wherein a processor is coupled to the memory, a computer program is stored in the memory, and the processor executes the computer program stored in the memory when the electronic device is running to cause the electronic device to execute instructions to implement a distributed robust filtering based INS/GPS tightly coupled navigation method, the method comprising:
according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measurement model of the INS/GPS tightly-coupled navigation system is constructed;
and constructing a distribution robust Kalman filter based on the moment according to a state space-measurement model, and adopting parameters of a dynamic filter to inhibit the initial error of the system to perform information fusion of the navigation system to obtain the optimal estimation values of the position, the speed and the attitude.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Although the embodiments of the present invention have been described with reference to the accompanying drawings, it is not intended to limit the scope of the invention, and it should be understood by those skilled in the art that various modifications and variations can be made without inventive changes in the embodiments of the present invention.
Claims (10)
1. An INS/GPS tightly-coupled navigation method based on distributed robust filtering is characterized by comprising the following steps:
according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo moment and pseudo range rate information output by the GPS integrated navigation system, a state space-measurement model of the INS/GPS tightly-coupled navigation system is constructed;
and constructing a distribution robust Kalman filter based on the moment according to a state space-measurement model, and adopting parameters of a dynamic filter to inhibit the initial error of the system to perform information fusion of the navigation system to obtain the optimal estimation values of the position, the speed and the attitude.
2. The INS/GPS tight-coupled navigation method based on distributed robust filtering according to claim 1, wherein the method for constructing the state space-measurement model of the INS/GPS tight-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and the pseudo-moment and pseudo-range rate information output by the GPS combined navigation system comprises the following steps: defining a reference coordinate system required by an INS/GPS tightly-coupled navigation system; outputting real-time position, speed and attitude information of the mobile carrier and pseudo moment and pseudo range rate information output by the GPS integrated navigation system according to the INS system; and (3) constructing an INS/GPS tightly-coupled navigation system state space-measurement model.
3. The distributed robust filtering-based INS/GPS tightly-coupled navigation method according to claim 2, wherein the method comprises the following steps of outputting real-time position, speed and attitude information of the mobile carrier and pseudo-moment and pseudo-range rate information output by the GPS integrated navigation system according to the INS system, wherein the method comprises the following steps:
the INS system consists of an IMU and an inertial navigation resolving processing unit, and the angular velocity value output by the gyroscope and the specific force value output by the accelerometer are resolved through the data processing unit to obtain the current three-dimensional position (p) of the mobile carrier x ,p y ,p z ) Velocity (v) x ,v y ,v z ) And attitude information (psi, theta, gamma), wherein psi represents the heading angle of the carrier, theta represents the pitch angle of the carrier, and gamma represents the roll angle of the carrier; calculating the pseudo moment rho of the mth satellite according to the ephemeris data of the GPS system m And pseudorange rate ρ' m 。
4. The method for INS/GPS tightly coupled navigation based on distributed robust filtering according to claim 3, wherein the constructing of the INS/GPS tightly coupled navigation system state-measurement model comprises:
firstly, selecting an INS/GPS tightly-coupled navigation system state error vector:
in the formula, the three-dimensional position error δ p k Three dimensional velocity error δ v k (ii) a Expressing the rotational motion of the L system relative to the B system using a quaternion q, q k ＝[q 0 q 1 q 2 q 3 ]，δq k ＝[q 1 q 2 q 3 ]；
Is indicative of the drift error of the gyroscope,
representing a constant error of the accelerometer; δ s g,k ,δs a,k Respectively representing the scale factor errors of the gyroscope and the accelerometer; c δ t k 、cδt′ k Respectively representGPS receiver clock bias error and drift error;
after the state error equation of the nonlinear combined navigation system is subjected to first-order linearization and discretization through a Taylor formula, a state space error model is obtained:
δx k ＝(Ι+F k-1 ·T s )δx k-1 +Γ k w k-1
in the formula F k-1 Jacobian matrix, T, for a system at time k-1 s For sampling time intervals, Γ k For a noise-driven matrix, w k-1 Is system process noise;
measurement error model:
δy k ＝H k δx k-1 +v k
in the formula H k Measuring the Jacobian matrix v for time k k To measure noise;
the measurement error model is developed to obtain:
in the formula, δ ρ m,k Representing the pseudo-moment error for the mth satellite at time k,
representing the pseudo-moment rate error, I, of the m-th satellite at time k 3×1 Is a unit vector of 3 rows and 1 column, (δ x) k ,δy k ,δz k ) Indicates the three-dimensional position of the vector in the E system, (. Delta.v.) x,k ,δv y,k ,δv z,k ) Represents the three-dimensional speed of the carrier in the E system, and c represents the speed of light.
5. The INS/GPS tightly-coupled navigation method based on distributed robust filtering according to claim 4, wherein a distributed robust Kalman filter based on moment is constructed according to a state space-measurement model, and a dynamic filter parameter is adopted to suppress a system initial error and perform navigation system information fusion to obtain optimal estimation values of position, speed and attitude, comprising:
recording the historical measurement value at the time k as Y k-1 ＝[y 1 ,y 2 ,L,y k-1 ]Then the system status-measurement combined calibration distribution is P k [(x k ,y k )|Y k-1 ]The variable z under the framework of Gaussian filtering k ＝[x k ,y k ] T Obey mean value of
Variance of
Of Gaussian distribution, i.e. P k [z k |Y k-1 ]:
Constructing a distribution fuzzy set based on moments to describe the modeling uncertainty of the INS/GPS tightly-coupled navigation system, and introducing a fuzzy set omega:
in the formula, gamma 2 、γ 3 For filter uncertainty parameters, c k For the state at time k-measured true value, S k Is a state-metric true covariance matrix, the fuzzy set represents the k time, and the state-metric joint distribution is located at
A ball with a centre of sphere. According to the distribution robust estimation theory, when
Namely, it is
Then, the state estimation is optimal;
integrated navigation system filter parameter initialization: the process includes distributed robust filter parameter initialization and system state initialization, giving an initial state estimate
Sum error covariance matrix P 0 Calculating a state vector dimension x _ dim of the combined navigation system;
calculating a state prior estimate and a prior error covariance matrix:
according to the system jacobian matrix:
solving a system state transition matrix phi k,k-1 ＝Ι+F k-1 ·T s ；
Computing state-measurement prior error covariance matrix
In the formula (I), the compound is shown in the specification,
further, a state prior error covariance matrix P is obtained k/k-1 ：
Measurement updating: computing optimal state-measurement covariance
Design dynamic parameter gamma 2 Inhibiting the initial error of the INS/GPS tightly-coupled navigation system;
order to
Calculating the Kalman gain K according to k ：
Calculating the optimal estimated value of the system state
In the formula, e k In order to filter the information of interest,
y k the measured value at the time k is the measured value,
is a prior estimated value of the state at the moment k;
updating the state error covariance matrix P k ：
6. The distributed robust filtering-based INS/GPS tightly-coupled navigation method according to claim 5, wherein the specific process of dynamic filtering parameter design comprises:
dynamic parameter gamma 2 By dynamic factors
The dynamic factor can increase the state posterior error covariance matrix P when the initial error of the integrated navigation system is larger k The robustness of the filter at the k +1 moment is improved, the dynamic factor tends to be zero along with the increase of the filtering times, and the dynamic parameter gamma at the moment 2 Tends to be stable, i.e. gamma 2 ≈γ；
In the formula, gamma 2 More than or equal to 1, alpha is an adjustable parameter, the value range is (alpha is more than 1), n GPS For the current GPS measurement update times, N is a positive integer and needs to be guaranteed
Gamma is a steady-state factor, and the value interval is (gamma is more than or equal to 1).
7. An INS/GPS tightly coupled navigation system based on distributed robust filtering, comprising:
the building module is used for building a state space-measuring model of the INS/GPS tightly-coupled navigation system according to the real-time position, speed and attitude information of the mobile carrier output by the INS system and pseudo moment and pseudo range rate information output by the GPS integrated navigation system;
and the calculation module is used for constructing a moment-based distribution robust Kalman filter according to the state space-measurement model, inhibiting the initial error of the system by adopting the parameters of the dynamic filter, and carrying out information fusion on the navigation system to obtain the optimal estimation values of the position, the speed and the attitude.
8. A non-transitory computer readable storage medium for storing computer instructions which, when executed by a processor, implement the distributed robust filtering based INS/GPS tight-coupled navigation method of any of claims 1-6.
9. A computer program product comprising a computer program which, when run on one or more processors, is configured to implement a distributed robust filtering based INS/GPS tight-coupled navigation method as claimed in any one of claims 1-6.
10. An electronic device, comprising: a processor, a memory, and a computer program; wherein a processor is connected to a memory, wherein a computer program is stored in the memory, wherein the processor executes the computer program stored in the memory when the electronic device is running to cause the electronic device to execute instructions to implement the distributed robust filtering based INS/GPS tight coupled navigation method according to any of the claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211200811.7A CN115451955A (en) | 2022-09-29 | 2022-09-29 | INS/GPS tightly-coupled navigation method and system based on distributed robust filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211200811.7A CN115451955A (en) | 2022-09-29 | 2022-09-29 | INS/GPS tightly-coupled navigation method and system based on distributed robust filtering |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115451955A true CN115451955A (en) | 2022-12-09 |
Family
ID=84309097
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211200811.7A Pending CN115451955A (en) | 2022-09-29 | 2022-09-29 | INS/GPS tightly-coupled navigation method and system based on distributed robust filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115451955A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116381760A (en) * | 2023-06-05 | 2023-07-04 | 之江实验室 | GNSS RTK/INS tight coupling positioning method, device and medium |
-
2022
- 2022-09-29 CN CN202211200811.7A patent/CN115451955A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116381760A (en) * | 2023-06-05 | 2023-07-04 | 之江实验室 | GNSS RTK/INS tight coupling positioning method, device and medium |
| CN116381760B (en) * | 2023-06-05 | 2023-08-15 | 之江实验室 | GNSS RTK/INS tight coupling positioning method, device and medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN103235328B (en) | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method | |
| CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
| CN101949703B (en) | Strapdown inertial/satellite combined navigation filtering method | |
| CN102169184B (en) | Method and device for measuring installation misalignment angle of double-antenna GPS (Global Position System) in integrated navigation system | |
| CN110567454B (en) | SINS/DVL tightly-combined navigation method in complex environment | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN109471146B (en) | Self-adaptive fault-tolerant GPS/INS integrated navigation method based on LS-SVM | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN104344837A (en) | Speed observation-based redundant inertial navigation system accelerometer system level calibration method | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| Nagui et al. | Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN106595669B (en) | Method for resolving attitude of rotating body | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
| CN115451955A (en) | INS/GPS tightly-coupled navigation method and system based on distributed robust filtering | |
| Zhou et al. | Applying quaternion-based unscented particle filter on INS/GPS with field experiments | |
| CN110873577B (en) | Underwater rapid-acting base alignment method and device | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN116660579A (en) | Wind speed data correction method, system and device | |
| CN114018262B (en) | Improved derivative volume Kalman filtering integrated navigation method | |
| Hao et al. | Research on data fusion for SINS/GPS/magnetometer integrated navigation based on modified CDKF |
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 |