A method for ultra-short baseline error calibration based on set membership filtering and secondary calibration

Abstract

The present invention discloses an ultra-short baseline error calibration method based on set membership filtering and secondary calibration, comprising the following steps: step 1, setting a shore-based transponder with a shore GNSS locator on the water, and setting an underwater transponder and an underwater locator on the AUV carrier; step 2, obtaining the first relative distance information between the shore-based transponder and the AUV carrier in the navigation coordinate system, and converting the first relative distance information to the AUV carrier coordinate system through the coordinate conversion relationship between the navigation coordinate system and the AUV carrier coordinate system; step 3, constructing a state space equation to solve and obtain the optimal installation deflection angle and the optimal lever arm error; step 4, recursively repeating steps 2 and 3 with a fixed iterative step size until each parameter in the state estimate converges to a stable value, and the stable value is used for error calibration and update. The method provided by the present invention can improve the robustness, accuracy and real-time performance in the real-time calibration process.

Description

Technical Field

[0001] The present invention relates to the field of underwater vehicles and error calibration, in particular to an ultra-short baseline error calibration method based on set membership filtering and secondary calibration. Background Art

[0002] In both military and civilian applications, autonomous underwater vehicles (AUVs) are indispensable underwater vehicles for completing underwater missions, and underwater positioning technology is one of the key technologies to ensure the successful completion of underwater missions. Currently, the application of ultra-short baseline (USBL) underwater positioning technology is becoming increasingly widespread, and the requirements for its positioning accuracy are gradually increasing. The main factors affecting USBL positioning accuracy include system errors and measurement errors caused by ocean factors. Installation error refers to the error caused by the inconsistency between the USBL array coordinate system and the AUV carrier coordinate system, including installation deflection and arm error, and is the main error source in the positioning system. Therefore, it must be accurately calibrated before actual use.

[0003] Currently, USBL calibration primarily involves two methods: online and offline. Representative online calibration methods are based on Kalman filtering or its related extensions. These algorithms are fast, but the abnormal noise introduced by environmental complexity often affects the calibration accuracy. Furthermore, these methods require the statistical characteristics of the noise distribution to be determined before calibration. However, in many cases, the specific distribution of the noise cannot be determined due to insufficient observation data or environmental factors, posing a challenge to many calibration algorithms based on Kalman filtering. Offline calibration algorithms, such as particle swarm optimization, are popular for their high accuracy. However, for situations where the USBL communication range is limited or shore-based hydroacoustic supports can be dynamically rotated, offline calibration methods, which require pre-acquisition of data and calibration of the data, exhibit significant drawbacks. Environmental changes can lead to changes in installation errors, resulting in a lack of real-time performance and a complex calibration process.

[0004] Publication No. CN 115790652A discloses a spatial online calibration method for an odometer / dual-antenna GNSS system. The method includes: selecting an initial attitude error and a lever arm as state variables, establishing and discretizing a Kalman filter state equation; using the difference between the relative position of the odometer and the coordinate-transformed dual-antenna GNSS position at the same moment as a Kalman filter observation, and establishing a Kalman filter measurement equation; then performing Kalman filtering to obtain an initial attitude error estimate and a lever arm estimate; using the initial attitude error estimate as feedback to correct the initial attitude and zero the initial attitude error estimate; then using the initial attitude to align the relative attitude of the odometer to obtain an absolute attitude; then using the dual-antenna GNSS absolute heading and the odometer absolute attitude at the same moment to calculate the installation deflection angle between the dual-antenna GNSS system and the odometer; and repeating the last five steps until the initial attitude and lever arm converge to their respective accuracy. Existing techniques employ Kalman filtering for estimation, which is suitable for linear systems or systems that have undergone linearization and is not effective for nonlinear problems. The estimation results are insufficiently accurate in dynamic environments such as underwater. Summary of the Invention

[0005] To address the problems of insufficient calibration accuracy and inability to calibrate in real time according to environmental changes in existing calibration methods, the present invention provides an ultra-short baseline error calibration method based on set membership filtering and quadratic calibration. The measurement model is predicted and updated through set membership filtering, and quadratic calibration is performed through least squares and gradient descent, thereby improving the robustness, accuracy and real-time performance of the real-time calibration process.

[0006] In order to achieve the purpose of the present invention, the following technical solution is provided: an ultra-short baseline error calibration method based on set membership filtering and secondary calibration, comprising the following steps:

[0007] Step 1: Set up a shore-based transponder with a shore-based GNSS locator on the water, and set up an underwater transponder and underwater locator on the AUV carrier;

[0008] Step 2: Obtain first relative distance information between the shore-based transponder and the AUV carrier in the navigation coordinate system, and convert the first relative distance information to the AUV carrier coordinate system through the coordinate conversion relationship between the navigation coordinate system and the AUV carrier coordinate system;

[0009] Step 3: The shore-based transponder and the onshore GNSS locator are regarded as one, and the first installation error information between the shore-based transponder and the onshore GNSS locator in the navigation coordinate system is accumulated to the second installation error information between the underwater transponder and the AUV carrier coordinate system through the coordinate conversion relationship. The second relative distance information in the AUV carrier coordinate system is obtained through signal transmission between the shore-based transponder and the underwater transponder, and a measurement equation is constructed based on the installation error information and the second relative distance information in the AUV carrier coordinate system;

[0010] Setting state estimates for the measurement equation to construct a state space equation, wherein the state estimates include an installation deflection angle and a lever arm error. The installation deflection angle refers to the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system. The lever arm error refers to the relative distance between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system.

[0011] The first relative distance information in the AUV carrier coordinate system is input into the state space equation, and the solution is obtained by the set membership filter algorithm to output the optimal installation deflection angle. The optimal installation deflection angle is then placed in an empty matrix for dynamic incremental update over time to obtain the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment.

[0012] An optimization equation for the lever arm error is constructed based on the design parameters of the AUV carrier, and the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment is substituted into the optimization equation to solve the corresponding optimal lever arm error;

[0013] Step 4: Perform recursion with a fixed iteration step size, repeating steps 2 to 3 until each parameter in the state estimate converges to a stable value, and update the stable value by calibrating the error.

[0014] The present invention constructs a vector observation model by using the combined navigation information of underwater inertial navigation / Doppler odometer, the relative distance information of the hydroacoustic system and the absolute position information of the shore-based support GPS, and uses the set membership filter collaborative weighted gradient descent method to ensure the robustness, accuracy and real-time performance of the online calibration process.

[0015] Specifically, the expression for updating the error calibration is as follows:

[0016] Specifically, in step 1, the coordinate transformation relationship is expressed as follows:

[0017]

[0018] in, Indicates the position information of the shore-based transponder relative to the navigation coordinate system, Indicates the position information of the AUV carrier relative to the navigation coordinate system, is the relative distance between USBL and shore-based transponder, Represents the direction cosine matrix of the AUV carrier coordinate system relative to the navigation coordinate system; Indicates the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system; L b Represents the relative distance between the underwater transponder and the AUV carrier coordinate system.

[0019] Specifically, the expression of the state space equation is as follows:

[0020]

[0021] Among them, x k =[θ x ,θ y ,θ z ,l x ,l y ,l z ] represents the state quantity at time k, x k+1 represents the state quantity at time k+1, f(x k ) represents the state transfer matrix, z k Indicates measurement information, h k represents a nonlinear function, w k represents process noise, v k Indicates the measurement noise.

[0022] Specifically, the bounding ellipsoid solution is used to constrain the correlated noise to construct the corresponding process noise and measurement noise:

[0023]

[0024] Among them, Q k Represents the shape matrix of the ellipsoid corresponding to the process noise, R k The shape matrix representing the ellipsoid corresponding to the measurement noise.

[0025] Specifically, the set membership filter algorithm includes initialization, state prediction, and measurement update, and its process is as follows:

[0026] An initial ellipsoid set containing the initial values ​​of the state quantities is defined by the center point and the shape matrix, and state prediction and measurement updates are performed based on the initial values ​​of the state quantities at a fixed time step.

[0027] State prediction means predicting the state of the ellipsoid set based on the first state quantity, that is, according to the state transfer function in the state space equation, with the goal of determining the minimum ellipsoid set, solving the second state quantity and the second shape matrix at the next moment to obtain the predicted ellipsoid set;

[0028] The measurement update means updating the predicted ellipsoid set based on the measurement information of the actual measurement, with the goal of determining the minimum ellipsoid set, while making the updated ellipsoid reflect the most actual measurement result, thereby updating the second state quantity and the second morphology matrix.

[0029] Specifically, the process of state prediction is as follows:

[0030] Contains the first state quantity x at time k k The ellipsoid set is represented as:

[0031]

[0032] Among them, P k represents the ellipsoid ε k The shape matrix, is the ellipsoid ε k The center of the k-time state x k , E k Represents the matrix P k Cholesky decomposition of η k Represents a random number less than or equal to 1;

[0033] According to the state quantity x at time k k The ellipsoid set of , the state space equation is converted into the state update equation, which is expressed as:

[0034]

[0035] in, represents the ellipsoid ε k and ellipsoid Minkowski sum, f(x k )=Fx k , F is an identity matrix;

[0036] The state prediction of the center value and shape matrix at time k+1 is expressed as:

[0037]

[0038] Among them, the parameter p k > 0, when there exists a unique minimum ellipsoid containing the Minkowski sum, p k Expressed as:

[0039]

[0040] By obtaining the parameter p k After completing the state prediction at time k+1, the predicted ellipsoid set at time k+1 is expressed as:

[0041]

[0042] Among them, P k+1k Represents the ellipsoid E k+1|k The shape matrix, The ellipsoid E k+1|k The center of the k+1 time contains the state x k+1|k , E k+1|k Represents the matrix P k+1|k Cholesky decomposition of η k+1|k Represents a random number.

[0043] Specifically, the measurement update process is as follows:

[0044] Based on actual measurement information, the minimum shape matrix is ​​obtained Expressed as:

[0045]

[0046] in, Indicates the measurement value of the ellipsoid center, v k+1 Denotes the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Get the minimum shape matrix Expressed as:

[0047] X=repmat(z k+1 ,1,N)-sample(R k ,N);

[0048] Among them, repmat represents the measurement information z k+1 To expand the dimension, sample represents the shape matrix R k Sampling is performed, the corresponding gamma cumulative distribution function value is calculated, the scale of the normal distribution sample is modified to conform to the distribution of the actual measurement information, and the point set matrix X is solved by the first-order Frank-Wolfe method to obtain the minimum shape matrix containing X

[0049] According to the shape matrix P k+1k and the minimum shape matrix Determine the minimum ellipsoid set ε k+1 , we get the measurement update formula, which is expressed as:

[0050]

[0051] in, Parameter ρ k+1 Obtained by the following formula:

[0052]

[0053] The updated ellipsoid set at time k+1 is obtained, which is expressed as:

[0054]

[0055] Among them, P k+1 represents the ellipsoid ε k+1 The shape matrix, is the ellipsoid ε k+1 The center of the k+1 time contains the state x k+1 , E k+1 Represents the matrix P k+1 Cholesky decomposition of η k+1 represents a random number; the ellipsoid ε at time k+1 k+1 The central value of is used as the estimated state quantity, which includes the installation deflection angle estimation and the lever arm error estimation.

[0056] Specifically, the underwater locator refers to underwater inertial navigation / Doppler odometer combined navigation.

[0057] Specifically, the optimization equation is constructed by the constrained least squares method, the lever arm error estimate is used as the initial value of the optimization model, the upper and lower bounds, the maximum number of iterations and the convergence threshold are set, and the gradient descent method is used to solve the optimal lever arm error.

[0058] Specifically, the expression of the optimization equation is as follows:

[0059]

[0060] stlb≤b≤ub;

[0061] Where b represents the lever arm error, lb and ub represent the lower and upper bounds of the lever arm error, A represents an empty matrix, and d represents the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system after the relative distance information is processed by the optimal rotation matrix corresponding to the optimal installation deflection angle.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] Combining the advantages of set membership filtering, least squares and gradient descent methods, the installation deflection angle and arm error of the ultra-short baseline relative to the carrier coordinate system are calibrated, realizing real-time calibration of the ultra-short baseline. This not only overcomes the problem that there are many abnormal values ​​in the observation values ​​that cannot be accurately calibrated, but also ensures that underwater calibration is carried out in real time. It can perform real-time calibration according to the dynamic changes of the environment and the changes in the communication range of the ultra-short baseline, thereby improving the robustness, accuracy and real-time performance of the error calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 Schematic diagram of the ultra-short baseline underwater acoustic positioning principle provided in this embodiment;

[0065] Figure 2 A schematic diagram of installation errors provided for this embodiment;

[0066] Figure 3 Flowchart of the ultra-short baseline error calibration method provided in this embodiment;

[0067] Figure 4 A comparison chart of the estimation results of different algorithms over time when the measurement noise provided by this embodiment is Gaussian noise;

[0068] Figure 5 A comparison chart showing the time-varying estimation results of different algorithms when the measurement noise provided in this embodiment is non-Gaussian noise;

[0069] Figure 6 This is a comparison chart of the estimation results of the two types of noise in Table 1 provided in this embodiment. DETAILED DESCRIPTION

[0070] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0071] In the ultra-short baseline underwater acoustic positioning mission, the AUV carrier coordinate system, the underwater locator navigation coordinate system, the acoustic system coordinate system constructed by the underwater transponder on the AUV carrier and the shore-based transponder (subsequently expressed using USBL) and the earth coordinate system are included. The positional relationship of each coordinate system is as follows: Figure 1 shown.

[0072] The underwater locator in this embodiment adopts underwater inertial navigation and Doppler odometer combined navigation.

[0073] During the positioning process, the structural characteristics of the AUV carrier and the installation position of the sensor will cause positioning errors. Therefore, the installation error needs to be calibrated during the actual positioning process. Figure 2 shown.

[0074] In order to solve the above technical problems, this embodiment provides Figure 3 The ultra-short baseline error calibration method shown includes the following steps:

[0075] Step 1: Set up a shore-based transponder with a shore-based GNSS locator on the water, and set up an underwater transponder and a Northeast Sky navigation locator on the AUV carrier;

[0076] Step 2: Obtain first relative distance information between the shore-based transponder and the AUV carrier in the navigation coordinate system, and convert the first relative distance information to the AUV carrier coordinate system through the coordinate conversion relationship between the navigation coordinate system and the AUV carrier coordinate system;

[0077] Step 3: The shore-based transponder and the onshore GNSS locator are regarded as one, and the first installation error information between the shore-based transponder and the onshore GNSS locator in the navigation coordinate system is accumulated to the second installation error information between the underwater transponder and the AUV carrier coordinate system through the coordinate conversion relationship. The second relative distance information in the AUV carrier coordinate system is obtained through signal transmission between the shore-based transponder and the underwater transponder, and a measurement equation is constructed based on the second installation error information and the second relative distance information in the AUV carrier coordinate system.

[0078] Setting state estimates for the measurement equation to construct a state space equation, wherein the state estimates include an installation deflection angle and a lever arm error. The installation deflection angle refers to the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system. The lever arm error refers to the relative distance between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system.

[0079] The first relative distance information in the AUV carrier coordinate system is input into the state space equation, and the solution is obtained by the set membership filter algorithm to output the optimal installation deflection angle. The optimal installation deflection angle is then placed in an empty matrix for dynamic incremental update over time to obtain the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment.

[0080] An optimization equation for the lever arm error is constructed based on the design parameters of the AUV carrier, and the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment is substituted into the optimization equation to solve the corresponding optimal lever arm error;

[0081] Step 4: Perform recursion with a fixed iteration step size, repeating steps 2 to 3 until each parameter in the state estimate converges to a stable value, and update the stable value by calibrating the error.

[0082] The expression for updating the error calibration is as follows:

[0083] Furthermore, the coordinate transformation relationship between the navigation coordinate system, the AUV carrier coordinate system and the USBL coordinate system is expressed as follows:

[0084]

[0085] in, Indicates the position information of the shore-based transponder relative to the navigation coordinate system, Indicates the position information of the AUV carrier relative to the navigation coordinate system, is the relative distance between USBL and shore-based transponder, Represents the direction cosine matrix of the AUV carrier coordinate system relative to the navigation coordinate system; Indicates the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system; L b Indicates the relative distance between the underwater transponder and the underwater GNSS locator.

[0086] The subsequent measurement equation is expressed as follows:

[0087]

[0088] Where x is the state variable, h(x) represents a nonlinear function that describes how the measurement information changes according to the state variable, and v represents noise or error.

[0089] Set the state estimator, which includes the installation deflection angle expressed as θ = [θ x ,θ y ,θ z ], and its corresponding direction cosine matrix is The lever arm error is expressed as L b =[l x ,l y ,l z ], and combined with the above measurement equation to construct the state space equation, its expression is as follows:

[0090]

[0091] Among them, x k =[θ x ,θ y ,θ z ,l x ,l y ,l z ] represents the state quantity at time k, x k+1 represents the state quantity at time k+1, f(x k ) represents the state transfer matrix, z k Indicates measurement information, h k Represents a nonlinear function. In this embodiment, h kTake an identity matrix I, w k represents process noise, v k Indicates the measurement noise.

[0092] Due to the complex underwater environment during the calibration process, the USBL positioning data has many abnormal values. In addition, the USBL observation data is not sufficient to determine the statistical characteristics of the noise and the specific distribution of the noise. Therefore, in this embodiment, it is assumed that the process noise w k and measurement noise v k

[0093] In order to constrain the state of the correlated noise, the process noise w in the state space equation k and measurement noise v k The following boundary conditions are met:

[0094]

[0095] Among them, Q k Represents the shape matrix of the ellipsoid corresponding to the process noise, R k The shape matrix representing the ellipsoid corresponding to the measurement noise.

[0096] The set membership filter algorithm proposed in this embodiment includes initialization, state prediction, and measurement update, and its process is as follows:

[0097] Define an initial ellipsoid set with a given boundary, expressed as:

[0098]

[0099] in, is the center of the ellipsoid ε0, including the initial value of the state quantity x0, P0 represents the shape matrix of the ellipsoid ε0, which is a symmetric positive definite matrix, R n represents the set of real numbers.

[0100] In this embodiment, in order to meet the initial value and noise bounded conditions, the following initial shape matrices P0, R0 and Q0 are set:

[0101]

[0102] And the state space equation:

[0103] Among them, x k =[θ x ,θ y ,θ z ,l x ,l y ,l z ] represents the three installation deflections and arm errors in three directions of the ultra-short baseline to be estimated.

[0104] Based on the initial ellipsoid set, after state prediction and measurement update at a certain time step, the state quantity x at time k is obtained. k The ellipsoid set is expressed as:

[0105]

[0106] Among them, P k represents the ellipsoid ε k The shape matrix, is the ellipsoid ε k The center of the k-time state x k , E k Represents the matrix P k Cholesky decomposition of η k Represents a random number.

[0107] According to the state quantity x at time k k The ellipsoid set of , the state space equation is converted into a state update equation for state prediction, which is expressed as:

[0108]

[0109] in, represents the ellipsoid ε k and ellipsoid Minkowski sum; in the state update process, the state update equation is linear, so f(x k ) can be expressed as:

[0110] Based on the ellipsoid ε at time k k The predicted ellipsoid set at time k+1 is obtained by combining the state transfer function in the state space equation. The process is as follows:

[0111] The state prediction of the center value and shape matrix at time k+1 can be expressed by the following equation:

[0112]

[0113] Among them, the parameter p k > 0, when there exists a unique minimum ellipsoid containing the Minkowski sum, p k Expressed as:

[0114] In this way, the state prediction at time k+1 is completed, and the predicted ellipsoid set at time k+1 is expressed as:

[0115]

[0116] Among them, Pk+1k Represents the ellipsoid E k+1|k The shape matrix, The ellipsoid E k+1|k The center of the k+1 time contains the state x k+1|k , E k+1|k Represents the matrix P k+1|k Cholesky decomposition of η k+1|k express.

[0117] The predicted ellipsoid set at time k+1 is further updated based on the actual measurement information and the established measurement equation. The process is as follows:

[0118] For actual measurement information, considering the uncertainty of measurement noise, find the minimum shape matrix that can represent the measurement information That is, solve the following semi-infinite optimization problem:

[0119]

[0120] in, Indicates the measurement ellipsoid center value, v k+1 Denotes the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Nonlinear transformation of the covering ellipsoid set, solving the minimum shape matrix Expressed as:

[0121] X=repmat(z k+1 ,1,N)-sample(R k ,N);

[0122] Among them, repmat represents the measurement information z k+1 Expand the dimension to make it a 3×N matrix; sample represents the shape matrix R k Sampling is performed, the corresponding gamma cumulative distribution function value is calculated, the scale of the normal distribution sample is modified to conform to the distribution of the actual measurement information, and the point set matrix X is solved by the first-order Frank-Wolfe method to obtain the minimum shape matrix containing X

[0123] According to the shape matrix P k+1k and the minimum shape matrix Determine the minimum ellipsoid set ε k+1 , expressed as:

[0124]

[0125] Further expressed as:

[0126]

[0127] in, ρ k+1 The parameters can be obtained by solving the following problem using the golden section method:

[0128]

[0129] The above formula can be expressed as the measurement update formula:

[0130]

[0131] The solved ρ k+1 Substituting the parameters into the measurement update formula (16), we get the updated ellipsoid set at time k+1, which is expressed as:

[0132]

[0133] Among them, P k+1 represents the ellipsoid ε k+1 The shape matrix, is the ellipsoid ε k+1 The center of the k+1 time contains the state x k+1 , E k+1 Represents the matrix P k+1 Cholesky decomposition of η k+1 Represents a random number.

[0134] Ellipsoid ε k+1 The central value of is used as the estimated state quantity, which includes the installation deflection angle estimation and the lever arm error estimation.

[0135] Step 4: Construct a lever arm error optimization model, perform secondary calibration on the lever arm error estimation, and obtain the optimal lever arm error;

[0136] Since the arm error is highly dependent on the measurement information, the uncertainty of the measurement information will have a significant impact on the arm error estimation result. The upper and lower bounds of the shape matrix set in S3 to ensure robustness are large, resulting in the arm error estimation result being more dependent on the state prediction step. Although stable, it may cause the estimation result to fluctuate around the initial value of the arm error, while the installation deflection angle is unaffected. Therefore, when the installation deflection angle estimation result is known, considering the physical structure information of the AUV, the quadratic calibration of the arm error is transformed into solving a constrained least squares problem, which can be expressed as:

[0137]

[0138] Where b represents the arm error, lb and ub represent the lower and upper bounds of the arm error, respectively. The first information matrix A is used to store the rotation matrix formed by the installation deflection angle estimation. After using the rotation matrix to perform coordinate transformation, the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system is obtained and stored in the second information matrix d. The first information matrix A and the second information matrix d are dynamically updated with the time step, and are expressed as follows:

[0139]

[0140] Where, c(*)=cos(*), s(*)=sin(*), It represents the difference between the position information obtained by the combined navigation of inertial navigation and Doppler odometer and the position information obtained by the shore-based transponder GPS, that is, the relative distance between the combined navigation and shore-based transponder in the carrier coordinate system. Indicates the relative distance information between USBL and the shore-based transponder in the USBL coordinate system.

[0141] The process of solving formula (17) by gradient descent method to obtain the optimal lever arm error is as follows:

[0142] Use the lever arm error estimate b obtained in S3 as the initial value V prev =0.5||Ab-d|| 2 , calculate the initial error, and initialize a weight matrix W = I(n), where I is the identity matrix and n is the length of the first information matrix A. Set the lower and upper bounds lb and ub, the maximum number of iterations and the convergence threshold τ, and perform iterative solution through the gradient descent method.

[0143] Based on the weight matrix W and the first information matrix A and the second information matrix d, the objective function gradient is calculated:

[0144] grad = A T W(Ab-d) (19)

[0145] The lever arm error b is updated based on formula (19) and is expressed as:

[0146] b=b-α×grad (20)

[0147] Among them, α represents the iteration step size;

[0148] Project the updated lever arm error onto the constraint set and calculate the error value:

[0149]

[0150] The weight matrix W is adaptively updated through the residual information, which is expressed as:

[0151] W = Diag(1 / (abs(Ab-d) + 10 -8 )) (twenty two)

[0152] Determine whether the convergence condition |V is met curr -V prev |<τ, if it is satisfied, then exit the iteration loop, if not, then exit the loop until the maximum number of iterations is reached, and get the optimal lever arm error b=[b x ,b y ,b z ].

[0153] In order to better illustrate the effect of the technical solution provided by this embodiment, the final results estimated by the KF, UKF, PSO algorithms and the method proposed in the present invention are compared. The results are shown in Table 1. It can be seen that compared with the online calibration algorithm, the method proposed in the present invention can ensure robustness and high precision when measuring noise anomalies. Compared with the offline calibration algorithm, the method proposed in the present invention can not only ensure real-time performance, but also achieve calibration accuracy similar to that of the particle swarm optimization algorithm.

[0154] In addition, if Figure 4 The figure shows the estimated trend changes over time of the correlation algorithm proposed in the present invention, the Kalman filter algorithm (KF), and the unscented Kalman filter algorithm (UKF) when the measurement noise is Gaussian noise.

[0155] like Figure 5 The figure shows the estimated trend changes over time of the correlation algorithm proposed in the present invention, the Kalman filter algorithm (KF), and the unscented Kalman filter algorithm (UKF) when the measurement noise is non-Gaussian noise.

[0156] according to Figure 4 and Figure 5 It can be seen from the content that for the measurement information of USBL relative distance, whether it is Gaussian noise or non-Gaussian noise with outliers, the method proposed in the present invention can ensure robustness.

[0157] Table 1

[0158]

[0159] like Figure 6 The relationship between each estimated value and the corresponding reference value shown can clearly show that the method provided by this embodiment has high accuracy under different noise conditions.

[0160] In summary, the present invention combines the advantages of set membership filtering with least squares and gradient descent methods to calibrate the installation deflection angle and arm error of the ultra-short baseline relative to the carrier coordinate system, thereby realizing real-time calibration of the ultra-short baseline. It not only overcomes the problem that there are many abnormal values ​​in the observation values ​​that cannot be accurately calibrated, but also ensures that underwater calibration is carried out in real time. It can perform real-time calibration according to the dynamic changes of the environment and the changes in the communication range of the ultra-short baseline, thereby improving the robustness, accuracy and real-time performance of the error calibration.

[0161] In addition, the terms "upper", "lower", "inner", "outer", "front", and "back" are used for descriptive purposes only and should not be understood as indicating or implying relative importance. Unless otherwise specifically stated, the relative steps, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the present invention.

[0162] Of course, the above description is only a specific embodiment of the present invention and is not intended to limit the scope of implementation of the present invention. Any equivalent changes or modifications made based on the structure, features and principles described in the scope of the patent application of the present invention should be included in the scope of the patent application of the present invention.

[0163] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the above-described embodiments, those skilled in the art should understand that any person skilled in the art can modify or easily conceive of changes to the technical solutions described in the above-described embodiments within the technical scope disclosed by the present invention, or replace some of the technical features therein with equivalents. Such modifications, changes, or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. An ultra-short baseline error calibration method based on set membership filtering and secondary calibration, characterized in that: The following steps are involved: Step 1: Set up a shore-based transponder with a shore-based GNSS locator on the water, and set up an underwater transponder and underwater locator on the AUV carrier; Step 2: Obtain first relative distance information between the shore-based transponder and the AUV carrier in the navigation coordinate system, and convert the first relative distance information to the AUV carrier coordinate system through the coordinate conversion relationship between the navigation coordinate system and the AUV carrier coordinate system; Step 3: The shore-based transponder and the onshore GNSS locator are regarded as one, and the first installation error information between the shore-based transponder and the onshore GNSS locator in the navigation coordinate system is accumulated to the second installation error information between the underwater transponder and the AUV carrier coordinate system through the coordinate conversion relationship. The second relative distance information in the AUV carrier coordinate system is obtained through signal transmission between the shore-based transponder and the underwater transponder, and a measurement equation is constructed based on the second installation error information and the second relative distance information in the AUV carrier coordinate system. Setting state estimates for the measurement equation to construct a state space equation, wherein the state estimates include an installation deflection angle and a lever arm error. The installation deflection angle refers to the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system. The lever arm error refers to the relative distance between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system. The first relative distance information in the AUV carrier coordinate system is input into the state space equation, and the solution is obtained by the set membership filter algorithm to output the optimal installation deflection angle. The optimal installation deflection angle is then placed in an empty matrix for dynamic incremental update over time to obtain the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment. An optimization equation for the lever arm error is constructed based on the design parameters of the AUV carrier, and the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system at each moment is substituted into the optimization equation to solve the corresponding optimal lever arm error; Step 4: Perform recursion with a fixed iteration step size, repeating steps 2 to 3 until each parameter in the state estimate converges to a stable value, and update the stable value by calibrating the error.

2. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 1 is characterized in that: In step 1, the coordinate transformation relationship is expressed as follows: in, Indicates the position information of the shore-based transponder relative to the navigation coordinate system, Indicates the position information of the AUV carrier relative to the navigation coordinate system, is the relative distance between USBL and shore-based transponder, Represents the direction cosine matrix of the AUV carrier coordinate system relative to the navigation coordinate system; Indicates the conversion relationship between the ultra-short baseline coordinate system where the underwater transponder is located and the AUV carrier coordinate system; L b Represents the relative distance between the underwater transponder and the AUV carrier coordinate system.

3. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 1, characterized in that: The state space equation is expressed as follows: Among them, x k =[θ x ,θ y ,θ z ,l x ,l y ,l z ] represents the state quantity at time k, x k+1 represents the state quantity at time k+1, f(x k ) represents the state transfer matrix, z k Indicates measurement information, h k represents a nonlinear function, w k represents process noise, v k Indicates the measurement noise.

4. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 3, characterized in that: The bounding ellipsoid solution is used to constrain the correlated noise to construct the corresponding process noise and measurement noise: Among them, Q k Represents the shape matrix of the ellipsoid corresponding to the process noise, R k The shape matrix representing the ellipsoid corresponding to the measurement noise.

5. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 1, characterized in that: The ensemble membership filter algorithm includes initialization, state prediction, and measurement update, and its process is as follows: An initial ellipsoid set containing the initial values ​​of the state quantities is defined by the center point and the shape matrix, and state prediction and measurement updates are performed based on the initial values ​​of the state quantities at a fixed time step. State prediction means predicting the state of the ellipsoid set based on the first state quantity, that is, according to the state transfer function in the state space equation, with the goal of determining the minimum ellipsoid set, solving the second state quantity and the second shape matrix at the next moment to obtain the predicted ellipsoid set; The measurement update means updating the predicted ellipsoid set based on the measurement information of the actual measurement, with the goal of determining the minimum ellipsoid set, while making the updated ellipsoid reflect the most actual measurement result, thereby updating the second state quantity and the second morphology matrix.

6. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 5, characterized in that: The process of state prediction is as follows: Contains the first state quantity x at time k k The ellipsoid set is represented as: Among them, P k represents the ellipsoid ε k The shape matrix, is the ellipsoid ε k The center of the k-time state x k , E k Represents the matrix P k Cholesky decomposition of η k Represents a random number less than or equal to 1; According to the state quantity x at time k k The ellipsoid set of , the state space equation is converted into the state update equation, which is expressed as: in, represents the ellipsoid ε k and ellipsoid W k Minkowski sum, f(x k )=Fx k , F is an identity matrix; The state prediction of the center value and shape matrix at time k+1 is expressed as: Among them, the parameter p k > 0, when there exists a unique minimum ellipsoid containing the Minkowski sum, p k Expressed as: By obtaining the parameter p k After completing the state prediction at time k+1, the predicted ellipsoid set at time k+1 is expressed as: Among them, P k+1|k Represents the ellipsoid E k+1|k The shape matrix, The ellipsoid E k+1|k The center of the k+1 time contains the state x k+1|k , E k+1|k Represents the matrix P k+1|k Cholesky decomposition of η k+1|k Represents a random number.

7. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 5, characterized in that: The measurement update process is as follows: Based on actual measurement information, the minimum shape matrix is ​​obtained Expressed as: in, Indicates the measurement ellipsoid center value, v k+1 Denotes the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Get the minimum shape matrix Expressed as: X=repmat(z k+1 ,1,N)-sample(R k ,N) Among them, repmat represents the measurement information z k+1 To expand the dimension, sample represents the shape matrix R k Sampling is performed, the corresponding gamma cumulative distribution function value is calculated, the scale of the normal distribution sample is modified to conform to the distribution of the actual measurement information, and the point set matrix X is solved by the first-order Frank-Wolfe method to obtain the minimum shape matrix containing X According to the shape matrix P k+1|k and the minimum shape matrix Determine the minimum ellipsoid set ε k+1 , we get the measurement update formula, which is expressed as: in, Parameter ρ k+1 Obtained by the following formula: The updated ellipsoid set at time k+1 is obtained, which is expressed as: Among them, P k+1 represents the ellipsoid ε k+1 The shape matrix, is the ellipsoid ε k+1 The center of the k+1 time contains the state x k+1 , E k+1 Represents the matrix P k+1 Cholesky decomposition of η k+1 represents a random number; the ellipsoid ε at time k+1 k+1 The central value of is used as the estimated state quantity, which includes the installation deflection angle estimation and the lever arm error estimation.

8. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 1, characterized in that: The underwater locator refers to the combined navigation of underwater inertial navigation and Doppler odometer.

9. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 1, characterized in that: The expression of the optimization equation is as follows: Where b represents the lever arm error, lb and ub represent the lower and upper bounds of the lever arm error, A represents an empty matrix, and d represents the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system after the relative distance information is processed by the optimal rotation matrix corresponding to the optimal installation deflection angle.

Images