USBL system installation deviation angle calibration method based on iterative reweighted two-step method

Abstract

The application discloses a USBL system installation deviation angle calibration method based on an iterative reweighted two-step method, and on the basis of a traditional two-step method, estimates a transponder position and an installation deviation angle in the same closed loop iteration process, so that position error and installation deviation angle error are mutually corrected, thereby weakening the error transmission problem in the traditional two-step method; in view of the problem that different measurement points have different qualities, a comprehensive weight is constructed for each measurement point to suppress the influence of abnormal values, gross error points and measurement points with poor geometric conditions; through the comprehensive weight, the contribution of different measurement points to position updating and installation deviation angle updating is no longer equal, thereby improving the overall estimation stability and robustness; in each iteration, the comprehensive weight is used to construct a weighted least square updating model of the transponder position and a weighted nonlinear least square updating model of the installation deviation angle, so that the alternating feedback structure and the reweighting mechanism are truly coupled, and a closed loop calibration method capable of stable convergence is formed.

Description

Technical Field

[0001] This invention relates to the field of underwater acoustic positioning and integrated navigation technology, and in particular to a USBL system installation deviation angle calibration method based on an iterative reweighted two-step method. Background Technology

[0002] Ultra-short baseline (USBL) underwater acoustic positioning systems are widely used in marine engineering, underwater target tracking, underwater operation support, and autonomous underwater vehicle navigation due to their small size, flexible deployment, and strong adaptability. Integrated inertial navigation / acoustic array USBL systems, through the combination of an inertial navigation system and an acoustic array, can achieve continuous positioning of underwater beacons, transponders, or targets.

[0003] In practical engineering, acoustic arrays are typically mounted on a carrier platform. Due to factors such as mechanical assembly errors, installation process errors, and platform structural limitations, there is usually an installation deviation angle between the acoustic array coordinate system and the carrier coordinate system. The installation deviation angle mainly includes three components: heading angle, pitch angle, and roll angle. If this installation deviation angle cannot be accurately calibrated, it will introduce systematic errors during USBL measurements when converting from the acoustic array coordinate system to the carrier coordinate system and navigation coordinate system, thereby significantly reducing the final positioning accuracy.

[0004] Current technology uses a traditional two-step method for calibrating the installation deviation angle. This type of method typically includes the following steps:

[0005] Ignoring the installation deviation angle, the position of the seabed fixed transponder is estimated based on USBL measurement data, carrier position and attitude information;

[0006] Based on the estimated position of the fixed transponder, the installation deviation angle between the acoustic array and the carrier is then calculated.

[0007] The traditional two-step method has the advantages of being simple to implement and easy to apply in engineering, but it still has the following problems:

[0008] (1) Error propagation problem

[0009] The first step estimates the transponder position while ignoring the installation deviation angle, resulting in a systematic deviation in the position estimate. This deviation will be further propagated to the second step of solving for the installation deviation angle, affecting the final calibration accuracy.

[0010] (2) The equal weighting process is unreasonable.

[0011] Traditional two-step methods typically assume that all measurement points have the same weight, without considering the differences between different measurement points in terms of noise level, geometric configuration, outlier interference, etc. Therefore, it is difficult to obtain stable and accurate results in complex marine environments.

[0012] (3) Sensitive to outliers

[0013] In real-world marine environments, USBL measurements are affected by factors such as multipath propagation, changes in the sound velocity field, gross errors, and transient loss of lock, causing some measurement points to deviate significantly from their normal distribution. Traditional least squares estimation is significantly affected by these large residuals, reducing the algorithm's robustness.

[0014] (4) Lack of closed-loop feedback optimization mechanism

[0015] The traditional two-step method uses an open-loop sequential solution. The installation deviation angle obtained in the second step will not be fed back to correct the position estimate in the first step, resulting in the parameters not being able to correct each other and the overall optimization capability being limited. Summary of the Invention

[0016] To overcome the shortcomings of existing technologies and achieve the goals of ensuring installation deviation angle calibration accuracy, target positioning accuracy, and algorithm robustness simultaneously in complex marine environments, this invention adopts the following technical solution:

[0017] The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method includes the following steps:

[0018] The transponder position and installation deviation angle are obtained using a traditional two-step method as initial estimates.

[0019] Based on the current transponder position and the current installation deviation angle, calculate the error of each measurement point, and construct weights based on the errors;

[0020] Fix the current installation deviation angle and estimate the transponder position using the weights; fix the estimated transponder position and update the installation deviation angle using the weights.

[0021] The estimated transponder position and the updated installation deviation angle are used as the current transponder position and the current installation deviation angle, and the process is iterated until the installation deviation angle converges to obtain the final installation deviation angle.

[0022] Furthermore, a transition matrix is ​​constructed between the navigation coordinate system and the carrier coordinate system; the installation error angle is obtained based on the rotation transformation relationship from the acoustic array coordinate system to the carrier coordinate system to construct an installation deviation matrix; the position of the underwater transponder in the acoustic array coordinate system is calculated using the slant range and direction cosine obtained by the USBL underwater acoustic positioning system; the acoustic array coordinate system and the carrier coordinate system are set to have the same origin, and the position of the underwater transponder in the carrier coordinate system is obtained through the installation deviation matrix and the position of the underwater transponder; based on the transition matrix, the position of the underwater transponder in the carrier coordinate system, and the position of the carrier in the navigation system, the true position of the underwater transponder in the navigation coordinate system is obtained; an error term is introduced to generate an observation equation for the error between the true position and the observed position of the underwater transponder in the navigation coordinate system.

[0023] Furthermore, a two-step method is used to obtain the initial estimated value;

[0024] The first step is to estimate the transponder position, ignoring installation deviations;

[0025] To obtain the observed position of the underwater transponder in the navigation coordinate system at the measurement point, the error is obtained. Since the installation deviation matrix contains the installation error angle, and the error observation equation contains the installation deviation matrix and the position of the underwater transponder in the navigation coordinate system, the least squares estimation can be used to construct a minimum objective function for the error. The derivative is taken and set to zero, that is, the installation deviation is ignored, so as to obtain the optimal estimate of the transponder position.

[0026] The second step is to fix the transponder position and estimate the installation deviation angle;

[0027] Using the optimal estimate as the true position, the observation equation is used to obtain the error of the transponder position at the measurement point. Based on the error, a nonlinear least squares objective function about the installation deviation angle is constructed, and the final estimated value of the installation deviation angle is obtained by solving the equation.

[0028] The optimal estimate of the transponder position and the final estimate of the installation deviation angle are used as the initial estimate.

[0029] Furthermore, the weights include residual weight terms. Based on the transponder position and installation deviation angle from the previous iteration, the residuals and their moduli at the measurement points are calculated using the observation equation of the error. The residual weight terms are constructed using the Huber function. If the residual moduli is less than or equal to the product of the Huber constant and the absolute deviation estimate of the median moduli, the residual weight term is 1. Otherwise, the residual weight term is the ratio of the product to the moduli, thereby improving the robustness of the algorithm to gross errors and outliers.

[0030] Furthermore, the weight is the product of the residual weight term and the geometric weight term. The geometric weight term uses 1 plus the product of the geometric accuracy factor of the measurement point and the adjustment parameter as the denominator and 1 as the numerator. By introducing the geometric weight term, the difference in geometric configuration quality is reflected.

[0031] Furthermore, the estimated transponder position is calculated by fixing the installation deviation angle estimate from the previous iteration, calculating the estimated value of the underwater transponder in the navigation coordinate system of the previous iteration, and subtracting it from the actual position to construct the observation equation of the error and obtain the observation error function. Based on the weighted least squares method, the position of the underwater transponder in the navigation coordinate system with the smallest observation error containing the weights is found as the optimal estimated value of the underwater transponder in the navigation coordinate system of this iteration.

[0032] Furthermore, the updated installation deviation angle is obtained by fixing the optimal estimate of the underwater transponder in the navigation coordinate system of this iteration, subtracting it from the position of the underwater transponder at the observation point of this iteration to construct the observation equation of the error, and obtaining the observation error function; the Gauss-Newton method is used to solve it, and based on the weight of each measurement point and the installation deviation angle estimate of the previous iteration, the installation deviation angle with the smallest observation error containing the weight is found as the installation deviation angle estimate of this iteration.

[0033] Furthermore, due to the inclusion of unknowns of This is the installation deviation matrix, which contains trigonometric functions and is a nonlinear vector function of the installation deviation angle. The nonlinear vector function needs to be obtained from the previous round of estimation. A first-order Taylor expansion is performed nearby, therefore, the estimated installation deviation angle from the previous iteration... At this point, the observation error function is expanded into a linear form using the first-order Taylor expansion, yielding the intermediate residual calculated from the underwater transponder position in this round and the installation deviation angle in the previous round, as well as the product of the Jacobian matrix from the previous round and the increment of the installation deviation angle in this round:

[0034]

[0035]

[0036] in, Let be the Jacobian matrix, representing the derivative of the residual vector with respect to the installation deviation angle vector, i.e., the sensitivity matrix of the residual to changes in the installation deviation angle. ;

[0037] Since the installation deviation angles are all small-angle deviations, under the condition of small-angle approximation, a first-order approximation is made to the small-angle installation deviation matrix of the previous round of installation deviation angle estimates. This approximates the originally nonlinear installation deviation matrix into a linear term and an angle increment term, i.e.:

[0038]

[0039] in, For antisymmetric matrices:

[0040]

[0041] Substituting this back into the observation equations, we obtain the following new observation equations:

[0042]

[0043] Further expansion yields:

[0044]

[0045] Will Substituting into the above equation, we get:

[0046]

[0047] Further development

[0048]

[0049] We obtain the constant term, and the product of the previous round's Jacobian matrix and the current round's installation deviation angle, e(Φ) ≈ constant term + Jacobian matrix. Through with By comparison, the Jacobian matrix can be directly read:

[0050]

[0051] Solve for the installation deviation angle increment in this round:

[0052] .

[0053] Furthermore, an objective function is constructed based on the weighted linear least squares method to seek an increment. This minimizes the weighted sum of squared errors. The weight matrix for the k-th iteration is:

[0054]

[0055] Differentiating the objective function yields the Gauss-Newton equation, which in turn provides the increment of the installation deviation angle for this round:

[0056]

[0057] The sum of the installation deviation angle from the previous round and the increment of the installation deviation angle in this round is taken as the installation deviation angle for this round:

[0058]

[0059] Repeat the iteration until convergence to obtain the estimated installation deviation angle for this round. .

[0060] The USBL system based on the iterative reweighted two-step method is used to calibrate the installation deviation angle of the USBL system.

[0061] The advantages and beneficial effects of this invention are as follows:

[0062] This invention, through alternating feedback iteration, prevents the transponder position and installation deviation angle from being solved in isolation. Instead, they are mutually corrected within a closed loop, fundamentally reducing the error propagation problem in the traditional two-step method. Furthermore, this invention significantly weakens the influence of outliers and gross errors in optimization through robust weighting and geometric weighting mechanisms, thereby improving the robustness of installation deviation angle and position estimation. Simultaneously, it suppresses the contribution of geometrically poor measurement points, preventing a few poorly configured measurement points from having an excessive impact on the overall solution. Meanwhile, if only alternating iteration is used without weighting, it is still susceptible to outliers; if only robust weighting is applied without introducing geometric quality constraints, its effectiveness is limited in geometrically deteriorated scenarios; and if only geometric weighting is applied without introducing closed-loop feedback, it cannot effectively solve the error propagation problem. Therefore, this invention achieves synergistic optimization of the installation deviation angle calibration problem by organically combining alternating feedback structure, robust weighting, and geometric weighting. Attached Figure Description

[0063] Figure 1 This is a flowchart of the method in this invention.

[0064] Figure 2 This is a diagram showing the carrier trajectory and beacon position in an embodiment of the present invention.

[0065] Figure 3 This is a measurement error diagram of the corresponding measurement sequence number in multiple directions and trajectory positions in the implementation of this invention.

[0066] Figure 4 This is a beacon point cloud distribution map calculated from measurement data in the implementation of this invention.

[0067] Figure 5 This is a convergence curve of the installation deviation angle in the implementation of this invention.

[0068] Figure 6 This is a point cloud comparison diagram of two correction methods in the implementation of this invention.

[0069] Figure 7 This is an iterative curve of the IRTS cost function in an embodiment of the present invention.

[0070] Figure 8 This is the final weight distribution diagram in the implementation of this invention. Detailed Implementation

[0071] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0072] like Figure 1 As shown, this invention proposes a USBL system installation deviation angle calibration method based on the Iterative Reweighted Two-Step (IRTS) method. Building upon the traditional two-step method, it introduces an alternating iterative optimization mechanism for position and installation deviation angle, combined with robust weights and geometric quality weights, to suppress outliers and poor geometric measurement points, thereby improving the accuracy and robustness of the installation deviation angle calibration algorithm. This invention can be used for underwater beacon positioning, marine engineering operation support, autonomous underwater vehicle navigation assistance, and the installation and calibration of underwater acoustic positioning equipment. Specifically, it includes the following steps:

[0073] Step 1: Obtain the transponder position and installation deviation angle using the traditional two-step method as the initial estimate. This includes the following steps:

[0074] Step 1.1: Coordinate system definition; This invention relates to three coordinate systems:

[0075] Navigation coordinate system: North-East Geodetic Coordinate System (NED), abbreviated as NED system, origin At the acoustic array center of the USBL system The axis points due north. The axis points due east. The axis points vertically downwards, towards the Earth's center.

[0076] Carrier coordinate system: abbreviated as system, origin As the measurement center of the inertial navigation system, The axis points forward. The axis points to the starboard side. The axis is vertically downward.

[0077] Acoustic array coordinate system: abbreviated as system, origin The acoustic array center of the USBL system is approximately aligned with the INS volume coordinate system in its three axes. The axis points forward. The axis points to the starboard side. The axis is vertically downward.

[0078] The rotational transformation relationship between the navigation coordinate system and the INS (Inertial Navigation System) volume coordinate system (i.e., the vehicle coordinate system) is the vehicle attitude, usually described by the heading angle, pitch angle, and roll angle, denoted as . ;

[0079] The rotation matrix from the navigation coordinate system to the vehicle coordinate system is:

[0080]

[0081] The rotational transformation relationship between the matrix coordinate system and the INS volume coordinate system is the installation error angle, denoted as . Since the installation deviation angle is small, the installation deviation matrix can be written as:

[0082]

[0083] Step 1.2: Measurement Model;

[0084] The USBL system can directly measure the slope distance. and direction cosine Then the position of the underwater transponder in the matrix coordinate system can be represented as:

[0085]

[0086] The transition matrix from the base coordinate system to the carrier coordinate system is: Assuming the base coordinate system and the carrier coordinate system share the same origin, the position of the underwater transponder in the carrier coordinate system is:

[0087]

[0088] The transition matrix from the vehicle coordinate system to the navigation coordinate system is: Assuming the position of the vehicle in the navigation system is Then the actual position of the underwater transponder in the navigation coordinate system is:

[0089]

[0090] Expanding, we get:

[0091]

[0092] Actual measurements involve factors such as deviation and noise; therefore, an error term is introduced. Subsequently, the actual position of the underwater transponder in the navigation coordinate system is represented as follows:

[0093]

[0094] After rearranging the terms, the observation equation is:

[0095]

[0096] Specifically, regarding the number The observation equations for each measurement point are:

[0097]

[0098] Therefore, the joint estimation problem of the installation deviation angle and the transponder position can be expressed as the following least squares optimization problem:

[0099]

[0100] Step 1.3: Initial solution using the traditional two-step method;

[0101] This invention first uses a traditional two-step method to obtain an initial solution. Since the transponder position and installation deviation angle are coupled in the measurement model, the traditional two-step method breaks them down into two steps for solution.

[0102] The first step is to ignore installation deviations and estimate the transponder position, i.e. .

[0103] Measurement points The observed position of the transponder at that location is:

[0104] ,

[0105] The estimation error is:

[0106]

[0107] The optimal estimate of the transponder's location can be obtained using least squares estimation. The objective function to be minimized is as follows:

[0108]

[0109] Take the derivative and set it to zero:

[0110]

[0111] Solving this problem, we can obtain the following:

[0112]

[0113] In the above formula, the estimated position of the underwater transponder Around the real location Dispersed, therefore it can be considered This is the actual location of the underwater transponder.

[0114] The second step is to fix the position of the transponder. Estimate installation deviation angle .

[0115] At this time, at the measurement point At this location, the error in the transponder's position is:

[0116]

[0117] Similarly, a nonlinear least-squares objective function is constructed regarding the installation deviation angle:

[0118]

[0119] The above equation represents a nonlinear optimization problem, which is typically solved using the Gauss-Newton method or the Levenberg-Marquardt method to obtain the final estimate. This will not be elaborated upon here.

[0120] Step 2: Calculate the residuals of each measurement point and update their weights.

[0121] In the In this iteration, the parameter estimates from the previous iteration are used. and Calculate the residuals at each measurement point:

[0122]

[0123] Its mold length is:

[0124]

[0125] To improve the algorithm's robustness to gross errors and outliers, the Huber function is used to construct the residual weight term:

[0126]

[0127] in, Huber constant, This is an estimate of the absolute deviation of the median.

[0128] Meanwhile, to reflect differences in geometric configuration quality, a geometric weighting term is introduced:

[0129]

[0130] in, For the first Geometric accuracy factor for each measurement point, parameters To adjust the parameters.

[0131] In summary, the weight function is constructed as follows:

[0132] .

[0133] Step 3: Fix the installation deviation angle Estimate the location of the transponder .

[0134] In the nth iteration, the fixed installation deviation angle is the estimated value from the previous round. The observed value of the transponder position is... ,as follows:

[0135]

[0136] The observation error is:

[0137]

[0138] The problem of estimating the position of a transponder can be transformed into solving... , making Minimum, that is, finding the position of the transponder that minimizes the observation error:

[0139]

[0140] The optimal estimate can be obtained using the weighted least squares method. ,as follows:

[0141] .

[0142] Step 4: Fix the transponder position Estimate installation deviation angle .

[0143] After completing the transponder position update, fix Similarly, the Gauss-Newton method is used to solve for the installation deviation angle. .

[0144] The residual function is:

[0145]

[0146] The problem of updating the installation deviation angle can be expressed as:

[0147]

[0148] Right now:

[0149]

[0150] The Gauss-Newton method is used to solve the problem below. .

[0151] Let the installation deviation angle increment be:

[0152]

[0153] Because it contains unknowns of It is a rotation matrix containing trigonometric functions, which is a nonlinear vector function of the installation deviation angle. This nonlinear vector function requires the previous estimated value... Perform a first-order Taylor expansion nearby, therefore, in Expanding the residual function into a linear form, we get:

[0154]

[0155] in, Let be the Jacobian matrix, representing the derivative of the residual vector with respect to the installation deviation angle vector, i.e., the sensitivity matrix of the residual to changes in the installation deviation angle. .

[0156] Since the installation deviation angles are all small angle deviations, under the condition of small angle approximation, a first-order approximation is made to the small angle rotation matrix, approximating the originally nonlinear rotation effect as "linear term + angle increment term", that is:

[0157]

[0158] in, For antisymmetric matrices:

[0159]

[0160] Substituting it back into the residual equation: Further expansion yields:

[0161]

[0162] Will Substituting into the above equation, we get:

[0163]

[0164] Further development

[0165]

[0166] Thus, the residual is written as e(Φ) ≈ constant term + Jacobian matrix. Then through with By comparison, the Jacobian matrix can be directly read as follows:

[0167]

[0168] The problem of solving for the installation deviation angle increment can be written as:

[0169]

[0170] That is, to seek an increment. This minimizes the weighted sum of squared residuals. The weight matrix for the k-th iteration is:

[0171]

[0172] This is a standard weighted linear least squares problem, and the derivative yields the Gauss-Newton method equation.

[0173] make , can be expanded to obtain

[0174]

[0175] because It is a real diagonal matrix, therefore:

[0176]

[0177] Differentiating the above equation and setting it to zero, we obtain the Gauss-Newton equation:

[0178]

[0179] Therefore, we get:

[0180]

[0181] Then, the updated formula for the installation deviation angle is:

[0182]

[0183] Repeat the iterations until convergence to obtain the result. .

[0184] Step 5: Repeat the iteration until the estimated installation deviation angle is reached. convergence.

[0185] Convergence judgment, i.e. , Given a minimum value, output the final result.

[0186] In this embodiment of the invention, to verify the effectiveness of the iterative reweighting two-step method, the following simulation examples and results verification were conducted under error conditions that are closer to actual engineering practice:

[0187] 1. Simulation condition settings.

[0188] By superimposing random errors, slowly varying errors, and outlier interferences on USBL ranging, angle measurement, and carrier position and attitude, the working conditions under complex sea trial environments are simulated.

[0189] The specific parameter settings are as follows: The actual position of the seabed beacon in the navigation coordinate system is [0,0,-500]m. The carrier moves along an approximately circular track with a track coverage angle greater than 360°, and collects 150 measurement points throughout the entire track. Actual installation deviation angle. Set to use heading deviation angle Pitch deviation angle and roll deviation angle The sequence is represented as [3.2°, -1.8°, 2.5°].

[0190] To simulate a real-world environment, the following measurement noise is introduced:

[0191] (1) USBL slant distance random noise, with a standard deviation of 0.25m, is used to simulate random measurement errors in USBL ranging.

[0192] (2) USBL direction random noise, with a standard deviation of 0.18°, is used to simulate the influence of azimuth and elevation angle measurement errors on direction cosine measurement.

[0193] (3) Slant distance ratio deviation, set to 0.08%, is used to simulate factors such as sound speed model mismatch and system time delay calibration deviation.

[0194] (4) Low-frequency undulation deviation of slant range, a low-frequency sinusoidal undulation term with an amplitude of 0.18 m, is used to characterize the influence of slow changes in the sound velocity field and changes in propagation path conditions on the ranging results.

[0195] (5) Random noise of the carrier position, with a standard deviation of 0.45m, is used to simulate the instantaneous position error in the carrier navigation solution.

[0196] (6) Slow position drift of the carrier, with the maximum drift amplitude set to [0.60, 0.45, 0.20]m respectively, to simulate the slow position error caused by the cumulative drift of the inertial navigation system, the low-frequency residual of the integrated navigation and the change of the platform motion state.

[0197] (7) Random noise of carrier attitude, with a standard deviation of 0.12°, is used to simulate random errors in the attitude measurement system.

[0198] (8) The carrier attitude slowly changes the bias, with the amplitude set to [0.12°, 0.08°, 0.10°], to simulate the zero-bias drift of inertial devices, low-frequency error of attitude calculation, and slow misalignment under long-term working conditions.

[0199] (9) Outlier interference: Select 8% of the measurement points and add large-amplitude disturbances to their slope distance measurement and direction measurement respectively, as outlier points.

[0200] 2. Simulation results and analysis of the observation trajectory.

[0201] like Figure 2 and Figure 3 As shown, the carrier travels in an approximately circular trajectory. The measured trajectory is generally consistent with the real trajectory, but due to the influence of random position noise and slow position drift, the measured trajectory has a slight deviation from the real trajectory.

[0202] The beacon point cloud distribution obtained by back-calculation from carrier trajectory measurement data, such as Figure 4 As shown in the figure, the original inversely calculated point cloud is scattered around the actual beacon location, with a few outliers that significantly deviate from the main cluster region. This indicates that relying solely on the original measurement data to directly estimate the beacon location, in the presence of gross slant range errors, orientation perturbations, and navigation errors, leads to a large dispersion range. Outliers, in particular, exhibit significantly larger deviations than normal measurement points; applying an equal-weighted approach would negatively impact the overall estimation results. Therefore, calibration is necessary to reduce measurement errors.

[0203] 3. Comparison and analysis of calibration methods and simulation results.

[0204] 3.1 Estimation of installation deviation angle.

[0205] The installation deviation angle of the observed data was estimated using both the traditional two-step method and the iterative reweighted two-step method (IRTS) proposed in this paper. The estimation results are shown in Table 1.

[0206] Table 1 Comparison of Installation Deviation Angle Estimation Results

[0207]

[0208] As shown in Table 1, the proposed iterative reweighted two-step method outperforms the traditional two-step method in estimating the three-axis mounting deviation angles, particularly the roll deviation angle. The most significant improvement is the reduction of the error from 1.39° to 0.06°. Calculating the Euclidean norm of the triaxial deviation angle error vector reveals that IRTS reduces the installation deviation angle estimation error from 1.74° to 0.66°, demonstrating that the proposed method can significantly improve the accuracy of installation deviation angle calibration.

[0209] The convergence curves of the installation deviation angles further demonstrate that the heading deviation angle, pitch deviation angle, and roll deviation angle all gradually approach their true values ​​during the iteration process. Among them, the roll deviation angle converges more smoothly, while the heading and pitch angles gradually approach their reference values ​​even with large initial deviations. Figure 5 As shown.

[0210] 3.2 Positioning Error Estimation

[0211] The beacon position of each measurement point was calculated from the estimated installation deviation angle. The average, standard deviation and maximum value of the positioning error of all measurement points were statistically analyzed. The results are shown in Table 2.

[0212] Table 2. Statistics of Simulation Positioning Errors

[0213] uncorrected 33.81 7.51 83.50 Traditional two-step method 17.43 7.27 66.94 IRTS 5.69 9.20 53.79

[0214] Table 2 shows that the uncorrected average positioning error reaches 33.81m, indicating that the USBL positioning results are significantly affected by system errors when the installation deviation angle is not compensated. The traditional two-step method can reduce the average positioning error to 17.43m, while IRTS further reduces it to 5.69m, representing a 67% improvement in average error compared to the traditional two-step method. Simultaneously, the maximum error of IRTS is reduced to 53.79m, demonstrating that the proposed method can significantly improve overall positioning performance.

[0215] like Figure 6 As shown, IRTS not only reduces the offset of the estimated center relative to the true position, but also reduces the discrete range of the measured point cloud, indicating that it improves the stability of the results while improving the positioning accuracy.

[0216] 4. Convergence analysis.

[0217] like Figure 7 The figure shows the variation curve of the sum of squared residuals at each time step. As can be seen from the figure, the objective function generally shows a significant downward trend as the number of iterations increases, and tends to stabilize after several iterations, indicating that the proposed method has good numerical stability.

[0218] like Figure 8 As shown, IRTS can automatically assign lower weights to outlier measurement points during the iteration process, thereby effectively reducing the impact of gross errors and poor measurement points on the final estimation results. This demonstrates that the reweighting mechanism has a significant effect under complex error conditions, and is one of the important reasons why IRTS is superior to the traditional two-step method.

[0219] This invention also proposes a USBL system based on an iterative reweighting two-step method. The installation deviation angle of the USBL system is calibrated using the above-mentioned USBL system installation deviation angle calibration method based on the iterative reweighting two-step method. The implementation method is the same as the above calibration method, and will not be repeated here.

[0220] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A USBL system installation deviation angle calibration method based on an iterative reweighted two-step method, characterized in that: The transponder position and installation deviation angle are obtained using a traditional two-step method as initial estimates. Based on the current transponder position and the current installation deviation angle, the error at each measurement point is calculated, and weights are constructed using the errors. A residual weight term is constructed using the Huber function. If the residual modulus is less than or equal to the product of the Huber constant and the absolute deviation estimate of the median modulus, the residual weight term is 1; otherwise, the residual weight term is the ratio of the product to the modulus. The weight is the product of the residual weight term and the geometric weight term, where the geometric weight term has 1 plus the product of the geometric accuracy factor of the measurement point and the adjustment parameter as the denominator, and 1 as the numerator. Fix the current installation deviation angle and estimate the transponder position using the weights; fix the estimated transponder position and update the installation deviation angle using the weights. The estimated transponder position and the updated installation deviation angle are used as the current transponder position and the current installation deviation angle, and the process is iterated until the installation deviation angle converges to obtain the final installation deviation angle.

2. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 1, characterized in that: A transition matrix is ​​constructed between the navigation coordinate system and the carrier coordinate system. The installation error angle is obtained based on the rotational transformation relationship from the acoustic array coordinate system to the carrier coordinate system, thus constructing an installation deviation matrix. The position of the underwater transponder in the acoustic array coordinate system is calculated using the slant range and direction cosine measured by the USBL underwater acoustic positioning system. The acoustic array coordinate system and the carrier coordinate system are set to have the same origin. The position of the underwater transponder in the carrier coordinate system is obtained using the installation deviation matrix and the position of the underwater transponder. Based on the transition matrix, the position of the underwater transponder in the carrier coordinate system, and the position of the carrier in the navigation system, the true position of the underwater transponder in the navigation coordinate system is obtained. An error term is introduced to generate an observation equation for the error between the true position and the observed position of the underwater transponder in the navigation coordinate system.

3. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 2, characterized in that: The initial estimate is obtained using a two-step method; The first step is to estimate the transponder position, ignoring installation deviations; Obtain the observed position of the underwater transponder in the navigation coordinate system at the measurement point to obtain the error. Use least squares estimation to construct a minimum objective function for the error, take the derivative and set it to zero to obtain the optimal estimate of the transponder position. The second step is to fix the transponder position and estimate the installation deviation angle; Using the optimal estimate as the true position, the observation equation is used to obtain the error of the transponder position at the measurement point. Based on the error, a nonlinear least squares objective function about the installation deviation angle is constructed, and the final estimated value of the installation deviation angle is obtained by solving the equation. The optimal estimate of the transponder position and the final estimate of the installation deviation angle are used as the initial estimate.

4. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 2, characterized in that: The weights include residual weights, which are calculated based on the transponder position and installation deviation angle from the previous iteration, using the error observation equation to calculate the measurement point residuals and their moduli.

5. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 2, characterized in that: The estimated transponder position is calculated by fixing the installation deviation angle estimate from the previous iteration, calculating the estimated value of the underwater transponder in the navigation coordinate system of the previous iteration, and subtracting it from the actual position to construct the observation equation of the error and obtain the observation error function. Based on the weighted least squares method, the underwater transponder position in the navigation coordinate system with the smallest observation error containing the weights is found as the optimal estimated value of the underwater transponder in the navigation coordinate system of this iteration.

6. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 5, characterized in that: The updated installation deviation angle is obtained by fixing the optimal estimate of the underwater transponder in the navigation coordinate system of this iteration, subtracting it from the position of the underwater transponder at the observation point of this iteration to construct the observation equation of the error, and obtaining the observation error function; the Gauss-Newton method is used to solve it, and based on the weight of each measurement point and the installation deviation angle estimate of the previous iteration, the installation deviation angle with the smallest observation error containing the weight is found as the installation deviation angle estimate of this iteration.

7. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 6, characterized in that: At the installation deviation angle estimate in the previous iteration, the observation error function is expanded into a linear form using a first-order Taylor expansion. This yields the intermediate residual calculated under the installation deviation angle in the current iteration and the previous iteration, as well as the product of the previous iteration's Jacobian matrix and the current iteration's installation deviation angle increment. A first-order approximation is made to the installation deviation matrix of the previous iteration's installation deviation angle estimate, approximating the nonlinear installation deviation matrix as a linear term and an angle increment term. This is then substituted back into the current iteration's observation equation and expanded further to obtain a constant term and the product of the previous iteration's Jacobian matrix and the current iteration's installation deviation angle. By comparing this with the linear form of the first-order Taylor expansion, the previous iteration's Jacobian matrix is ​​directly read.

8. The USBL system installation deviation angle calibration method based on the iterative reweighting two-step method according to claim 7, characterized in that: The objective function is constructed based on the weighted linear least squares method to find an installation deviation angle increment that minimizes the sum of squared weighted errors. The Gauss-Newton method equation is obtained by differentiating the objective function, and then the installation deviation angle increment for this round is obtained. The sum of the installation deviation angle of the previous round and the installation deviation angle increment for this round is used as the installation deviation angle for this round. The iteration is repeated until convergence to generate the final installation deviation angle estimate.

9. A USBL system based on an iterative reweighting two-step method, characterized in that: The installation deviation angle of the USBL system is calibrated using the USBL system installation deviation angle calibration method based on the iterative reweighting two-step method as described in any one of claims 1 to 8.

Images