A fully autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system

Abstract

The application discloses a kind of fully autonomous high-precision strapdown inertial navigation system gravity disturbance compensation methods, by analyzing the internal coupling between gravity disturbance, gravity disturbance gradient and gravity disturbance change rate, gravity disturbance and disturbance gradient state model that can accurately reflect the change law of gravity disturbance is established;And design a kind of gravity disturbance unaffected Doppler velocity meter-gyroscope autonomous navigation scheme, utilize the output speed, position and attitude navigation parameters and the corresponding navigation parameters of strapdown inertial navigation system output are matched comprehensively, so as to realize low cost, fully autonomous and high-precision gravity disturbance compensation.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of navigation technology, and more particularly to a full-autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system. BACKGROUND

[0002] The strapdown inertial navigation system has the advantages of strong autonomy, good concealment, complete navigation parameters, high short-time precision, high data output rate, etc., and is therefore widely used in the navigation tasks of various carriers such as spacecraft, aircraft, ships and vehicles. The traditional strapdown inertial navigation system usually assumes that the earth is an ideal rotating ellipsoid, and combines an ideal gravity model to compensate for the specific force information measured by the accelerometer, thereby obtaining the motion acceleration of the carrier. However, in reality, due to the irregular shape and uneven mass distribution of the earth, there will inevitably be differences between the actual gravity and the ideal gravity at any location, i.e., there is gravity disturbance. The gravity disturbance will cause errors in the calculation of the motion acceleration by the strapdown inertial navigation system, and further cause navigation errors such as velocity, position and attitude that accumulate over time. Therefore, gravity disturbance has become a major error source affecting the performance of long-time high-precision strapdown inertial navigation systems.

[0003] The existing gravity disturbance compensation methods mainly include the following four kinds: ① based on global gravity field spherical harmonic model, ② based on regional gravity grid data, ③ based on gravity gradiometer and ④ based on differential satellite navigation system. The first method inputs the position information calculated by the strapdown inertial navigation system into a pre-built global gravity field spherical harmonic model to calculate the gravity disturbance and then implement compensation. However, since the global gravity field spherical harmonic model cannot accurately describe the gravity disturbance in local areas, the compensation accuracy of this method is low in areas such as mountains where the gravity disturbance changes significantly. The second method reads the gravity grid data near the position calculated by the strapdown inertial navigation system, and then calculates the gravity disturbance at the current position using an interpolation algorithm. However, this method cannot be used in areas not covered by the regional gravity grid data, so it cannot meet the application requirements of long-time moving carriers. In addition, since the first two methods both need to use the position information calculated by the strapdown inertial navigation system, the gravity disturbance compensation accuracy of both methods will decrease with the accumulation of inertial navigation position errors. The third method combines a gravity gradiometer with a strapdown inertial navigation system to form a integrated navigation system, and then uses the high-resolution gravity gradient tensor measured by the gravity gradiometer in real time as the measurement information to estimate and compensate for the gravity disturbance through an optimal estimation algorithm. However, since the manufacturing process of the gravity gradiometer is complex, the cost is high, and the volume is large, this compensation method is costly and not suitable for light and small carriers. The fourth method obtains high-precision position, velocity and acceleration information through a differential satellite navigation receiver, and then estimates and compensates for the gravity disturbance using an optimal estimation algorithm. However, since satellite navigation signals are easily disturbed and blocked, the differential satellite navigation system needs to be additionally arranged with reference stations, and the action distance of the reference stations is limited, so the autonomy and reliability of this method cannot be guaranteed.

[0004] Therefore, how to provide a high-precision, low-cost and autonomous strong strapdown inertial navigation system gravity disturbance compensation method is a problem to be solved by those skilled in the art. SUMMARY

[0005] Therefore, the application provides a full autonomous high-precision strapdown inertial navigation system gravity disturbance compensation method, which analyzes the internal coupling relationship among gravity disturbance, gravity disturbance gradient and gravity disturbance change rate, establishes a state model capable of accurately reflecting the gravity disturbance change law, and designs a Doppler velocimeter- gyroscope autonomous navigation scheme not affected by gravity disturbance, uses the output speed, position and attitude navigation parameters and the corresponding navigation parameters output by the strapdown inertial navigation system for comprehensive matching, so as to realize low-cost, full-autonomous and high-precision gravity disturbance compensation.

[0006] In order to achieve the above-mentioned purpose, the application adopts the following technical scheme:

[0007] A full autonomous high-precision strapdown inertial navigation system gravity disturbance compensation method comprises the following steps:

[0008] S1: according to the internal relationship among gravity disturbance potential, gravity disturbance and gravity disturbance gradient in the earth coordinate system, the internal coupling relationship among gravity disturbance, gravity disturbance gradient and gravity disturbance change rate in the geographical coordinate system is established;

[0009] S2: based on the single-value continuity of gravity disturbance potential and the conservation of gravity disturbance field, a constraint equation of gravity disturbance gradient component is constructed, and a random process is used to describe the mutually independent gravity disturbance gradient components to construct a gravity disturbance and disturbance gradient state model;

[0010] S3: Doppler velocimeter- gyroscope navigation calculation is performed according to the speed measured by the Doppler velocimeter and the angular velocity measured by the gyroscope, so as to obtain navigation parameters irrelevant to gravity disturbance;

[0011] S4: the navigation parameters related to gravity disturbance output by the strapdown inertial navigation system navigation calculation and the navigation parameters irrelevant to gravity disturbance are used for parameter matching to obtain matching parameters;

[0012] S5: according to the gravity disturbance and disturbance gradient state model, the gravity disturbance component and the gravity disturbance gradient component are one-step predicted in the state estimation filter; the matching parameters are used for measurement updating of the one-step prediction result to obtain a final prediction result;

[0013] S6: the final prediction result of the gravity disturbance component is used to correct the navigation calculation process of the strapdown inertial navigation system.

[0014] Preferably, the construction process of the inner coupling relationship in S1 comprises:

[0015] S11: obtaining the inner relationship of the gravity disturbance potential, the gravity disturbance and the gravity disturbance gradient in the earth coordinate system:

[0016]

[0017]

[0018] In the formula, T represents the gravity disturbance potential; e represents the earth coordinate system, r represents the vector from the center of the earth to the position of the carrier; δg e represents the gravity disturbance in the earth coordinate system e, δΓ e represents the gravity disturbance gradient in the earth coordinate system e;

[0019] S12: projecting the gravity disturbance δg e in the earth coordinate system e and the gravity disturbance gradient δΓ e in the earth coordinate system e to the geographic coordinate system n, to obtain the gravity disturbance δg n in the geographic coordinate system n and the gravity disturbance gradient δΓ n in the geographic coordinate system n:

[0020]

[0021]

[0022] In the formula, e represents the earth coordinate system; n represents the geographic coordinate system; represents the direction cosine matrix of the earth coordinate system e relative to the geographic coordinate system n; δg n represents the gravity disturbance in the geographic coordinate system n; δΓ n represents the gravity disturbance gradient in the geographic coordinate system n;

[0023] S13: time derivation is performed on the gravity disturbance δg n in the geographic coordinate system n to obtain the gravity disturbance rate δg

[0024]

[0025] In the formula, represents the corresponding anti-symmetric matrix, represents the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n, which is an ideal value;

[0026] At the same time, the gravity disturbance rate δg in the earth coordinate system e and the gravity disturbance gradient δΓe and the carrier velocity v in the earth coordinate system e e satisfy the following relationship:

[0027]

[0028] wherein v e represents the carrier velocity in the earth coordinate system e; represents the rate of change of the gravity disturbance in the earth coordinate system e;

[0029] Substituting equation (6) and equation (4) into equation (5) obtains the intrinsic coupling relationship in the geographic coordinate system:

[0030]

[0031] wherein v n represents the carrier velocity in the geographic coordinate system n; δΓ n represents the gravity disturbance gradient in the geographic coordinate system n; δg n represents the gravity disturbance in the geographic coordinate system n; represents the corresponding anti-symmetric matrix, represents the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n;

[0032] wherein the gravity disturbance gradient δΓ n Specifically:

[0033]

[0034] δΓ EE represents the second-order partial derivative of the gravity disturbance potential T with respect to the east direction E; δΓ EN represents the partial derivative of the gravity disturbance potential T with respect to the east direction E first and then with respect to the north direction N; δΓ EU represents the partial derivative of the gravity disturbance potential T with respect to the east direction E first and then with respect to the sky direction U; δΓ NE represents the partial derivative of the gravity disturbance potential T with respect to the north direction N first and then with respect to the east direction E; δΓ NN represents the second-order partial derivative of the gravity disturbance potential T with respect to the north direction N; δΓ NU represents the partial derivative of the gravity disturbance potential T with respect to the north direction N first and then with respect to the sky direction U; δΓ UE represents the partial derivative of the gravity disturbance potential T with respect to the sky direction U first and then with respect to the east direction E; δΓ UN represents the partial derivative of the gravity disturbance potential T with respect to the sky direction U first and then with respect to the north direction N; δΓ UU represents the second-order partial derivative of the gravity disturbance potential T with respect to the sky direction U.

[0035] Preferably, the construction process of the gravity disturbance and disturbance gradient state model in S2 comprises:

[0036] The gravity disturbance gradient δΓ in the geographical coordinate system n is constructed according to the single-value continuity of the gravity disturbance potential n The constraint equation is:

[0037]

[0038] The Laplace equation of the gravity disturbance potential T is constructed according to the conservation of the gravity disturbance field:

[0039]

[0040] In the formula, δg represents the Laplace operator;

[0041] Substitute formula (8) and formula (9) into formula (7) to obtain:

[0042]

[0043] In the formula, δg represents the gravity disturbance rate of change in the geographical coordinate system n in the east, north and sky directions; δΓ EE represents the second-order partial derivative of the gravity disturbance potential T with respect to the east direction E; δΓ EN represents the partial derivative of the gravity disturbance potential T with respect to the east direction E, and then the partial derivative with respect to the north direction N; δΓ EU represents the partial derivative of the gravity disturbance potential T with respect to the east direction E, and then the partial derivative with respect to the sky direction U; δΓ NN represents the second-order partial derivative of the gravity disturbance potential T with respect to the north direction N; δΓ NU represents the partial derivative of the gravity disturbance potential T with respect to the north direction N, and then the partial derivative with respect to the sky direction U; δΓ E , δg N , δg U represents the gravity disturbance δg n in the east, north and sky directions in the geographical coordinate system n; v E , v N , v U represents the carrier velocity v n in the east, north and sky directions in the geographical coordinate system n, ω enE , ω enN , ω enU represents the angular velocity in the east, north and sky directions, represents the projection of the angular velocity of the geographical coordinate system n relative to the earth coordinate system e in the geographical coordinate system n.

[0044] The gravity disturbance gradient component δΓ is calculated by using a first-order Markov processk The description is obtained:

[0045]

[0046] In the formula, d k represents the relevant distance; δΓ k includes δΓ EE , δΓ EN , δΓ EU , δΓ NN , δΓ NU ; represents the change rate of the gravity disturbance gradient component δΓ k ; w δΓ,k represents the excitation white noise; v represents the module of the carrier motion speed v n ;

[0047] Equations (10) and (11) are the constructed gravity disturbance and disturbance gradient state models.

[0048] Preferably, the navigation parameters irrelevant to the gravity disturbance in S3 include: a velocity parameter irrelevant to the gravity disturbance a position parameter irrelevant to the gravity disturbance and an attitude matrix irrelevant to the gravity disturbance

[0049] The navigation solving process of the navigation parameters irrelevant to the gravity disturbance includes the following steps:

[0050] S31: converting the velocity measured by the Doppler velocimeter into the geographic coordinate system n by using the installation matrix and the attitude matrix , to obtain a velocity parameter irrelevant to the gravity disturbance

[0051]

[0052] S32: performing integral calculation on the velocity parameter irrelevant to the gravity disturbance to obtain a position parameter irrelevant to the gravity disturbance

[0053]

[0054] S33: updating the angular velocity and the angular velocity by using the velocity parameter irrelevant to the gravity disturbance and the position parameter irrelevant to the gravity disturbance :

[0055]

[0056]

[0057] The updated angular velocity and the angular velocity are projected to the inertial device measurement coordinate system b, and are combined with the angular velocity The updated attitude angular velocity

[0058]

[0059] The attitude angular velocity is used to update the attitude matrix in real time to obtain the updated attitude matrix

[0060]

[0061] wherein, represents the velocity measured by the Doppler velocity sensor, D represents the Doppler velocity sensor, and m represents the Doppler velocity sensor measurement coordinate system; represents the projection of the angular velocity of the inertial device measurement coordinate system b relative to the inertial coordinate system i in the inertial device measurement coordinate system b, that is, the angular velocity measured by the gyroscope, i represents the inertial coordinate system, and b represents the inertial device measurement coordinate system; represents the projection of the velocity in the geographic coordinate system n, L represents the Doppler velocity sensor-gyroscope navigation, and n represents the geographic coordinate system; represents the projection of the position in the geographic coordinate system n, L represents the Doppler velocity sensor-gyroscope navigation, and n represents the geographic coordinate system; represents the attitude matrix of the inertial device measurement coordinate system b relative to the geographic coordinate system n, b represents the inertial device measurement coordinate system, L represents the Doppler velocity sensor-gyroscope navigation, and n represents the geographic coordinate system; and respectively represent the initial latitude, longitude and altitude, which are consistent with the initial position parameters of the strapdown inertial navigation system; R M and R N respectively represent the meridian radius and the prime vertical radius; represents the velocity in the east, north and sky directions; represents the latitude, longitude and altitude of the Doppler velocity sensor-gyroscope navigation; represents the projection of the angular velocity of the earth coordinate system e relative to the inertial coordinate system i in the geographic coordinate system n; represents the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n; ω ie is the earth rotation angular rate, which is a constant; This represents the projection of the angular velocity of the inertial device measurement coordinate system b relative to the geographic coordinate system n onto the inertial device measurement coordinate system b. This represents the initial attitude matrix; Indicates attitude angular velocity The corresponding antisymmetric matrix.

[0062] Preferably, the navigation parameters related to gravity disturbances mentioned in S4 include: velocity parameters related to gravity disturbances. Location parameters related to gravity disturbance and attitude matrix related to gravity perturbation

[0063] The matching parameters include: velocity matching amount Z. vel Location matching quantity Z pos and attitude matching quantity Z att ;

[0064] The parameter matching process includes the following steps:

[0065] Velocity parameters related to gravitational disturbances Subtract the velocity parameters that are independent of gravitational disturbances Obtain the velocity matching quantity Z vel :

[0066]

[0067] Location parameters related to gravity disturbance Subtract position parameters that are independent of gravitational perturbation Get the position matching quantity Z pos :

[0068]

[0069] Using attitude matrix related to gravity perturbation and attitude matrix independent of gravity perturbation Calculate the direction cosine matrix between the strapdown inertial navigation system's calculated navigation frame n′ and the Doppler velocimeter-gyroscope navigation system's calculated navigation frame n″:

[0070]

[0071] matrix The element is taken as the pose matching quantity Z. att :

[0072]

[0073] Where n′ represents the strapdown inertial navigation system (SINS) calculated navigation system; n″ represents the Doppler velocimeter-gyroscope navigation system calculated navigation system; denotes the attitude matrix independent of gravity disturbance; denotes the attitude matrix dependent on gravity disturbance; p,q denotes the p-th row, q-th column element of matrix .

[0074] Preferably, the step of predicting the gravity disturbance component and the gravity disturbance gradient component in S5 comprises the following steps:

[0075] S51: defining the state vector X of the state estimation filter:

[0076]

[0077] wherein δg E , δg N , δg U denote the gravity disturbance δg n in the east, north, and up directions in the geographic coordinate system n; δg EE denotes the second order partial derivative of the gravity disturbance potential T with respect to the east direction E; δg EN denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E first and then with respect to the north direction N; δg EU denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E first and then with respect to the up direction U; δg NN denotes the second order partial derivative of the gravity disturbance potential T with respect to the north direction N; δg NU denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N first and then with respect to the up direction U; δv E,S , δv N,S , δv U,S and δL S , δλ S , δh S are the attitude misalignment angle, the velocity error, and the position error of the strapdown inertial navigation system, respectively; and δL L , δλ L , δh L are the attitude misalignment angle and the position error of the Doppler-aided gyro navigation system, respectively; ε x , ε y , ε z and are the constant drift of the gyro and the bias of the accelerometer, respectively; δa x , δa z are the installation error angles of the Doppler velocity sensor relative to the inertial devices; δK D is the scale factor error of the Doppler velocity sensor;

[0078] S52: Establishing the state equation according to formula (10), formula (11), the error model of the strapdown inertial navigation system and the error model of the Doppler-velocity- gyroscope navigation system as follows:

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085]

[0086]

[0087]

[0088] In the formula, represents the rate of change of the state vector X; W is the process noise, A is the system matrix and G is the process noise driving matrix; w δΓ,EE ,w δΓ,EN ,w δΓ,EU ,w δΓ,NN ,w δΓ,NU represents the gravity disturbance gradient component δΓ EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NU corresponding to the excitation white noise; w ε,x ,w ε,y ,w ε,z and are the gyro random drift and the accelerometer random noise respectively; A3 and G2 can be determined according to the error model of the strapdown inertial navigation system and the Doppler-velocity- gyroscope navigation system; d EE , d EN , d EU , d NN , d NU : represents the gravity disturbance gradient component δΓ EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NU corresponding to the correlation distance; v represents the carrier velocity v nThe modulus; diag represents a diagonal matrix, and the variables in [] are the diagonal elements of the diagonal matrix; 0 m1×m2 I represents a zero matrix with m1 rows and m2 columns; n1×n2 This represents an identity matrix with n1 rows and n2 columns.

[0089] The preferred measurement update formula is:

[0090] Z(t)=H(t)X(t)+V(t) (24)

[0091] In the formula, Let V be the measurement vector, V be the measurement noise vector, and H be the measurement matrix.

[0092]

[0093]

[0094]

[0095] Among them, c p,q (p,q=1,2,3) represents the attitude matrix The element in the p-th row and q-th column.

[0096] Preferably, the correction process includes:

[0097] Acceleration of strapdown inertial navigation system The solution process has been corrected:

[0098]

[0099]

[0100]

[0101] In the formula, Indicates the latitude and altitude of the strapdown inertial navigation system; Indicates the speed in the strapdown inertial navigation system The components in the east and north directions; This indicates the specific force measured by the accelerometer; It is the ideal gravity calculated based on the ideal gravity model; Represents velocity parameters related to gravitational disturbances; and These represent the Earth's angular velocity and displacement angular velocity calculated by the strapdown inertial navigation system, respectively. This represents the final predicted result of the gravity disturbance.

[0102] Compared with the prior art, the application provides a full-autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system.

[0103] (1) The gravity disturbance and disturbance gradient state model constructed by mining the internal relationship of the gravity disturbance potential, gravity disturbance and gravity disturbance gradient in the space domain, and fully utilizing the single-value continuity of the gravity disturbance potential and the conservation of the gravity disturbance field can accurately reflect the actual change rule of the gravity disturbance. Based on the gravity disturbance and disturbance gradient state model, the gravity disturbance, gravity disturbance gradient and other states can be accurately predicted in one step;

[0104] (2) In the Doppler velocimeter- gyroscope navigation system designed by the application, the speed, position and attitude parameters irrelevant to the gravity disturbance can be calculated by using the high-precision speed information measured by the Doppler velocimeter and the angular velocity information measured by the gyroscope. Then, the speed, position and attitude parameters irrelevant to the gravity disturbance and the speed, position and attitude parameters relevant to the gravity disturbance output by the strapdown inertial navigation system are comprehensively matched in speed, position and attitude. Then, the speed matching quantity, position matching quantity and attitude matching quantity are used to accurately predict the gravity disturbance and the error states of each sensor in the measurement update process, so that the gravity disturbance, gravity disturbance gradient and error states of each sensor can be accurately predicted in the measurement update process;

[0105] (3) The application establishes a high-precision gravity disturbance and disturbance gradient state model, and uses the high-precision, low-cost and autonomous Doppler velocimeter and strapdown inertial navigation system for information fusion, and then accurately estimates and compensates the gravity disturbance by using the optimal estimation algorithm. That is, the method designed by the application has the advantages of full autonomy, low cost and high precision. BRIEF DESCRIPTION OF DRAWINGS

[0106] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description are only embodiments of the application, and for those skilled in the art, other drawings can be obtained without creative labor on the basis of the provided drawings.

[0107] Fig. 1 A schematic diagram of a full-autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system provided by the application;

[0108] Fig. 2 An operation flowchart of the method provided by the application;

[0109] Fig. 3 A principle block diagram of a Doppler velocimeter- gyroscope navigation system. DETAILED DESCRIPTION

[0110] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative effort belong to the scope of protection of the present application.

[0111] As shown in Figs. 1-3 , the embodiments of the present application disclose a fully autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system, comprising the following steps:

[0112] S1: establishing an internal coupling relationship between gravity disturbance, gravity disturbance gradient and gravity disturbance rate in a geographical coordinate system according to the internal relationship of gravity disturbance potential, gravity disturbance and gravity disturbance gradient in an earth coordinate system;

[0113] In an embodiment, the construction process of the internal coupling relationship in S1 comprises:

[0114] S11: obtaining the internal relationship of gravity disturbance potential, gravity disturbance and gravity disturbance gradient in an earth coordinate system:

[0115]

[0116]

[0117] In the formula, T represents gravity disturbance potential; e represents an earth coordinate system, r represents a vector pointing to the center of the earth and the position of the carrier; δg e represents gravity disturbance in the earth coordinate system e, δΓ e represents gravity disturbance gradient in the earth coordinate system e;

[0118] S12: projecting gravity disturbance δg e in the earth coordinate system e and gravity disturbance gradient δΓ e in the earth coordinate system e to a geographical coordinate system n to obtain gravity disturbance δg n in the geographical coordinate system n and gravity disturbance gradient δΓ n in the geographical coordinate system n:

[0119]

[0120]

[0121] In the formula, e represents an earth coordinate system; n represents a geographical coordinate system; represents a direction cosine matrix of the earth coordinate system e relative to the geographical coordinate system n; δgn denotes the gravity disturbance in the geographic coordinate system n; δg n denotes the gravity disturbance gradient in the geographic coordinate system n; δg

[0122] S13: the gravity disturbance δg n in the geographic coordinate system n is obtained by time derivation of the gravity disturbance δg

[0123]

[0124] wherein, denotes the corresponding anti-symmetric matrix, denotes the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n, which is an ideal value;

[0125] At the same time, the gravity disturbance rate of change δg the gravity disturbance gradient δg e in the earth coordinate system e and the carrier velocity v e satisfy the following relationship:

[0126]

[0127] wherein, v e denotes the carrier velocity in the earth coordinate system e; denotes the gravity disturbance rate of change in the earth coordinate system e;

[0128] Substituting equation (6) and equation (4) into equation (5) to obtain the intrinsic coupling relationship in the geographic coordinate system:

[0129]

[0130] wherein, v n denotes the carrier velocity in the geographic coordinate system n; δg n denotes the gravity disturbance gradient in the geographic coordinate system n; δg n denotes the gravity disturbance in the geographic coordinate system n; denotes the corresponding anti-symmetric matrix, denotes the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n;

[0131] wherein, the gravity disturbance gradient δg n Specifically,

[0132]

[0133] δg EEdenotes the second-order partial derivative of the gravity disturbance potential T with respect to the eastward direction E; δΓ EN denotes the partial derivative of the gravity disturbance potential T with respect to the eastward direction E and then with respect to the northward direction N; δΓ EU denotes the partial derivative of the gravity disturbance potential T with respect to the eastward direction E and then with respect to the upward direction U; δΓ NE denotes the partial derivative of the gravity disturbance potential T with respect to the northward direction N and then with respect to the eastward direction E; δΓ NN denotes the second-order partial derivative of the gravity disturbance potential T with respect to the northward direction N; δΓ NU denotes the partial derivative of the gravity disturbance potential T with respect to the northward direction N and then with respect to the upward direction U; δΓ UE denotes the partial derivative of the gravity disturbance potential T with respect to the upward direction U and then with respect to the eastward direction E; δΓ UN denotes the partial derivative of the gravity disturbance potential T with respect to the upward direction U and then with respect to the northward direction N; δΓ UU denotes the second-order partial derivative of the gravity disturbance potential T with respect to the upward direction U.

[0134] S2: constructing constraint equations of the gravity disturbance gradient components based on the single-value continuity of the gravity disturbance potential and the conservation of the gravity disturbance field and describing the mutually independent gravity disturbance gradient components by using a random process to construct a gravity disturbance and disturbance gradient state model;

[0135] In an embodiment, the constructing process of the gravity disturbance and disturbance gradient state model in S2 comprises:

[0136] constructing constraint equations of the gravity disturbance gradient δΓ n in the geographic coordinate system n according to the single-value continuity of the gravity disturbance potential T:

[0137]

[0138] Since the gravity disturbance potential T has the single-value continuity, its second-order partial derivatives are independent of the derivative order. That is, the gravity disturbance gradient δΓ n is a symmetric matrix, so the non-diagonal elements in the matrix satisfy the constraint of formula (8).

[0139] constructing the Laplace equation of the gravity disturbance potential T according to the conservation of the gravity disturbance field:

[0140]

[0141] In formula (9), Laplacian denotes the Laplace operator;

[0142] According to formula (8) and formula (9), the gravity disturbance gradient δΓ nThe nine components in satisfy four constraint conditions, so only five of the nine components are independent. Substituting equation (8) and equation (9) into equation (7) and eliminating the non-independent components in the gravity disturbance gradient yields:

[0143]

[0144] wherein denotes the rate of change of the gravity disturbance in the geographic coordinate system n denotes the component in the east, north, and up directions; δg EE denotes the second-order partial derivative of the gravity disturbance potential T with respect to the east direction E; δg EN denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the north direction N; δg EU denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the up direction U; δg NN denotes the second-order partial derivative of the gravity disturbance potential T with respect to the north direction N; δg NU denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the up direction U; the subscripts “E”, “N”, and “U” respectively denote the east direction, the north direction, and the up direction; δg E ,δg N ,δg U denotes the gravity disturbance δg in the geographic coordinate system n n denotes the component in the east, north, and up directions; v E ,v N ,v U denotes the carrier velocity v in the geographic coordinate system n n denotes the component in the east, north, and up directions, ω enE ,ω enN ,ω enU denotes the angular velocity denotes the component in the east, north, and up directions, denotes the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n.

[0145] To use equation (10) as the state space model of the gravity disturbance, the gravity disturbance gradient components δg EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NU need to be modeled. Since the gravity disturbance is mainly caused by the uneven distribution of the earth's mass and the uneven terrain, the gravity disturbance gradient at adjacent positions is correlated. In addition, the gravity disturbance is also affected by tides, earthquakes, glaciers, and other complex factors, and thus exhibits a certain randomness. Therefore, the independent components of the gravity disturbance gradient can be regarded as independent random processes in three-dimensional space.

[0146] In this embodiment, the gravity disturbance gradient component δΓ k is described, and the following is obtained:

[0147]

[0148] In the formula, d k represents the correlation distance; δΓ k includes δΓ EE , δΓ EN , δΓ EU , δΓ NN , δΓ NU ; represents the rate of change of the gravity disturbance gradient component δΓ k ; w δΓ,k represents the exciting white noise; v represents the module of the carrier motion velocity v n ;

[0149] The formula (10) and the formula (11) are the constructed gravity disturbance and disturbance gradient state model.

[0150] S3: Doppler velocity meter- gyroscope navigation solution is performed according to the velocity measured by the Doppler velocity meter and the angular velocity measured by the gyroscope, and the navigation parameters irrelevant to the gravity disturbance are obtained;

[0151] In an embodiment, the navigation parameters irrelevant to the gravity disturbance in S3 include: the velocity parameters irrelevant to the gravity disturbance the position parameters irrelevant to the gravity disturbance and the attitude matrix irrelevant to the gravity disturbance

[0152] The navigation solution process of the navigation parameters irrelevant to the gravity disturbance includes the following steps:

[0153] S31: the velocity measured by the Doppler velocity meter is converted to the geographic coordinate system n by using the installation matrix and the attitude matrix , and the velocity parameters irrelevant to the gravity disturbance are obtained.

[0154]

[0155] S32: the velocity parameters irrelevant to the gravity disturbance are integrated to obtain the position parameters irrelevant to the gravity disturbance

[0156]

[0157] S33: updating the velocity parameter independent of the gravity disturbance and the position parameter independent of the gravity disturbance the angular velocity and the angular velocity

[0158]

[0159]

[0160] and the angular velocity

[0161]

[0162]

[0163]

[0164] wherein, denotes the velocity measured by the Doppler velocity sensor, D denotes the Doppler velocity sensor, and m denotes the Doppler velocity sensor measurement coordinate system; denotes the projection of the angular velocity of the inertial device measurement coordinate system b relative to the inertial coordinate system i in the inertial device measurement coordinate system b, i.e. the angular velocity measured by the gyroscope, i denotes the inertial coordinate system, and b denotes the inertial device measurement coordinate system; denotes the projection of the velocity in the geographic coordinate system n, L denotes the Doppler velocity sensor-gyroscope navigation, and n denotes the geographic coordinate system; denotes the projection of the position in the geographic coordinate system n, L denotes the Doppler velocity sensor-gyroscope navigation, and n denotes the geographic coordinate system; denotes the attitude matrix of the inertial device measurement coordinate system b relative to the geographic coordinate system n, b denotes the inertial device measurement coordinate system, L denotes the Doppler velocity sensor-gyroscope navigation, and n denotes the geographic coordinate system; and denote the initial latitude, longitude and altitude, respectively, which are consistent with the initial position parameters of the strapdown inertial navigation system; R M and R N denote the meridian radius and the prime vertical radius, respectively; denotes the velocity in the east, north and sky directions; ​​​​​​​latitude, longitude and altitude of Doppler- gyro navigation; projection of angular velocity of earth coordinate system e relative to inertial coordinate system i in geographic coordinate system n; projection of angular velocity of geographic coordinate system n relative to earth coordinate system e in geographic coordinate system n; ω ie is the earth rotation angular rate, which is a constant; projection of angular velocity of inertial device measurement coordinate system b relative to geographic coordinate system n in inertial device measurement coordinate system b; represents an initial attitude matrix; represents an attitude angular velocity corresponding skew-symmetric matrix.

[0165] S4: performing parameter matching using the navigation parameters related to gravity disturbance and the navigation parameters irrelevant to gravity disturbance output by the strapdown inertial navigation system to obtain matching parameters;

[0166] In an embodiment, the navigation parameters related to gravity disturbance in S4 include: velocity parameters related to gravity disturbance position parameters related to gravity disturbance and attitude matrices related to gravity disturbance

[0167] the matching parameters include: velocity matching quantity Z vel , position matching quantity Z pos and attitude matching quantity Z att ;

[0168] The parameter matching process includes the following steps:

[0169] velocity parameters related to gravity disturbance are subtracted from velocity parameters irrelevant to gravity disturbance to obtain velocity matching quantity Z vel :

[0170]

[0171] position parameters related to gravity disturbance are subtracted from position parameters irrelevant to gravity disturbance to obtain position matching quantity Z pos :

[0172]

[0173] attitude matrices related to gravity disturbance and attitude matrices irrelevant to gravity disturbance calculating the direction cosine matrix between the navigation system n' solved by the strapdown inertial navigation and the navigation system n" solved by the Doppler velocity log- gyroscope navigation system:

[0174]

[0175] taking the elements of the matrix as the attitude matching quantity Z att :

[0176]

[0177] wherein n' represents the navigation system solved by the strapdown inertial navigation; n" represents the navigation system solved by the Doppler velocity log- gyroscope navigation system; represents the attitude matrix irrelevant to the gravity disturbance; represents the attitude matrix relevant to the gravity disturbance; C p,q (p, q = 1, 2, 3) represents the pth row and qth column element of the matrix .

[0178] S5: performing one-step prediction on the gravity disturbance component and the gravity disturbance gradient component in the state estimation filter according to the gravity disturbance and the disturbance gradient state model; and performing measurement update on the one-step prediction result by using the matching parameter to obtain a final prediction result;

[0179] In an embodiment, the one-step prediction on the gravity disturbance component and the gravity disturbance gradient component in S5 comprises the following steps:

[0180] S51: defining the state vector X of the state estimation filter:

[0181]

[0182] wherein δg E , δg N , δg U represent the components of the gravity disturbance δg n in the east, north and up directions in the geographic coordinate system n; δΓ EE represents the second-order partial derivative of the gravity disturbance potential T with respect to the east direction E; δΓ EN represents the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the north direction N; δΓ EU represents the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the up direction U; δΓ NN represents the second-order partial derivative of the gravity disturbance potential T with respect to the north direction N; δΓ NU represents the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the up direction U; δv E,S , δv N,S , δv U,Sand δL S ,δλ S ,δh S These are the attitude misalignment angle, velocity error, and position error of the strapdown inertial navigation system, respectively. and δL L ,δλ L ,δh L These represent the attitude misalignment angle and position error of the Doppler velocimeter-gyroscope navigation system, respectively; ε x ,ε y ,ε z and These represent gyroscope constant drift and accelerometer zero bias, respectively; δα x ,δα z The installation error angle of the Doppler velocimeter relative to the inertial device; δK D The scale factor error of the Doppler velocimeter;

[0183] S52: Based on formula (10), formula (11), the error model of the strapdown inertial navigation system, and the error model of the Doppler velocimeter-gyroscope navigation system, the state equation is established as follows:

[0184]

[0185]

[0186]

[0187]

[0188]

[0189]

[0190]

[0191]

[0192]

[0193] In the formula, The variable represents the rate of change of the state vector X; W is the process noise, A is the system matrix, and G is the process noise driving matrix; w δΓ,EE ,w δΓ,EN ,w δΓ,EU ,w δΓ,NN ,w δΓ,NU The gradient component δΓ represents the gravitational perturbation. EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NUCorresponding excitation white noise; w ε,x ,w ε,y ,w ε,z and are the gyro random drift and accelerometer random noise respectively; A3and G2can be determined according to the error model of strapdown inertial navigation system and Doppler velocity log-gyro navigation system; d EE , d EN , d EU , d NN , d NU : represents the gravity disturbance gradient component δΓ EE , δΓ EN , δΓ EU , δΓ NN , δΓ NU corresponding correlation distance; v represents the module of carrier velocity v n ; diag represents a diagonal matrix, and the variable in [] is the diagonal line element of the diagonal matrix; 0 m1×m2 represents a zero matrix with m1rows and m2columns; I n1×n2 represents a unit matrix with n1rows and n2columns.

[0194] In an embodiment, the measurement update formula is:

[0195] Z(t) = H(t)X(t) + V(t) (24)

[0196] wherein, is the measurement vector, V is the measurement noise vector, and the measurement matrix H is

[0197]

[0198]

[0199]

[0200] wherein, c p,q (p, q = 1, 2, 3) represents the pth row and qth column element of the attitude matrix .

[0201] S6: using the final prediction result of the gravity disturbance component to correct the navigation calculation process of the strapdown inertial navigation system.

[0202] In an embodiment, the correction process includes:

[0203] correcting the solution process of the acceleration of the strapdown inertial navigation system:

[0204]

[0205]

[0206]

[0207] In the formula, denotes the latitude and altitude of the SINS; denotes the velocity of the SINS in the east and north components; denotes the specific force measured by the accelerometer; is the ideal gravity calculated according to the ideal gravity model; denotes the velocity parameter related to the gravity disturbance; and denote the earth angular velocity and displacement angular velocity calculated by the SINS respectively; denotes the final prediction result of the gravity disturbance.

[0208] The various embodiments described in the specification are described in progressive order, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be mutually referred to. For the apparatus disclosed by the embodiments, since it corresponds to the method disclosed by the embodiments, the description is relatively simple, and the relevant parts can be referred to the method part. The above description of the disclosed embodiments enables those skilled in the art to implement or use the present application. Various modifications to the disclosed embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A fully autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system, characterized in that ,comprising the following steps: S1: establishing an internal coupling relationship between the gravity disturbance, the gravity disturbance gradient and the gravity disturbance rate of change in the geographical coordinate system according to the internal relationship between the gravity disturbance potential, the gravity disturbance and the gravity disturbance gradient in the earth coordinate system; The construction process of the internal coupling relationship in S1 comprises: S11: obtaining the internal relationship between the gravity disturbance potential, the gravity disturbance and the gravity disturbance gradient in the earth coordinate system: where T represents the gravity disturbance; e represents the earth coordinate system; r represents a vector pointing from the center of the earth to the location of the carrier; δg e represents the gravity disturbance in the earth coordinate system e, δΓ e represents the gravity disturbance gradient in the earth coordinate system e; S12: Projecting the gravity disturbance δg e and the gravity disturbance gradient δΓ in the earth coordinate system e into the geographical coordinate system n, obtaining the gravity disturbance δg e and the gravity disturbance gradient δΓ in the geographical coordinate system n n and the gravity disturbance gradient δΓ in the geographical coordinate system n n : where e denotes the earth coordinate system; n denotes the geographic coordinate system; denotes the direction cosine matrix of the earth coordinate system e with respect to the geographic coordinate system n; δg n denotes the gravity disturbance in the geographic coordinate system n; δΓ n denotes the gravity disturbance gradient in the geographic coordinate system n; S13: the gravity disturbance δg in the geographic coordinate system n n The time derivative is taken to obtain the rate of change of the gravity disturbance δg in the geographic coordinate system n wherein denotes the corresponding anti-symmetric matrix, denotes the projection of the angular velocity of the geographical coordinate system n relative to the earth coordinate system e in the geographical coordinate system n, which is an ideal value; At the same time, the rate of change of the gravitational disturbance in the earth coordinate system e The gravitational disturbance gradient δΓ in the earth coordinate system e e And the carrier velocity v in the earth coordinate system e e Satisfies the following relationship: where v e denotes the carrier velocity in the earth coordinate system e; denotes the rate of change of the gravitational disturbance in the earth coordinate system e; Substitute formula (6) and formula (4) into formula (5) to obtain the internal coupling relationship in the geographical coordinate system: where v n denotes the carrier velocity in the geographical coordinate system n; δΓ n denotes the gravity disturbance gradient in the geographical coordinate system n; δg n denotes the gravity disturbance in the geographical coordinate system n; denotes the corresponding anti-symmetric matrix, denotes the projection of the angular velocity of the geographical coordinate system n with respect to the earth coordinate system e in the geographical coordinate system n; S2: constructing a constraint equation of the gravity disturbance gradient component based on the single-value continuity of the gravity disturbance potential and the conservation of the gravity disturbance field, and describing the mutually independent gravity disturbance gradient components by using a random process to construct a gravity disturbance and disturbance gradient state model; S3: performing Doppler velocimeter-gyroscope navigation calculation according to the velocity measured by the Doppler velocimeter and the angular velocity measured by the gyroscope to obtain navigation parameters irrelevant to the gravity disturbance; S4: performing parameter matching by using the navigation parameters related to the gravity disturbance output by the strapdown inertial navigation system navigation calculation and the navigation parameters irrelevant to the gravity disturbance to obtain matching parameters; S5: performing one-step prediction of the gravity disturbance component and the gravity disturbance gradient component in a state estimation filter according to the gravity disturbance and disturbance gradient state model; and performing measurement updating on the one-step prediction result by using the matching parameters to obtain a final prediction result; S6: correcting the navigation calculation process of the strapdown inertial navigation system by using the final prediction result of the gravity disturbance component.

2. The fully autonomous high-precision gravity disturbance compensation method for a SINS according to claim 1, characterized in that , gravity disturbance gradient δΓ n Specifically: δΓ EE denotes the second order partial derivative of the gravity disturbance potential T with respect to the east direction E; δΓ EN denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the east direction E; δΓ EU denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the up direction U; δΓ NE denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the east direction E; δΓ NN denotes the second order partial derivative of the gravity disturbance potential T with respect to the north direction N; δΓ NU denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the up direction U; δΓ UE denotes the partial derivative of the gravity disturbance potential T with respect to the up direction U and then with respect to the east direction E; δΓ UN denotes the partial derivative of the gravity disturbance potential T with respect to the up direction U and then with respect to the north direction N; δΓ UU denotes the second order partial derivative of the gravity disturbance potential T with respect to the up direction U.

3. The fully autonomous high-precision gravity disturbance compensation method for a SINS according to claim 2, characterized in that The construction process of the gravity disturbance and disturbance gradient state model in S2 comprises: According to the single-value continuity of gravity disturbance potential, the gravity disturbance gradient δΓ in the geographic coordinate system n is constructed n The constraint equation is: According to the conservation of the gravity disturbance field, a Laplace equation of the gravity disturbance potential T is constructed: ▽ 2 T = δΓ EE + δΓ NN + δΓ UU = 0 (9) wherein ∇ 2 denotes the Laplacian operator; Substitute formula (8) and formula (9) into formula (7) to obtain: wherein denotes the rate of change of the gravity disturbance in the geographical coordinate system n denotes the components in the east, north, and up directions; δg EE denotes the second order partial derivative of the gravity disturbance potential T with respect to the east direction E; δg EN denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the north direction N; δg EU denotes the partial derivative of the gravity disturbance potential T with respect to the east direction E and then with respect to the up direction U; δg NN denotes the second order partial derivative of the gravity disturbance potential T with respect to the north direction N; δg NU denotes the partial derivative of the gravity disturbance potential T with respect to the north direction N and then with respect to the up direction U; the subscripts "E", "N", and "U" denote the east, north, and up directions, respectively; δg E , δg N , δg U denotes the gravity disturbance δg in the geographical coordinate system n n denotes the components in the east, north, and up directions; v E , v N , v U denotes the carrier velocity v in the geographical coordinate system n n denotes the components in the east, north, and up directions, ω enE , ω enN , ω enU denotes the angular velocity denotes the components in the east, north, and up directions, denotes the projection of the angular velocity of the geographical coordinate system n with respect to the Earth coordinate system e in the geographical coordinate system n; Using a first order Markov process for gravity disturbance gradient components δΓ k is described, resulting in: where d k denotes the correlation distance; δΓ k denotes the correlation distance; δΓ EE denotes the correlation distance; δΓ EN denotes the correlation distance; δΓ EU denotes the correlation distance; δΓ NN denotes the correlation distance; δΓ NU ; denotes the rate of change of the gravity disturbance gradient component δΓ k ; w δΓ,k denotes the excitation white noise; v denotes the norm of the carrier velocity v n ; Formula (10) and formula (11) are the constructed gravity disturbance and disturbance gradient state model.

4. The fully autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system according to claim 1, characterized in that , The navigation parameters independent of gravity disturbance described in S3 include: a velocity parameter independent of gravity disturbance a position parameter independent of gravity disturbance and an attitude matrix independent of gravity disturbance The navigation calculation process of the navigation parameters irrelevant to the gravity disturbance comprises the following steps: S31: Utilize installation matrix and attitude matrix Convert the velocity measured by the Doppler velocimeter to the geographic coordinate system n to obtain a velocity parameter independent of the gravity disturbance Convert the velocity measured by the Doppler velocimeter to the geographic coordinate system n to obtain a velocity parameter independent of the gravity disturbance S32: velocity parameter independent of gravity disturbance ​ S33: Utilize a velocity parameter that is independent of gravity perturbations and a position parameter that is independent of gravity perturbations to angular velocity and angular velocity update: The updated angular velocity and the angular velocity is projected into the inertial device measurement coordinate system b and combined with the angular velocity The updated angular velocity Utilizing attitude angular velocity to the attitude matrix is updated in real time to obtain an updated attitude matrix wherein, denotes the velocity measured by Doppler velocity log, D denotes Doppler velocity log, and m denotes the Doppler velocity log measurement coordinate system; denotes the projection of the angular velocity of the inertial device measurement coordinate system b relative to the inertial coordinate system i in the inertial device measurement coordinate system b, i.e. the angular velocity measured by the gyroscope, i denotes the inertial coordinate system, and b denotes the inertial device measurement coordinate system; denotes the projection of the velocity in the geographic coordinate system n, L denotes Doppler velocity log-gyroscope navigation, and n denotes the geographic coordinate system; denotes the projection of the position in the geographic coordinate system n, L denotes Doppler velocity log-gyroscope navigation, and n denotes the geographic coordinate system; denotes the attitude matrix of the inertial device measurement coordinate system b relative to the geographic coordinate system n, b denotes the inertial device measurement coordinate system, L denotes Doppler velocity log-gyroscope navigation, and n denotes the geographic coordinate system; and denote the initial latitude, longitude and altitude respectively, which are consistent with the initial position parameters of the strapdown inertial navigation system; R M and R N denote the meridian radius and the prime vertical radius respectively; denotes the velocity in the east, north and sky directions; denote the latitude, longitude and altitude of the Doppler velocity log-gyroscope navigation; denotes the projection of the angular velocity of the earth coordinate system e relative to the inertial coordinate system i in the geographic coordinate system n; denotes the projection of the angular velocity of the geographic coordinate system n relative to the earth coordinate system e in the geographic coordinate system n; ie is the earth rotation angular rate, which is a constant; denotes the projection of the angular velocity of the inertial device measurement coordinate system b relative to the geographic coordinate system n in the inertial device measurement coordinate system b; denotes the initial attitude matrix; denotes the attitude angular velocity corresponding to the skew-symmetric matrix.

5. The fully autonomous high-precision gravity disturbance compensation method for a SINS according to claim 4, characterized in that , The navigation parameters related to the gravity disturbance in S4 include: a velocity parameter related to the gravity disturbance a position parameter related to the gravity disturbance and an attitude matrix related to the gravity disturbance The matching parameter includes: a speed matching amount Z vel , a position matching amount Z pos , and a posture matching amount Z att ; The parameter matching process comprises the following steps: Subtracting the velocity parameter related to the gravity disturbance Subtracting the velocity parameter not related to the gravity disturbance Obtaining the velocity matching quantity Z vel : Subtracting position parameters not related to gravity disturbances Subtracting position parameters not related to gravity disturbances Obtaining position match quantity Z pos : Utilizing attitude matrices related to gravity perturbations and attitude matrices independent of gravity perturbations The direction cosine matrix between the navigation system n' of the strapdown inertial navigation solution and the navigation system n" of the Doppler- gyro navigation system solution is calculated: Taking elements of a matrix as pose matching quantities Z att : wherein n' represents a strapdown inertial navigation system; and n" represents a Doppler- gyro navigation system; represents an attitude matrix independent of gravity disturbance; represents an attitude matrix dependent on gravity disturbance; and p,q (p, q = 1, 2, 3) represents the pth row, qth column element of the matrix .

6. The fully autonomous high-precision gravity disturbance compensation method for a strapdown inertial navigation system according to claim 3, characterized in that , The one-step prediction of the gravity disturbance component and the gravity disturbance gradient component in S5 comprises the following steps: S51: defining a state vector X of the state estimation filter: where δg E ,δg N ,δg U denotes the gravity disturbance δg n in the geographic coordinate system n; δg EE denotes the gravity disturbance δg EN in the geographic coordinate system n; δg EU denotes the gravity disturbance δg NN in the geographic coordinate system n; δg NU denotes the gravity disturbance δg δv E,S ,δv N,S ,δv U,S and δL S ,δλ S ,δh S are the attitude misalignment angle, velocity error and position error of the strapdown inertial navigation system, respectively; and δL L ,δλ L ,δh L are the attitude misalignment angle and position error of the Doppler-velometer- gyro navigation system, respectively; ε x ,ε y ,ε z and ▽ x ,▽ y ,▽ z are the gyro constant drift and accelerometer zero offset, respectively; δa x ,δα z is the installation error angle of the Doppler-velometer relative to the inertial device; δK D is the scale factor error of the Doppler-velometer; S52: establishing a state equation according to formula (10), formula (11), an error model of the strapdown inertial navigation system and an error model of the Doppler velocimeter-gyroscope navigation system as follows: W = [w δΓ,EE ,w δΓ,EN ,w δΓ,EU ,w δΓ,NN ,w δΓ,NU ,w ε,x ,w ε,y ,w ε,z ,w ▽,x ,w ▽,y ,w ▽,z ] T wherein represents the rate of change of state vector X; W is the process noise, A is the system matrix and G is the process noise driven matrix; w δΓ,EE ,w δΓ,EN ,w δΓ,EU ,w δΓ,NN ,w δΓ,NU represents the gravity disturbance gradient component δΓ EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NU corresponding excitation white noise; w ε,x ,w ε,y ,w ε,z and w ▽,x ,w ▽,y ,w ▽,z are the gyro random drift and accelerometer random noise, respectively; A3 and G2 are determined according to the error model of the strapdown inertial navigation system and the Doppler velocity log-gyro navigation system; d EE , d EN , d EU , d NN , d NU : represents the gravity disturbance gradient component δΓ EE ,δΓ EN ,δΓ EU ,δΓ NN ,δΓ NU corresponding correlation distance; v represents the module of carrier velocity v n ; diag represents a diagonal matrix, and the variable in [] is the diagonal line element of the diagonal matrix; 0 m1×m2 represents a zero matrix with m1 rows and m2 columns; I n1×n2 represents a unit matrix with n1 rows and n2 columns.

7. The fully autonomous high-precision gravity disturbance compensation method for a SINS according to claim 6, characterized in that , The formula of measurement updating is: Z(t)=H(t)X(t)+V(t) (24) where is the measurement vector, V is the measurement noise vector, and the measurement matrix H is where c p,q (p, q = 1, 2, 3) denotes the p-th row, q-th column element of the attitude matrix R.

8. The fully autonomous high-precision gravity disturbance compensation method for a SINS according to claim 5, characterized in that The correction process comprises: Acceleration of the solution process for strapdown inertial navigation systems is modified: wherein denotes the latitude and altitude of the SINS; denotes the velocity of the SINS in the east and north components; denotes the acceleration measured by the accelerometer; denotes the specific force measured by the accelerometer; is the ideal gravity calculated according to an ideal gravity model; denotes the velocity parameter related to the gravity disturbance; and denote the earth angular velocity and the displacement angular velocity calculated by the SINS, respectively; denotes the final prediction result of the gravity disturbance.

Images