Method and System for Determining Initial Values of Symmetric Four-Position Full Attitude Angles of Inertial Navigation Equipment

Abstract

An embodiment of the present invention discloses a method and system for determining the initial values of the full attitude angles of an inertial navigation device in four symmetric positions. The method includes: Step 1: Control the indexing mechanism to stop at the first position, the second position, the third position, and the fourth position for a preset time respectively. At the same time, collect the inertial navigation data at the four positions in real time and perform compensation and correction to obtain the corrected inertial navigation data at the four positions. Then, perform screening processing on the corrected inertial navigation data at the four positions respectively to obtain the screened inertial navigation data at the four positions. Step 2: Calculate the pitch angle, roll angle, and heading angle. The present invention fully considers the possible micro-interferences during use, isolates the error data at each position respectively, and performs result data screening before the final result is obtained, greatly improving the detection accuracy and anti-interference ability.

Description

Technical Field

[0001] The present invention relates to the technical field of single gyro orientation, and particularly to a method and system for determining the initial values of full attitude angles of an inertial navigation device at symmetric four positions. Background Art

[0002] The determination of attitude angles 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 values of attitude angles generally uses double-vector attitude determination. For low-precision MEMS inertial navigation devices, the method of track angle binding is often used to determine the initial attitude values. In the application of a north-seeking instrument system, "a rotating mechanism + a single gyro" realizes the problem of determining the initial heading value under static horizontal conditions. In an inclinometer, three accelerometers are used to determine the horizontal attitude angles under static conditions. The existing full-attitude determination schemes under static conditions have poor anti-interference ability 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 values of full attitude angles of an inertial navigation device at symmetric four positions, so as to improve the accuracy and anti-interference ability.

[0004] To solve the above technical problem, an embodiment of the present invention provides a method for determining the initial values of full attitude angles of an inertial navigation device at symmetric four positions. The inertial navigation device includes a rotating mechanism, an X-axis accelerometer, a Y-axis accelerometer, and a gyroscope. The method includes:

[0005] Step 1: Control the rotating mechanism to stop at a preset time at the first position, the second position, the third position, and the fourth position respectively. At the same time, collect the inertial navigation data at the four positions in real time and perform compensation and correction to obtain the corrected inertial navigation data at the four positions. Then, perform screening processing on the corrected inertial navigation data at the four positions respectively to obtain the screened inertial navigation data at the four positions.

[0006] Step 2: Calculate the pitch angle, roll angle, and heading angle.

[0007] Correspondingly, an embodiment of the present invention further provides a system for determining the initial values of full attitude angles of an inertial navigation device at symmetric four positions. The inertial navigation device includes a rotating mechanism, an X-axis accelerometer, a Y-axis accelerometer, and a gyroscope. The system includes:

[0008] Compensation and correction module: Control the rotating mechanism to stop at a preset time at the first position, the second position, the third position, and the fourth position respectively. At the same time, collect the inertial navigation data at the four positions in real time and perform compensation and correction to obtain the corrected inertial navigation data at the four positions. Then, perform screening processing on the corrected inertial navigation data at the four positions respectively to obtain the screened inertial navigation data at the four positions.

[0009] Screening and calculation module: Calculate the pitch angle, roll angle, and heading angle.

[0010] The beneficial effects of the present invention are as follows: The present invention adopts a symmetric four-position full attitude angle initial value determination method, fully considering the possible micro-interferences that may occur during use, isolating the error data at each position respectively, and screening the result data before finally obtaining the result, which greatly improves the detection accuracy and anti-interference ability. Brief Description of the Drawings

[0011] Figure 1 It is a schematic flowchart of the symmetric four-position full attitude angle initial value determination method of the inertial navigation device in the embodiment of the present invention. Detailed Embodiments

[0012] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments can be combined with each other. The present invention will be further described in detail below with reference to the drawings and specific embodiments.

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

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

[0015] The inertial navigation device includes a indexing mechanism, an X-axis accelerometer, a Y-axis accelerometer, and a gyroscope.

[0016] Please refer to Figure 1 , the symmetric four-position full attitude angle initial value determination method of the inertial navigation device in the embodiment of the present invention includes Step 1 and Step 2.

[0017] Step 1: Control the indexing mechanism to stop for a certain number of seconds (i.e., the preset time) at the first position (i.e., the current position), the second position (i.e., the position rotated 90° from the current position), the third position (i.e., the position rotated 180° from the current position), and the fourth position (i.e., the position rotated 270° from the current position) respectively, and simultaneously collect the inertial navigation data at the 4 positions and perform compensation and correction to obtain the corrected inertial navigation data at the 4 positions; then perform screening processing on the corrected inertial navigation data at the 4 positions respectively to obtain the screened inertial navigation data at the 4 positions.

[0018] The indexing mechanism stops at the first position for M seconds, and collects N 1One-axis gyro data and two-axis accelerometer data. The indexing mechanism stops at the second position for M seconds to collect N 2 One-axis gyro data and two-axis accelerometer data. The indexing mechanism stops at the third position for M seconds to collect N 3 One-axis gyro data and two-axis accelerometer data. The indexing mechanism stops at the fourth position for M seconds to collect N 4 One-axis gyro data and two-axis accelerometer data.

[0019] In step 1, the following operations are performed.

[0020] a) Zero the indexing mechanism.

[0021] b) Perform temperature compensation on the gyroscope and accelerometer using the pre-bound temperature compensation coefficient.

[0022] c) Use the pre-bound gyroscope zero bias and gyro scale factor to correct the temperature-compensated gyroscope data and convert the unit to: rad / s.

[0023] d) Use the pre-bound accelerometer zero bias and accelerometer calibration factor to correct the temperature-compensated accelerometer data and convert the unit to: g.

[0024] e) Screen the corrected gyroscope data in the following way:

[0025] First, process the gyroscope data into a 1-second average value with the unit of rad;

[0026] Second, directly eliminate the data with an absolute value greater than 7.3e-5;

[0027] Third, through the sorting method, arrange the obtained N (N is the number of sampling points) seconds of gyroscope data in sequence, and eliminate the largest 20% and the smallest 20% of the data. The remaining 60% of the data is the normally screened data for backup.

[0028] f) Screen the corrected accelerometer data in the following way (the X-axis and Y-axis accelerometer data are both screened in the following way):

[0029] First, process the accelerometer data into a 1-second average value with the unit of g;

[0030] Second, through the sorting method, arrange the obtained N (N is the number of sampling points) seconds of gyroscope data in sequence, and eliminate the largest 15% and the smallest 15% of the data. The remaining 70% of the data is the normally screened data for backup.

[0031] Step 2: Calculate the pitch angle, roll angle, and heading angle based on the data obtained at the 4 positions.

[0032] First, calculate the average value of the collected gyroscope data and name them respectively as: , , , .

[0033] Calculate the average value of the collected accelerometer data and name them respectively as: , , , , , , , .

[0034] Secondly, according to the basic formula of the inertial navigation system, calculate the static horizontal attitude angle of the current position.

[0035] The formula for calculating the pitch angle is:

[0036] ;

[0037] ;

[0038] The formula for calculating the roll angle is:

[0039] ;

[0040] ;

[0041] Among them, is the pitch angle, is the roll angle, is the average output of the Y-axis accelerometer, is the average output of the X-axis accelerometer, g is the local gravitational acceleration, represents the average value of the X-axis accelerometer data at the first position after screening and processing, represents the average value of the Y-axis accelerometer data at the first position after screening and processing, represents the average value of the X-axis accelerometer data at the second position after screening and processing, represents the average value of the Y-axis accelerometer data at the second position after screening and processing, represents the average value of the X-axis accelerometer data at the third position after screening and processing, represents the average value of the axial accelerometer data at the third position after screening and processing, represents the average value of the X-axis accelerometer data at the fourth position after screening and processing, represents the average value of the Y-axis accelerometer data at the fourth position after screening and processing.

[0042] The local gravitational acceleration can be calculated by the following formula:

[0043] ;

[0044] Wherein, represents the acceleration due to gravity near the equator, generally taken as 9.7803253361, and L is the local latitude.

[0045] The heading angle is calculated again through the gyro data, pitch angle, and roll angle. The specific formula is as follows.

[0046] The attitude matrix is

[0047] where b represents the symbol of the vehicle coordinate system, n represents the symbol of the navigation coordinate system, represents the pitch angle, represents the heading angle, represents the roll angle.

[0048] The output of the i-th (i = 1, 2, 3, 4) position gyro is

[0049] ;

[0050] where mi represents the symbol of the body coordinate system of the i-th (i = 1, 2, 3, 4) position IMU, is the component of the earth's angular velocity of rotation.

[0051] Then the first position transformation matrix is the identity matrix because the first position indexing mechanism is zero and the body coordinate system coincides with the vehicle coordinate system.

[0052] Then the second position transformation matrix is

[0053]

[0054] Then the third position transformation matrix is

[0055]

[0056] Then the fourth position transformation matrix is

[0057]

[0058] The expression formulas for the forward gyro output values at each position are derived as follows.

[0059] The expression formula for the first position gyro output:

[0060] ;

[0061] Second position gyro output representation formula:

[0062] ;

[0063] Third position gyro output representation formula:

[0064] ;

[0065] Fourth position gyro output representation formula:

[0066] .

[0067] 、 、 、 are the output values of the gyroscopes at the first position, the second position, the third position, and the fourth position respectively, represents the first row and second column of the attitude matrix ; represents the first row and third column of the attitude matrix ; represents the random error term of the output value of the gyroscope at the first position, represents the random error term of the output value of the gyroscope at the second position, represents the random error term of the output value of the gyroscope at the third position, represents the random error term of the output value of the gyroscope at the fourth position.

[0068] If it is considered that the random errors at each position of the gyro output are equal in a short time, then subtracting the gyro output at the third position from the gyro output at the first position gives the following formula:

[0069] ;

[0070] Then subtracting the gyro output at the fourth position from the gyro output at the second position gives the following formula:

[0071] ;

[0072] Then, the course angle The calculation result is:

[0073] .

[0074] is the angular rate of the Earth's rotation.

[0075] The symmetric four-position full attitude angle initial value determination system of the inertial navigation device according to the embodiment of the present invention, the inertial navigation device includes:

[0076] Compensation and correction module: Control the indexing mechanism to stop at the first position, the second position, the third position, and the fourth position for a preset time. At the same time, collect the inertial navigation data at the four positions in real time and perform compensation and correction to obtain the corrected inertial navigation data at the four positions. Then, perform screening processing on the corrected inertial navigation data at the four positions respectively to obtain the screened inertial navigation data at the four positions.

[0077] Screening and calculation module: Calculate the pitch angle, roll angle, and heading angle.

[0078] As an implementation method, the compensation and correction module screens and processes the gyroscope data in the corrected inertial navigation data according to the following method:

[0079] Process the corrected gyroscope data into 1-second average values to obtain gyroscope data for several seconds. Directly eliminate the gyroscope data with an absolute value greater than 7.3e-5. Eliminate the largest 20% and the smallest 20% of the remaining gyroscope data, and use the remaining 60% of the gyroscope data as the qualified gyroscope data after screening.

[0080] Screen and process the accelerometer data in the X direction and Y direction in the corrected inertial navigation data according to the following method:

[0081] Process the corrected X-direction accelerometer data into 1-second average values to obtain X-direction accelerometer data for several seconds. Eliminate the largest 15% and the smallest 15% of the X-direction accelerometer data, and use the remaining 70% of the X-direction accelerometer data as the qualified X-direction accelerometer data after screening. Process the corrected Y-direction accelerometer data into 1-second average values to obtain Y-direction accelerometer data for several seconds. Eliminate the largest 15% and the smallest 15% of the Y-direction accelerometer data, and use the remaining 70% of the Y-direction accelerometer data as the qualified Y-direction accelerometer data after screening.

[0082] As an implementation method, the calculation module first calculates the average values of the gyroscope data and accelerometer data in the screened inertial navigation data at each position respectively, and uses the obtained average values of the gyroscope data and accelerometer data at each position as the gyroscope output value and accelerometer output value at that position respectively, and then calculates the pitch angle, roll angle, and heading angle.

[0083] As an implementation method, the calculation module calculates the pitch angle and roll angle according to the following formula:

[0084] ;

[0085] ;

[0086] ;

[0087] ;

[0088] Among them, is the pitch angle, is the roll angle, is the average output of the Y-axis accelerometer, is the average output of the X-axis accelerometer, and g is the local acceleration due to gravity, represents the mean value of the X-axis accelerometer data at the first position after screening and processing, represents the mean value of the Y-axis accelerometer data at the first position after screening and processing, represents the mean value of the X-axis accelerometer data at the second position after screening and processing, represents the mean value of the Y-axis accelerometer data at the second position after screening and processing, represents the mean value of the X-axis accelerometer data at the third position after screening and processing, represents the mean value of the axial accelerometer data at the third position after screening and processing, represents the mean value of the X-axis accelerometer data at the fourth position after screening and processing, represents the mean value of the Y-axis accelerometer data at the fourth position after screening and processing.

[0089] As an implementation, the calculation module calculates the heading angle according to the following formula :

[0090] ;

[0091] ;

[0092] ;

[0093] Among them, , , , are the output values of the gyroscopes at the first position, the second position, the third position, and the fourth position respectively, is the angular rate of the Earth's rotation, and L is the local latitude value.

[0094] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalent scope.

Claims

1. A method for determining the initial value of the symmetrical four-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: Control the indexing mechanism to stop for a preset time at the first position, the second position, the third position, and the fourth position, respectively, and collect the inertial navigation data of the four positions in real time and perform compensation correction to obtain the inertial navigation data after correction at the four positions; then filter and process the inertial navigation data after correction at the four positions respectively to obtain the inertial navigation data after filter processing at the four positions; wherein, filter and process the gyroscope data in the corrected inertial navigation 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 accelerometer data in the X and Y directions of the corrected inertial navigation data are filtered and processed according to the following method: Processing the corrected X-direction accelerometer data into a 1-second average value to obtain several seconds of X-direction accelerometer data; eliminating the largest 15% and the smallest 15% of the X-direction accelerometer data, and using the remaining 70% of the X-direction accelerometer data as the screened qualified X-direction accelerometer data; processing the corrected Y-direction accelerometer data into a 1-second average value to obtain several seconds of Y-direction accelerometer data; eliminating the largest 15% and the smallest 15% of the Y-direction accelerometer data, and using the remaining 70% of the Y-direction accelerometer data as the screened qualified Y-direction accelerometer data; Step 2: Calculate the pitch angle, roll angle, and heading angle; In step 2, the gyroscope data and accelerometer data in the inertial navigation data after screening and processing at each position are averaged respectively, and the average gyroscope data and accelerometer data of each position are used as the gyroscope output value and accelerometer output value of the position respectively, and then the pitch angle, roll angle and heading angle are calculated; In step 2, the pitch angle and roll angle are calculated according to the following formula: Among them, θ is the pitch angle, γ is the roll angle, is the average output of the Y-axis accelerometer, is the average output of the X-axis accelerometer, g is the local gravity acceleration, Ax1 represents the average value of the X-axis accelerometer data at the first position after screening, Ay1 represents the average value of the Y-axis accelerometer data at the first position after screening, Ax2 represents the average value of the X-axis accelerometer data at the second position after screening, Ay2 represents the average value of the Y-axis accelerometer data at the second position after screening, Ax3 represents the average value of the X-axis accelerometer data at the third position after screening, Ay3 represents the average value of the axial accelerometer data at the third position after screening, Ax4 represents the average value of the X-axis accelerometer data at the fourth position after screening, and Ay4 represents the average value of the Y-axis accelerometer data at the fourth position after screening; In step 2, the heading angle φ is calculated using the gyro data, pitch angle, and roll angle. The specific formula is as follows: The output of the gyro at the i-th position (i=1, 2, 3, 4) is Where mi represents the coordinate system symbol of the IMU at the i-th (i=1, 2, 3, 4) position. is the angular velocity component of the Earth's rotation; Then the first position transformation matrix is the unit matrix, because the first position indexing mechanism is zero, and the stage coordinate system coincides with the carrier coordinate system; Then the second position transformation matrix for Then the third position transformation matrix for Then the fourth position transformation matrix for The forward gyro output value expression formula for each position is derived as follows: The first position gyro output expression formula: oh m1 =ω ie cosLcosθcosφ+ω ie sinLsinθ+ε1; The second position gyro output expression formula: The third position gyro output expression formula: oh m3 =-ω ie cosLcosθcosφ-ω ie sinLsinθ+ε3; The fourth position gyro output expression formula: ω m1 ,ω m2 ,ω m3 ,ω m4 are the gyroscope output value of the first position, the gyroscope output value of the second position, the gyroscope output value of the third position, and the gyroscope output value of the fourth position, respectively. 12 Represents the attitude matrix The first row and second column of T 13 Represents the attitude matrix In the first row and third column of , ε1 represents the random error term of the output value of the gyroscope at the first position, ε2 represents the random error term of the output value of the gyroscope at the second position, ε3 represents the random error term of the output value of the gyroscope at the third position, and ε4 represents the random error term of the output value of the gyroscope at the fourth position; In the short term, it is assumed that the random errors of each position of the gyro output are equal, that is, ε1=ε2=ε3=ε4; Then the first position gyro output minus the third position gyro output is used to obtain the following formula: cosφ=(ω m1 -oh m3 -2h ie sin Lsinθ) / 2ω ie cos Lcosθ; Then the second position gyro output minus the fourth position gyro output is used to obtain the following formula: In order to reduce the calculation error, the calculation result of the heading angle φ is: Among them, ω ie is the Earth's rotation angular rate, and L is the local latitude.

2. A symmetrical four-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: Compensation correction module: control the indexing mechanism to stop for a preset time at the first position, the second position, the third position, and the fourth position, and collect the inertial navigation data of the four positions in real time and perform compensation correction to obtain the inertial navigation data after correction at the four positions; then filter and process the inertial navigation data after correction at the four positions respectively to obtain the inertial navigation data after filter processing at the four positions; wherein, the compensation correction module filters and processes the gyroscope data in the corrected inertial navigation 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 accelerometer data in the X and Y directions of the corrected inertial navigation data are filtered and processed according to the following method: Processing the corrected X-direction accelerometer data into a 1-second average value to obtain several seconds of X-direction accelerometer data; eliminating the largest 15% and the smallest 15% of the X-direction accelerometer data, and using the remaining 70% of the X-direction accelerometer data as the screened qualified X-direction accelerometer data; processing the corrected Y-direction accelerometer data into a 1-second average value to obtain several seconds of Y-direction accelerometer data; eliminating the largest 15% and the smallest 15% of the Y-direction accelerometer data, and using the remaining 70% of the Y-direction accelerometer data as the screened qualified Y-direction accelerometer data; Calculation module: calculate the pitch angle, roll angle and heading angle; The calculation module first calculates the average of the gyro data and accelerometer data in the inertial navigation data after screening and processing at each position, and uses the average of the gyro data and the accelerometer data at each position as the gyro output value and the accelerometer output value at the position, and then calculates the pitch angle, roll angle, and heading angle; The calculation module calculates the pitch angle and roll angle according to the following formula: Among them, θ is the pitch angle, γ is the roll angle, is the average output of the Y-axis accelerometer, is the average output of the X-axis accelerometer, g is the local gravity acceleration, Ax1 represents the average value of the X-axis accelerometer data at the first position after screening, Ay1 represents the average value of the Y-axis accelerometer data at the first position after screening, Ax2 represents the average value of the X-axis accelerometer data at the second position after screening, Ay2 represents the average value of the Y-axis accelerometer data at the second position after screening, Ax3 represents the average value of the X-axis accelerometer data at the third position after screening, Ay3 represents the average value of the axial accelerometer data at the third position after screening, Ax4 represents the average value of the X-axis accelerometer data at the fourth position after screening, and Ay4 represents the average value of the Y-axis accelerometer data at the fourth position after screening; The calculation module calculates the heading angle φ through gyro data, pitch angle, and roll angle: The specific formula is as follows: The output of the gyro at the i-th position (i=1, 2, 3, 4) is Where mi represents the coordinate system symbol of the IMU at the i-th (i=1, 2, 3, 4) position. is the angular velocity component of the Earth's rotation; Then the first position transformation matrix is the unit matrix, because the first position indexing mechanism is zero, and the stage coordinate system coincides with the carrier coordinate system; Then the second position transformation matrix for Then the third position transformation matrix for Then the fourth position transformation matrix for The forward gyro output value expression formula for each position is derived as follows: The first position gyro output expression formula: oh m1 =ω ie cosLcosθcosφ+ω ie sinLsinθ+ε1; The second position gyro output expression formula: The third position gyro output expression formula: oh m3 =-ω ie cosLcosθcosφ-ω ie sinLsinθ+ε3; The fourth position gyro output expression formula: ω m1 ,ω m2 ,ω m3 ,ω m4 are the gyroscope output value of the first position, the gyroscope output value of the second position, the gyroscope output value of the third position, and the gyroscope output value of the fourth position, respectively. 12 Represents the attitude matrix The first row and second column of T 13 Represents the attitude matrix In the first row and third column of , ε1 represents the random error term of the output value of the gyroscope at the first position, ε2 represents the random error term of the output value of the gyroscope at the second position, ε3 represents the random error term of the output value of the gyroscope at the third position, and ε4 represents the random error term of the output value of the gyroscope at the fourth position; In the short term, it is assumed that the random errors of each position of the gyro output are equal, that is, ε1=ε2=ε3=ε4; Then the first position gyro output minus the third position gyro output is used to obtain the following formula: cosφ=(ω m1 -oh m3 -2h ie sin Lsinθ) / 2ω ie cos Lcosθ; Then the second position gyro output minus the fourth position gyro output is used to obtain the following formula: In order to reduce the calculation error, the calculation result of the heading angle φ is: Among them, ω ie is the Earth's rotation angular rate, and L is the local latitude.

Images