Symmetrical three-position full attitude angle initial value determination method and system of inertial navigation equipment

Abstract

The embodiment of the invention discloses a symmetric three-position full attitude angle initial value determination method and system for inertial navigation equipment, and the method comprises the steps: 1, collecting inertial navigation data of the inertial navigation equipment at a first position, a second position and a third position respectively, and carrying out compensation to obtain corrected inertial navigation data of each position, correspondingly screening out qualified inertial navigation data at each position from the corrected inertial navigation data at each position; 2, respectively calculating pitch angles, roll angles and course angles of the first position, the second position and the third position; and step 3, performing data screening on the course angles of the three positions obtained by calculation, and outputting the attitude angle of the first position after screening. According to the method, a symmetric three-position full attitude angle initial value determination method is adopted, micro-amplitude interference possibly occurring in the use process is fully considered, error data isolation is performed at each position, and result data screening is performed before a final result is obtained, so that the detection precision and the anti-interference performance are greatly improved.

Description

Technical Field

[0001] The present invention relates to the field of single gyro orientation technology, and in particular to a method and system for determining initial values ​​of symmetrical three-position full attitude angles of an inertial navigation device. Background Art

[0002] The determination of attitude angle is an important requirement in the field of inertial navigation. For inertial navigation devices with three gyros and three accelerometers, the method for determining the initial value of the attitude angle generally adopts dual-vector attitude determination. For low-precision MEMS inertial navigation devices, the method of track angle binding is often used to determine the initial value of the attitude. In the application of the north-finder system, the "transfer mechanism + single gyroscope" realizes the problem of determining the initial value of the heading under static horizontal conditions. In the inclinometer, three accelerometers are used to realize the determination of the horizontal attitude angle under static conditions. The existing full attitude determination scheme under static conditions has poor anti-interference and low accuracy. Summary of the invention

[0003] The technical problem to be solved by the embodiments of the present invention is to provide a method and system for determining the initial value of the symmetrical three-position full attitude angle of an inertial navigation device to improve the accuracy and anti-interference performance.

[0004] In order to solve the above technical problems, an embodiment of the present invention proposes a method for determining the initial value of the symmetrical three-position full attitude angle of an inertial navigation device, wherein the inertial navigation device includes a transfer mechanism, an X-axis accelerometer, a Y-axis accelerometer and a gyroscope, and the method includes: Step 1: respectively collecting inertial navigation data of the inertial navigation device at the first position, the second position, and the third position, compensating the collected inertial navigation data in real time, obtaining corrected inertial navigation data at each position, and selecting qualified inertial navigation data at each position from the corrected inertial navigation data at each position; Step 2: Calculate the pitch angle, roll angle, and heading angle of the first position, the second position, and the third position respectively; Step 3: Filter the heading angles of the three calculated positions and output the attitude angle of the first position after filtering.

[0005] Accordingly, an embodiment of the present invention further provides a symmetrical three-position full attitude angle initial value determination system for an inertial navigation device, wherein the inertial navigation device includes a transfer mechanism, an X-axis accelerometer, a Y-axis accelerometer, and a gyroscope, and the system includes: Acquisition and screening module: collects the inertial navigation data of the inertial navigation device at the first position, the second position and the third position respectively, compensates the collected inertial navigation data in real time, obtains the corrected inertial navigation data of each position, and screens out the qualified inertial navigation data of each position from the corrected inertial navigation data of each position; Attitude angle calculation module: calculate the pitch angle, roll angle and heading angle of the first position, the second position and the third position respectively; Attitude angle determination module: performs data screening on the heading angles of the three calculated positions, and outputs the attitude angle of the first position after screening.

[0006] The beneficial effects of the present invention are as follows: the present invention adopts a symmetrical three-position full attitude angle initial value determination method, and fully considers the slight interference that may occur during use, isolates the erroneous data at each position, and screens the result data before the final result is output, which greatly improves the detection accuracy and anti-interference ability. BRIEF DESCRIPTION OF THE DRAWINGS

[0007] Figure 1 It is a flow chart of a method for determining initial values ​​of symmetrical three-position full attitude angles of an inertial navigation device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0008] It should be noted that, in the absence of conflict, the embodiments in the present application and the features in the embodiments may be combined with each other. The present invention is further described in detail below in conjunction with the drawings and specific embodiments.

[0009] In the embodiments of the present invention, if there are directional indications (such as up, down, left, right, front, back, etc.), they are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0010] In addition, in the present invention, the descriptions of "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features.

[0011] The inertial navigation device of the present invention includes a transfer mechanism, an X-axis accelerometer, a Y-axis accelerometer and a gyroscope. Figure 1 The method for determining the initial value of the symmetrical three-position full attitude angle of the inertial navigation device of an embodiment of the present invention includes steps 1 to 3.

[0012] Step 1: Collect the inertial navigation data of the inertial navigation device at the first position, the second position, and the third position respectively. The first position is the current position, the second position is the position after the indexing mechanism rotates 120° from the current position; the third position is the position after the indexing mechanism rotates 120° from the second position again. Compensate the collected inertial navigation data in real time to obtain the corrected inertial navigation data of each position, and select the qualified inertial navigation data of each position from the corrected inertial navigation data of each position.

[0013] In step 1, perform the following tasks: a) The indexing mechanism returns to zero.

[0014] b) Temperature compensation of gyroscopes and accelerometers using pre-set temperature compensation coefficients.

[0015] c) Use the pre-set gyro bias and gyro scale factor to correct the temperature compensated gyro data and convert it to rad / s.

[0016] d) Use the pre-set accelerometer zero bias and accelerometer calibration factor to correct the temperature compensated accelerometer data and convert it to g.

[0017] e) The corrected gyro data is filtered as follows: First, the gyroscope data is processed into a 1-second average value in rad; Secondly, the data with an absolute value greater than 7.3e-5 are directly eliminated; By sorting again, the obtained N (N is the number of sampling points) second gyroscope data are arranged in sequence, the largest 20% and the smallest 20% of the data are eliminated, and the remaining 60% of the data are used for screening normal data for backup (preferably, the average is calculated for backup).

[0018] f) The corrected accelerometer data is filtered as follows: First, the accelerometer data is processed into a 1-second average value in g; Secondly, the obtained N (N is the number of sampling points) seconds of accelerometer data are arranged in sequence by sorting, the largest 15% and the smallest 15% of the data are eliminated, and the remaining 70% of the data are used for screening normal data for backup (preferably, the average is calculated for backup).

[0019] Step 2: Calculate the pitch angle, roll angle, and heading angle of the first position, the second position, and the third position respectively.

[0020] In step 2, the angle value of the first position is calculated first. According to the basic formula of the inertial navigation system, the static horizontal attitude angle of the current position is obtained.

[0021] The pitch angle calculation formula is: ; The roll angle calculation formula is: ; In the formula, is the pitch angle of the first position, is the roll angle at the first position, is the corrected Y-axis accelerometer data of the first position, is the corrected X-axis accelerometer data at the first position, g is the local gravity acceleration, and the local gravity acceleration can be calculated by the following formula: ; In the formula, It represents the acceleration of gravity near the equator, and its value is generally 9.7803253361. L is the local latitude.

[0022] The attitude matrix is:

[0023] Where b represents the symbol of the carrier coordinate system, n represents the symbol of the navigation coordinate system, represents the pitch angle, represents the heading angle, Represents the roll angle.

[0024] The first position gyro output is ; Among them, m represents the symbol of the IMU body coordinate system. The first position body coordinate system coincides with the carrier coordinate system, so is the identity matrix, is the angular velocity component of the Earth's rotation.

[0025] Then, the output result of the single gyroscope in the first position can be expressed as: ; in, is the Earth's rotation angular rate, which is 7.2921e-05rad / s, and L is the local latitude value. is the pitch angle of the current first position, is the heading angle of the first position, is the corrected gyro data of the first position. The heading angle can be solved according to the formula.

[0026] Then calculate the angle value of the second position.

[0027] First, control the internal indexing mechanism and rotate it to the 120° position.

[0028] Secondly, the same method is used to calculate the pitch angle at the second position and roll angle .

[0029] Again, calculate the heading angle, In the second position,

[0030] Then the following holds: in, is the corrected gyro data for the second position, Represents the attitude matrix The first row and third column of Represents the attitude matrix The first row and second column of Represents the attitude matrix The second row and second column of Represents the attitude matrix The 2nd row and 3rd column of .

[0031] Pitch angle in the second position , Roll Angle , Gyro output , latitude L are known quantities, the heading angle of the second position can be calculated by the above formula .

[0032] Finally, calculate the angle value of the third position.

[0033] First, control the internal indexing mechanism and rotate it to the 240° position.

[0034] Secondly, calculate the pitch angle at the third position and roll angle

[0035] Again, calculate the heading angle, In the third position, ; Then the following holds: in, is the corrected gyro data for the third position, Represents the attitude matrix The first row and third column of Represents the attitude matrix The first row and second column of Represents the attitude matrix The second row and second column of Represents the attitude matrix The 2nd row and 3rd column of .

[0036] The pitch angle of the third position , Roll Angle , Gyro output , latitude L are known quantities, the heading angle of the third position can be calculated by the above formula .

[0037] Step 3: Filter the heading angles of the three calculated positions and output the attitude angle of the first position after filtering.

[0038] , , These three values ​​are the heading angle values ​​of the first position. Sort these three values ​​separately, and the sorting results are , , .

[0039] like and ,in, is the heading angle repeatability technical indicator of the current inertial navigation device, and the output heading angle is: .

[0040] like and ,in, is the heading angle repeatability technical indicator of the current inertial navigation device, and the output heading angle is: .

[0041] like and ,in, is the heading angle repeatability technical indicator of the current inertial navigation device, and the output heading angle is: .

[0042] like and ,in, is the heading angle repeatability technical indicator of the current inertial navigation device, and the output heading angle is: .

[0043] The three attitude angles of the first position finally determined are: pitch angle , Roll Angle , heading angle .

[0044] The symmetrical three-position full attitude angle initial value determination system of the inertial navigation device of the embodiment of the present invention comprises: Acquisition and screening module: collects the inertial navigation data of the inertial navigation device at the first position, the second position and the third position respectively, compensates the collected inertial navigation data in real time, obtains the corrected inertial navigation data of each position, and screens out the qualified inertial navigation data of each position from the corrected inertial navigation data of each position; Attitude angle calculation module: calculate the pitch angle, roll angle and heading angle of the first position, the second position and the third position respectively; Attitude angle determination module: performs data screening on the heading angles of the three calculated positions, and outputs the attitude angle of the first position after screening.

[0045] As an implementation mode, the inertial navigation data includes gyroscope data and accelerometer data, and the acquisition and screening module screens the corrected gyroscope data according to the following method: The corrected gyroscope data is processed into a 1-second average value to obtain several seconds of gyroscope data; the gyroscope data with an absolute value greater than 7.3e-5 is directly eliminated; the largest 20% and the smallest 20% of the remaining gyroscope data are eliminated, and the remaining 60% of the gyroscope data are used as the screened qualified gyroscope data; The corrected accelerometer data is filtered according to: The corrected accelerometer data are processed into a 1-second average value to obtain several seconds of accelerometer data; the largest 15% and the smallest 15% of the accelerometer data are eliminated, and the remaining 70% of the accelerometer data are used as the screened qualified accelerometer data.

[0046] As an implementation method, the attitude angle of the first position after being screened by the attitude angle determination module is: pitch angle , Roll Angle , heading angle ; Among them, the pitch angle , Roll Angle are the pitch angle, roll angle, and heading angle of the first position calculated respectively. Filtered by: The heading angles of the three positions calculated are sorted according to the angle values, and the sorting results are recorded as , , , like and , then we get the heading angle for: ; It is the technical index of the heading angle repeatability of the inertial navigation equipment; like and , then we get the heading angle for: ; like and , then we get the heading angle for: ; like and , then we get the heading angle for: .

[0047] As an implementation method, the attitude angle calculation module calculates the pitch angle and roll angle of the first position according to the following formula: ; ; In the formula, is the pitch angle of the first position, is the roll angle at the first position, is the corrected Y-axis accelerometer data of the first position, is the corrected X-axis accelerometer data at the first position, and g is the local gravity acceleration; The pitch angle and roll angle of the second position are calculated according to the following formula: ; ; In the formula, is the pitch angle of the second position, is the roll angle of the second position, is the corrected Y-axis accelerometer data of the second position, is the corrected X-direction accelerometer data of the second position; The pitch angle and roll angle of the third position are calculated according to the following formula: ; ; In the formula, is the pitch angle of the third position, is the rolling angle at the third position, is the corrected Y-axis accelerometer data at the third position, is the corrected X-direction accelerometer data at the third position.

[0048] As an implementation mode, the first position is the current position, the second position is the position after the indexing mechanism rotates 120° from the current position; the third position is the position after the indexing mechanism rotates 120° again from the second position; The attitude angle calculation module calculates the heading angle of the first position according to the following formula: : ; in, is the Earth's rotation angular rate, L is the local latitude, is the pitch angle of the first position, is the heading angle of the first position, is the corrected gyroscope data of the first position; The heading angle of the second position is calculated according to the following formula: : ; in, is the corrected gyroscope data of the second position; The heading angle of the second position is calculated according to the following formula: : ;in, is the corrected gyroscope data of the third position.

[0049] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for determining the initial value of the symmetrical three-position full attitude angle of an inertial navigation device, characterized in that: The inertial navigation device includes a transfer mechanism, an X-axis accelerometer, a Y-axis accelerometer and a gyroscope, and the method includes: Step 1: respectively collecting inertial navigation data of the inertial navigation device at the first position, the second position, and the third position, compensating the collected inertial navigation data in real time, obtaining corrected inertial navigation data at each position, and selecting qualified inertial navigation data at each position from the corrected inertial navigation data at each position; Step 2: Calculate the pitch angle, roll angle, and heading angle of the first position, the second position, and the third position respectively; Step 3: Filter the heading angles of the three calculated positions and output the attitude angle of the first position after filtering.

2. The method for determining the initial value of the symmetrical three-position full attitude angle of the inertial navigation device according to claim 1, characterized in that: The inertial navigation data includes gyroscope data and accelerometer data. In step 1, the corrected gyroscope data is screened according to the following method: The corrected gyroscope data is processed into a 1-second average value to obtain several seconds of gyroscope data; the gyroscope data with an absolute value greater than 7.3e-5 is directly eliminated; the largest 20% and the smallest 20% of the remaining gyroscope data are eliminated, and the remaining 60% of the gyroscope data are used as the screened qualified gyroscope data; The corrected accelerometer data is filtered according to: The corrected accelerometer data are processed into a 1-second average value to obtain several seconds of accelerometer data; the largest 15% and the smallest 15% of the accelerometer data are eliminated, and the remaining 70% of the accelerometer data are used as the screened qualified accelerometer data.

3. The method for determining the initial value of the symmetrical three-position full attitude angle of the inertial navigation device according to claim 1, characterized in that: In step 3, the attitude angle of the first position after screening is: pitch angle , Roll Angle , heading angle ; Among them, the pitch angle , Roll Angle are the pitch angle, roll angle, and heading angle of the first position calculated respectively. Filtered by: The heading angles of the three positions calculated are sorted according to the angle values, and the sorting results are recorded as , , , like and , then we get the heading angle for: ; It is the technical index of the heading angle repeatability of the inertial navigation equipment; like and , then we get the heading angle for: ; like and , then we get the heading angle for: ; like and , then we get the heading angle for: .

4. The method for determining the initial value of the symmetrical three-position full attitude angle of the inertial navigation device according to claim 1, characterized in that: In step 2, the pitch angle and roll angle of the first position are calculated according to the following formula: ； ； In the formula, is the pitch angle of the first position, is the roll angle at the first position, is the corrected Y-axis accelerometer data of the first position, is the corrected X-axis accelerometer data at the first position, and g is the local gravity acceleration; The pitch angle and roll angle of the second position are calculated according to the following formula: ； ； In the formula, is the pitch angle of the second position, is the roll angle of the second position, is the corrected Y-axis accelerometer data of the second position, is the corrected X-direction accelerometer data of the second position; The pitch angle and roll angle of the third position are calculated according to the following formula: ； ； In the formula, is the pitch angle of the third position, is the rolling angle at the third position, is the corrected Y-axis accelerometer data at the third position, is the corrected X-direction accelerometer data at the third position.

5. The method for determining the initial value of the symmetrical three-position full attitude angle of the inertial navigation device according to claim 4, characterized in that: The first position is the current position, the second position is the position after the indexing mechanism rotates 120° from the current position; the third position is the position after the indexing mechanism rotates 120° again from the second position; In step 2, the heading angle of the first position is calculated according to the following formula: : ； in, is the Earth's rotation angular rate, L is the local latitude, is the pitch angle of the first position, is the heading angle of the first position, is the corrected gyroscope data of the first position; The heading angle of the second position is calculated according to the following formula: : ； in, is the corrected gyroscope data of the second position; The heading angle of the second position is calculated according to the following formula: : ； in, is the corrected gyroscope data of the third position.

6. A symmetrical three-position full attitude angle initial value determination system for an inertial navigation device, characterized in that: The inertial navigation device includes a transfer mechanism, an X-axis accelerometer, a Y-axis accelerometer and a gyroscope, and the system includes: Acquisition and screening module: collects the inertial navigation data of the inertial navigation device at the first position, the second position and the third position respectively, compensates the collected inertial navigation data in real time, obtains the corrected inertial navigation data of each position, and screens out the qualified inertial navigation data of each position from the corrected inertial navigation data of each position; Attitude angle calculation module: calculate the pitch angle, roll angle and heading angle of the first position, the second position and the third position respectively; Attitude angle determination module: performs data screening on the heading angles of the three calculated positions, and outputs the attitude angle of the first position after screening.

7. The symmetrical three-position full attitude angle initial value determination system of the inertial navigation device according to claim 6, characterized in that: The inertial navigation data includes gyroscope data and accelerometer data. The acquisition and screening module screens the corrected gyroscope data according to the following method: The corrected gyroscope data is processed into a 1-second average value to obtain several seconds of gyroscope data; the gyroscope data with an absolute value greater than 7.3e-5 is directly eliminated; the largest 20% and the smallest 20% of the remaining gyroscope data are eliminated, and the remaining 60% of the gyroscope data are used as the screened qualified gyroscope data; The corrected accelerometer data is filtered according to: The corrected accelerometer data are processed into a 1-second average value to obtain several seconds of accelerometer data; the largest 15% and the smallest 15% of the accelerometer data are eliminated, and the remaining 70% of the accelerometer data are used as the screened qualified accelerometer data.

8. The symmetrical three-position full attitude angle initial value determination system of the inertial navigation device according to claim 6, characterized in that: The attitude angle of the first position after the attitude angle determination module is screened is: pitch angle , Roll Angle , heading angle ; Among them, the pitch angle , Roll Angle are the pitch angle, roll angle, and heading angle of the first position calculated respectively. Filtered by: The heading angles of the three positions calculated are sorted according to the angle values, and the sorting results are recorded as , , , like and , then we get the heading angle for: ; It is the technical index of the heading angle repeatability of the inertial navigation equipment; like and , then we get the heading angle for: ; like and , then we get the heading angle for: ; like and , then we get the heading angle for: .

9. The symmetrical three-position full attitude angle initial value determination system of the inertial navigation device according to claim 6, characterized in that: The attitude angle calculation module calculates the pitch angle and roll angle of the first position according to the following formula: ； ； In the formula, is the pitch angle of the first position, is the roll angle at the first position, is the corrected Y-axis accelerometer data of the first position, is the corrected X-axis accelerometer data at the first position, and g is the local gravity acceleration; The pitch angle and roll angle of the second position are calculated according to the following formula: ； ； In the formula, is the pitch angle of the second position, is the roll angle of the second position, is the corrected Y-axis accelerometer data of the second position, is the corrected X-direction accelerometer data of the second position; The pitch angle and roll angle of the third position are calculated according to the following formula: ； ； In the formula, is the pitch angle of the third position, is the rolling angle at the third position, is the corrected Y-axis accelerometer data at the third position, is the corrected X-direction accelerometer data at the third position.

10. The symmetrical three-position full attitude angle initial value determination system of the inertial navigation device according to claim 9, characterized in that: The first position is the current position, the second position is the position after the indexing mechanism rotates 120° from the current position; the third position is the position after the indexing mechanism rotates 120° again from the second position; The attitude angle calculation module calculates the heading angle of the first position according to the following formula: : ； in, is the Earth's rotation angular rate, L is the local latitude, is the pitch angle of the first position, is the heading angle of the first position, is the corrected gyroscope data of the first position; The heading angle of the second position is calculated according to the following formula: : ； in, is the corrected gyroscope data of the second position; The heading angle of the second position is calculated according to the following formula: : ； in, is the corrected gyroscope data of the third position.

Images