An inertial navigation initial alignment method and system

Abstract

This invention relates to the field of navigation technology, and more particularly to an inertial navigation initial alignment method and system. The method includes the following steps: applying an excitation signal to the excitation electrodes according to a pre-set standing wave angle to cause a hemispherical resonator gyroscope to oscillate; the hemispherical resonator gyroscope enters a force feedback mode, and the standing wave angle stabilizes at a pre-set position; controlling the hemispherical resonator gyroscope to enter a full-angle mode, allowing the standing wave angle to precess freely; coarse alignment to obtain a coarse initial direction cosine matrix; the hemispherical resonator gyroscope enters a rotation modulation mode, controlling the standing wave angle to change periodically and uniformly; fine alignment to obtain the precise attitude value of the hemispherical resonator gyroscope; after fine alignment, switching the rotation modulation mode back to the full-angle mode, and using the precise attitude value of the hemispherical resonator gyroscope for subsequent navigation calculations. The method and system provided by this invention improve the startup speed of the hemispherical resonator gyroscope, achieve zero drift integral over the entire cycle, and improve the initial alignment accuracy of the hemispherical resonator gyroscope.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to an inertial navigation initial alignment method and system. Background Art

[0002] Initial alignment technology is one of the important technologies that determine the accuracy of inertial navigation. How to improve the speed and accuracy of initial alignment to meet the use requirements of different carriers is a key issue that needs to be solved urgently. As the core component of inertial navigation, the performance level of the gyroscope determines the initial alignment accuracy of inertial navigation. The traditional hemispherical resonant gyro inertial navigation initial alignment technology mainly improves the alignment accuracy by improving the gyro accuracy level. This method is technically difficult and is limited by the processing and manufacturing process. Once the gyroscope accuracy reaches a certain level, it is difficult to continue to improve. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide an inertial navigation initial alignment method and system, which actively controls the precession of standing waves by reasonably switching between three working modes: force feedback, full angle, and rotation modulation, so that the standing wave angle is within one cycle. The interval changes evenly, so that the gyro drift is suppressed due to averaging within a period, and the gyro drift self-modulation is realized, which effectively improves the gyro accuracy level.

[0004] The present invention is achieved through the following technical solutions:

[0005] An inertial navigation initial alignment method comprises the following steps:

[0006] S1: After the inertial navigation system is powered on, an excitation signal is applied to the excitation electrode according to the preset standing wave angle to start the hemispherical resonant gyroscope.

[0007] S2: Start the force feedback control loop, control the hemispherical resonant gyroscope to enter the force feedback mode, and control the standing wave angle to stabilize at a preset position;

[0008] S3: Close the force feedback control loop, open the full-angle control loop, control the hemispherical resonant gyroscope to enter the full-angle mode, and the standing wave angle precesses freely;

[0009] S4: Coarsely align the inertial navigation system and obtain a rough initial direction cosine matrix;

[0010] S5: After the rough alignment is completed, the rotation modulation control loop is turned on to make the hemispherical resonant gyro enter the rotation modulation mode and control the standing wave angle to Internal periodic uniform changes;

[0011] S6: Using the rough initial direction cosine matrix as the initial value, perform fine alignment to obtain the precise direction cosine matrix and obtain the precise attitude value of the hemispherical resonant gyroscope;

[0012] S7: After the fine alignment is completed, the rotation modulation control loop is closed, the rotation modulation mode is switched to the full-angle mode, and the subsequent navigation solution is performed with the precise attitude value of the hemispherical resonant gyro.

[0013] Furthermore, in step S1, an excitation signal is applied to the excitation electrode according to formula (1) to start the hemispherical resonant gyroscope:

[0014] (1);

[0015] in: Indicates the excitation electrode The excitation signal applied, Indicates the excitation electrode The excitation signal applied, represents the excitation voltage, represents the standing wave angle, represents the initial phase angle, represents the resonant frequency, Indicates time.

[0016] Furthermore, in step S4, a rough alignment is performed according to equation (2) to obtain a rough initial direction cosine matrix:

[0017] (2);

[0018] in: is the rough initial direction cosine matrix, is the acceleration vector measured by the accelerometer, is the angular velocity vector measured by the hemispherical resonant gyroscope, Transpose the matrix.

[0019] Optimized, in step S6, the rough initial direction cosine matrix is ​​used as the initial value, and the Kalman filter method is used for fine alignment.

[0020] Further, the precise alignment by the Kalman filter method in step S6 includes the following steps:

[0021] S61: Establish Kalman filter state equation;

[0022] S62: predicting and measuring the Kalman filter state;

[0023] S63: Perform precise alignment posture update.

[0024] Furthermore, the Kalman filter state equation established in step S61 is formula (3):

[0025] (3);

[0026] in: express The state quantity at the moment, express The state quantity at the moment, express The state transfer matrix at time, express The system noise at the moment, express Time measurement, express The moment measurement matrix, express The moment measurement noise matrix;

[0027] Further, step S62 predicts the Kalman filter state according to equation (4):

[0028] (4);

[0029] in: express The one-step prediction value of the state quantity at a moment, express The estimated value of the state quantity at the moment, express The moment error covariance matrix is ​​the one-step prediction value, express The estimated value of the moment error covariance matrix, express The state transfer matrix at time, express The system noise matrix at time instant, Transpose the matrix;

[0030] Further, step S62 performs measurement update according to equation (5):

[0031] (5);

[0032] in: express The moment filter gain matrix, express The moment measurement matrix, express The noise matrix is ​​measured at each moment, express The estimated value of the state quantity at the moment, express The estimated value of the moment error covariance matrix, represents the identity matrix, Transpose the matrix.

[0033] Further, in step S63, fine alignment is performed according to formula (6):

[0034] (6);

[0035] in: represents the exact direction cosine matrix, represents the rough initial direction cosine matrix, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction.

[0036] An inertial navigation initial alignment system, used for executing an inertial navigation initial alignment method as described in any one of the above, comprises a hemispherical resonant gyroscope, an excitation electrode, a force feedback control loop, a full-angle control loop and a rotation modulation control loop, wherein there are three hemispherical resonant gyroscopes, each of which is provided with a pair of excitation electrodes, and the force feedback control loop, the full-angle control loop and the rotation modulation control loop are respectively connected between the pair of excitation electrodes.

[0037] Beneficial effects of the invention:

[0038] The present invention provides an inertial navigation initial alignment method and system. In the initial alignment stage of an inertial navigation system, a gyroscope is controlled to be in different working modes to increase the starting speed of the gyroscope. In a rotation modulation mode, the gyroscope standing wave angle is actively controlled to periodically precess, so that the drift integral in the entire period is zero, thereby improving the initial alignment accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a schematic diagram of the process of the present invention.

[0040] Figure 2 It is a schematic diagram of the system of the present invention.

[0041] In the figure: 1. Hemispherical resonant gyroscope; 2. Excitation electrode; 3. Force feedback control loop; 4. Full-angle control loop; 5. Rotation modulation control loop. DETAILED DESCRIPTION

[0042] An inertial navigation initial alignment method includes the following steps, and its flow chart is as follows: Figure 1 As shown:

[0043] S1: After the inertial navigation system is powered on, an excitation signal is applied to the excitation electrode according to the preset standing wave angle to start the hemispherical resonant gyroscope.

[0044] Specifically, an excitation signal can be applied to the excitation electrode according to formula (1) to start the hemispherical resonant gyroscope:

[0045] (1);

[0046] in: Indicates the excitation electrode The excitation signal applied, Indicates the excitation electrode The excitation signal applied, represents the excitation voltage, represents the standing wave angle, represents the initial phase angle, represents the resonant frequency, Indicates time.

[0047] S2: Start the force feedback control loop, control the hemispherical resonant gyroscope to enter the force feedback mode, and control the standing wave angle to stabilize at a preset position;

[0048] In this way, after the hemispherical resonant gyroscope is started, the standing wave angle is at a preset position. Compared with the random start-up method, the time for controlling the precession of the standing wave angle is saved, and the starting speed of the hemispherical resonant gyroscope is improved.

[0049] S3: Close the force feedback control loop, open the full-angle control loop, control the hemispherical resonant gyroscope to enter the full-angle mode, and the standing wave angle precesses freely;

[0050] S4: Coarsely align the inertial navigation system and obtain a rough initial direction cosine matrix;

[0051] Specifically, we can perform coarse alignment according to formula (2) to obtain a rough initial direction cosine matrix:

[0052] (2);

[0053] in: is the rough initial direction cosine matrix, is the acceleration vector measured by the accelerometer, is the angular velocity vector measured by the hemispherical resonant gyroscope, Transpose the matrix.

[0054] S5: After the rough alignment is completed, the rotation modulation control loop is turned on to make the hemispherical resonant gyro enter the rotation modulation mode and control the standing wave angle to Internal periodic uniform changes;

[0055] In this process, the gyro drift value changes periodically and evenly with the change of the standing wave angle, and the integral of the gyro drift value in the whole cycle is zero. Compared with the full-angle mode, the precision level of fine alignment can be greatly improved.

[0056] S6: Using the rough initial direction cosine matrix as the initial value, perform fine alignment to obtain the precise direction cosine matrix and obtain the precise attitude value of the hemispherical resonant gyroscope;

[0057] Specifically, a rough initial direction cosine matrix can be used as an initial value, and a Kalman filter method can be used for fine alignment.

[0058] Further, the fine alignment by the Kalman filter method includes the following steps:

[0059] S61: Establish Kalman filter state equation;

[0060] Specifically, the Kalman filter state equation is (3):

[0061] (3);

[0062] in: express The state quantity at the moment, express The state quantity at the moment, express The state transfer matrix at the moment, express The system noise at the moment, express Time measurement, express The moment measurement matrix, express The moment measurement noise matrix;

[0063] in: , express time Gyro attitude misalignment angle error in the axis direction, express time Gyro attitude misalignment angle error in the axis direction, express time Gyro attitude misalignment angle error in the axis direction, express time Gyro velocity error in the axis direction, express time Gyro velocity error in the axis direction, express time Gyro velocity error in the axis direction, express time The gyro position error in the axis direction, express time The gyro position error in the axis direction, express time The gyro position error in the axis direction, express time Gyro drift error in the axis direction, express time Gyro drift error in the axis direction, express time Gyro drift error in the axis direction, express time The accelerometer bias error in the axis direction, express time The accelerometer bias error in the axis direction, express time The accelerometer bias error in the axis direction, Represents matrix transpose.

[0064] ;

[0065] express The moment direction cosine matrix, express Accelerometer output relative force at time.

[0066]

[0067] express The gyro drift noise variance in the axis direction, express The gyro drift noise variance in the axis direction, express The gyro drift noise variance in the axis direction, express The accelerometer bias noise variance in the axis direction, express The accelerometer bias noise variance in the axis direction, express The accelerometer bias noise variance in the axis direction.

[0068] ;

[0069] ;

[0070] ; express The gyro speed in the axis direction, express The gyro speed in the axis direction, express The gyro speed in the axis direction, Represents a 3-row, 3-column identity matrix.

[0071] S62: predicting and measuring the Kalman filter state;

[0072] Specifically, the Kalman filter state can be predicted according to formula (4):

[0073] (4);

[0074] in: express The one-step prediction value of the state quantity at a moment, express The estimated value of the state quantity at the moment, express The moment error covariance matrix is ​​the one-step prediction value, express The estimated value of the moment error covariance matrix, express The state transfer matrix at the moment, express The system noise matrix at time instant, Transpose the matrix;

[0075] The initial values ​​of the Kalman filter parameters can be taken as , .

[0076] , express The gyro noise variance in the axis direction, express The gyro noise variance in the axis direction, express The gyro noise variance value in the axis direction;

[0077] Specifically, the measurement update can be performed according to formula (5):

[0078] (5);

[0079] in: express The moment filter gain matrix, express The moment measurement matrix, express The noise matrix is ​​measured at each moment, express The estimated value of the state quantity at the moment, express The estimated value of the moment error covariance matrix, represents a 4-row 4-column identity matrix, Transpose the matrix.

[0080] , express The accelerometer noise variance in the axis direction, express The accelerometer noise variance in the axis direction, express The accelerometer noise variance in the axis direction.

[0081] S63: Perform precise alignment posture update.

[0082] Further, in step S63, fine alignment is performed according to formula (6):

[0083] (6);

[0084] in: represents the exact direction cosine matrix, represents the rough initial direction cosine matrix, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction.

[0085] S7: After the fine alignment is completed, the rotation modulation control loop is closed, the rotation modulation mode is switched to the full-angle mode, and the subsequent navigation solution is performed with the precise attitude value of the hemispherical resonant gyro.

[0086] The present invention designs a resonant inertial navigation initial alignment method of a hemispherical resonant gyroscope drift self-modulation. In the initial alignment stage of the inertial navigation, the gyroscope is controlled to be in different working modes, so that the starting speed of the hemispherical resonant gyroscope is improved. At the same time, in a rotation modulation mode, the periodic precession of the standing wave angle of the hemispherical resonant gyroscope is actively controlled, so that the drift integral in the whole period is achieved to be zero, and the initial alignment accuracy of the hemispherical resonant gyroscope is improved.

[0087] An inertial navigation initial alignment system, the system diagram is as follows Figure 2 As shown, it is used to execute an inertial navigation initial alignment method as described in any one of the above items, including a hemispherical resonant gyroscope 1, an excitation electrode 2, a force feedback control loop 3, a full-angle control loop 4 and a rotation modulation control loop 5, the number of the hemispherical resonant gyroscopes is three, a pair of excitation electrodes is respectively installed on each hemispherical resonant gyroscope, and the force feedback control loop, the full-angle control loop and the rotation modulation control loop are respectively connected between a pair of excitation electrodes.

[0088] Figure 2 Only the connection of one hemispherical resonant gyroscope is shown in the figure, and the connection of the other hemispherical resonant gyroscopes is the same as in the figure.

[0089] In summary, the inertial navigation initial alignment method and system provided by the present invention improve the startup speed of the hemispherical resonant gyroscope, achieve zero drift integral within the entire cycle, and improve the initial alignment accuracy of the hemispherical resonant gyroscope.

[0090] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An inertial navigation initial alignment method, characterized in that: The steps include: S1: After the inertial navigation system is powered on, an excitation signal is applied to the excitation electrode according to the preset standing wave angle to start the hemispherical resonant gyroscope. S2: Start the force feedback control loop, control the hemispherical resonant gyroscope to enter the force feedback mode, and control the standing wave angle to stabilize at a preset position; S3: Close the force feedback control loop, open the full-angle control loop, control the hemispherical resonant gyroscope to enter the full-angle mode, and the standing wave angle precesses freely; S4: Coarsely align the inertial navigation system and obtain a rough initial direction cosine matrix; S5: After the rough alignment is completed, the rotation modulation control loop is turned on to make the hemispherical resonant gyro enter the rotation modulation mode and control the standing wave angle to Internal periodic uniform changes; S6: Using the rough initial direction cosine matrix as the initial value, perform fine alignment to obtain the precise direction cosine matrix and obtain the precise attitude value of the hemispherical resonant gyroscope; S7: After the fine alignment is completed, the rotation modulation control loop is closed, the rotation modulation mode is switched to the full-angle mode, and the subsequent navigation solution is performed with the precise attitude value of the hemispherical resonant gyro.

2. The inertial navigation initial alignment method according to claim 1, characterized in that: In step S1, an excitation signal is applied to the excitation electrode according to formula (1) to start the hemispherical resonant gyroscope: （1）； in: Indicates the excitation electrode The excitation signal applied, Indicates the excitation electrode The excitation signal applied, represents the excitation voltage, represents the standing wave angle, represents the initial phase angle, represents the resonant frequency, Indicates time.

3. The inertial navigation initial alignment method according to claim 2, characterized in that: In step S4, a rough alignment is performed according to equation (2) to obtain a rough initial direction cosine matrix: （2）； in: is the rough initial direction cosine matrix, is the acceleration vector measured by the accelerometer, is the angular velocity vector measured by the hemispherical resonant gyroscope, Transpose the matrix.

4. The inertial navigation initial alignment method according to claim 1, characterized in that: In step S6, the rough initial direction cosine matrix is ​​used as the initial value, and the Kalman filter method is used for fine alignment.

5. The inertial navigation initial alignment method according to claim 4, characterized in that: The precise alignment by the Kalman filter method in S6 includes the following steps: S61: Establish Kalman filter state equation; S62: predicting and measuring the Kalman filter state; S63: Perform precise alignment posture update.

6. The inertial navigation initial alignment method according to claim 5, characterized in that: The Kalman filter state equation established in step S61 is formula (3): （3）； in: express The state quantity at the moment, express The state quantity at the moment, express The state transfer matrix at time, express The system noise at the moment, express Time measurement, express The moment measurement matrix, express The noise matrix is ​​measured at each moment.

7. The inertial navigation initial alignment method according to claim 5, characterized in that: Step S62 predicts the Kalman filter state according to equation (4): （4）； in: express The one-step prediction value of the state quantity at a moment, express The estimated value of the state quantity at the moment, express The moment error covariance matrix is ​​the one-step prediction value, express The estimated value of the moment error covariance matrix, express The state transfer matrix at time, express The system noise matrix at time instant, Transpose the matrix.

8. The inertial navigation initial alignment method according to claim 5, characterized in that: Step S62 performs measurement update according to equation (5): （5）； in: express The moment filter gain matrix, express The moment measurement matrix, express The noise matrix is ​​measured at each moment, express The estimated value of the state quantity at the moment, express The estimated value of the moment error covariance matrix, represents the identity matrix, is the matrix transpose, express The moment error covariance matrix is ​​the one-step prediction value, express The one-step prediction value of the state quantity at a moment, express Moment measurement.

9. The inertial navigation initial alignment method according to claim 5, characterized in that: In step S63, fine alignment is performed according to formula (6): （6）； in: represents the exact direction cosine matrix, represents the rough initial direction cosine matrix, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction, express time The attitude misalignment angle error in the axis direction.

10. An inertial navigation initial alignment system, characterized in that: Used to execute an inertial navigation initial alignment method as described in any one of claims 1 to 9, characterized in that it includes a hemispherical resonant gyroscope, an excitation electrode, a force feedback control loop, a full-angle control loop and a rotation modulation control loop, the number of the hemispherical resonant gyroscopes is three, a pair of excitation electrodes is respectively installed on each hemispherical resonant gyroscope, and the force feedback control loop, the full-angle control loop and the rotation modulation control loop are respectively connected between a pair of excitation electrodes.

Images