AUV Co-localization Method Based on Generalized Maximum Correlation Entropy and Volumetric Kalman Filter

Abstract

This invention relates to the field of underwater positioning technology, specifically to an AUV cooperative positioning method based on generalized maximum correlation entropy and capacitive Kalman filtering (CKF) that can effectively improve positioning accuracy. This invention employs GMCC and introduces a generalized Gaussian density kernel function to measure the error of the nonlinear filtering algorithm using generalized correlation entropy. The generalized Gaussian kernel function has the advantages of having many parameters, flexible variation, and adaptability. It can achieve the maximum correlation entropy when the error is minimized. Because the generalized Gaussian kernel contains even-order higher moments of the errors of two variables, it can better handle heavy-tailed noise and large outliers. Experimental verification shows that the technical solution of this invention has better robustness and reliability, and can better handle problems such as outliers and heavy-tailed noise interference. Compared with the traditional CKF, this invention can improve positioning accuracy.

Description

Technical fields:

[0001] This invention relates to the field of underwater positioning technology, specifically to an AUV cooperative positioning method based on generalized maximum correlation entropy and capacitive Kalman filtering that can effectively improve positioning accuracy. Background technology:

[0002] Underwater positioning is a technology that provides information such as attitude, velocity, and position of Autonomous Underwater Vehicles (AUVs), and it is a prerequisite for AUVs to successfully complete various underwater tasks. Master-slave AUV collaboration based on a single beacon can improve work efficiency. During underwater positioning, the slave AUV lacks external auxiliary positioning information. However, the single beacon and the master AUV can transmit position, distance, and other information to the slave AUV, using distance measurement as a measurement tool to assist in positioning, thereby improving the positioning accuracy of the slave AUV. Single-beacon-assisted AUV collaborative positioning effectively reduces positioning errors and is suitable for underwater collaborative positioning systems.

[0003] Currently, Extended Kalman Filter (EPF) and CKF are commonly used filtering algorithms in underwater cooperative positioning systems. However, underwater single-beacon master-slave AUV cooperative positioning systems are nonlinear systems that generate heavy tail noise. Traditional EPF and CKF are not effective in handling heavy tail noise and outliers. Furthermore, in single-beacon master-slave AUV cooperative positioning systems, the information transmission between devices is often affected by the complex underwater environment, resulting in outliers in the measured distance information of the sound wave transmission.

[0004] GMCC is a novel optimization criterion that uses a generalized Gaussian density kernel function to measure the generalized correlation entropy of the error in nonlinear filtering algorithms. The generalized Gaussian kernel function has the advantages of having many parameters, flexible variation, and adaptability. It achieves the maximum correlation entropy when the error is minimized. Because it includes even-order higher moments of the errors of two variables, the generalized Gaussian kernel can better handle heavy-tailed noise and large outliers. It exhibits better robustness and reliability, and can better handle problems such as outliers and noise interference. Summary of the Invention:

[0005] This invention addresses the shortcomings and deficiencies of existing technologies by proposing an AUV cooperative positioning method based on generalized maximum correlation entropy and commensurate Kalman filtering. This method utilizes the ranging and position information of a single beacon and the main AUV to assist in positioning from the AUV, thereby effectively improving positioning accuracy.

[0006] This invention is achieved through the following measures:

[0007] A cooperative localization method for AUVs based on generalized maximum correlation entropy and capacitive Kalman filtering includes the following steps:

[0008] Step 1: Construct the state equation and observation equation of the single beacon-assisted master-slave AUV positioning system: where the cost function, mixed entropy, and kernel function of GMCC are defined, the information transmission time between the single beacon and the slave AUV and between the master AUV and the slave AUV are used as the observation information, and the abscissa, ordinate, depth, speed, heading angle and underwater sound speed of the slave AUV are used as the state information.

[0009] Step 2: Based on the state equation and observation equation of the single-beacon-assisted master-slave AUV positioning system, perform a one-step prediction at time k+1 using the information at time k, and obtain the state estimate at time k+1. and estimate the covariance P k+1|k Measurement estimation in the measurement step Update and state measurement cross covariance P xz,k+1|k Matrix update;

[0010] Step 3: Set the values ​​of the matrices required for iteration, and set the initial and final values ​​for iteration to obtain the error vector and weight matrix;

[0011] Step 4: Begin iterating to a stable state, and retain the state vector estimate from the last iteration. Calculate the state vector error covariance matrix P. k+1|k+1 Update.

[0012] Step 1 of this invention specifically includes the following steps:

[0013] Step 1-1: Construct the cost function, mixture entropy, and kernel function as follows:

[0014] J GMCC (x,y)=E[G α，β (x,y))]=E[γ α,β exp(-λ|e| α )]

[0015]

[0016]

[0017] Where x and y are two input vectors, α is the shape parameter of the kernel function, β is the size parameter of the kernel function, and λ = 1 / β α For kernel parameters, γ α，β =α / [2β·Γ(1 / α)] is the normalization constant, Γ(·) is the gamma function, m is the number of mixing kernel functions, n represents the dimension of the input vector, and σ i The different weights of the kernel function, x jand y j Let G represent the j-th element of x and y respectively. i (x j ,y j ) represents the i-th kernel function.

[0018] Step 1-2: Observation information z at time k k for:

[0019]

[0020] Among them, V k This represents observation noise, where t1 is the signal transmission time between the master AUV and the slave AUV, t2 is the signal transmission time between the single beacon and the slave AUV, d1 is the distance between the master AUV and the slave AUV, d2 is the distance between the single beacon and the slave AUV, and c is the distance between the master AUV and the slave AUV. k It is the speed of sound underwater, x k and y k Let x and y represent the x and y coordinates of the AUV at time k, respectively. and Let x and y represent the x and y coordinates of the main AUV at time k, respectively, and let x0 and y0 represent the x and y coordinates of the beacon, respectively; the state vector x at time k... k For: x k =[x,y,z,v,θ,c] T Where x is the abscissa of the AUV, y is the ordinate, z is the depth, v is the velocity, θ is the heading angle, and c is the underwater speed of sound;

[0021] Constructing the state equation and observation equation:

[0022]

[0023] Where f(·) represents the state transition function, W k Representing process noise, h(·) is the observation matrix, V k This indicates observation noise.

[0024] Step 2 of the present invention specifically includes:

[0025] Step 2-1: The one-step prediction at time k+1 based on the information at time k is as follows:

[0026] According to the third-order spherical radial volume criterion of CKF, several volume points can be used to approximate the mean and covariance of the state vector in a nonlinear system. The state vector can be obtained by Cholesky decomposition:

[0027]

[0028] Where, xi,k+1|k The sampling points represent the state vector. i = 1, 2, ..., 2n x n x Let S be the dimension of the state vector x. k|k =chol(P k|k ), where chole(·) represents the Cholesky decomposition function.

[0029] For the state prediction of the volume point, based on the one-step prediction updated at time k+1, the one-step predicted state vector is as follows: The predicted state of the volumetric points is as follows:

[0030]

[0031] Step 2-2: The one-step prediction error covariance matrix at time k is:

[0032]

[0033] Among them, Q k This represents the process noise matrix.

[0034] Following step 2-1, we can obtain:

[0035]

[0036] in, This indicates a volumetric measurement prediction. P represents the predicted measurement value. xz,k+1|k This represents the cross-covariance matrix of state measurements.

[0037] Step 3 of this invention specifically includes:

[0038] Step 3-1: Apply the cost function to x k+1 Differentiation yields the cost function J with respect to the change x. k+1 The maximum point is the optimal estimate. set up:

[0039] G i (x j ,y j ) = G i (x j -y j ) = G i (e j )

[0040] Among them, e j =x j -y j Let represent the error vector. Then:

[0041]

[0042] Step 3-2: Define the error as a combination of state error and measurement error. State error is the difference between the state value and the predicted state value. Measurement error is the difference between the measured value and the state value after the measurement transformation matrix. in Then multiply both sides of the combined error vector by... in get:

[0043]

[0044] Let A be the left side of the equation. k+1 , as well as and n z Let E be the dimension of the observation vector. Then the standard error vector can be expressed as: E k+1 =A k+1 -W k+1 x k+1 ;

[0045] Step 3-3: Set initial values ​​for iteration Where t represents the number of iterations.

[0046]

[0047] Set a sufficiently small iteration termination value ε, such as ε = 0.03.

[0048] Step 4 of this invention specifically includes:

[0049] Step 4-1: Start iteration and update the error matrix E k+1 =A k+1 -W k+1 x k+1|k+1,t-1 Update iteration value t = t + 1;

[0050] Step 4-2: Iterate until the following equation is satisfied:

[0051]

[0052] Using the value of the last iteration as the state estimate, we can obtain:

[0053] in:

[0054]

[0055] Step 4-3: Update the state error covariance matrix:

[0056] In order to make the next prediction.

[0057] This invention employs GMCC (Generalized Gaussian Census Function) to measure the generalized correlation entropy of the error in the nonlinear filtering algorithm by introducing a generalized Gaussian density kernel function. The generalized Gaussian kernel function has the advantages of having many parameters, flexible variation, and adaptability. It can achieve the maximum correlation entropy when the error is minimized. Because the generalized Gaussian kernel contains even-numbered higher-order moments of the errors of two variables, it can better handle heavy-tailed noise and large outliers. Experimental verification shows that the technical solution of this invention has better robustness and reliability, and can better handle problems such as outliers and heavy-tailed noise interference. Compared with the traditional CKF (Continuous Calibration Filter), this invention can improve positioning accuracy. Attached image description:

[0058] Appendix Figure 1 This is a schematic diagram of a single beacon-assisted master-slave AUV positioning system in an embodiment of the present invention.

[0059] Appendix Figure 2 This is a comparison chart of the predicted trajectory, the actual trajectory, and the trajectory predicted by traditional methods in an embodiment of the present invention.

[0060] Appendix Figure 3 This is a graph showing the mean square error in the embodiments of the present invention and the mean square error of the traditional method.

[0061] Appendix Figure 4 This is a CDF plot comparing the mean square error in this embodiment of the invention with the mean square error in the traditional method.

[0062] Appendix Figure 5 This is a flowchart of the present invention. Detailed implementation method:

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

[0064] Example:

[0065] While traditional filtering techniques can effectively address Gaussian noise, they struggle with highly nonlinear systems like single-beacon-assisted master-slave AUV cooperative systems, particularly those with large outliers and heavy-tailed noise from complex environments. This paper proposes a single-beacon-assisted AUV cooperative localization method based on a combination of GMCC and CKF. The applied system is as follows: Figure 1 As shown, this is applicable to systems that utilize ranging information between a single beacon and a slave AUV, as well as ranging information between a master AUV and a slave AUV.

[0066] The specific steps in this example are as follows:

[0067] (1) Construct the state equation and observation equation of the single-beacon-assisted master-slave AUV positioning system: The cost function, mixture entropy, and kernel function of GMCC are defined as follows:

[0068] J GMCC (x,y)=E[G α，β (x,y))]=E[γ α,β exp(-λ|e|α)]

[0069]

[0070]

[0071] Where x and y are two input vectors, α is the shape parameter of the kernel function, β is the size parameter of the kernel function, and λ = 1 / β α For kernel parameters, γ α，β =α*[2β·Γ(1 / α)] is the normalization constant, Γ(·) is the gamma function, m is the number of mixing kernel functions, n represents the dimension of the input vector, and σ i The different weights of the kernel function, x j and y j Let G represent the j-th element of x and y respectively. i (x j ,y j ) represents the i-th kernel function.

[0072] Using the information transmission time between a single beacon and the slave AUV, and between the master AUV and the slave AUV, as the observation information z at time k. k for:

[0073]

[0074] Among them, V k This represents observation noise, where t1 is the signal transmission time between the master AUV and the slave AUV, t2 is the signal transmission time between the single beacon and the slave AUV, d1 is the distance between the master AUV and the slave AUV, d2 is the distance between the single beacon and the slave AUV, and c is the distance between the master AUV and the slave AUV. k It is the speed of sound underwater, x k and y k Let x and y represent the x and y coordinates of the AUV at time k, respectively. and Let x0 and y0 represent the x and y coordinates of the main AUV at time k, respectively, and let x0 and y0 represent the x and y coordinates of the beacon, respectively.

[0075] The state information of the AUV will be its x-coordinate, y-coordinate, depth, velocity, heading angle, and underwater sound speed, and the state vector x at time k will be... k for:

[0076] x k =[x,y,z,v,θ,c] T

[0077] Where x is the abscissa of the AUV, y is the ordinate, z is the depth, v is the velocity, θ is the heading angle, and c is the underwater speed of sound;

[0078] The state equations and observation equations are constructed as follows:

[0079]

[0080] Where f(·) represents the state transition function, W k Representing process noise, h(·) is the observation matrix, V k Indicates observation noise;

[0081] Step (2): Using the information at time k, perform a one-step prediction at time k+1 to obtain the state estimate at time k+1:

[0082]

[0083] According to the third-order spherical radial volume criterion of CKF, several volume points can be used to approximate the mean and covariance of the state vector in a nonlinear system. Cholesky decomposition of the state vector yields:

[0084]

[0085] Where, x i,k+1|k The sampling points represent the state vector. i = 1, 2, ..., 2n x n x Let S be the dimension of the state vector x. k|k =chol(P k|k ), where chol(·) represents the cholesky decomposition function.

[0086] For the state prediction of the volume point, based on the one-step prediction updated at time k+1, the one-step predicted state vector can be obtained as follows:

[0087]

[0088] The volumetric point state prediction is as follows:

[0089] The one-step prediction error covariance matrix at time k is:

[0090]

[0091] Q k This represents the process noise matrix.

[0092] Similarly, we can obtain:

[0093]

[0094] in, This indicates a volumetric measurement prediction. P represents the predicted measurement value. xz,k+1|k This represents the cross-covariance matrix of state measurements.

[0095] Step (3): Apply the cost function to x k+1 Differentiation yields the cost function J with respect to the change x. k+1 The maximum point is the optimal estimate. set up:

[0096] G i (x j ,y j ) = G i (x j -y j ) = G i (e j )

[0097] Among them, e j =x j -y j This represents the error vector.

[0098]

[0099] Calculate the error vector, defining the error as a combination of state error and measurement error, where the state error is the difference between the state value and the predicted state value:

[0100]

[0101] Measurement error is the difference between the measured value and the state value after the measurement transformation matrix.

[0102]

[0103] in,

[0104] To calculate the value of the matrix required for the iteration, multiply both sides of the combined error vector by... in get:

[0105]

[0106] Let A be the left side of the equation. k+1 , as well as and n z Let be the dimension of the observation vector. Then the standard error vector can be expressed as:

[0107] E k+1 =A k+1 -W k+1 x k+1 .

[0108] Set initial values ​​for iteration Where t represents the number of iterations.

[0109]

[0110] Set a sufficiently small iteration termination value ε, such as ε = 0.03.

[0111] Step (4): Start iteration and update the error matrix:

[0112] E k+1 =A k+1 -W k+1 x k+1|k+1,t-1

[0113] Update iteration value:

[0114] t = t + 1, iterate until the following expression is satisfied:

[0115]

[0116] Using the value of the last iteration as the state estimate, we can obtain:

[0117]

[0118] in:

[0119]

[0120] Finally, the state error covariance matrix is ​​updated:

[0121]

[0122] In order to make the next prediction.

[0123] The final state estimate is:

[0124]

[0125] Example:

[0126] In this example, the initial position of the main AUV is set to (0, 1000, 250), the initial position of the slave AUV is set to (1595, 1000, 250), the position of the single beacon is set to (0, 1000, 250), and the state vector is: x = (x, y, z, v, θ, c). T The initial value of the state vector is

[0127] x0=(1595,1000,250,1.79,1.56,1500) T The initial position of the single beacon is (0m, 0m, 250m). Heavy tail noise is added to the speed of the AUV and the time delay of the received acoustic information.

[0128] The trajectory comparison curves obtained by locating the AUV using both the traditional CKF and the GMCC-CKF proposed in this invention are shown below. Figure 2 As shown, the trajectory obtained using GMCC-CKF is closer to the actual AUV trajectory than the trajectory obtained using CKF.

[0129] according to Figure 2 The trajectory is shown below, and the relationship between error and time is analyzed. For example... Figure 3 As shown in the figure, the method of the present invention has a smaller position estimation error than CKF, indicating that the performance of the GMCC-CKF method is better than that of the traditional CKF algorithm.

[0130] from Figure 4 The error CDF curves show that only 85% of the errors of the traditional CKF algorithm are within 20m, while the errors of the GMCC-CKF algorithm are all within 20m. This indicates that the method proposed in this invention has a better ability to suppress outliers and handle heavy-tailed noise than CKF.

[0131] This invention employs Generalized Gaussian Census Function (GMCC) to measure the generalized correlation entropy of the nonlinear filtering algorithm's error. The GMCC offers advantages such as numerous parameters, flexible variation, and adaptability, achieving maximum correlation entropy at the minimum error. Furthermore, the inclusion of even-order moments of the two variable errors within the GMCC allows for better handling of heavy-tailed noise and large outliers. Combining GMCC with CKF provides improved robustness and reliability, better addressing outliers and heavy-tailed noise interference. Compared to traditional CKF, this invention enhances positioning accuracy.

Claims

1. A cooperative localization method for AUVs based on generalized maximum correlation entropy and capacitive Kalman filtering, characterized in that, Includes the following steps: Step 1: Construct the state equation and observation equation of the single beacon-assisted master-slave AUV positioning system: where the cost function, mixed entropy, and kernel function of GMCC are defined, the information transmission time between the single beacon and the slave AUV and between the master AUV and the slave AUV are used as the observation information, and the abscissa, ordinate, depth, speed, heading angle and underwater sound speed of the slave AUV are used as the state information. Step 2: Based on the state equation and observation equation of the single-beacon-assisted master-slave AUV positioning system, from... k Information at any time k The one-step prediction at time +1 yields... k State estimate at time +1 and estimate covariance The measurement estimation update and state measurement cross-covariance are performed in the measurement step. Matrix update; Step 3: Set the values ​​of the matrices required for iteration, and set the initial and final values ​​for iteration to obtain the error vector and weight matrix; Step 4: Begin iterating to a stable state, retaining the state vector estimate from the last iteration, and calculate the state vector error covariance matrix. Update.

2. The AUV cooperative localization method based on generalized maximum correlation entropy and capacitive Kalman filtering according to claim 1, characterized in that, Step 1 specifically includes the following steps: Step 1-1: Construct the cost function, mixture entropy, and kernel function as follows: ， ， ， in and These are two input vectors, The shape parameter of the kernel function. The size parameter of the kernel function. For kernel parameters, The normalization constant is For gamma function, The number of mixed kernel functions. This represents the dimension of the input vector. This represents the different weights of the kernel function. and They represent and The One element, Indicates the first One kernel function; Steps 1-2: In Observational information at any time for: ,in, It is observation noise. It is the signal transmission time between the master AUV and the slave AUV. It is the signal transmission time between the single beacon and the AUV. It is the distance between the main AUV and the slave AUV. It is the distance between the single beacon and the AUV. It is the speed of sound underwater. and They represent The x and y coordinates of the time from the AUV, and They represent The x and y coordinates of the main AUV at time [time]. and These represent the x and y coordinates of the beacon, respectively. exist State vector at time step for: ,in, The x-axis is from the AUV. The vertical axis is , For depth, For speed, For heading angle, The speed of sound underwater; Constructing the state equation and observation equation: ， in, Represents the state transition function. Indicates process noise. For the observation matrix, This indicates observation noise.

3. The AUV cooperative localization method based on generalized maximum correlation entropy and capacitive Kalman filtering according to claim 1, characterized in that, Step 2 specifically includes: Step 2-1: From k Information at any time k The one-step prediction at time +1 is as follows: According to the third-order spherical radial volume criterion of CKF, several volume points can be used to approximate the mean and covariance of the state vector in a nonlinear system. The state vector can be obtained by Cholesky decomposition: ， in, , , State vector x Dimensions , This represents the Cholesky decomposition function. State prediction of the volume points is obtained from the one-step prediction updated in time. k The predicted state vector at time +1 is: The volumetric point state prediction is as follows: ; Step 2-2: The one-step prediction error covariance matrix at time t is: ， in, Represents the process noise matrix. Following step 2-1, we can obtain: ， in, This indicates a volumetric measurement prediction. Indicates the predicted measurement value. This represents the cross-covariance matrix of state measurements.

4. The AUV cooperative localization method based on generalized maximum correlation entropy and capacitive Kalman filtering according to claim 1, characterized in that, Step 3 specifically includes: Step 3-1: Pair the cost function with The cost function can be obtained by differentiation. In the amount of change The maximum point is the optimal estimate. ,set up: , in, This is the error vector; ， Step 3-2: Define the error as a combination of state error and measurement error. State error is the difference between the state value and the predicted state value. Measurement error is the difference between the measured value and the state value after the measurement transformation matrix. ,in Then multiply both sides of the combined error vector by... ,in ,get: ， Let the left side of the above equation be . , as well as ,and , Let be the dimension of the observation vector, then the standard error vector can be expressed as: ; Step 3-3: Set initial values ​​for iteration , ,in Indicates the number of iterations. ， Set a sufficiently small iteration termination value , .

5. The AUV cooperative localization method based on generalized maximum correlation entropy and capacitive Kalman filtering according to claim 1, characterized in that, Step 4 specifically includes: Step 4-1: Start iteration and update the error matrix Update iteration value , ; Step 4-2: Iterate until the following equation is satisfied: ， Using the value of the last iteration as the state estimate, we can obtain: ,in: ， Step 4-3: Update the state error covariance matrix: This is so that we can make further predictions.

Images