Anti-shake alignment method combining integration and rotation modulation

Abstract

The invention discloses an integral and rotation modulation combined anti-shaking alignment method, which comprises the following steps of: analyzing the principle of a rotation modulation technology in the aspect of error compensation, and constructing an output model of an inertial device under the condition of shaking a base; integral-based inertial system coarse alignment is deduced, the influence of angle shake is eliminated through attitude updating, and the interference of linear vibration is weakened by comparing force integral; establishing a system state and a measurement equation under a shaking condition, and performing optimal estimation of fine alignment by adopting a Kalman filtering algorithm for feedback correction; according to the method, the interference problem in a shaking environment is successfully overcome, the initial alignment precision of the system is remarkably improved, and the adaptive capacity of the SINS in application in a complex environment is enhanced.

Description

Technical Field

[0001] The invention relates to the technical field of strapdown inertial navigation systems, in particular to an anti-shake alignment method combining integral and rotation modulation. Background Art

[0002] SINS is a fully autonomous navigation system widely used in moving vehicles such as aircraft, vehicles, rockets, and ships. Before performing navigation tasks, SINS needs to complete the initial alignment task to ensure that navigation parameters such as attitude, position, and velocity of the navigation position can be accurately calculated. SINS can achieve high-precision alignment in a short time by completing the two stages of coarse alignment and fine alignment. When SINS is in a shaking environment, if the analytical coarse alignment method is still used, it will be difficult to obtain effective initial conditions for the subsequent fine alignment, resulting in failure to meet the alignment accuracy requirements.

[0003] In order to solve the problem that SINS is difficult to achieve effective alignment in a sloshing environment, Wang Yongjun et al. proposed the H ∞ Robust filtering algorithm, and focused on the influence of the selection of γ factor on the alignment performance in the self-alignment of shipborne inertial navigation system. It was found that a smaller γ value helps to improve the SINS alignment stability, but will lead to a decrease in filtering accuracy; Zhou Yue et al. proposed an algorithm for initial alignment of SINS under a shaking base based on the inertial system attitude determination method of Davenport-q recursive algorithm, and adopted a velocity vector observation method to deal with external interference, effectively suppressing noise interference; Li Qian, Di Jingbo et al. proposed a large misalignment angle anti-interference alignment method based on the gravity acceleration cone projection angle drift, which effectively improved the alignment accuracy of SINS. However, the alignment accuracy of SINS is limited by the constant error of the inertial device itself, which seriously restricts the further improvement of accuracy. Summary of the invention

[0004] In order to solve the problems existing in the prior art, the purpose of the present invention is to provide an anti-shake alignment method combining integral and rotation modulation. The present invention not only successfully overcomes the interference problem in a shaking environment, but also significantly improves the initial alignment accuracy of the system and enhances the adaptability of SINS in complex environments.

[0005] To achieve the above object, the technical solution adopted by the present invention is: an anti-shake alignment method combining integral and rotation modulation, comprising the following steps:

[0006] Step 1: Analyze the principle of rotation modulation technology in error compensation and construct an output model of the inertial device under the condition of shaking base;

[0007] Step 2: derive the rough alignment of the inertial system based on integration, eliminate the influence of angular shaking through attitude update, and weaken the interference of linear vibration by contrast integration;

[0008] Step 3: Establish the system state and measurement equations under shaking conditions, and use the feedback-corrected Kalman filter algorithm to perform optimal estimation of precise alignment.

[0009] As a further improvement of the present invention, the step 1 is specifically as follows:

[0010] Assume that the geocentric inertial coordinate system is the i system; the earth coordinate system is the e system; the east-north-sky ENU geographic coordinate system is selected as the navigation coordinate system, that is, the n system; the navigation inertial coordinate system is the n(0) system; the carrier coordinate system is the b system; the carrier inertial coordinate system is the b(0) system; the inertial measurement unit IMU rotation coordinate system is the s system, and initially, the s system coincides with the b system;

[0011] Make the IMU rotate around the z-axis at an angular velocity ω, and get the state transfer matrix of the s system and the b system for:

[0012]

[0013] The gyro constant drift of the three axes of the inertial element is The accelerometer bias is Driven by the rotating mechanism, at time t, the components of the gyroscope constant drift and accelerometer zero bias in the b system are They are:

[0014]

[0015]

[0016] Then integrate equations (2) and (3) over time (0, t), and the angle error δφ in system b at time t is bi (i = x, y, z) and velocity error δν bi (i = x, y, z) is:

[0017]

[0018] The inertial element modulates the IMU constant error on the x-axis and y-axis perpendicular to the z-axis into a periodic signal through its own rotational motion. The principle of rotational modulation is to make the integral value of the pitch angle and roll angle in a complete rotation period zero to eliminate the constant error of the inertial sensor, thereby improving the alignment accuracy of the system.

[0019] When the IMU rotates, the actual output of the gyroscope is The actual output of the accelerometer is Finally, by multiplying the matrix The carrier attitude matrix is ​​obtained; the actual output of the gyroscope and accelerometer in the b system is:

[0020]

[0021] In the above formula, represents the real output of the gyroscope and accelerometer in frame b, represents the projection of the disturbing angular velocity caused by angular shake and linear vibration in the n system, represents the projection of the Earth's rotation angular velocity in the n system, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, represents the projection of the disturbance acceleration caused by the linear vibration in the n system, ε b , Indicates the gyroscope bias and accelerometer bias in frame b, represents the random error of the gyroscope and accelerometer in frame b, f n represents the specific force output under the n system; among them, the interference term is as follows:

[0022]

[0023] (8) In the formula, C represents cosine, S represents sin, θ, γ, and ψ represent the pitch angle, roll angle, and yaw angle, respectively. They represent the pitch angular rate, roll angular rate and yaw angular rate respectively;

[0024]

[0025] (9) where L is the local latitude, h is the local altitude, and R M and R N They represent the curvature radius of the meridian and the meridian respectively. and are the projections of the eastward and northward velocities in the navigation coordinate system respectively;

[0026]

[0027] In formula (10), is the projection of the acceleration caused by linear vibration in the navigation coordinate system, for The velocity ν caused by the line vibration n The projection of the Coriolis acceleration formed by the interaction between them in the navigation coordinate system.

[0028] As a further improvement of the present invention, the step 2 is specifically as follows:

[0029] According to the chain rule:

[0030]

[0031] (11)Where: is the transformation matrix from n system to n(0) system; is the transformation matrix from system b to system n at the initial moment; represents the transformation matrix from b system to b(0) system;

[0032] pass and Update the differential equation of the transformation matrix to obtain and Right now:

[0033]

[0034] (12)Where: is the antisymmetric matrix of the projection of the earth's rotation angular velocity in the n system; is the antisymmetric matrix of the actual output of the gyroscope;

[0035] exist In the process of updating is a constant, and the Rodriguez rotation formula gives:

[0036]

[0037] In the case of non-fixed axis rotation, if the angle increment is used directly to solve A non-commutative rotation error will occur, which is compensated using the equivalent rotation vector algorithm:

[0038]

[0039] Where: represents the state transfer matrix between the b system and the b(0) system at time t-1, α(T) represents the equivalent rotation vector corresponding to the b(0) system transferred to the b system at time t-1, and is obtained by the equivalent rotation vector two-sample algorithm:

[0040]

[0041] (16), and The angular increments are [0, T / 2] and [T2, T] respectively.

[0042] Ignore the specific force equation When the line shake exists, the stem is projected in the b(0) system, and the result is:

[0043]

[0044] Integrating both ends of equation (17) simultaneously, we get:

[0045]

[0046] At the end of the integration, the line vibration interference is negligible, that is, And then:

[0047]

[0048] make Using the dual vector attitude determination principle to determine Right now

[0049]

[0050] As a further improvement of the present invention, the step 3 is specifically as follows:

[0051] In a shaking environment, full consideration Velocity Error Differential Equation and attitude differential equation as follows:

[0052]

[0053] (21) Where: T represents tan, R is the radius of the earth, f i n (i=E, N, U) is the projection of the specific force components in the east, north and sky directions under the navigation system, φ i (i=E, N, U) are the attitude angles in the east, north and sky directions, for The i-th row element in is the projection of the velocity error in the east, north and sky directions in the navigation system; the Kalman filter state equation further constructed is:

[0054]

[0055] (22) In the formula, is the state variable, F=[F1 10×3 F2 10×2 F3 10× 3F4 10×2 ] is the state transfer matrix, where:

[0056]

[0057] is the noise driving matrix, is Gaussian white noise with a mean of 0.

[0058] The true speed of the carrier in a sloshing environment is zero, and the SINS east and north speeds are output As the observed quantity, the measurement equation of Kalman filtering is:

[0059] Z=HX+V(23)

[0060] (23) In the formula, H = [0 2×3 I 2×2 0 2×3 0 2×2 ], V is a Gaussian white noise with a mean of 0.

[0061] Aiming at the problem that the strapdown inertial navigation system (SINS) has insufficient anti-interference ability in a shaking environment, resulting in low alignment accuracy, the present invention proposes an anti-sway alignment method combining integration and rotation modulation. First, the principle of rotation modulation technology in error compensation is analyzed, and the output model of inertial devices under shaking base is constructed; then, the inertial system coarse alignment based on integration is derived, the influence of angular shaking is eliminated through attitude update, and the interference of linear vibration is weakened by contrast force integration; finally, the system state and measurement equations under shaking conditions are established, and the feedback-corrected Kalman filter algorithm is used to perform the optimal estimation of fine alignment; among them, rotation modulation compensates for the constant error of the inertial device itself, the inertial system attitude update eliminates the interference of angular shaking, and the contrast force integration suppresses the influence of linear vibration. Experimental results show that compared with the anti-sway alignment method using only rotation modulation, the horizontal accuracy of this method is improved by about 88%, and the celestial accuracy is improved by about 86%, which greatly improves the alignment accuracy under shaking conditions.

[0062] The beneficial effects of the present invention are:

[0063] In view of the problem that SINS has insufficient anti-interference ability and low alignment accuracy in a shaking environment, the present invention provides an anti-shake alignment method combining integration and rotation modulation; this method compensates for the constant error inherent in the inertial device through rotation modulation, and further effectively eliminates the influence of angular shaking interference through the attitude update of the inertial coordinate system. At the same time, the contrast integration technology is used to suppress the influence of linear vibration on the system, thereby ensuring the stability of the system in a complex vibration environment. In addition, the present invention also introduces a feedback-corrected Kalman filter algorithm to further improve the accuracy and robustness of the initial alignment. Experimental verification shows that the anti-shake alignment method improves the horizontal accuracy by about 88% and the celestial accuracy by about 86% compared with the alignment method that only uses rotation modulation technology. The results show that this method not only successfully overcomes the interference problem in a shaking environment, but also significantly improves the initial alignment accuracy of the system, and enhances the adaptability of SINS in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is a schematic diagram of the vibration disturbance velocity and disturbance Euler angle of the lower carrier in three directions on the shaking base in an embodiment of the present invention;

[0065] Figure 2Schematic diagram of east misalignment angles of three schemes in an embodiment of the present invention;

[0066] Figure 3 Schematic diagram of north misalignment angles of three solutions in an embodiment of the present invention;

[0067] Figure 4 Schematic diagram of antenna misalignment angles of three solutions in an embodiment of the present invention;

[0068] Figure 5 Schematic diagram of misalignment angles in three directions of scheme three in an embodiment of the present invention. DETAILED DESCRIPTION

[0069] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0070] Example

[0071] An anti-shake alignment method combining integral and rotation modulation, comprising:

[0072] 1. Rotational modulation principle and inertial device output model:

[0073] First, the coordinate systems used are marked: the geocentric inertial coordinate system (i system); the earth coordinate system (e system); the east-north-sky (ENU) geographic coordinate system is selected as the navigation coordinate system (n system); the navigation inertial coordinate system (n(0) system); the carrier coordinate system (b system); the carrier inertial coordinate system (b(0) system); the inertial measurement unit (IMU) rotating coordinate system (s system). Initially, the s system coincides with the b system.

[0074] 1.1. Rotation modulation principle:

[0075] By rotating the IMU around the z-axis at an angular velocity ω, the state transfer matrix of the s system and the b system can be obtained. for:

[0076]

[0077] The gyro constant drift of the three axes of the inertial element is The accelerometer bias is Driven by the rotating mechanism, at time t, the components of the gyroscope constant drift and accelerometer zero bias in the b system are They are:

[0078]

[0079] Then integrate equations (2) and (3) over time (0, t), and the angle error δφ in system b at time t is bi (i = x, y, z) and velocity error δν bi (i = x, y, z) is:

[0080]

[0081] It can be seen from equations (4) and (5) that the inertial element modulates the IMU constant error on the x-axis and y-axis perpendicular to the z-axis into a periodic signal through its own rotational motion. The principle of rotational modulation is to eliminate the constant error of the inertial sensor by making the integral value of the pitch angle and roll angle in a complete rotation cycle zero, thereby improving the alignment accuracy of the system. Although the integral value of the modulated signal is not completely zero due to the gradual random error of the inertial sensor, the integral value will eventually tend to zero with continuous rotation. This feature significantly improves the alignment calculation accuracy in actual complex environments, allowing the strapdown inertial navigation system to maintain high-precision alignment capabilities in the presence of random errors.

[0082] 1.2. Inertial device output model:

[0083] When the IMU rotates, the actual output of the gyroscope is The actual output of the accelerometer is Finally, by multiplying the matrix The carrier attitude matrix is ​​obtained. The actual output of the gyroscope and accelerometer in the b system is:

[0084]

[0085] In the above formula, represents the real output of the gyroscope and accelerometer in frame b, represents the projection of the disturbing angular velocity caused by angular shake and linear vibration in the n system, represents the projection of the Earth's rotation angular velocity in the n system, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, represents the projection of the disturbance acceleration caused by the linear vibration in the n system, ε b , Indicates the gyroscope bias and accelerometer bias in frame b, represents the random error of the gyroscope and accelerometer in the b frame, and fn represents the specific force output in the n frame. The interference terms are given below.

[0086]

[0087] (8) In the formula, C represents cosine, S represents sin, θ, γ, and ψ represent the pitch angle, roll angle, and yaw angle, respectively. They represent the pitch angular rate, roll angular rate, and yaw angular rate, respectively.

[0088]

[0089] (9) where L is the local latitude, h is the local altitude, and R M and R N They represent the curvature radius of the meridian and the meridian respectively. and They are the projections of the eastward and northward velocities in the navigation coordinate system, respectively.

[0090]

[0091] In formula (10), is the projection of the acceleration caused by linear vibration in the navigation coordinate system, for The velocity ν caused by the line vibration n The projection of the Coriolis acceleration formed by the interaction between them in the navigation coordinate system.

[0092] 2. Rough alignment of inertial system based on integration:

[0093] According to the chain rule:

[0094]

[0095] (11)Where: is the transformation matrix from n system to n(0) system; is the transformation matrix from system b to system n at the initial moment; Represents the transformation matrix from b system to b(0) system.

[0096] pass and Update the differential equation of the transformation matrix to obtain and Right now:

[0097]

[0098] (12)Where: is the antisymmetric matrix of the projection of the earth's rotation angular velocity in the n system; is the antisymmetric matrix of the actual output of the gyroscope.

[0099] exist In the process of updating is a constant, and the Rodrigues rotation formula gives:

[0100]

[0101] In the case of non-fixed axis rotation, if the angle increment is used directly to solve Non-commutative rotation errors may occur, which are compensated using an equivalent rotation vector algorithm.

[0102]

[0103] Where: represents the state transfer matrix between the b system and the b(0) system at time t-1, α(T) represents the equivalent rotation vector corresponding to the b(0) system transferred to the b system at time t-1, and the equivalent rotation vector two-sample algorithm can be obtained:

[0104]

[0105] (16), and The angular increments are [0, T / 2] and [T / 2, T] respectively.

[0106] The specific force equation The interference when the line shake exists is minimal, so this term is ignored and the projection is performed in the b(0) system. The result is:

[0107]

[0108] Integrating both ends of equation (17) simultaneously, we get:

[0109]

[0110] Since the line vibration interference is approximately a periodic change, the interference gradually decreases with the extension of the integration time. Therefore, at the end of the integration, the line vibration interference can be ignored, that is, And then:

[0111]

[0112] make Using the dual vector attitude determination principle to determine Right now:

[0113]

[0114] 3. Precision alignment:

[0115] In a shaking environment, full consideration should be given to Velocity Error Differential Equation and attitude differential equation as follows:

[0116]

[0117] (21) Where: T represents tan, R is the radius of the earth, f i n(i=E, N, U) is the projection of the specific force components in the east, north and sky directions under the navigation system, φ i (i=E, N, U) are the attitude angles in the east, north and sky directions, for The i-th row element in is the projection of the velocity error in the east, north and sky directions in the navigation system. The Kalman filter state equation further constructed is:

[0118]

[0119] (22) In the formula, is the state variable, F=[F1 10×3 F2 10×2 F3 10× 3F4 10×2 ] is the state transfer matrix, where:

[0120]

[0121] is the noise driving matrix, is Gaussian white noise with a mean of 0.

[0122] The true velocity of the carrier in a sloshing environment should be zero. Output the velocity of the SINS east and north As an observation, the measurement equation of Kalman filtering can be obtained as:

[0123] Z=HX+V(23)

[0124] (:3) In the formula, H = [0 2×3 I 2×2 0 2×3 0 2×2 ], V is a Gaussian white noise with a mean of 0.

[0125] 4. Experimental results and analysis:

[0126] 4.1 Simulation results:

[0127] Matlab is used for simulation. The specific simulation conditions and main parameters of IMU are as follows:

[0128] Table 1 Simulation conditions and main parameters of IMU

[0129]

[0130] The coarse alignment time is set to 30 seconds, and the fine alignment time is set to 270 seconds. The time-varying laws of the pitch angle θ, roll angle γ, and heading angle ψ are as follows:

[0131]

[0132] (22) In the formula, θ m =2°,γ m =2°,ψ m =3° represents the amplitude of pitch angle, roll angle and heading angle respectively; ω θ =π / 6,ω γ =π / 6,ω ψ =π / 9 represent the angular frequencies of pitch, roll and heading angle changes respectively; θ0=π / 4, γ0=π / 4, ψ0=π / 3 represent the initial phases of pitch, roll and heading angle respectively.

[0133] At the same time, the carrier linear velocity model caused by linear vibration interference is:

[0134]

[0135] (25)Where: represent the amplitudes of the eastward, northward, and celestial velocities, respectively; Represent the vibration angular frequencies of the east, north, and celestial velocities respectively; They represent the initial phases of the eastward, northward and celestial velocities respectively.

[0136] The disturbance Euler angles and vibration disturbance velocities of the carrier in three directions on the shaking base are as follows: Figure 1 As shown in Table 2, three alignment schemes are set for comparative verification.

[0137] Table 2 Initial alignment scheme

[0138]

[0139] Comparison of misalignment angles of the three schemes in the east, north and sky directions Figure 2 , Figure 3 and Figure 4 As shown. Figure 2 and Figure 3 It can be clearly seen that under the dual interference conditions of angular shaking and linear vibration, at 300 seconds, the final east misalignment angle of Scheme 1 was 0.3872′, and the north misalignment angle was 0.3556′. In contrast, the east misalignment angle of Scheme 2 was reduced to 0.1711′, and the north misalignment angle was reduced to 0.0869′, which was significantly reduced, indicating that rotation modulation can effectively improve the IMU's own error and improve the alignment accuracy. The east misalignment angle of Scheme 3 was further reduced to 0.0447′, and the north misalignment angle was reduced to 0.0273′, and the alignment accuracy was significantly improved. This shows that the anti-shake alignment method combining integration and rotation modulation can greatly improve the initial alignment effect in a shaking environment and significantly improve the alignment accuracy. Figure 4It shows that at 300 seconds, the final celestial misalignment angle of Scheme 1 is 47.8381′, the celestial misalignment angle of Scheme 2 is reduced to 26.0325′, and that of Scheme 3 is significantly reduced to 3.3898′. Compared with Scheme 2, the horizontal accuracy of Scheme 3 is improved by about 88%, and the celestial accuracy is improved by about 86%. This result shows that with the gradual optimization of the alignment scheme, especially the introduction of Scheme 3, the celestial misalignment angle has been greatly reduced. The performance of Scheme 3 is significantly better than the previous two schemes, which fully demonstrates that this method is extremely effective in improving the celestial alignment accuracy. By combining the anti-shake alignment method of integral and rotational modulation, Scheme 3 can more effectively offset the interference of shake and vibration, thereby greatly improving the accuracy of celestial alignment, providing strong support for solving the alignment problem in a shaking base environment, and also verifying the application potential of this method in complex environments.

[0140] 4.2. Measured results:

[0141] In order to verify the effectiveness of Scheme 3 in practical applications, an actual experimental verification was carried out. The equipment used was the laboratory CHA-7MSD fiber-optic inertial navigation system. The detailed experimental conditions and the main parameters of the CHA-7MSD fiber-optic inertial navigation system are shown in the following table.

[0142] Table 3 Experimental conditions and main parameters of CHA-7MSD fiber-optic inertial navigation system

[0143]

[0144]

[0145] And under the condition of unchanged shaking, the alignment results of scheme 3 in the east, north and sky directions are as follows Figure 5 shown; from Figure 5 The measured results show that the stable values ​​of the east misalignment angle, north misalignment angle and celestial misalignment angle of Scheme 3 are 0.3474′, 0.3449′ and 4.0381′ respectively, showing a high alignment accuracy that can meet the needs of practical applications. This further shows that the anti-shake alignment method of Scheme 3 combined with integration and rotation modulation can successfully eliminate the influence of shake interference, greatly improve the alignment accuracy of SINS, and provide reliable guarantee for precise navigation in complex environments.

[0146] The above-mentioned embodiments only express the specific implementation of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. An anti-shake alignment method combining integral and rotation modulation, characterized in that: The following steps are involved: Step 1: Analyze the principle of rotation modulation technology in error compensation and construct an output model of the inertial device under the condition of shaking base; Step 2: derive the rough alignment of the inertial system based on integration, eliminate the influence of angular shaking through attitude update, and weaken the interference of linear vibration by contrast integration; Step 3: Establish the system state and measurement equations under shaking conditions, and use the feedback-corrected Kalman filter algorithm to perform optimal estimation of precise alignment.

2. The anti-shake alignment method combining integral and rotation modulation according to claim 1, characterized in that: The step 1 is specifically as follows: Assume that the geocentric inertial coordinate system is the i system; the earth coordinate system is the e system; the east-north-sky ENU geographic coordinate system is selected as the navigation coordinate system, that is, the n system; the navigation inertial coordinate system is the n(0) system; the carrier coordinate system is the b system; the carrier inertial coordinate system is the b(0) system; the inertial measurement unit IMU rotation coordinate system is the s system, and initially, the s system coincides with the b system; Make the IMU rotate around the z-axis at an angular velocity ω, and get the state transfer matrix of the s system and the b system for: The gyro constant drift of the three axes of the inertial element is The accelerometer bias is Driven by the rotating mechanism, at time t, the components of the gyroscope constant drift and accelerometer zero bias in the b system are They are: Then integrate equations (2) and (3) over time (0, t), and the angle error δφ in system b at time t is bi (i = x, y, z) and velocity error δν bi (i = x, y, z) is: The inertial element modulates the IMU constant error on the x-axis and y-axis perpendicular to the z-axis into a periodic signal through its own rotational motion. The principle of rotational modulation is to make the integral value of the pitch angle and roll angle in a complete rotation period zero to eliminate the constant error of the inertial sensor, thereby improving the alignment accuracy of the system. When the IMU rotates, the actual output of the gyroscope is The actual output of the accelerometer is Finally, by multiplying the matrix The carrier attitude matrix is ​​obtained; the actual output of the gyroscope and accelerometer in the b system is: In the above formula, represents the real output of the gyroscope and accelerometer in frame b, represents the projection of the disturbing angular velocity caused by angular shake and linear vibration in the n system, represents the projection of the Earth's rotation angular velocity in the n system, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, represents the projection of the disturbance acceleration caused by the linear vibration in the n system, ε b , Indicates the gyroscope bias and accelerometer bias in frame b, represents the random error of the gyroscope and accelerometer in frame b, f n represents the specific force output under the n system; among them, the interference term is as follows: (8) In the formula, C represents cosine, S represents sin, θ, γ, and ψ represent the pitch angle, roll angle, and yaw angle, respectively. They represent the pitch angular rate, roll angular rate and yaw angular rate respectively; (9) where L is the local latitude, h is the local altitude, and R M and R N They represent the curvature radius of the meridian and the meridian respectively. and are the projections of the eastward and northward velocities in the navigation coordinate system respectively; In formula (10), is the projection of the acceleration caused by linear vibration in the navigation coordinate system, for The velocity ν caused by the line vibration n The projection of the Coriolis acceleration formed by the interaction between them in the navigation coordinate system.

3. The anti-shake alignment method combining integral and rotation modulation according to claim 2, characterized in that: The step 2 is specifically as follows: According to the chain rule: (11)Where: is the transformation matrix from n system to n(0) system; is the transformation matrix from system b to system n at the initial moment; represents the transformation matrix from b system to b(0) system; pass and Update the differential equation of the transformation matrix to obtain and Right now: (12)Where: is the antisymmetric matrix of the projection of the earth's rotation angular velocity in the n system; is the antisymmetric matrix of the actual output of the gyroscope; exist In the process of updating is a constant, and the Rodriguez rotation formula gives: In the case of non-fixed axis rotation, if the angle increment is used directly to solve A non-commutative rotation error will occur, which is compensated using the equivalent rotation vector algorithm: Where: represents the state transfer matrix between the b system and the b(0) system at time t-1, α(T) represents the equivalent rotation vector corresponding to the b(0) system transferred to the b system at time t-1, and is obtained by the equivalent rotation vector two-sample algorithm: (16), and The angular increments are [0, T / 2] and [T / 2, T] respectively. Ignore the specific force equation When the line shake exists, the stem is projected in the b(0) system, and the result is: Integrating both ends of equation (17) simultaneously, we get: At the end of the integration, the line vibration interference is negligible, that is, And then: make Using the dual vector attitude determination principle to determine Right now 4. The anti-shake alignment method combining integral and rotation modulation according to claim 3, characterized in that: The step 3 is as follows: In a shaking environment, full consideration Velocity Error Differential Equation and attitude differential equation as follows: (21) Where: T represents tan, R is the radius of the earth, f i n (i=E, N, U) is the projection of the specific force components in the east, north and sky directions under the navigation system, φ i (i=E, N, U) are the attitude angles in the east, north and sky directions, for The i-th row element in is the projection of the velocity error in the east, north and sky directions in the navigation system; the Kalman filter state equation further constructed is: (22) In the formula, is the state variable, F=[F1 10×3 F2 10×2 F3 10×3 F4 10×2 ] is the state transfer matrix, where: is the noise driving matrix, W=[(w δv (1:2) T (w φ ) T ] T is Gaussian white noise with a mean of 0. The true speed of the carrier in a sloshing environment is zero, and the SINS east and north speeds are output As the observed quantity, the measurement equation of Kalman filtering is: Z=HX+V (23) (23) In the formula, H = [0 2×3 I 2×2 0 2×3 0 2×2 ], V is a Gaussian white noise with a mean of 0.

Images