HMM-Based Electromagnetic Log Ocean Current Estimation and Integrated Navigation Method and System

Abstract

The present invention relates to the field of inertial navigation technology, and particularly to an electromagnetic log ocean current estimation and integrated navigation method and system based on HMM, including: the forward speed information output by the inertial navigation system and the speed relative to water output by the electromagnetic log are fused through an implicit Markov filter to obtain the speed relative to water fusion after noise filtering, and secondary filtering parameters are obtained through a two-stage Kalman filter; according to the secondary speed relative to the ground and the speed relative to water fusion, the ocean current speed is obtained, and the ocean current speed is fed back to the implicit Markov model to use the ocean current correction speed to compensate the speed relative to water fusion to obtain the speed relative to water compensation; feedback correction is performed using the secondary filtering parameters and the speed relative to water compensation to obtain integrated navigation parameter information. The present invention completes ocean current estimation and integrated navigation through a two-stage Kalman filter and a feedback correction algorithm, improves the navigation accuracy, and has the advantages of small computational amount, strong practicability, good robustness, etc., providing a reference value for the research of integrated navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation, and particularly to an electromagnetic log ocean current estimation and integrated navigation method and system based on HMM. Background Art

[0002] Ocean exploration is inseparable from equipment such as ships, unmanned boats, submersible manned vehicles, autonomous underwater vehicles (AUVs), etc. Especially for underwater operations, AUVs are the best choice. In the civilian aspect, AUVs can complete tasks such as ocean environmental investigation, oil drilling platform support, inspection and search of wrecked ships, etc.; in the military aspect, AUVs can complete special tasks such as underwater reconnaissance and anti-reconnaissance, mine sweeping in seabed areas, and tactical oceanography investigations. The characteristics and advantages of AUVs make them the main development direction in the current field of ocean development and research. Of course, navigation and control are the key technologies for developing these equipment such as AUVs and ships. High-precision control and navigation can achieve better results. On the contrary, low-precision navigation may lead to low operation efficiency or even failure.

[0003] Currently, most AUVs use an integrated navigation method of an optical inertial navigation system (INS) / underwater speed measurement equipment / global navigation satellite system (GNSS). Among them, inertial navigation (abbreviated as INS) is a kind of dead reckoning method and is a cutting-edge technology that integrates disciplines such as mechatronics, optics, mathematics, mechanics, control, and computer. Optical INS refers to using optical gyroscopes such as lasers or optical fibers as the gyroscopes. The most important indicators in INS are the zero biases of the optical gyroscope and the accelerometer. The zero bias ranges of the gyroscopes and accelerometers used in large and medium-sized AUVs are 0.005 - 0.02° / h and 10 - 100 μg respectively; in addition, underwater speed measurement equipment includes Doppler log, acoustic correlation log, electromagnetic log, etc. The advantages of Doppler log and acoustic correlation log are: high speed measurement accuracy, and they can give the ground speeds in the right, front, and up directions of the carrier (when the distance from the water bottom is less than the maximum sounding range of the equipment), etc. Their disadvantages are: high price (the selling price is 200,000 - 300,000 for a speed measurement accuracy higher than 0.4% and a sounding distance greater than 30 meters, and the selling price is 800,000 - 1,000,000 for a speed measurement accuracy higher than 0.4% and a sounding distance greater than 200 meters), large volume, heavy weight, complex installation, and the presence of outliers in measurements, etc.

[0004] The electromagnetic log (EML) is a device that measures the forward speed of a vehicle relative to water using the Hall effect. It has the advantages of low cost, small size, light weight, and easy installation. Currently, there have been many studies on EML error analysis and applications, as well as some algorithms for removing measurement outliers from devices such as EML. There have also been many studies on integrated navigation algorithms. Some scholars have proposed non-linear integrated navigation algorithms, various latest integrated navigation algorithms for vehicles such as AUVs, ocean current estimation and compensation algorithms based on high-gain observers, a fast and high-precision alignment algorithm suitable for large misalignment angles, and integrated navigation algorithms suitable for underwater gliders, etc. However, some of the existing studies can only remove the outliers of the EML and cannot suppress the measurement noise of normal (measurements other than outliers) speed values. Some integrated navigation algorithms have excessive computational complexity, poor engineering practicability, and there is the drawback that the EML can only measure the convective speed and cannot measure the ground speed.

[0005] In summary, the traditional INS / EML / GNSS integrated navigation method has poor accuracy, cannot suppress the measurement noise of normal speed values, and is complex to operate. Summary of the Invention

[0006] The present invention provides a method and system for electromagnetic log ocean current estimation and integrated navigation based on HMM, aiming to solve the technical problems that the traditional INS / EML / GNSS integrated navigation method has poor accuracy, cannot suppress the measurement noise of normal speed values, and is complex to operate.

[0007] To solve the above technical problems, the present invention provides a method and system for electromagnetic log ocean current estimation and integrated navigation based on HMM.

[0008] In a first aspect, the present invention provides a method for electromagnetic log ocean current estimation and integrated navigation based on HMM, the method comprising the following steps:

[0009] Receive the forward speed information output by the inertial navigation system and the speed relative to water output by the electromagnetic log, and fuse the forward speed information and the speed relative to water through an implicit Markov filter to obtain the fused speed relative to water after noise filtering;

[0010] Input the fused speed relative to water into a first-level Kalman filter to obtain first-level filtering parameters; the first-level filtering parameters include the first-level ground speed and first-level navigation information;

[0011] Receive the position information of the global satellite navigation system, and input the position information and the first-level filtering parameters into a second-level Kalman filter to obtain second-level filtering parameters; the second-level filtering parameters include the second-level ground speed and second-level latitude;

[0012] Based on the secondary ground speed and the water fusion speed, the ocean current speed is obtained, and the ocean current speed is fed back to the implicit Markov model to correct the water fusion speed using the ocean current correction speed and obtain the water compensation speed;

[0013] Perform feedback correction using the secondary filtering parameters and the water compensation speed to obtain the integrated navigation parameter information.

[0014] In a further embodiment, the first-order Kalman filter and the second-order Kalman filter models are both:

[0015]

[0016] In the formula, X k represents the n-dimensional state vector of the integrated navigation system at time k, where φ E , φ N , φ U represent the attitude errors in the east, north, and up directions respectively, δv E , δv N , δv U represent the velocity errors in the east, north, and up directions respectively, δλ, δL, and δh represent the errors in longitude, latitude, and altitude respectively, ε E , ε N , ε U represent the gyro constant drift errors in the east, north, and up directions respectively, represent the accelerometer bias errors in the east, north, and up directions respectively; Φ k|k-1 represents the state transition matrix from time (k - 1) to time k; Γ k|k-1 represents the noise distribution matrix from time (k - 1) to time k; W k-1 represents the system noise vector; Z k represents the observation vector at time k; H k represents the observation matrix at time k; V k represents the observation noise vector at time k.

[0017] In a further embodiment, in different navigation modes, the observation vector and the observation matrix are respectively:

[0018] When the position information of the global satellite navigation system and the water speed of the electromagnetic log are received simultaneously, the inertial navigation is in the integrated navigation mode based on the inertial navigation system, the global satellite navigation system, and the electromagnetic log, and the observation vector and the observation matrix are respectively:

[0019]

[0020]

[0021] wherein, v′ HMM-i (i = E, N, U) and v INS-i (i = E, N, U) are respectively three velocity components in the n - frame output by the HMM after ocean current compensation and three velocity components in the n - frame calculated by the INS; v′ HMM-i is obtained by multiplying the vector [0 v′ HMM-y 0] T by the attitude matrix , where v′ HMM-y is obtained by adding v HMM-y to the estimated ocean current velocity v C , where v HMM-y represents the velocity component of the y - axis output by the HMM; λ GNSS , L GNSS , h DG are respectively the longitude output by the GNSS, the latitude output by the GNSS, and the depth output by the depth gauge; λ INS , L INS , h INS are the longitude, latitude, and depth output by the INS.

[0022] When only the water - relative velocity of the electromagnetic log is received, the inertial navigation is in a combined navigation mode based on the inertial navigation system and the electromagnetic log, and the observation vector and the observation matrix are respectively:

[0023] Obtain the observed quantity after GNSS correction:

[0024]

[0025] Obtain the observed quantity before the first GNSS correction or when GNSS correction cannot be obtained for a long time:

[0026]

[0027]

[0028] When there is no external measurement information, the inertial navigation is in a pure inertial navigation mode, and the first - order Kalman filter and the second - order Kalman filter only perform time updates.

[0029] In a further embodiment, the expression of the implicit Markov filter is:

[0030]

[0031] wherein, v EML represents the forward velocity output by the electromagnetic log; v INS-y represents the forward velocity calculated by the inertial navigation system; v HMM-y represents the forward velocity output by the implicit Markov filter; represents the forward speed of the vehicle relative to the earth; v C represents the ocean current speed in the same direction as the vehicle's forward movement; δV INS represents the measurement noise of the inertial navigation system; δV EML represents the measurement noise of the electromagnetic log.

[0032] In a further embodiment, the step of using the secondary filtering parameters and performing feedback correction on the water compensation speed to obtain the combined navigation parameter information includes:

[0033] Respectively feedback the secondary ground speed to the specific force model, the first-level Kalman filter, and the second-level Kalman filter model to obtain the geometric motion acceleration of the vehicle in the navigation system;

[0034] According to the geometric motion acceleration of the vehicle in the navigation system, obtain the pure inertial solution results of position and speed;

[0035] Solve the secondary latitude to obtain the calculation error of the earth's angular velocity of rotation and the calculation error of the rotation angular velocity of the navigation system relative to the earth system;

[0036] Respectively feedback the calculation error of the earth's angular velocity of rotation and the calculation error of the rotation angular velocity of the navigation system relative to the earth system to the first-level Kalman filter, the second-level Kalman filter model, and the transformed attitude differential model, and obtain the pure inertial solution result of the attitude angle according to the water compensation speed;

[0037] After the first-level Kalman filter and the second-level Kalman filter model are stable, output the combined navigation parameter information according to the pure inertial solution results of position, speed, and attitude angle.

[0038] In a further embodiment, the specific force model is:

[0039]

[0040] In the formula, represents the geometric motion acceleration of the vehicle in the navigation system; represents the attitude matrix of the vehicle system relative to the navigation coordinate system; represents the specific force measured by the accelerometer; represents the Coriolis acceleration caused by the vehicle's movement and the earth's rotation; represents the centripetal acceleration relative to the earth caused by the vehicle's movement; g n represents the gravitational acceleration; collectively referred to as harmful accelerations;

[0041] The transformed attitude differential model is:

[0042]

[0043] Among them, represents the differential matrix of the attitude after transformation; represents that the output of the gyroscope is the angular velocity of the b system relative to the inertial system; represents the rotation of the n system relative to the i system, which includes two parts: the rotation of the navigation system caused by the earth's rotation, and the rotation of the n system caused by the curvature of the earth's surface when the inertial navigation system moves near the earth's surface, that is, there is Among them:

[0044]

[0045]

[0046] In the formula, ω ie is the angular velocity of the earth's rotation; represents the rotation of the n system caused by the curvature of the earth's surface when the inertial navigation system moves near the earth's surface; L and h are the geographical latitude and altitude respectively.

[0047] In a further embodiment, the calculation error of the earth's angular velocity of rotation and the calculation error of the angular velocity of the rotation of the navigation system relative to the earth system are respectively:

[0048]

[0049]

[0050] In the formula, represents the calculation error of the earth's angular velocity of rotation; ω ie represents the earth's angular velocity of rotation; δL represents the latitude error; L represents the latitude; represents the calculation error of the angular velocity of the rotation of the navigation system relative to the earth system; V′ HMM-N represents the northward velocity component of the HMM output after ocean current compensation; R Mh represents the earth's meridian radius containing altitude information; V INS-N represents the pure inertial northward velocity calculated by the INS; V′ HMM-E represents the eastward velocity component of the HMM output after ocean current compensation; V INS-E represents the pure inertial eastward velocity calculated by the INS; R Nh represents the radius of the prime vertical containing altitude information.

[0051] In a second aspect, the present invention provides an electromagnetic log ocean current estimation and integrated navigation system based on HMM, and the system includes:

[0052] The HMM fusion module is used to receive the forward speed information output by the inertial navigation system and the speed relative to water output by the electromagnetic log, and fuse the forward speed information and the speed relative to water through an implicit Markov filter to obtain the speed relative to water fusion speed after noise filtering;

[0053] The first-level filtering module is used to input the speed relative to water fusion speed into a first-level Kalman filter to obtain first-level filtering parameters; the first-level filtering parameters include the first-level speed relative to the ground and the first-level navigation information;

[0054] The second-level filtering module is used to receive the position information of the global satellite navigation system, and input the position information and the first-level filtering parameters into a second-level Kalman filter to obtain second-level filtering parameters; the second-level filtering parameters include the second-level speed relative to the ground and the second-level latitude;

[0055] The ocean current compensation module is used to obtain the ocean current speed according to the second-level speed relative to the ground and the speed relative to water fusion speed, and feedback the ocean current speed to the implicit Markov model to use the ocean current correction speed to compensate the speed relative to water fusion speed to obtain the speed relative to water compensation speed;

[0056] The feedback correction module is used to perform feedback correction by using the second-level filtering parameters and the speed relative to water compensation speed to obtain the combined navigation parameter information.

[0057] Meanwhile, on the third aspect, the present invention also provides a computer device, including a processor and a memory, the processor is connected to the memory, the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory so that the computer device executes the steps of implementing the above method.

[0058] On the fourth aspect, the present invention also provides a computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the steps of implementing the above method are realized.

[0059] The present invention provides a method and system for electromagnetic log ocean current estimation and integrated navigation based on HMM. The method uses the HMM algorithm to fuse the speed obtained by pure inertial solution and the speed measured by the electromagnetic log to obtain the speed of convection with suppressed noise. Using this speed for linear Kalman indirect filtering integrated navigation can effectively improve the integrated navigation accuracy and can also cope with time-varying ocean currents. Compared with the prior art, this method has good effects on wild value rejection of EML, improvement of measurement accuracy, and suppression of integrated navigation positioning error, and this method has the advantages of small calculation amount, strong engineering practicability, and good robustness. Description of the Drawings

[0060] Figure 1It is a schematic flow chart of an electromagnetic log ocean current estimation and integrated navigation method based on HMM provided by an embodiment of the present invention;

[0061] Figure 2 It is a schematic diagram of the principle of an ocean current estimation and integrated navigation algorithm based on HMM provided by an embodiment of the present invention;

[0062] Figure 3 It is a flow chart of an ocean current estimation and integrated navigation algorithm based on HMM provided by an embodiment of the present invention;

[0063] Figure 4 It is a block diagram of an electromagnetic log ocean current estimation and integrated navigation system based on HMM provided by an embodiment of the present invention;

[0064] Figure 5 It is a schematic structural diagram of a computer device provided by an embodiment of the present invention. Detailed implementation manners

[0065] The following specifically clarifies the implementation manners of the present invention in conjunction with the accompanying drawings. The given embodiments are only for illustrative purposes and should not be construed as a limitation of the present invention. The accompanying drawings are only for reference and illustration and do not constitute a limitation on the scope of patent protection of the present invention, because many changes can be made to the present invention without departing from the spirit and scope of the present invention.

[0066] Reference Figure 1 An embodiment of the present invention provides an electromagnetic log ocean current estimation and integrated navigation method based on HMM. As Figure 1 shown, the method includes the following steps:

[0067] S1. Receive the forward speed information output by the inertial navigation system and the speed through water output by the electromagnetic log, and fuse the forward speed information and the speed through water through an implicit Markov filter to obtain the speed through water fusion speed after noise filtering.

[0068] Specifically, as Figure 2 shown, the integrated navigation method adopted in this embodiment is based on INS / EML / GNSS. Among them, INS, EML, HMM, GNSS, and DG respectively represent an inertial navigation system, an electromagnetic log, an implicit Markov filter, a global navigation satellite system, and a depth gauge (DG). In this embodiment, the inertial navigation system INS is used to collect the forward speed information v y-INS , the electromagnetic log EML is used to collect the speed through water v y-EML , and the working principle of the electromagnetic log EML is:

[0069]

[0070] In the formula, ε represents the induced electromotive force; ΦB represents magnetic flux; B represents magnetic field strength; l represents the height of the magnetic field cross-section; represents the carrier's forward speed, where b represents the carrier coordinate system, using a right-front-up coordinate system, and the y-component represents the forward direction; v C represents the ocean current speed in the same direction as the carrier's forward movement.

[0071] It should be noted that if there is no ocean current, in this embodiment, the induced electromotive force generated by the EML can be divided by B×l to calculate However, due to the existence of ocean currents in the actual environment all the time, the information output by the EML becomes the speed of the water or the speed of the carrier passing through the water. At the same time, since the speed calculated by the inertial navigation system is the speed relative to the ground or the bottom, there is a difference in ocean current speed between the speed output by the EML and the speed relative to the ground. As described above, for the high-precision integrated navigation algorithm based on the EML, the factor of ocean current needs to be considered.

[0072] In this embodiment, after the forward speed information and the speed relative to the water pass through the implicit Markov filter, outliers are removed and noise is suppressed, and the speed relative to the water fusion speed v y-HMM is obtained, and this speed relative to the water fusion speed is input into the first-level Kalman filter, where the speed relative to the water fusion speed v y-HMM is the forward water speed of the carrier after HMM filtering (excluding the measurement noise of the instrument). The implicit Markov filter is a Kalman filter based on the implicit Markov (HMM) model, and the expression of the implicit Markov filter is:

[0073]

[0074] In the formula, v EML represents the forward speed output by the electromagnetic log; v INS-y represents the forward speed solved by the inertial navigation system; v HMM-y represents the forward speed output by the implicit Markov filter; represents the forward speed of the carrier relative to the earth; v C represents the ocean current speed in the same direction as the carrier's forward movement; δV INS represents the measurement noise of the inertial navigation system; δV EML represents the measurement noise of the electromagnetic log.

[0075] In this embodiment, it is assumed that the HMM can filter out most of the EML measurement noise, then v HMM-y and v C The relationship between them is: Therefore, for the HMM filter with an ideal filtering effect, v HMM-yApproximately equal to the forward speed of the vehicle relative to water. If the forward speed of the vehicle relative to water is denoted as then there is Therefore, v HMM-y can be approximately used to replace for integrated navigation. It should be noted that is the true forward speed of the vehicle relative to the earth, and this value needs to be collected in real time. The forward speed of the vehicle relative to the earth calculated by INS is denoted as v INS-y .

[0076] It should be noted that if there is prior ocean current observation data or ocean current measurement data such as ADCP / buoy to compensate the forward speed v HMM-y relative to water output by the HMM, the forward speed relative to water (speed relative to the earth) after ocean current compensation can be obtained: Using the forward speed v' HMM-y relative to water after ocean current compensation can further improve the integrated navigation accuracy. In this embodiment, the forward speed v' HMM-y relative to water after ocean current compensation is estimated by using the method of GNSS calibration or GNSS integrated navigation. The implicit Markov (HMM) model is as follows:

[0077]

[0078] where

[0079]

[0080] Z k = [v INS-y

[0081] In the formula, X k+1 represents the state vector at time k; Z k represents the measurement vector at time k; F k|k-1 represents the state transition matrix from time (k - 1) to time k; H k represents the measurement matrix; ξ k represents the system noise; v k represents the measurement noise.

[0082] Assume that the elements of the two matrices constituting the state transition matrix and the measurement matrix are F ij and H ij respectively. Then there is a relational expression: and F ij , H ij >0, F ij and H ij The specific values are selected according to experience. The principle of selection is that the one with higher accuracy in INS and EML has a greater weight, and the one with lower accuracy has a smaller weight. For EML and INS with common accuracy, the recommended values are: H k = [0.5 0.5].

[0083] The recursive algorithm of the HMM filter is the same as that of the Kalman filter, as shown in the following equation:

[0084]

[0085] In the formula, represents the predicted state variable from time (k - 1) to time k; K k represents the gain matrix; P k-1 represents the variance matrix; Q k-1 represents the system noise matrix; P k|k-1 represents the one-step prediction variance matrix from time (k - 1) to time k; P k-1|k-2 represents the predicted variance matrix from time (k - 2) to time (k - 1); R k-1 represents the measurement noise matrix; K off represents the filtering gain.

[0086] S2. Input the water fusion speed into the first-level Kalman filter to obtain the first-level filtering parameters; the first-level filtering parameters include the first-level ground speed and the first-level navigation information.

[0087] This embodiment adopts an indirect integrated navigation algorithm based on linear Kalman filter (LKF). Before introducing the Kalman filter, first introduce the strapdown inertial navigation error propagation model derived using perturbation theory. The strapdown inertial navigation error propagation model is:

[0088]

[0089]

[0090]

[0091]

[0092]

[0093] After derivation, the improved strapdown inertial navigation error propagation model is obtained:

[0094]

[0095]

[0096]

[0097] In the formula, Indicates the misalignment angle change vector; Indicates the misalignment angle vector, Indicates the projection of the rotational angular velocity of the navigation system n relative to the inertial system i in the navigation system n; Indicates the calculation error of the rotational angular velocity of the navigation system relative to the inertial system; Indicates the transformation matrix from the IMU system p to the navigation system n; Indicates the gyroscope measured angular velocity error; δv n 、 Respectively indicate the velocity error and its change, Respectively indicate the accelerometer measured specific force and its error; Indicates the projection of the Earth's angular velocity of rotation in the navigation coordinate system; Indicates the projection of the rotational angular velocity of the navigation system n relative to the Earth system e in the navigation system n; Indicates the calculation error of the Earth's angular velocity of rotation; Indicates the calculation error of the rotational angular velocity of the navigation system relative to the Earth system; v n Indicates the first - order filtering parameter, i.e., velocity; δp indicates the position error vector, δp = [δL δλ δh] T ; ε b Indicates the gyro zero - bias in the b system, Indicates the accelerometer zero - bias in the b system,

[0098] The elements of each matrix in the above formula are as follows:

[0099] M ap = M 1 + M 2

[0100]

[0101] M vp =(v n ×)(2M 1 + M 2 )+ M 3

[0102]

[0103]

[0104]

[0105]

[0106] Where, M xy represents the transformation sub - matrix from variable x to y in F INS , where x / y is one of the attitude angle a, velocity v, and position p; F INS is the state transition matrix in the Kalman model; the state equation of the continuous Kalman model is as follows:

[0107]

[0108] Where,

[0109]

[0110]

[0111] The first - order and second - order Kalman filter models of the integrated navigation system involved in this embodiment are:

[0112]

[0113] Where, X k represents the n - dimensional state vector of the integrated navigation system at time k, where φ E , φ N , φ U represent the attitude errors in the east, north, and up directions respectively, δv E , δv N , δv U represent the velocity errors in the east, north, and up directions respectively, δλ, δL, and δh represent the errors in longitude, latitude, and altitude respectively, ε E , ε N , ε U represent the gyro constant drift errors in the east, north, and up directions respectively, represent the accelerometer zero - bias errors in the east, north, and up directions respectively; Φ k|k-1 represents the state transition matrix from time (k - 1) to time k; Γ k|k-1 represents the noise distribution matrix from time (k - 1) to time k; W k-1 represents the system noise vector; Z k represents the observation vector at time k; H k represents the observation matrix at time k; V k represents the observation noise vector at time k.

[0114] Φ k|k-1 is the state transition matrix, which can be obtained by discretizing FINS calculation as follows:

[0115]

[0116] Where, Fk = F INS (t k ), where T is the discrete period, generally consistent with the sampling period of the inertial measurement unit, taken as 1 - 10 ms, and generally truncated to the 2 - 3 order terms during discretization.

[0117] W k-1 and V k are the system noise vector and the measurement noise vector, both of which are zero - mean Gaussian white noise vector sequences and are uncorrelated with each other, that is, they satisfy:

[0118]

[0119] Z k and H k are the observation vector and the observation matrix at time k, which are selected according to different external measurement information;

[0120] 1) When the position and velocity measurement information of GNSS and EML are received simultaneously (second - order Kalman filter), the inertial navigation is in the INS / GNSS / EML (or HMM) combined mode. At this time:

[0121]

[0122]

[0123] In the formula, v′ HMM-i (i = E, N, U) and v INS-i (i = E, N, U) are respectively the three velocity components in the n - frame output by the HMM after ocean current compensation and the three velocity components in the n - frame calculated by the INS; v′ HMM-i is obtained by multiplying the vector [0 v′ HMM-y 0] T by the attitude matrix , where v′ HMM-y is obtained by adding v HMM-y to the estimated ocean current velocity v C , where v HMM-y represents the velocity component of the y - axis output by the HMM; λ GNSS , L GNSS , h DG are respectively the longitude output by GNSS, the latitude output by GNSS, and the depth output by the depth gauge; λ INS , L INS , h INS are the longitude, latitude, and depth output by INS. For the second - order filter, only use v i (i = E, N, U) output by the first - order filter to replace v′ HMM-i (i = E, N, U), and the rest remains unchanged.

[0124] 2) When only the speed measurement information of EML (first - order Kalman filter) is received, the inertial navigation is in the INS / EML (or HMM) combined mode:

[0125] Obtain the observed quantities after GNSS correction:

[0126]

[0127] Obtain the observed quantities before the first GNSS correction or when GNSS correction cannot be obtained for a long time:

[0128]

[0129]

[0130] Similarly, for the second - level filtering, just use the v i (i = E, N, U) output by the first - level filtering to replace v′ HMM-i (i = E, N, U), and the rest remains unchanged.

[0131] 3) When there is no external measurement information, the inertial navigation is in the pure inertial navigation mode, and the LKF filter only performs time update.

[0132] In this embodiment, when the inertial navigation is in the pure inertial navigation mode, the pure inertial solution algorithm is introduced as follows. Among them, the attitude angle is defined as:

[0133]

[0134] Select the "east - north - up (E - N - U)" geographical coordinate system as the navigation reference coordinate system of the strap - down inertial navigation system, denoted as the n - system. Then, the attitude differential model with the n - system as the reference system is:

[0135]

[0136] Among them, the matrix represents the attitude matrix of the vehicle body coordinate system (b - system) relative to the navigation coordinate system (n - system). Since the gyro outputs the angular velocity of the b - system relative to the inertial coordinate system (i - system) and the angular velocity information cannot be directly measured, the following transformation needs to be made to the attitude differential model to obtain the transformed attitude differential model:

[0137]

[0138] Among them, represents the rotation of the n - system relative to the i - system, which includes two parts: the rotation of the navigation system caused by the earth's rotation, and the rotation of the n - system caused by the curvature of the earth's surface due to the movement of the inertial navigation system near the earth's surface. That is, there is Wherein:

[0139]

[0140]

[0141] In the formula, ω ie is the angular velocity of the Earth's rotation; L and h are the geographic latitude and altitude respectively; In this embodiment, the two-sample method is preferably used to solve the transformed attitude differential model.

[0142] The following introduces the specific force model for solving velocity and position:

[0143]

[0144] In the formula, represents the specific force measured by the accelerometer; represents the Coriolis acceleration caused by the motion of the carrier and the Earth's rotation; represents the centripetal acceleration to the Earth caused by the motion of the carrier; g n represents the acceleration due to gravity; are collectively referred to as harmful accelerations.

[0145] It can be seen from the specific force model that only after subtracting the harmful accelerations from the accelerometer output can the geometric motion acceleration of the vehicle in the navigation system be obtained Integrating the acceleration once gives the velocity, and integrating again gives the position. Therefore, the specific force model is the basic equation for inertial navigation calculation.

[0146] The differential equations for the position (latitude, longitude, and altitude) of the strapdown inertial navigation system are as follows:

[0147]

[0148] Rewrite the differential equations for the position of the strapdown inertial navigation system into matrix form:

[0149]

[0150] Wherein,

[0151]

[0152] R Mh = R M + h, R Nh = R N + h

[0153]

[0154] In the formula, R M represents the radius of curvature of the Earth's meridian; R NIt represents the principal curvature radius of the prime vertical circle; f represents the elliptic flattening of the Earth; e represents the elliptic eccentricity or the first eccentricity of the Earth.

[0155] S3. Receive the position information of the global satellite navigation system, and input the position information and the primary filtering parameters into the secondary Kalman filter to obtain the secondary filtering parameters; the secondary filtering parameters include the secondary ground velocity and the secondary latitude.

[0156] S4. Obtain the ocean current velocity based on the secondary ground velocity and the water fusion velocity, and feedback the ocean current velocity to the implicit Markov model to use the ocean current correction velocity to compensate the water fusion velocity to obtain the water compensation velocity.

[0157] Specifically, as Figure 3 shown, the forward velocity information of the INS and the water velocity information of the EML are fused through the HMM to obtain the water velocity with effectively suppressed noise. After this water velocity enters the primary Kalman filter, the primary combined ground velocity and other navigation information (position, acceleration, attitude angle, attitude angular velocity, etc.) are obtained. If the GNSS information cannot be received for a long time, the velocity information and navigation information will be output. Once the AUV floats to the water surface and receives the GNSS information, the velocity information, navigation information, and GNSS information will be input into the secondary Kalman filter together. The difference between the ground velocity output by the secondary Kalman filter and the water velocity output by the HMM is used to obtain the ocean current velocity, and this ocean current velocity is feedback to the output of the HMM to obtain the corrected HMM velocity, thereby improving the combined navigation accuracy. It should be noted that before the next GNSS correction arrives, this ocean current velocity is always used for compensation. Since the change of the ocean current is generally not too drastic, this correction can ensure a certain combined navigation accuracy.

[0158] S5. Use the secondary filtering parameters and the water compensation velocity for feedback correction to obtain the combined navigation parameter information.

[0159] In this embodiment, the ground velocity in the n system output by the 2-level KF filtering is brought into the specific force equation and the relevant elements of the Kalman filter model, which can improve the pure inertial solution accuracy of the velocity and position; the latitude information output by the 2-level KF filtering is solved through the observation matrix H k and then brought into the attitude angle update process and the relevant elements of the Kalman filter model, which can improve the pure inertial solution accuracy of the attitude angle to obtain a more stable and faster-converging KF recursion process. Among them, the feedback correction algorithm is specifically:

[0160]

[0161]

[0162] In the formula, represents the calculation error of the Earth's angular velocity; ωie represents the angular velocity of the Earth's rotation; δL represents the latitude error; L represents the latitude; represents the calculation error of the rotation angular velocity of the navigation system relative to the Earth system; V′ HMM-N represents the northward velocity component of the HMM output after ocean current compensation; R Mh represents the Earth's meridian radius containing altitude information, R Mh =R M +h, where R M represents the Earth's meridian radius, h represents altitude information; V INS-N represents the pure inertial northward velocity calculated by the INS; V H ′ MM-E represents the eastward velocity component of the HMM output after ocean current compensation; V INS-E represents the pure inertial eastward velocity calculated by the INS; R Nh represents the radius of the prime vertical containing altitude information, R Nh =R N +h, where R Nh represents the radius of the prime vertical, h represents altitude information.

[0163] In order to further improve the navigation accuracy, the present embodiment adopts a feedback correction algorithm: the ground velocity and the and and other variables are fed back to the pure inertial solution such as the specific force equation and the KF model. After the second-level Kalman filter is stable, the navigation information such as the ground velocity and latitude output by the second-level filter is used as the final output result. The GNSS correction frequency depends on the change period of the ocean current. For steady or slowly changing ocean currents, only one correction is needed for a long time; while for ocean currents with a shorter change period, a higher correction frequency is required.

[0164] An embodiment of the present invention provides an electromagnetic log ocean current estimation and integrated navigation method based on HMM. The method fuses the forward speed information output by an inertial navigation system and the speed relative to water output by an electromagnetic log through an implicit Markov filter to obtain a speed relative to water fusion speed after noise filtering, and obtains a second-level speed relative to the ground and a second-level latitude through a two-stage Kalman filter; according to the second-level speed relative to the ground and the speed relative to water fusion speed, the ocean current speed is obtained, and the ocean current speed is fed back to the implicit Markov model to use the ocean current correction speed to compensate the speed relative to water fusion speed to obtain a speed relative to water compensation speed; feedback correction is performed using the second-level filtering parameters and the speed relative to water compensation speed to obtain integrated navigation parameter information. Compared with the prior art, this embodiment uses two-stage Kalman filtering and the observable quantity of GNSS-based position information to estimate the ocean current. At the same time, HMM is used to preprocess the output of EML, and then Kalman filtering is performed, which can effectively suppress the measurement noise of EML, thereby improving the estimation accuracy of the ocean current, effectively suppressing the growth of navigation errors, and feeding back variables such as ocean current, speed relative to the ground, and and into pure inertial solutions such as the specific force equation and the KF model, which can effectively improve the ocean current estimation and integrated navigation accuracy, and can also cope with time-varying ocean currents and has a certain anti-interference ability.

[0165] It should be noted that the magnitudes of the serial numbers of the above processes do not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

[0166] In one embodiment, as Figure 4 shown, an embodiment of the present invention provides an electromagnetic log ocean current estimation and integrated navigation system based on HMM. The system includes:

[0167] An HMM fusion module 101, configured to receive the forward speed information output by an inertial navigation system and the speed relative to water output by an electromagnetic log, and fuse the forward speed information and the speed relative to water through an implicit Markov filter to obtain a speed relative to water fusion speed after noise filtering;

[0168] A first-level filtering module 102, configured to input the speed relative to water fusion speed into a first-level Kalman filter to obtain first-level filtering parameters; the first-level filtering parameters include a first-level speed relative to the ground and first-level navigation information;

[0169] A second-level filtering module 103, configured to receive the position information of a global satellite navigation system, and input the position information and the first-level filtering parameters into a second-level Kalman filter to obtain second-level filtering parameters; the second-level filtering parameters include a second-level speed relative to the ground and a second-level latitude;

[0170] The ocean current compensation module 104 is configured to obtain the ocean current speed based on the secondary ground speed and the water fusion speed, and feedback the ocean current speed to the implicit Markov model to correct the water fusion speed by using the ocean current, so as to obtain the water compensation speed.

[0171] The feedback correction module 105 is configured to perform feedback correction by using the secondary filtering parameter and the water compensation speed to obtain the combined navigation parameter information.

[0172] For the specific limitations of an electromagnetic log ocean current estimation and integrated navigation system based on HMM, reference can be made to the above limitations of an electromagnetic log ocean current estimation and integrated navigation method based on HMM, which will not be elaborated here. Those of ordinary skill in the art can realize that, in combination with the various modules and steps described in the embodiments disclosed in this application, they can be implemented by hardware, software, or a combination of both. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

[0173] The embodiment of the present invention provides an electromagnetic log ocean current estimation and integrated navigation system based on HMM. The system obtains reliable and accurate navigation information such as speed through a two-stage Kalman filter and a feedback correction algorithm, realizes ocean current estimation and integrated navigation, and solves the technical problems of poor accuracy, inability to suppress the measurement noise of normal speed values, and complex operation in the traditional INS / EML / GNSS integrated navigation method. Compared with the prior art, this application uses a feedback correction algorithm to effectively improve the accuracy of ocean current speed estimation, which is beneficial to improving the navigation accuracy and has certain practical value for the precise navigation of AUVs in complex underwater environments.

[0174] Figure 5 This is a computer device provided by an embodiment of the present invention, including a memory, a processor, and a transceiver, which are connected through a bus; the memory is used to store a set of computer program instructions and data, and can transmit the stored data to the processor, and the processor can execute the program instructions stored in the memory to perform the steps of the above method.

[0175] Among them, the memory may include volatile memory or non-volatile memory, or may include both volatile and non-volatile memory; the processor may be a central processing unit, a microprocessor, an application-specific integrated circuit, a programmable logic device, or a combination thereof. By way of example but not limitation, the above programmable logic device may be a complex programmable logic device, a field programmable gate array, a generic array logic, or any combination thereof.

[0176] In addition, the memory can be a physically independent unit or integrated with the processor.

[0177] Those of ordinary skill in the art can understand that Figure 5 the structure shown in [the figure] is only a block diagram of some structures related to the solution of this application, and does not constitute a limitation on the computer device to which the solution of this application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have the same component arrangement.

[0178] In one embodiment, the embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above method are implemented.

[0179] A method and system for electromagnetic log ocean current estimation and integrated navigation based on HMM provided by the embodiment of the present invention. A method for electromagnetic log ocean current estimation and integrated navigation based on HMM can obtain the ground and ocean current speeds during the diving / ascending process of an AUV and complete error correction through a feedback correction algorithm to obtain reliable and accurate navigation information such as speed, etc., and has the advantages of high navigation accuracy, low cost, and high real-time performance.

[0180] In the above embodiment, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions described in the embodiments of the present invention are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by wire (such as coaxial cable, optical fiber, digital subscriber line) or wirelessly (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that the computer can access or a data storage device such as a server or data center that includes one or more integrated available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as an SSD), etc.

[0181] Those skilled in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods.

[0182] The above embodiments only represent several preferred embodiments of the present application. The description is relatively specific and detailed, but it should not be construed as a limitation on the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the technical principle of the present invention, several improvements and substitutions can be made, and these improvements and substitutions should also be regarded as the protection scope of the present application. Therefore, the protection scope of the patent of the present application should be subject to the protection scope of the claims.

Claims

1. An electromagnetic log ocean current estimation and integrated navigation method based on HMM, characterized in that, it includes the following steps: Receive the forward speed information output by the inertial navigation system and the speed relative to water output by the electromagnetic log, and fuse the forward speed information and the speed relative to water through an implicit Markov filter to obtain the speed relative to water fusion speed after noise filtering; Input the speed relative to water fusion speed into a first-level Kalman filter to obtain first-level filtering parameters; the first-level filtering parameters include the first-level speed relative to the ground and the first-level navigation information; Receive the position information of the global satellite navigation system, and input the position information and the first-level filtering parameters into a second-level Kalman filter to obtain second-level filtering parameters; The second-level filtering parameters include the second-level speed relative to the ground and the second-level latitude; According to the second-level speed relative to the ground and the speed relative to water fusion speed, obtain the ocean current speed, and feedback the ocean current speed to the implicit Markov model to use the ocean current correction speed to compensate the speed relative to water fusion speed to obtain the speed relative to water compensation speed; Use the second-level filtering parameters and the speed relative to water compensation speed for feedback correction to obtain integrated navigation parameter information.

2. An electromagnetic log ocean current estimation and integrated navigation method based on HMM according to claim 1, characterized in that, The models of both the first-level Kalman filter and the second-level Kalman filter are: wherein, , represents the n-dimensional state vector of the integrated navigation system at time k, where, , , represent the attitude errors in the east, north, and up directions respectively, , , represent the velocity errors in the east, north, and up directions respectively, , , represent the errors in longitude, latitude, and altitude respectively, , , represent the constant drift errors of the gyroscopes in the east, north, and up directions respectively, , , represent the zero bias errors of the accelerometers in the east, north, and up directions respectively; represents the state transition matrix from time (k - 1) to time k; represents the noise distribution matrix from time (k - 1) to time k; represents the system noise vector; represents the observation vector at time k; represents the observation matrix at time k; represents the observation noise vector at time k.

3. An electromagnetic log ocean current estimation and integrated navigation method based on HMM according to claim 2, characterized in that, In different navigation modes, the observation vector and the observation matrix are respectively: When receiving the position information of the global satellite navigation system and the speed relative to water of the electromagnetic log at the same time, the inertial navigation is in an integrated navigation mode based on the inertial navigation system, the global satellite navigation system and the electromagnetic log, and the observation vector and the observation matrix are respectively: In the formula, and are respectively the three velocity components in the n-frame output by the HMM after ocean current compensation and the three velocity components in the n-frame calculated by the INS; is obtained by multiplying the vector by the attitude matrix , where is obtained by adding to the estimated ocean current velocity v C , where represents the velocity component of the y-axis output by the HMM; , , are respectively the longitude output by the GNSS, the latitude output by the GNSS, and the depth output by the depth gauge; , , are the longitude, latitude, and depth output by the INS; When only receiving the speed relative to water of the electromagnetic log, the inertial navigation is in an integrated navigation mode based on the inertial navigation system and the electromagnetic log, and the observation vector and the observation matrix are respectively: Obtain the observed quantity after GNSS correction: Obtain the observed quantity before the first GNSS correction or when GNSS correction cannot be obtained for a long time: When there is no external measurement information, the inertial navigation is in a pure inertial navigation mode, and only time updates are performed on the first-level Kalman filter and the second-level Kalman filter.

4. An electromagnetic log ocean current estimation and integrated navigation method based on HMM according to claim 1, characterized in that, The expression of the implicit Markov filter is: In the formula, represents the forward speed output by the electromagnetic log; represents the forward speed information resolved by the inertial navigation system; represents the forward speed output by the hidden Markov filter; represents the forward speed of the vehicle relative to the earth; represents the ocean current speed in the same direction as the vehicle's forward movement; represents the measurement noise of the inertial navigation system; represents the measurement noise of the electromagnetic log.

5. An electromagnetic log ocean current estimation and integrated navigation method based on HMM according to claim 1, characterized in that, The step of using the second-level filtering parameters and the speed relative to water compensation speed for feedback correction to obtain integrated navigation parameter information includes: Respectively feedback the second-level speed relative to the ground to the specific force model, the first-level Kalman filter and the second-level Kalman filter model to obtain the geometric motion acceleration of the vehicle in the navigation system; According to the geometric motion acceleration of the vehicle in the navigation system, obtain the pure inertial solution results of the position and the speed; Perform calculations on the second-level latitude to obtain the calculation error of the earth's angular velocity of rotation and the calculation error of the angular velocity of rotation of the navigation system relative to the earth system; The calculation errors of the angular velocity of the Earth's rotation and the calculation error of the rotation angular velocity of the navigation system relative to the Earth system are respectively fed back to the first-level Kalman filter, the second-level Kalman filter model, and the transformed attitude differential model, and the pure inertial solution result of the attitude angle is obtained according to the water compensation velocity; After the first-level Kalman filter and the second-level Kalman filter model are stable, the integrated navigation parameter information is output according to the pure inertial solution result of the position, the pure inertial solution result of the velocity, and the pure inertial solution result of the attitude angle.

6. A method for estimating ocean currents and integrated navigation based on HMM according to claim 5, characterized in that, the specific force model is: wherein, represents the geometric motion acceleration of the vehicle under the navigation system; represents the attitude matrix of the vehicle body relative to the navigation coordinate system; represents the specific force measured by the accelerometer; represents the Coriolis acceleration caused by the vehicle motion and the earth rotation; represents the centripetal acceleration to the ground caused by the vehicle motion; represents the gravitational acceleration; are collectively referred to as harmful accelerations; the transformed attitude differential model is: Among them, represents the differential matrix of the transformed attitude; represents that the gyroscope outputs the angular velocity of the b system relative to the inertial system; represents the rotation of the n system relative to the i system, which consists of two parts: the rotation of the navigation system caused by the earth's rotation, and the rotation of the n system caused by the curvature of the earth's surface when the inertial navigation system moves near the earth's surface, that is, , where: In the formula, is the angular velocity of the Earth's rotation; represents the rotation of the n - system caused by the curvature of the Earth's surface when the inertial navigation system moves near the Earth's surface; L and h are the geographical latitude and altitude respectively; represents the radius of the principal curvature of the Earth's meridian; represents the radius of the principal curvature of the prime vertical.

7. A method for estimating ocean currents and integrated navigation based on HMM according to claim 5, characterized in that, the calculation errors of the angular velocity of the Earth's rotation and the calculation error of the rotation angular velocity of the navigation system relative to the Earth system are respectively: In the formula, represents the calculation error of the angular velocity of the Earth's rotation; represents the angular velocity of the Earth's rotation; represents the latitude error; L represents the latitude; represents the calculation error of the angular velocity of the navigation system relative to the Earth system; represents the northward velocity component of the HMM output after ocean current compensation; represents the radius of the Earth's meridian circle containing altitude information; represents the pure inertial northward velocity calculated by the INS; represents the eastward velocity component of the HMM output after ocean current compensation; represents the pure inertial eastward velocity calculated by the INS; represents the radius of the prime vertical circle containing altitude information.

8. An integrated navigation system for estimating ocean currents of an electromagnetic log based on HMM, characterized in that, the system includes: An HMM fusion module, configured to receive the forward speed information output by the inertial navigation system and the water speed output by the electromagnetic log, and fuse the forward speed information and the water speed through an implicit Markov filter to obtain a water fusion speed after noise filtering; A first-level filtering module, configured to input the water fusion speed into a first-level Kalman filter to obtain first-level filtering parameters; the first-level filtering parameters include the first-level ground speed and the first-level navigation information; A second-level filtering module, configured to receive the position information of the global satellite navigation system, and input the position information and the first-level filtering parameters into a second-level Kalman filter to obtain second-level filtering parameters; the second-level filtering parameters include the second-level ground speed and the second-level latitude; An ocean current compensation module, configured to obtain the ocean current speed according to the second-level ground speed and the water fusion speed, and feed back the ocean current speed to the implicit Markov model to use the ocean current correction speed to compensate the water fusion speed to obtain a water compensation speed; A feedback correction module, configured to perform feedback correction by using the second-level filtering parameters and the water compensation speed to obtain integrated navigation parameter information.

9. A computer device, characterized in that: It includes a processor and a memory. The processor is connected to the memory. The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory so that the computer device executes the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: A computer program is stored in the computer-readable storage medium, and when the computer program is run, the method according to any one of claims 1 to 7 is implemented.

Images