A large misalignment angle initial alignment method for vehicle-mounted strapdown inertial navigation system

Abstract

The invention discloses a method for initial alignment of a vehicle-mounted strapdown inertial navigation system with a large misalignment angle, and belongs to the field of navigation technology. The specific steps are: first, establishing a nonlinear model for the initial alignment of the strapdown inertial navigation system; second, retaining nonlinear characteristics in the process of model simplification under the condition of a large misalignment angle; third, using the EKF algorithm recursive process to solve the state space equation of the above model, estimate the misalignment angle, and complete the initial alignment. The initial alignment algorithm for the vehicle-mounted strapdown inertial navigation system with a large misalignment angle proposed by the present invention retains nonlinear factors in the process of model simplification and algorithm solving, thereby improving the accuracy of the model and the stability of the algorithm. Simulation results show that this method can enhance the observability of the celestial misalignment angle and improve the stability of the algorithm, thereby obtaining a better alignment effect.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, and in particular relates to a large misalignment angle initial alignment method for a vehicle-mounted strapdown inertial navigation system. Background Art

[0002] The strapdown inertial navigation system can be used to obtain the position, speed and attitude information of the vehicle. Initial alignment is the basis of the strapdown inertial navigation system, which can determine the initial attitude for the subsequent navigation calculation. The speed of initial alignment determines the responsiveness of the navigation system, and its accuracy directly affects the subsequent navigation accuracy.

[0003] In the initial alignment problem of strapdown inertial navigation system, the establishment of error model and the selection of filtering algorithm are particularly critical. The classic linear differential equation error model is obtained under the condition of small misalignment angle. As the research on initial alignment problem gradually deepens, the small misalignment angle error model and linear Kalman filter show great limitations in practical applications. For example, when the vehicle enters the communication interruption area and the inertial navigation becomes the only available navigation system, or when the system is subject to large interference, or when the inertial element accuracy of the inertial navigation system is not high, the navigation system will have obvious large misalignment angle (for vehicles, large misalignment angle generally appears in the celestial direction). Due to the large misalignment angle, the model presents nonlinear characteristics, and nonlinear filtering algorithms such as EKF (Extended Kalman Filter) should be used.

[0004] When dealing with the nonlinearity of the model, one method is to try to establish a more complete mathematical model so that the attitude angle alignment process is closer to the real process, thereby improving the estimation accuracy. This is bound to increase the complexity of the model and reduce the alignment speed. At the same time, the improvement in accuracy is relatively limited. Another method is to choose a nonlinear filtering algorithm that is more complex than EKF to improve the estimation effect from the algorithm level. Most studies in recent years have adopted this technical route. However, due to the limitations of the computing power of the single-chip microcomputer, complex algorithms are difficult to implement in engineering practice.

[0005] The nonlinearity of the alignment model is mainly reflected in the influence of the actual working environment factors of the vehicle operation on the attitude angle estimation and the nonlinear relationship between the attitude angle and certain internal states. Through continuous research in recent years, it is found that in the error angle differential equation, the matrix related to the misalignment angle will have an important impact on the system under large misalignment angles, which should not be ignored when simplifying the model, that is, the influence of nonlinear factors should be considered, the nonlinear factors existing in the alignment model should be analyzed, and the specific impact of nonlinear factors on the algorithm should be studied, so as to find ways to improve the performance of the algorithm. This method is relatively more basic, and few people in the navigation field consider nonlinear problems in initial alignment from this perspective.

[0006] Therefore, starting from the nonlinear factors of the model, studying a nonlinear initial alignment method with good alignment effect and practical application has positive significance for improving the performance of the vehicle-mounted strapdown inertial navigation system. Summary of the invention

[0007] In order to solve the problem of initial alignment of a vehicle-mounted strapdown inertial navigation system caused by model nonlinearity due to a large misalignment angle, the present invention provides an initial alignment method of a vehicle-mounted strapdown inertial navigation system with a large misalignment angle.

[0008] The technical solution adopted by the present invention is:

[0009] A large misalignment angle initial alignment method for a vehicle-mounted strapdown inertial navigation system comprises the following steps:

[0010] Step 1, establishing a nonlinear model for the initial alignment of the strapdown inertial navigation system;

[0011] Step 2: Under the condition of large misalignment angle, the nonlinear characteristics are retained in the process of model simplification;

[0012] Step 3: Use the EKF algorithm recursive process to solve the state space equation of the above model, estimate the misalignment angle, and complete the initial alignment.

[0013] Furthermore, in step 1, according to the motion relationship between the misalignment angle and the computing platform coordinate system (n' system), the nonlinear attitude error differential equation is obtained:

[0014]

[0015] Where φ is the attitude error, is the angular velocity transformation matrix, I is the unit matrix, is the attitude transformation matrix from the navigation coordinate system (n system) to the computing platform coordinate system (n' system), is the posture transformation matrix from the vehicle body coordinate system (b system) to the computing platform coordinate system (n' system), is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, for The calculation error of is the projection of the angular velocity of the body coordinate system relative to the inertial coordinate system in the body coordinate system, for The calculation error is:

[0016]

[0017] In the formula, φ E ,φ N ,φ U They represent the misalignment angles in the east, north and celestial directions respectively.

[0018] According to the measurement principle of the inertial navigation element accelerometer, the velocity error equation is derived as follows:

[0019]

[0020] Among them, δv n is the speed error, is the specific force measured by the accelerometer, is the projection of the Earth's rotation angular velocity in the navigation coordinate system, is the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the accelerometer measurement error.

[0021] Then the initial alignment nonlinear model of the strapdown inertial navigation system is obtained as follows:

[0022]

[0023] Among them, ε b For gyro drift.

[0024] Furthermore, in step 2, the attitude error differential equation in the initial alignment nonlinear model of the strapdown inertial navigation system involves a large number of matrix multiplications, and the matrix contains a large number of elements, so solving the Jacobian matrix is ​​very cumbersome. Coefficient Matrix The trigonometric functions in are approximated, that is, It can be simplified to the unit matrix I. However, under the condition of large azimuth misalignment angle, this processing method will cause the model to lose some nonlinear characteristics.

[0025] If the attitude error differential equation is Simplified to the unit matrix I, and similarly, when the initial alignment model is linearized using Taylor's formula, some small quantities are further approximated to 0, the following Jacobian matrix F can be obtained:

[0026]

[0027] In the formula, ω N ,ω U They represent the angular velocity in the north direction and the celestial direction respectively, R represents the radius of the earth, and L represents the latitude.

[0028] For the sake of convenience, this commonly used initial alignment method is called the EKF-F algorithm.

[0029] In order to improve the stability and observability of the initial alignment algorithm under large misalignment angle conditions, the influence of nonlinear characteristics in the process of simplifying the error model is fully considered.

[0030] First, the coefficient matrix in the retention model is Do not simplify it to the unit matrix, that is:

[0031]

[0032] Secondly, in the process of solving the Jacobian matrix, the elements related to the azimuth misalignment angle are retained to reduce the error caused by linearization. Thus, the Jacobian matrix A that retains more nonlinear factors is obtained:

[0033]

[0034] It can be seen that compared with the F matrix, the A matrix retains the nonlinear information related to the celestial misalignment angle during the simplification process, that is, the matrix containing φ U The initial alignment method proposed in this patent is called EKF-A algorithm.

[0035] Furthermore, in step three,

[0036] The initial quasi-nonlinear model of the strapdown inertial navigation system in step 2 is linearized and discretized to obtain:

[0037]

[0038] In the formula, x k+1 and x k are the state vectors of the system at time k+1 and time k respectively, A k is the system state transition Jacobian matrix, G k is the noise allocation matrix, W k is the process noise, y k is the observation vector of the system at time k, H k is the observation matrix of the system, V k is the observation noise, and:

[0039]

[0040]

[0041] In the formula, is the velocity error vector at time k, δv E With δv N are the velocity errors in the east and north directions, ε N , ε U They are the gyro drift in the east and north directions respectively.

[0042] The EKF algorithm is used to recursively solve the state space equations of the above model, estimate the misalignment angle, and complete the initial alignment.

[0043] Furthermore, the recursive process of the EKF algorithm can be expressed as follows:

[0044] Initialization of algorithm related parameters:

[0045]

[0046] In the formula, is the initial value of the state quantity; P0 is the initial value of the error covariance; x0 is the true value of the state quantity.

[0047] When k=1,2,……

[0048] Status time update:

[0049]

[0050] In the formula, is the prior estimate of the state quantity, is the posterior estimate of the state quantity.

[0051] Error covariance time update:

[0052]

[0053] Where P k|k-1 is the prior error covariance of the state quantity, P k-1 is the posterior error covariance of the state quantity, Q k-1 is the process noise W k The variance of .

[0054] Kalman gain update:

[0055]

[0056] In the formula, K k is the Kalman gain, R k is the observation noise V k The variance of .

[0057] Status measurement update:

[0058]

[0059] Error covariance update:

[0060] P k =(IK k H k ) k|k-1

[0061] Compared with the prior art, the present invention has the following beneficial effects:

[0062] The present invention aims at the situation that the initial alignment method of the vehicle-mounted strapdown inertial navigation system based on EKF sometimes has poor effect. It is believed that there is a phenomenon of excessive neglect of nonlinear factors in the process of model simplification, and a method of retaining nonlinear factors to improve the stability of the algorithm is proposed. The nonlinear factors in the error model and the Jacobian matrix solution process are retained as much as possible, the model accuracy is improved, and the divergence trend of the algorithm is suppressed. The present invention achieves the improvement of the alignment accuracy and speed of the celestial misalignment angle, so that the algorithm stability and the observability of the celestial misalignment angle are improved; at the same time, because other more complex nonlinear algorithms are difficult to run in the vehicle controller, the initial alignment algorithm based on EKF proposed by the present invention is more practical in engineering. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 The present invention is a flow chart of a large misalignment angle initial alignment method for a vehicle-mounted strapdown inertial navigation system.

[0064] Figure 2 This is a stability comparison chart between the EKF-A algorithm of the present invention and the traditional EKF-F algorithm.

[0065] Figure 3 This is a comparison chart of the observable degree of the celestial misalignment angle between the EKF-A algorithm of the present invention and the traditional EKF-F algorithm.

[0066] Figure 4 This is the alignment effect of the traditional EKF-F algorithm under several different initial value errors.

[0067] Figure 5 This is the alignment effect of the EKF-A algorithm of the present invention under several different initial value errors. DETAILED DESCRIPTION

[0068] The present invention will be further described below in conjunction with specific examples. The examples are used to explain the present invention but are not intended to limit the present invention.

[0069] The present invention provides a large misalignment angle initial alignment method for a vehicle-mounted strapdown inertial navigation system, comprising the following steps:

[0070] Step 1: Establish the initial alignment nonlinear model of the strapdown inertial navigation system.

[0071] According to the motion relationship between the misalignment angle and the computing platform coordinate system (n' system), the nonlinear attitude error differential equation is obtained:

[0072]

[0073] Where φ is the attitude error, is the angular velocity transformation matrix, I is the unit matrix, is the attitude transformation matrix from the navigation coordinate system (n system) to the computing platform coordinate system (n' system), is the posture transformation matrix from the vehicle body coordinate system (b system) to the computing platform coordinate system (n' system), is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, for The calculation error of is the projection of the angular velocity of the body coordinate system relative to the inertial coordinate system in the body coordinate system, for The calculation error is:

[0074]

[0075] In the formula, φ E ,φ N ,φ U They represent the misalignment angles in the east, north and celestial directions respectively.

[0076] According to the measurement principle of the inertial navigation element accelerometer, the velocity error equation is derived as follows:

[0077]

[0078] Among them, δv n is the speed error, is the specific force measured by the accelerometer, is the projection of the Earth's rotation angular velocity in the navigation coordinate system, is the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the accelerometer measurement error.

[0079] Then the initial alignment nonlinear model of the strapdown inertial navigation system is obtained as follows:

[0080]

[0081] Among them, ε b For gyro drift.

[0082] Step 2: Under the condition of large misalignment angle, the nonlinear characteristics are retained in the process of model simplification.

[0083] The attitude error differential equation in the initial alignment nonlinear model of the strapdown inertial navigation system involves a lot of matrix multiplications, and the matrix contains a lot of elements, so solving the Jacobian matrix is ​​very cumbersome. The usual approach is to Coefficient Matrix The trigonometric functions in are approximated and Simplified to unit matrix I. However, under the condition of large azimuth misalignment angle, this processing method will cause the model to lose some nonlinear characteristics.

[0084] If the attitude error differential equation is Simplified to the unit matrix I, and similarly, when the initial alignment model is linearized using Taylor's formula, some small quantities are further approximated to 0, the following Jacobian matrix F can be obtained:

[0085]

[0086] In the formula, ω N ,ω U They represent the angular velocity in the north direction and the celestial direction respectively, R represents the radius of the earth, and L represents the latitude.

[0087] In order to improve the stability and observability of the initial alignment algorithm under large misalignment angle conditions, the influence of nonlinear characteristics in the process of simplifying the error model is fully considered.

[0088] First, the coefficient matrix in the retention model is Do not simplify it to the unit matrix, that is:

[0089]

[0090] Secondly, in the process of solving the Jacobian matrix, the elements related to the azimuth misalignment angle are retained to reduce the error caused by linearization. Thus, the Jacobian matrix A that retains more nonlinear factors is obtained:

[0091]

[0092] It can be seen that compared with the F matrix, the A matrix retains the nonlinear information related to the celestial misalignment angle during the simplification process, that is, the matrix containing φ in the above formula U elements.

[0093] Step 3: Use the EKF algorithm recursive process to solve the state space equation of the above model, estimate the misalignment angle, and complete the initial alignment.

[0094] The initial quasi-nonlinear model of the strapdown inertial navigation system is linearized and discretized to obtain:

[0095]

[0096] In the formula, x k+1 and x k are the state vectors of the system at time k+1 and time k respectively, A k is the system state transition Jacobian matrix, G k is the noise allocation matrix, W k is the process noise, y k is the observation vector of the system at time k, H kis the observation matrix of the system, V k is the observation noise, and:

[0097]

[0098]

[0099] In the formula, is the velocity error vector at time k, δv E With δv N are the velocity errors in the east and north directions, ε N , ε U They are the gyro drift in the east and north directions respectively.

[0100] The EKF algorithm is used to recursively solve the state space equations of the above model, estimate the misalignment angle, and complete the initial alignment.

[0101] The recursive process of the EKF algorithm can be expressed as follows:

[0102] Initialization of algorithm related parameters:

[0103]

[0104] In the formula, is the initial value of the state quantity; P0 is the initial value of the error covariance; x0 is the true value of the state quantity.

[0105] When k=1,2,……

[0106] Status time update:

[0107]

[0108] In the formula, is the prior estimate of the state quantity, is the posterior estimate of the state quantity.

[0109] Error covariance time update:

[0110]

[0111] Where P k|k-1 is the prior error covariance of the state quantity, P k-1 is the posterior error covariance of the state quantity, Q k-1 is the process noise W k The variance of .

[0112] Kalman gain update:

[0113]

[0114] In the formula, K kis the Kalman gain, R k is the observation noise V k The variance of .

[0115] Status measurement update:

[0116]

[0117] Error covariance update:

[0118] P k =(IK k H k ) k|k-1

[0119] The above provides a method for initial alignment of a vehicle-mounted strapdown inertial navigation system with a large misalignment angle. The process is as follows: Figure 1 shown.

[0120] Consider an initial alignment situation, the initial value error of the azimuth misalignment angle is 15°, and the noise variance Q k With R k They are 50*1e-14 and 50*1e-2 respectively. Figure 2 This is a comparison chart of the algorithm stability of EKF-F and EKF-A. It can be seen that at 300s, the alignment error of EKF-F is 63', while the alignment error of EKF-A is 28', which is less volatile than the alignment process of EKF-F. At the end of the 600s simulation, the alignment error of EKF-F is 44', but the curve shows a divergent trend; while the alignment error of EKF-A is 1.6', and the alignment accuracy is greatly improved and remains stable. By retaining nonlinear factors, the EKF-A algorithm significantly improves stability when both the initial value error and the noise variance are large, while greatly improving the alignment accuracy and enhancing the alignment effect.

[0121] Figure 3 The following is a comparison of the observability of the celestial misalignment angle between EKF-F and EKF-A. It can be seen that after 100 seconds, the observability of the celestial misalignment angle of the EKF-A method gradually exceeds that of the EKF-F method. At the end of the simulation, the observability of the celestial misalignment angle of the EKF-A method is 56, while the observability of the celestial misalignment angle of the EKF-F method is 32. The EKF-A method has significantly improved the observability of the celestial misalignment angle.

[0122] The difference between the two methods in alignment effect is further investigated. The simulation conditions are set as follows: the initial azimuth misalignment angles are 25°, 30°, 35°, and 40° respectively. Figure 4 and Figure 5The alignment of the celestial misalignment angle of EKF-F and EKF-A under four groups of initial error conditions is given respectively. It can be seen that with the increase of the initial misalignment angle, the alignment effect of the EKF-F method fluctuates more. Under the initial error of 40°, the error of the celestial misalignment angle at 100s once reached 240′, and then slowly became smaller. The alignment speed of EKF-F was fast in the initial 0-100s, but the error would have an "overshoot" phenomenon at about 100s, and then the alignment speed dropped significantly, the accuracy decreased, and the error remained relatively large. At 200s, the celestial alignment error under the four groups of conditions was 15′~25′, and the final error at the end of the simulation was 10′~15′. The alignment accuracy of EKF-A was higher throughout the whole process, and the curve was smooth. Under the four groups of conditions, the celestial alignment error could converge to 3′~6′ at 200s, and the final error at the end of the simulation was 1′~3′, and the accuracy was significantly improved compared with EKF-F. Therefore, in terms of the alignment effect of the celestial misalignment angle, the EKF-A method that retains the nonlinear factors is better in both alignment accuracy and speed, while the alignment accuracy of the EKF-F method has an upper limit.

[0123] Aiming at the poor stability of the initial alignment algorithm of the strapdown inertial navigation system under large misalignment angle, this paper studies the problem of nonlinear factors and finds that the nonlinear factors are overly ignored in the process of model simplification. It proposes a method to improve the stability of the algorithm and improve the alignment accuracy and speed. The simulation results show that the stability of the improved algorithm and the alignment effect are significantly improved.

[0124] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements can be made without departing from the principle of the present invention, and these improvements should also be considered as the protection scope of the present invention.

Claims

1. A large misalignment angle initial alignment method for a vehicle-mounted strapdown inertial navigation system, characterized in that: The following steps are involved: Step 1: Derived the attitude error differential equation based on the motion relationship between the misalignment angle and the computing platform coordinate system, and derived the velocity error equation based on the measurement principle of the inertial navigation element accelerometer, and then established the initial alignment nonlinear model of the strapdown inertial navigation system: in, is the attitude error, is the angular velocity transformation matrix, I is the identity matrix, is the attitude transformation matrix from the navigation coordinate system n to the computing platform coordinate system n', is the attitude transformation matrix from the vehicle body coordinate system b to the computing platform coordinate system n', is the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, for The calculation error of is the projection of the angular velocity of the body coordinate system relative to the inertial coordinate system in the body coordinate system, for The calculation error of is the speed error, is the specific force measured by the accelerometer, is the projection of the Earth's rotation angular velocity in the navigation coordinate system, is the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the accelerometer measurement error, for gyro drift; Step 2: In order to improve the stability and observability of the initial alignment algorithm under large misalignment angle conditions, the influence of nonlinear characteristics is fully considered during the model simplification process, and these nonlinear characteristics are retained; Step 3: Use the EKF algorithm recursive process to solve the state space equation of the above model, estimate the misalignment angle, and complete the initial alignment; In step 2: Preserving the coefficient matrix in the attitude error differential equation of the nonlinear model of the initial alignment of the strapdown inertial navigation system , instead of simplifying it to the unit matrix, that is: In the formula, Respectively represent the misalignment angles in the east, north and celestial directions; In the process of solving the Jacobian matrix, the elements related to the celestial misalignment angle are retained to reduce the error caused by linearization. Thus, the Jacobian matrix A that retains more nonlinear factors is obtained: In the formula, denote the north and celestial angular velocities respectively, R represents the radius of the Earth, L Indicates latitude; A In the process of simplification, the matrix retains the nonlinear information related to the celestial misalignment angle, that is, elements.

2. The large misalignment angle initial alignment method of a vehicle-mounted strapdown inertial navigation system according to claim 1, characterized in that: In step three, The initial quasi-nonlinear model of the strapdown inertial navigation system in step 2 is linearized and discretized to obtain: In the formula, x k+1 and x k are the state vectors of the system at time k+1 and time k respectively, A k is the system state transfer Jacobian matrix, G k is the noise assignment matrix, W k is the process noise, y k is the observation vector of the system at time k, H k is the observation matrix of the system, V k is the observation noise, and: In the formula, is the velocity error vector at time k, and are the velocity errors in the east and north directions respectively, They are the gyro drifts in the east and north directions respectively; The EKF algorithm is used to recursively solve the state space equations of the above model, estimate the misalignment angle, and complete the initial alignment.

3. The large misalignment angle initial alignment method of a vehicle-mounted strapdown inertial navigation system according to claim 2, characterized in that: The recursive process of the EKF algorithm can be expressed as follows: Initialization of algorithm related parameters: In the formula, is the initial value of the state quantity; is the initial value of the error covariance; is the true value of the state quantity; When k=1,2,... Status time update: In the formula, is the prior estimate of the state quantity, is the posterior estimate of the state quantity; Error covariance time update: Where, is the prior error covariance of the state quantity, is the posterior error covariance of the state quantity, Process noise W k The variance of Kalman gain update: In the formula, is the Kalman gain, is the observation noise V k The variance of Status measurement update: Error covariance update: 。

Images