Inertial navigation system attitude correction method and computer equipment

Abstract

The invention provides an inertial navigation system attitude correction method and computer equipment. The method comprises the following steps: step 1, constructing an error correction model; 2, filtering is carried out based on the error correction model, correction strategy switching is completed by judging the motion state of a carrier in the filtering process, and attitude self-correction is achieved, specifically, when attitude self-correction is carried out by entering filtering for the first time, the motion state of the carrier is judged in real time according to a designed motion state judgment method (a), and the motion state of the carrier is judged in real time according to the designed motion state judgment method (b); if the attitude angle is judged to be high dynamic in the judgment process, filtering is stopped, and when the duration time of the high dynamic is larger than preset time delta t, the motion state of the carrier is judged in real time according to a designed motion state judgment method b), if the attitude angle is judged to be low dynamic in the judgment process, filtering is restarted until the attitude angle is corrected for preset K times, and then filtering is stopped. And performing dynamic judgment again according to the designed motion state judgment method a).

Description

Technical Field

[0001] The present invention belongs to the technical field of inertial navigation, and in particular relates to an inertial navigation system attitude correction method and computer equipment, mainly targeting the application field of underwater autonomous navigation. Background Art

[0002] With the in-depth implementation of the major strategy of "ocean power" and the intensification of international maritime rights and interests conflicts, the importance of underwater weapons and equipment and operating equipment has become increasingly prominent, and has become a national weapon for maintaining national sovereignty, security and development. Autonomous navigation is the key to underwater equipment's mission, and determines the long-range delivery, precise arrival, precision strike and fine operation capabilities of weapons and equipment.

[0003] There is limited auxiliary information available for underwater environmental motion measurement, especially under deep and large-scale operating conditions, where inertial measurement is usually the only means. However, under complex sea conditions, affected by factors such as surges and ocean currents, the carrier motion is complex, and the inertial measurement line-angle motion is severely coupled, making it difficult to identify and separate, and the measurement error increases by 1 to 2 orders of magnitude. High-precision attitude measurement is difficult under complex sea conditions, which is a bottleneck restricting the improvement of underwater platform precision operation capabilities. Summary of the invention

[0004] The present invention aims to solve at least one of the technical problems existing in the prior art.

[0005] To this end, the present invention provides an inertial navigation system attitude correction method and computer equipment.

[0006] The technical solution of the present invention is as follows:

[0007] According to one aspect, a method for correcting an inertial navigation system attitude is provided, the method comprising:

[0008] Step 1: Build an error correction model, including:

[0009] Establish the system state equation;

[0010] Establish measurement equations;

[0011] Among them, when establishing the measurement equation, take and As an observation, The north and east acceleration values ​​in the navigation coordinate system respectively;

[0012] Step 2: filtering based on the error correction model, and switching the correction strategy by judging the carrier motion state during the filtering process to achieve attitude self-correction, wherein:

[0013] When entering the filter for the first time to perform attitude self-correction, the carrier motion state is judged in real time according to the designed motion state judgment method a). If it is judged to be high dynamic during the judgment process, the filtering stops, and when the high dynamic duration is greater than the preset time Δt, the carrier motion state is judged in real time according to the designed motion state judgment method b). If it is judged to be low dynamic during the judgment process, the filtering is restarted, and until the attitude angle has been corrected for the preset K times, the dynamic judgment is performed again according to the designed motion state judgment method a);

[0014] The motion state determination method a) is: the acceleration value in the navigation coordinate system of the current filtering period The absolute value of the average value of the Δt1 time is less than the threshold When , the carrier is in low dynamics, otherwise it is in high dynamics;

[0015] The motion state determination method b) is: judging the absolute value of the mean of the root mean square value Δt2 of the acceleration value in the body coordinate system of the current filtering period, if it is less than the threshold Then the dynamic discrimination result is set to low dynamic.

[0016] Furthermore, the root mean square value of the acceleration value in the body coordinate system of the current filtering period is Obtained by the following formula:

[0017]

[0018] in, is the acceleration value in the body coordinate system of the current filtering period. g0 is the local gravity acceleration.

[0019] Furthermore, Δt is taken as 100 s.

[0020] Furthermore, Δt1 and Δt2 are both set to 1 s.

[0021] Furthermore, the threshold Designed to be less than 1m / s 2 .

[0022] Furthermore, the preset K times is designed to be 5 times.

[0023] Furthermore, the system state equation is established in the following way:

[0024] Design the system state vector X as follows:

[0025] X=[φxφyφzε x ε y ε z ] T (1)

[0026] Where: φx, φy, φz, εx, εy, εz are the north misalignment angle, celestial misalignment angle, east misalignment angle, X gyro drift, Y gyro drift and Z gyro drift respectively;

[0027] The system error model is established according to the Kalman filter state quantity, and the system equation is established as follows:

[0028]

[0029] Where Ω is the Earth's rotation angular rate, is the carrier latitude, C ij (i=1,2,3;j=1,2,3) are the corresponding elements of the inertial navigation attitude transfer matrix;

[0030] Then, the system state equation is established as follows:

[0031]

[0032] in,

[0033]

[0034] W is the system random noise vector.

[0035] Furthermore, the measurement equation is established by the following formula:

[0036] Pick As the observed quantity, the observation equation is established:

[0037]

[0038] Among them, the measurement matrix R is the measurement noise vector, and g is the local gravitational acceleration.

[0039] Further, Obtained by the following formula:

[0040]

[0041] Where g is the local gravitational acceleration, ▽ N ,▽ E They are the north and east zero bias of the accelerometer respectively.

[0042] According to another aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned posture correction method when executing the computer program.

[0043] The above technical solution is aimed at the high-precision and highly autonomous attitude measurement needs of underwater weapons and equipment, and proposes an inertial navigation system attitude correction method. First, the inertial navigation system's own information is used to establish an error correction model, combined with the characteristics of underwater motion, and the acceleration information is used as the observed quantity to establish a measurement equation; and by judging the motion state of the carrier, the correction strategy is switched to achieve high-precision self-correction of the attitude, solving the problem of difficult underwater attitude measurement. This method does not need to rely on any external auxiliary navigation information, and makes full use of the inertial navigation system's own sensor information. At the same time, combined with the motion characteristics of underwater equipment, the motion state is judged in real time, and then the attitude is adaptively self-corrected. It not only has strong autonomy, but also adapts to the flexibility of underwater weapon and equipment movement, and has high reliability and high precision. DETAILED DESCRIPTION

[0044] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention are clearly and completely described below. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0045] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0046] Unless otherwise specifically stated, the relative arrangement of the parts and steps set forth in these embodiments, numerical expressions and numerical values ​​do not limit the scope of the present invention. The technology, method and equipment known to those of ordinary skill in the relevant art may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be considered as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values.

[0047] In one embodiment of the present invention, a method for correcting an attitude of an inertial navigation system is provided, the method comprising:

[0048] Step 1: Build an error correction model, including:

[0049] Establish the system state equation;

[0050] Establish measurement equations;

[0051] Among them, when establishing the measurement equation, take and As an observation, The north and east acceleration values ​​in the navigation coordinate system respectively;

[0052] Step 2: filtering based on the error correction model, and switching the correction strategy by judging the carrier motion state during the filtering process to achieve attitude self-correction, wherein:

[0053] When entering the filter for the first time to perform attitude self-correction, the carrier motion state is judged in real time according to the designed motion state judgment method a). If it is judged to be high dynamic during the judgment process, the filtering stops, and when the high dynamic duration is greater than the preset time Δt, the carrier motion state is judged in real time according to the designed motion state judgment method b). If it is judged to be low dynamic during the judgment process, the filtering is restarted, and until the attitude angle has been corrected for the preset K times, the dynamic judgment is performed again according to the designed motion state judgment method a);

[0054] The motion state determination method a) is: the acceleration value in the navigation coordinate system of the current filtering period The absolute value of the average value of the Δt1 time is less than the threshold When , the carrier is in low dynamics, otherwise it is in high dynamics;

[0055] The motion state determination method b) is: judging the absolute value of the mean of the root mean square value Δt2 of the acceleration value in the body coordinate system of the current filtering period, if it is less than the threshold Then the dynamic discrimination result is set to low dynamic.

[0056] That is, when in low dynamics, filtering is continued; if the carrier is in high dynamics, the filter stops, and the carrier's attitude output directly takes the result of strapdown navigation solution; when starting or re-entering low dynamics for the first time (that is, the previous moment is high dynamics and the current moment is low dynamics), the filter is restarted to correct the attitude error.

[0057] It can be seen that the embodiment of the present invention is oriented to the high-precision and high-autonomous attitude measurement needs of underwater weapons and equipment, and proposes an inertial navigation system attitude correction method. First, the inertial navigation system's own information is used to establish an error correction model, combined with the underwater motion characteristics, and the acceleration information is used as the observed quantity to establish a measurement equation; and by judging the motion state of the carrier, the correction strategy is switched to achieve high-precision self-correction of the attitude, solving the problem of difficult underwater attitude measurement. This method does not need to rely on any external auxiliary navigation information, and makes full use of the inertial navigation system's own sensor information. At the same time, combined with the motion characteristics of underwater equipment, the motion state is judged in real time, and then the attitude is adaptively self-corrected. It not only has strong autonomy, but also adapts to the flexibility of underwater weapon equipment movement, and has high reliability and high precision.

[0058] In the above embodiment, the system state equation is established in the following manner:

[0059] Design the system state vector X as follows:

[0060] X=[φxφyφzε x ε y ε z ] T (1)

[0061] Where: φx, φy, φz, εx, εy, εz are the north misalignment angle, celestial misalignment angle, east misalignment angle, X gyro drift, Y gyro drift and Z gyro drift respectively;

[0062] The system error model is established according to the Kalman filter state quantity, and the system equation is established as follows:

[0063]

[0064] Where Ω is the Earth's rotation angular rate, is the carrier latitude, C ij (i=1,2,3;j=1,2,3) are the corresponding elements of the inertial navigation attitude transfer matrix;

[0065] Then, the system state equation is established as follows:

[0066]

[0067] in,

[0068]

[0069] W is the system random noise vector.

[0070] In the above embodiment, when the measurement equation is established, when the system is in low dynamics, the accelerometer measurement value is mainly the gravity vector, and the two horizontal components are almost zero, so the following relationship is established:

[0071]

[0072] in, The projection of the accelerometer measurement value in the north and east directions in the navigation coordinate system (n system), where g is the local gravity acceleration. N ,▽ E They are the north and east zero bias of the accelerometer respectively.

[0073] Pick As the observed quantity, the observation equation is established:

[0074]

[0075] Among them, the measurement matrix R is the measurement noise vector, and g is the local gravitational acceleration.

[0076] In the above embodiment, the root mean square value of the acceleration value in the body coordinate system of the current filtering period is Obtained by the following formula:

[0077]

[0078] in, It is the acceleration value in the body coordinate system of the current filtering period.

[0079] In the above embodiment, in order to better ensure the filtering effect, the following optimization design is also performed:

[0080] Preferably, Δt is taken as 100s.

[0081] Preferably, Δt1 and Δt2 are both set to 1 s.

[0082] Preferably, the threshold Designed to be less than 1m / s 2 .

[0083] Preferably, the preset K times is designed to be 5 times.

[0084] According to another embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned posture correction method when executing the computer program.

[0085] Features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or used in place of features in other embodiments.

[0086] It should be emphasized that the term “include / comprises” when used herein refers to the presence of features, integers, steps or components, but does not exclude the presence or addition of one or more other features, integers, steps, components or combinations thereof.

[0087] The above method of the present invention can be implemented by hardware, or by hardware combined with software. The present invention relates to such a computer-readable program, which, when executed by a logic component, enables the logic component to implement the above-mentioned device or component, or enables the logic component to implement the above-mentioned various methods or steps. The present invention also relates to a storage medium for storing the above program, such as a hard disk, a magnetic disk, an optical disk, a DVD, a flash memory, etc.

[0088] The many features and advantages of these embodiments are apparent from this detailed description, and thus the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since numerous modifications and changes will readily occur to those skilled in the art, it is not intended that the embodiments of the invention be limited to the exact construction and operation illustrated and described, but rather all suitable modifications and equivalents falling within the scope thereof are intended to be covered.

[0089] Parts of the present invention that are not described in detail are well known to those skilled in the art.

Claims

1. A method for correcting an inertial navigation system attitude, characterized in that: The posture correction method comprises: Step 1: construct an error correction model, including: Establish the system state equation; Establish measurement equations; Among them, when establishing the measurement equation, take and As an observation, The north and east acceleration values ​​in the navigation coordinate system respectively; Step 2: filtering based on the error correction model, and switching the correction strategy by judging the carrier motion state during the filtering process to achieve attitude self-correction, wherein: When entering the filter for the first time to perform attitude self-correction, the carrier motion state is judged in real time according to the designed motion state judgment method a). If it is judged to be high dynamic during the judgment process, the filtering stops, and when the high dynamic duration is greater than the preset time Δt, the carrier motion state is judged in real time according to the designed motion state judgment method b). If it is judged to be low dynamic during the judgment process, the filtering is restarted, and until the attitude angle has been corrected for the preset K times, the dynamic judgment is performed again according to the designed motion state judgment method a); The motion state determination method a) is: the acceleration value in the navigation coordinate system of the current filtering period The absolute value of the average value of the Δt1 time is less than the threshold When , the carrier is in low dynamics, otherwise it is in high dynamics; The motion state determination method b) is: judging the absolute value of the mean of the root mean square value Δt2 of the acceleration value in the body coordinate system of the current filtering period, if it is less than the threshold Then the dynamic discrimination result is set to low dynamic.

2. The inertial navigation system attitude correction method according to claim 1, characterized in that: The root mean square value of the acceleration value in the body coordinate system during the current filtering period Obtained by the following formula: in, is the acceleration value in the body coordinate system of the current filtering period. g0 is the local gravity acceleration.

3. The inertial navigation system attitude correction method according to claim 1, characterized in that: Δt is taken as 100s.

4. The inertial navigation system attitude correction method according to any one of claims 1 to 3, characterized in that: Both Δt1 and Δt2 are set to 1 s.

5. The inertial navigation system attitude correction method according to claim 4, characterized in that: Threshold Designed to be less than 1m / s 2 .

6. The inertial navigation system attitude correction method according to claim 5, characterized in that: The preset K-fold design is 5 times.

7. An inertial navigation system attitude correction method according to any one of claims 1 to 6, characterized in that: The system state equation is established in the following way: Design the system state vector X as follows: X=[φxφyφze x e y e z ] T (1) Where: φx, φy, φz, εx, εy, εz are the north misalignment angle, celestial misalignment angle, east misalignment angle, X gyro drift, Y gyro drift and Z gyro drift respectively; The system error model is established according to the Kalman filter state quantity, and the system equation is established as follows: Where Ω is the Earth's rotation angular rate, is the carrier latitude, C ij (i=1,2,3;j=1,2,3) are the corresponding elements of the inertial navigation attitude transfer matrix; Then, the system state equation is established as follows: in, W is the system random noise vector.

8. The inertial navigation system attitude correction method according to claim 7, characterized in that: The measurement equation is established by the following formula: Pick As the observed quantity, the observation equation is established: Among them, the measurement matrix R is the measurement noise vector, and g is the local gravitational acceleration.

9. The inertial navigation system attitude correction method according to claim 8, characterized in that: Obtained by the following formula: Where g is the local gravitational acceleration, They are the north and east zero bias of the accelerometer respectively.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the posture correction method described in any one of claims 1 to 9 is implemented.

Images