Hemispherical resonant inertial navigation backtracking filtering alignment method and system based on force feedback mode

Abstract

The present invention relates to the field of navigation technology, and in particular to a hemispherical resonant inertial navigation backtracking filter alignment method and system based on a force feedback mode. The method comprises the following steps: initializing the hemispherical resonant inertial navigation, the system enters the force feedback mode, the vibration mode angle of the resonant gyroscope changes uniformly and unidirectionally, and the output of the hemispherical resonant inertial navigation is transmitted to a memory and a self-alignment module; the self-alignment module performs self-alignment, the memory performs frequency reduction storage, and transmits the result to the backtracking filter alignment module; the backtracking filter alignment module performs multiple backtracking filter alignments, and transmits the final result to the feedback correction module; after the feedback correction module performs feedback correction, the system enters the navigation mode. The method and system provided by the present invention drive the vibration mode angle of the hemispherical resonant gyroscope to change uniformly and traverse a complete cycle during the alignment process, and identify and compensate for the gyro symmetry drift in a short time through backtracking filtering, thereby improving the system alignment convergence speed and accuracy.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a hemispherical resonance inertial navigation backtracking filtering alignment method and system based on a force feedback mode. Background Art

[0002] The core component of the hemispherical resonant inertial navigation is the hemispherical resonant gyroscope, which is a Coriolis vibratory gyroscope. It uses the radial vibration standing wave precession effect of the hemispherical lip shell to sense the rotation of the base. It is a new type of high-precision pure solid-state vibratory gyroscope with the advantages of few components, long service life, short start-up time, and stable physical properties. It has received more and more attention and application in the field of inertial navigation, and has become one of the research hotspots in the field of inertial technology at home and abroad.

[0003] The error of the hemispherical resonant gyro includes symmetry drift, which changes periodically with the gyro vibration angle and will significantly affect the system alignment accuracy. The hemispherical resonant gyro symmetry drift calibration in the prior art usually uses the gyro output as the measurement and is obtained based on parameter identification methods such as the least squares method. Although this calibration method is simple, it takes a long time. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a hemispherical resonant inertial navigation backtracking filtering alignment method and system based on a force feedback mode, during the alignment process, the hemispherical resonant gyroscope vibration angle is driven to change at a uniform speed and traverse a complete cycle, and the gyroscope symmetry drift is identified and compensated in a short time through backtracking filtering, thereby improving the system alignment convergence speed and accuracy.

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

[0006] A hemispherical resonant inertial navigation backtracking filtering alignment method based on a force feedback mode comprises the following steps:

[0007] S1: Initialize the hemispherical resonant inertial navigation system and assign initial values ​​to variables. The system enters the force feedback mode. The vibration angle of the resonant gyroscope changes uniformly and unidirectionally. The output of the hemispherical resonant inertial navigation system is transmitted to the memory and the self-alignment module.

[0008] S2: The self-alignment module performs self-alignment and transmits the self-alignment result to the back-tracking filter alignment module. Meanwhile, the memory performs down-conversion storage and transmits the down-conversion storage data to the back-tracking filter alignment module.

[0009] S3: The back-tracking filter alignment module performs multiple back-tracking filter alignments and transmits the final result of the back-tracking filter alignment to the feedback correction module;

[0010] S4: After the feedback correction module performs feedback correction on the final alignment result based on the backtracking filter, the system enters the navigation mode.

[0011] Optimized, the rate of change of the vibration angle of the resonant gyro in step S1 is 3 .

[0012] Optimized, the variables in step S1 include the attitude, speed, position and filtering parameters of the hemispherical resonant inertial navigation system, and the output of the hemispherical resonant inertial navigation system includes the original angular velocity of the hemispherical resonant inertial navigation system, the vibration angle control signal, the actual sensed external angular velocity, angular increment and specific force increment.

[0013] Optimally, the self-alignment in step S2 includes a coarse alignment stage and a fine alignment stage. The coarse alignment stage uses a solidification filter method for coarse alignment, and the fine alignment stage uses a Kalman filter method for fine alignment.

[0014] Further, in step S2, the memory uses the following method to perform down-scaling storage:

[0015] S21: The system variables are updated according to formula (1):

[0016] (1);

[0017] in: represents the initial carrier coordinate system, represents the carrier coordinate system, Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates Solve the carrier coordinate system to the Solve the quaternion of the carrier coordinate system. It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is Indicates The solution is calculated by The resulting transformation matrix is Indicates the relative relationship between the carrier coordinate system and the initial carrier coordinate system, Indicates The specific force increment of the solution;

[0018] S22: After each running set time, the initial carrier coordinate system is moved to the The quaternion of the carrier coordinate system is solved for the first time, and the specific force increment is the first component of the initial carrier coordinate system. The integral of the solution and the current resonant gyro vibration angle are stored in the memory, and then the specific force increment is calculated in the initial carrier coordinate system. The integral of the solution is set to zero.

[0019] Optimally, the setting time described in step S22 is 1 second.

[0020] Furthermore, in step S3, when the backtracking filter is aligned, the inertial navigation solution mode is divided into forward navigation and reverse navigation according to time, and each state update process performs forward and reverse solutions respectively.

[0021] Further, the method of backtracking filter alignment in step S3 is as follows:

[0022] S31: The inertial navigation solution mode is divided into forward navigation and reverse navigation according to time. In forward navigation, the state update equation of the inertial navigation is formula (2), and in reverse navigation, the state update equation of the inertial navigation is formula (3):

[0023] (2);

[0024] (3);

[0025] in: Represents the transformation matrix from the carrier coordinate system to the geographic coordinate system, represents the derivative of the transformation matrix from the carrier coordinate system to the geographic coordinate system, Represents the component of the angular velocity output by the gyroscope in the carrier coordinate system, represents an antisymmetric matrix, It represents the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the velocity of the carrier relative to the Earth-centered Earth-fixed coordinate system, represents the component of the specific force in the carrier coordinate system, represents the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. represents the acceleration due to gravity, represents the derivative of the carrier position, represents the transformation matrix of position update relative to velocity, It means to negate the component of the angular velocity output by the gyroscope in the carrier coordinate system. It means to negate the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the derivative of the carrier's velocity relative to the Earth-centered Earth-fixed coordinate system, It means to negate the component of the contrast force in the carrier coordinate system. It means to negate the component of the earth's rotation angular velocity in the geographic coordinate system. It means to negate the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. It means to negate the acceleration due to gravity. Indicates the inversion of the transformation matrix of position update relative to velocity;

[0026] S32: Perform system navigation calculation according to discrete equation (4) and record the state error variable of the inertial navigation system:

[0027] (4);

[0028] in: Represents the transformation matrix from the navigation coordinate system to the carrier coordinate system, Indicates the initial navigation coordinate system to the The quaternion of the navigation coordinate system rotation vector in seconds, Represents the attitude quaternion at the initial moment of self-alignment, Indicates the initial carrier coordinate system to the The quaternion of the carrier coordinate system rotation vector in seconds, Indicates The inertial navigation velocity calculated by the second solution is Indicates The inertial navigation velocity calculated by the second solution is represents the identity matrix, Represents the time interval between two adjacent solutions. Indicates the specific force increment at The integral of the components in the carrier coordinate system at time , Indicates The carrier position of the second solution, Indicates The carrier position of the sub-solution;

[0029] S33: Substitute the state error variables of the inertial navigation system into the state error equation (5) of the forward inertial navigation system and the state error equation (6) of the reverse inertial navigation system, take the velocity position as the external observation, and obtain the Kalman filter equation (7) of the forward inertial navigation system and the Kalman filter equation (8) of the reverse inertial navigation system:

[0030]

[0031]

[0032]

[0033] in: represents the system error angle, represents the derivative of the system error angle, represents the system speed error, represents the derivative of the system velocity error, represents the constant gyro drift, represents the gyro symmetry drift sinusoidal coefficient, represents the vibration mode angle stored in the memory, represents the gyro symmetry drift cosine coefficient, represents the system gyro noise, represents the derivative of the inertial velocity, Indicates comparison, represents the constant accelerometer bias, represents the system plus table noise, represents the derivative of the carrier position, represents the transformation matrix of position error relative to position, Indicates the carrier position, represents the carrier position error, represents the derivative of the carrier position error, represents the derivative of the constant accelerometer bias, represents the derivative of the constant gyro drift, The derivative of the gyro symmetry drift sinusoidal coefficient, The derivative of the cosine coefficient representing the gyro symmetry drift, represents the matrix transpose, represents the derivative of the state error variable of the inertial navigation system, represents the error state transfer matrix, represents the state error variable of the inertial navigation system, represents the noise state transfer matrix, represents the system noise, Represents the measured value of speed and position, represents the measurement equation, represents the measurement noise, Indicates the direction coefficient, which is 1 for the forward direction and -1 for the reverse direction. It represents the error of the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. Represents the error of the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the error of negating the component of the Earth's rotational angular velocity in the geographic coordinate system. It represents the component error of the rotation angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system, It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system;

[0034] S34: After the system completes one forward and reverse back-tracing filtering, it records the state variables of the back-tracing filtering alignment module to form a covariance matrix of the state variables of the back-tracing filtering alignment module, and compares all the elements on the diagonal of the covariance matrix of the state variables of the back-tracing filtering alignment module with the set minimum value one by one. If it is less than the set minimum value, it is reset; if it is greater than or equal to the set minimum value, the back-tracing filtering alignment is completed.

[0035] Further, in step S4, feedback correction is performed according to formula (9):

[0036] (9);

[0037] in: Represents the corrected carrier attitude quaternion, The quaternion representing the carrier attitude error converted from the system error angle, represents the corrected carrier speed, represents the carrier speed before correction, Indicates the corrected carrier position.

[0038] A hemispherical resonant inertial navigation back-filter alignment system based on a force feedback mode, used to execute any one of the above-mentioned hemispherical resonant inertial navigation back-filter alignment methods based on a force feedback mode, comprising a memory, a self-alignment module, a back-filter alignment module and a feedback correction module;

[0039] The memory is used to receive the output data of the hemispherical resonant inertial navigation system and transmit it to the retrospective filtering alignment module after down-converting and storing it;

[0040] The self-alignment module is used to receive the output data of the hemispherical resonant inertial navigation system and perform self-alignment, and then transmit the self-alignment result to the back-tracking filtering alignment module;

[0041] The back-tracking filter alignment module receives the self-alignment module and the memory data, and after multiple back-tracking filter alignments, transmits the final result of the back-tracking filter alignment to the feedback correction module;

[0042] The feedback correction module performs feedback correction on the final result based on retrospective filtering.

[0043] Beneficial effects of the invention:

[0044] The hemispherical resonant inertial navigation backtracking filtering alignment method and system based on the force feedback mode provided by the present invention introduces the gyro symmetry drift coefficient term into the system state error equation, and uses the force feedback mode in the alignment process to make the symmetry drift change periodically with the vibration mode angle, so that the gyro symmetry drift coefficient can be identified in the subsequent filtering alignment stage, thereby improving the system alignment accuracy;

[0045] In addition, the output data of the hemispherical resonant inertial navigation system is stored at a reduced frequency, and multiple backtracking filtering alignments are performed on the basis of conventional self-alignment, which effectively extends the system alignment time. This allows the system state error to converge to a steady-state value in a short time, thereby improving the alignment accuracy and speed. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0047] Figure 2 It is a schematic diagram of the system of the present invention. DETAILED DESCRIPTION

[0048] A hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode includes the following steps, and its flow chart is as follows: Figure 1 As shown:

[0049] S1: Initialize the hemispherical resonant inertial navigation system and assign initial values ​​to variables. The system enters the force feedback mode. The vibration angle of the resonant gyroscope changes uniformly and unidirectionally. The output of the hemispherical resonant inertial navigation system is transmitted to the memory and the self-alignment module.

[0050] Specifically, the change rate of the resonant gyro vibration angle can be preferably 3 .

[0051] The variables include the attitude, velocity, position and filtering parameters of the hemispherical resonant inertial navigation system, and the output of the hemispherical resonant inertial navigation system includes the original angular velocity of the hemispherical resonant inertial navigation system, the vibration angle control signal, the actual external angular velocity sensed, the angular increment and the specific force increment.

[0052] The hemispherical resonant gyroscope is a Coriolis gyroscope that uses the standing wave precession effect to sense rotation. Based on Lynch's non-ideal oscillator error model, the hemispherical resonant gyroscope motion equation is:

[0053]

[0054] Where: Indicates the detection axis The displacement at Indicates the detection axis The first derivative of the displacement at , Indicates the detection axis The second derivative of the displacement at , Indicates the detection axis The displacement at Indicates the detection axis The first derivative of the displacement at , Indicates the detection axis The second derivative of the displacement at , represents the order of the vibration mode, represents the Blaine coefficient, represents the external input angular velocity, represents the average circumferential damping of the resonator, It indicates that the circumferential damping of the resonator is uneven. represents the angle between the damping axis and the electrode axis, represents the average frequency of the gyroscope, Indicates the frequency breakup value of the gyroscope, represents the angle between the frequency axis and the electrode axis, Represents a harmonic oscillator The control force applied axially, represents the modal mass of the resonator, Represents a harmonic oscillator Control force applied axially.

[0055] In order to facilitate measurement, the hemispherical resonant gyroscope uses The second-order vibration mode of the resonator is in a four-antinode oscillation state. The motion equation of the hemispherical resonant gyroscope is a set of second-order linear differential equations. The two equations of the equation system describe the Axis direction and The axis directions are spaced in space vibration.

[0056] By using the average method to analyze and ignoring the second-order small quantities in the equation system, we can get the following formula:

[0057]

[0058] Where: represents the amplitude control of the resonant gyro control loop output, represents the amplitude of the vibration antinode point on the resonator, represents the derivative of the amplitude of the vibration antinode point on the resonator, represents the vibration mode angle, represents the derivative of the vibration mode angle, represents the orthogonal control of the resonant gyro control loop output, represents the derivative of the amplitude of the node of the vibration wave on the resonator, Indicates the reference frequency set by the frequency control loop, represents the frequency control of the resonant gyro control loop output, Represents the vibration angle control of the resonant gyro control loop output.

[0059] From the above formula, we can see that the gyro drift changes periodically with the vibration angle in a sinusoidal function. The vibration angle changes in one direction every time. , the drift also changes in a whole cycle, which is called symmetric drift. The system alignment accuracy is mainly affected by the symmetric drift. Therefore, for the resonant inertial navigation system, its gyro drift has the following form:

[0060] ;

[0061] In the formula, For gyro drift, is the gyro constant drift, represents the gyro symmetry drift sinusoidal coefficient, Represents the gyro symmetry drift cosine coefficient.

[0062] S2: The self-alignment module performs self-alignment and transmits the self-alignment result to the back-tracking filter alignment module. Meanwhile, the memory performs down-conversion storage and transmits the down-conversion storage data to the back-tracking filter alignment module.

[0063] Specifically, the self-alignment includes a coarse alignment stage and a fine alignment stage. The coarse alignment stage uses a solidification filter method for coarse alignment, and the fine alignment stage uses a Kalman filter method for fine alignment.

[0064] Assume that when the self-alignment stage begins, the carrier coordinate system is recorded as System, the initial carrier coordinate system at this moment and System coincidence, initial quaternion , initial specific force increment , the two variables are updated as the system is solved:

[0065] The memory can be down-converted using the following methods:

[0066] S21: The system variables are updated according to formula (1):

[0067] (1);

[0068] in: represents the initial carrier coordinate system, represents the carrier coordinate system, Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates Solve the carrier coordinate system to the Solve the quaternion of the carrier coordinate system. It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is Indicates The solution is calculated by The resulting transformation matrix is Indicates the relative relationship between the carrier coordinate system and the initial carrier coordinate system, Indicates The specific force increment of the solution;

[0069] S22: After each running set time, the initial carrier coordinate system is moved to the The quaternion of the carrier coordinate system is solved for the first time, and the specific force increment is the first component of the initial carrier coordinate system. The integral of the solution and the current resonant gyro vibration angle are stored in the memory, and then the specific force increment is calculated in the initial carrier coordinate system. The integral of the solution is set to zero.

[0070] The setting time may preferably be 1 second.

[0071] Here, the down-conversion storage of the original output data of the resonant inertial navigation system exists independently of the self-alignment process, which can provide data for the subsequent back-tracking filter alignment and improve the solution speed.

[0072] In order to estimate the gyro symmetry drift sinusoidal coefficient and gyro symmetry drift cosine coefficient , the vibration angle needs to be stored in the memory, and the vibration angle stored in the memory each time can be as follows:

[0073] ;

[0074] represents the vibration mode angle stored in the memory, Indicates The vibration angle of the solution is Indicates The vibration angle of the sub-solution;

[0075] S3: The back-tracking filter alignment module performs multiple back-tracking filter alignments and transmits the final result of the back-tracking filter alignment to the feedback correction module;

[0076] Specifically, during the back-tracking filter alignment, the inertial navigation solution mode can be divided into forward navigation and reverse navigation according to time, and each state update process performs forward and reverse solutions respectively.

[0077] Further, the method of backtracking filter alignment in step S3 is as follows:

[0078] S31: The inertial navigation solution mode is divided into forward navigation and reverse navigation according to time. In forward navigation, the state update equation of the inertial navigation is formula (2), and in reverse navigation, the state update equation of the inertial navigation is formula (3):

[0079] (2);

[0080] (3);

[0081] in: Represents the transformation matrix from the carrier coordinate system to the geographic coordinate system, represents the derivative of the transformation matrix from the carrier coordinate system to the geographic coordinate system, Represents the component of the angular velocity output by the gyroscope in the carrier coordinate system, represents an antisymmetric matrix, It represents the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the velocity of the carrier relative to the Earth-centered Earth-fixed coordinate system, represents the component of the specific force in the carrier coordinate system, represents the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. represents the acceleration due to gravity, represents the derivative of the carrier position, represents the transformation matrix of position update relative to velocity, It means to negate the component of the angular velocity output by the gyroscope in the carrier coordinate system. It means to negate the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the derivative of the carrier's velocity relative to the Earth-centered Earth-fixed coordinate system, It means to negate the component of the contrast force in the carrier coordinate system. It means to negate the component of the earth's rotation angular velocity in the geographic coordinate system. It means to negate the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. It means to negate the acceleration due to gravity. Indicates the inversion of the transformation matrix of position update relative to velocity;

[0082] Comparing forward and reverse navigation, we can see that for attitude and velocity updates, the navigation solution only needs to , , , Reverse navigation can be achieved by negating the equation. Negate.

[0083] Compared with the general reverse navigation, which requires the speed to be negative, the above reverse navigation equation maintains the speed The continuity of sign and value also ensures the continuity of the state error equation of the subsequent inverse filtering system. continuity.

[0084] S32: Considering the non-commutative error during frequency reduction, the system navigation solution is performed according to the discrete formula (4), and the state error variable of the inertial navigation system is recorded:

[0085] (4);

[0086] in: Represents the transformation matrix from the navigation coordinate system to the carrier coordinate system, Indicates the initial navigation coordinate system to the The quaternion of the navigation coordinate system rotation vector in seconds, Represents the attitude quaternion at the initial moment of self-alignment, Indicates the initial carrier coordinate system to the The quaternion of the carrier coordinate system rotation vector in seconds, Indicates The inertial navigation velocity calculated by the second solution is Indicates The inertial navigation velocity calculated by the second solution is represents the identity matrix, Represents the time interval between two adjacent solutions. Indicates the specific force increment at The integral of the components in the carrier coordinate system at time , Indicates The carrier position of the second solution, Indicates The carrier position of the sub-solution;

[0087] S33: Substitute the state error variables of the inertial navigation system into the state error equation (5) of the forward inertial navigation system and the state error equation (6) of the reverse inertial navigation system, take the velocity position as the external observation, and obtain the Kalman filter equation (7) of the forward inertial navigation system and the Kalman filter equation (8) of the reverse inertial navigation system:

[0088]

[0089]

[0090]

[0091] in: represents the system error angle, represents the derivative of the system error angle, represents the system speed error, represents the derivative of the system velocity error, represents the constant gyro drift, represents the gyro symmetry drift sinusoidal coefficient, represents the vibration mode angle stored in the memory, represents the gyro symmetry drift cosine coefficient, represents the system gyro noise, represents the derivative of the inertial velocity, Indicates comparison, represents the constant accelerometer bias, represents the system plus table noise, represents the derivative of the carrier position, represents the transformation matrix of position error relative to position, Indicates the carrier position, represents the carrier position error, represents the derivative of the carrier position error, represents the derivative of the constant accelerometer bias, represents the derivative of the constant gyro drift, The derivative of the gyro symmetry drift sinusoidal coefficient, represents the derivative of the gyro symmetry drift cosine coefficient, represents the matrix transpose, represents the derivative of the state error variable of the inertial navigation system, represents the error state transfer matrix, represents the state error variable of the inertial navigation system, represents the noise state transfer matrix, represents the system noise, Represents the measured value of speed and position, represents the measurement equation, represents the measurement noise, Indicates the direction coefficient, which is 1 for the forward direction and -1 for the reverse direction. It represents the error of the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. Represents the error of the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the error of negating the component of the Earth's rotational angular velocity in the geographic coordinate system. It represents the component error of the rotation angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system, It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system;

[0092] Here the inertial navigation system state error variable .

[0093] S34: After the system completes one forward and reverse back-tracing filtering, it records the state variables of the back-tracing filtering alignment module to form a covariance matrix of the state variables of the back-tracing filtering alignment module, and compares all the elements on the diagonal of the covariance matrix of the state variables of the back-tracing filtering alignment module with the set minimum value one by one. If it is less than the set minimum value, it is reset; if it is greater than or equal to the set minimum value, the back-tracing filtering alignment is completed.

[0094] The retrospective filter alignment uses the down-converted stored data to perform multiple alignments, which effectively prolongs the alignment time, so that the system state error can converge to a steady-state value in a short time, thereby improving the alignment accuracy and speed.

[0095] S4: After the feedback correction module performs feedback correction on the final alignment result based on the backtracking filter, the system enters the navigation mode.

[0096] Specifically, feedback correction can be performed according to formula (9):

[0097] (9);

[0098] in: Represents the corrected carrier attitude quaternion, The quaternion representing the carrier attitude error converted from the system error angle, , represents the corrected carrier speed, represents the carrier speed before correction, Indicates the corrected carrier position.

[0099] The hemispherical resonant inertial navigation backtracking filtering alignment method based on the force feedback mode provided by the present invention introduces a gyro symmetry drift coefficient term into the system state error equation, and adopts the force feedback mode in the alignment process to make the symmetry drift show a periodic change with the vibration mode angle, so that the gyro symmetry drift coefficient can be identified in the subsequent filtering alignment stage, thereby improving the system alignment accuracy.

[0100] A hemispherical resonant inertial navigation backtracking filter alignment system based on force feedback mode, the system diagram is as follows Figure 2As shown, a method for performing a hemispherical resonant inertial navigation backtracking filtering alignment method based on a force feedback mode as described above includes a memory, a self-alignment module, a backtracking filtering alignment module and a feedback correction module;

[0101] The memory is used to receive the output data of the hemispherical resonant inertial navigation system and transmit it to the retrospective filtering alignment module after down-converting and storing it;

[0102] The self-alignment module is used to receive the output data of the hemispherical resonant inertial navigation system and perform self-alignment, and then transmit the self-alignment result to the back-tracking filtering alignment module;

[0103] The back-tracking filter alignment module receives the self-alignment module and the memory data, and after multiple back-tracking filter alignments, transmits the final result of the back-tracking filter alignment to the feedback correction module;

[0104] The feedback correction module performs feedback correction on the final result based on retrospective filtering.

[0105] In summary, the hemispherical resonant inertial navigation backtracking filtering alignment method and system based on the force feedback mode provided by the present invention drives the hemispherical resonant gyroscope vibration mode angle to change at a uniform speed and traverse a complete cycle during the alignment process, and identifies and compensates for the gyroscope symmetry drift in a short time through backtracking filtering, thereby improving the system alignment convergence speed and accuracy.

[0106] 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. A hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode, characterized in that: The steps include: S1: Initialize the hemispherical resonant inertial navigation system and assign initial values ​​to variables. The system enters the force feedback mode. The vibration angle of the resonant gyroscope changes uniformly and unidirectionally. The output of the hemispherical resonant inertial navigation system is transmitted to the memory and the self-alignment module. S2: The self-alignment module performs self-alignment and transmits the self-alignment result to the back-tracking filter alignment module. Meanwhile, the memory performs down-conversion storage and transmits the down-conversion storage data to the back-tracking filter alignment module. S3: The back-tracking filter alignment module performs multiple back-tracking filter alignments and transmits the final result of the back-tracking filter alignment to the feedback correction module; The method of retrospective filtering alignment is as follows: S31: The inertial navigation solution mode is divided into forward navigation and reverse navigation according to time. In forward navigation, the state update equation of the inertial navigation is formula (2), and in reverse navigation, the state update equation of the inertial navigation is formula (3): （2）； （3）； in: Represents the transformation matrix from the carrier coordinate system to the geographic coordinate system, represents the derivative of the transformation matrix from the carrier coordinate system to the geographic coordinate system, Represents the component of the angular velocity output by the gyroscope in the carrier coordinate system, represents an antisymmetric matrix, It represents the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the velocity of the carrier relative to the Earth-centered Earth-fixed coordinate system, represents the component of the specific force in the carrier coordinate system, represents the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. represents the acceleration due to gravity, represents the derivative of the carrier position, represents the transformation matrix of position update relative to velocity, It means to negate the component of the angular velocity output by the gyroscope in the carrier coordinate system. It means to negate the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. represents the derivative of the carrier's velocity relative to the Earth-centered Earth-fixed coordinate system, It means to negate the component of the contrast force in the carrier coordinate system. It means to negate the component of the earth's rotation angular velocity in the geographic coordinate system. It means to negate the component of the rotation angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system. It means to negate the acceleration due to gravity. Indicates the inversion of the transformation matrix of position update relative to velocity; S32: Perform system navigation calculation according to discrete equation (4) and record the state error variable of the inertial navigation system: （4）； in: Represents the transformation matrix from the navigation coordinate system to the carrier coordinate system, Indicates the initial navigation coordinate system to the The quaternion of the navigation coordinate system rotation vector in seconds, Represents the attitude quaternion at the initial moment of self-alignment, Indicates the initial carrier coordinate system to the The quaternion of the carrier coordinate system rotation vector in seconds, Indicates The inertial navigation velocity calculated by the second solution is Indicates The inertial navigation velocity calculated by the second solution is represents the identity matrix, Represents the time interval between two adjacent solutions. Indicates the specific force increment at The integral of the components in the carrier coordinate system at seconds, Indicates The carrier position of the second solution, Indicates The carrier position of the sub-solution; S33: Substitute the state error variables of the inertial navigation system into the state error equation (5) of the forward inertial navigation system and the state error equation (6) of the reverse inertial navigation system, take the velocity position as the external observation, and obtain the Kalman filter equation (7) of the forward inertial navigation system and the Kalman filter equation (8) of the reverse inertial navigation system: in: represents the system error angle, represents the derivative of the system error angle, represents the system speed error, represents the derivative of the system velocity error, represents the constant gyro drift, represents the gyro symmetry drift sinusoidal coefficient, represents the vibration mode angle stored in the memory, represents the gyro symmetry drift cosine coefficient, represents the system gyro noise, represents the derivative of the inertial velocity, Indicates comparison, represents the constant accelerometer bias, represents the system plus table noise, represents the derivative of the carrier position, represents the transformation matrix of position error relative to position, Indicates the carrier position, represents the carrier position error, represents the derivative of the carrier position error, represents the derivative of the constant accelerometer bias, represents the derivative of the constant gyro drift, The derivative of the gyro symmetry drift sinusoidal coefficient, represents the derivative of the gyro symmetry drift cosine coefficient, represents the matrix transpose, represents the derivative of the state error variable of the inertial navigation system, represents the error state transfer matrix, represents the state error variable of the inertial navigation system, represents the noise state transfer matrix, represents the system noise, Represents the measured value of speed and position, represents the measurement equation, represents the measurement noise, Indicates the direction coefficient, which is 1 for the forward direction and -1 for the reverse direction. It represents the error of the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the inertial coordinate system in the geographic coordinate system. Represents the error of the component of the Earth's rotation angular velocity in the geographic coordinate system, It represents the error of negating the component of the Earth's rotational angular velocity in the geographic coordinate system. It represents the component error of the rotation angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system, It represents the error of negating the component of the angular velocity of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the geographic coordinate system; S34: After the system completes one forward and reverse back-tracing filtering, the back-tracing filtering alignment module state variables are recorded to form a covariance matrix of the back-tracing filtering alignment module state variables, and all elements on the diagonal of the covariance matrix of the back-tracing filtering alignment module state variables are compared with the set minimum value one by one. If the elements are less than the set minimum value, they are reset; if the elements are greater than or equal to the set minimum value, the back-tracing filtering alignment is completed; S4: After the feedback correction module performs feedback correction on the final alignment result based on the backtracking filter, the system enters the navigation mode.

2. The method for hemispherical resonant inertial navigation backtracking filtering alignment based on force feedback mode according to claim 1, characterized in that: The rate of change of the vibration angle of the resonant gyro in step S1 is 3 .

3. The hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode according to claim 1 is characterized in that: The variables in step S1 include the attitude, speed, position and filtering parameters of the hemispherical resonant inertial navigation system, and the output of the hemispherical resonant inertial navigation system includes the original angular velocity of the hemispherical resonant inertial navigation system, the vibration angle control signal, the actual external angular velocity sensed, the angular increment and the specific force increment.

4. The method for hemispherical resonant inertial navigation backtracking filtering alignment based on force feedback mode according to claim 1, characterized in that: The self-alignment in step S2 includes a coarse alignment stage and a fine alignment stage. The coarse alignment stage adopts a solidification filter method for coarse alignment, and the fine alignment stage adopts a Kalman filter method for fine alignment.

5. The hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode according to claim 1 is characterized by: In step S2, the memory uses the following method to perform down-frequency storage: S21: The system variables are updated according to formula (1): （1）； in: represents the initial carrier coordinate system, represents the carrier coordinate system, Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates the initial carrier coordinate system to the Solve the quaternion of the carrier coordinate system. Indicates Solve the carrier coordinate system to the Solve the quaternion of the carrier coordinate system. It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is It represents the component of the specific force increment in the initial carrier coordinate system. The integral of the solution is Indicates The solution is calculated by The resulting transformation matrix is Indicates the relative relationship between the carrier coordinate system and the initial carrier coordinate system, Indicates The specific force increment of the solution; S22: After each running set time, the initial carrier coordinate system is moved to the The quaternion of the carrier coordinate system is solved for the first time, and the specific force increment is the first component of the initial carrier coordinate system. The integral of the solution and the current resonant gyro vibration angle are stored in the memory, and then the specific force increment is calculated in the initial carrier coordinate system. The integral of the solution is set to zero.

6. The method for hemispherical resonant inertial navigation backtracking filtering alignment based on force feedback mode according to claim 5, characterized in that: The setting time described in step S22 is 1 second.

7. The hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode according to claim 1 is characterized by: In step S3, when the backtracking filter is aligned, the inertial navigation solution mode is divided into forward navigation and reverse navigation according to time, and each state update process performs forward and reverse solutions respectively.

8. The hemispherical resonant inertial navigation backtracking filtering alignment method based on force feedback mode according to claim 1 is characterized by: In step S4, feedback correction is performed according to formula (9): （9）； in: Represents the corrected carrier attitude quaternion, The quaternion representing the carrier attitude error converted from the system error angle, represents the corrected carrier speed, represents the carrier speed before correction, Indicates the corrected carrier position.

9. A hemispherical resonant inertial navigation backtracking filtering alignment system based on force feedback mode, characterized in that: Used to execute a hemispherical resonant inertial navigation back-tracking filtering alignment method based on a force feedback mode as claimed in any one of claims 1 to 8, comprising a memory, a self-alignment module, a back-tracking filtering alignment module and a feedback correction module; The memory is used to receive the output data of the hemispherical resonant inertial navigation system and transmit it to the retrospective filtering alignment module after down-converting and storing it; The self-alignment module is used to receive the output data of the hemispherical resonant inertial navigation system and perform self-alignment, and then transmit the self-alignment result to the back-tracking filtering alignment module; The back-tracking filter alignment module receives the self-alignment module and the memory data, and after multiple back-tracking filter alignments, transmits the final result of the back-tracking filter alignment to the feedback correction module; The feedback correction module performs feedback correction on the final result based on retrospective filtering.

Images