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

Abstract

The invention belongs to the technical field of navigation, and discloses an unmanned pod strapdown inertial reference system moving base rapid alignment method which is suitable for rapid initial alignment of a strapdown inertial reference system with GNSS external observation information in the moving process. A more accurate initial attitude is obtained through an optimization algorithm based on singular value decomposition, and the error convergence time is shortened; through a backtracking alignment mode, data of a coarse alignment stage of the strapdown inertial reference system is used for a fine alignment stage, the alignment precision can be improved within limited alignment time, the process can be completed only by storing a small amount of data, the requirement for a computing platform is reduced, and meanwhile, the accuracy of alignment is improved. By designing the invariant Kalman filter, the problem of inaccurate error estimation caused by high system instability under a dynamic condition is solved, and the method has important engineering practice significance.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, relates to an alignment method of a strapdown inertial reference system, and in particular to a fast alignment method of a moving base of an unmanned pod strapdown inertial reference system, and is suitable for fast initial alignment of a strapdown inertial navigation system with GNSS external observation information during movement. Background Art

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

[0003] The strapdown inertial reference system can output the attitude, velocity, and position information of the carrier in real time, provide the necessary navigation parameter information for the normal operation of the optoelectronic pod system, and is the core key unit of the capture, tracking, and aiming system. Therefore, it is widely used in unmanned pod control systems. However, due to the dead reckoning navigation output characteristics of the strapdown inertial reference system, the accuracy of the initial attitude will have an important impact on the subsequent navigation accuracy. Therefore, for the strapdown inertial reference system, initial alignment is a key core technology. According to the different motion states of the carrier during the alignment process, it can be divided into static base alignment and dynamic base alignment. Static base alignment refers to the initial alignment of the strapdown inertial reference system when the carrier is in a static or quasi-static state, and dynamic base alignment refers to the initial alignment of the inertial navigation system during the motion of the carrier. In the actual use of the strapdown inertial reference system, the external constraints for static base alignment are difficult to meet. Therefore, the dynamic base alignment technology without strict restrictions on the motion state of the carrier has been one of the research focuses of the strapdown inertial reference system alignment technology in recent years.

[0004] Traditional dynamic base alignment technology usually requires coarse alignment and fine alignment to be completed relatively independently in sequence. However, under dynamic conditions, the uncertainty of navigation information measurement is high, and both coarse and fine alignment processes require a long convergence time, which limits the rapid startup capability of the inertial reference system. At the same time, the stability of the carrier in motion is poor, which easily leads to large navigation errors in the strapdown inertial reference system during the alignment process, which significantly increases the nonlinear characteristics of the system, making it difficult for the linear filter to accurately estimate and compensate for the error.

[0005] The present invention designs a fast alignment method for a moving base of an unmanned pod strapdown inertial reference system based on an invariant Kalman filter in view of the existing problems. The method uses a retrospective alignment framework under an inertial system, and only needs to store a small amount of calculation results in a rough alignment process for a fine alignment process, thereby solving the problem that the initial alignment time of the strapdown inertial reference system is too long, and at the same time, compared with the traditional method, the storage capacity is reduced and the calculation efficiency is improved; in the retrospective rough alignment process, the method designs an optimized alignment method based on singular value decomposition, thereby solving the problem that the uncertainty of the rough alignment result under dynamic conditions is large; in the retrospective fine alignment process, the method uses an open-loop invariant Kalman filter to estimate and compensate for the navigation information, thereby solving the problem of inaccurate error estimation caused by the nonlinear effect of the system. Using the above method, the strapdown inertial reference system can complete the fast initial alignment under motion conditions with the aid of a global navigation satellite system (GNSS) for observation. Summary of the invention

[0006] The present invention proposes a method for fast alignment of a strapdown inertial reference system moving base of an unmanned pod, the method uses a backtracking alignment framework under an inertial system, stores part of the calculation results in a rough alignment process for a fine alignment process, and uses an invariant Kalman filter to estimate and compensate for the nonlinear error of the system, thereby realizing fast initial alignment of the moving base of the strapdown inertial reference system under GNSS-assisted observation. The present invention has low storage cost, little influence of the backtracking process on the navigation process, and the filtering process is not easily affected by the nonlinear characteristics of the system, and the alignment accuracy can meet the fast alignment requirements of the strapdown inertial reference system, and has important engineering practical value.

[0007] In order to solve the above technical problems, the solution proposed by the present invention is:

[0008] A method for quickly aligning a moving base of an unmanned pod strapdown inertial reference system comprises the following steps:

[0009] (1) Establish a retrospective rough alignment scheme based on singular value decomposition. The specific steps are as follows:

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

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

[0012]

[0013] 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 system 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 can be directly solved and expressed as:

[0014]

[0015] Among them, t represents the current time; t 0 It indicates the initial moment of alignment; the e system indicates 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; Indicates t 0 The direction cosine matrix between the time e system and the time n system; λ 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;

[0016] (1.2) Update the direction cosine matrix of the load system relative to the initial load system

[0017] 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:

[0018] The angle increment θ of the gyro twin output is compensated by the cone compensation algorithm:

[0019]

[0020] Among them, △θ 1 is the angle increment at the previous moment, △θ 2 is the gyro angle increment at the next moment;

[0021] Construct the quaternion angle increment matrix Θ:

[0022]

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

[0024] The attitude quaternion update process is:

[0025]

[0026] 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-based system and the initial time-based system, and

[0027] t k+1 The quaternion of the attitude 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;

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

[0029] (1.3.1) Define vector 1 α(t) and vector 2 β(t), and determine their expressions as:

[0030]

[0031] 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 under the system; It represents the velocity vector of the carrier relative to the Earth-centered inertial system i at time t. The projection under the system; Indicates t 0 At time i, the velocity vector of the carrier relative to the Earth-centered inertial system is The projection under the system; Represents n series and Direction cosine matrix between systems; It 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 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), and the update frequency is the twin-sample output frequency of the strapdown inertial reference system:

[0033]

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

[0035] Among them, α(t k+1 ) means tk +1 The calculated value of the time vector α, α(t k ) represents the calculated value of the vector α at time tk, and the initial value is α(t 0 );v g (t k+1 ) means tk +1 The velocity increment v caused by gravity at time g The calculated value of v g (t k ) represents the calculated value of the velocity increment vg caused by gravity at time tk, with an initial value of v g (t 0 ); Indicates that the b system at time tk is The direction cosine matrix between represents the relationship between system b and t at time t k The direction cosine matrix between time b systems;

[0036] (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. According to the GNSS data update frequency, t K Time speed Location And the integral result α(t K ), β(t K ),v g (t K ) is stored and used for back-tracking the Kalman filter of the precise alignment process, except Except for the 3×3 matrix, the other storage variables are three-dimensional column vectors;

[0037] (1.4) Solve using the optimization method based on singular value decomposition

[0038] The α(t K ), β(t K ) N time intervals △Ti, i = 1, 2, 3, ..., N are selected from the calculation results; where △T i represents the i-th time interval, corresponding to the time interval from t m Time to t n The time interval between moments satisfies m,n∈K and m <n;

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

[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 that satisfy 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 time obtained by backtracking rough alignment and the stored in step (1.3.3) α(t K ), β(t K ) and v g (tK ), establish a backtracking precise alignment scheme based on the invariant Kalman filter, the specific steps are:

[0050] (2.1) Update the system status using the data stored in the backtracking rough alignment phase 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 carrier's speed, position and attitude 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; Indicates t 0 At the moment, the position vector of the carrier relative to the Earth-centered inertial system i is The projection under 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;

[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 the accelerometer assembly are expressed as:

[0062]

[0063] in, and Represent the actual output of gyroscope and accelerometer respectively; and Represent the measurement errors of gyro and accelerometer respectively; w g and w a represents the Gaussian white noise of the gyroscope and accelerometer respectively; ε b and is the constant zero 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 condition of perturbation analysis is used to determine the inertial system The error differential equation is:

[0067]

[0068] in, Represents the load system b system 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 theory, the right invariant error matrix η is established. R :

[0071]

[0072] Among them, χ 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 nonlinear velocity error; represents 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 follows:

[0074]

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

[0076]

[0077] Among them, 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 is expressed as:

[0078]

[0079] (·×) represents the antisymmetric matrix corresponding to the · vector; 0 m×n represents a zero matrix of m rows and n columns;

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

[0081] The nonlinear error in step (2.2) is observed using the velocity and position information output by GNSS:

[0082]

[0083] 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 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;υ vrepresents the velocity observation noise of the GNSS system; υ r represents the GNSS system position observation noise;

[0084] (2.5) Determine the system covariance matrix of the filter at the initial moment:

[0085] 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:

[0086]

[0087] 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;

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

[0089] 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. The error is estimated and compensated to the navigation information at once after the alignment phase. In the open-loop Kalman filtering process,

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

[0091] Furthermore, 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:

[0092]

[0093] Furthermore, when calculating the integrals of the vectors α(t) and β(t) in step (1.3.2), the following discrete calculation scheme is adopted:

[0094]

[0095] α(t 0 )=0 3×1 ,v g (t 0 )=0 3×1 ;

[0096] Among them, △v 1 is the velocity increment output of the accelerometer at the previous moment, △v 2 It is the velocity increment output of the accelerometer at the next moment;

[0097] Furthermore, in the step (1.4), the cross-selection time interval ΔT is determined. i The following selection scheme is adopted:

[0098] Assume t 2 The time is the alignment end time, and the time interval length is determined as △T i =(t 2 -t 0 ) / 4, the starting time of the first time interval is t 0 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 t 2 At the moment, a total of 7 time intervals are selected, that is, N = 7;

[0099] Furthermore, when initializing the error covariance matrix at the initial moment in the step (2.5), the influence of the 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:

[0100]

[0101] Where T is the nonlinear incidence matrix, They are In t 0 The value of the moment;

[0102] Furthermore, when performing error compensation on the navigation information in the step (2.6), the following compensation scheme is adopted:

[0103]

[0104] 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 χ.

[0105] In summary, the advantages and positive effects of the present invention are as follows: the present invention uses the data of the coarse alignment stage of the strapdown inertial reference system for the fine alignment stage by backtracking alignment, so as to improve the alignment accuracy within a limited alignment time, and only needs to store a small amount of data to complete the above process, which reduces the requirements for the computing platform. At the same time, 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. BRIEF DESCRIPTION OF THE DRAWINGS

[0106] Figure 1 It is a flow chart provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0107] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0108] In some special application environments, such as when an aircraft performs an emergency mission or the inertial navigation system restarts during a mission, the strapdown inertial navigation system does not have the external conditions for long-term static base initial alignment and needs to be aligned on a dynamic base. The traditional dynamic base alignment scheme has a slow alignment speed, which will affect the startup speed and alignment accuracy of the inertial navigation system. In addition, the traditional velocity and position error model does not consider the nonlinear problem caused by the inconsistency of the projection coordinate system, resulting in the system being unable to accurately estimate the error in a dynamic environment.

[0109] In order to solve the above technical problems, the present invention proposes a fast alignment method of a moving base based on an invariant Kalman filter, the method is as follows: Figure 1 As shown, the specific implementation method is as follows:

[0110] (1) Establish a retrospective rough alignment scheme based on singular value decomposition. The specific steps are as follows:

[0111] (1.1) Define the projection coordinate system and determine the calculation method of the direction cosine matrix:

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

[0113]

[0114] 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 system 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; Indicates i n0 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 can be directly solved and expressed as:

[0115]

[0116] Among them, t represents the current time; t 0 It indicates the initial moment of alignment; the e system indicates 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; Indicates t 0 The direction cosine matrix between the time e system and the time n system; λ 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;

[0117] (1.2) Update the direction cosine matrix of the load system relative to the initial load system

[0118] 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:

[0119] The angle increment θ of the gyro twin output is compensated by the cone compensation algorithm:

[0120]

[0121] Among them, △θ 1 is the angle increment at the previous moment, △θ 2 is the gyro angle increment at the next moment;

[0122] Construct the quaternion angle increment matrix Θ:

[0123]

[0124] Among them, θ(1), θ(2), θ(3) are the three components of the angle increment θ;

[0125] The attitude quaternion update process is:

[0126]

[0127] 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-based system and the initial time-based system, and

[0128] t k+1 The quaternion of the attitude 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;

[0129] (1.3) Calculate two sets of projection vectors for solving

[0130] (1.3.1) Define vector 1 α(t) and vector 2 β(t), and determine their expressions as:

[0131]

[0132] 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 under the system; It represents the velocity vector of the carrier relative to the Earth-centered inertial system i at time t. The projection under the system; Indicates t 0 At time i, the velocity vector of the carrier relative to the Earth-centered inertial system is The projection under the system; Represents n series and Direction cosine matrix between systems; It 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 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;

[0133] (1.3.2) Update α(t) and β(t), and the update frequency is the twin-sample output frequency of the strapdown inertial reference system:

[0134]

[0135] α(t 0 )=0 3×1 ,v g (t 0 )=0 3×1 ;

[0136] Among them, α(t k+1 ) means tk +1 The calculated value of the time vector α, α(t k ) represents the calculated value of the vector α at time tk, and the initial value is α(t 0 );v g (t k+1 ) means tk +1 The velocity increment v caused by gravity at time g The calculated value of v g (t k ) represents the calculated value of the velocity increment vg caused by gravity at time tk, with an initial value of v g (t 0 ); Indicates that the b system at time tk is The direction cosine matrix between represents the relationship between system b and t at time t k The direction cosine matrix between time b systems;

[0137] (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. According to the GNSS data update frequency, t K Time speed Location And the integral result α(t K ), β(t K ),v g (tK ) is stored and used for back-tracking the Kalman filter of the precise alignment process, except Except for the 3×3 matrix, the other storage variables are three-dimensional column vectors;

[0138] (1.4) Solve using the optimization method based on singular value decomposition

[0139] The α(t K ), β(t K ) N time intervals △Ti, i = 1, 2, 3, ..., N are selected from the calculation results; where △T i represents the i-th time interval, corresponding to the time interval from t m Time to t n The time interval between moments satisfies m,n∈K and m <n;

[0140] definition Where α(t m ) represents the calculated value of vector α at time tm; α(t n ) represents the calculated value of vector α at time tn; β(t m ) represents the calculated value of vector β at time tm; β(t n ) represents the calculated value of vector β at time tn;

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

[0142]

[0143] Performing singular value decomposition on M yields:

[0144] M=UDV T ;

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

[0146] make,

[0147]

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

[0149]

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

[0151] (2.1) Update the system status using the data stored in the backtracking rough alignment phase and

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

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

[0154]

[0155] 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

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

[0157] Update the carrier's speed, position and attitude according to the period △τ of the back-tracking coarse alignment stored data:

[0158]

[0159] in, v g (t K+1 ),v g (t K ), α(t K+1 ), α(t K ), C b ib0 (t K+1 ) 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; Indicates t 0 At the moment, the position vector of the carrier relative to the Earth-centered inertial system i is The projection under 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;

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

[0161] (2.2.1) Sensor error modeling:

[0162] The output of the gyro assembly and the accelerometer assembly are expressed as:

[0163]

[0164] in, and Represent the actual output of gyroscope and accelerometer respectively; and Represent the measurement errors of gyro and accelerometer respectively; w g and w a represents the Gaussian white noise of the gyroscope and accelerometer respectively; ε b and is the constant zero bias of the gyro and accelerometer assembly, that is

[0165]

[0166] (2.2.2) Navigation error modeling:

[0167] 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 condition of perturbation analysis is used to determine the inertial system The error differential equation is:

[0168]

[0169] in, Represents the load system b system 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;

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

[0171] According to the relevant theory of Lie group, the right invariant error matrix η is established R :

[0172]

[0173] Among them, χ 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 nonlinear velocity error; represents nonlinear position error;

[0174] 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 follows:

[0175]

[0176] (2.3) Determine the filtering state equation:

[0177]

[0178] Among them, 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 is expressed as:

[0179]

[0180]

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

[0182] (·×) represents the antisymmetric matrix corresponding to the · vector; 0 m×n represents a zero matrix of m rows and n columns;

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

[0184] The nonlinear error in step (2.2) is observed using the velocity and position information output by GNSS:

[0185]

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

[0187] 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 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 velocity observation noise of the GNSS system; υ r represents the GNSS system position observation noise;

[0188] (2.5) Determine the system covariance matrix of the filter at the initial moment:

[0189] 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:

[0190]

[0191] 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;

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

[0193] 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. The error is estimated and compensated to the navigation information at once after the alignment phase. In the open-loop Kalman filtering process,

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

[0195] Furthermore, 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:

[0196]

[0197] Furthermore, when calculating the integrals of the vectors α(t) and β(t) in step (1.3.2), the following discrete calculation scheme is adopted:

[0198]

[0199] α(t 0 )=0 3×1 ,v g (t 0 )=0 3×1 ;

[0200] Among them, △v 1 is the velocity increment output of the accelerometer at the previous moment, △v 2 It is the velocity increment output of the accelerometer at the next moment;

[0201] Furthermore, in the step (1.4), the cross-selection time interval ΔT is determined. i The following selection scheme is adopted:

[0202] Assume t 2 The time is the alignment end time, and the time interval length is determined as △T i =(t 2 -t 0 ) / 4, the starting time of the first time interval is t 0 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 t 2 At the moment, a total of 7 time intervals are selected, that is, N = 7;

[0203] Furthermore, when initializing the error covariance matrix at the initial moment in the step (2.5), the influence of the 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:

[0204]

[0205] P'=TPT T ;

[0206] Where T is the nonlinear incidence matrix, They are In t 0 The value of the moment;

[0207] Furthermore, when performing error compensation on the navigation information in the step (2.6), the following compensation scheme is adopted:

[0208]

[0209] 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 χ.

[0210] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. All technical solutions within the scope of the present invention belong to the protection scope of the present invention. Some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

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 rough 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 system 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 can be directly solved and expressed as: Among them, t represents the current time; t0 represents the initial time of alignment; e system 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), θ(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-based system and the initial time-based system, and t k+1 The quaternion of the attitude 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 for solving (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 under the system; It represents the velocity vector of the carrier relative to the Earth-centered inertial system i at time t. The projection under the system; It represents the velocity vector of the carrier relative to the Earth-centered inertial system i at time t0. The projection under the system; Represents n series and Direction cosine matrix between systems; It 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 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; (1.3.2) Update α(t) and β(t), and the update frequency is the twin-sample output frequency of the strapdown inertial reference system: α(t0)=0 3×1 ,v g (t0)=0 3×1 ; Among them, α(t k+1 ) indicates t k+1 The calculated value of the time vector α, α(t k ) indicates t k The calculated value of the time vector α, the initial value is α(t0); v g (t k+1 ) indicates t k+1 The velocity increment v caused by gravity at time g The calculated value of v g (t k ) indicates t k The velocity increment v caused by gravity at time g The calculated value, the initial value is v g (t0); Indicates that the b system at time tk is The direction cosine matrix between represents the relationship between system b and t at time t k The direction cosine matrix between time b systems; (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. According to the GNSS data update frequency, t K Time speed Location And the integral result α(t K ), β(t K ),v g (t K ) is stored and used for back-tracking the Kalman filter of the precise 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 ) N time intervals △Ti, i = 1, 2, 3, ..., N are selected from the calculation results; where △T i represents the i-th time interval, corresponding to the time interval from t m Time to t n The time interval between moments satisfies m,n∈K and m <n; definition Where α(t m ) indicates t m The calculated value of the time vector α; α(t n ) indicates t n The calculated value of the time vector α; β(t m ) indicates t m The calculated value of the time vector β; β(t n ) indicates t n The calculated value of the time vector β; Construct 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 that satisfy 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 time obtained by backtracking rough alignment and the stored in step (1.3.3) α(t K ), β(t K ) and v g (t K ), establish a backtracking precise alignment scheme based on the invariant Kalman filter, the specific steps are: (2.1) Update the system status using the data stored in the backtracking rough alignment phase 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 carrier's speed, position and attitude 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 under 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 the accelerometer assembly are expressed as: in, and Represent the actual output of gyroscope and accelerometer respectively; and Represent the measurement errors of gyro and accelerometer respectively; w g and w a represents the Gaussian white noise of the gyroscope and accelerometer respectively; ε b and is the constant zero 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 condition of perturbation analysis is used to determine the inertial system The error differential equation is: in, Represents the load system b system 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; (2.2.3) Define the nonlinear error model: According to the relevant theory of Lie group, the right invariant error matrix η is established R : Among them, χ 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 nonlinear velocity error; represents 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 follows: (2.3) Determine the filtering state equation: Among them, 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 is 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 of 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: V=[(υ v ) T (u r ) T ] T ; 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 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 velocity observation noise of the GNSS system; υ 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 an 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. The error is estimated and compensated to the navigation information at once after the alignment phase. 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, characterized in that: 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, characterized in that: 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, characterized in that: 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, and a total of 7 time intervals are selected, that is, N=7.

5. The method for rapid alignment of a moving base of an unmanned pod strapdown inertial reference system according to claim 1, characterized in that: When initializing the error covariance matrix at the initial moment in the step (2.5), the influence of the 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 incidence matrix, They are The value at time t0.

6. The method for rapid alignment of a moving base of an unmanned pod strapdown inertial reference system according to claim 1, characterized in that: When performing error compensation on the navigation information in the 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