Gravity gradient / SINS / starlight autonomous integrated navigation method

Abstract

The invention relates to the technical field of aircraft navigation, in particular to a gravity gradient / SINS / starlight autonomous integrated navigation method. Establishing a gravity gradient / SINS / starlight autonomous integrated navigation system, wherein the gravity gradient / SINS / starlight autonomous integrated navigation system comprises a gravity gradient / SINS integrated navigation subsystem and a SINS / starlight integrated navigation subsystem; selecting east, north and sky geographic coordinate systems as navigation coordinate systems, and establishing a state equation and a measurement equation of the autonomous integrated navigation system; carrying out filtering solution on the integrated navigation system by adopting a set nonlinear filtering algorithm to obtain a state estimation value and a covariance matrix corresponding to each subsystem; fusing the state estimation values of the subsystems by using a fusion algorithm to obtain a global optimal estimation value; and correcting the error of the integrated navigation system in real time according to the global optimal estimated value. The autonomous integrated navigation system provided by the invention is high in navigation precision, strong in autonomy and good in reliability, and can meet the requirement of a long-distance long-endurance aircraft on the navigation precision.

Description

Technical Field

[0001] The invention relates to the technical field of aircraft navigation, and in particular to a gravity gradient / SINS / starlight autonomous combined navigation method. Background Art

[0002] Autonomous navigation of aircraft means that the aircraft can determine its position, speed, attitude information and navigation parameter solutions independently without relying on ground support, using the measurement equipment carried by the aircraft itself. That is to say, when the aircraft is in an unknown, complex, dynamic unstructured environment, it can reach the desired destination autonomously through environmental perception without human intervention. Autonomous navigation is a new and highly challenging research topic proposed for the development of aircraft. It is an inevitable trend for aircraft navigation technology to develop in the direction of autonomous combined navigation. At present, the methods used for flight navigation mainly include ground radio navigation, inertial navigation, astronomical navigation, satellite navigation, geomagnetic navigation, optical navigation and visual information navigation, etc. These methods each have their own characteristics, scope of application and limitations.

[0003] The inertial navigation system can continuously provide the carrier's position, speed and attitude information. It does not rely on external equipment, does not radiate information to the outside world, and is not affected by external interference factors such as electromagnetics. It has the advantages of simple structure, strong autonomy, good concealment, and all-weather operation. Its disadvantage is that navigation errors accumulate over time. Therefore, pure inertial navigation systems are difficult to meet the needs of high-precision navigation of spacecraft.

[0004] Astronomical navigation uses natural celestial bodies with known ephemeris as navigation beacons, uses optical navigation sensors to image navigation beacons, and uses image processing algorithms to identify and locate navigation beacons. Based on the ephemeris information or feature information of the navigation beacon and the parameters of the optical navigation sensor, high-precision inertial line of sight pointing is provided to determine the carrier's attitude and position. Astronomical navigation does not require the participation of ground radio equipment, has strong autonomy, good security and concealment. Its disadvantages are low data update rate, navigation performance is subject to factors such as the number, distance and space environment of target celestial bodies, and speed cannot be measured directly. In addition, due to the constraints of sensor accuracy and system integration technology, my country's astronomical navigation technology is still immature and cannot be used alone as an independent navigation method.

[0005] Optical navigation is a navigation method that uses airborne imaging technology to determine the orbit of the aircraft and the ephemeris of the target. Optical navigation technology provides direct direction measurement from the aircraft to the target carrier. In general, the attitude determination system of the aircraft can provide the attitude information of the camera, and the target image taken by the optical camera is the optical navigation data. The linearly related image coordinates are obtained. After converting them into pixel coordinates, the required optical observation values ​​can be extracted from them through image processing technology. However, optical navigation images usually contain most of the gray sky or deep celestial surface, the background color is single and dark, and there are only one or two small targets and landmarks. It is difficult to extract the pixel coordinates of the target from the image.

[0006] Radio navigation is a widely used non-autonomous navigation system. Its advantages are that it is not limited by usage time and climatic conditions, the equipment is relatively simple, and the reliability is relatively high. However, the information it outputs is mainly the location of the carrier. Compared with the precise navigation system, its positioning accuracy is not high and it is limited by the coverage area of ​​the ground station. The system operation is related to the radio wave propagation conditions and is easily affected by artificial interference.

[0007] Satellite navigation is the product of the combination of astronomical navigation and radio navigation. Typical satellite navigation systems include the United States' GPS system, Europe's "Galileo" system, and my country's "Beidou" system. Satellite navigation has the advantages of high positioning accuracy and simple use, but its positioning accuracy is heavily dependent on the number and geometric distribution of visible stars. For areas with poor observation environments, the accuracy is greatly reduced due to the small number of visible stars, and the signal is easily interfered or obscured, and is easily interfered with and cannot achieve complete autonomy. Satellite navigation has low anti-interference capabilities and poor tolerance to malicious interference attacks. Once it is damaged or fails, the user's flight safety cannot be guaranteed.

[0008] Geomagnetic navigation developed later than astronomical navigation. This method has the advantages of low cost, passivity, good concealment, and no accumulation of navigation errors over time. However, geomagnetic navigation is easily affected by magnetic storms, and geomagnetic navigation is not effective for high-orbit navigation. Therefore, the effect of geomagnetic navigation alone is not ideal. High-precision and strong autonomous combined navigation technology has always been the goal of research, exploration and pursuit of navigation scholars in the field of national defense. Although some scholars have made many attempts in this regard and proposed many combined navigation methods, there is still a large gap with application needs.

[0009] The key technology to realize integrated navigation is multi-sensor information fusion. As a cutting-edge technology developed in recent decades, multi-sensor information fusion has a very broad application field. This technology comprehensively utilizes the different characteristics of various types of sensors and can obtain information on different attributes of targets in all directions. It not only improves the coverage of autonomous navigation systems in time and space, but also improves the efficiency of using navigation sensor information and increases the credibility of information. In particular, the emergence of various filtering algorithms provides a theoretical basis and mathematical tools for integrated navigation systems.

[0010] At present, the filtering methods used in engineering combined guidance systems mainly include: Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), unscented particle filter (UPF), etc. Although the above filtering algorithms can effectively solve the filtering calculation problems of nonlinear systems, they all require accurate prior knowledge, such as state estimation, measurement error, etc., which cannot be accurately obtained in the navigation system. At the same time, the existing nonlinear filtering algorithms must make a comprehensive trade-off between indicators such as convergence, stability, estimation accuracy and calculation amount according to specific application scenarios and conditions.

[0011] In summary, the existing aircraft navigation does not yet have a combined navigation method that can meet the actual application needs and has good navigation effects. At the same time, the filtering algorithms that are indispensable in the combined navigation method also have their own disadvantages. It is necessary to propose a new nonlinear algorithm to solve the high-precision fusion of autonomous combined navigation. Summary of the invention

[0012] In order to solve the problems existing in the prior art, the present invention provides a gravity gradient / SINS / starlight autonomous combined navigation method, which includes: establishing a gravity gradient / SINS / starlight autonomous combined navigation system, including a gravity gradient / SINS combined navigation subsystem and a SINS / starlight combined navigation subsystem; selecting the east, north, and sky geographic coordinate systems as navigation coordinate systems, and establishing the state equation and measurement equation of the autonomous combined navigation system; using a set nonlinear filtering algorithm to filter and solve the combined navigation system, and obtaining the state estimation value and covariance matrix corresponding to each subsystem; using a fusion algorithm to fuse the state estimation value of each subsystem to obtain a global optimal estimation value; and correcting the error of the combined navigation system in real time according to the global optimal estimation value. The autonomous combined navigation system proposed by the present invention has high navigation accuracy, strong autonomy and good reliability, and can meet the navigation accuracy requirements of long-range and long-endurance aircraft.

[0013] The present invention adopts the following technical scheme, a gravity gradient / SINS / starlight autonomous combined navigation method, comprising:

[0014] Establishing a gravity gradient / SINS / starlight autonomous integrated navigation system, wherein the autonomous integrated navigation system comprises a gravity gradient / SINS integrated navigation subsystem and a SINS / starlight integrated navigation subsystem;

[0015] Selecting the east, north and sky geographic coordinate systems as navigation coordinate systems, and establishing the state equation and measurement equation of the autonomous integrated navigation system;

[0016] Using a set nonlinear filtering algorithm to filter and solve the integrated navigation system, and obtaining a state estimation value and a covariance matrix corresponding to each subsystem;

[0017] The state estimation values ​​of each subsystem are fused using a fusion algorithm to obtain the global optimal estimation value;

[0018] The error of the integrated navigation system is corrected in real time according to the global optimal estimate.

[0019] Furthermore, the state equation of the autonomous integrated navigation system is:

[0020] Selecting the east, north and sky geographic coordinate systems as the navigation coordinate systems, the state variables of the state equation of the autonomous integrated navigation system are:

[0021]

[0022] Among them, δv e ,δv n and δv u are the velocity error components in the east, north and celestial directions respectively, δL, δλ and δH are the position error components in the east, north and celestial directions respectively, and φ e ,φ n and φ u are the aircraft's east, north and celestial attitude errors, ε x ,ε y ,ε z is the gyroscope constant drift, is the accelerometer zero bias;

[0023] The state equation of the gravity gradient / SINS / starlight autonomous integrated navigation system is established as:

[0024]

[0025] Where f(·) is a nonlinear function, X(t) is the state variable of the strapdown inertial navigation system, G(t) is the noise driving matrix of the strapdown inertial navigation system, and w(t) is the noise matrix of the SINS system.

[0026] Further, the measurement equations include: measurement equations of the gravity gradient / SINS integrated navigation subsystem and measurement equations of the gravity gradient / SINS / starlight autonomous integrated navigation system;

[0027] The measurement equations of the gravity gradient / SINS integrated navigation subsystem include:

[0028] The true value of the gravity gradient measured by the gravity gradiometer in the geographic coordinate system is set to Z0, the position information is obtained through SINS solution, and the estimated value of the gravity gradient is obtained by filtering the position information. Subtract the true value from the estimated value as a quantity measurement

[0029]

[0030] Where:

[0031]

[0032] in, is the coordinate transformation matrix from the e system to the n system, Γ e is the gravity gradient tensor matrix in the e system, X n and X e are the positions of the carriers under n-system and e-system, respectively;

[0033] The measurement equations for establishing the gravity gradient / SINS integrated navigation subsystem are:

[0034] Z1(t)=H1X(t)+v1(t)

[0035] Where v1(t) is the system measurement noise, H1=[0 6×6 H n 0 6×6 ] 9×15 is the measurement matrix;

[0036] The measurement equations of the gravity gradient / SINS / starlight autonomous integrated navigation system include:

[0037] Set the carrier attitude angle information output by SNS to The attitude angle information output by the star sensor is The difference between the two is taken as the measurement Z2 of the SINS / starlight integrated navigation subsystem:

[0038]

[0039] In the formula, and γ i is the SINS attitude information, and γ c is the star sensor attitude, and δγ is the SINS attitude error, and Δγ are the star sensor attitude errors;

[0040] The measurement equation for establishing the SINS / starlight integrated navigation subsystem is:

[0041] Z2(t)=H2(t)X(t)+v2(t)

[0042] In the formula, the measurement matrix H2 = [0 3×6 I 3×3 0 3×6 ] 9×15 , v2(t) is the star sensor measurement noise vector.

[0043] Furthermore, the nonlinear function expression is:

[0044]

[0045] In the formula, C ω is the platform error angle matrix, and They are and The calculation error of is the specific force output by the accelerometer, is the specific force error output by the accelerometer, v and δv are velocity and velocity error respectively, δg is the gravity acceleration error, L and H are latitude and altitude respectively, and R M and R N They are the principal curvature radii of the meridian and the meridian respectively.

[0046] Furthermore, the expression of the SINS system noise matrix is:

[0047]

[0048] In the formula, and are the random noise of the gyroscope and accelerometer, respectively.

[0049] Furthermore, a set nonlinear filtering algorithm is used to filter and solve the integrated navigation system, including:

[0050] The nonlinear model of the integrated navigation system is set as:

[0051]

[0052] In the formula, x k ∈R n is the system n-dimensional state vector, y k ∈R mis an m-dimensional measurement vector, f(·) and h(·) are the nonlinear system function and measurement function respectively, W k and V k is an uncorrelated Gaussian white noise sequence;

[0053] The random weighted cubature Kalman filter algorithm is used to filter and solve the nonlinear model. The steps are as follows:

[0054] (1) Initialization:

[0055]

[0056] In the formula, is the estimate of the initial state x0, P0 is the initial covariance;

[0057] (2) Status prediction:

[0058] The random weighted estimate of the state prediction value at time k is

[0059]

[0060] In the formula, v i is a random weighting factor, Calculate the volume point after passing through the state equation;

[0061] The random weighted estimate of the predicted value of the state estimation error covariance at time k is:

[0062]

[0063] In the formula, Q k is the system noise covariance matrix;

[0064] (3) Measurement prediction

[0065] The random weighted estimate of the predicted value of the measurement at time k is:

[0066]

[0067] In the formula, It is the volume point calculated after the measurement equation is transferred Random weighted estimates of ;

[0068] The randomly weighted estimate of the predicted value of the autocorrelation vector covariance matrix is:

[0069]

[0070] In the formula, R k is the measurement noise covariance matrix;

[0071] The randomly weighted estimate of the predicted value of the cross-correlation vector covariance matrix is:

[0072]

[0073] (4) Status update:

[0074] Calculate the filter gain matrix at time k as:

[0075]

[0076] The estimated state value at time k is:

[0077]

[0078] The estimated value of the state error covariance matrix at time k is:

[0079]

[0080] Return to step (2) and repeat to complete the filtering solution of the nonlinear model of the integrated navigation system.

[0081] Furthermore, the state estimation values ​​of each subsystem are fused using a fusion algorithm, specifically:

[0082] According to the obtained state estimation value and covariance matrix corresponding to each subsystem, the main filter is input to establish;

[0083] The main filter uses an information fusion algorithm to obtain a global optimal estimation value of the autonomous integrated navigation system; the global optimal estimation value includes: an optimal state estimation result and a corresponding optimal covariance matrix;

[0084] The fusion algorithm adopted by the main filter is:

[0085]

[0086] Where, the estimated value of the state variable of the gravity gradient / SINS sub-filter is And the covariance matrix is The state variables of the SINS / star photon filter are estimated to be And the covariance matrix is

[0087] The beneficial effects of the present invention are as follows: the present invention proposes a gravity gradient / SINS / starlight combined navigation method, introduces starlight navigation, is used to realize the rapid correction of the aircraft attitude, and organically combines the gravity gradient, SINS and starlight navigation systems to form a gravity gradient / SINS / starlight autonomous combined navigation system, and realizes the correction of the carrier speed, position and attitude at the same time, greatly improving the navigation accuracy of the aircraft; in this system, gravity gradient navigation is a passive autonomous navigation method, does not need to transmit and receive signals externally, is basically not affected by external interference, has the advantages of good concealment, strong real-time performance and high accuracy, and is one of the most suitable navigation methods for auxiliary inertial systems. The present application proposes to use gravity gradient to correct SIN S position error, introduce starlight navigation, for realizing rapid correction of aircraft attitude, and organically combine gravity gradient, SINS and starlight navigation system to form gravity gradient / SINS / starlight autonomous combined navigation system, and realize correction of aircraft speed, position and attitude at the same time; at the same time, the application adopts the proposed random weighted nonlinear filtering algorithm to design gravity gradient / SINS / starlight autonomous combined navigation system, and improves the accuracy of filtering solution of combined navigation system, so that the gravity gradient / SINS / starlight autonomous combined navigation system proposed in the present invention has the advantages of high navigation accuracy, strong autonomy, good reliability, etc., can meet the demand of long-range and long-endurance aircraft for navigation accuracy, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0089] Figure 1 A schematic diagram of the principle of a gravity gradient / SINS / starlight autonomous combined navigation method according to an embodiment of the present invention;

[0090] Figure 2 A schematic diagram of the principle of a gravity gradient / SINS combined navigation subsystem according to an embodiment of the present invention;

[0091] Figure 3 The figure is a schematic diagram of the principle of a SINS / starlight combined navigation subsystem according to an embodiment of the present invention. DETAILED DESCRIPTION

[0092] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0093] In the navigation process of the gravity gradient / SINS / starlight combined navigation system, the state equation of the system is established according to the propagation mechanism of inertial navigation errors. The measurement equation of the system is constructed using the measurement information of the gravity gradiometer, the gravity gradient description function and the star sensor measurement information. Combined with the filtering technology, the position, velocity and attitude errors of the inertial navigation are estimated to achieve the purpose of correcting the navigation error of the aircraft.

[0094] like Figure 1 As shown, it is a schematic diagram of the principle of a gravity gradient / SINS / starlight autonomous combined navigation method according to an embodiment of the present invention. In the embodiment of the present invention, a gravity gradient / SINS sub-filter is first designed to realize the information fusion between the gravity gradient measurement and the inertial navigation, and obtain the state variable estimation value and the covariance matrix containing the position correction information; then, the SINS / starlight sub-filter is used to realize the fusion of the inertial navigation and the starlight measurement information, and obtain the state variable estimation value and the covariance matrix containing the attitude correction information; then, the estimation result of the sub-filter is input into the main filter, and the optimal state estimation result and the corresponding error covariance matrix of the combined navigation system are obtained; finally, the optimal state estimation variable is used to correct the position, velocity, attitude, gyroscope bias and accelerometer bias of the carrier, and the high-precision navigation information of the carrier is obtained. The specific steps are as follows:

[0095] The gravity gradient / SINS / starlight autonomous integrated navigation system is composed of a gravity gradient / SINS integrated navigation subsystem and a SINS / starlight integrated navigation subsystem. The embodiment of the present invention selects the east, north and ground geographic coordinate systems as navigation coordinate systems, establishes the state equation of the autonomous integrated navigation system, and the state variables are:

[0096]

[0097] In the formula, is the platform misalignment angle error; is the velocity error of the carrier in the east, north and ground directions in the navigation coordinate system; δp n =[δLδλδh] T is the latitude, longitude and altitude error of the carrier in the navigation coordinates; is the three-axis bias error of the gyroscope; is the three-axis bias error of the accelerometer.

[0098] From the error model of the strapdown inertial navigation system, we can know that:

[0099]

[0100] In the formula, The attitude matrix from the carrier system to the navigation system; is the projection vector of the earth's rotation angular velocity in the navigation system; is the projection vector of the angular velocity relative to the earth caused by the motion of the carrier in the navigation system, g n is the projection of the gravity vector in the navigation system; is the output of the gyroscope; f b is the specific force output by the accelerometer; R E R is the radius of curvature of the Earth's circumplex; N is the radius of curvature of the Earth's meridian.

[0101] Therefore, the state equation of the gravity gradient / SINS / starlight autonomous integrated navigation system is obtained as follows:

[0102]

[0103] Where f(x, t) is a nonlinear function, G(t) is the system noise driving matrix, and w(t) is the input noise vector, which is defined as follows:

[0104]

[0105] In the formula, and are the random noises of the three axes of the gyroscope; and are the random noises of the three acceleration axes.

[0106] The system state matrix and noise driving matrix are:

[0107]

[0108]

[0109] The measurement equation of the gravity gradient / SINS / starlight autonomous integrated navigation system is composed of the measurement equations of the gravity gradient / SINS and SINS / starlight integrated navigation subsystems. In the embodiment of the present invention, the measurement equation of the gravity gradient / SINS integrated navigation subsystem is first established:

[0110] Assuming that the gravity gradiometer is strapdown mounted on the carrier, the full tensor matrix of the gradiometer measurement output is L b According to the position information solved by SINS, the gravity gradient matrix Γ of the position can be obtained by interpolating in the gravity gradient library. n, which can be converted into the full tensor matrix measured by the gravity gradiometer by the following formula

[0111]

[0112] In the formula, is the conversion matrix from the carrier system to the navigation system, Output of gyroscope The corresponding antisymmetric matrix.

[0113] The measured value L of the gravity gradiometer b and Both are symmetric matrices, containing only 6 independent variables. Subtract the two and take their independent components as the measurement Z1 of the gravity gradient SINS integrated navigation subsystem, that is:

[0114]

[0115] According to the measurement principle of gravity gradiometer:

[0116]

[0117] In the formula, is Γ n Column vector of independent components; is the gyroscope random noise vector; is the transformation matrix from navigation system to carrier system, which can be obtained by Convert to get, It can be obtained by n Convert to get; can be Convert to get;

[0118] The measurement equation of the gravity gradient / SINS integrated navigation subsystem is obtained as follows:

[0119] Z1(t)=δL b =H1(t)X(t)+v1(t)

[0120] Where v1(t) is the gravity gradiometer measurement noise vector, and the measurement matrix H1(t) can be expressed as

[0121]

[0122] Further, the embodiment of the present invention establishes the measurement equation of the SINS / starlight combined navigation subsystem, and the steps are as follows:

[0123] By performing navigation calculation on SINS, the attitude information of the carrier can be output, but it contains accumulated errors and has low accuracy. However, the star sensor can calculate high-precision carrier attitude information by observing multiple stars, and there is no error accumulation. Therefore, the attitude output by the star sensor can be used to correct the attitude of SINS.

[0124] Assume that the attitude information output by SINS is [θφγ] T , the attitude information output by the star sensor is [θ0φ0γ0] T , subtracting the two can get the three-axis attitude error angle δξ. Since the attitude error in the strapdown inertial navigation system is the mathematical platform error angle, the attitude error angle δξ needs to be converted into

[0125] The mathematical platform error angle As the measurement equation Z2 of the SINS / starlight integrated navigation subsystem, the measurement equation of the SINS / starlight integrated navigation subsystem is

[0126] Z2(t)=H2(t)X(t)+v2(t)

[0127] Where v2(t) is the star sensor measurement noise vector, and the measurement matrix H2(t) can be expressed as

[0128] H2(t)=[I 3×3 0 3×12 ]

[0129] According to the measurement equations of the gravity gradient / SINS integrated navigation subsystem and the SINS / starlight integrated navigation subsystem, the measurement equation of the gravity gradient / SINS / starlight autonomous integrated navigation system can be obtained as follows:

[0130] Z(t)=H(t)X(t)+v(t)

[0131] Where v(t) is the measurement noise, and the measurements Z(t) and H(t) are:

[0132]

[0133]

[0134] The estimation accuracy of the cubature Kalman filter (CKF) algorithm can reach the third-order accuracy of Taylor expansion. Compared with the extended Kalman filter (EKF), CKF avoids the linearization of nonlinear system functions in EKF. Therefore, the filtering accuracy of CKF is higher. Compared with the unscented Kalman filter (UKF) that requires 2n+1 sampling points (n is the system state dimension), CKF only uses 2n sampling points, which is relatively reduced by 1 sampling point, and has better numerical stability.

[0135] The disadvantage of CKF is that its filtering performance depends on the setting of its initial value, and it cannot adaptively adjust the weight size of the volume point according to the size of the system error. This will lead to inaccurate estimation of the system state error, resulting in a decrease in filter performance or even divergence.

[0136] In the cubature Kalman filter algorithm, the calculation of the system state prediction value, observation prediction value, state error covariance prediction value, autocorrelation covariance prediction value and cross-correlation covariance prediction value all adopts the arithmetic mean method, that is, a unified weight is adopted, and the magnitude of the prediction value estimation error at different moments cannot be distinguished. This will lead to inaccurate state prediction value, observation prediction value, state error covariance prediction value, autocorrelation covariance prediction value and cross-correlation covariance prediction value, which will in turn affect the accuracy of the filtering calculation.

[0137] In order to overcome the limitations of the volumetric Coleman filter algorithm and enhance the robustness of the algorithm, a random weighted CKF (RWCKF) algorithm is proposed in an embodiment of the present invention. The algorithm selects different weights according to the size of the state prediction, observation prediction, state error covariance prediction, autocorrelation covariance prediction and cross-correlation covariance prediction errors at different times, thereby improving the accuracy of the prediction estimation, and further improving the accuracy of the filtering calculation of the nonlinear system. The information fusion between the gravity gradient measurement information, the starlight attitude measurement information and the inertial navigation is realized by adopting the random weighted volumetric Kalman filter algorithm (RWCKF).

[0138] Firstly, the gravity gradient / SINS sub-filter is designed using the RWCKF algorithm to achieve information fusion between gravity gradient measurement and inertial navigation, and obtain the estimated value of the state variable and covariance matrix

[0139] Then, RWCKF is used to design the SINS / star photon filter, so that the starlight attitude measurement information is integrated with the inertial navigation to obtain the estimated value of the state variable. and covariance matrix

[0140] Finally, the state variables and error covariance estimation results obtained by the gravity gradient / SINS sub-filter and the SINS / starlight sub-filter are input into the main filter to obtain the final state estimation result of the gravity gradient / SINS / starlight autonomous integrated navigation system. and covariance And use The position, velocity, attitude, gyroscope bias and accelerometer bias of the carrier are corrected to obtain high-precision autonomous combined navigation information.

[0141] The information fusion algorithm used by the main filter is:

[0142]

[0143] In the embodiment of the present invention, the specific steps of using the random weighted volumetric Kalman filter algorithm (RWCKF) to achieve information fusion between gravity gradient measurement information, starlight attitude measurement information and inertial navigation are as follows:

[0144] Assume that the nonlinear system model of the integrated navigation is:

[0145]

[0146] In the formula, x k ∈R n is the system n-dimensional state vector, y k ∈R m is an m-dimensional measurement vector, f(·) and h(·) are the nonlinear system function and measurement function respectively, W k and V k are mutually uncorrelated Gaussian white noise sequences that satisfy:

[0147]

[0148] Among them, Q k is a non-negative definite matrix (Q k ≥0), R k is a positive definite matrix (R k >0),δ kj is the Kronecker-δ function.

[0149] Assume that g = p[x k+1 ,q,Q,r,R,z k+1 ] is x k+1 ,q,Q,r,R and z k+1 The joint probability density function is:

[0150] g = p[x k+1 ,q,Q,r,R,z k+1 ]=p[z k+1 |x k+1 ,q,Q,r,R]p[x k+1 |q,Q,r,R]p[q,Q,r,R]

[0151] In the formula, p[q,Q,r,R] can be obtained through prior information, so it can be regarded as a constant, and the following formula can be obtained:

[0152]

[0153] In the formula, and are the maximum a posteriori estimates of noise q, Q, r and R respectively, is the state x j (j=0,1,…,k+1) posterior estimate.

[0154] In the above formula, the embodiment of the present invention uses and replace and get:

[0155]

[0156] The random weighted estimate of the above formula is

[0157]

[0158] Further, assume that the posterior probability density function at time k-1 is Random weighted CKF algorithm steps for nonlinear systems:

[0159] (1) Initialization;

[0160]

[0161] (2) Calculate volume points and time updates;

[0162] The covariance of the state prediction at time k-1 is recorded as P k-1|k-1 , after Cholesky decomposition, P k-1|k-1 It can be expressed as:

[0163]

[0164] The volume points are calculated as follows:

[0165]

[0166] In the formula, x i,k-1k-1 (i=1,2,...,m) is the state quantity of the i-th volume point at time k-1, where S k-1k-1 It is a diagonal matrix; the state quantity of the i-th volume point from k-1 to k is:

[0167]

[0168] (3) State prediction

[0169] The state prediction of the i-th volume point from k-1 to k is:

[0170]

[0171] In the formula, yes is the arithmetic mean estimator of ; accordingly, The randomly weighted estimate of is:

[0172] In the formula, Formula q k-1 The random weighted estimate of the state prediction covariance matrix P k|k-1 can be described as

[0173] In the formula, It's Q k-1 The arithmetic mean estimate of k|k-1 The randomly weighted estimate of is:

[0174] In the formula, It's Q k-1 The randomly weighted estimate of .

[0175] (4) measurement and prediction;

[0176] The quantity measurement of the i-th volume point from k-1 to k is:

[0177]

[0178] The state prediction of the i-th volume point from k-1 to k is:

[0179]

[0180] In the formula, Yes k The arithmetic mean of is estimated; accordingly, The random weighted estimate of can be expressed as:

[0181] In the formula, Yes k A randomly weighted estimate of .

[0182] The autocovariance matrix P of the measurement prediction zz,kk-1 for:

[0183]

[0184] In the formula, YesR k The arithmetic mean estimate of ; correspondingly, the autocovariance matrix P of the measurement prediction zz,kk-1 The random weighted estimate of

[0185]

[0186] In the formula, YesR kA randomly weighted estimate of .

[0187] The cross-covariance matrix P of measurement prediction xz,k|k-1 for

[0188]

[0189] Correspondingly, the cross-covariance matrix P of the measurement prediction xz,k|k-1 The randomly weighted estimate of is:

[0190]

[0191] (5) Status Update

[0192]

[0193] Here K k is the filter gain matrix.

[0194] From the above steps, it can be seen that the proposed adaptive random weighted CKF algorithm inserts random weighting factors into the standard CKF algorithm to simulate the interference caused by system process noise, measurement noise and their covariance matrix, thereby improving the filtering calculation accuracy.

[0195] For the random weighting factor proposed in the embodiment of the present invention, the state estimation and process prediction values ​​at time ki (i = 1, 2, L, n) are assumed to be and According to the definition of residual vector, the state prediction residual vector is:

[0196]

[0197] The measurement residual vector is:

[0198]

[0199] In the formula,

[0200] When the system process noise statistics change, the process prediction The impact on state estimation is reduced, resulting in biased predictions, and the residual error Δx of the process prediction k-i Similarly, when the system measurement noise statistics change, the measurement residual Δz k-i will be biased, and measure the residual Δz k-i It will also get bigger.

[0201] In order to obtain the variation information of the noise, the random weighting factor v is required i The following conditions must be met:

[0202] v i ∞||Δxk-i ||·||Δz k-i || (i=1,2,…,n)

[0203] In the formula, The symbol “∝” indicates proportionality; note:

[0204] w i =||Δx k-i ||·||Δz k-i || (i=1,2,L,n)

[0205] In the formula, w i is the regularization factor.

[0206] The random weighting factor can be determined by the following formula

[0207]

[0208] The random weighted estimation of noise statistics in the embodiment of the present invention can adaptively adjust the weights of volume points, improve the accuracy of state prediction and measurement prediction, further prevent the interference of system process noise and measurement on state estimation, and improve the accuracy of nonlinear system filtering calculation.

[0209] In another specific embodiment of the present invention, a random weighted adaptive Gaussian filtering (RWAGF) method is also proposed. This method overcomes the shortcomings of traditional Gaussian filtering for nonlinear system state estimation by adaptively estimating system noise characteristics. In this method, based on the MAP principle, a theory of random weighted estimation is established to estimate the mean and covariance of system process noise and measurement noise online; then, by adaptively adjusting the weights of dynamic system process noise and measurement noise statistics, the interference of system noise on system state estimation is suppressed, and the accuracy of filtering calculation is improved, which specifically includes:

[0210] Assume that the nonlinear system model of the integrated navigation is

[0211]

[0212] In the formula, x k ∈R n is the system n-dimensional state vector, y k ∈R m is an m-dimensional measurement vector, f(·) and g(·) are the nonlinear system function and measurement function respectively, and w k ∈R n and v k ∈R m are the nonlinear system process noise vector and measurement noise vector respectively, and assume that w k and v k is uncorrelated Gaussian white noise.

[0213]

[0214] In the formula, cov(·) represents the covariance function, A k ≥0 and B k ≥0 is a symmetric matrix, δ kj is the Kronecker-δ function.

[0215] The above formula can be rewritten as:

[0216]

[0217] The randomly weighted estimate of the noise statistic described in the formula is:

[0218]

[0219] Where v j is a random weighting factor, and The steps of random weighted adaptive Gaussian filtering (RWAGF) are as follows:

[0220] (1) Initialization

[0221]

[0222] (2) Calculate the random weighted estimate of the state prediction and its error covariance as

[0223]

[0224] In the formula, (3) Calculate the random weighted estimate of the measurement prediction and its error covariance as:

[0225]

[0226] In the formula,

[0227] (4) Status Update

[0228]

[0229] Return to step (2) and recursively calculate the state estimate at the next moment based on the new observation.

[0230] The above steps indicate that the randomly weighted adaptive Gaussian filtering algorithm proposed in the embodiment of the present invention can adaptively adjust the statistical weights of the process noise and measurement noise of the dynamic system, suppress the interference of the system noise on the state estimation, and thus improve the estimation accuracy.

[0231] In another specific embodiment of the present invention, the present invention also proposes a new random weighted fitting H∞ Filter (RWFH ∞ F) algorithm; This method not only overcomes the existing H ∞ The filter is not applicable to the limitations of nonlinear system filter calculation, and it also overcomes the AFH ∞ In the F algorithm, the statistical characteristics of process noise and measurement noise and their covariance matrix are calculated using arithmetic mean values ​​with common weights, which cannot accurately characterize the actual noise statistical characteristics of the dynamic system, thus leading to the limitation of poor filtering calculation accuracy. In this method, the weights of system noise statistics are adaptively adjusted to estimate the mean and covariance of system process noise and measurement noise online, thereby suppressing the influence of system process noise and measurement noise on state estimation, thereby improving the accuracy of dynamic system filtering calculation. The algorithm includes the following contents:

[0232] Assuming that the system state vector x is an n×1-dimensional vector, the nonlinear system measurement function can be described as follows after linearization:

[0233]

[0234] Where y is an m×1 dimensional vector, is a (n+1)×1 dimensional vector, e is the linearization error, It is an m×(n+1)-order matrix. and The expressions are:

[0235]

[0236]

[0237] Where A is an m×n matrix and b is an m×1 constant vector. By weighted least squares, the linear fitting parameters of γ(x) are It can be determined by the following formula:

[0238]

[0239] Where W is a positive definite weight matrix. It should be noted that it is generally difficult to find The analytical solution of is obtained by fitting transformation (FT) in the embodiment of the present invention. Analytical solution of .

[0240] Suppose the mean and covariance of a set of sample points of the probability distribution of x are x 00 and P 00 , select the Sigma point and weight as:

[0241]

[0242] In the formula, sp[·] is used to generate the sigma point and weight Point Set I n is a 1×2n vector with all elements equal to 1; note:

[0243]

[0244] In the formula,

[0245] here, is a (n+1)×2n-order matrix composed of sample points, E∈R m×2n is the linearization error, Y T The jth column of is:

[0246]

[0247] In the formula, is a vector associated with the sample point, and j=1,L,m.

[0248] Estimating parameters using weighted least squares Make the error Minimize, thereby solving The minimization problem described.

[0249] parameter The estimation of can be described as follows:

[0250]

[0251] Fitting Matrix The estimation formula is as follows:

[0252]

[0253] In the formula That is what is required Analytical solution of .

[0254] Assuming that the nonlinear system model of the integrated navigation is as described in the above formula, according to the nonlinear system model, the discrete fitting function at time k can be expressed as:

[0255]

[0256] The above formula can be rewritten as:

[0257]

[0258] Where e x,k-1 and e y,k-1 are the system state model error and the measurement model error, respectively, and there are:

[0259]

[0260] Assume that the system process noise and measurement noise obey Gaussian distribution, that is, here and are the mean and variance of the system process noise, and are the mean and variance of the system measurement noise, respectively, and they are both bounded.

[0261] The randomly weighted estimate of the system noise statistics is:

[0262]

[0263] Where v j is a random weighting factor, and

[0264] H ∞ The steps of the filtering algorithm are as follows:

[0265] (1) Initialization

[0266]

[0267] (2) Calculate the estimated value of the system state at time k and its covariance matrix

[0268] (3) Using fitting transformation to update state and measurement:

[0269]

[0270] In the formula, the matrix and are the digital fitting matrices of the system state function f(·) and the measurement function h(·), respectively.

[0271] (4) Calculate the attenuation level factor

[0272]

[0273] In the formula, is a positive real matrix, ρ(A) is the largest eigenvalue of matrix A, η k is the coefficient,

[0274] (5) Update the gain matrix

[0275]

[0276] (6) The state at the new k+1 time Quantity and its covariance

[0277]

[0278] Finally, randomly weighted estimates of process noise and measurement noise statistics are constructed:

[0279]

[0280] The state estimate at the next moment can be recursively calculated based on the new observations.

[0281] In the gravity gradient / SINS combined navigation subsystem, SINS uses gyroscopes and accelerometers to measure the linear and angular motion of the carrier, obtain the carrier's angular increment and specific force increment, and then uses the three-dimensional attitude update algorithm, velocity update algorithm and position update algorithm to perform navigation calculations, and can obtain the carrier's real-time three-dimensional attitude information, velocity information and position information. In the SINS system, the drift of the gyroscope will accumulate over time, resulting in increasing navigation errors.

[0282] Gravity gradient / SINS combined navigation uses gravity gradient information to correct the positioning error accumulated over time by inertial navigation. First, the inertial navigation system provides position information and predicts gravity gradient data on the reference map; then, the gravity gradiometer measures the actual gravity gradient measurement value of its own position in real time to obtain a series of real-time gravity gradient data; finally, a gravity gradient / SINS sub-filter is designed to achieve information fusion between gravity gradient measurement and inertial navigation, and obtain state variable estimates and covariance matrices containing position correction information. The optimal estimation point is obtained through a filtering algorithm using real-time data to complete positioning. The principle of the gravity gradient / SINS combined navigation subsystem is as follows: Figure 2 shown.

[0283] In the SINS / starlight integrated navigation subsystem, the embodiment of the present invention utilizes the characteristics of strong autonomy and high attitude determination accuracy of the star sensor, can effectively estimate error terms such as the mathematical platform error and gyro drift of the strapdown inertial navigation system, and perform error correction on the strapdown inertial navigation system in real time, thereby improving the accuracy of the integrated navigation system.

[0284] The SINS navigation system outputs information such as the position, velocity, and attitude of the carrier. Both the SINS and the star sensor are installed on the carrier. After star map recognition and attitude calculation, the star sensor outputs the attitude quaternion of the star sensor coordinate system relative to the inertial coordinate system. Therefore, according to the output of the star sensor, it is easy to obtain the transformation matrix of the inertial coordinate system relative to the star sensor coordinate system:

[0285] First, according to the system error equation of the strapdown inertial navigation system and the star sensor, the system state matrix is ​​reasonably selected to establish the state equation of the strapdown inertial / starlight combined navigation system; then, the carrier position and attitude information output by the strapdown inertial navigation system is used to calculate the transformation matrix of the inertial coordinate system relative to the carrier coordinate system. And calculate the transformation matrix The transformation matrix of the star sensor output Subtract the measurement matrix Z of the integrated navigation system c ;Finally, according to the system state matrix X c and the measurement matrix Z c , the measurement equation of the integrated navigation system is constructed, and thus the strapdown inertial / starlight integrated navigation Kalman filter is designed.

[0286] The measurement matrix Z obtained by the above method is c The data is continuously sent to the integrated navigation Kalman filter for filtering calculation to obtain the optimal estimate of the system state (including the strapdown inertial navigation error). The estimated value is used to correct the error of the strapdown inertial navigation system in real time. Finally, the navigation parameters output by the corrected strapdown inertial navigation system are used as the output of the integrated navigation system. The principle of strapdown inertial / starlight integrated navigation is as follows: Figure 3 shown.

[0287] In summary, in the navigation process of the gravity gradient / SINS / starlight autonomous combined navigation system, the embodiment of the present invention establishes the state equation of the system according to the inertial navigation error propagation mechanism, uses the gravity gradiometer measurement information, the gravity gradient description function and the star sensor measurement information to construct the measurement equation of the system, and combines the filtering technology to estimate the position, velocity and attitude errors of the inertial navigation, so as to achieve the purpose of correcting the navigation error of the aircraft.

[0288] A gravity gradient / SINS sub-filter is designed to realize the information fusion between gravity gradient measurement and inertial navigation, and obtain the state variable estimation value and covariance matrix containing position correction information; then, the SINS / starlight sub-filter is used to realize the fusion of inertial navigation and starlight measurement information, and obtain the state variable estimation value and covariance matrix containing attitude correction information; then, the estimation result of the sub-filter is input into the main filter to obtain the optimal state estimation result and the corresponding error covariance matrix of the integrated navigation system; finally, the optimal state estimation variable is used to correct the position, velocity, attitude, gyroscope bias and accelerometer bias of the carrier.

[0289] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A gravity gradient / SINS / starlight autonomous combined navigation method, characterized in that: include: Establishing a gravity gradient / SINS / starlight autonomous integrated navigation system, wherein the autonomous integrated navigation system comprises a gravity gradient / SINS integrated navigation subsystem and a SINS / starlight integrated navigation subsystem; Selecting the east, north and sky geographic coordinate systems as navigation coordinate systems, and establishing the state equation and measurement equation of the autonomous integrated navigation system; Using a set nonlinear filtering algorithm to filter and solve the integrated navigation system, and obtaining a state estimation value and a covariance matrix corresponding to each subsystem; The state estimation values ​​of each subsystem are fused using a fusion algorithm to obtain the global optimal estimation value; The error of the integrated navigation system is corrected in real time according to the global optimal estimate.

2. The gravity gradient / SINS / starlight autonomous combined navigation method according to claim 1, characterized in that: The state equation of the autonomous integrated navigation system is: Selecting the east, north and sky geographic coordinate systems as the navigation coordinate systems, the state variables of the state equation of the autonomous integrated navigation system are: Among them, δv e ,δv n and δv u are the velocity error components in the east, north and celestial directions respectively, δL, δλ and δH are the position error components in the east, north and celestial directions respectively, and φ e ,φ n and φ u are the aircraft's east, north and celestial attitude errors, ε x ,ε y ,ε z is the gyroscope constant drift, is the accelerometer zero bias; The state equation of the gravity gradient / SINS / starlight autonomous integrated navigation system is established as: Where f(·) is a nonlinear function, X(t) is the state variable of the strapdown inertial navigation system, G(t) is the noise driving matrix of the strapdown inertial navigation system, and w(t) is the noise matrix of the SINS system.

3. The gravity gradient / SINS / starlight autonomous combined navigation method according to claim 1, characterized in that: The measurement equations include: the measurement equations of the gravity gradient / SINS integrated navigation subsystem and the measurement equations of the gravity gradient / SINS / starlight autonomous integrated navigation system; The measurement equations of the gravity gradient / SINS integrated navigation subsystem include: The true value of the gravity gradient measured by the gravity gradiometer in the geographic coordinate system is set to Z0, the position information is obtained through SINS solution, and the estimated value of the gravity gradient is obtained by filtering the position information. Subtract the true value from the estimated value as a quantity measurement Where: in, is the coordinate transformation matrix from the e system to the n system, Γ e is the gravity gradient tensor matrix in the e system, X n and X e are the positions of the carriers under n-system and e-system, respectively; The measurement equations for establishing the gravity gradient / SINS integrated navigation subsystem are: Z1(t)=H1X(t)+v1(t) Where v1(t) is the system measurement noise, H1=[0 6×6 H n 0 6×6 ] 9×15 is the measurement matrix; The measurement equations of the gravity gradient / SINS / starlight autonomous integrated navigation system include: Set the carrier attitude angle information output by SNS to The attitude angle information output by the star sensor is The difference between the two is taken as the measurement Z2 of the SINS / starlight integrated navigation subsystem: In the formula, θ i and γ i is the SINS attitude information, θ c and γ c is the star sensor attitude, δθ and δγ are SINS attitude errors, Δθ and Δγ are the attitude errors of the star sensor; The measurement equation for establishing the SINS / starlight integrated navigation subsystem is: Z2(t)=H2(t)X(t)+v2(t) In the formula, the measurement matrix H2 = [0 3×6 I 3×3 0 3×6 ] 9×15 , v2(t) is the star sensor measurement noise vector.

4. The gravity gradient / SINS / starlight autonomous combined navigation method according to claim 2, characterized in that: The nonlinear function expression is: In the formula, C ω is the platform error angle matrix, and They are and The calculation error of is the specific force output by the accelerometer, is the specific force error output by the accelerometer, v and δv are velocity and velocity error respectively, δg is the gravity acceleration error, L and H are latitude and altitude respectively, and R M and R N They are the principal curvature radii of the meridian and the meridian respectively.

5. The gravity gradient / SINS / starlight autonomous combined navigation method according to claim 2, characterized in that: The expression of the SINS system noise matrix is: In the formula, and are the random noise of the gyroscope and accelerometer, respectively.

6. The gravity gradient / SINS / starlight autonomous integrated navigation method according to claim 1, characterized in that: The integrated navigation system is filtered and solved using a set nonlinear filtering algorithm, including: The nonlinear model of the integrated navigation system is set as: In the formula, x k ∈R n is the system n-dimensional state vector, y k ∈R m is an m-dimensional measurement vector, f(·) and h(·) are the nonlinear system function and measurement function respectively, W k and V k is an uncorrelated Gaussian white noise sequence; The random weighted cubature Kalman filter algorithm is used to filter and solve the nonlinear model. The steps are as follows: (1) Initialization: In the formula, is the estimate of the initial state x0, P0 is the initial covariance; (2) Status prediction: The random weighted estimate of the state prediction value at time k is In the formula, v i is a random weighting factor, Calculate the volume point after passing through the state equation; The random weighted estimate of the predicted value of the state estimation error covariance at time k is: In the formula, Q k is the system noise covariance matrix; (3) Measurement prediction The random weighted estimate of the predicted value of the measurement at time k is: In the formula, It is the volume point calculated after the measurement equation is transferred Random weighted estimates of ; The randomly weighted estimate of the predicted value of the autocorrelation vector covariance matrix is: In the formula, R k is the measurement noise covariance matrix; The randomly weighted estimate of the predicted value of the cross-correlation vector covariance matrix is: (4) Status update: Calculate the filter gain matrix at time k as: The estimated state value at time k is: The estimated value of the state error covariance matrix at time k is: Return to step (2) and repeat to complete the filtering solution of the nonlinear model of the integrated navigation system.

7. The gravity gradient / SINS / starlight autonomous integrated navigation method according to claim 1, characterized in that: The state estimation values ​​of each subsystem are fused using the fusion algorithm, specifically: According to the obtained state estimation value and covariance matrix corresponding to each subsystem, the main filter is input to establish; The main filter uses an information fusion algorithm to obtain a global optimal estimation value of the autonomous integrated navigation system; the global optimal estimation value includes: an optimal state estimation result and a corresponding optimal covariance matrix; The fusion algorithm adopted by the main filter is: Where, the estimated value of the state variable of the gravity gradient / SINS sub-filter is And the covariance matrix is The state variables of the SINS / star photon filter are estimated to be And the covariance matrix is

Images