A SINS / EML integrated navigation method based on measurement characteristics, a program, a device and a storage medium

Abstract

The present application belongs to the technical field of integrated navigation, and particularly relates to a SINS / EML integrated navigation method, program, device and storage medium based on measurement characteristics. The present application designs a new Kalman filtering measurement update equation according to the measurement characteristics of the electromagnetic log, fully considers the characteristics that the electromagnetic log can only measure the ship's head-tail direction velocity, and expands the ocean current velocity to the state quantity. In the case of considering the platform misalignment angle, the attitude angle calculated by the strapdown inertial navigation system is firstly used to project the inertial calculation velocity in the geographic coordinate system to the carrier coordinate system to obtain the carrier head-tail direction velocity, then the Kalman filtering measurement value is obtained by subtracting the electromagnetic log velocity, and finally the Kalman filtering measurement update is performed and the strapdown inertial navigation system is feedback corrected. The present application reasonably utilizes the measurement information, reduces the influence of the ship's side slip and ocean current on the electromagnetic log velocity measurement, and improves the navigation precision of the electromagnetic log auxiliary strapdown inertial navigation system.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of integrated navigation, and particularly relates to a SINS / EML integrated navigation method, program, device and storage medium based on measurement characteristics. BACKGROUND

[0002] In recent years, with the continuous progress and development of ship speed measurement technology, various types of speed measurement equipment have emerged, such as differential pressure log, electromagnetic log (EML) and Doppler log. The Doppler log in the above speed measurement equipment can measure the ship's speed against the ground, but when in deep sea area, it can only measure the speed against the water. At this time, a high-power low-frequency signal can be used to measure the absolute speed, but the acoustic exposure problem caused thereby cannot be ignored. The electromagnetic log is a kind of log that measures the speed of the ship according to the principle of electromagnetic induction, which can measure the forward and reverse speed of the ship, and has the characteristics of high sensitivity, low cost, high speed measurement accuracy, good linearity, large range and good concealment, and does not cause acoustic exposure. When using the speed measurement information of the electromagnetic log, it is generally considered that the lateral speed and the skyward speed of the carrier are zero, and then the speed measurement information is projected into the geographic coordinate system directly by using the attitude angle solved by the strapdown inertial navigation system. However, the ship will have a certain degree of lateral slip during driving, especially when turning, which will introduce a large speed error into the system. Therefore, how to accurately assist the strapdown inertial navigation system with the electromagnetic log is more challenging than other logs. In order to solve the above problems and improve the precision of the strapdown inertial / electromagnetic log integrated navigation system, many patents have also studied the speed measurement problem of the electromagnetic log.

[0003] For example, patent document CN112649619A published on April 13, 2021 discloses a speed error suppression method and system for a multi-channel electromagnetic log. This patent document uses multiple ship speed measurement data on the ship to perform weighted fusion, improves the speed measurement dispersion of the electromagnetic log, and reduces the speed measurement mean square error by an order of magnitude.

[0004] For example, patent document CN115752453A published on March 7, 2023 discloses an electromagnetic log current estimation and integrated navigation method and system based on HMM. This patent document estimates the current speed based on the global satellite navigation system and the electromagnetic log speed against the water, and compensates for the electromagnetic log error, thereby eliminating the influence of the current on the electromagnetic log measurement error. The above patents do not consider the influence of ship side slip on the speed measurement of the electromagnetic log. SUMMARY

[0005] The application aims to overcome the problem that the velocity measurement error of the electromagnetic log affected by the ocean current will introduce a new error source when the electromagnetic log is used to assist the SINS, and provides a SINS / EML combined navigation method based on measurement characteristics, so as to provide more accurate measurement information for the SINS and improve the accuracy of the combined navigation system.

[0006] A SINS / EML combined navigation method based on measurement characteristics, comprising the following steps:

[0007] Step 1: initial alignment of the SINS;

[0008] Step 2: establishing a Kalman filter state space model according to the error equation of the SINS, and initializing the Kalman filter;

[0009] Step 3: performing Kalman filter time update according to the initial value of the state vector of the system at the current time;

[0010] Step 4: if there is a velocity update of the electromagnetic log at the current time, calculating the ship's head-tail direction velocity by using the solution value of the SINS at the current time;

[0011] Step 5: obtaining the measurement vector of the system at the current time according to the ship's head-tail direction velocity and the measurement velocity of the electromagnetic log;

[0012] Step 6: Kalman filter measurement update based on measurement characteristics, obtaining the estimated value of the state vector of the system at the current time;

[0013] Step 7: taking the estimated value of the state vector of the system at the current time as the initial value of the state vector of the system at the next time, and repeating steps 2-7 until the navigation work is completed.

[0014] Further, the step 2 is specifically:

[0015] The Kalman filter state space model is:

[0016]

[0017] wherein, Φ k / k-1 is a state transition matrix; W k-1 is a system noise vector at k-1 time; Γ k / k-1 is a system noise distribution matrix; Z k is a measurement vector of the system at k time; H k is a measurement matrix; V k is a measurement noise matrix; X k , X k-1 are state vectors of the system at k time and k-1 time, respectively.

[0018]

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

[0020] Furthermore, step 3 specifically includes:

[0021]

[0022] 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 6 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.

[0023] Furthermore, step 4 specifically includes:

[0024] If the electromagnetic logger updates its speed at time k, then v log The bow and stern velocities of the ship can then be calculated using the values ​​obtained from the strapdown inertial navigation system at time k.

[0025]

[0026] Where θ and φ are the pitch angle and heading angle calculated by the strapdown inertial navigation system, respectively; δθ, δγ, and δφ are the eastward, northward, and celestial velocities calculated by the strapdown inertial navigation system, respectively; δθ, δγ, and δφ are the platform misalignment angles calculated by the strapdown inertial navigation system, respectively.

[0027] Further, step 5 specifically involves: using the electromagnetic speed v obtained in step 4... log and the speed of the ship in the bow and stern directions Obtain the measurement vector of the system at time k

[0028] Further, the step 6 is specifically:

[0029] The measurement update equation is:

[0030]

[0031] Wherein, K k is the gain matrix of the Kalman filter at time k; P k / k-1 is the one-step prediction mean square error matrix at time k; R k is the measurement variance matrix at time k; is the one-step prediction value of the system state vector; is the estimated value of the system state vector at time k; P k is the state estimation mean square error matrix at time k; I is the unit matrix; H k is the Kalman filter measurement matrix at time k;

[0032]

[0033] Wherein, 0 i×j is the zero element matrix of i rows and j columns; H i,j is the element of the i-th row and j-th column of H k , and has:

[0034]

[0035] H 1,4 = -cos(θ)sin(φ)

[0036] H 1,5 = cos(θ)cos(φ) H 1,6 = sin(θ)

[0037] H 1,16 = -cos(θ)sin(φ) H 1,17 = cos(θ)cos(φ)

[0038] Wherein, θ and φ are respectively the pitch angle and the heading angle calculated by the strapdown inertial navigation system; are respectively the eastward velocity, the northward velocity and the skyward velocity calculated by the strapdown inertial navigation system.

[0039] A computer device / apparatus / system, comprising a memory, a processor and a computer program stored on the memory, the processor executing the computer program to implement the steps of the above-mentioned SINS / EML combined navigation method based on measurement characteristics.

[0040] A computer readable storage medium, having stored thereon computer programs / instructions, which, when executed by a processor, implement the steps of the above-mentioned SINS / EML integrated navigation method based on measurement characteristics.

[0041] A computer program product, comprising computer programs / instructions, which, when executed by a processor, implement the steps of the above-mentioned SINS / EML integrated navigation method based on measurement characteristics.

[0042] The present application has the following advantages:

[0043] The present application designs a new Kalman filtering measurement update equation according to the measurement characteristics of the electromagnetic log, fully considers the characteristics that the electromagnetic log can only measure the head-tail direction velocity of the ship, and expands the ocean current velocity to the state quantity, in the case of considering the misalignment angle of the platform, first projects the inertial calculation velocity in the geographic coordinate system to the carrier coordinate system by using the attitude angle calculated by the SINS, obtains the carrier head-tail direction velocity, then obtains the Kalman filtering measurement value by subtracting the electromagnetic log velocity, and finally performs Kalman filtering measurement update and feedback correction to the SINS. The present application reasonably utilizes the measurement information, reduces the influence of the ship side slip and the ocean current on the electromagnetic log velocity measurement, and improves the navigation precision of the electromagnetic log assisted SINS. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 The present application is a general flowchart.

[0045] Figure 2 The present application is a general flowchart.

[0046] Figure 3 The present application is a general flowchart. DETAILED DESCRIPTION

[0047] The present application is a general flowchart.

[0048] The present application considers the influence of the ship side slip on the electromagnetic log velocity measurement, designs a new Kalman filtering measurement update equation according to the measurement characteristics of the electromagnetic log, reasonably utilizes the electromagnetic log velocity measurement information, fully considers the influence of the ocean current on the electromagnetic log velocity measurement, expands the ocean current velocity to the state quantity, reduces the influence of the ship side slip and the ocean current on the electromagnetic log measurement error, and improves the precision of the electromagnetic log assisted SINS.

[0049] In combination with Figure 1 , Figure 2 , the specific embodiment of the present application includes the following steps:

[0050] Step 1: After the strapdown inertial navigation system is fully preheated, initial alignment is performed, and then the navigation working mode is entered;

[0051] Step 2: A Kalman filter state space model is established according to an error equation of the strapdown inertial navigation system, and initialization of the Kalman filter is completed;

[0052] The Kalman filter state space model is:

[0053]

[0054] wherein, Φ k / k-1 is a state transition matrix; W k-1 is a system noise vector at k-1; Γ k / k-1 is a system noise distribution matrix; Z k is a measurement vector of the system at k; H k is a measurement matrix; V k is a measurement noise matrix; X k and X k-1 are state vectors of the system at k and k-1 respectively;

[0055]

[0056] wherein, φ E , φ N , and φ U are misalignment angles of the platform in east, north, and sky directions respectively; δv E , δv N , and δv U are velocity errors in east, north, and sky directions respectively; δλ, δL, and δh are longitude, latitude, and height errors respectively; ε E , ε N , and ε U are constant drift errors of gyroscopes in east, north, and sky directions respectively; are accelerometer zero offset errors in east, north, and sky directions respectively; are eastward and northward ocean current velocities.

[0057] Step 3: Kalman filter time updating is performed according to an initial value of a state vector of the system at a current k;

[0058]

[0059] wherein, is an initial value of the state vector of the system at the current k, that is, an estimated value of the state vector of the system at the last time obtained through step 6; is a one-step prediction value of the state at k; Φ k / k-1 is a state transition matrix at k; P k-1is the state estimation mean square error matrix at k-1 time; P k / k-1 is the one-step prediction mean square error matrix at k time; Q k-1 is the system noise variance matrix; Γ k / k-1 is the system noise distribution matrix.

[0060] Step 4: If the electromagnetic log has a speed update v log at the current k time, then the ship's head-tail direction speed v

[0061]

[0062] where θ and φ are the pitch angle and heading angle calculated by the strapdown inertial navigation system respectively; are the eastward speed, northward speed and skyward speed calculated by the strapdown inertial navigation system respectively; δθ, δγ and δφ are the platform misalignment angles calculated by the strapdown inertial navigation system respectively.

[0063] Step 5: Using the electromagnetic log speed v log and the ship's head-tail direction speed v obtained in Step 4, the measurement vector of the system at k time is obtained.

[0064] Step 6: Based on the measurement characteristics of Kalman filtering, the state vector estimation value of the system at the current time is obtained.

[0065] The measurement update equation is:

[0066]

[0067] where K k is the gain matrix of the Kalman filter at k time; P k / k-1 is the one-step prediction mean square error matrix at k time; R k is the measurement variance matrix at k time; is the one-step prediction value of the system state vector; is the estimation value of the system state vector at k time; P k is the state estimation mean square error matrix at k time; I is the unit matrix; H k is the Kalman filtering measurement matrix at k time;

[0068]

[0069] where 0 i×j is the zero element matrix of i rows and j columns; H i,j is the element of the i-th row and j-th column of H k , and has:

[0070]

[0071] H 1,4 = -cos (θ) sin (φ)

[0072] H 1,5 = cos (θ) cos (φ) 1,6 = sin (θ)

[0073] H 1,16 = -cos (θ) sin (φ) 1,17 = cos (θ) cos (φ)

[0074] Wherein, θ, φ are respectively pitch angle and heading angle solved by strapdown inertial navigation system; Vx, Vy, Vz are respectively eastward velocity, northward velocity and skyward velocity solved by strapdown inertial navigation system.

[0075] Step 7: taking the estimated value of the state vector of the system at the current time as the initial value of the state vector of the system at the next time, repeating steps 2-7 until the navigation work is finished.

[0076] The application designs a new Kalman filtering measurement update equation according to the measurement characteristics of electromagnetic log, fully considers the characteristics that electromagnetic log can only measure the velocity in the bow-tail direction of the ship, and expands the ocean current velocity to the state quantity, in the case of considering platform misalignment angle, first, the inertial calculation velocity in the geographic coordinate system is projected to the carrier coordinate system by using the attitude angle solved by strapdown inertial navigation system, the carrier bow-tail direction velocity is obtained, then the difference between the electromagnetic log velocity and the carrier bow-tail direction velocity is obtained to obtain the Kalman filtering measurement value, finally, the Kalman filtering measurement update is carried out and the strapdown inertial navigation system is feedback corrected. The application reasonably utilizes the measurement information, reduces the influence of ship side slip and ocean current on the electromagnetic log velocity measurement, and improves the navigation precision of the electromagnetic log auxiliary strapdown inertial navigation system. The application is suitable for the technical field of electromagnetic log velocity auxiliary strapdown inertial navigation.

[0077] In order to illustrate the effectiveness of the application, the algorithm is simulated. The simulation conditions are set as follows: the constant drift of three-axis gyroscope is 0.03 ° / h, the gyroscope random walk is 0.001 ° / sqrt (h), the accelerometer zero offset is 1x10 -4 g, the accelerometer random walk is 5x10 -6 g / sqrt (hz), the ship motion state is: first, the ship is accelerated to travel in the direction of 45 ° east of north at an acceleration of 0.5 m / s 2 for 5 s, then the ship is turned left by 90 ° at a speed of 1 ° / s, then the ship travels at a speed of 2.5 m / s for 1000 s, and finally the ship travels at a speed of-0.5 m / s 2The acceleration of the ship will reduce the speed to 0 and be stationary for 100 s, and the eastward current velocity is set to 0.4 m / s and the northward current velocity is set to 0.3 m / s. The velocity error of the simulation results is shown in FIG. 8, and it can be seen that the velocity error of the measurement characteristic-based measurement updating method is lower, and this is particularly evident when the ship is turning. Figure 3

[0078] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. The present application can be variously changed and altered by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.​

Claims

1. A SINS / EML integrated navigation method based on measured characteristics, characterized in that, The method comprises the following steps: Step 1: initial alignment of a strapdown inertial navigation system; Step 2: establishing a Kalman filter state space model according to an error equation of the strapdown inertial navigation system, and initializing the Kalman filter; Step 3: performing Kalman filter time updating according to an initial value of a state vector of the system at a current time; Step 4: if there is a speed update of the electromagnetic log at the current time, calculating a ship head-tail direction speed by using a solution value of the strapdown inertial navigation system at the current time; If the electromagnetic log has a speed update at time k , then the ship's head-tail direction speed is calculated using the value of the strapdown inertial navigation system at time k : ； wherein, , are respectively the pitch angle and the heading angle solved by the strapdown inertial navigation system; , , are respectively the eastward velocity, the northward velocity, the skyward velocity solved by the strapdown inertial navigation system; , , are respectively the platform misalignment angles solved by the strapdown inertial navigation system; Step 5: According to the ship's head-tail direction speed and the electromagnetic log measured speed Get the measurement vector of the system at the current k moment ; Step 6: performing Kalman filter measurement updating based on a measurement characteristic to obtain an estimated value of the state vector of the system at the current time; ； wherein, is the gain matrix of the Kalman filter at time k; is the gain matrix of the Kalman filter at time k; is the one-step prediction error matrix at time k; is the one-step prediction error matrix at time k; is the measurement variance matrix at time k; is the measurement variance matrix at time k; is the one-step prediction of the system state vector; is the estimate of the system state vector at time k; is the one-step prediction of the system state vector; is the one-step prediction error matrix at time k; is the identity matrix; is the measurement matrix of the Kalman filter at time k; ； wherein is row zero element matrix; is the element in the row column, and has: ； Step 7: taking the estimated value of the state vector of the system at the current time as an initial value of a state vector of the system at a next time, and repeating steps 2-7 until a navigation work is finished.

2. The SINS / EML integrated navigation method based on measurement characteristics according to claim 1, characterized in that: The step 2 is specifically: The Kalman filter state space model is: ； wherein, is a state transition matrix; is a system noise vector at time k-1; is a system noise allocation matrix; is a measurement vector of the system at time k; is a measurement matrix; is a measurement noise matrix; , are state vectors of the system at time k and k-1, respectively; ； where, are the platform misalignment angles in east, north, and sky directions, respectively; are the velocity errors in east, north, and sky directions, respectively; are the longitude, latitude, and altitude errors, respectively; are the gyro constant drift errors in east, north, and sky directions, respectively; are the accelerometer bias errors in east, north, and sky directions, respectively; are the eastward and northward ocean current velocities, respectively.

3. The SINS / EML integrated navigation method based on measurement characteristics according to claim 1, characterized in that: The step 3 is specifically: ； wherein, is the initial value of the state vector of the system at the current time, i.e. the estimated value of the state vector of the system at the previous time obtained by step 6; is the state vector of the system at the current time; is the one-step prediction value of the state vector at the current time; is the state transition matrix at the current time; is the state vector of the system at the current time; is the state transition matrix at the current time; is the state estimation error matrix at the current time; is the one-step prediction error matrix at the current time; is the one-step prediction error matrix at the current time; is the system noise variance matrix; is the system noise distribution matrix.

4. A computer device comprising a memory, a processor, and a computer program stored on the memory, wherein: The processor executes the computer program to realize the steps of the method in any one of claims 1-3.

5. A computer readable storage medium having stored thereon computer programs / instructions, characterized in that: The computer program / instruction is executed by the processor to realize the steps of the method in any one of claims 1-3.

6. A computer program product comprising computer programs / instructions, characterized in that: The computer program / instruction is executed by the processor to realize the steps of the method in any one of claims 1-3.

Images