Latitude initial value estimation method and system of inertial navigation equipment in mooring state
By collecting and compensating sensor data in the inertial navigation device and establishing the least squares estimation equation, the problem that the inertial navigation device cannot estimate the initial latitude and gravity acceleration in the mooring state is solved, and accurate estimation is achieved without an initial position and navigation initial alignment.
Patent Information
- Application Number
- CN202411053169.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-02
- Publication Date
- 2024-11-08
AI Technical Summary
Inertial navigation equipment cannot effectively estimate the initial latitude and gravity acceleration in the mooring state, resulting in difficulty in initial navigation alignment. The existing methods are weak in adaptability and cannot be used without position initial values and shaking base conditions.
By collecting sensor data from inertial navigation equipment for compensation processing, establishing the least squares estimation equation fit, calculating the estimated initial value of latitude, and using gyroscope and accelerometer data to estimate the initial latitude and gravity acceleration in the mooring state.
In the case where the initial position is agnostic, the initial latitude and gravity acceleration can be accurately estimated, supporting subsequent navigation initial alignment and combined navigation updates, without external auxiliary information, and is suitable for a variety of application scenarios.
Smart Images

Figure CN118913314A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of inertial navigation technology, and in particular to a method and system for estimating the initial latitude of an inertial navigation device in a moored state. Background Art
[0002] Before the inertial navigation device can perform navigation, it is necessary to determine the initial value. In some application fields, the inertial navigation device cannot obtain the initial position information due to various reasons, and the initial alignment must be iteratively calculated based on the initial position information. The general initial position estimation method is to use the three-axis gyroscope output data to directly calculate the initial value of latitude in proportion. This method can only be used in purely static conditions and has weak adaptability. However, in the absence of initial position values, under shaking bases or mooring conditions, it is necessary to estimate the initial latitude value and gravity acceleration value so that the inertial navigation device can further perform initial alignment and combined navigation updates. 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 estimating the initial latitude of an inertial navigation device in a moored state, so as to achieve the estimation of the initial latitude value and the gravity acceleration value under the condition of no initial position value and a shaking base or mooring.
[0004] In order to solve the above technical problems, an embodiment of the present invention proposes a method for estimating the initial value of latitude of an inertial navigation device in a moored state, comprising: Step 1: Collect inertial sensor data of the inertial navigation device and perform compensation processing on the inertial sensor data; Step 2: Establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting. Calculate the estimated initial value of the latitude based on the fitting result.
[0005] Accordingly, an embodiment of the present invention further provides a system for estimating the initial latitude of an inertial navigation device in a moored state, comprising: Acquisition module: collects inertial sensor data of inertial navigation equipment and performs compensation processing on the inertial sensor data; Latitude estimation module: establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting, and calculate the estimated initial value of the latitude based on the fitting result.
[0006] The beneficial effects of the present invention are as follows: when the initial position is unknown, the present invention can estimate the initial latitude and gravity acceleration in a moored state through the gyroscope and speedometer of the inertial navigation device, thereby providing latitude information for subsequent initial alignment; the present invention does not require any external assistance and can be directly applied to certain application scenarios that require latitude estimation. BRIEF DESCRIPTION OF THE DRAWINGS
[0007] Figure 1 It is a flow chart of a method for estimating an initial value of latitude of an inertial navigation device in a moored state according to an embodiment of the present invention.
[0008] Figure 2 It is a latitude estimation curve diagram of the latitude initial value estimation method of the inertial navigation device in the moored state according to Example 1 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 estimating the initial latitude of an inertial navigation device in a moored state according to an embodiment of the present invention includes steps 1 and 2.
[0013] Step 1: Collect the inertial sensor data of the inertial navigation device and compensate the inertial sensor data. In step 1, the gyro raw data and accelerometer raw data of the inertial navigation device can be obtained through a sampling frequency of 200 Hz, and the angle increment data and velocity increment data can be obtained after temperature compensation and tool error compensation. Then, the inertial device data is preprocessed by the sliding window smoothing method, and the sliding window depth is set to 10.
[0014] Step 2: Establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting. Calculate the estimated initial value of the latitude based on the fitting result.
[0015] First construct the equation for the latitude to be found.
[0016] Generally, the carrier Velocity vector of the system It can be solved directly based on the inertial device output of the IMU.
[0017] ; in, The carrier inertial system The velocity vector under is the specific force output, which can be determined by the output of the accelerometer, is the angular position change matrix, which can be solved by the gyro output; Navigation coordinate system The gravity vector under ; in, is the local gravitational acceleration, and its specific calculation formula is as follows.
[0018] ; g cannot be accurately obtained when the initial position (latitude) is unknown.
[0019] Position reference matrix Determined by the following formula.
[0020]
[0021] ; in, , Expressed as cos, represents sin, represents the initial longitude value at the time of initial alignment, t represents the time of entering the initial alignment, Represents the Earth's rotation angular rate, as long as the current latitude L and longitude Known, then It's confirmed.
[0022] When the carrier is in static condition, swaying base condition, or moored condition. Since the carrier has basically no obvious linear motion, that is, , , then the above formula is transformed into: ; Will The gravitational acceleration vector is projected onto Tie: ; in, is the estimated initial value of latitude, i.e., the quantity to be determined in this example of the present invention. is the Earth's rotation angular velocity, is the initial alignment time.
[0023] Velocity Vector Through It is calculated in the form of integral, namely: .
[0024] It can be seen that Only with alignment time Changes, within the alignment time (generally inertial systems are aligned for 5 minutes), Can be regarded as a small amount, and Carry out Taylor series expansion, then ; Will The Taylor expansion of is approximately expressed as: ; Due to latitude and the acceleration due to gravity Unknown, the right side of the equation Cannot be solved directly.
[0025] According to the matrix chain multiplication rule, we have the following relationship.
[0026] ; in and Inertial system and The velocity vector under the system. The initial moment attitude matrix representing the initial alignment is a constant matrix.
[0027] but It can also be expressed as a polynomial equation in time t.
[0028] ; The coefficient is a 3-dimensional constant column vector, and ; .
[0029] Establish the least squares equation. The measurement equation is as follows ; in, To measure the noise, is the state vector, H(t) is the measurement matrix; ; ; ; The state vector is calculated by using the recursive least squares algorithm. Perform fitting and obtain its coefficients .
[0030] because is a constant attitude matrix that satisfies the orthogonality of the matrix. After transformation, we get the estimated initial values of gravity acceleration and latitude: ; .
[0031] in, , is the coefficient obtained, , is the second norm of the coefficient, The positive sign represents the northern hemisphere. The negative sign represents the Southern Hemisphere. In China, positive values are generally used, and the Northern and Southern Hemispheres are determined manually.
[0032] The present invention does not require the carrier to be completely still and does not require additional auxiliary information, and can solve the problem of initial latitude estimation under shaking base or mooring conditions.
[0033] Example 1: Please refer to Figure 2 In Example 1, an inertial navigation device with a gyro bias stability of 0.03° / h and an accelerometer bias stability of 100 mg is used, and its latitude estimation accuracy is within 10 -2 Magnitude.
[0034] The latitude initial value estimation system of the inertial navigation device in the moored state comprises a collection module and a latitude estimation module.
[0035] Acquisition module: collects the inertial sensor data of the inertial navigation device and performs compensation processing on the inertial sensor data.
[0036] Latitude estimation module: establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting, and calculate the estimated initial value of the latitude based on the fitting result.
[0037] As an implementation method, the latitude estimation module calculates the estimated initial value of gravity acceleration g and latitude according to the following formula: : ; ; in, , is the coefficient obtained by fitting the least squares estimation equation, , Respectively represent the coefficients , The second norm of; in the northern hemisphere, the estimated initial value of latitude Take a positive sign, in the southern hemisphere, the estimated initial value of latitude Take the negative sign; is the angular rate of the Earth's rotation.
[0038] As an implementation method, the latitude estimation module is fitted according to the following process: First establish the least squares estimation equation: ; ; ; in and The carrier is in the inertial system and The velocity vector under the system, represents the initial moment pose matrix of the initial alignment, is the initial alignment moment; The coefficient is a 3-dimensional column vector of undetermined constant values, and: ; ; Then establish the least squares equation, the measurement equation is as follows: ; in, To measure the noise, is the state vector, H(t) is the measurement matrix; ; ; ; Finally, the recursive least squares algorithm is used to solve the state vector Perform fitting and obtain coefficients .
[0039] As an implementation method, the latitude estimation module calculates the velocity vector by the following formula: : ; in, is the specific force output, is the angular position change matrix, is the integrating factor.
[0040] As an implementation, the latitude estimation module represents the latitude in the moored state. : .
[0041] 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 estimating the initial latitude of an inertial navigation device in a moored state, characterized in that: include: Step 1: Collect inertial sensor data of the inertial navigation device and perform compensation processing on the inertial sensor data; Step 2: Establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting. Calculate the estimated initial value of the latitude based on the fitting result.
2. The method for estimating the initial latitude of an inertial navigation device in a moored state according to claim 1, characterized in that: In step 2, the estimated initial values of gravitational acceleration g and latitude are calculated according to the following formula: : ; ; in, , is the coefficient obtained by fitting the least squares estimation equation, , Respectively represent the coefficients , The second norm of; in the northern hemisphere, the estimated initial value of latitude Take a positive sign, in the southern hemisphere, the estimated initial value of latitude Take the negative sign; is the angular rate of the Earth's rotation.
3. The method for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 2, characterized in that: In step 2, fitting is performed according to the following process: First establish the least squares estimation equation: ; ; ; in Expressed as the carrier in the carrier inertial coordinate system The velocity vector under the system, Expressed as the carrier in the navigation inertial system The velocity vector under the system, represents the initial moment pose matrix of the initial alignment, is the initial alignment moment; The coefficient is a 3-dimensional column vector of undetermined constant values, and: ; ; Then establish the least squares equation, the measurement equation is as follows: ; in, To measure the noise, is the state vector, H(t) is the measurement matrix; ; ; ; Finally, the recursive least squares algorithm is used to solve the state vector Perform fitting and obtain coefficients .
4. The method for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 3, characterized in that: In step 2, the velocity vector is calculated by the following formula : ; in, is the specific force output, is the angular position change matrix, is the integrating factor.
5. The method for estimating the initial latitude of an inertial navigation device in a moored state according to claim 4, characterized in that: In step 2, the matrix can be used to accurately describe the carrier in the moored system. The velocity vector under the system : 。 6. A system for estimating the initial latitude of an inertial navigation device in a moored state, characterized in that: include: Acquisition module: collects inertial sensor data of inertial navigation equipment and performs compensation processing on the inertial sensor data; Latitude estimation module: establish the relationship between the latitude value to be determined and the known quantity, and establish the least squares estimation equation for fitting, and calculate the estimated initial value of the latitude based on the fitting result.
7. The system for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 6, characterized in that: The latitude estimation module calculates the estimated initial value of gravity acceleration g and latitude according to the following formula: : ; ; in, , is the coefficient obtained by fitting the least squares estimation equation, , Respectively represent the coefficients , The second norm of; in the northern hemisphere, the estimated initial value of latitude Take a positive sign, in the southern hemisphere, the estimated initial value of latitude Take the negative sign; is the angular rate of the Earth's rotation.
8. The system for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 7, characterized in that: The latitude estimation module is fitted according to the following process: First establish the least squares estimation equation: ; ; ; in and The carrier is in the inertial system and The velocity vector under the system, represents the initial moment pose matrix of the initial alignment, is the initial alignment moment; The coefficient is a 3-dimensional column vector of undetermined constant values, and: ; ; Then establish the least squares equation, the measurement equation is as follows: ; in, To measure the noise, is the state vector, H(t) is the measurement matrix; ; ; ; Finally, the recursive least squares algorithm is used to solve the state vector Perform fitting and obtain coefficients .
9. The system for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 8, characterized in that: The latitude estimation module calculates the velocity vector by the following formula : ; in, is the specific force output, is the angular position change matrix, is the integrating factor.
10. The system for estimating the initial latitude of an inertial navigation device in a moored state as claimed in claim 9, characterized in that: The latitude estimation module is represented by the moored state : 。
