Ocean current estimation and co-localization robust filtering method, program, equipment and storage medium in unknown time-varying ocean current environment

Abstract

The invention belongs to the technical field of multi-AUV (Autonomous Underwater Vehicle) cooperative positioning in a complex ocean environment, and particularly relates to an ocean current estimation and cooperative positioning robust filtering method, program and equipment in an unknown time-varying ocean current environment, and a storage medium. According to the method, a cooperative positioning system model under ocean current interference is constructed, a one-step prediction state value and a one-step prediction state covariance matrix are calculated by using Kalman filtering, and time updating is carried out; a statistical linear regression model considering the simultaneous interference of the ocean current speed and the abnormal value is reconstructed based on the constructed cooperative positioning system model, a traditional variational maximum entropy criterion is improved by using a Laplace kernel function, and an updated filtering state value and a state covariance matrix are solved by using an unfixed point iteration method to perform measurement updating. The method can cope with unknown time-varying ocean current speed influence and complex non-Gaussian noise interference at the same time, the ocean current is compensated in real time, the positioning precision and robustness of the system in a complex scene are improved, and the method is more suitable for a complex engineering environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-AUV cooperative positioning in complex marine environments, and particularly relates to an ocean current estimation and cooperative positioning robust filtering method, program, device, and storage medium in an unknown time-varying ocean current environment. Background Art

[0002] When AUVs navigate on the ocean, they will inevitably be affected by ocean currents. During the navigation of AUVs, when encountering terrains such as cliffs and extremely deep trenches, the ranging distance of the Doppler velocity log (DVL) will be less than the height of the AUVs from the seabed, and the DVL can only measure the velocity relative to the water. Most traditional cooperative positioning algorithms ignore the interference of ocean current velocity. However, when the ocean current acts on the movement of AUVs, the ranging error between the follower AUV and the master AUV accumulates, ultimately leading to the failure of the positioning algorithm. At the same time, affected by the uncertainties of ocean currents, sound velocity, and the multipath effect of the underwater acoustic channel, the noise of the cooperative system mostly presents a non-Gaussian distribution. Therefore, it is extremely important to explore the influence of unknown time-varying ocean currents and complex non-Gaussian noise on the cooperative positioning accuracy, and to consider how to compensate the system for the errors caused by ocean currents, for the development of the multi-AUV cooperative positioning system. Summary of the Invention

[0003] The purpose of the present invention is to provide an ocean current estimation and cooperative positioning robust filtering method, program, device, and storage medium in an unknown time-varying ocean current environment, aiming at the defects of the existing cooperative positioning technology, which can simultaneously cope with the influence of unknown time-varying ocean current velocity and complex non-Gaussian noise interference, improve the positioning accuracy and robustness of the system in complex scenarios through real-time compensation of ocean currents, and be more adaptable to complex engineering environments.

[0004] The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment includes the following steps:

[0005] Step 1: In the AUV formation, the slave AUV obtains the position information of the master AUV at the current moment and the measurement information of itself relative to the master AUV.

[0006] Step 2: According to the cooperative positioning system model considering ocean current interference, using the state estimation vector at the previous moment as the input, obtain the state transition matrix and the system noise driving matrix; the state estimation vector includes the position estimation information of the slave AUV and the velocity information of the unknown ocean current.

[0007] Step 3: Calculate the one-step prediction state vector and the one-step prediction state covariance matrix through Kalman filtering for time update.

[0008] Step 4: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, improve the traditional variational maximum entropy criterion using the Laplace kernel function, solve the state estimation vector and state estimation covariance matrix at the current moment using the fixed-point iteration method, and perform measurement update.

[0009] Further, the specific steps of Step 4 are as follows:

[0010] Step 4.1: Set the maximum number of iterations and initialize the state estimation vector at the current moment randomly.

[0011] Step 4.2: According to the cooperative positioning system model considering ocean current interference, take the state estimation vector at the current moment in the current iteration as the input to obtain the measurement transfer matrix.

[0012] Step 4.3: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, and update the one-step prediction state covariance matrix and measurement noise matrix according to the reconstructed model.

[0013] Step 4.4: Calculate the filtering gain matrix according to the measurement transfer matrix, the updated one-step prediction state covariance matrix and the measurement noise matrix, and update the state estimation vector at the current moment according to the filtering gain matrix as the input for the next iteration.

[0014] Step 4.5: If the maximum number of iterations is not reached and the state estimation vector at the current moment does not converge, return to Step 4.2; otherwise, stop the iteration, output the state estimation vector and state estimation covariance matrix at the current moment, and complete the measurement update.

[0015] Further, in Step 1, obtain the position information of the main AUV at the current k moment from the AUV the measurement information d of itself relative to the main AUV k ;

[0016] The state estimation vector at the current k moment represents the position information estimation of the AUV at the k moment. and respectively represent the eastward velocity estimation and northward velocity estimation of the unknown ocean current at the k moment.

[0017] Further, the cooperative positioning system model considering ocean current interference in Step 2 is:

[0018] x k = f(x k-1 ) + ω k = f * (x k-1 , ω k )

[0019] d k = h(x k ) + ν k = h * (x k , ν k )

[0020] where u k-1 and φ k-1 represent the velocity and heading angle of the AUV at time k - 1, respectively; ω k and ν k represent the process noise and measurement noise at time k, respectively; Δt represents the time duration between time k and time k - 1;

[0021] In step 2, according to the cooperative positioning system model considering ocean current interference, the state estimation vector at the previous moment is used as the input to obtain the state transition matrix F k,k-1 and the system noise driving matrix N k,k-1 :

[0022]

[0023] In step 4.2, according to the cooperative positioning system model considering ocean current interference, the state estimation vector at the current moment in the current t - th iteration is used as the input to obtain the measurement transition matrix :

[0024]

[0025] Furthermore, in step 3, the one - step predicted state vector and the one - step predicted state covariance matrix are calculated through Kalman filtering:

[0026]

[0027]

[0028] where is the state estimation covariance matrix at the previous moment; Q k-1 is the process noise covariance matrix at the previous moment;

[0029] Furthermore, step 4.3 is specifically as follows:

[0030] Step 4.3.1: Reconstruct the statistical linear regression model of the cooperative positioning system under ocean current interference:

[0031]

[0032] where \(I\) is the identity matrix; the residual matrix

[0033] Step 4.3.2: Calculate According to Obtain the measurement noise matrix

[0034] Step 4.3.3: Let R k Perform Cholesky decomposition on them respectively to obtain matrices

[0035]

[0036] Step 4.3.4: Let Calculate the matrix

[0037]

[0038] where the matrix and are both matrices with 5 rows and 1 column; the matrix is a matrix with 5 rows and 4 columns;

[0039] Step 4.3.5: Construct the matrices and

[0040]

[0041] where \(L σ (·)\) is the Laplace probability density function; \(c\) is a constant;

[0042] Step 4.3.6: Update the one-step prediction state covariance matrix and the measurement noise matrix

[0043]

[0044] Furthermore, in Step 4.4, according to the measurement transition matrix the updated one-step prediction state covariance matrix and the measurement noise matrix calculate the filtering gain matrix :

[0045]

[0046] According to the filtering gain matrix Update the state estimation vector at the current moment as the input for the next iteration :

[0047]

[0048] In step 4.5, if the maximum number of iterations is reached or is less than the error threshold, stop the iteration and let Output the state estimation vector at the current moment and the state estimation covariance matrix Complete the measurement update.

[0049] A computer device / system, comprising a memory, a processor, and a computer program stored on the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment.

[0050] A computer-readable storage medium, on which a computer program / instructions are stored, and when the computer program / instructions are executed by a processor, the steps of the above-mentioned ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment are implemented.

[0051] A computer program product, comprising a computer program / instructions, and when the computer program / instructions are executed by a processor, the steps of the above-mentioned ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment are implemented.

[0052] The beneficial effects of the present invention are as follows:

[0053] The present invention constructs a cooperative positioning system model under ocean current interference, uses Kalman filtering to calculate the one-step predicted state value and the one-step predicted state covariance matrix for time update; reconstructs a statistical linear regression model considering the simultaneous interference of ocean current speed and outliers based on the constructed cooperative positioning system model, and at the same time uses the Laplace kernel function to improve the traditional variational maximum entropy criterion, and uses the fixed-point iteration method to solve for the updated filtering state value and state covariance matrix for measurement update. The present invention can simultaneously cope with the influence of unknown time-varying ocean current speed and complex non-Gaussian noise interference, improve the positioning accuracy and robustness of the system in complex scenarios through real-time compensation of ocean currents, and be more adaptable to complex engineering environments. Description of the Drawings

[0054] Figure 1 It is a trajectory diagram of the AUV.

[0055] Figure 2 It is an error distribution and Laplace distribution kernel function diagram.

[0056] Figure 3 It is an eastward ocean current estimation diagram under different algorithms.

[0057] Figure 4Northward ocean current estimation diagrams under different algorithms.

[0058] Figure 5 Position error comparison diagrams under different algorithms. Specific implementation manners

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

[0060] The present invention provides a robust filtering method for ocean current estimation and cooperative localization in an unknown time-varying ocean current environment, including the following steps:

[0061] Step 1: In the AUV formation, obtain the position information of the master AUV at the current moment The measurement information d of itself relative to the master AUV k ;

[0062] Denote the position information estimation of the slave AUV at time k; And Respectively denote the eastward velocity estimation and northward velocity estimation of the unknown ocean current at time k

[0063] Step 2: According to the cooperative localization system model considering ocean current interference, use the state estimation vector at the previous moment as the input to obtain the state transition matrix and the system noise driving matrix;

[0064] The cooperative localization system model considering ocean current interference is:

[0065] x k = f(x k-1 ) + ω k = f * (x k-1 , ω k )

[0066] d k = h(x k ) + ν k = h * (x k , ν k )

[0067] Wherein, u k-1 And φ k-1 Respectively denote the velocity and heading angle of the slave AUV at time k - 1; ω k And ν k Respectively denote the process noise and measurement noise at time k; Δt represents the time duration between time k and time k - 1;

[0068] According to the cooperative localization system model considering ocean current interference, use the state estimation vector at the previous moment As the input, obtain the state transition matrix F k,k-1 and the system noise driving matrix N k,k-1 :

[0069]

[0070] Step 3: Calculate the one-step predicted state value and the one-step predicted state covariance matrix for time update;

[0071]

[0072] where is the state estimation covariance matrix at the previous moment; Q k-1 is the process noise covariance matrix at the previous moment;

[0073] Step 4: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, improve the traditional variational maximum entropy criterion using the Laplace kernel function, and solve the state estimation vector and state estimation covariance matrix at the current moment using the fixed-point iteration method for measurement update;

[0074] Step 4.1: Set the maximum number of iterations t max , initialize the current iteration number t = 1, and initialize the randomly set state vector from the AUV

[0075] Step 4.2: According to the cooperative positioning system model considering ocean current interference, use the state estimation vector at the current moment in the current iteration as the input to obtain the measurement transfer matrix

[0076]

[0077] Step 4.3: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, and update the one-step predicted state covariance matrix and measurement noise matrix according to the reconstructed model;

[0078] Step 4.3.1: Reconstruct the statistical linear regression model of the cooperative positioning system under ocean current interference:

[0079]

[0080] where I is the identity matrix; the residual matrix

[0081] Step 4.3.2: Calculate According to obtain the measurement noise matrix

[0082] Step 4.3.3: Let R k be respectively decomposed by Cholesky decomposition to obtain matrix :

[0083]

[0084] Step 4.3.4: Let and calculate matrix

[0085]

[0086] wherein, matrices and are both matrices with 5 rows and 1 column; matrix is a matrix with 5 rows and 4 columns;

[0087] Step 4.3.5: Construct matrices and

[0088]

[0089] wherein, L σ (·) is the Laplace probability density function; c is a constant;

[0090] Step 4.3.6: Update the one-step prediction state covariance matrix and the measurement noise matrix

[0091]

[0092] Step 4.4: Calculate the filter gain matrix according to the measurement transition matrix the updated one-step prediction state covariance matrix and the measurement noise matrix :

[0093]

[0094] Update the state estimation vector at the current moment according to the filter gain matrix as the input for the next iteration :

[0095]

[0096] Step 4.5: If the maximum number of iterations is reached or is less than the error threshold, stop the iteration and let output the state estimation vector at the current moment With the state estimation covariance matrix Complete the measurement update; otherwise, set t = t + 1 and return to step 4.2.

[0097] Different from the existing positioning methods considering ocean currents that only consider constant ocean currents and Gaussian noise, the present invention can handle complex non-Gaussian noise and can also handle unknown time-varying ocean currents. The present invention reconstructs a statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, and at the same time uses the Laplace kernel function to improve the traditional variational maximum entropy criterion, which improves the robustness to complex non-Gaussian noise and ocean current velocity disturbances.

[0098] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A robust filtering method for ocean current estimation and cooperative positioning in an unknown time-varying ocean current environment, characterized in that, It includes the following steps: Step 1: In the AUV formation, obtain the position information of the main AUV at the current moment and the measurement information of itself relative to the main AUV from the AUV; Step 2: According to the cooperative positioning system model considering ocean current interference, take the state estimation vector at the previous moment as the input to obtain the state transition matrix and the system noise driving matrix; the state estimation vector includes the position estimation information of the AUV from and the velocity information of the unknown ocean current; Step 3: Calculate the one-step prediction state vector and the one-step prediction state covariance matrix through Kalman filtering for time update; Step 4: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, improve the traditional variational maximum entropy criterion using the Laplace kernel function, and use the fixed-point iteration method to solve the state estimation vector and the state estimation covariance matrix at the current moment for measurement update.

2. The robust filtering method for ocean current estimation and cooperative positioning in an unknown time-varying ocean current environment according to claim 1, characterized in that: The specific content of Step 4 is as follows: Step 4.1: Set the maximum number of iterations and initialize the state estimation vector at the current moment randomly; Step 4.2: According to the cooperative positioning system model considering ocean current interference, take the state estimation vector at the current moment in the current iteration as the input to obtain the measurement transfer matrix; Step 4.3: Reconstruct the statistical linear regression model considering the simultaneous interference of ocean current velocity and outliers, and update the one-step prediction state covariance matrix and the measurement noise matrix according to the reconstructed model; Step 4.4: Calculate the filter gain matrix according to the measurement transfer matrix, the updated one-step prediction state covariance matrix and the measurement noise matrix, and update the state estimation vector at the current moment according to the filter gain matrix as the input for the next iteration; Step 4.5: If the maximum number of iterations is not reached and the state estimation vector at the current moment does not converge, return to Step 4.2; Otherwise, stop the iteration, output the state estimation vector and the state estimation covariance matrix at the current moment, and complete the measurement update.

3. The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment according to claim 2, wherein: In the said step 1, obtain the position information of the main AUV at the current moment k from the AUV The measurement information d of itself relative to the main AUV k ; The current state estimation vector at time k represents the estimation of the position information of the AUV at time k; and respectively represent the estimation of the eastward velocity and the northward velocity of the unknown ocean current at time k.

4. The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment according to claim 3, wherein: The cooperative positioning system model considering ocean current interference in Step 2 is: x k = f(x k-1 ) + ω k = f * (x k-1 , ω k ) d k = h(x k ) + ν k = h * (x k , ν k ) Among them, u k-1 and φ k-1 respectively represent the velocity and heading angle of the AUV at time k - 1; ω k and ν k respectively represent the process noise and measurement noise at time k; Δt represents the time duration between time k and time k - 1; In step 2, according to the cooperative positioning system model considering ocean current interference, the state estimation vector at the previous moment is used as the input to obtain the state transition matrix F k,k-1 and the system noise driving matrix N k,k-1 : In step 4.2, according to the cooperative positioning system model considering ocean current interference, the state estimation vector at the current moment in the current t-th iteration is used as the input to obtain the measurement transition matrix 5. The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment according to claim 4, characterized in that: In step 3, the one-step predicted state vector is calculated through Kalman filtering and the one-step predicted state covariance matrix Among them, is the state estimation covariance matrix at the previous moment; Q k-1 is the process noise covariance matrix at the previous moment.

6. The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment according to claim 5, characterized in that: The specific content of Step 4.3 is as follows: Step 4.3.1: Reconstruct the statistical linear regression model of the cooperative positioning system under ocean current interference: where I is the identity matrix; the residual matrix Step 4.3.2: Calculate According to Obtain the measurement noise matrix Step 4.3.3: Respectively pass R k through Cholesky decomposition to obtain the matrix Step 4.3.4: Let Calculate the matrix Among them, the matrix and are both matrices with 5 rows and 1 column; the matrix is a matrix with 5 rows and 4 columns; Step 4.3.5: Construct a matrix and where, L σ (·) is the Laplace probability density function; c is a constant; Step 4.3.6: Update the one-step prediction state covariance matrix and the measurement noise matrix 7. The ocean current estimation and cooperative positioning robust filtering method in an unknown time-varying ocean current environment according to claim 6, characterized in that: In step 4.4, according to the measurement transfer matrix the updated one-step prediction state covariance matrix and the measurement noise matrix calculate the filtering gain matrix According to the filtering gain matrix Update the state estimation vector at the current moment as the input for the next iteration If the maximum number of iterations is reached or less than the error threshold in step 4.5, stop the iteration and let output the state estimation vector at the current moment and the state estimation covariance matrix Complete the measurement update.

8. A computer device / equipment / system, comprising a memory, a processor, and a computer program stored on 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 7.

9. A computer-readable storage medium having computer programs / instructions stored thereon, characterized in that: When the computer program / instructions are executed by the processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising computer programs / instructions, characterized in that: When the computer program / instructions are executed by the processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images