Ultra-short baseline error calibration method based on set membership filtering and secondary calibration

Abstract

The invention discloses an ultra-short baseline error calibration method based on set membership filtering and secondary calibration, and the method comprises the following steps: 1, arranging a shore-based transponder with a shore GNSS positioner on water, and arranging an underwater transponder and an underwater positioner on an AUV carrier; step 2, acquiring first relative distance information between the shore-based transponder and the AUV carrier under the navigation coordinate system, and converting the first relative distance information to the AUV carrier coordinate system through a coordinate conversion relation between the navigation coordinate system and the AUV carrier coordinate system; step 3, constructing a state-space equation to solve and obtain an optimal installation deflection angle and an optimal lever arm error; and step 4, recursion is carried out with a fixed iteration step length, the step 2 to the step 3 are repeated until each parameter in the state estimator is converged to a stable value, and the stable value is used for updating error calibration. The method provided by the invention can improve the robustness, accuracy and real-time performance in the real-time calibration process.

Description

Technical Field

[0001] The 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 the military and civilian fields, autonomous underwater vehicles (AUVs) are indispensable underwater carriers for completing underwater missions, and underwater positioning technology is one of the key technologies to ensure the smooth completion of underwater missions. At present, the application of ultra-short baseline (USBL) underwater positioning technology is becoming more and more widespread, and the requirements for its positioning accuracy are gradually increasing. The main factors affecting the positioning accuracy of USBL include system errors and measurement errors caused by ocean factors. Among them, the 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, which is the main error source in the positioning system. Therefore, it must be accurately calibrated before actual use.

[0003] At present, the calibration of USBL mainly includes two methods: online calibration and offline calibration. The most representative online calibration method is based on Kalman filtering or its related extended algorithm. The algorithm is fast, but the abnormal noise brought to USBL by environmental complexity often affects its calibration accuracy. At the same time, the above method needs to determine the statistical characteristics of the noise distribution in advance before calibration. However, in many cases, due to insufficient observation data or environmental factors, the specific distribution of noise cannot be determined, which poses a challenge to many calibration algorithms based on Kalman filtering. Offline calibration algorithms are popular for their high calibration accuracy, such as particle swarm optimization algorithms. However, for situations where the USBL communication range is limited or the shore-based hydroacoustic bracket can rotate dynamically, offline calibration requires obtaining data in advance and calibrating the data processing. Such methods show huge defects. Changes in the environment lead to changes in installation errors. It is not real-time and the calibration process is complicated.

[0004] The specification with the publication number of CN 115790652A discloses an odometer / dual-antenna GNSS space online calibration method, including: selecting the initial attitude error and the lever arm as state quantities, establishing and discretizing the Kalman filter state equation; taking the difference between the relative position of the odometer at the same time and the position of the dual-antenna GNSS after coordinate transformation as the Kalman filter observation quantity, and establishing the Kalman filter measurement equation; then performing Kalman filtering to obtain the initial attitude error estimate and the lever arm estimate; using the initial attitude error estimate feedback to correct the initial attitude and set the initial attitude error estimate to zero; then using the initial attitude to align the relative attitude of the odometer to obtain the absolute attitude; then using the dual-antenna GNSS absolute heading at the same time and the odometer absolute attitude to calculate the installation deflection of the dual-antenna GNSS and the odometer; repeating the last 5 steps until the initial attitude and the lever arm converge to their respective accurate quantities. The prior art uses the Kalman filter method for estimation, which is suitable for linear systems or systems that have been linearized, and cannot be effectively used to deal with nonlinear problems. In an environment with large dynamic changes such as underwater, there is a problem of insufficient accuracy of the estimation results. Summary of the invention

[0005] In order to solve 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 secondary calibration. The measurement model is predicted and updated through set membership filtering, and secondary calibration is performed through least squares and gradient descent to improve 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: Setting a shore-based transponder with a shore-based GNSS locator on the water, and setting an underwater transponder and underwater locator on the AUV carrier;

[0008] Step 2: obtaining 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;

[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, and 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 a state estimate for the measurement equation to construct a state space equation, wherein the state estimate includes an installation deflection angle and a lever arm error, wherein the installation deflection angle refers to a conversion relationship between an ultra-short baseline coordinate system where the underwater transponder is located and an AUV carrier coordinate system, and the lever arm error refers to a 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 optimal installation deflection angle is output by solving it through the set membership filter algorithm, and the optimal installation deflection angle is put into the empty matrix to be dynamically updated 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: recursively perform steps 2 to 3 with a fixed iteration step size until each parameter in the state estimate converges to a stable value, and the stable value is used to calibrate the error for update.

[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 GPS absolute position information of the shore-based bracket, 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 the process noise, v k Represents 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 the process is as follows:

[0026] An initial ellipsoid set containing initial values ​​of state quantities is defined through the center point and shape matrix, and state prediction and measurement update are performed based on the initial values ​​of 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, and obtaining the predicted ellipsoid set;

[0028] The measurement update means updating the predicted ellipsoid set based on the measurement information of the actual measurement, so as to determine the minimum ellipsoid set as the goal, and at the same time make the updated ellipsoid reflect the most actual measurement result, so as to update the second state quantity and the second morphology matrix.

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

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

[0031]

[0032] Among them, P k Represents the ellipsoid ε k A matrix of the shape, is the ellipsoid ε k The center of , including the state quantity x at time k 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 transforms the state space equation into the state update equation, which is expressed as:

[0034]

[0035] in, Represents the ellipsoid ε k and ellipsoid The Minkowski sum, f(x k )=Fx k , F is a unit 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 It is 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 A matrix of the shape, The ellipsoid E k+1|k The center of the k+1 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 the actual measurement information, the minimum shape matrix is ​​obtained It is expressed as:

[0045]

[0046] in, Indicates the measurement value of the ellipsoid center, v k+1 represents the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Get the minimum shape matrix It is 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, 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 A matrix of the shape, is the ellipsoid ε k+1 The center of the k+1 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 taken 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 constrained least squares method, the lever arm error estimation 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 optimal lever arm error is solved by gradient descent method.

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

[0059]

[0060] stlb≤b≤ub;

[0061] Wherein, 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 method, the installation deflection angle and arm error of the ultra-short baseline relative to the carrier coordinate system are calibrated, realizing the 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 the 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 A schematic diagram of the ultra-short baseline hydroacoustic positioning principle provided in this embodiment;

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

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

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

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

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

[0070] In order to make the purpose, technical scheme and advantages of the embodiments of the present invention clearer, the technical scheme 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 here 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 claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments in 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 hydroacoustic 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 in 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 the combined navigation of underwater inertial navigation and Doppler odometer.

[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 An ultra-short baseline error calibration method shown includes the following steps:

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

[0076] Step 2: obtaining 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;

[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, and the second relative distance information in the AUV carrier coordinate system is obtained through the signal transmission between the shore-based transponder and the underwater transponder, and the 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 a state estimate for the measurement equation to construct a state space equation, wherein the state estimate includes an installation deflection angle and a lever arm error, wherein the installation deflection angle refers to a conversion relationship between an ultra-short baseline coordinate system where the underwater transponder is located and an AUV carrier coordinate system, and the lever arm error refers to a 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 optimal installation deflection angle is output by solving it through the set membership filter algorithm, and the optimal installation deflection angle is put into the empty matrix to be dynamically updated 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: recursively perform steps 2 to 3 with a fixed iteration step size until each parameter in the state estimate converges to a stable value, and the stable value is used to calibrate the error for update.

[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 represented by θ = [θ 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 the process noise, v k Represents the measurement noise.

[0092] Due to the complex underwater environment during the calibration process, the USBL positioning data has many outliers. In addition, the USBL observation data is not sufficient to determine the statistical characteristics of the noise, and the specific distribution of the noise cannot be determined. 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, contains the initial value of the state quantity x0, P0 represents the shape matrix of the ellipsoid ε0, 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] The state space equation is:

[0103] Among them, x k =[θ x ,θ y ,θ z ,l x ,l y ,l z ] represents the three installation angles 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 A matrix of the shape, is the ellipsoid ε k The center of , including the state quantity x at time k 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 converts the state space equation into the 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 using 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 It is 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 A matrix of the shape, The ellipsoid E k+1|k The center of the k+1 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 the actual measurement information, considering the uncertainty of the 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 represents the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Nonlinear transformation of the set of covering ellipsoids, solving for the minimum shape matrix It is 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 problems 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, expressed as:

[0132]

[0133] Among them, P k+1 Represents the ellipsoid ε k+1 A matrix of the shape, is the ellipsoid ε k+1 The center of the k+1 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 taken 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, which is stable but may cause the estimation result to fluctuate around the initial value of the arm error, while the installation deflection angle is not affected. Therefore, when the installation deflection angle estimation result is known, the physical structure information of the AUV is considered, and the secondary calibration of the arm error is transformed into solving a constrained least squares problem, which can be expressed as:

[0137]

[0138] Wherein, b represents the lever arm error, lb and ub represent the lower and upper bounds of the lever arm error, respectively; the first information matrix A is used to store the rotation matrix formed by the estimated installation deflection angle, and the difference between the first relative distance and the second relative distance in the ultra-short baseline coordinate system is obtained after the coordinate transformation using the rotation matrix, and is 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 respectively expressed as:

[0139]

[0140] Among them, c(*)=cos(*), s(*)=sin(*), It represents the difference between the position information obtained by the combined navigation of inertial navigation and Doppler log and the position information obtained by the shore-based transponder GPS, that is, the relative distance between the combined navigation and the shore-based transponder in the carrier coordinate system. It 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 unit matrix, 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 to the constraint set and calculate the error value:

[0149]

[0150] The weight matrix W is adaptively updated through the residual information, 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, the iteration loop is exited, if not, the loop is exited until the maximum number of iterations is reached, and 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 in 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, and 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 change of the correlation algorithm proposed by the present invention, the Kalman filter algorithm (KF) and the unscented Kalman filter algorithm (UKF) over time when the measurement noise is Gaussian noise.

[0155] like Figure 5 The figure shows the estimated trend change of the correlation algorithm proposed by the present invention, the Kalman filter algorithm (KF) and the unscented Kalman filter algorithm (UKF) over time 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 noises.

[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 error calibration.

[0161] In addition, the terms "upper", "lower", "inner", "outer", "front", "rear" 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. All equivalent changes or modifications made according to the structure, characteristics and principles described in the patent application scope of the present invention should be included in the patent application scope of the present invention.

[0163] Finally, it should be noted that the above-described embodiments are only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention is described in detail with reference to the above-described embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the above-described embodiments within the technical scope disclosed by the present invention, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope 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: Setting a shore-based transponder with a shore-based GNSS locator on the water, and setting an underwater transponder and underwater locator on the AUV carrier; Step 2: obtaining 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: 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, and 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 a state estimate for the measurement equation to construct a state space equation, wherein the state estimate includes an installation deflection angle and a lever arm error, wherein the installation deflection angle refers to a conversion relationship between an ultra-short baseline coordinate system where the underwater transponder is located and an AUV carrier coordinate system, and the lever arm error refers to a 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 optimal installation deflection angle is output by solving it through the set membership filter algorithm, and the optimal installation deflection angle is put into the empty matrix to be dynamically updated 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: recursively perform steps 2 to 3 with a fixed iteration step size until each parameter in the state estimate converges to a stable value, and the stable value is used to calibrate the error for update.

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 is 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 the process noise, v k Represents the measurement noise.

4. The ultra-short baseline error calibration method based on set membership filtering and secondary calibration according to claim 3 is 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 set membership filter algorithm includes initialization, state prediction and measurement update, and its process is as follows: An initial ellipsoid set containing initial values ​​of state quantities is defined through the center point and shape matrix, and state prediction and measurement update are performed based on the initial values ​​of 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, and obtaining the predicted ellipsoid set; The measurement update means updating the predicted ellipsoid set based on the measurement information of the actual measurement, so as to determine the minimum ellipsoid set as the goal, and at the same time make the updated ellipsoid reflect the most actual measurement result, so as to update 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 x at time k k The ellipsoid set is represented as: Among them, P k Represents the ellipsoid ε k A matrix of the shape, is the ellipsoid ε k The center of , including the state quantity x at time k 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 transforms the state space equation into the state update equation, which is expressed as: in, Represents the ellipsoid ε k and the ellipsoid W k The Minkowski sum, f(x k )=Fx k , F is a unit 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 It is 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+1k Represents the ellipsoid E k+1|k A matrix of the shape, The ellipsoid E k+1|k The center of the k+1 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 the actual measurement information, the minimum shape matrix is ​​obtained It is expressed as: in, Indicates the measurement ellipsoid center value, v k+1 represents the uncertainty measurement noise, combined with the measurement noise v k The shape of the matrix R k Get the minimum shape matrix It is 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+1k and the minimum shape matrix Determine the minimum ellipsoid set ε k+1 , we get the measurement update formula, 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 A matrix of the shape, is the ellipsoid ε k+1 The center of the k+1 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 taken 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 log.

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: Wherein, 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