Fast alignment method of moving base for unmanned pod strapdown inertial reference system

Abstract

The application belongs to the technical field of navigation, and discloses a moving base rapid alignment method for unmanned pod strapdown inertial reference system, which is suitable for rapid initial alignment of the strapdown inertial reference system with GNSS external observation information in the movement process. An optimization algorithm based on singular value decomposition is used to obtain a more accurate initial attitude, so as to shorten the error convergence time. Through the backtracking alignment mode, the data of the rough alignment stage of the strapdown inertial reference system is used for the fine alignment stage, so that the alignment accuracy can be improved within a limited alignment time, and the above process can be completed by storing a small amount of data, thereby reducing the requirement for the calculation platform. Meanwhile, the problem of inaccurate error estimation caused by high system instability under dynamic conditions is solved by designing an invariant Kalman filter, which has important engineering practical significance.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation, and relates to a strapdown inertial reference system alignment method, in particular to a moving base rapid alignment method of an unmanned pod strapdown inertial reference system, and is suitable for rapid initial alignment of a strapdown inertial navigation system with GNSS external observation information in a motion process. BACKGROUND

[0002] An unmanned pod carried on an aircraft can be installed with multiple types of optoelectronic sensors such as multispectral cameras and laser radars, and provides optical assistance for functions such as navigation, positioning, tracking and aiming of the aircraft. In order to ensure high-precision collaborative work of multiple sensors and ensure the stability of the attitude control of the optoelectronic pod, it is crucial.

[0003] A strapdown inertial reference system can output real-time attitude, velocity and position information of a carrier, and provides necessary navigation parameter information for normal work of an optoelectronic pod system, and is a core unit of a capture tracking and aiming system, and is therefore widely used in unmanned pod control systems. However, due to the output characteristics of the strapdown inertial reference system, the accuracy of the initial attitude will have an important influence on the subsequent navigation accuracy. Therefore, for a strapdown inertial reference system, initial alignment is a key core technology. According to the motion state of the carrier during the alignment process, it can be divided into static base alignment and dynamic base alignment. Static base alignment refers to initial alignment of the strapdown inertial reference system under the condition that the carrier is in a static state or a quasi-static state, and dynamic base alignment refers to initial alignment of the inertial navigation system during the motion of the carrier. In the actual use process of the strapdown inertial reference system, the external constraint conditions for realizing static base alignment are difficult to meet, and therefore, the dynamic base alignment technology which has no strict restrictions on the motion state of the carrier is one of the research focuses of the strapdown inertial reference system alignment technology in recent years.

[0004] Traditional dynamic base alignment technology usually needs to be completed independently in sequence. However, under dynamic conditions, the navigation information measurement uncertainty is high, and the coarse and fine alignment processes both need a long convergence time, which limits the rapid start-up capability of the inertial reference system; at the same time, the stability of the carrier in motion is poor, which easily leads to a large navigation error of the strapdown inertial reference system in the alignment process, and significantly increases the nonlinearity of the system, which is not conducive to accurate estimation and compensation of errors by a linear filter.

[0005] The present application is directed to the current problems, based on the invariant Kalman filter unmanned pod strapdown inertial reference system of dynamic base fast alignment method, the method uses the backtracking alignment framework in the inertial system, only need to store a small amount of calculation results in the coarse alignment process for fine alignment process, solve the problem of long initial alignment time of strapdown inertial reference system, compared with the traditional method reduces the storage capacity, improves the calculation efficiency;In the backtracking coarse alignment process, the method designs an optimization alignment method based on singular value decomposition, solves the problem of large uncertainty of coarse alignment result under dynamic conditions;In the backtracking fine alignment process, the method uses open loop invariant Kalman filter to estimate and compensate the navigation information error, solves the problem of inaccurate error estimation caused by system nonlinear effect. Using the above method, the strapdown inertial reference system can complete the fast initial alignment under the motion condition with the global navigation satellite system (GNSS) auxiliary observation. SUMMARY

[0006] The present application proposes a dynamic base fast alignment method for unmanned pod strapdown inertial reference system, which uses the backtracking alignment framework in the inertial system, stores part of the calculation results in the coarse alignment process for fine alignment process, estimates and compensates the nonlinear error of the system by using invariant Kalman filter, realizes the dynamic base fast initial alignment of strapdown inertial reference system under the observation of GNSS. The storage cost of the present application is low, the backtracking process has little effect on the navigation process, and the filtering process is not easy to be affected by the nonlinear characteristics of the system. The alignment accuracy can meet the demand of fast alignment of strapdown inertial reference system, and has important engineering practical value.

[0007] To solve the above technical problems, the solution proposed by the present application is:

[0008] The dynamic base fast alignment method for unmanned pod strapdown inertial reference system comprises the following steps:

[0009] (1) establish a backtracking coarse alignment scheme based on singular value decomposition, the specific steps are:

[0010] (1.1) define the projection coordinate system and determine the direction cosine matrix calculation method:

[0011] According to the chain rule, the direction cosine matrix of the carrier is decomposed into:

[0012]

[0013] Wherein, n system represents the navigation coordinate system, corresponding to the east-north-sky geographical coordinate system g system;B system represents the carrier coordinate system, corresponding to the right-front-up axis direction of the carrier; denotes the inertial coordinate system aligned with the initial time navigation coordinate system; denotes the inertial system aligned with the initial time carrier coordinate system; denotes the direction cosine matrix between the b system and the system; denotes the direction cosine matrix between the b system and the system; denotes the direction cosine matrix between the b system and the system; denotes the direction cosine matrix between the b system and the n system, which is directly solved by the initial position, current position, earth rotation angular velocity ω ie and motion time of the carrier, and is expressed as:

[0014]

[0015] where t represents the current time; t0 represents the alignment initial time; the e system represents the earth coordinate system; denotes the inertial coordinate system aligned with the initial time earth coordinate system; denotes the direction cosine matrix between the e system and the n system at t time; denotes the direction cosine matrix between the e system and the n system at t0 time; λ represents the longitude at the position of the carrier; L represents the latitude at the position of the carrier; denotes the direction cosine matrix between the n system and the e system at t time; denotes the direction cosine matrix between the n system and the e system at t0 time;

[0016] (1.2) updating the direction cosine matrix of the carrier system relative to the initial time carrier system

[0017] The initial time carrier system coincides with itself, that is, I 3×3 denotes a three-row and three-column unit matrix, which is used as an initial value, and a quaternion attitude updating algorithm is used to update the direction cosine matrix of the carrier system relative to the initial time carrier system in real time The specific steps are as follows,

[0018] The angle increment θ of the gyro double sample output is compensated by using a conic compensation algorithm:

[0019]

[0020] where △θ1 is the angle increment at the previous time, and △θ2 is the gyro angle increment at the next time;

[0021] A quaternion angle increment matrix Θ is constructed:

[0022]

[0023] where θ(1), θ(2), θ(3) are three components of the angle increment θ, respectively;

[0024] The attitude quaternion update process is:

[0025]

[0026] where t k represents the time of double sample output of the strapdown inertial reference system, k = 0, 1, 2, …, t k+1 and t k is the interval, corresponding to the time interval between two double sample outputs of the strapdown inertial reference system; represents the attitude quaternion between the carrier body at t k and the carrier body at the initial time, and

[0027] The attitude quaternion at t k+1 is converted into a direction cosine matrix, that is, the direction cosine matrix of the carrier body at the current time relative to the carrier body at the initial time is obtained is completed

[0028] (1.3) Calculate two sets of projection vectors:

[0029] (1.3.1) Define vector one a(t) and vector two β(t), and determine their expressions as follows:

[0030]

[0031] where a(t) and β(t) represent the calculated values of vector one and vector two at the current t time; is the specific force output of the accelerometer; is the projection of the carrier body ground speed in the navigation system, corresponding to the GNSS system speed output; is the projection of the local universal gravitation vector in the system; represents the projection of the velocity vector of the carrier body relative to the geocentric inertial system i at t in the system; represents the projection of the velocity vector of the carrier body relative to the geocentric inertial system i at t0 in the system; represents the direction cosine matrix between the n system and the system; represents the projection of the earth rotation angular velocity vector in the n system; is the projection of the position vector of the carrier body relative to the geocentric inertial system i in the n system, corresponding to the GNSS system position output; is the projection of the local gravity vector in the n system;

[0032] ​​(1.3.2) Update α(t) and β(t) with the update frequency being the double sample output frequency of the strapdown inertial reference system:

[0033]

[0034] α(t0) = 0 3×1 ,v g (t0) = 0 3×1 ;

[0035] wherein α(t k+1 ) denotes the calculated value of vector α at time t k+1 , α(t k ) denotes the calculated value of vector α at time t k , the initial value being α(t0); v g (t k+1 ) denotes the calculated value of the velocity increment v k+1 caused by the universal gravitation at time t g ; v g (t k ) denotes the calculated value of the velocity increment v k caused by the universal gravitation at time t g , the initial value being v g (t0); denotes the direction cosine matrix between b-frame at time t k and ; denotes the direction cosine matrix between b-frame at time t and b-frame at time t k ;

[0036] (1.3.3) Assume that GNSS data is updated at time t K , K = 0, 1, 2…, the interval between t K+1 and t K is △τ, which corresponds to the time interval between two outputs of the GNSS system, the time interval △τ being greater than the output interval △t of the strapdown inertial reference system; store the velocity position and integral results α(t K ), β(t K ), v g (t K ) at time t K according to the GNSS data update frequency, for use in the Kalman filtering of the retrograde precise alignment process, wherein all the storage variables are three-dimensional column vectors except that is a 3 × 3 matrix;

[0037] (1.4) Solve

[0038] The α(t K ), β(t K ) Cross-select N time intervals △T in the calculation results i ,i=1,2,3,…,N; where △T i represents the i-th time interval, corresponding to the time interval from t m Time to t n The time interval of the moment, satisfying m,n∈K and m <n;

[0039] definition Where α(t m ) represents t m The calculated value of the time vector α; α(t n ) represents t n The calculated value of the time vector α; β(t m ) represents t m The calculated value of the time vector β; β(t n ) represents t n The calculated value of the time vector β;

[0040] Construct the variable M, the expression is as follows:

[0041]

[0042] Performing singular value decomposition on M yields:

[0043] M=UDV T ;

[0044] Among them, U and V are orthogonal matrices, satisfying UU T =VV T =I 3×3 , D is a diagonal matrix consisting of the eigenvalues ​​of M;

[0045] make,

[0046]

[0047] Then the direction cosine matrix of the strapdown inertial reference system at the initial moment is calculated as:

[0048]

[0049] (2) The direction cosine matrix of the system at the initial moment obtained by backtracking rough alignment and stored in step (1.3.3) α(t K ),β(t K ) and v g (t K), establish a backtracking fine alignment scheme based on the invariant Kalman filter, the specific steps are:

[0050] (2.1) Update the system state using the data stored in the backtracking rough alignment stage and

[0051] (2.1.1) Establishing an inertial system The following attitude, velocity and position differential equations:

[0052] The navigation information update differential equations are determined as follows:

[0053]

[0054] in, Load system and inertial system The direction cosine matrix between ; is the true value of angular velocity; is the position of the load system relative to the inertial system i in the inertial system Projection in

[0055] (2.1.2) Discretization update carrier in inertial system Navigation information below:

[0056] Update the velocity, position and attitude of the carrier according to the period △τ of the back-tracking coarse alignment stored data:

[0057]

[0058] in, v g (t K+1 ),v g (t K ),α(t K+1 ),α(t K ), All are the results stored in step (1.3.3); Indicates the speed at t K The calculated value at the moment; Indicates the speed at t K+1 The calculated value at the moment; The position vector of the carrier relative to the Earth-centered inertial system i at time t0 is The projection of the system; The position vector of the carrier relative to the Earth-centered inertial system i is The projection of the system is at t K The calculated value at the moment; The position vector of the carrier relative to the Earth-centered inertial system i is The projection of the system is at t K+1The calculated value at the moment; is the strapdown inertial reference system b relative to The calculated value of the direction cosine matrix of the system's attitude;

[0059] (2.2) Determine the nonlinear error and its error transmission differential equation:

[0060] (2.2.1) Sensor error modeling:

[0061] The output of the gyro assembly and accelerometer assembly is expressed as:

[0062]

[0063] in, and Represent the actual output of gyroscope and accelerometer respectively; and Represent the true values ​​of gyroscope and accelerometer measurements respectively; and Represent the measurement errors of gyroscope and accelerometer respectively; w g and w a represent the Gaussian white noise of the gyroscope and accelerometer respectively; ε b and is the constant bias of the gyro and accelerometer assembly, that is

[0064]

[0065] (2.2.2) Navigation error modeling:

[0066] According to the sensor error model determined in (2.2.1), the inertial system described in step (2.1.1) The navigation information update equation under the perturbation analysis is performed to determine the inertial system The error differential equation is:

[0067]

[0068] in, Represents the carrier system b and the inertial system The attitude error angle of the system; Indicates the linear velocity error; represents the linear position error; represents the gravitational error caused by position error; Indicates speed The calculated value of Indicates location Calculated value;

[0069] (2.2.3) Define the nonlinear error model:

[0070] According to the Lie group related theory, a right invariant error matrix η is established R :

[0071]

[0072] where χ represents the true value of the system state matrix; represents the calculated value of the system state matrix; represents the direction cosine matrix calculated value; represents the nonlinear velocity error; represents the nonlinear position error;

[0073] According to the linear error model differential equation established in step (2.2.2), the differential equation of the nonlinear error model is determined as:

[0074]

[0075] (2.3) Determine the filter state equation:

[0076]

[0077] where X is the system error state, F is the state transition matrix, G is the noise driving matrix, W is the system noise matrix, and is represented as:

[0078]

[0079] W = [(w g ) T (w a ) T ] T ;

[0080] (· ×) represents the skew-symmetric matrix corresponding to the · vector; 0 m×n represents the zero matrix of m rows and n columns;

[0081] (2.4) Determine the state observation equation:

[0082] Use the velocity and position information output by GNSS to observe the nonlinear error in step (2.2):

[0083]

[0084] V = [(υ v ) T (υ r ) T ] T ;

[0085] where Z represents the observation vector; V represents the observation noise vector; H represents the observation matrix; the projection of the velocity vector output by the strapdown inertial reference system in the inertial system; the projection of the position vector output by the strapdown inertial reference system in the inertial system; the projection of the velocity vector output by the GNSS system in the inertial system; the projection of the position vector output by the GNSS system in the inertial system; v denotes the GNSS system velocity observation noise; r denotes the GNSS system position observation noise;

[0086] (2.5) determining the filter initial time system covariance matrix:

[0087] determining the filter initial time covariance matrix P, Q, R using the gyroscope assembly and accelerometer assembly noise characteristics, and the GNSS system observation noise characteristics:

[0088]

[0089] wherein P denotes the error covariance matrix; Q denotes the measurement noise covariance matrix; R denotes the observation noise covariance matrix; σ φ denotes the initial attitude error standard deviation; σ v denotes the initial velocity error standard deviation; σ r denotes the initial position error standard deviation; σ ε denotes the gyroscope bias standard deviation; denotes the accelerometer bias standard deviation; denotes the gyroscope noise standard deviation; denotes the accelerometer noise standard deviation; denotes the velocity observation standard deviation; denotes the position observation standard deviation;

[0090] (2.6) establishing an open loop Kalman filter process:

[0091] using the navigation information updating mode described in step (2.1.2) and the filter state equation, state observation equation and filter covariance matrix established in steps (2.3), (2.4) and (2.5), constructing a Kalman filter to estimate the error in the navigation information, and after the alignment phase ends, compensating the error once to the navigation information , realizing an open loop Kalman filter process.

[0092] ​​​​Based on the coarse and fine alignment scheme designed in the above steps, the partial calculation results are stored at a lower frequency (GNSS signal update frequency) in the coarse alignment phase, and the attitude matrix of the system at the initial time is solved; then the stored data is used for pure inertial navigation update and open-loop Kalman filtering in the fine alignment phase; the alignment result obtained is compensated to the inertial navigation system update process at one time at the end of the alignment phase, that is, the initial alignment of the GNSS-aided strapdown inertial reference system is realized;

[0093] Further, in the step (1.2) of updating the attitude quaternion, the Bick quadratic algorithm is used to approximate the quaternion angular increment matrix , which is expressed as:

[0094]

[0095] Further, in the step (1.3.2) of calculating the integral expressions of vectors α(t) and β(t), the following discrete calculation scheme is adopted:

[0096]

[0097] α(t0)=0 3×1 ,v g (t0)=0 3×1 ;

[0098] wherein △v1 is the velocity increment output of the accelerometer at the previous time, and △v2 is the velocity increment output of the accelerometer at the next time;

[0099] Further, in the step (1.4) of determining the cross selection time interval △T i , the following selection scheme is adopted:

[0100] Let t2 be the end time of the alignment, and the time interval length is determined as △T i =(t2-t0) / 4, the starting time of the first time interval is t0, and the starting time of each subsequent time interval is the middle time of the previous time interval, and the end time of the last time interval is t2, a total of 7 time intervals are selected, that is, N=7;

[0101] Further, in the step (2.5) of initializing the initial time error covariance matrix, the influence of the nonlinear error on the error covariance matrix is considered to improve the alignment accuracy, and the initial time nonlinear error covariance matrix P' is expressed as:

[0102]

[0103] P'=TPT T ;

[0104] wherein T is a nonlinear correlation matrix, respectively the value at the time t0;

[0105] Further, in the step (2.6), when the navigation information is compensated for errors, the following compensation scheme is adopted:

[0106]

[0107] wherein the system attitude matrix at the end of alignment is the first to third rows and the first to third columns of X; the system velocity at the end of alignment is the first to third rows and the fourth column of X; and the system position at the end of alignment is the first to third rows and the fifth column of X.

[0108] To sum up, the advantages and positive effects of the present application are as follows: the present application uses the data of the coarse alignment stage of the strapdown inertial reference system for the fine alignment stage through the retrograde alignment mode, can improve the alignment accuracy within a limited alignment time, and only needs to store a small amount of data to complete the above process, reduces the requirement for the calculation platform, at the same time, solves the problem of inaccurate error estimation caused by high system instability under dynamic conditions through the design of the invariant Kalman filter, has important engineering practical significance. BRIEF DESCRIPTION OF DRAWINGS

[0109] Figure 1 is the flowchart provided by the embodiment of the present application. DETAILED DESCRIPTION

[0110] In order to make the purpose, technical scheme and advantages of the present application more clear and explicit, the present application is further described in detail below in combination with embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application.

[0111] In some special application environments, such as the execution of emergency tasks by aircraft, the restart of the inertial navigation system in the task, etc., the strapdown inertial navigation system does not have the external conditions for long-time static base initial alignment, and needs to complete dynamic base alignment. The traditional dynamic base alignment scheme is slow in alignment speed, which affects the start speed and alignment accuracy of the inertial navigation system. In addition, the traditional speed and position error model does not consider the nonlinear problem caused by the inconsistency of the projection coordinate system, which leads to the system being unable to accurately estimate the error under dynamic environment.

[0112] To solve the above technical problems, the present application proposes a dynamic base fast alignment method based on an invariant Kalman filter, as shown in Figure 1 The specific implementation method is as follows:

[0113] (1) a retrograde coarse alignment scheme based on singular value decomposition is established, and the specific steps are as follows:

[0114] (1.1) Define the projection coordinate system, determine the direction cosine matrix calculation method:

[0115] According to the chain rule, the direction cosine matrix of the carrier is decomposed as:

[0116]

[0117] Where n represents the navigation coordinate system, corresponding to the East-North-Sky geographic coordinate system g; b represents the carrier coordinate system, corresponding to the right-front-up axis of the carrier; represents the inertial coordinate system aligned with the initial time navigation coordinate system; represents the inertial coordinate system aligned with the initial time navigation coordinate system; represents the direction cosine matrix between b and n system; represents the direction cosine matrix between b system and n system; represents the direction cosine matrix between b and n system, which is directly solved by the initial position, current position, and earth rotation angular velocity ω ie of the carrier, and is expressed as:

[0118]

[0119] Where t represents the current time; t0 represents the initial time of alignment; e represents the earth coordinate system; represents the inertial coordinate system aligned with the initial time earth coordinate system; represents the direction cosine matrix between e and n system at t time; represents the direction cosine matrix between e and n system at t0 time; λ represents the longitude at the position of the carrier; L represents the latitude at the position of the carrier; represents the direction cosine matrix between e and e system at t time;

[0120] (1.2) Update the direction cosine matrix of the carrier relative to the initial time carrier coordinate system

[0121] The initial time carrier coordinate system coincides with itself, i.e. I 3×3 represents a three-row and three-column unit matrix, which is used as the initial value, and the quaternion attitude update algorithm is used to update the direction cosine matrix of the carrier relative to the initial time carrier coordinate system in real time The specific steps are as follows,

[0122] The angle increment θ of the gyro double sample output is compensated by the conic compensation algorithm as:

[0123]

[0124] wherein, △θ1 is the angle increment of the previous time, △θ2 is the angle increment of the gyroscope at the next time;

[0125] Construct a quaternion angle increment matrix Θ:

[0126]

[0127] wherein, θ(1), θ(2), θ(3) are three components of the angle increment θ, respectively;

[0128] The attitude quaternion updating process is:

[0129]

[0130] wherein, t k represents the double sample output time of the strapdown inertial reference system, k = 0, 1, 2, …, t k+1 and t k interval is △t, corresponding to the time interval between the two double sample outputs of the strapdown inertial reference system; represents the attitude quaternion between the carrier body at t k and the carrier body at the initial time, and

[0131] The attitude quaternion at t k+1 is converted into a direction cosine matrix, that is, the direction cosine matrix of the carrier body at the current time relative to the carrier body at the initial time is obtained is updated;

[0132] (1.3) Calculate two sets of projection vectors:

[0133] (1.3.1) Define vector one α(t) and vector two β(t), and determine their expressions as follows:

[0134]

[0135] wherein, α(t) and β(t) represent the calculated values of vector one and vector two at the current t time; is the specific force output of the accelerometer; is the projection of the carrier body ground speed in the navigation system, corresponding to the GNSS system speed output; is the projection of the local universal gravitation vector in the system; represents the projection of the velocity vector of the carrier body relative to the geocentric inertial system i at t in the system; ​​The velocity vector of the carrier relative to the Earth's inertial system i at time t0 is The projection of the system; Represents n series and Direction cosine matrix between systems; represents the projection of the Earth's rotation angular velocity vector in the n system; is the projection of the position vector of the carrier relative to the Earth-centered inertial system i in the n-frame, corresponding to the GNSS system position output; is the projection of the local gravity vector in the n-frame;

[0136] (1.3.2) Update α(t) and β(t) at the twin-sample output frequency of the strapdown inertial reference system:

[0137]

[0138] α(t0)=0 3×1 ,v g (t0)=0 3×1 ;

[0139] Among them, α(t k+1 ) represents t k+1 The calculated value of the time vector α, α(t k ) represents t k The calculated value of the time vector α, the initial value is α(t0); v g (t k+1 ) represents t k+1 The velocity increment v caused by gravity at time g The calculated value of v g (t k ) represents t k The velocity increment v caused by gravity at time g The calculated value, the initial value is v g (t0); Indicates t k Time b system and The direction cosine matrix between Indicates that the b system at time t is equal to t k The direction cosine matrix between time frames b;

[0140] (1.3.3) Assume that GNSS data is at t K Update at any time, K = 0, 1, 2…, t K+1 With t K The interval is △τ, which corresponds to the time interval between two GNSS system outputs. The time interval △τ is greater than the output interval △t of the strapdown inertial reference system. K Time speed Location and integral results a(t K ), b(t K ), v g (t K ) are stored for Kalman filtering of backtracking fine alignment process, where all the storage variables are three-dimensional column vectors except a(t is a 3x3 matrix;

[0141] (1.4) solve the optimization problem based on singular value decomposition

[0142] Cross-select N time intervals △T K ,i=1,2,3,…,N from the calculation results of a(t K ), b(t i ) stored in step (1.3); where △T i represents the i-th time interval, corresponding to the time interval from t m to t n , satisfying m,n∈K and m<n;

[0143] Define where a(t m ) represents the calculated value of vector a at t m ; a(t n ) represents the calculated value of vector a at t n ; b(t m ) represents the calculated value of vector b at t m ; b(t n ) represents the calculated value of vector b at t n ;

[0144] Construct variable M, expressed as follows:

[0145]

[0146] Perform singular value decomposition on M to obtain:

[0147] M=UDV T ;

[0148] where U and V are orthogonal matrices, satisfying UU T =VV T =I 3×3 , and D is a diagonal matrix composed of the eigenvalues of M;

[0149] Let

[0150]

[0151] Then the calculated direction cosine matrix of the initial time of the strapdown inertial reference system is:

[0152]

[0153] (2) The direction cosine matrix of the system at the initial time calculated by the backtracking coarse alignment and the α(t K ), β(t K ) and v g (t K ) stored in step (1.3.3), a backtracking fine alignment scheme based on the invariant Kalman filter is established, and the specific steps are as follows:

[0154] (2.1) Update the system state using the data stored in the backtracking coarse alignment stage and

[0155] (2.1.1) Establish the inertial system attitude, velocity and position differential equations:

[0156] Determine the navigation information update differential equation set as:

[0157]

[0158] wherein, is the direction cosine matrix between the carrier system and the inertial system ; is the angular velocity true value; is the projection of the position of the carrier system relative to the inertial system i in the inertial system ;

[0159] (2.1.2) Discretize and update the navigation information of the carrier in the inertial system :

[0160] Update the velocity, position and attitude of the carrier according to the period △τ of the data stored in the backtracking coarse alignment:

[0161]

[0162] wherein, v g (t K+1 ), v g (t K ), α(t K+1 ), α(t K ), are all the results stored in the step (1.3.3); represents the calculated value of the velocity at t K ; represents the calculated value of the velocity at t K+1 ; represents the projection of the position vector of the carrier relative to the geocentric inertial system i in the at time t0; represents the projection of the position vector of the carrier relative to the geocentric inertial system i in the at time t0; K represents the projection of the position vector of the carrier relative to the geocentric inertial system i in the at time t0; K+1 represents the projection of the position vector of the carrier relative to the geocentric inertial system i in the at time t0; represents the projection of the position vector of the carrier relative to the geocentric inertial system i in the at time t0;

[0163] (2.2) determining the nonlinear errors and their error propagation differential equations:

[0164] (2.2.1) sensor error modeling:

[0165] Let the outputs of the gyro assembly and the accelerometer assembly be represented as:

[0166]

[0167] wherein, and represent the actual outputs of the gyro and the accelerometer, respectively; and represent the true values measured by the gyro and the accelerometer, respectively; and represent the measurement errors of the gyro and the accelerometer, respectively; w g and w a represent the Gaussian white noises of the gyro and the accelerometer, respectively; ε b and are the constant biases of the gyro and the accelerometer assembly, i.e.

[0168]

[0169] (2.2.2) navigation error modeling:

[0170] According to the sensor error model determined in (2.2.1), the navigation information update equation in the inertial system is perturbed to determine the error differential equation in the inertial system as:

[0171]

[0172] wherein, represents the attitude error angle of the carrier system b relative to the inertial system ; and represents the linear velocity error. represents linear position error; represents universal gravitation error due to position error; represents velocity calculation value, represents position calculation value;

[0173] (2.2.3) defines a nonlinear error model:

[0174] According to the Lie group related theory, a right invariant error matrix η R is established:

[0175]

[0176] wherein χ represents system state matrix true value; represents system state matrix calculation value; represents directional cosine matrix calculation value; represents nonlinear velocity error; represents nonlinear position error;

[0177] According to the linear error model differential equation established in step (2.2.2), the differential equation of the nonlinear error model is determined as:

[0178]

[0179] (2.3) determines filter state equation:

[0180]

[0181] wherein X is system error state, F is state transition matrix, G is noise driving matrix, W is system noise matrix, and is represented as:

[0182]

[0183]

[0184] W = [(w g ) T (w a ) T ] T ;

[0185] (· ×) represents the skew-symmetric matrix corresponding to the · vector; 0 m×n represents an m-row n-column zero matrix;

[0186] (2.4) determines state observation equation:

[0187] The non-linear error in step (2.2) is observed using the velocity and position information output by the GNSS:

[0188]

[0189] V = [(υ v ) T (υ r ) T ] T ;

[0190] where Z represents the observation vector; V represents the observation noise vector; H represents the observation matrix; is the projection of the velocity vector output by the SINS in the inertial system ; is the projection of the position vector output by the SINS in the inertial system ; is the projection of the velocity vector output by the GNSS in the inertial system ; is the projection of the position vector output by the GNSS in the inertial system ; υ v represents the GNSS system velocity observation noise; υ r represents the GNSS system position observation noise;

[0191] (2.5) Determine the system covariance matrix at the initial time of the filter:

[0192] Determine the covariance matrix P, Q, R of the filter at the initial time using the noise characteristics of the gyro assembly and the accelerometer assembly, and the observation noise characteristics of the GNSS system:

[0193]

[0194] where P represents the error covariance matrix; Q represents the measurement noise covariance matrix; R represents the observation noise covariance matrix; σ φ represents the standard deviation of the initial attitude error; σ v represents the standard deviation of the initial velocity error; σ r represents the standard deviation of the initial position error; σ ε represents the standard deviation of the gyro bias; represents the standard deviation of the accelerometer bias; represents the standard deviation of the gyro noise; represents the standard deviation of the accelerometer noise; represents the velocity observation standard deviation; represents the position observation standard deviation;

[0195] (2.6) Establish an open-loop Kalman filter process:

[0196] The navigation information updating method described in step (2.1.2) and the filter state equation, the state observation equation and the filter covariance matrix established by steps (2.3), (2.4) and (2.5) are used to construct a Kalman filter to correct the error in the navigation information The error is estimated and compensated to the navigation information at the end of the alignment phase The open-loop Kalman filter process is realized.

[0197] Based on the coarse and fine alignment scheme designed in the above steps, the partial calculation results are stored at a low frequency (GNSS signal updating frequency) in the coarse alignment phase and the attitude matrix at the initial time of the system is solved; then the stored data is used for pure inertial navigation updating and open-loop Kalman filtering in the fine alignment phase; the alignment result obtained is compensated to the inertial navigation system updating process at the end of the alignment phase, that is, the initial alignment of the GNSS-aided SINS system is realized;

[0198] Further, in the step (1.2) of updating the attitude quaternion, the Bick quadratic algorithm is used to approximate the quaternion angular increment matrix , which is expressed as:

[0199]

[0200] Further, in the step (1.3.2) of calculating the integral expressions of vectors α(t) and β(t), the following discrete calculation scheme is adopted:

[0201]

[0202] α(t0)=0 3×1 ,v g (t0)=0 3×1 ;

[0203] wherein △v1 is the velocity increment output of the accelerometer at the previous time, and △v2 is the velocity increment output of the accelerometer at the next time;

[0204] Further, in the step (1.4) of determining the cross selection time interval △T i , the following selection scheme is adopted:

[0205] Let t2 be the end time of the alignment, and the time interval length is determined as △T i =(t2-t0) / 4, the starting time of the first time interval is t0, and the starting time of each subsequent time interval is the middle time of the previous time interval, and the end time of the last time interval is t2, and a total of 7 time intervals are selected, that is, N=7;

[0206] Further, in the step (2.5), when the initial time error covariance matrix is initialized, the influence of the nonlinear error on the error covariance matrix is considered to improve the alignment accuracy, and the initial time nonlinear error covariance matrix P' is expressed as:

[0207]

[0208] P'=TPT T ;

[0209] Wherein, T is a nonlinear correlation matrix, respectively, The value at t0 time;

[0210] Further, in the step (2.6), when the navigation information is error compensated, the following compensation scheme is adopted:

[0211]

[0212] Wherein, the system attitude matrix at the end of alignment is the first to third rows and the first to third columns of χ; the system velocity at the end of alignment is the first to third rows of the fourth column of χ; and the system position at the end of alignment is the first to third rows of the fifth column of χ.

[0213] The above only describes the preferred embodiments of the present application, and does not limit the present application, and any technical solution belonging to the idea of the present application is within the protection scope of the present application. Any improvement and decoration without departing from the principle of the present application should be considered as the protection scope of the present application.

Claims

1. A method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system, characterized in that: The method comprises the following steps: (1) Establish a retrospective coarse alignment scheme based on singular value decomposition. The specific steps are as follows: (1.1) Define the projection coordinate system and determine the calculation method of the direction cosine matrix: According to the chain rule, the direction cosine matrix of the vector is decomposed into: Among them, the n system represents the navigation coordinate system, corresponding to the east-north-sky geographic coordinate system g system; the b system represents the carrier coordinate system, corresponding to the right-front-up axis of the carrier; Represents the inertial coordinate system aligned with the navigation coordinate system at the initial moment; represents the inertial frame aligned with the carrier coordinate system at the initial moment; Represents b series and Direction cosine matrix between systems; express Department and Direction cosine matrix between systems; express The direction cosine matrix between the system and the n system is composed of the initial position of the carrier, the current position, the angular velocity of the earth's rotation ω ie The motion time is directly solved and expressed as: Where t represents the current time; t0 represents the initial time of alignment; e represents the Earth coordinate system; The system represents the inertial coordinate system aligned with the Earth coordinate system at the initial moment; Represents the direction cosine matrix between the e-system and the n-system at time t; represents the direction cosine matrix between the e-system and the n-system at time t0; λ represents the longitude of the carrier; L represents the latitude of the carrier; Indicates time t The direction cosine matrix between the system and the e system; (1.2) Update the direction cosine matrix of the load system relative to the initial load system At the initial moment, the load system coincides with itself, that is, I 3×3 Represents a three-row and three-column identity matrix, which is used as the initial value. The quaternion attitude update algorithm is used to update the direction cosine matrix of the carrier system relative to the initial moment in real time. The specific steps are as follows: The angle increment θ of the gyro twin output is compensated by the cone compensation algorithm: Among them, △θ1 is the angle increment at the previous moment, and △θ2 is the gyro angle increment at the next moment; Construct the quaternion angle increment matrix Θ: Among them, θ(1), θ(2), and θ(3) are the three components of the angle increment θ; The attitude quaternion update process is: Among them, t k represents the twin-sample output time of the strapdown inertial reference system, k=0,1,2,…,t k+1 With t k The interval is △t, which corresponds to the time interval between two twin sample outputs of the strapdown inertial reference system; Indicates t k The attitude quaternion between the time-dependent load system and the initial time-dependent load system, and t k+1 Attitude quaternion at the moment Converted into a direction cosine matrix, that is, the direction cosine matrix of the current load system relative to the initial load system is obtained Finish renew; (1.3) Calculate two sets of projection vectors: (1.3.1) Define vector 1 α(t) and vector 2 β(t), and determine their expressions as: Among them, α(t) and β(t) represent the calculated values ​​of vector 1 and vector 2 at the current time t; is the specific force output of the accelerometer; It is the projection of the carrier ground speed in the navigation system, corresponding to the GNSS system speed output; is the local gravitational vector The projection of the system; The velocity vector of the carrier relative to the Earth's center inertial system i at time t is The projection of the system; The velocity vector of the carrier relative to the Earth's inertial system i at time t0 is The projection of the system; Represents n series and Direction cosine matrix between systems; represents the projection of the Earth's rotation angular velocity vector in the n system; is the projection of the position vector of the carrier relative to the Earth-centered inertial system i in the n-frame, corresponding to the GNSS system position output; is the projection of the local gravity vector in the n-frame; (1.3.2) Update α(t) and β(t) at the twin-sample output frequency of the strapdown inertial reference system: Among them, α(t k+1 ) represents t k+1 The calculated value of the time vector α, α(t k ) represents t k The calculated value of the time vector α, the initial value is α(t0); v g (t k+1 ) represents t k+1 The velocity increment v caused by gravity at time g The calculated value of v g (t k ) represents t k The velocity increment v caused by gravity at time g The calculated value, the initial value is v g (t0); Indicates t k Time b system and The direction cosine matrix between Indicates that the b system at time t is equal to t k The direction cosine matrix between time frames b; (1.3.3) Assume that GNSS data is at t K Update at any time, K = 0, 1, 2…, t K+1 With t K The interval is △τ, which corresponds to the time interval between two GNSS system outputs. The time interval △τ is greater than the output interval △t of the strapdown inertial reference system. K Time speed Location and integral results α(t K ),β(t K ),v g (t K ) is stored and used for backtracking the Kalman filter of the fine alignment process, except Except for the 3×3 matrix, the other storage variables are three-dimensional column vectors; (1.4) Solve using the optimization method based on singular value decomposition The α(t K ), β(t K ) Cross-select N time intervals △T in the calculation results i ,i=1,2,3,…,N; where △T i represents the i-th time interval, corresponding to the time interval from t m Time to t n The time interval of the moment, satisfying m,n∈K and m <n; definition Where α(t m ) represents t m The calculated value of the time vector α; α(t n ) represents t n The calculated value of the time vector α; β(t m ) represents t m The calculated value of the time vector β; β(t n ) represents t n The calculated value of the time vector β; Construct the variable M, the expression is as follows: Performing singular value decomposition on M yields: M=UDV T ; Among them, U and V are orthogonal matrices, satisfying UU T =VV T =I 3×3 , D is a diagonal matrix consisting of the eigenvalues ​​of M; make, Then the direction cosine matrix of the strapdown inertial reference system at the initial moment is calculated as: (2) The direction cosine matrix of the system at the initial moment obtained by backtracking rough alignment and stored in step (1.3.3) α(t K ),β(t K ) and v g (t K ), establish a backtracking fine alignment scheme based on the invariant Kalman filter, the specific steps are: (2.1) Update the system state using the data stored in the backtracking rough alignment stage and (2.1.1) Establishing an inertial system The following attitude, velocity and position differential equations: The navigation information update differential equations are determined as follows: in, Load system and inertial system The direction cosine matrix between ; is the true value of angular velocity; is the position of the load system relative to the inertial system i in the inertial system Projection in (2.1.2) Discretization update carrier in inertial system Navigation information below: Update the velocity, position and attitude of the carrier according to the period △τ of the back-tracking coarse alignment stored data: in, v g (t K+1 ),v g (t K ),α(t K+1 ),α(t K ), All are the results stored in step (1.3.3); Indicates the speed at t K The calculated value at the moment; Indicates the speed at t K+1 The calculated value at the moment; The position vector of the carrier relative to the Earth-centered inertial system i at time t0 is The projection of the system; The position vector of the carrier relative to the Earth-centered inertial system i is The projection of the system is at t K The calculated value at the moment; The position vector of the carrier relative to the Earth-centered inertial system i is The projection of the system is at t K+1 The calculated value at the moment; is the strapdown inertial reference system b relative to The calculated value of the direction cosine matrix of the system's attitude; (2.2) Determine the nonlinear error and its error transmission differential equation: (2.2.1) Sensor error modeling: The output of the gyro assembly and accelerometer assembly is expressed as: in, and Represent the actual output of gyroscope and accelerometer respectively; and Represent the true values ​​of gyroscope and accelerometer measurements respectively; and Represent the measurement errors of gyroscope and accelerometer respectively; w g and w a represents the Gaussian white noise of the gyroscope and accelerometer respectively; ε b and is the constant bias of the gyro and accelerometer assembly, that is (2.2.2) Navigation error modeling: According to the sensor error model determined in (2.2.1), the inertial system described in step (2.1.1) The navigation information update equation under the perturbation analysis is performed to determine the inertial system The error differential equation is: in, Represents the carrier system b and the inertial system The attitude error angle of the system; Indicates the linear velocity error; represents the linear position error; represents the gravitational error due to position error; Indicates speed The calculated value of Indicates location Calculated value; (2.2.3) Define the nonlinear error model: According to the relevant theory of Lie group, the right invariant error matrix η is established R : Where, χ represents the true value of the system state matrix; Represents the calculated value of the system state matrix; represents the direction cosine matrix Calculated value; represents the nonlinear velocity error; represents the nonlinear position error; According to the linear error model differential equation established in step (2.2.2), the differential equation of the nonlinear error model is determined as: (2.3) Determine the filtering state equation: Where X is the system error state, F is the state transfer matrix, G is the noise driving matrix, and W is the system noise matrix, which can be expressed as: In=[(in g ) T (In a ) T ] T ; (·×) represents the antisymmetric matrix corresponding to the · vector; 0 m×n represents a zero matrix with m rows and n columns; (2.4) Determine the state observation equation: The nonlinear error in step (2.2) is observed using the velocity and position information output by GNSS: Where Z represents the observation vector; V represents the observation noise vector; H represents the observation matrix; The velocity vector output by the strapdown inertial reference system is Projection in The position vector output by the strapdown inertial reference system is in the inertial system Projection in The velocity vector output by the GNSS system is in the inertial system Projection in The position vector output by the GNSS system is in the inertial system Projection in;υ v represents the GNSS system velocity observation noise; υ r represents the GNSS system position observation noise; (2.5) Determine the system covariance matrix of the filter at the initial moment: Using the noise characteristics of the gyro component and accelerometer component, as well as the GNSS system observation noise characteristics, the covariance matrix P, Q, and R of the filter at the initial moment are determined: Where P represents the error covariance matrix; Q represents the measurement noise covariance matrix; R represents the observation noise covariance matrix; σ φ represents the standard deviation of the initial attitude error; σ v represents the standard deviation of the initial velocity error; σ r represents the standard deviation of the initial position error; σ ε represents the standard deviation of gyro bias; represents the standard deviation of the accelerometer bias; represents the standard deviation of gyro noise; represents the standard deviation of accelerometer noise; represents the standard deviation of velocity observations; represents the standard deviation of position observations; (2.6) Establish the open-loop Kalman filter process: Using the navigation information update method described in step (2.1.2) and the filter state equation, state observation equation and filter covariance matrix established in steps (2.3), (2.4) and (2.5), a Kalman filter is constructed to detect the error in the navigation information. Make an estimate and, after the alignment phase, compensate the error to the navigation information once for all In the open-loop Kalman filtering process, 2. The method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system according to claim 1, wherein: When updating the attitude quaternion in step (1.2), the quaternion angle increment matrix is ​​calculated using the Picard fourth-order algorithm. Approximately, it can be expressed as:

3. The method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system according to claim 1, wherein: When calculating the integrals of the vectors α(t) and β(t) in step (1.3.2), the following discrete calculation scheme is used: α(t0)=0 3×1 ,v g (t0)=0 3×1 ; Among them, △v1 is the velocity increment output of the accelerometer at the previous moment, and △v2 is the velocity increment output of the accelerometer at the next moment.

4. The method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system according to claim 1, wherein: In the step (1.4), the cross-selection time interval ΔT is determined. i The following selection scheme is adopted: Assume that time t2 is the end time of alignment, and the time interval length is determined as △T i =(t2-t0) / 4, the starting time of the first time interval is t0, the starting time of each time interval selected thereafter is the middle time of the previous time interval, and the ending time of the last time interval is t2. A total of 7 time intervals are selected, that is, N=7.

5. The method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system according to claim 1, wherein: When initializing the error covariance matrix at the initial moment in step (2.5), the influence of nonlinear error on the error covariance matrix is ​​considered to improve the alignment accuracy. The nonlinear error covariance matrix P' at the initial moment is expressed as: P'=TPT T ; Where T is the nonlinear correlation matrix, They are The value at time t0.

6. The method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system according to claim 1, wherein: When performing error compensation on the navigation information in step (2.6), the following compensation scheme is adopted: Among them, the system attitude matrix at the end of alignment is the first to third rows and the first to third columns of χ; the system velocity at the end of alignment is the first to third rows and the fourth column of χ; and the system position at the end of alignment is the first to third rows and the fifth column of χ.

Images