An underwater integrated positioning method and system based on INS, DVL and USBL
By inferring the acoustic beam propagation path from prior information from INS and DVL, pseudo-USBL observation data is constructed and fused with USBL device observation data, solving the problem of unstable positioning of USBL under complex sea conditions and achieving higher accuracy and stability of underwater positioning.
Patent Information
- Application Number
- CN202611203855.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-08-10
- Publication Date
- 2026-11-10
AI Technical Summary
Existing USBL underwater positioning systems are prone to false peaks and peak jumps in complex sea conditions, resulting in unstable positioning results and affecting the stability and positioning accuracy of navigation filters.
By using prior quantities provided by INS and DVL to infer the sound beam propagation path and peak confidence, pseudo-USBL observation data is constructed. This data is then fused and combined with USBL device observation data to improve positioning accuracy and stability.
It significantly improves the positioning performance of the USBL positioning system under shallow water reverberation conditions, suppresses positioning jumps caused by false peaks, and enhances the continuity and robustness of integrated navigation output.
Smart Images

Figure CN123065448A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of underwater navigation and positioning technology, and more specifically to an underwater combined positioning method and system based on INS, DVL and USBL, particularly to an ultra-deep combined positioning method and system for AUV, ROV and other underwater vehicle platforms using an ultra-short baseline positioning system (USBL) and an inertial navigation system (INS) and a Doppler velocimeter (DVL). Background Technology
[0002] In existing underwater positioning systems, underwater targets are typically located using multiple underwater acoustic array elements deployed on the mother ship, platform, or fixed support, combined with information on the propagation time, phase difference, or distance difference of the transponder returned signals, based on USBL.
[0003] However, under conditions such as complex sea states, low signal-to-noise ratio, multipath propagation, rapid attitude changes, bubble interference, or changes in sound velocity field, peak extraction of USBL direct waves is prone to false peaks, skipped peaks, or missed detections. Specifically, existing USBL peak detection methods typically use global maximum values, fixed thresholds, or pulse compression. In shallow water or multipath reflection, it is difficult to stably lock the true acoustic arrival peak when multiple peaks coexist in the relevant calculation results, or when the main lobe is distorted or the side lobes are raised. When USBL produces false peaks or skipped peaks, abnormal observations will directly enter the navigation filter, causing sudden jumps in positioning results. In severe cases, it may even cause divergence in the combined filter, thereby affecting the stability of USBL azimuth calculation.
[0004] Therefore, how to overcome the above problems and improve underwater positioning accuracy is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention
[0005] In view of the above problems, this invention proposes an underwater combined positioning method and system based on INS, DVL and USBL; it aims to use the prior quantities provided by INS and DVL to infer the sound beam propagation path, arrival time window and peak confidence, and then construct pseudo USBL observations to correct the conventional output observations of USBL devices.
[0006] To achieve the above objectives, the present invention adopts the following technical solution:
[0007] In a first aspect, embodiments of the present invention provide an underwater combined positioning method based on INS, DVL, and USBL, comprising: Acquire INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curves; The prior pose information of the carrier is obtained by combining and recursively deriving the INS measurement data and DVL measurement data. The theoretical propagation delay of the target acoustic signal to each USBL array element is calculated based on the prior pose information to obtain a local search window; Within the local search window, a peak search is performed on the original acoustic amplitude curve of the USBL to determine the optimal peak value and construct pseudo USBL observation data. The pseudo-USBL observation data is fused with the observation data calculated by the USBL device, and the fused USBL observation data is combined and filtered with the INS measurement data and the DVL measurement data to obtain the navigation state estimation result of the carrier.
[0008] Preferably, after obtaining the raw acoustic amplitude curve of USBL, preprocessing is performed first, including at least mean removal, amplitude limiting, bandpass, or smoothing.
[0009] Preferably, the acquired INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curve are time-synchronized and spatially calibrated to align the data under a unified time reference and spatial coordinate system.
[0010] Preferably, the prior pose information includes the predicted position, predicted velocity, and predicted attitude of the carrier; Based on the prior pose information, the theoretical propagation delay of the target acoustic signal to each USBL array element is calculated using the following formula:
[0011] In the formula, For the propagation structure coefficient, The INS / DVL predicted position of the carrier at the k-th epoch. This is the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system. This refers to the mounting arm of the USBL transponder or acoustic transmission center on the carrier relative to the INS reference center. The INS / DVL predicted velocity of the carrier at the k-th epoch. To compensate for time during exercise, Let be the position of the i-th USBL element in the navigation coordinate system. For equivalent speed of sound, This is the system delay compensation term for the i-th channel; online correction is achieved using moving average or first-order filtering.
[0012] In the formula, For update coefficients less than 1, To lock in the optimal peak latency, This represents the theoretical propagation distance.
[0013] Preferably, the local search window is represented as:
[0014] Indicates the theoretical propagation delay. This represents the half-width of the search window for the i-th channel at the k-th epoch.
[0015] In the formula, This refers to the clip function. The proportionality coefficient corresponding to the confidence probability; This represents the standard deviation of the time delay obtained from the conversion of the INS / DVL predicted position uncertainty; The time delay standard deviation is calculated after the attitude uncertainty is amplified by the acoustic arm and array element baseline. This represents the motion compensation uncertainty caused by the rate of change of velocity or the time synchronization residual. This represents the standard deviation of time delay caused by the uncertainty of the speed of sound; This represents the clock synchronization residual or system delay compensation residual. and These represent the minimum and maximum allowed window half-width values for the project, respectively.
[0016] Preferably, peak search is performed within the local search window to determine the optimal peak value, including: The amplitude, signal-to-noise ratio, peak sharpness, and temporal continuity of the candidate peaks within the local search window are obtained, and a weighted score is calculated to obtain the score result. The optimal peak value is determined based on the scoring results.
[0017] Preferably, fusing the pseudo-USBL observation data with the observation data solved by the USBL device includes:
[0018]
[0019] in, This represents the observations of the fused data. This represents the covariance of the fused data. This represents the adjusted covariance of the pseudo-USBL observation data. The covariance of the observed data calculated for the adjusted USBL device. This is pseudo-USBL observation data. Observational data processed for USBL devices.
[0020] Secondly, embodiments of the present invention provide an underwater integrated positioning system based on INS, DVL, and USBL, for implementing the underwater integrated positioning method based on INS, DVL, and USBL as described in any of the preceding claims, including: The data acquisition device is used to acquire INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curves; The INS / DVL prior estimation module is used to obtain the prior pose information of the carrier by combining and recursively deriving the INS measurement data and DVL measurement data; and to calculate the theoretical propagation delay of the target acoustic signal to each USBL array element based on the prior pose information to obtain a local search window. The acoustic peak inversion module is used to perform peak search on the original acoustic amplitude curve of the USBL within the local search window to determine the optimal peak and construct pseudo USBL observation data. An adaptive measurement fusion module is used to fuse the pseudo USBL observation data with the observation data solved by the USBL device; The central integrated navigation filtering module is used to combine and filter the fused USBL observation data with the INS measurement data and the DVL measurement data to obtain the navigation state estimation result of the vehicle.
[0021] Preferably, it also includes a time synchronization and coordinate calibration module, used to perform time synchronization and spatial coordinate calibration on the acquired INS measurement data, DVL measurement data and USBL raw acoustic amplitude curve, so that the data are aligned under a unified time reference and spatial coordinate system.
[0022] Preferably, it also includes a result output and fault discrimination module, which is used to output the navigation state estimation result of the carrier and perform false peak alarm and failure discrimination.
[0023] This invention provides an underwater combined positioning method and system based on INS, DVL, and USBL. First, INS and DVL are used to establish the prior motion state of the carrier, and the theoretical propagation delay and local search window corresponding to each acoustic baseline are deduced. Then, constrained peak search is performed on the original USBL correlation amplitude curve to generate pseudo-USBL observations. Next, the pseudo-USBL observations are adaptively fused with the conventional output observations of the USBL device to obtain a more reliable calibrated USBL measurement. Finally, the calibrated USBL measurement is combined with INS and DVL through a central-depth combined filter to output high-precision position, velocity, and attitude information.
[0024] Compared with the prior art, the beneficial effects of this application include at least the following: 1. For the first time, INS / DVL prior motion information is used to detect the first peak of the original correlation calculation amplitude curve of USBL, which changes the acoustic direct wave search from "global blind search" to "geometrically constrained local locking", significantly improving the stability of USBL direct wave positioning and enhancing the positioning performance of the USBL system under shallow water reverberation conditions.
[0025] 2. This invention integrates three types of information: "backward acoustic observation + equipment observation + INS / DVL prior," which has a higher depth of information utilization than the traditional method of only integrating the final output position or pseudorange of USBL. 3. By introducing peak amplitude, sharpness, sidelobe ratio, temporal consistency and innovation statistics, the reliability of USBL observations can be evaluated online and the measurement covariance can be dynamically adjusted; The positioning method provided by this invention can effectively suppress positioning jumps caused by false peaks under conditions of multipath interference, noise interference, partial obstruction, or USBL echo distortion, thereby improving the continuity and robustness of the integrated navigation output. The provided positioning system is highly modular and can be deployed in AUVs, ROVs, launching vessels, underwater work platforms, and shipborne underwater acoustic positioning systems, demonstrating strong engineering feasibility. This application can output corrected USBL positioning information and can also be further used as a highly reliable external observation input for the INS / DVL central integrated navigation system, facilitating engineering system expansion. Attached Figure Description
[0026] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0027] Figure 1 This is a schematic diagram of peak locking provided in an embodiment of the present invention; Figure 2 This is a flowchart illustrating the underwater combined positioning method based on INS, DVL, and USBL provided in this embodiment of the invention. Figure 3 This is a schematic diagram of the underwater combined positioning system based on INS, DVL and USBL provided in an embodiment of the present invention. Detailed Implementation
[0028] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. The described embodiments are only some embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are all within the scope of protection of the present invention.
[0029] In this embodiment, INS represents inertial navigation system, DVL represents Doppler velocimeter, and USBL represents ultra-short baseline positioning system. For ease of description, the above abbreviations will be used in the following description.
[0030] INS can continuously output the position, velocity, and attitude of the carrier, while DVL can provide velocity information relative to the water body or seabed. This application integrates INS, DVL, and USBL, and by utilizing the prior motion and geometric constraint information provided by INS / DVL, it reverse-engineers and reconstructs the direct wave information in the original acoustic signal of USBL, making full use of the fine-grained time delay information in the original acoustic correlation amplitude curve, thereby improving underwater positioning accuracy and positioning continuity.
[0031] In one specific embodiment, the underwater combined positioning method based on INS, DVL, and USBL provided by the present invention includes the following steps: S1. Acquire INS measurement data, DVL measurement data, USBL raw acoustic amplitude curve, and USBL device-calculated observation data, and cache them; S2. Based on the combination and recursion of INS measurement data and DVL measurement data, the prior pose information of the carrier is obtained, wherein the prior pose information includes predicted position, predicted velocity and predicted attitude. S3. Calculate the theoretical propagation delay of the target acoustic signal to each USBL array element based on the prior pose information, and calculate the local search window; S4. Perform peak search on the original acoustic amplitude curve of USBL within the local search window to determine the optimal peak value, and construct pseudo USBL observation data based on the optimal peak value. S5. Perform consistency checks and adaptive fusion on the pseudo-USBL observation data and the observation data calculated by the USBL device. Combine and filter the fused USBL observation data with the INS measurement data and DVL measurement data to obtain the navigation state estimation result of the carrier.
[0032] The step numbers above are for descriptive convenience only and do not constitute a strict limitation on the execution order. In some implementations, data acquisition, preprocessing, time synchronization, and filtering recursion can be performed in parallel according to the sensor sampling frequency.
[0033] In one optional implementation, after acquiring multi-source data in step S1, preprocessing, time synchronization, spatial coordinate calibration, and unified modeling are performed first. Specifically, This embodiment uses the following coordinate system: the navigation coordinate system is denoted as... The coordinate system of the carrier is denoted as The DVL coordinate system is denoted as The USBL array coordinate system is denoted as... Tie. This represents the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system. This represents the installation transformation matrix from the DVL coordinate system to the vehicle coordinate system. This represents the transformation matrix from the USBL array coordinate system to the navigation coordinate system. This indicates the mounting arm of the DVL measurement center relative to the INS reference center. This refers to the mounting arm of the USBL transponder or acoustic transmission center on the carrier relative to the INS reference center. Indicates the first The baseline vector of each USBL element relative to the acoustic reference center of the USBL array.
[0034] First, the raw acoustic amplitude profile of the USBL is preprocessed. For each USBL receiver channel, the sampling sequence is denoted as... The system sequentially performs one or more of the following processes: mean removal, amplitude limiting, bandpass filtering, and smoothing. Mean removal eliminates DC bias; amplitude limiting suppresses transient spike noise; bandpass filtering preserves effective frequency bands near the transmitted signal bandwidth; and smoothing reduces the impact of random noise on peak sharpness calculations. Simultaneously, outlier removal is performed on the DVL velocity. Outlier removal can be based on DVL beam validity indicators, bottom tracking or water tracking status, velocity residuals, velocity change rates between adjacent epochs, and differences from the short-time INS predicted velocity. When any beam quality indicator is invalid, the velocity mutation exceeds a preset upper limit, or the difference between the DVL velocity and the INS predicted velocity exceeds the corresponding covariance threshold, the DVL velocity is marked as an anomaly and rejected, or the observation covariance of the DVL velocity is increased.
[0035] Secondly, time synchronization is performed on the INS measurement data, DVL measurement data, USBL raw acoustic amplitude curve, and USBL device-derived observation data. In this embodiment, time synchronization is not simply based on abstract existing alignment conclusions, but can be achieved through the following specific steps: using the INS time or external PPS synchronization time as a unified time reference; reading the timestamps inherent in the INS, DVL, and USBL data packets, and subtracting the pre-calibrated fixed acquisition delay, serial port or network port transmission delay, and trigger delay of each sensor; interpolating or extrapolating the high-frequency INS position, velocity, and attitude sequences to obtain the carrier state corresponding to the USBL acoustic transmission or reception time; aligning the DVL velocity to the same epoch using nearest neighbor hold or linear interpolation; and aligning the USBL raw amplitude curve according to the sampling frequency. Convert sampling point n to relative time delay The relative delay is then correlated with the transmission trigger time or reception time of the USBL data packet, thereby obtaining the acoustic delay curve under a unified time reference.
[0036] Next, spatial coordinate calibration is performed. For DVL data, a rotation matrix is installed using DVL. and lever arm Transform the velocity in the DVL coordinate system to the vehicle coordinate system or navigation coordinate system, and compensate for the lever arm velocity term caused by the angular motion of the vehicle; one way to express this is:
[0037] in, The speed of DVL output. The symbol "×" represents the cross product of the carrier angular velocity measured by the INS. This compensates for the installation deviation of the DVL relative to the INS reference center, ensuring that the DVL velocity and the INS state are at the same spatial reference point.
[0038] For USBL data, the acoustic reference center position of the USBL array is utilized. Array attitude transformation matrix Baseline vector of array elements Transform the coordinates of the i-th USBL element to the navigation coordinate system; simultaneously, utilize the carrier attitude... and transponder or acoustic transmission center lever arm This converts the carrier's INS reference center position to the acoustic center position. Its expression is:
[0039]
[0040] in, This represents the position of the i-th USBL element in the navigation coordinate system. This indicates the position of the target's acoustic center in the navigation coordinate system. When the USBL array is fixed to the mother ship, a fixed support, or a shore-based platform... and The position, orientation, or calibration values of the platform can be used to determine this; when the USBL array is mounted on a carrier, and It can be calculated from the carrier INS status and the installation parameters of the USBL array relative to the carrier.
[0041] Sound velocity compensation specifically includes: acquiring a CTD, sound velocity meter, or pre-measured sound velocity profile, and calculating the equivalent sound velocity corresponding to the current sound path. Both theoretical time delay calculation and distance conversion from time delay are performed using... If the output distance of a USBL device is calculated using the nominal speed of sound... Then you can press Sound velocity compensation is applied to the equipment distance observation; if the distance is directly constructed from the original peak time delay, then... Perform the conversion, where, This represents the distance observation value directly output by the USBL device. Let be the propagation structure coefficient, and take for one-way propagation. =1, two-way propagation takes =2, This is a system latency compensation item.
[0042] Based on the aforementioned time synchronization and spatial coordinate calibration, this embodiment further unifies the modeling of INS, DVL, and USBL data. The installation parameters include the arm length, installation angle, scaling factor, and fixed delay of each sensor relative to the INS reference center; the baseline geometric parameters include the USBL array acoustic reference center and the baseline vector of each array element. Reference element number and baseline difference between elements; the sound velocity parameter includes equivalent sound velocity. Uncertainty about the speed of sound and the nominal speed of sound used inside USBL devices The coordinate system transformation relationship includes: , , And the transformation relationships related to the mounting arm. Unified modeling means representing DVL velocity observations, USBL distance or distance difference observations, and INS state propagation as the same state vector. The observation equation.
[0043]
[0044]
[0045] in, and These represent DVL observation noise and USBL observation noise, respectively. Through this unified modeling, subsequent local peak search, observation fusion, and combined filtering are all performed under a unified time base, a unified spatial coordinate system, and a unified noise description.
[0046] In one optional implementation, step S2 involves combining INS measurement data and DVL measurement data to recursively deduce the prior pose information of the carrier. Specifically, the INS performs strapdown inertial navigation recursion based on the gyroscope angular velocity and accelerometer specific force to obtain the predicted attitude, predicted velocity, and predicted position; the DVL velocity, after installation deviation compensation, is used as a velocity observation to constrain INS velocity drift.
[0047]
[0048] in, Represents the state vector. This indicates the position of the carrier's INS reference center in the navigation coordinate system, i.e., the predicted position. Indicates the predicted speed. This represents the attitude quaternion or equivalent attitude parameters, i.e., the predicted attitude. and These represent zero bias of the gyroscope and zero bias of the accelerometer, respectively.
[0049] In one optional implementation, step S3 calculates the arrival time of the target acoustic signal at each USBL array element in the current epoch based on the prior pose information. Theoretical propagation delay; expressed by the formula:
[0050] In the formula, Let be the theoretical propagation delay of the target acoustic signal to reach the i-th USBL array element. For the propagation structure coefficient, The INS / DVL predicted position of the carrier at the k-th epoch. This is the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system. This refers to the mounting arm of the USBL transponder or acoustic transmission center on the carrier relative to the INS reference center. The INS / DVL predicted velocity of the carrier at the k-th epoch. To compensate for time during exercise, Let be the position of the i-th USBL element in the navigation coordinate system. For equivalent speed of sound, This is the system delay compensation term for the i-th channel.
[0051] This formula explains that: the predicted position is used to determine the spatial location of the carrier's INS reference center; the predicted attitude is used to position the transponder or acoustic transmission center boom. The system transforms from the carrier coordinate system to the navigation coordinate system to obtain the true acoustic center position; the predicted velocity is used to compensate for short-term motion between INS / DVL state epochs and USBL acoustic transmit or receive epochs. If the carrier velocity is low or the time alignment error is negligible, it can be set as follows: =0; If movement during propagation needs to be considered, it can be started with As the initial value, then Take as Alternatively, perform one or two iterations on its two-way equivalent value.
[0052] System latency compensation item Used to characterize the fixed or slowly varying delays caused by USBL transmit triggering, receive filtering, correlation processing, timestamp writing, and cables and electronic links.
[0053] This can be determined through calibration tests: at a known acoustic distance and the known speed of sound Peak delay measured in a calibrated environment such as a pool or dock and calculate .
[0054] During operation, when the peak quality is high and the NIS test passes, online correction can be performed using moving average or first-order filtering.
[0055] in, To lock in the optimal peak latency, For update coefficients less than 1, upper and lower limits can be set during online updates to avoid sudden changes in system latency estimation caused by erroneous peaks. For the propagation structure coefficient, For equivalent speed of sound, Let represent the theoretical propagation distance between the i-th USBL element and the target acoustic center, which is determined by the magnitude of the relative geometric vector, i.e.
[0056]
[0057] in, This represents the relative geometric vector of the target acoustic center with respect to the i-th USBL element at the current epoch, used to describe the relative spatial position between the target acoustic center and the i-th USBL element. Let be the position of the i-th USBL element in the navigation coordinate system. express The position of the target acoustic center in the navigation coordinate system at the k-th epoch.
[0058] Construct a local search window based on the theoretical propagation delay:
[0059] in, This represents the half-width of the search window for the i-th channel at the k-th epoch, in seconds. This parameter can be adaptively adjusted. A specific adaptive method is as follows:
[0060] In the formula, The proportionality coefficient corresponding to the confidence probability; This represents the standard deviation of the time delay obtained from the conversion of the INS / DVL predicted position uncertainty; The time delay standard deviation is calculated after the attitude uncertainty is amplified by the acoustic arm and array element baseline. This represents the motion compensation uncertainty caused by the rate of change of velocity or the time synchronization residual. This represents the standard deviation of time delay caused by the uncertainty of the speed of sound; This represents the clock synchronization residual or system delay compensation residual. The `clip` function is used to limit the window's half-width to the minimum allowed by the project. and maximum value Between. When the rate of change of velocity, the rate of change of attitude, or the uncertainty of the speed of sound increases, Automatically increases; when the carrier movement is stable and the observation quality is high. Automatically decreases.
[0061] In one alternative implementation, step S4 is performed only in the local search window. Peak search is performed on the raw acoustic amplitude curve of USBL. For each candidate peak m within the window, the peak amplitude is extracted. Signal-to-noise ratio Peak width consistency index Peak sharpness index and time continuity indicators And perform normalized weighted scoring:
[0062]
[0063] Among them, peak width consistency index It can be determined based on the difference between the full width at half maximum (FWHM) or equivalent pulse width of the candidate peak and the expected width of the transmitted signal, for example... Peak sharpness index Used to characterize the prominence of a candidate peak relative to its neighborhood amplitude; time continuity index It is used to characterize the consistency between the candidate peak delay and the peak value locked in the previous epoch and the change in INS / DVL predicted delay. Including peak width in the score can suppress wide and blunt multipath peaks and narrow and isolated spike noise.
[0064] Select rating The largest candidate peak is selected as the optimal peak. When the maximum score is lower than the preset quality threshold If the candidate peak is not directly adopted, a restricted window search can be performed to reduce the confidence of false observations, or the USBL false observation of that epoch can be marked as invalid. This embodiment does not blindly search for the global peak in the entire time domain, but searches and compares candidate peaks within a local window based on the theoretical arrival delay predicted by INS / DVL, thereby improving the probability of locking the true peak and reducing the risk of false peaks and peak skipping.
[0065] Then, based on the scoring results, the optimal peak value is determined as the effective measurement of the current sound beam. A peak locking diagram is shown below. Figure 1 After locking onto the optimal peak value, construct pseudo-USBL observation data. If the USBL observations use absolute distance, then convert the optimal peak delay into pseudo-distance observations:
[0066] If USBL observations use the distance difference relative to reference element 0, then a pseudo-distance difference observation is constructed:
[0067] For example, in a two-way propagation structure, USBL sends an interrogation signal and receives an acknowledgment signal. If the round-trip delay corresponding to a candidate peak is... The corresponding pseudo-distance is If it is a one-way passive receiving structure, the corresponding pseudo-range is: For USBL arrays containing multiple elements, it is possible to... or Δ Composition of observation vectors If it is necessary to obtain the pseudo-USBL position first, it can also be solved using least squares. Find the minimum target position p and use this position as the pseudo-USBL position observation.
[0068] The pseudo-USBL observation covariance can be determined by the peak quality, sampling resolution, system time delay residual, and sound velocity uncertainty. One way to express this is:
[0069] in, This is used to characterize the uncertainty of the optimal peak delay estimate for the i-th channel, and is subsequently used to construct the delay observation covariance matrix. This allows us to determine the reliability of pseudo-distance observations or pseudo-distance difference observations. The sampling interval is... For the optimal peak quality score, ε is a small amount to prevent the denominator from being zero.
[0070] Each channel Composition of time delay covariance matrix Then, the pseudo-USBL observation covariance in distance form can be obtained:
[0071] If distance observations are subsequently used for pseudo-USBL observation fusion and combined filtering, then let:
[0072] If we construct distance difference observations, we can obtain the pseudo-USBL observation covariance in distance difference form through the difference matrix D: .
[0073] in, Used to characterize the uncertainty of pseudo-distance difference observations, serving as the basis for measurement covariance in subsequent fusion of pseudo-USBL observations and equipment observations, combined filter updates, and anomaly observation discrimination.
[0074] In one optional implementation, step S5 first acquires the observation data and quality flags calculated by the USBL device. The quality flags can be directly derived from the calculation status, locking status, number of effective array elements, signal-to-noise ratio, residuals, positioning accuracy factor, sound velocity status, transponder number consistency, or the quality level given by the device in the USBL device protocol frame. If the device does not explicitly provide complete quality flags, equivalent quality flags can be derived by inversely calculating from the device output residuals, adjacent epoch position jump variables, the difference between the device distance and the INS / DVL predicted distance, and the original peak quality. The quality flags are used to determine the validity of the device observations and to adjust the device observation covariance. .
[0075] Pseudo-USBL observation Observation with USBL devices After unifying the observation types and coordinate systems to the same level, a consistency check is performed. If both are observations of the same dimension, then the following calculation is performed:
[0076]
[0077]
[0078] in, The observation difference or consistency test residual between pseudo-USBL observations and USBL device observations before fusion is used to characterize the degree of deviation between the two types of USBL observations under the same observation type and the same coordinate system. The covariance matrix corresponding to the consistency test residuals is obtained from the pseudo-USBL observation covariance. and USBL device observation covariance Jointly determined, for use in Perform normalized measurement; This is a consistency statistic before fusion. When... When the value is less than the chi-square threshold corresponding to the observation dimension, the spurious observation is considered consistent with the equipment observation, and information-form weighted fusion can be performed; when... If the threshold is exceeded but one type of observation has significantly higher quality, the higher-quality observation is used first and the covariance of the other type of observation is expanded; if both are of low quality or the consistency is seriously poor, observation rejection or short-term inertial navigation hold is performed.
[0079] Dynamic weighting is achieved by adjusting the covariance. This is based on the pseudo-observation quality score. and equipment quality rating The adjusted covariance can be obtained separately. and ,For example , .in, Assess the quality of pseudo-USBL observations. The USBL device observation quality score can be normalized to the [0,1] interval; This is the pseudo-USBL observation covariance inflation coefficient, used to control the magnitude of covariance increase when the quality of pseudo-observations deteriorates; This is the observation covariance inflation factor for USBL devices, used to control the magnitude of covariance increase when the observation quality of the device deteriorates. and All coefficients are non-negative and can be set based on sensor calibration results, historical observation stability, or engineering experience. Higher quality results in smaller covariance and greater information weight; lower quality results in larger covariance and smaller information weight. The corrected USBL observations and their covariance are then obtained using the following formula:
[0080]
[0081] The above fusion formula also gives the fused observations. Covariance . Not by Instead of being calculated separately, it is obtained by weighting the covariance and quality adjustment results of the two types of input observations through information; when there is a slight inconsistency between the two but they still pass the threshold, it can be additionally superimposed with... Related conservative terms are included to improve the robustness of the fused covariance. Therefore, when the quality of spurious observations is higher than that of instrument observations, Smaller, spurious observations The weight in the data is increased; when the equipment observation quality is higher, When the weighting is relatively small, the weighting of equipment observations increases; when both are unreliable, relying solely on the weighting formula is insufficient to guarantee safe output, and consistency check logic is needed to trigger rejection, delayed confirmation, or covariance inflation.
[0082] Furthermore, the fused USBL observations and its covariance The INS and DVL measurement data are input into the central integrated navigation filter. The central integrated navigation filter can be implemented using an error-state extended Kalman filter, whose error states include at least position error δp, velocity error δv, attitude error δθ, and gyro bias error. and accelerometer zero bias error It may also include DVL scaling factor error, USBL delay error, or sound speed error as needed.
[0083]
[0084] in, This is the error state vector in the integrated navigation filter. Indicates positional error. Indicates speed error, This indicates a small angular error in attitude. This indicates the gyroscope's zero-bias error. This indicates the zero bias error of the accelerometer.
[0085] In the filtering prediction stage, the nominal state and covariance are propagated according to the INS mechanical orchestration equations:
[0086]
[0087] in, For the predicted nominal state of the k-th epoch, This is the nominal state after the update in the previous epoch. ( ) represents the INS mechanical arrangement state recursive function. This represents the IMU measurement input at the k-th epoch. Indicates the filter recursion time interval; To predict the covariance matrix, This is the covariance matrix updated in the previous epoch. Here is the error state transition matrix. Let be the system process noise covariance matrix.
[0088] During the filtering and update phase, observation equations are established based on DVL velocity observations and fused USBL observations, respectively. The DVL observation equation can be written as follows: ;when When the distance is USBL distance or distance difference, the USBL observation equation can be written as: or ;when When the USBL position is used, the observation equation can be written as follows: .
[0089] in, These are DVL velocity observations. For DVL observation prediction function, This is for DVL observation noise; These are the merged USBL observations. This represents the USBL distance observation prediction function corresponding to the i-th array element. This represents the USBL distance difference observation prediction function for the i-th array element relative to the reference array element 0. Represents the USBL location observation prediction function; This indicates the position of the INS reference center in the navigation coordinate system. This represents the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system. The mounting arm representing the target acoustic center relative to the INS reference center. This represents the position of the i-th USBL element in the navigation coordinate system. This indicates the position of the reference array element in the navigation coordinate system.
[0090] For any valid observation Calculate innovation, innovation covariance, Kalman gain, and error state correction:
[0091]
[0092]
[0093]
[0094] in, This represents the currently valid observations participating in the filter update, which can be DVL observations or fused USBL observations; These are the observed predicted values calculated based on the predicted state; For observational innovation or observational residuals, it is used to represent the difference between actual observations and predicted observations; This is the linearization matrix of the observation equation with respect to the error state; To innovate the covariance matrix; This is the measurement noise covariance matrix corresponding to the current observation; Kalman gain; This is the error state correction amount obtained from the filtering calculation.
[0095] according to Update nominal status: , The attitude is corrected using quaternions or direction cosine matrices at a small angle δθ. The gyroscope zero bias and accelerometer zero bias are respectively... and Correction; covariance can be calculated as follows: Update. Among them, , These are the updated position and velocity, respectively. , These are the predicted location and the predicted velocity, respectively. The updated covariance matrix, It is an identity matrix. Therefore, the position, velocity, attitude, and related bias states are updated through filtering.
[0096] The anomaly detection threshold is determined based on NIS statistics, peak quality index, and historical continuity. For observations of dimension m, the basic threshold can be derived from a chi-square distribution. Quantiles at confidence level α; Peak quality index and historical continuity indicators Used to adjust confidence levels or covariance. One specific method is: , .in, Let be the confidence level used for anomaly detection in the k-th epoch. Represents the amplitude limiting function. This is a historical continuity indicator, where m is the observation dimension. Let k be the NIS detection threshold. This represents a chi-square distribution with m degrees of freedom at a confidence level. quantiles below, This is a normalized innovation squared statistic used to determine whether the current observed state matches the predicted state. When the peak quality is poor or historical continuity is poor, As the threshold decreases, it becomes more stringent; when the peak quality is high and historically continuous, the threshold is used at the normal confidence level.
[0097]
[0098] like If the observation quality meets the requirements, then the observation will be accepted and updated normally; if Slightly higher However, it did not exceed the rejection threshold. If so, a delayed confirmation or sliding window re-evaluation will be performed, and can be pressed. expansion ;like If the peak quality is below the minimum threshold, the observation will be rejected, triggering short-term inertial navigation hold, false peak alarm, or fault detection to prevent abnormal USBL observations from destroying the combined solution.
[0099] The navigation state includes the carrier's position, speed, attitude, gyroscope zero bias, accelerometer zero bias, and related bias states such as DVL scaling factor, USBL system delay, or sound speed error introduced according to the engineering configuration.
[0100] In one exemplary embodiment, such as Figure 2 As shown, the positioning method includes the following steps: (1) Data preparation: Collect the original amplitude curve of USBL, USBL device output, INS data and DVL data; perform time synchronization, coordinate calibration, sound velocity compensation, installation deviation compensation and abnormal point removal.
[0101] (2) Preprocessing layer: Based on INS / DVL, state propagation and velocity constraints are performed, and the predicted position, predicted velocity and predicted attitude are output. The theoretical propagation delay and local search window are also calculated.
[0102] (3) Prior and back-inference layer: Extract candidate peaks within the local search window, calculate peak amplitude, SNR, peak width consistency, sharpness and time continuity; determine whether the score exceeds the threshold. If it exceeds the threshold, lock the optimal peak and construct pseudo-USBL observations. Otherwise, expand the search window once or reduce the observation confidence; finally output pseudo-USBL observations and peak confidence.
[0103] (4) Observation fusion layer: Read USBL device observations and quality indicators, perform consistency checks, dynamic weighting and covariance adjustment; fuse pseudo-USBL observations with device observations, and output corrected USBL measurements and their covariance.
[0104] (5) State estimation and output layer: The corrected USBL measurement and INS / DVL data are input into the central integrated navigation filtering module for combined filtering to update the position, velocity, attitude and bias; if the residual or NIS is abnormal, covariance expansion, delay confirmation, error alarm or fault judgment are performed; otherwise, the navigation results and state information are directly output.
[0105] This invention does not only perform post-fusion on the final USBL solution, but first uses the INS / DVL prior constraints to search for the peak value in the original correlation amplitude curve, then forms the corrected USBL distance, distance difference or position observation, and enters the central integrated navigation filter to achieve a deeper level of information coupling.
[0106] In another embodiment, the present invention provides an underwater combined positioning system based on INS, DVL, and USBL, such as Figure 3 As shown, it includes: A data acquisition device is used to acquire INS measurement data, DVL measurement data, USBL raw acoustic amplitude curves, and USBL device-derived observation data; exemplarily, the device includes a USBL raw data acquisition module, a USBL device decoding output module, an INS measurement module, and a DVL measurement module.
[0107] The time synchronization and coordinate calibration module is used to synchronize the time and transform the spatial coordinates of INS measurement data, DVL measurement data, USBL raw acoustic amplitude curves, and USBL device-derived observation data, ensuring that all data are aligned under a unified time reference and a unified spatial coordinate system. This module performs installation angle compensation, boom velocity compensation, and abnormal velocity removal on the DVL data; and performs array element baseline coordinate transformation, transponder boom compensation, system delay compensation, and sound velocity compensation on the USBL data.
[0108] The INS / DVL prior estimation module is used to combine and recursively deduce the predicted position, predicted velocity, and predicted attitude based on the INS measurement data and DVL measurement data; and to calculate the theoretical propagation distance and theoretical propagation delay of the target acoustic signal to each USBL array element based on the prior pose information, thereby obtaining the local search window.
[0109] The acoustic peak inversion module is used to perform peak search on the original acoustic amplitude curve of USBL within a local search window. It determines the optimal peak value based on peak amplitude, signal-to-noise ratio, peak width consistency, peak sharpness, and time continuity, and constructs pseudo USBL observation data and its covariance.
[0110] The adaptive measurement fusion module is used to perform consistency assessment, dynamic weighting, and covariance adjustment on pseudo-USBL observation data and USBL device-derived observation data, and output corrected USBL measurements and their covariance.
[0111] The central integrated navigation filtering module is used to combine and filter the fused USBL observation data with INS measurement data and DVL measurement data to obtain the estimated results of the carrier's position, velocity, attitude and bias state.
[0112] The output and fault diagnosis module is used to output the navigation status estimation results of the carrier and perform false peak alarms, observation rejection, covariance inflation and failure diagnosis based on NIS statistics, peak quality and historical continuity.
[0113] In this embodiment, the inputs, outputs, and main functions of each module are shown in Table 1 below; Table 1
[0114] In a preferred embodiment, the system is installed on an underwater vehicle and its support platform, and includes: at least one INS, one DVL, one USBL array element and transponder system, one embedded navigation calculator, and a time synchronization unit. During initialization, the system first reads the mounting arm, array element baseline coordinates, transponder number, sound velocity parameters, time delay compensation parameters, and initial filter values; it then establishes the transformation relationship between the vehicle coordinate system, navigation coordinate system, and USBL array coordinate system; subsequently, the INS outputs angular velocity, specific force, attitude, and velocity priors; the DVL outputs the vehicle's relative velocity; and the USBL acquires the original correlation amplitude curves and the equipment's conventional calculation results. After alignment by the time synchronization unit, the navigation calculator performs peak back-calculation, adaptive fusion, and central combined filtering.
[0115] To verify the effectiveness of this application, pool tests, near-shore sea trials, or dockside static deployment tests were conducted. In one embodiment, the test platform is an underwater vehicle equipped with INS, DVL, and USBL systems; USBL array elements are arranged on the hull, support, or fixed platform; during the test, the original acoustic correlation amplitude curves, USBL device output, INS data, and DVL data are recorded simultaneously.
[0116] The test trajectory may include straight sections, turning sections, acceleration / deceleration sections, and low signal-to-noise ratio interference sections to cover different motion states and different acoustic environments. To verify the ability of this invention to suppress false peaks and jump peaks, time periods with obvious multipaths, main lobe distortion, or strong side lobes can be selected as key test intervals.
[0117] The specific test items and implementation methods are shown in Table 2 below: Table 2
[0118] During testing, the method of this invention was compared with traditional methods on the same original dataset. First, the peak detection results were compared, and the correct peak locking rate and false peak rate were statistically analyzed. Then, the smoothness and continuity of the USBL positioning sequence were compared. Finally, the root mean square error of position, maximum error, and number of abnormal jumps in the combined navigation results were compared.
[0119] If an external reference true value is available, high-precision LBL, USBL, RTK surface calibration points, or offline post-processing results can be used as reference trajectories; if no high-precision true value is available, the effectiveness of this invention can be verified through cross-comparison of multiple methods, consistency of repeated voyages, and consistency of innovative statistics.
[0120] In this embodiment, the effects are compared as shown in Table 3 below: Table 3
[0121] Experimental results show that, compared with traditional global peak detection methods, the present invention can more stably lock the true acoustic peak under the theoretical arrival window constraint, and the phenomena of false peaks, peak jumps and missed detections are significantly reduced. Compared with the conventional method of using only USBL device output and INS / DVL for conventional combination, the USBL observation sequence output by the present invention is smoother and abnormal jumps are significantly reduced.
[0122] Furthermore, by feeding the corrected USBL observations obtained in this invention into the central integrated navigation filter, the positioning accuracy and trajectory continuity of the integrated navigation can be effectively improved, the filter innovation statistics can be kept within a reasonable range, and the robustness and availability of the system in long-term operation can be improved.
[0123] In summary, this invention achieves deep fusion of raw USBL acoustic information and inertial / velocity information through a system-level implementation route of "INS / DVL prior acoustic peak estimation - USBL observation correction - central deep integrated navigation," making it suitable for highly reliable underwater positioning and navigation in complex sea conditions.
[0124] The embodiments of the present invention have been described in detail above, and the principles and implementation methods of the present invention have been explained. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of the present invention. Those skilled in the art should understand that the embodiments of the present invention can be provided as methods, computer software program products, or electronic devices, etc. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0125] It should be noted that the word "comprising" does not exclude the presence of components or steps not listed in the claims. The words "a" or "an" preceding a component do not exclude the presence of a plurality of such components. This invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer.
[0126] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.
[0127] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. An underwater combined positioning method based on INS, DVL, and USBL, characterized in that, include: Acquire INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curves; The prior pose information of the carrier is obtained by combining and recursively deriving the INS measurement data and DVL measurement data. The theoretical propagation delay of the target acoustic signal to each USBL array element is calculated based on the prior pose information to obtain a local search window; Within the local search window, a peak search is performed on the original acoustic amplitude curve of the USBL to determine the optimal peak value and construct pseudo USBL observation data. The pseudo-USBL observation data is fused with the observation data calculated by the USBL device, and the fused USBL observation data is combined and filtered with the INS measurement data and the DVL measurement data to obtain the navigation state estimation result of the carrier.
2. The method as described in claim 1, characterized in that, After obtaining the raw acoustic amplitude curve of USBL, preprocessing should be performed first, including at least mean removal, limiting, bandpass, or smoothing.
3. The method as described in claim 1, characterized in that, The acquired INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curves are time-synchronized and spatially calibrated to align the data under a unified time reference and spatial coordinate system.
4. The method as described in claim 1, characterized in that, The prior pose information includes the predicted position, predicted velocity, and predicted attitude of the carrier. Based on the prior pose information, the theoretical propagation delay of the target acoustic signal to each USBL array element is calculated using the following formula: In the formula, For the propagation structure coefficient, The INS / DVL predicted position of the carrier at the k-th epoch. This is the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system. This refers to the mounting arm of the USBL transponder or acoustic transmission center on the carrier relative to the INS reference center. The INS / DVL predicted velocity of the carrier at the k-th epoch. To compensate for time during exercise, Let be the position of the i-th USBL element in the navigation coordinate system. For equivalent speed of sound, This is the system delay compensation term for the i-th channel; online correction is achieved using moving average or first-order filtering. In the formula, For update coefficients less than 1, To lock in the optimal peak latency, This represents the theoretical propagation distance.
5. The method as described in claim 1, characterized in that, The local search window is represented as follows: in Indicates the theoretical propagation delay. This represents the half-width of the search window for the i-th channel at the k-th epoch. In the formula, This refers to the clip function. The proportionality coefficient corresponding to the confidence probability; This represents the standard deviation of the time delay obtained from the conversion of the INS / DVL predicted position uncertainty; The time delay standard deviation is calculated after the attitude uncertainty is amplified by the acoustic arm and array element baseline. This represents the motion compensation uncertainty caused by the rate of change of velocity or the time synchronization residual. This represents the standard deviation of time delay caused by the uncertainty of the speed of sound; This represents the clock synchronization residual or system delay compensation residual. and These represent the minimum and maximum allowed window half-width values for the project, respectively.
6. The method as described in claim 1, characterized in that, Peak search is performed within the local search window to determine the optimal peak value, including: The amplitude, signal-to-noise ratio, peak sharpness, and temporal continuity of the candidate peaks within the local search window are obtained, and a weighted score is calculated to obtain the score result. The optimal peak value is determined based on the scoring results.
7. The method as described in claim 1, characterized in that, The pseudo-USBL observation data is fused with the observation data calculated by the USBL device, including: in, This represents the observations of the fused data. This represents the covariance of the fused data. This represents the adjusted covariance of the pseudo-USBL observation data. The covariance of the observed data calculated for the adjusted USBL device. This is pseudo-USBL observation data. Observational data processed for USBL devices.
8. An underwater combined positioning system based on INS, DVL, and USBL, characterized in that, The underwater combined positioning method based on INS, DVL, and USBL as described in any one of claims 1-7 includes: The data acquisition device is used to acquire INS measurement data, DVL measurement data, and USBL raw acoustic amplitude curves; The INS / DVL prior estimation module is used to obtain the prior pose information of the carrier by combining and recursively deriving the INS measurement data and DVL measurement data; and to calculate the theoretical propagation delay of the target acoustic signal to each USBL array element based on the prior pose information to obtain a local search window. The acoustic peak inversion module is used to perform peak search on the original acoustic amplitude curve of the USBL within the local search window to determine the optimal peak and construct pseudo USBL observation data. An adaptive measurement fusion module is used to fuse the pseudo USBL observation data with the observation data solved by the USBL device; The central integrated navigation filtering module is used to combine and filter the fused USBL observation data with the INS measurement data and the DVL measurement data to obtain the navigation state estimation result of the vehicle.
9. The system as described in claim 8, characterized in that, It also includes a time synchronization and coordinate calibration module, which is used to perform time synchronization and spatial coordinate calibration on the acquired INS measurement data, DVL measurement data and USBL raw acoustic amplitude curve, so that the data are aligned under a unified time reference and spatial coordinate system.
10. The system as described in claim 8, characterized in that, It also includes a result output and fault diagnosis module, which is used to output the navigation status estimation results of the carrier and perform false peak alarms and failure diagnosis.
