A non-holonomic constraint aided SINS / EML integrated navigation method, program, device and storage medium

Abstract

This invention belongs to the field of integrated navigation technology, specifically relating to a non-holonomic constraint-assisted SINS / EML integrated navigation method, program, device, and storage medium. This invention utilizes measurement information from accelerometers and strapdown inertial navigation systems to calculate the ship's lateral acceleration and determine its motion state. For ships in a straight-ahead state, it fully leverages the strapdown inertial navigation calculation information and non-holonomic constraint methods to extract electromagnetic log measurement information, expanding the ship's bow-stern one-dimensional velocity to three dimensions before performing integrated navigation filtering, thus improving the accuracy of the electromagnetic log-assisted strapdown inertial navigation system. For ships in a maneuvering state, it reconstructs the measurement equations based on the electromagnetic log measurement characteristics, reducing the impact of ship sideslip on navigation accuracy. This invention requires no additional equipment, has certain engineering application value, and is applicable to the field of electromagnetic log speed-assisted strapdown inertial navigation technology.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation technology, specifically relating to a non-integrity constraint-assisted SINS / EML integrated navigation method, program, device, and storage medium. Background Technology

[0002] Inertial navigation systems (INS) and Global Navigation Satellite Systems (GNSS) are used in combination for positioning, providing high-frequency, high-precision position, velocity, and attitude data in open environments. However, GNSS signals are susceptible to external interference, leading to decreased positioning accuracy and signal loss. In GNSS-denied environments, ships typically use velocity measurement devices such as differential pressure logs, electromagnetic logs (EMLs), and Doppler logs to assist inertial navigation.

[0003] An electromagnetic speed log is a type of speed log that measures ship speed based on the principle of electromagnetic induction. It can measure both forward and reverse speeds and features high sensitivity, low cost, high accuracy, good linearity, wide range, and good concealment, without causing acoustic exposure. Therefore, when combined with a strapdown inertial navigation system (SINS), it forms a highly accurate and concealed integrated navigation system, making it more suitable for highly concealed navigation missions and gradually becoming standard equipment for ships and submarines. When using an electromagnetic speed log to measure speed information, it is generally assumed that the lateral and vertical velocities of the vessel are both zero. Then, the attitude angles calculated by the strapdown inertial navigation system are directly projected onto the geographic coordinate system. However, during coastal navigation, ships experience pitch and roll angles, or when maneuvering, they may experience lateral sideslip due to inertia. Both of these situations invalidate the assumption that the lateral and vertical velocities are zero, introducing additional speed errors and ultimately reducing the accuracy of the integrated navigation system.

[0004] To improve the accuracy of strapdown inertial / electromagnetic log integrated navigation systems, patent document CN115752453A, published on March 7, 2023, discloses a method and system for ocean current estimation and integrated navigation based on a Hidden Markov Model (HMM) electromagnetic log. This patent document estimates ocean current speed by jointly measuring water velocity using a global navigation satellite system and an electromagnetic log, and compensates for electromagnetic log errors, eliminating the influence of ocean currents on electromagnetic log measurement errors. However, the aforementioned patent does not consider the issues of ship lateral and vertical speeds. Summary of the Invention

[0005] The purpose of this invention is to provide a non-integrity constraint-assisted SINS / EML integrated navigation method that fully considers the influence of ship motion state on electromagnetic log measurements. Ship motion is divided into two states: straight-line and maneuvering. The lateral acceleration of the ship is calculated using inertial navigation solution information to determine the maneuvering situation. Different measurement equations are selected for measurement updates based on different motion states. This reduces the impact of ship sideslip on the integrated navigation system and significantly improves the accuracy of the electromagnetic log-assisted strapdown inertial navigation system.

[0006] A non-integrity constraint-assisted SINS / EML integrated navigation method includes the following steps:

[0007] Step 1: Initial alignment of the strapdown inertial navigation system and initialization of the Kalman filter;

[0008] Step 2: Obtain the ship's acceleration in the geographic coordinate system using an accelerometer, calculate the ship's heading angle using a strapdown inertial navigation system, calculate the ship's lateral acceleration, and determine the ship's motion state based on the lateral acceleration; the motion state includes maneuvering state and straight-line state;

[0009] Step 3: Perform Kalman filter time update based on the initial value of the system's state vector at the current moment;

[0010] Step 4: If the electromagnetic log has a speed update at the current moment, calculate the system's measurement matrix and measurement vector at the current moment based on the ship's motion state;

[0011] Step 5: Kalman filter measurement update, obtain the estimated state vector of the system at the current time;

[0012] Step 6: Use the estimated value of the system's state vector at the current moment as the initial value of the system's state vector at the next moment, and repeat steps 2-5 until the navigation work is completed.

[0013] Furthermore, the state-space model of the Kalman filter is as follows:

[0014]

[0015] Where, Φ k / k-1 W is the state transition matrix; k-1 Γ is the system noise vector at time k-1; k / k-1 Assign a noise matrix to the system; Z k H is the measurement vector of the system at time k; k V is the measurement matrix; k For the measurement noise matrix; X k X k-1 These are the state vectors of the system at time k and k-1, respectively.

[0016]

[0017] Where, φ E φ N φ U These are the platform misalignment angles in the east, north, and sky directions, respectively; δv E δv N δv U δλ, δL, and δh represent the velocity errors in the east, north, and celestial directions, respectively; δλ, δL, and δh represent the longitude, latitude, and altitude errors, respectively; ε E ε N ε U These represent the constant drift errors of the gyroscope in the east, north, and sky directions, respectively; ▽ E 、▽ N 、▽ U These are the accelerometer zero bias errors in the east, north, and sky directions, respectively. These represent the speeds of the eastward and northward ocean currents, respectively.

[0018] Furthermore, step 2 specifically includes:

[0019] lateral acceleration of a ship The calculation method is as follows:

[0020]

[0021] in, φ is the ship's acceleration in the geographic coordinate system, obtained from the specific force equation and the specific force measured by the accelerometer; φ is the ship's heading angle calculated by the strapdown inertial navigation system.

[0022] In actual ship use, to avoid the influence of noise, a sliding window is set up to calculate the average value λ of the ship's lateral acceleration within the sliding window;

[0023]

[0024] in, This represents the ship's lateral acceleration calculated using the i-th update data from the strapdown inertial navigation system within the sliding window; N is the size of the sliding window.

[0025] The system compares λ with a preset threshold. If λ is greater than the threshold, the ship is determined to be in a maneuvering state; otherwise, the ship is determined to be in a straight-ahead state.

[0026] Furthermore, step 3 specifically includes:

[0027]

[0028] in, This is the initial value of the system's state vector at the current moment, which is the estimated value of the system's state vector obtained in step 5 at the previous moment; Φ is the one-step prediction value of the state quantity at time k; k / k-1 P is the state transition matrix at time k; k-1 P is the mean square error matrix for state estimation at time k-1; k / k-1 Let Q be the mean square error matrix of the one-step prediction at time k; k-1 Γ is the system noise variance matrix; k / k-1 Assign a matrix to the system noise.

[0029] Furthermore, step 4 specifically includes:

[0030] If the ship is sailing in a straight line, the electromagnetic log measurement data is first supplemented using non-integrity constraints and information from the strapdown inertial navigation system.

[0031]

[0032] in, The speed information is measured by the electromagnetic log; θ and γ are the pitch and roll angles calculated by the strapdown inertial navigation system, respectively.

[0033] The expanded electromagnetic log velocity vector Projecting onto geographic coordinate system n, we get

[0034]

[0035] in, This is the coordinate transformation matrix from the carrier coordinate system b to the geographic coordinate system n;

[0036] Measurement matrix H of the ship in straight sailing mode k and measurement vector Z k for:

[0037]

[0038] Where I is the identity matrix; These are the eastward, northward, and celestial velocities calculated by the strapdown inertial navigation system in the geographic coordinate system, respectively. for Projections to the east, north, and sky; h is the altitude calculated by the strapdown inertial navigation system;

[0039] If the ship is in a maneuvering state, the speed information measured by the electromagnetic log is used directly. Get

[0040]

[0041] Measurement matrix H of the ship in maneuver kand measurement vector Z k for:

[0042]

[0043] in, C 12 for The element in the 1st row and 2nd column, C 22 for The element in the second row and second column.

[0044] Furthermore, step 5 specifically includes:

[0045] The measurement update equation is:

[0046]

[0047] Among them, K k P is the gain matrix of the Kalman filter at time k; k / k-1 R is the mean square error matrix for one-step prediction at time k; k Let k be the measurement variance matrix at time k; This is a one-step prediction of the system state vector; P is the estimated value of the system state vector at time k; k Here is the mean square error matrix for the state estimation at time k; the measurement matrix H k and measurement vector Z k This is obtained in step 4.

[0048] A computer device / apparatus / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-described non-integrity constraint-assisted SINS / EML integrated navigation method.

[0049] A computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the aforementioned non-integrity constraint-assisted SINS / EML integrated navigation method.

[0050] A computer program product includes a computer program / instructions that, when executed by a processor, implement the steps of the aforementioned non-integrity constraint-assisted SINS / EML integrated navigation method.

[0051] The beneficial effects of this invention are as follows:

[0052] This invention designs a non-holonomic constraint-assisted SINS / EML integrated navigation method. In straight-line navigation, it fully utilizes the solution information from the strapdown inertial navigation system (SINS) and the non-holonomic constraint method to extract comprehensive information from the electromagnetic log (EML) measurement. The one-dimensional velocity in the bow and stern directions is expanded to three dimensions before integrated navigation filtering, improving the accuracy of the EML-assisted strapdown inertial navigation system. In maneuvering navigation, the measurement equations are reconstructed based on the EML measurement characteristics, mitigating the impact of ship sideslip on navigation accuracy. This invention requires no additional equipment, has significant engineering application value, and is applicable to the field of EML speed-assisted strapdown inertial navigation technology. Attached Figure Description

[0053] Figure 1 This is the overall flowchart of the present invention.

[0054] Figure 2 This is a flowchart illustrating the algorithm implementation of the present invention.

[0055] Figure 3 This is a comparison chart of the speed error of the integrated navigation system of the present invention and the speed error of the traditional method.

[0056] Figure 4 This is a comparison chart of the position error of the integrated navigation system of the present invention and the position error of the traditional method. Detailed Implementation

[0057] The present invention will now be further described with reference to the accompanying drawings.

[0058] The purpose of this invention is to overcome the problem that electromagnetic logs can only measure the one-dimensional velocity of a ship in the bow and stern directions. When the ship has a roll angle or is in a maneuvering state, its lateral and vertical velocities are not zero, which leads to a decrease in the accuracy of integrated navigation. Therefore, this invention provides a non-integrity constraint-assisted SINS / EML integrated navigation method to provide more accurate measurement information for strapdown inertial navigation systems and improve the accuracy of integrated navigation systems.

[0059] Combination Figure 1 , Figure 2 The specific embodiments of the present invention include the following steps:

[0060] Step 1: After the strapdown inertial navigation system has fully warmed up, perform initial alignment and complete the initialization of the Kalman filter, and then enter the navigation working mode;

[0061] The Kalman filter state-space model is as follows:

[0062]

[0063] In the formula, Φ k / k-1 W is the state transition matrix; k-1 Γ is the system noise vector; k / k-1Assign a noise matrix to the system; Z k For measurement; H k V is the measurement matrix; k For the measurement noise matrix; X k X k-1 These are the 17-dimensional state vectors of the integrated navigation system at time k and time k-1, respectively. Where φ E φ N φ U The platform misalignment angles are δv in the east, north, and sky directions, respectively. E δv N δv U The velocity errors are δλ, δL, and δh, representing the longitude, latitude, and altitude errors, respectively. E ε N ε U These represent the constant drift errors of the gyroscope in the east, north, and sky directions, respectively. E 、▽ N 、▽ U These represent the accelerometer zero bias errors in the east, north, and sky directions, respectively. The magnitudes of the eastward and northward ocean currents;

[0064] Step 2: Perform inertial navigation calculations, use the inertial navigation calculation information to calculate the lateral acceleration, and determine whether the ship is in a maneuvering state;

[0065] The method for calculating the lateral acceleration of a ship is as follows:

[0066]

[0067] In the formula, The acceleration of the carrier in the geographic coordinate system can be obtained from the specific force equation and the specific force measured by the accelerometer; φ represents the lateral acceleration of the carrier; φ is the heading angle calculated by the strapdown inertial navigation system.

[0068] In actual ship use, to avoid the influence of noise, a 10-second sliding window is set to calculate the average value of the ship's lateral acceleration within the sliding window. The specific calculation method is shown in the following formula:

[0069]

[0070] In the formula, λ represents the ship's lateral acceleration calculated using the i-th update data from the inertial navigation system within the sliding window; N is the size of the sliding window; and λ is the average lateral acceleration of the ship within the sliding window.

[0071] Finally, the data is compared with a preset threshold. If the value is greater than the threshold, the system is considered to be in a maneuvering state; if the value is less than the threshold, the system is considered to be in a straight-line state.

[0072] Step 3: Perform Kalman filtering time update based on the state transition matrix at time k, specifically as follows:

[0073]

[0074] In the formula, This is the optimal state estimate at time k-1; Φ is the one-step prediction value of the state quantity at time k; k / k-1 P is the state transition matrix at time k; k-1 Let P be the mean square error matrix for the state estimation at time k-1; k / k-1 Let Q be the mean square error matrix for the one-step prediction at time k; k-1 Γ is the system noise variance matrix; k / k-1 Assign a matrix to the system noise.

[0075] Step 4: If the electromagnetic speedometer updates v at time k eml Then, the measurement matrix H is calculated based on the ship's motion state. k Sum Measurement Z k The specific method is as follows:

[0076] If the ship is sailing in a straight line, the electromagnetic log measurement data is first supplemented using non-integrity constraints and inertial navigation solution information:

[0077]

[0078] In the formula, The speed information is measured by the electromagnetic log; θ and γ are the pitch and roll angles calculated by the strapdown inertial navigation system, respectively. The speed is the expanded electromagnetic log.

[0079] Project the expanded electromagnetic log speed onto the geographic coordinate system n:

[0080]

[0081] In the formula, These are the projections of the expanded electromagnetic log speed onto the carrier coordinate system b and the geographic coordinate system n, respectively. The coordinate transformation matrix from the b-frame to the n-frame is calculated for inertial navigation.

[0082] Then the measurement matrix H in the straight flight state k Sum Measurement Z k They are shown below:

[0083]

[0084] In the formula, I is the identity matrix; These are the eastward, northward, and upward velocities calculated by inertial navigation, respectively. h represents the projection of the attitude information calculated by the electromagnetic log using inertial navigation onto the east, north, and sky directions; h represents the altitude calculated by inertial navigation.

[0085] If the ship is in a maneuvering state, the projection of the electromagnetic log speed onto the geographic coordinate system n can be obtained by the following formula:

[0086]

[0087] In the formula, The projection of the electromagnetic log speed onto the geographic coordinate system n; The coordinate transformation matrix from the b-frame to the n-frame is calculated for inertial navigation.

[0088] The measurement matrix H under maneuvering conditions is reconstructed based on the measurement characteristics of the electromagnetic log. k Sum Measurement Z k They are shown below:

[0089]

[0090] In the formula, C ij (i, j = 1, 2, 3) represents the coordinate transformation matrix from system b to system n. The element in the i-th row and j-th column; These are the eastward, northward, and upward velocities calculated by inertial navigation, respectively. h represents the projection of the attitude information calculated by the electromagnetic log using inertial navigation in the east and north directions; h is the altitude calculated by inertial navigation.

[0091] Step 5: Use the measurement matrix H from step 4 k Sum Measurement Z k Perform measurement updates; the measurement update equation is:

[0092]

[0093] In the formula, K k P is the filter gain matrix at time k; k / k-1 Let R be the mean square error matrix for one-step prediction at time k; k The variance is measured at time k; One-step prediction of the state variable; Z is the state vector at time k; k For measurement; P k Let I be the mean square error matrix of the state estimation at time k; I is the identity matrix; H is the mean square error matrix of the state estimation at time k. k Let K be the Kalman filter measurement matrix at time k.

[0094] Step 6: Use the optimal estimate of the state variable at time k as the initial value of the state variable at the next time. Repeat steps 2-5 until the navigation operation is completed.

[0095] This completes the SINS / EML integrated navigation method assisted by non-integrity constraints.

[0096] To demonstrate the effectiveness of this invention, simulation experiments were conducted on the algorithm. The simulation conditions were set as follows: the constant drift of the three-axis gyroscope was set to 0.05° / h, and the gyroscope random walk was... The accelerometer zero bias is 2.5 × 10⁻⁶. -4 g, accelerometer random walk is The ship's motion relative to the water is as follows: first, it remains stationary for 100 seconds, then moves at a speed of 0.5 m / s in a direction 135° west of north. 2 After accelerating for 5 seconds, the vehicle travels at a constant speed of 2.5 m / s for 1000 seconds. Then, it turns 90° to the left with an angular velocity of 1° / s and travels at a constant speed for 2000 seconds. The eastward ocean current speed is set to 0.4 m / s, and the northward ocean current speed is set to -0.3 m / s. The simulation results show the velocity error comparison. Figure 3 As shown, the position error is compared to... Figure 4 As shown, it can be seen that when the SINS / EML integrated navigation method with non-integrity constraint assistance is performing a turning maneuver at 1000, the eastward velocity error is reduced from 0.53 m / s to 0.18 m / s, and the northward velocity error is reduced from 0.34 m / s to 0.19 m / s. Furthermore, the position error before the turning maneuver is slightly lower than that of the traditional integrated navigation method, and the position error after the turning maneuver is significantly lower than that of the traditional integrated navigation method.

[0097] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A non-integrity constraint-assisted SINS / EML integrated navigation method, characterized in that, Includes the following steps: Step 1: Initial alignment of the strapdown inertial navigation system and initialization of the Kalman filter; Step 2: Obtain the ship's acceleration in the geographic coordinate system using an accelerometer, calculate the ship's heading angle using a strapdown inertial navigation system, calculate the ship's lateral acceleration, and determine the ship's motion state based on the lateral acceleration; the motion state includes maneuvering state and straight-line state; Step 3: Perform Kalman filter time update based on the initial value of the system's state vector at the current moment; Step 4: If the electromagnetic log has a speed update at the current moment, calculate the system's measurement matrix and measurement vector at the current moment based on the ship's motion state; If the ship is sailing in a straight line, the electromagnetic log measurement data is supplemented using non-integrity constraints and information calculated by the strapdown inertial navigation system: in, Speed ​​information measured by an electromagnetic log; , These are the pitch and roll angles calculated by the strapdown inertial navigation system, respectively. The expanded electromagnetic log velocity vector Projected onto geographic coordinate system ,get ; in, For the carrier coordinate system To geographic coordinate system The coordinate transformation matrix; Measurement matrix of a ship in straight sailing mode and measurement vector for: in, It is the identity matrix; , , These are the eastward, northward, and celestial velocities calculated by the strapdown inertial navigation system in the geographic coordinate system, respectively. , , for Projections to the east, north, and sky; The altitude calculated by the strapdown inertial navigation system; If the ship is in a maneuvering state, the speed information measured by the electromagnetic log is used directly. Get : Measurement matrix of ships in maneuver and measurement vector for: in, ; for The element in the 1st row and 2nd column, for The element in the 2nd row and 2nd column; Step 5: Kalman filter measurement update, obtain the estimated state vector of the system at the current time; Step 6: Use the estimated value of the system's state vector at the current moment as the initial value of the system's state vector at the next moment, and repeat steps 2-5 until the navigation work is completed.

2. The SINS / EML integrated navigation method assisted by non-integrity constraints according to claim 1, characterized in that: The state-space model of the Kalman filter is as follows: in, This is the state transition matrix; This is the system noise vector at time k-1; Assign a matrix to the system noise; Let k be the measurement vector of the system at time k; For measurement matrix; For measuring the noise matrix; , These are the state vectors of the system at time k and k-1, respectively. in, These are the platform misalignment angles in the east, north, and sky directions, respectively. These represent the velocity errors in the east, north, and sky directions, respectively. These are the errors in longitude, latitude, and altitude, respectively. These represent the constant drift errors of the gyroscope in the east, north, and sky directions, respectively. These are the accelerometer zero bias errors in the east, north, and sky directions, respectively. These represent the speeds of the eastward and northward ocean currents, respectively.

3. The SINS / EML integrated navigation method assisted by non-integrity constraints according to claim 1, characterized in that: Step 2 specifically involves: lateral acceleration of a ship The calculation method is as follows: in, , The acceleration of the ship in the geographic coordinate system is obtained from the specific force equation and the specific force measured by the accelerometer; Calculate the ship's heading angle for the strapdown inertial navigation system; In actual shipboard use, to avoid the impact of noise, a sliding window is set up to calculate the average value of the ship's lateral acceleration within the sliding window. ; in, This indicates that the sliding window utilizes a strapdown inertial navigation system. The ship's lateral acceleration calculated from the next updated data; To adjust the sliding window size; use Compared with a preset threshold, if If the value is greater than the threshold, the vessel is determined to be in a maneuvering state; otherwise, the vessel is determined to be in a straight-ahead state.

4. The SINS / EML integrated navigation method assisted by non-integrity constraints as described in claim 1, characterized in that: Step 3 specifically involves: in, This is the initial value of the system's state vector at the current moment, which is the estimated value of the system's state vector obtained in step 5 at the previous moment; for One-step prediction of the state quantity at time step; for The state transition matrix at each time step; for The mean square error matrix of the state estimation at each time step; for The one-step prediction mean square error matrix at time t; Here is the system noise variance matrix; Assign a matrix to the system noise.

5. The SINS / EML integrated navigation method assisted by non-integrity constraints according to claim 1, characterized in that: Step 5 specifically involves: The measurement update equation is: in, for The gain matrix of the Kalman filter at time step; for The mean square error matrix for one-step prediction at any given time; for Time-measurement variance matrix; This is a one-step prediction of the system state vector; This is the estimated value of the system state vector at time k; for Mean square error matrix for time-state estimation; measurement matrix and measurement vector This is obtained in step 4.

6. A computer device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 5.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the steps of the method according to any one of claims 1 to 5.

8. A computer program product comprising computer instructions, characterized in that: When executed by a processor, the computer instructions implement the steps of the method according to any one of claims 1 to 5.

Images