A self-alignment method for an interference-resistant compass system

Abstract

The application discloses a self-alignment method of a compass system with anti-interference, which comprises the following steps: performing Fourier transform on a line velocity increment to obtain a frequency spectrum of interference acceleration, and recording the lowest frequency in the frequency spectrum of the interference acceleration; designing a cut-off frequency of a low-pass digital filter according to the obtained lowest frequency, filtering the line velocity increment by using the low-pass digital filter to obtain a filtering result; and finally converting a posture matrix of a body in an inertial system into a posture of the body in a geographical coordinate system to realize self-alignment of the compass system.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation technology, specifically relating to an interference-resistant self-alignment method for a compass system. Background Technology

[0002] The fiber optic compass system mounts a gyroscope and an accelerometer on the carrier. As the carrier moves, the gyroscope directly measures the same angular velocity change as the carrier, while the accelerometer measures the change in the carrier's acceleration. The carrier's motion parameters are calculated by measuring the output values ​​of the accelerometer and the gyroscope, as well as the initial position and attitude.

[0003] However, the accumulation of initial position and attitude errors over time will affect the accuracy of the system navigation. Therefore, for the fiber optic compass system to achieve alignment, it must be provided with accurate external velocity. GPS and DVL velocimetry devices can be used as external velocity measurement units. GPS is usually chosen as the velocity measurement unit, but because radio wave signals cannot propagate in seawater, GPS cannot be used in the deep sea environment. While DVL velocimetry devices can be used in the deep sea, they are expensive and require calibration. Summary of the Invention

[0004] In view of this, the present invention provides an interference-resistant self-alignment method for a compass system, which can achieve self-alignment of the compass system in a deep-sea environment without the assistance of a velocity measuring device, thereby improving the anti-interference capability and system adaptability of the compass system.

[0005] This invention is achieved through the following technical solution:

[0006] A method for self-alignment of a compass system that is resistant to interference, comprising the following steps:

[0007] Step 1: After the compass system is powered on and enters the initial state, the gyroscope output angular displacement increment and the accelerometer output linear velocity increment are collected at each moment.

[0008] Step 2: The linear velocity increments collected in Step 1 include gravitational acceleration and disturbance acceleration. Perform a Fourier transform on the linear velocity increments to obtain the spectrum of the disturbance acceleration, and record the lowest frequency in the spectrum of the disturbance acceleration; wherein, the disturbance acceleration is the linear velocity increment.

[0009] Step 3: Design the cutoff frequency of the low-pass digital filter based on the lowest frequency obtained in Step 2, and use the linear velocity increment filtering of the low-pass digital filter to obtain the filtering result.

[0010] The formula for designing the cutoff frequency of a low-pass digital filter based on the lowest frequency obtained in step two is as follows:

[0011]

[0012] where n > 1, 0 < m < 1, m and n are both constants, and f rc is the cut-off frequency of the low-pass digital filter; f rl is the lowest frequency obtained in Step 2;

[0013] Step 4: Calculate the attitude matrix of the vehicle in the inertial system based on the angular displacement increment, linear velocity increment in Step 1, and the attitude matrix of the vehicle in the inertial system at the initial moment; calculate the transformation matrix of the local geodetic coordinate system relative to the inertial system using the filtering result obtained in Step 3. Finally, calculate the attitude of the vehicle in the local geodetic coordinate system based on the attitude matrix of the vehicle in the inertial system and the transformation matrix of the vehicle, to achieve the self-alignment of the compass system.

[0014] Furthermore, the method for calculating the attitude matrix of the vehicle in the inertial system according to the angular displacement increment, linear velocity increment in Step 1, and the attitude matrix of the vehicle in the inertial system at the initial moment in Step 4 is as follows:

[0015] The attitude matrix of the vehicle at time t1 The calculation formula is:

[0016] where is the attitude matrix of the vehicle in the inertial system at the initial moment t0,

[0017] The attitude matrix of the vehicle at time t2 The calculation formula is:

[0018] And so on, the attitude matrix of the vehicle at time t i The calculation formula is:

[0019] where is the transformation matrix of the attitude of the vehicle at time t i relative to the attitude of the vehicle at time t i-1 ;

[0020]

[0021] where I is the identity matrix,

[0022] is a transition quantity, which is known, and the angular displacement increment output by the gyroscope is The linear velocity increment output by the accelerometer is

[0023] denotes ​​

[0024] Furthermore, the method for calculating the carrier's transformation matrix using the filtering results obtained in step three, as described in step four, is as follows:

[0025]

[0026] in, For t i The transformation matrix of the local geographic coordinate system relative to the inertial frame at any given time. and All are transitional quantities;

[0027]

[0028] in, The result is the filtered result, where dt represents the sampling interval of the compass system, and dt = t. i+1 -t i ;

[0029]

[0030]

[0031] Furthermore, the method for calculating the attitude of the downloaded body in the local geographic coordinate system based on the attitude matrix and transformation matrix of the carrier in the inertial frame, as described in step four, is as follows:

[0032]

[0033] in, For t i The orientation of the object in the local geographic coordinate system at any given time. For t i The transformation matrix of the local geographic coordinate system relative to the inertial frame at any given time. For t i The attitude array of the inertial frame at all times.

[0034] Furthermore, n = 5 and m = 0.2.

[0035] Beneficial effects:

[0036] (1) This invention performs a Fourier transform on the linear velocity increment output by the accelerometer to obtain the spectrum of the linear velocity increment, and records the lowest frequency in the spectrum of the linear velocity increment; then, based on the lowest frequency obtained in step two, it designs the cutoff frequency of a low-pass digital filter to filter out the interference of interfering acceleration on the compass. In the deep-sea environment, since the carrier cannot travel at high speed, it is easy to hover or sit on the bottom. Under these environmental conditions, the interference angular velocity generated by the ground speed of the carrier is relatively small, and the entrainment acceleration and carrier acceleration generated by the ground speed are also relatively small. Therefore, this invention can achieve self-alignment of the compass system without the assistance of a speed measuring device; and after analyzing the linear velocity increment output by the accelerometer, this invention designs the cutoff frequency of a low-pass digital filter to filter out the interference of interfering acceleration on the compass, thereby improving the anti-interference capability (i.e., alignment accuracy) and system adaptability of the compass system.

[0037] (2) The present invention uses the filtering result after filtering out the interference acceleration to calculate the transformation matrix of the carrier, thereby improving the alignment accuracy.

[0038] (3) In this invention, n=5 and m=0.2, which can ensure that the delay of the filtering result is small and that interference acceleration is filtered out as much as possible, thereby improving the alignment accuracy. Attached Figure Description

[0039] Figure 1 This is a flowchart of a self-alignment method for a compass system that is resistant to interference, according to the present invention. Detailed Implementation

[0040] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0041] This embodiment provides an interference-resistant self-alignment method for a compass system; see appendix. Figure 1 This includes the following steps:

[0042] Step 1: After the compass system is powered on and enters the initial state, it collects data at each time point t. i The corresponding gyroscope output angular displacement increment and the linear velocity increment output by the accelerometer i = 1, 2, 3, ..., len, where i represents the time sequence number and len represents the sequence number of the final time of data acquisition.

[0043] Step two, the linear velocity increment collected in step one. This includes gravitational acceleration and disturbance acceleration. Gravitational acceleration is constant, while the acceleration due to changes in linear velocity is constant. Perform a Fourier transform to obtain the spectrum of the disturbance acceleration, and record the lowest frequency in the spectrum of the disturbance acceleration as f. rl .

[0044] Step 3, based on the lowest frequency f obtained in Step 2rl Design the cut-off frequency f of the low-pass digital filter rc , and use the low-pass digital filter to filter the linear velocity increment , filter out the interference of the interference acceleration on the compass system, and obtain the filtering result

[0045]

[0046] Wherein, n>1, 0<m<1, m and n are both constants. In a specific embodiment, according to experience, to ensure that the delay of the filtering result is small, n = 5 and m = 0.2;

[0047] Step Four, according to the angular displacement increment in Step One linear velocity increment and the attitude matrix of the carrier in the inertial system at the initial moment calculate the attitude matrix of the carrier in the inertial system at time t i Use the filtering result obtained in Step Three to calculate the transformation matrix of the local geographic coordinate system relative to the inertial system at time t Finally, according to the attitude matrix of the carrier in the inertial system i and the transformation matrix of the carrier

[0048] calculate the attitude of the carrier in the local geographic coordinate system to achieve the self-alignment of the compass system.

[0049] Furthermore, according to the angular displacement increment in Step One linear velocity increment and the attitude matrix of the carrier at the initial moment calculate the attitude matrix of the carrier in the inertial system at time t i The method is as follows: <0000,180>

[0050] Then the attitude matrix of the carrier at time t i can be calculated as:

[0051] The attitude matrix of the carrier at time t1 The calculation formula is:

[0052] Wherein, is the attitude matrix of the carrier in the inertial system at the initial moment t0,

[0053] The attitude matrix of the carrier at time t2 The calculation formula is:

[0054]

[0054] And so on, t i The carrier attitude array of time The calculation formula is:

[0055] in, In an inertial frame of reference t i time relative to t i-1 The transformation matrix of the carrier's attitude at any given time;

[0056]

[0057] Where I is the identity matrix,

[0058] As a transitional quantity,

[0059] express The antisymmetric matrix;

[0060] Furthermore, the filtering results obtained in step three are used... Calculate t i Transformation matrix of local geographic coordinate system relative to inertial frame at any given time The formula is:

[0061]

[0062] in, and All are transitional quantities;

[0063]

[0064] Where dt represents the sampling interval of the compass system, dt = t i+1 -t i ;

[0065]

[0066]

[0067] Furthermore, The calculation formula is:

[0068]

[0069] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A self-alignment method for a compass system with interference resistance, characterized in that, Includes the following steps: Step 1: After the compass system is powered on and enters the initial state, the gyroscope output angular displacement increment and the accelerometer output linear velocity increment are collected at each moment. Step 2: The linear velocity increments collected in Step 1 include gravitational acceleration and disturbance acceleration. Perform a Fourier transform on the linear velocity increments to obtain the spectrum of the disturbance acceleration, and record the lowest frequency in the spectrum of the disturbance acceleration; wherein, the disturbance acceleration is the linear velocity increment. Step 3: Design the cutoff frequency of the low-pass digital filter based on the lowest frequency obtained in Step 2, and use the linear velocity increment filtering of the low-pass digital filter to obtain the filtering result. The formula for designing the cutoff frequency of a low-pass digital filter based on the lowest frequency obtained in step two is as follows: where n > 1, 0 < m < 1, m and n are both constants, and f rc is the cut-off frequency of the low-pass digital filter; f rl is the lowest frequency obtained in step 2; Step four: Calculate the attitude matrix of the inertial frame based on the angular displacement increment, linear velocity increment, and attitude matrix of the inertial frame at the initial moment obtained in step one; calculate the transformation matrix of the local geographic coordinate system relative to the inertial frame using the filtering results obtained in step three; finally, calculate the attitude of the inertial frame based on the attitude matrix of the carrier in the inertial frame and the transformation matrix of the carrier, thus achieving self-alignment of the compass system.

2. The interference-resistant self-alignment method for a compass system as described in claim 1, characterized in that, The method for calculating the attitude matrix of the inertial frame of reference in step four, based on the angular displacement increment, linear velocity increment, and the attitude matrix of the inertial frame of reference at the initial moment, is as follows: Carrier attitude array at time t1 The calculation formula is: in, Let t0 be the attitude matrix of the inertial frame at the initial time. Carrier attitude array at time t2 The calculation formula is: And so on, t i The carrier attitude array of time The calculation formula is: in, In an inertial frame of reference t i time relative to t i-1 The transformation matrix of the carrier's attitude at any given time; Where I is the identity matrix, Given that the transient response is a given value, the gyroscope output angular displacement increment is... The linear velocity increment output by the accelerometer is × indicates An antisymmetric matrix.

3. The interference-resistant self-alignment method for a compass system as described in claim 1, characterized in that, The method for calculating the carrier's transformation matrix using the filtering results obtained in step three, as described in step four, is as follows: in, For t i The transformation matrix of the local geographic coordinate system relative to the inertial frame at any given time. and All are transitional quantities; in, The result is the filtered result, where dt represents the sampling interval of the compass system, and dt = t. i+1 -t i ; 4. The interference-resistant self-alignment method for a compass system as described in claim 1, characterized in that, The method described in step four for calculating the attitude of the downloaded body in the local geographic coordinate system based on the attitude matrix and transformation matrix of the carrier in the inertial frame is as follows: in, For t i The orientation of the object in the local geographic coordinate system at any given time. For t i The transformation matrix of the local geographic coordinate system relative to the inertial frame at any given time. For t i The attitude array of the inertial frame at all times.

5. The interference-resistant self-alignment method for a compass system as described in any one of claims 1-4, characterized in that, The n=5 and the m=0.2.

Images