Method and system for determining initial value of attitude angle of full-period continuous rotation north seeker

Abstract

The embodiment of the invention discloses a method and a system for determining the initial value of an attitude angle of a north seeker capable of continuously rotating in a whole period, and the method comprises the following steps: 1, controlling an indexing mechanism of the north seeker to continuously rotate in the whole period according to a preset rotation mode, and collecting the data of a gyroscope and an additive in real time, the preset rotation mode comprises preset rotation speed, rotation acceleration and rotation turns; and 2, calculating a pitch angle, a roll angle and a course angle according to the collected gyroscope and additional meter data. According to the method, the attitude initial value of the north seeker is determined under the condition of full-period rotation, and an anti-interference attitude angle can be obtained after continuous rotation for multiple circles, sine modulation, cosine modulation and full-period integration by utilizing the favorable condition that the position precision of a rotating mechanism is relatively high.

Description

Technical Field

[0001] The present invention relates to the technical field of north finders, and in particular to a method and system for determining an initial value of an attitude angle of a full-cycle continuously rotating north finder. Background Art

[0002] The determination of attitude angle is an important requirement in the field of inertial navigation. For inertial navigation equipment with three gyros and three adders, the method for determining the initial value of the attitude angle generally adopts dual-vector attitude determination. In the application of the north-finder system, the "transposition mechanism + single gyro" 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. A low-cost and high-precision full attitude determination method under static conditions adopts the "single horizontal gyro + dual horizontal adders + transposition mechanism" hardware solution.

[0003] The existing attitude determination schemes of north finders include dual-position attitude determination method, three-position attitude determination method, and four-position attitude determination method. However, these existing attitude determination schemes rely on the accuracy of the rotating mechanism, resulting in the disadvantages of high cost, poor accuracy, and poor anti-interference performance. Summary of the invention

[0004] 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 attitude angle of a full-cycle continuously rotating north finder, so as to improve the accuracy and anti-interference performance and reduce the cost.

[0005] In order to solve the above technical problems, an embodiment of the present invention proposes a method for determining an initial value of an attitude angle of a full-cycle continuously rotating north finder, comprising: Step 1: Control the indexing mechanism of the north finder to rotate continuously in a full cycle according to a preset rotation mode, and collect gyro and accelerometer data in real time, wherein the preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations; Step 2: Calculate the pitch angle, roll angle, and heading angle based on the collected gyro and gage data.

[0006] Accordingly, an embodiment of the present invention further provides a system for determining an initial value of an attitude angle of a full-cycle continuously rotating north finder, comprising: Driving and collecting module: controlling the indexing mechanism of the north finder to rotate continuously in a whole cycle according to a preset rotation mode, and collecting gyro and accelerometer data in real time, wherein the preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations; Attitude angle calculation module: The pitch angle, roll angle and heading angle are calculated based on the collected gyro and accelerometer data.

[0007] The beneficial effects of the present invention are as follows: the present invention determines the initial attitude value of the north-finder based on the full-cycle rotation, utilizes the favorable condition of high position accuracy of the rotating mechanism, and after continuous rotation for multiple circles, and then through sine modulation and cosine modulation, after full-cycle integration, it can obtain an interference-resistant attitude angle with high accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 It is a flow chart of a method for determining an initial value of an attitude angle of a full-cycle continuously rotating north finder according to an embodiment of the present invention. DETAILED DESCRIPTION

[0009] 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.

[0010] 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.

[0011] 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.

[0012] Please refer to Figure 1 The method for determining the initial value of the attitude angle of a full-cycle continuously rotating north finder according to an embodiment of the present invention includes steps 1 and 2.

[0013] Step 1: Perform preparation work before finding the north, control the north finder's indexing mechanism to rotate continuously in a full cycle according to the preset rotation mode, and collect gyro and accelerometer data in real time. The preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations. The north finder includes a forward gyro, an indexing mechanism, a right accelerometer (i.e., an X-axis accelerometer), and a forward accelerometer (i.e., a Y-axis accelerometer).

[0014] The preparations for seeking the north are as follows: 1) Before collecting north-seeking data, it is necessary to self-check the gyro data, counter data, and indexing mechanism data to ensure that the inertial device data and indexing mechanism are normal.

[0015] 2) The indexing mechanism needs to be returned to zero 3) Send a command for the indexing mechanism to rotate. The rotation command must clearly specify the rotation speed, rotation acceleration, and number of rotations, and it must be a full circle rotation.

[0016] 4) Data collection starts immediately after the rotation command is sent, and ends after the rotation stops. The inertial device data needs to be temperature compensated, calibrated, and converted to radians before being used in the formula.

[0017] Step 2: Calculate the pitch angle, roll angle, and heading angle based on the collected gyro and gage data.

[0018] Definition: The navigation coordinate system is the geographic system, represented by n. The carrier coordinate system is the IMU upper right front coordinate system when the rotation mechanism is 0, represented by b. The IMU coordinate system is represented by p. The IMU rotates with the rotation mechanism.

[0019] During the rotation of the rotating mechanism, the degree of the code disk of the indexing mechanism is , then the rotation matrix is: ; The attitude matrix is

[0020] 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.

[0021] The total output of each position is: ; ; in, Represents the accelerometer output value at each position, It indicates the specific force output under the navigation system, and g is the local gravity acceleration value.

[0022] The local gravitational acceleration can be calculated using 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.

[0023] There are right-hand and forward add-ons in the north-finder. The output value of each add-on position can be obtained by the above formula: ; ; in, Represents the theoretical output value of rightward addition, represents the theoretical output value of the forward addition, represents the pitch angle, represents the roll angle, g is the local gravitational acceleration, is the code disk degree of the indexing mechanism, Multiply the summed output values ​​by and , perform sine and cosine modulation, and then perform full cycle integration.

[0024] ; ; ; ; in, is the first type of calculation variable for the rightward addition of the i-th circle, is the second type of calculation variable for the right addition of the i-th circle, is the first type of calculation variable for the forward addition of the i-th round, The second type of calculation variable for the forward addition of the i-th round.

[0025] Generally, the time for a North-seeker to find the North is 3 minutes. It rotates at 20° / s, and one rotation takes about 20 seconds. That is, it can rotate 9 times. The rotation angular rate and number of rotations are set according to different systems. The principle is to work in the optimal state of the rotation mechanism. The pitch angle and roll angle values ​​are: ; ; Among them, N represents the total number of continuous rotations, for example, here N=9.

[0026] In this way, the pitch angle and roll angle are calculated. Now we calculate the heading angle value.

[0027] The gyro outputs at each position are: ; ; in, Represents the gyro output value at each position, is the Earth's rotation angular velocity vector, The angular rate of the Earth's rotation is 15.0411° / h.

[0028] There is only a forward gyro in the north finder. The output value of each position of the forward gyro can be obtained by the above formula: ; in, Represents the gyro output value at each position, represents the pitch angle, represents the heading angle, represents the roll angle, L is the local latitude, is the code disk degree of the indexing mechanism, is the angular rate of the Earth's rotation.

[0029] Multiply the gyro output values ​​by and , and then perform full cycle integration.

[0030] ; ; in, represents the first calculation variable of the gyroscope in the i-th circle, It represents the second calculation variable of the gyro in the i-th circle. Generally, the north-seeking time of a north-seeking instrument is 3 minutes. It rotates at 20° / s, and one circle takes about 20 seconds. That is, it can rotate 9 circles. Prolonging the north-seeking time or increasing the rotation speed can increase the number of rotation circles. The rotation angular rate and the number of circles are set according to different systems. The principle is to work in the optimal state of the rotating mechanism. Then: ; ; ; Among them, N represents the number of continuous rotations, for example, here N=9 can be taken.

[0031] After the above steps, the calculation of attitude angles (pitch angle, roll angle, heading angle) is completed.

[0032] The attitude angle initial value determination system of the full-cycle continuous rotation north finder of the embodiment of the present invention comprises: Driving and collecting module: controlling the indexing mechanism of the north finder to rotate continuously in a whole cycle according to a preset rotation mode, and collecting gyro and accelerometer data in real time, wherein the preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations; Attitude angle calculation module: The pitch angle, roll angle and heading angle are calculated based on the collected gyro and accelerometer data.

[0033] As an implementation method, the attitude angle calculation module calculates the pitch angle and the roll angle according to the following formula: ; ; in, represents the pitch angle, represents the roll angle, g is the local gravitational acceleration, is the first type of calculation variable for the right-hand addition of the north-finder of the i-th circle, is the second calculation variable of the right-hand addition of the north-finder of the i-th circle, The first type of calculation variable is the forward addition of the north finder of the ith circle, The second calculation variable is the forward addition of the north-finder of the i-th circle, N represents the total number of continuous rotations of the whole cycle, i∈N.

[0034] As an implementation method, the attitude angle calculation module calculates according to the following formula: , , , : ; ; ; ; ; ; in, Represents the theoretical output value of the right-hand addition of the north-seeker. Represents the theoretical output value of the forward addition of the north-seeker. The degree of the code disk of the north-finding instrument's indexing mechanism.

[0035] As an implementation method, the attitude angle calculation module calculates the heading angle according to the following formula: ; ; ; in, is the heading angle, L is the local latitude, is the Earth's rotation angular velocity, represents the first calculation variable of the gyroscope in the i-th circle, Represents the second calculation variable of the gyroscope in the i-th circle.

[0036] As an implementation method, the attitude angle calculation module calculates according to the following formula: , : ;

[0037] in, Represents the gyroscope output value at each position.

[0038] 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 attitude angle of a full-cycle continuously rotating north finder, characterized in that: include: Step 1: Control the indexing mechanism of the north finder to rotate continuously in a full cycle according to a preset rotation mode, and collect gyro and accelerometer data in real time, wherein the preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations; Step 2: Calculate the pitch angle, roll angle, and heading angle based on the collected gyro and gage data.

2. The method for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 1, characterized in that: In step 2, the pitch angle and roll angle are calculated according to the following formula: ； ； in, represents the pitch angle, represents the roll angle, g is the local gravitational acceleration, is the first type of calculation variable for the right-hand addition of the north-finder of the i-th circle, is the second calculation variable of the right-hand addition of the north-finder of the i-th circle, The first type of calculation variable is the forward addition of the north finder of the ith circle, The second calculation variable is the forward addition of the north-finder of the i-th circle, N represents the total number of continuous rotations of the whole cycle, i∈N.

3. The method for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 2, characterized in that: , , , Calculated according to the following formula: ； ； ； ； ； ； in, Represents the theoretical output value of the right-hand addition of the north-seeker. Represents the theoretical output value of the forward addition of the north-seeker. The degree of the code disk of the north-finding instrument's indexing mechanism.

4. The method for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 3, characterized in that: In step 2, the heading angle is calculated according to the following formula: ； ； ； in, is the heading angle, L is the local latitude, is the Earth's rotation angular velocity, represents the first calculation variable of the gyroscope in the i-th circle, Represents the second calculation variable of the gyroscope in the i-th circle.

5. The method for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 4, characterized in that: , Calculated according to the following formula: ； ； in, Represents the gyroscope output value at each position.

6. A system for determining the initial value of the attitude angle of a full-cycle continuously rotating north finder, characterized in that: include: Driving acquisition module: controlling the indexing mechanism of the north finder to rotate continuously in a full cycle according to a preset rotation mode, and collecting gyro and accelerometer data in real time, wherein the preset rotation mode includes a preset rotation speed, rotation acceleration, and number of rotations; Attitude angle calculation module: The pitch angle, roll angle and heading angle are calculated based on the collected gyro and accelerometer data.

7. The system for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 6, characterized in that: The attitude angle calculation module calculates the pitch angle and roll angle according to the following formula: ； ； in represents the pitch angle, represents the roll angle, g is the local gravitational acceleration, is the first type of calculation variable for the right-hand addition of the north-finder of the i-th circle, is the second calculation variable of the right-hand addition of the north-finder of the i-th circle, is the first type of calculation variable for the forward addition of the north-finder of the ith circle, The second calculation variable is the forward addition of the north-finder of the i-th circle, N represents the total number of continuous rotations of the whole cycle, i∈N.

8. The attitude angle initial value determination system of the full-cycle continuous rotation north finder according to claim 7, characterized in that: The attitude angle calculation module is calculated according to the following formula , , , : ； ； ； ； ； ； in, Represents the theoretical output value of the right-hand addition of the north-seeker. Represents the theoretical output value of the forward addition of the north-seeker. The degree of the code disk of the north-finding instrument's indexing mechanism.

9. The system for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 8, characterized in that: The attitude angle calculation module calculates the heading angle according to the following formula: ； ； ； in, is the heading angle, L is the local latitude, is the Earth's rotation angular velocity, represents the first calculation variable of the gyroscope in the i-th circle, Represents the second calculation variable of the gyroscope in the i-th circle.

10. The system for determining the initial value of the attitude angle of the full-cycle continuous rotation north finder according to claim 9, characterized in that: The attitude angle calculation module is calculated according to the following formula , : ； ； in, Represents the gyroscope output value at each position.

Images