DVL quality adaptive underwater navigation method and system

Abstract

The application relates to the technical field of underwater navigation, and discloses a DVL quality adaptive underwater navigation method and system, which comprises the following steps: acquiring IMU data, DVL data and depth data of an underwater robot for time alignment, performing state prediction based on the IMU data, extracting DVL multi-dimensional quality information from the time-aligned DVL data, combining speed observation in the DVL data with predicted speed in prior navigation state to calculate speed residual error, constructing DVL comprehensive reliability for representing the reliability degree of current DVL observation, dynamically adjusting an initial DVL measurement noise matrix based on the DVL comprehensive reliability, performing vertical position constraint on the prior navigation state based on the depth data, determining an update mode according to the DVL comprehensive reliability and the number of effective beams, and outputting a navigation state, so that the application realizes smooth switching of the update mode when the DVL state changes, and avoids mode jitter and sudden recovery in a critical state.

Description

Technical Field

[0001] This invention relates to the field of underwater navigation technology, and in particular to a DVL quality-adaptive underwater navigation method and system. Background Technology

[0002] In operations such as marine resource development, underwater structure inspection, and subsea pipeline detection, underwater robots need to continuously acquire their position, velocity, and attitude information. Since satellite navigation signals are difficult to propagate in the underwater environment, underwater robots typically employ a combined navigation system consisting of an Inertial Measurement Unit (IMU), a Doppler Log (DVL), and a depth sensor. The IMU provides high-frequency, continuous state propagation, but its integration error accumulates rapidly over time. The DVL utilizes the acoustic Doppler effect to measure the underwater robot's velocity relative to the seabed and is a core external observation sensor for suppressing inertial drift. The depth sensor provides stable vertical position constraints. Therefore, IMU / DVL / depth combined navigation has become the primary technology for underwater robot navigation.

[0003] However, existing integrated navigation methods still have the following drawbacks. First, most existing methods adopt a loosely coupled structure, directly using the three-dimensional velocity calculated by the DVL manufacturer as the filter observation, and setting fixed measurement noise, assuming that the DVL measurement accuracy is constant. However, in the real underwater environment, the DVL beam quality changes rapidly with factors such as the height above the bottom, seabed material, attitude changes, and acoustic multipath. Fixed measurement noise cannot reflect the dynamic changes in the reliability of the observation, which leads to abnormal velocities contaminating the filtering results when the DVL quality deteriorates, causing navigation jitter or even filter divergence.

[0004] Secondly, the manufacturer's velocity is calculated from multiple beams. Anomalies in individual beams can contaminate the overall velocity. Existing methods cannot detect the source of degradation at the beam level. The hard threshold rejection strategy commonly used for DVL anomalies discards observations that are still partially usable, wasting valuable information. Furthermore, it is prone to frequent switching at critical states, causing estimation jitter. Immediately reconnecting with normal weights when DVL recovers will also introduce unstable velocities into the filter. Therefore, at this stage, there is a need for a DVL quality adaptive underwater navigation method and system. Summary of the Invention

[0005] To address the problems of fixed DVL measurement noise, inability to adapt to changes in the underwater environment, and easy contamination of filtering results by abnormal observations in existing IMU / DVL integrated navigation methods, this invention provides a DVL quality adaptive underwater navigation method and system.

[0006] In a first aspect, the present invention provides a DVL (Device Quality Adaptive) underwater navigation method, which adopts the following technical solution: A DVL quality-adaptive underwater navigation method includes: Acquire IMU data, DVL data, and depth data of the underwater robot. Use the timestamp of the IMU data as the master clock to time-align the DVL data and depth data. Based on the IMU data, perform state prediction to obtain the prior navigation state. Extract DVL multidimensional quality information from time-aligned DVL data, and calculate velocity residuals by combining velocity observations from DVL data with predicted velocities from prior navigation states. Based on the DVL multidimensional quality information and velocity residuals, the effective beam set is screened and the number of effective beams is determined. Based on the DVL multidimensional quality information, velocity residuals, effective beam set and number of effective beams, a DVL comprehensive reliability characterizing the reliability of the current DVL observation is constructed. The initial DVL measurement noise matrix is ​​dynamically adjusted based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix. Vertical position constraints are applied to the prior navigation state based on depth data to obtain the vertically constrained navigation state and its covariance matrix. The DVL update mode is determined based on the overall reliability of DVL and the number of effective beams. Based on the DVL update mode, the effective beam set, and the dynamic DVL measurement noise matrix, the navigation state after vertical constraints and its covariance matrix are filtered and updated to output the navigation state of the underwater robot.

[0007] Furthermore, the time alignment of DVL data and depth data includes calculating the time difference between the DVL data timestamp and the depth data timestamp and the most recent IMU prediction time, and comparing each time difference with a preset time synchronization threshold. When the time difference is less than the preset time synchronization threshold, the corresponding DVL data or depth data will be associated with the current IMU prediction state. When the time difference is greater than or equal to the preset time synchronization threshold, a time synchronization weight is constructed based on the time difference, and the credibility of the corresponding DVL observation or depth observation is reduced according to the time synchronization weight.

[0008] Furthermore, obtaining the prior navigation state includes reading the IMU's three-axis acceleration, IMU's three-axis angular velocity, and IMU timestamp, and calculating the sampling interval between adjacent IMU sampling times; Based on the attitude matrix of the previous moment, the system acceleration is transformed to the navigation coordinate system and the gravity term is subtracted to obtain the motion acceleration in the navigation coordinate system; Based on motion acceleration and sampling interval, discrete integral prediction is performed on the posterior velocity and posterior position of the previous time step to obtain the prior velocity and prior position of the current time step. Based on the propagation prediction covariance matrix of the state transition matrix and the process noise matrix, the prior covariance matrix is ​​obtained. The prior navigation state includes prior velocity, prior position and prior covariance matrix.

[0009] Furthermore, the calculation of velocity residuals by combining velocity observations in DVL data with predicted velocities in prior navigation states includes reading time-aligned DVL data, extracting radial velocity, beam state, beam quality, beam signal-to-noise ratio, and beam distance of the four beams, establishing a beam-by-beam data set, and extracting DVL altitude and the time difference between DVL data and IMU prediction time to constitute DVL multidimensional quality information. The predicted velocity is extracted from the prior navigation state, and the velocity residual between the velocity observation and the predicted velocity in the DVL data is calculated. The velocity residual is used to characterize the consistency between the DVL velocity observation and the state prediction.

[0010] Furthermore, the construction of the DVL comprehensive credibility characterizing the reliability of the current DVL observation includes constructing DVL validity weights based on validity indicators in the DVL data and obtaining time synchronization weights. The effective beam set is selected based on the beam status, beam quality, beam signal-to-noise ratio and beam distance of the four beams. The effective beam quantity weight is constructed based on the effective beam set, and the beam quality weight is constructed based on the normalized quality value of each effective beam. The distance consistency weight is constructed based on the dispersion of the four-beam distance. The height anomaly weight is constructed based on the change between the current DVL height and the previous frame's DVL height. The velocity residual weight is constructed based on the norm of the velocity residual. Finally, the weights are multiplied and fused, and the overall credibility of the DVL is obtained by constraining with the minimum credibility lower bound.

[0011] Furthermore, obtaining the dynamic DVL measurement noise matrix includes setting an initial DVL measurement noise matrix under normal operating conditions and reading the overall reliability of the DVL in the current frame. The initial DVL measurement noise matrix is ​​amplified according to the DVL comprehensive reliability to obtain the dynamic DVL measurement noise matrix. The dynamic DVL measurement noise matrix is ​​input into the filter as the measurement noise for updating the DVL observations of the current frame, in order to reduce the weight of DVL observations with reduced confidence in the filter update. The formula for calculating the DVL measurement noise matrix is ​​as follows: , in, The dynamic DVL measurement noise matrix for the k-th frame. This is the initial DVL measurement noise matrix under normal operating conditions. The overall credibility of the DVL in the k-th frame is... To prevent small constants from being divided by zero.

[0012] Furthermore, the vertical position constraint on the prior navigation state based on depth data includes reading depth sensor data and depth timestamps, and calculating the time difference between the depth timestamp and the current IMU prediction time; When the time difference meets the preset synchronization conditions, the depth sensor data is converted into a vertical position observation consistent with the navigation coordinate system, and a depth measurement residual is constructed based on the vertical position observation and the vertical position in the prior navigation state. Based on the depth measurement residual, the prior navigation state is updated with vertical position constraints to obtain the vertically constrained navigation state and its covariance matrix.

[0013] Further, determining the DVL update mode based on the DVL comprehensive reliability and the number of effective beams includes comparing the DVL comprehensive reliability of the k-th frame with a preset normal update threshold, a preset weak update threshold, and a preset failure threshold, respectively, and comparing the number of effective beams of the k-th frame with a preset minimum number of beams for normal update and a preset minimum number of beams for weak update, respectively. Based on the comparison results, the DVL update mode is determined as a normal update mode, a downweighted update mode, a weak update mode, or a DVL loss mode. The determination relationship is as follows: , In the formula, For the DVL update mode of frame k, , , and These correspond to normal update mode, degraded update mode, weak update mode, and DVL loss mode, respectively. The overall credibility of the DVL in the k-th frame is... For normal threshold updates, For weak update threshold, The failure threshold The minimum number of effective beams required to allow for normal updates. The number of effective beams in the k-th frame. The minimum number of effective beams required to allow weak updates or dimensionality reduction updates; When the DVL update mode is the DVL loss mode, the recovery detection is performed on the DVL data of each subsequent frame. Only when the DVL comprehensive confidence is not lower than the preset recovery threshold for M consecutive frames and the number of effective beams in each corresponding frame is not lower than the preset minimum number of beams for normal update, it switches to normal update mode.

[0014] Furthermore, the output of the underwater robot's navigation status includes, when the DVL update mode is normal update mode, reduced-weight update mode, or weak update mode, selecting an observation matrix based on the total number of DVL beams and the number of effective beams; when the number of effective beams is equal to the total number of DVL beams, constructing a full-beam observation matrix; when the number of effective beams is less than the total number of DVL beams but not less than the preset minimum number of effective beams for weak update, constructing a dimension-reduced observation matrix based on the set of effective beams. The corresponding mode coefficients are applied to the dynamic DVL measurement noise matrix according to the DVL update mode, and the Kalman gain is calculated based on the full beam observation matrix or the reduced-dimensional observation matrix, the covariance matrix after vertical constraint, and the dynamic DVL measurement noise matrix that matches the effective beam set. The position and velocity in the vertically constrained navigation state are corrected and updated using Kalman gain and effective beam observation residuals to obtain the posterior navigation state. When the DVL update mode is DVL loss mode, skip the DVL update and output the navigation state after vertical constraints as the posterior navigation state. The effective beam set satisfies: , in, For the set of effective beams in the k-th frame, This represents the beam state of the i-th beam in the k-th frame. The beam quality of the i-th beam in the k-th frame. To preset the quality threshold, Let be the beam signal-to-noise ratio of the i-th beam in the k-th frame. To preset the signal-to-noise ratio threshold, For the k-th frame i Beam distance of each beam As a preset lower limit of distance, This is the preset maximum distance.

[0015] Secondly, a DVL quality-adaptive underwater navigation system includes: The data acquisition module is used to acquire IMU data, DVL data, and depth sensor data of the underwater robot. The time synchronization module is used to align the DVL data and depth sensor data with the IMU timestamp as the master clock. The state prediction module is used to predict the state based on IMU data and obtain the prior navigation state. The DVL quality analysis module is used to extract DVL multidimensional quality information from time-aligned DVL data and calculate the velocity residual by combining DVL velocity observations with predicted velocities in prior navigation states. The comprehensive credibility calculation module is used to filter the effective beam set and determine the number of effective beams, and construct the DVL comprehensive credibility that characterizes the reliability of the current DVL observation; The dynamic measurement noise construction module is used to dynamically adjust the initial DVL measurement noise matrix based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix. The vertical position constraint module is used to apply vertical position constraints to the prior navigation state based on depth sensor data, and obtain the vertically constrained navigation state and its covariance matrix. The mode switching and filter update module is used to determine the DVL update mode based on the DVL comprehensive reliability and the number of effective beams, and to filter and update the navigation state and its covariance matrix after vertical constraint based on the DVL update mode, the effective beam set and the dynamic DVL measurement noise matrix. The navigation status output module is used to output the navigation status of the underwater robot.

[0016] In summary, the present invention has the following beneficial technical effects: 1. This invention constructs a multi-dimensional comprehensive reliability that integrates DVL validity, time synchronization error, effective beam count, beam quality, beam distance consistency, DVL height anomaly, and velocity residual. Based on this reliability, the DVL measurement noise matrix is ​​dynamically adjusted, thereby achieving real-time quantitative evaluation of DVL observation reliability and adaptive matching of measurement noise. This improves the navigation robustness of underwater robots under DVL degradation conditions.

[0017] 2. This invention filters the effective beam set based on beam status, beam quality, beam signal-to-noise ratio, and beam distance range. When the number of effective beams is less than the total number of beams but greater than or equal to the preset minimum number of effective beams, a dimension-reduced observation matrix is ​​constructed to perform partial beam updates. This enables full utilization of residual effective information when some beams in DVL are abnormal, avoids the waste of effective information caused by traditional hard threshold elimination strategies, and improves information utilization and navigation continuity under degraded conditions.

[0018] 3. This invention uses depth sensor data for vertical position constraints and uses DVL height change as a bottom tracking quality evaluation factor to maintain vertical position stability when DVL is downweighted, weakly updated, or lost, thereby enhancing the vertical stability of the system under short-term DVL failure conditions.

[0019] 4. This invention sets up multiple modes such as normal update, deweighted update, weak update, DVL loss and recovery detection based on the DVL comprehensive reliability and the number of effective beams. It also adopts a continuous multi-frame detection and recovery hysteresis interval mechanism to achieve smooth switching of update mode when DVL state changes, avoiding mode jitter and recovery abrupt changes in critical states. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the overall process of a DVL quality-adaptive underwater navigation method according to an embodiment of the present invention.

[0021] Figure 2 This is a flowchart of DVL comprehensive credibility calculation according to an embodiment of the present invention.

[0022] Figure 3 This is a schematic diagram of the adaptive measurement noise structure according to an embodiment of the present invention.

[0023] Figure 4 This is a state machine diagram for mode switching according to an embodiment of the present invention.

[0024] Figure 5 This is a flowchart of the IMU / DVL / depth filtering fusion process according to an embodiment of the present invention.

[0025] Figure 6 This is a depth and height auxiliary anomaly discrimination map according to an embodiment of the present invention.

[0026] Figure 7 This is a system module structure diagram of an embodiment of the present invention.

[0027] Figure 8 This is a schematic diagram illustrating the implementation results of an embodiment of the present invention.

[0028] Figure 9 This is a schematic diagram of the performance verification of the dynamic measurement noise algorithm under the DVL abnormal scenario in an embodiment of the present invention, wherein (a) is the DVL observation speed and abnormal score, (b) is the adaptive pseudo-measurement noise intensity, (c) is the Kalman gain and log10 noise matrix trace, and (d) is the local navigation and positioning error. Detailed Implementation

[0029] The present invention will be further described in detail below with reference to the accompanying drawings.

[0030] Example 1 Reference Figure 1 This embodiment of a DVL quality-adaptive underwater navigation method includes: S1. Acquire IMU data, DVL data and depth data of the underwater robot. Use the timestamp of the IMU data as the main clock to time-align the DVL data and depth data, and perform state prediction based on the IMU data to obtain the prior navigation state. S2. Extract DVL multidimensional quality information from the time-aligned DVL data, and calculate the velocity residual by combining the velocity observation in the DVL data with the predicted velocity in the prior navigation state. S3. Based on the DVL multidimensional quality information and velocity residuals, select the effective beam set and determine the number of effective beams. Based on the DVL multidimensional quality information, velocity residuals, effective beam set and number of effective beams, construct the DVL comprehensive credibility that characterizes the reliability of the current DVL observation. S4. Based on the DVL comprehensive credibility, the initial DVL measurement noise matrix is ​​dynamically adjusted to obtain the dynamic DVL measurement noise matrix. S5. Based on depth data, apply vertical position constraints to the prior navigation state to obtain the vertically constrained navigation state and its covariance matrix. S6. Determine the DVL update mode based on the DVL comprehensive reliability and the number of effective beams, and filter and update the navigation state and its covariance matrix after vertical constraint based on the DVL update mode, the effective beam set and the dynamic DVL measurement noise matrix, and output the navigation state of the underwater robot.

[0031] Specifically, a DVL quality-adaptive underwater navigation method includes the following: like Figure 1 As shown in Figure S1, in this embodiment, the underwater robot is equipped with an inertial measurement unit (IMU), a Doppler log (DVL), and a depth sensor. The IMU, as a high-frequency sensor, outputs triaxial acceleration, triaxial angular velocity, and timestamps. The DVL and depth sensor, as low-frequency observation sensors, asynchronously output observation data. The system uses the timestamp of the IMU data as the master clock to perform state prediction. The DVL data and depth data are only updated by filtering when they arrive and meet the time alignment conditions. Therefore, it is not necessary for each sensor to have the same sampling frequency.

[0032] Specifically, for the k-th frame of DVL data, let its timestamp be... The system iterates through all IMU prediction times and determines the IMU prediction time closest to the given timestamp. The system calculates the time difference between the two data points; depth data is processed in the same way to determine the nearest IMU prediction time and calculate the time difference. The system then compares each time difference with a preset time synchronization threshold. When the time difference is less than the preset time synchronization threshold, it indicates that the observation in this frame has a reliable time correspondence with the current IMU prediction state. The system associates the corresponding DVL data or depth data with the current IMU prediction state and resets the DVL time synchronization weight to 1. When the time difference of the DVL data is greater than or equal to the preset time synchronization threshold, the system constructs the DVL time synchronization weight based on the time difference in an exponentially decaying manner, so that the greater the time misalignment of the DVL observation, the weaker its impact in subsequent filtering updates. When the time difference of the depth data is greater than or equal to the preset time synchronization threshold and there are valid depth observations in adjacent moments, the system uses linear interpolation to obtain the depth observation at the current IMU prediction moment; otherwise, the depth data of this frame is deemed invalid and the vertical position constraint of this frame is skipped. The calculation formulas for the DVL time difference and the time synchronization weight are as follows: , , in, Let be the absolute value of the time difference between the k-th frame of DVL data and the most recent IMU prediction time. The time synchronization weight of the k-th frame of DVL data. The timestamp of the k-th frame of DVL data. The IMU prediction time closest to the k-th frame of DVL data. This is a parameter for time synchronization error attenuation; when the time difference is less than a preset time synchronization threshold... Set to 1. When the time difference is greater than or equal to the preset time synchronization threshold, calculate according to the above formula. Depth data is not scaled using this weight to reduce depth measurement noise; instead, it is directly correlated, linearly interpolated, or vertical position constraints are skipped based on time synchronization conditions.

[0033] After completing time alignment, the system performs state prediction based on IMU data. Specifically, at each IMU sampling moment, the system reads the IMU's three-axis acceleration, three-axis angular velocity, and timestamp, and calculates the sampling interval based on the timestamps of adjacent IMU sampling moments. The system uses discrete strapdown attitude update based on the three-axis angular velocity and sampling interval to obtain the current attitude matrix. Then, based on the current attitude matrix, the system's acceleration is converted to the navigation coordinate system and the gravity term is subtracted to obtain the motion acceleration in the navigation coordinate system. Subsequently, based on the motion acceleration and sampling interval, the system performs discrete integral prediction on the posterior velocity and posterior position of the previous moment to obtain the prior velocity and prior position of the current moment. The calculation formulas for the sampling interval, motion acceleration, prior velocity, and prior position are as follows: , , , , in, Let be the sampling interval at the k-th IMU sampling time. This is the timestamp of the k-th IMU sampling time. This is the timestamp of the (k-1)th IMU sampling time. Let be the acceleration in the navigation coordinate system at the k-th IMU sampling time. The attitude matrix from the machine system to the navigation coordinate system. For the system acceleration at the k-th IMU sampling time, This is the gravity vector in the navigation coordinate system. The prior velocity at the current moment, The posterior velocity of the previous time step. The prior position at the current moment, This is the posterior position of the previous time step.

[0034] Meanwhile, to characterize the uncertainty of state prediction and provide prior uncertainty for subsequent DVL velocity and depth constraint updates, the system predicts the covariance matrix based on the propagation of the state transition matrix and process noise matrix, obtaining the prior covariance matrix, which is calculated as follows: , in, Let be the prior covariance matrix at the k-th IMU sampling time. Let be the state transition matrix at the k-th IMU sampling time. Let be the posterior covariance matrix at the (k-1)th IMU sampling time. Let be the process noise matrix at the k-th IMU sampling time. The prior navigation state includes prior velocity, prior position, and prior covariance matrix. The system can output continuous prior navigation state at each IMU sampling time. Since the IMU integration error accumulates over time, subsequent steps use time-aligned DVL observations and depth observations to periodically correct the prior navigation state.

[0035] S2 and DVL observe seabed reflection information through four acoustic beams. The system reads the k-th frame of DVL data after time alignment in step S1, and extracts the radial velocity, beam state, beam quality, beam signal-to-noise ratio, and beam distance of the first to fourth beams to establish a beam-by-beam data set. Simultaneously, it extracts the DVL height of this frame and the time difference between the DVL data timestamp and the associated IMU prediction time, combining these information to constitute the DVL multidimensional quality information. The beam-by-beam data set is defined as follows: , in, For the data set of the i-th beam in the k-th frame of DVL data, Let be the radial velocity of the i-th beam. The beam state of the i-th beam. Let i be the beam quality of the i-th beam. Let be the beam signal-to-noise ratio of the i-th beam. Let be the beam distance of the i-th beam.

[0036] While extracting DVL multidimensional quality information, the system extracts the predicted velocity from the prior navigation state obtained in step S1, and reads the time-aligned DVL velocity observations, calculating the velocity residual between the two. Specifically, in the implementation using the DVL manufacturer's calculated velocity, the system directly reads the navigation system's three-dimensional velocity calculated by the manufacturer as the DVL velocity observation; this velocity residual is used to characterize the consistency between the DVL velocity observation and the state prediction. When the DVL velocity jumps, experiences a bottom tracking error, or the time synchronization error increases, the velocity residual increases accordingly, and its calculation formula is: , in, Let the velocity residual be the velocity of the k-th frame. For the time-aligned DVL velocity observation of the k-th frame, The predicted velocity in the prior navigation state of the k-th frame is used as the input for constructing the DVL comprehensive reliability in subsequent steps, along with the velocity residual and the DVL multidimensional quality information.

[0037] like Figure 2 As shown in Figure S3, a comprehensive DVL reliability score is constructed based on the multidimensional quality information and velocity residuals of the DVL to characterize the reliability of the current DVL observation. This comprehensive reliability score is not a single threshold judgment result, but rather unifies discrete beam state information, continuous quality numerical information, geometric consistency information, bottom tracking auxiliary information, and filter residual information into the same reliability evaluation framework, forming a quantitative trust score of the system for the current DVL observation; the closer the score is to 1, the more reliable the current DVL velocity is, and the closer it is to 0, the more likely the current DVL velocity is to be affected by abnormal beams, bottom tracking failures, time misalignment, or velocity jumps.

[0038] Specifically, the system first reads the vendor validity flag, bottom tracking status, or beam status flag from the k-th frame of DVL data and compares it with a preset valid status code to determine the DVL validity weight. When the DVL is valid and bottom tracking is normal, the weight is 1; when the DVL is invalid or bottom tracking fails, the weight is 0. , Meanwhile, the system obtains the time synchronization weight calculated in step S1; this weight is constructed based on the time difference between the DVL timestamp and the IMU prediction time in an exponentially decaying form, and is used to deal with the problem of incomplete alignment between the DVL and IMU timestamps; the larger the time synchronization error, the smaller the exponential term, and the weaker the impact of DVL on the filter update.

[0039] Subsequently, the system extracts four-beam data from the DVL output of the k-th frame, reading the radial velocity, status flag, quality value, signal-to-noise ratio (SNR), and distance observation values ​​of the first to fourth beams respectively, establishing a beam-by-beam data set. For each beam, the system compares its status flag with a preset valid status code, and simultaneously determines whether the beam quality, SNR, and distance range meet preset valid conditions. When the beam status belongs to the valid status set, the beam quality is greater than or equal to the quality threshold, the SNR is greater than or equal to the SNR threshold, and the beam distance is within a reasonable distance range, the beam is marked as a valid beam, and the beam-by-beam valid flag is set to 1; otherwise, it is set to 0. Based on this, the number of valid beams, the weight of the number of valid beams, and the weight of the beam quality are calculated, i.e.: , , , , in, This is the beam-by-beam valid flag for the i-th beam in the k-th frame. The number of effective beams in the k-th frame. Weighting for the number of effective beams. Let be the normalized quality value of the i-th beam in the k-th frame, with a value range of [0,1]. For beam quality weights, To avoid small constants with zero denominators, the system no longer relies solely on the overall DVL validity flag through the above beam-by-beam screening and counting process. Instead, it can distinguish between different states such as all beams valid, some beams degraded, and all beams failed.

[0040] When the beam quality value output by the DVL device is not directly normalized to [0,1], the system uses the lower quality limit specified by the device. and upper limit of quality Perform truncated linear normalization to make When the device directly outputs a quality value in the range [0,1], this quality value is directly used as the normalized quality value. This represents the original beam quality value of the i-th beam in the k-th frame.

[0041] Furthermore, the system reads the four-beam distance of the DVL in the k-th frame, calculates the average distance and standard deviation of the four beams, and constructs a distance consistency weight based on the distance dispersion. The more dispersed the four-beam distances, the smaller the distance consistency weight, and the lower the system's confidence in the DVL. Simultaneously, the system reads the current DVL height and compares it with the height of the previous frame, constructing a height anomaly weight based on the height change. When the height change of the DVL in adjacent frames increases abnormally, it indicates that the acoustic ranging relationship between the DVL and the seabed may be unstable, and the reliability of the DVL needs to be reduced. The above weights satisfy the following conditions: , , , , in, The beam distance of the i-th beam in the k-th frame. The average distance of the four beams in the k-th frame. Let $k$ be the standard deviation of the four-beam distance in the k-th frame. The distance consistency penalty coefficient, To avoid small constants with a denominator of zero, For distance consistency weights, The height of the DVL in frame k is... The height of the DVL in frame k-1. The penalty coefficient for high jumps, The aforementioned distance consistency weight and height anomaly weight are not directly used as position observations in the filter, but are used as auxiliary judgment factors for DVL bottom tracking quality in the comprehensive confidence calculation. Thus, the DVL update weight can be reduced in advance based on distance and height changes before DVL velocity becomes significantly abnormal.

[0042] like Figure 6 As shown, for the first valid height frame, the previous valid height frame not existing, or the time interval between adjacent valid height frames exceeding a preset interval, the system assigns a height anomaly weight. Set the value to 1; otherwise, use the valid height that meets the requirement in the most recent time interval as the height of the previous frame. Calculate the weight of height anomalies.

[0043] Distance consistency penalty coefficient Based on the normalized distance discreteness in the normal bottom tracking samples The statistical distribution and target attenuation degree are determined. Dimensionless coefficient; high jump penalty coefficient The height change characteristic scale of adjacent frames is calculated based on the reasonable vertical motion rate of the underwater robot, the DVL update cycle, and the height measurement noise. It has the dimension of the reciprocal of length. This makes both exponential terms dimensionless, and gives normal samples a higher weight while reducing the weight of abnormal samples according to a set attenuation level. The above two weights use beam geometry and acoustic ranging information to judge the bottom tracking quality, and can provide auxiliary degradation indications before the velocity residual increases significantly.

[0044] Furthermore, the system constructs velocity residual weights based on the velocity residuals calculated in step S2. These velocity residuals are used to measure the consistency between DVL velocity observations and IMU predicted velocities. The larger the residual, the smaller the residual weight, and the lower the contribution of DVL observations to overall reliability. Unlike the hard threshold elimination method, this step continuously reduces the weight of DVL observations through velocity residual weights. When the residual increases slightly, the system still retains a certain DVL velocity constraint. When the residual remains large, dynamic measurement noise increases, and the impact of DVL on state correction is significantly weakened. This residual-driven mechanism can avoid the filter frequently switching from fully using DVL to completely discarding DVL in critical states, thereby reducing estimation jitter caused by mode abrupt changes. The velocity residual weights satisfy the following: , in, Let the velocity residual be the velocity of the k-th frame. Norm operators, To reduce the weighting of velocity residuals, This represents the weight of the velocity residual.

[0045] Finally, the system multiplies and fuses the aforementioned DVL validity weights, time synchronization weights, effective beam count weights, beam quality weights, range consistency weights, height anomaly weights, and velocity residual weights, and constrains them with a minimum confidence lower bound to obtain the overall DVL confidence of the k-th frame. This fusion relationship expresses the combined effect of multiple quality cues; if any one of these quality categories significantly decreases, the overall confidence will decrease accordingly. Simultaneously, to avoid instability in numerical calculations due to excessively low confidence, the system sets a minimum confidence lower bound to prevent meaningless divergence in subsequent dynamic measurement noise. The overall confidence satisfies the following: , in, Indicates the DVL validity weight. Indicates the time synchronization weight. Indicates the weight of the number of effective beams. Indicates beam quality weights. Indicates beam distance consistency weight. Indicates the weight of highly abnormal DVL values. Indicates the velocity residual weight. This represents the minimum credibility threshold.

[0046] The minimum confidence level is determined based on the stability of engineering numerical values ​​and the maximum permissible measurement noise scaling factor. The system enables... Not exceeding the preset maximum scaling factor, where, To prevent small constants from being divided by zero, offline calibration is performed by combining the IMU accuracy level, the nominal noise level of DVL, and the numerical stability of the filter; thus, abnormal observations are suppressed when DVL is extremely unreliable, while avoiding the unbounded increase of the dynamic measurement noise matrix.

[0047] like Figure 3 As shown, S4, the initial DVL measurement noise matrix is ​​dynamically adjusted based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix.

[0048] Traditional filtering methods typically use DVL velocity as the input filter for velocity observation under a fixed noise level, assuming that DVL has stable measurement accuracy throughout the mission and is unaffected by environmental changes. However, in real near-bottom operating environments, DVL observation errors are time-varying, directional, and sudden, and a fixed measurement noise matrix cannot accurately reflect the dynamic changes in DVL observation reliability. This embodiment transforms the overall DVL reliability obtained in step S3 into a dynamic measurement noise matrix that can be directly used by the filter, enabling DVL observation to shift from a fixed-weight input to a quality-aware adaptive input. Specifically, the system pre-sets an initial DVL measurement noise matrix under normal operating conditions, which reflects the nominal measurement accuracy of DVL under ideal bottom tracking conditions. Subsequently, the system reads the overall confidence level of the current frame's DVL and constructs an adaptive scaling factor for measurement noise based on this confidence level. This scaling factor is inversely proportional to the overall confidence level of the DVL. Then, the system dynamically scales the initial DVL measurement noise matrix based on the scaling factor to obtain a dynamic DVL measurement noise matrix. This dynamic DVL measurement noise matrix is ​​input into a filter as the measurement noise for updating the current frame's DVL observations, ensuring that DVL observations with lower confidence levels have smaller filter update weights. The dynamic DVL measurement noise matrix satisfies the following: , in, The dynamic DVL measurement noise matrix for the k-th frame. This is the initial DVL measurement noise matrix under normal operating conditions. The overall credibility of the DVL in the k-th frame is... To prevent small constants from being divided by zero, The above formula uses a division method to ensure that when the overall confidence level of the DVL decreases, the dynamic DVL measurement noise matrix increases accordingly; when the overall confidence level of the DVL is close to 1, the dynamic measurement noise matrix is ​​close to the initial measurement noise matrix, and the DVL participates in the update with normal weights; when the overall confidence level of the DVL decreases, the dynamic measurement noise matrix is ​​amplified, and the filter automatically reduces the impact of the DVL on state correction; when the overall confidence level of the DVL is below the failure threshold, the system skips the DVL update and only performs IMU prediction and depth constraints. This design achieves a continuous transition from reliable to unreliable. Compared with direct elimination, the dynamic measurement noise matrix can more delicately express the degree of DVL degradation; compared with fixed noise, it can reduce the contamination of navigation results by erroneous observations in abnormal environments, thus enabling the filter to achieve adaptive robustness for abnormal DVL without changing the basic Kalman filter mathematical framework.

[0049] S5. Based on depth sensor data, apply vertical position constraints to the prior navigation state to obtain the vertically constrained navigation state and its covariance matrix. Depth or pressure sensors can provide stability constraints for the underwater robot in the vertical direction, especially suitable for suppressing the IMU's integral drift in the vertical position; however, depth information itself cannot independently constrain the horizontal position, nor can it directly determine whether the DVL horizontal velocity is abnormal. Therefore, depth constraints and DVL velocity constraints need to be fused together to obtain the navigation state estimation result.

[0050] Specifically, the system reads the depth sensor data and its depth timestamp of the k-th frame, and calculates the time difference between the depth timestamp and the current IMU prediction time. When the time difference is less than a preset time synchronization threshold, the depth sensor data is directly associated with the current IMU prediction state. When the time difference is greater than or equal to the preset time synchronization threshold and there are valid depth observations in adjacent times, linear interpolation is used to obtain the depth observation at the current time. When the interpolation conditions are not met, the depth data of this frame is determined to be invalid and the vertical position constraint of this frame is skipped. When using the ENU coordinate system, the U-axis in the navigation coordinate system is upward, while depth is usually downward and positive. Therefore, the system converts the valid depth data into a vertical position observation consistent with the navigation coordinate system, and constructs a depth measurement residual based on the vertical position observation and the vertical position in the prior navigation state. The system performs a vertical position constraint update on the prior navigation state based on the depth measurement residuals, obtaining the vertically constrained navigation state and its covariance matrix. This vertical position constraint update only selects the vertical position components in the state vector for correction, mainly correcting the U-axis position and its related covariance, without directly constraining the horizontal position. The vertical position observation and depth measurement residuals respectively satisfy: , , in, For the vertical position observation of the k-th frame, The depth measured by the depth sensor in the k-th frame. The depth measurement residual for the k-th frame. Given the vertical position in the prior navigation state of the k-th frame, the system uses the vertical position observation matrix. and depth measurement noise matrix Calculate the depth Kalman gain and in accordance with Update navigation status according to Joseph stable format. Update the covariance matrix; when the depth data is invalid or does not meet the time synchronization conditions, the system skips the vertical position constraints and lets ,in, For the depth Kalman gain of the k-th frame, This is the vertical position observation matrix. This is the depth measurement noise matrix. Let be the prior covariance matrix of the k-th frame. The depth measurement residual for the k-th frame. This is the covariance matrix updated after vertical position constraints. It is an identity matrix.

[0051] It should be noted that depth constraints and DVL height serve different purposes: depth constraints are entered into the filter update as an observation to stabilize the global vertical position; DVL height is not used as a global vertical position observation, but is used to determine the bottom tracking quality and participate in the DVL comprehensive reliability calculation. Through this division of labor, when DVL is in a deweighted, weak update, or lost mode, the system can still use the depth sensor to constrain the vertical position, avoiding significant vertical drift caused by short-term DVL unavailability, thereby enhancing the vertical stability of the system under DVL degradation conditions.

[0052] S6. The system determines the DVL update mode based on the DVL comprehensive reliability and the number of effective beams, and filters and updates the navigation state after vertical constraints based on the DVL update mode, the effective beam set and the dynamic DVL measurement noise matrix, and outputs the navigation state of the underwater robot.

[0053] like Figure 4 , Figure 5 As shown, specifically, the system compares the overall reliability of the DVL in the k-th frame with the preset normal update threshold, the preset weak update threshold, and the preset failure threshold, respectively, and compares the number of effective beams in the k-th frame with the preset minimum number of beams for normal update and the preset minimum number of beams for weak update, respectively; based on the comparison results, the DVL update mode is switched to normal update mode, de-weighted update mode, weak update mode, or DVL loss mode. In normal update mode, the overall reliability of DVL is not lower than the preset normal update threshold and the number of effective beams is not less than the preset minimum number of beams for normal update. The system uses DVL speed update normally, and the dynamic measurement noise is close to the initial value. In deweighted update mode, the overall reliability of DVL is not lower than the preset weak update threshold and the number of effective beams is not less than the preset minimum number of beams for weak update. However, if the reliability and effective beam number conditions of normal update mode are not met at the same time, the system amplifies the DVL measurement noise to weaken the correction of the state by DVL observation. In weak update mode, the overall reliability of DVL is not lower than the preset failure threshold and is lower than the preset weak update threshold. The number of effective beams is not less than the preset minimum number of beams for weak update. The system only performs low-weight speed update. To ensure a clear difference in execution between the degraded update mode and the weak update mode, the system sets mode measurement noise figures for each of the normal update mode, degraded update mode, and weak update mode. and and satisfy , The system multiplies the corresponding mode coefficients into the dynamic DVL measurement noise matrix obtained in step S4 before participating in the filtering update. The weighted update mode retains strong effective beam constraints, the weak update mode uses larger measurement noise coefficients and retains only low-weight corrections, and the DVL loss mode skips the DVL update. The subsequent notation RDVL,k represents the measurement noise matrix with the current mode coefficients applied and matched to the effective beam set.

[0054] In DVL loss mode, if the overall DVL confidence level is below the failure threshold or the number of effective beams is insufficient, the system skips DVL updates and only uses IMU propagation and depth constraints to maintain short-term stability. To avoid frequent switching of DVL near the critical confidence level, when the previous state is DVL loss mode, the system enters recovery detection mode. Only after the overall DVL confidence level is not lower than the preset recovery threshold for M consecutive frames and the number of effective beams in each corresponding frame is not lower than the preset minimum number of effective beams for normal updates is the system allowed to switch from DVL loss mode to normal update mode. If the recovery conditions are not met, the DVL loss mode is maintained, and recovery detection takes precedence over the following segmented judgments. The mode switching logic satisfies the following: , In the formula, For the DVL update mode of frame k, , , and These correspond to normal update mode, degraded update mode, weak update mode, and DVL loss mode, respectively. The overall credibility of the DVL in the k-th frame is... For normal threshold updates, For weak update threshold, The failure threshold The minimum number of effective beams required to allow for normal updates. The number of effective beams in the k-th frame. The minimum number of effective beams required to allow weak updates or dimensionality reduction updates.

[0055] To ensure the completeness of pattern determination, the minimum confidence threshold, each confidence threshold, and the recovery threshold satisfy the following: ,in, As the minimum confidence level, The preset recovery threshold is set; the effective beam count threshold is satisfied. The normal update threshold is determined based on the comprehensive reliability statistical distribution when the DVL is in a stable low-tracking state, the number of effective beams meets the normal update requirements, and the velocity observation error is within the nominal accuracy range. The weak update threshold is determined based on the comprehensive reliability boundary when the DVL changes from mild or moderate degradation to severe degradation, requiring a switch from weighted updates to low-weighted weak updates. The failure threshold is determined based on the reliability boundary when the DVL observations are below the minimum usability level, and continuing to introduce such observations may significantly contaminate the navigation state estimation. The preset minimum number of effective beams is determined based on the rank and condition number of the effective beam observation matrix, and the minimum number of effective beams for normal updates is determined based on the number of effective beams required for normal velocity updates.

[0056] The preset recovery conditions are met: , in, Let be the overall reliability of the DVL in frame kj, representing the quantified value of the reliability of the DVL observations when looking back to frame j at the current time. The recovery threshold is used to determine whether DVL possesses the basic reliability conditions for recovering from lost mode to normal update mode. This threshold is not lower than the normal update threshold. This creates a hysteresis interval between the DVL loss mode and the normal update mode. The number of effective beams in the k-j frame. This is the frame offset index during continuous detection. The number of consecutive detection frames represents the number of consecutive qualified frames required for DVL to recover from the lost mode to the normal update mode.

[0057] The recovery threshold should not be lower than the normal update threshold, and can exceed the normal update threshold by a certain margin, provided it does not exceed the upper limit of confidence, to form a recovery hysteresis range. This margin should be calibrated in conjunction with representative degradation-recovery data, taking into account both the false recovery rate and recovery delay. The number of consecutive detection frames M is determined based on the statistical distribution of the DVL update cycle and the time required to re-establish a stable acoustic lock after exiting bottom tracking loss, and is calibrated to suppress single-frame or short-term fluctuations without excessively delaying recovery.

[0058] The physical meaning of the above-mentioned preset recovery conditions is as follows: In the M consecutive data frames traced back from the current frame, the overall credibility of DVL in each frame is not lower than the recovery threshold, and the number of effective beams in each frame is not less than the minimum number of effective beams allowed for normal updates. Only then will the system allow DVL to switch from loss mode to normal update mode, thereby avoiding frequent switching of DVL near the critical credibility and ensuring the smoothness of navigation state estimation.

[0059] When the DVL update mode is normal update mode, deweighted update mode, or weak update mode, the system filters the effective beam set based on beam status, beam quality, beam signal-to-noise ratio, and beam distance range, and calculates the number of effective beams. The effective beam set satisfies the following: , in, For the set of effective beams in the k-th frame, This represents the beam state of the i-th beam in the k-th frame. The beam quality of the i-th beam in the k-th frame. To preset the quality threshold, Let be the beam signal-to-noise ratio of the i-th beam in the k-th frame. To preset the signal-to-noise ratio threshold, For the k-th frame i Beam distance of each beam As a preset lower limit of distance, This is the preset maximum distance.

[0060] When the number of effective beams is less than the total number of DVL beams but greater than or equal to the preset minimum number of beams, the system constructs a dimension-reduced observation matrix based on the effective beam set to remove observation rows corresponding to abnormal beams, and only uses effective beams to participate in the filtering update. The dimension-reduced observation matrix is ​​constructed using the effective beams as follows: , in, Indicates the first DVL beam direction vectors The rotation matrix representing the installation from the machine coordinate system to the DVL coordinate system. The above equation represents the attitude rotation matrix from the navigation coordinate system to the machine system. It shows that the system dynamically constructs a dimension-reduced observation matrix based on the effective beam set, thereby avoiding invalid beams from participating in the observation update.

[0061] In this embodiment, the system constructs an effective beam observation vector from the radial velocities corresponding to the effective beam set, and uses the effective beam set to construct a selection matrix. It then simultaneously selects corresponding rows and columns for the initial beam-by-beam DVL measurement noise matrix and the dynamic DVL measurement noise matrix, obtaining an effective beam dynamic measurement noise matrix with the same number of rows as the dimensionality-reduced observation matrix. The system constructs an effective beam observation residual by using the difference between the effective beam observation vector and the predicted observations obtained after applying vertical constraints to the navigation state. This velocity residual is used to construct the DVL synthesis reliability, and the effective beam observation residual is used for filtering updates.

[0062] Initial DVL measurement noise matrix In the four-beam radial velocity observation domain, is defined as a 4×4 diagonal matrix composed of the variances of the nominal radial velocities of each beam. For the effective beam set in the k-th frame, the system uses the selection matrix Sk to simultaneously select the effective components of the observation vector, observation matrix, and measurement noise matrix, ensuring that the effective beam measurement noise matrix satisfies... And it matches the number of rows in the dimensionality-reduced observation matrix, where, The effective beam measurement noise matrix for the k-th frame is... A selection matrix is ​​used for the k-th frame to simultaneously select the effective components corresponding to the effective beam set in the observation vector, observation matrix, and measurement noise matrix. Let be the dynamic DVL measurement noise matrix for the k-th frame.

[0063] When the number of effective beams is less than the preset minimum number of beams, the system skips the DVL update; when the number of effective beams is greater than or equal to the preset minimum number of beams, the system calculates the Kalman gain based on the full-beam observation matrix or the reduced-dimensional observation matrix, the vertically constrained covariance matrix, and the dynamic DVL measurement noise matrix matching the effective beam set; the system then uses the Kalman gain and the effective beam observation residuals to correct and update the position and velocity in the vertically constrained navigation state to obtain the posterior navigation state; subsequently, the covariance matrix is ​​updated using the Joseph stable form to ensure the symmetry and positive semidefiniteness of the covariance matrix, reflecting the uncertainty of the corrected state; wherein, the Joseph stable form satisfies: , in, Let be the posterior covariance matrix of the k-th frame. It is the identity matrix. For the Kalman gain of the k-th frame, This can be a full-beam observation matrix or a reduced-dimensional observation matrix. Let be the covariance matrix after vertical constraints in the k-th frame. Let be the dynamic DVL measurement noise matrix for the k-th frame. When the DVL update mode is in the DVL loss mode, the system skips the DVL update and directly uses the navigation state after vertical constraints as the posterior navigation state. Finally, the system outputs the underwater robot's position, velocity, attitude, DVL confidence level, mode flag, and anomaly warning information, completing the navigation state estimation for the current frame.

[0064] Example 2 The difference between this embodiment and Embodiment 1 is that this embodiment provides a simulation experiment of a DVL quality-adaptive underwater navigation method; like Figure 8 , Figure 9 As shown, the experiment uses a dataset containing 10 underwater robot operation sequences from 01 to 10. Each sequence includes circular motion and depth changes, generated by different random seeds or different operating conditions. The dataset has been organized into a unified IMU frequency data table, retaining IMU three-axis acceleration, three-axis angular velocity, alignment attitude, DVL manufacturer velocity, DVL four-beam velocity, beam quality, beam signal-to-noise ratio, beam distance, beam state, DVL height, DVL-IMU time difference, and Ground Truth position and velocity fields. The experiment selects 01, 02, 03, and 04 as relatively normal or slightly degraded scenarios, and 05, 06, 07, 08, 09, and 10 as robustness verification scenarios with more obvious DVL anomalies.

[0065] like Figure 8 As shown, three comparison methods were set up in the experiment: the first method was fixed measurement noise EKF, where the DVL measurement noise matrix was set to a fixed value, corresponding to 0.05 m / s velocity noise; the second method was hard threshold rejection EKF, where DVL observations were directly discarded when the DVL quality or effective indicator was lower than a set threshold; the third method was the adaptive measurement noise combination filtering method described in Embodiment 1 of this invention. All three methods are based on the same IMU propagation model and depth constraint model to ensure the fairness of the control baseline.

[0066] Figure 8 Table 2 is used to show the position error variation trend of the three types of methods; Table 2 is used to list the quantitative results of position RMSE of the fixed measurement noise EKF and the method of the present invention. EKF is removed by hard threshold as a trend comparison and is not listed again in Table 2.

[0067] In the anomaly injection phase, beam anomalies were injected into the DVL four-beam data. The injection amplitude was three times the original noise level, with multiplicative anomalies (velocity scaled proportionally) and additive anomalies (velocity superimposed bias) each accounting for 50%. The anomaly percentages were set to 0%, 2%, 5%, 8%, 10%, 15%, and 20% to verify the performance of the invention under different degradation levels.

[0068] Table 1 lists the indicators that can be used to evaluate navigation accuracy and robustness; in this embodiment, position RMSE is used as the main quantitative evaluation indicator, and the results are shown in Table 2. The other indicators are used to assist in the analysis of navigation error characteristics.

[0069] Table 1 Evaluation Indicators and Their Calculation Methods Location RMSE Root mean square error between estimated and true positions Evaluation of overall positioning accuracy Level RMSE Root mean square error of position in the east / north direction Evaluation of horizontal navigation accuracy Speed ​​RMSE Root mean square error between estimated and actual velocity Evaluation speed estimation quality Drift at the finish line Distance between estimated end position and true position Evaluation of long-term cumulative error Abnormal recovery time The time it takes for the error to recover to within the threshold after the DVL anomaly ends Evaluation of robust recovery ability Credibility - Error Correlation Correlation between DVL reliability and DVL speed error Verify the rationality of the credibility model Based on the above evaluation indicators, the experimental results are shown in the table below: Table 2. Experimental results corresponding to different abnormality ratios. 0% 0.050m 0.053m Baseline consistency 2% 0.242m 0.084m +65.3% 5% 0.430m 0.155m +64.0% 8% 0.599m 0.301m +49.7% 10% 0.700m 0.304m +56.6% 15% 0.733m 0.457m +37.7% 20% 0.710m 0.615m +13.4% The above results are the average values ​​of 10 underwater robot operation sequences from 01 to 10. The experimental results show that in a baseline scenario with no anomaly injection (0% anomaly ratio), the position RMSE of the method in this embodiment is on a similar order of magnitude to that of the traditional EKF; whether there are statistical differences still needs to be determined by considering the dispersion or confidence interval of each sequence. Within the anomaly ratio range of 2% to 10%, the position RMSE of the method in this invention is numerically lower than that of the traditional EKF, with an improvement rate maintained between 49.7% and 65.3%, indicating that this invention can effectively suppress the contamination of navigation results by anomaly observations when DVL is mild to moderately degraded. When the anomaly ratio further increases to 15% and 20%, the improvement rate decreases, but still maintains a positive advantage, indicating that this invention can still maintain relatively stable navigation performance through depth constraints and IMU propagation even when DVL is severely degraded.

[0070] Further analysis shows that the fixed measurement noise EKF still over-relyes on erroneous observations when DVL is abnormal, causing the position estimate to be skewed by the abnormal velocity, and the overall position RMSE increases with the proportion of abnormalities. Although hard thresholding of EKF can avoid contamination by serious abnormal observations to some extent, it directly discards all DVL information when some beams are still valid, resulting in a waste of effective information and significant drift after the system degenerates into pure IMU propagation. The method of this invention dynamically adjusts the measurement noise through DVL comprehensive credibility, continuously downweights instead of directly removing it when the anomaly is slight, performs dimensionality reduction update when some beams are abnormal, and maintains vertical stability by relying on depth constraints when DVL is lost, thereby obtaining a position RMSE lower than that of the fixed measurement noise EKF under each non-zero anomaly proportion.

[0071] Example 3 This embodiment provides a DVL quality-adaptive underwater navigation system, including: like Figure 7 As shown, the data acquisition module is used to acquire IMU data, DVL data, and depth sensor data of the underwater robot. The time synchronization module is used to align the DVL data and depth sensor data with the IMU timestamp as the master clock. The state prediction module is used to predict the state based on IMU data and obtain the prior navigation state. The DVL quality analysis module is used to extract DVL multidimensional quality information from time-aligned DVL data and calculate the velocity residual by combining DVL velocity observations with predicted velocities in prior navigation states. The comprehensive credibility calculation module is used to screen the effective beam set and determine the number of effective beams based on the DVL multidimensional quality information and velocity residuals, and construct the DVL comprehensive credibility that characterizes the reliability of the current DVL observation based on the DVL multidimensional quality information, velocity residuals, effective beam set and number of effective beams. The dynamic measurement noise construction module is used to dynamically adjust the initial DVL measurement noise matrix based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix. The vertical position constraint module is used to apply vertical position constraints to the prior navigation state based on depth sensor data, and obtain the vertically constrained navigation state and its covariance matrix. The mode switching and filter update module is used to determine the DVL update mode based on the DVL comprehensive reliability and the number of effective beams, and to filter and update the navigation state and its covariance matrix after vertical constraint based on the DVL update mode, the effective beam set and the dynamic DVL measurement noise matrix. The navigation status output module is used to output the navigation status of the underwater robot.

[0072] The above are all preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Therefore, all equivalent changes made in accordance with the structure, shape and principle of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A DVL (Device Variational Volume) quality-adaptive underwater navigation method, characterized in that, include: Acquire IMU data, DVL data, and depth data of the underwater robot. Use the timestamp of the IMU data as the master clock to time-align the DVL data and depth data. Based on the IMU data, perform state prediction to obtain the prior navigation state. Extract DVL multidimensional quality information from time-aligned DVL data, and calculate velocity residuals by combining velocity observations from DVL data with predicted velocities from prior navigation states. Based on the DVL multidimensional quality information and velocity residuals, the effective beam set is screened and the number of effective beams is determined. Based on the DVL multidimensional quality information, velocity residuals, effective beam set and number of effective beams, a DVL comprehensive reliability characterizing the reliability of the current DVL observation is constructed. The initial DVL measurement noise matrix is ​​dynamically adjusted based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix. Vertical position constraints are applied to the prior navigation state based on depth data to obtain the vertically constrained navigation state and its covariance matrix. The DVL update mode is determined based on the overall reliability of DVL and the number of effective beams. Based on the DVL update mode, the effective beam set, and the dynamic DVL measurement noise matrix, the navigation state after vertical constraints and its covariance matrix are filtered and updated to output the navigation state of the underwater robot.

2. The DVL quality-adaptive underwater navigation method according to claim 1, characterized in that, The time alignment of DVL data and depth data includes calculating the time difference between the DVL data timestamp and the depth data timestamp and the most recent IMU prediction time, and comparing each time difference with a preset time synchronization threshold. When the time difference is less than the preset time synchronization threshold, the corresponding DVL data or depth data will be associated with the current IMU prediction state. When the time difference is greater than or equal to the preset time synchronization threshold, a time synchronization weight is constructed based on the time difference, and the credibility of the corresponding DVL observation or depth observation is reduced according to the time synchronization weight.

3. The DVL quality-adaptive underwater navigation method according to claim 2, characterized in that, The process of obtaining the prior navigation state includes reading the IMU's three-axis acceleration, IMU's three-axis angular velocity, and IMU timestamp, and calculating the sampling interval between adjacent IMU sampling times. Based on the attitude matrix of the previous moment, the system acceleration is transformed to the navigation coordinate system and the gravity term is subtracted to obtain the motion acceleration in the navigation coordinate system; Based on motion acceleration and sampling interval, discrete integral prediction is performed on the posterior velocity and posterior position of the previous time step to obtain the prior velocity and prior position of the current time step. Based on the propagation prediction covariance matrix of the state transition matrix and the process noise matrix, the prior covariance matrix is ​​obtained. The prior navigation state includes prior velocity, prior position and prior covariance matrix.

4. The DVL quality-adaptive underwater navigation method according to claim 1, characterized in that, The calculation of velocity residuals by combining velocity observations in DVL data with predicted velocities in prior navigation states includes reading time-aligned DVL data, extracting radial velocity, beam state, beam quality, beam signal-to-noise ratio, and beam distance of the four beams, establishing a beam-by-beam data set, and extracting DVL altitude and the time difference between DVL data and IMU prediction time to form DVL multidimensional quality information. The predicted velocity is extracted from the prior navigation state, and the velocity residual between the velocity observation and the predicted velocity in the DVL data is calculated. The velocity residual is used to characterize the consistency between the DVL velocity observation and the state prediction.

5. The DVL quality-adaptive underwater navigation method according to claim 1, characterized in that, The construction of the DVL comprehensive credibility, which characterizes the reliability of the current DVL observation, includes constructing DVL validity weights based on validity indicators in the DVL data and obtaining time synchronization weights. The effective beam set is selected based on the beam status, beam quality, beam signal-to-noise ratio and beam distance of the four beams. The effective beam quantity weight is constructed based on the effective beam set, and the beam quality weight is constructed based on the normalized quality value of each effective beam. The distance consistency weight is constructed based on the dispersion of the four-beam distance. The height anomaly weight is constructed based on the change between the current DVL height and the previous frame's DVL height. The velocity residual weight is constructed based on the norm of the velocity residual. Finally, the weights are multiplied and fused, and the overall credibility of the DVL is obtained by constraining with the minimum credibility lower bound.

6. The DVL quality-adaptive underwater navigation method according to claim 5, characterized in that, The process of obtaining the dynamic DVL measurement noise matrix includes setting the initial DVL measurement noise matrix under normal operating conditions and reading the overall reliability of the DVL in the current frame. The initial DVL measurement noise matrix is ​​amplified according to the DVL comprehensive reliability to obtain the dynamic DVL measurement noise matrix. The dynamic DVL measurement noise matrix is ​​input into the filter as the measurement noise for updating the DVL observations of the current frame, in order to reduce the weight of DVL observations with reduced confidence in the filter update. The formula for calculating the DVL measurement noise matrix is ​​as follows: ， in, The dynamic DVL measurement noise matrix for the k-th frame. This is the initial DVL measurement noise matrix under normal operating conditions. The overall credibility of the DVL in the k-th frame is... To prevent small constants from being divided by zero.

7. The DVL quality-adaptive underwater navigation method according to claim 1, characterized in that, The vertical position constraint on the prior navigation state based on depth data includes reading depth sensor data and depth timestamp, and calculating the time difference between the depth timestamp and the current IMU prediction time. When the time difference meets the preset synchronization conditions, the depth sensor data is converted into a vertical position observation consistent with the navigation coordinate system, and a depth measurement residual is constructed based on the vertical position observation and the vertical position in the prior navigation state. The vertical position constraint is updated based on the depth measurement residual to obtain the vertically constrained navigation state and its covariance matrix.

8. The DVL quality-adaptive underwater navigation method according to claim 1, characterized in that, The step of determining the DVL update mode based on the overall reliability of DVL and the number of effective beams includes comparing the overall reliability of the DVL in the k-th frame with a preset normal update threshold, a preset weak update threshold, and a preset failure threshold, respectively, and comparing the number of effective beams in the k-th frame with a preset minimum number of beams for normal update and a preset minimum number of beams for weak update, respectively. Based on the comparison results, the DVL update mode is determined as a normal update mode, a downweighted update mode, a weak update mode, or a DVL loss mode. The determination relationship is as follows: ， In the formula, For the DVL update mode of frame k, , , and These correspond to normal update mode, degraded update mode, weak update mode, and DVL loss mode, respectively. The overall credibility of the DVL in the k-th frame is... For normal threshold updates, For weak update threshold, The failure threshold The minimum number of effective beams required to allow for normal updates. The number of effective beams in the k-th frame. The minimum number of effective beams required to allow weak updates or dimensionality reduction updates; When the DVL update mode is the DVL loss mode, the recovery detection is performed on the DVL data of each subsequent frame. Only when the DVL comprehensive confidence is not lower than the preset recovery threshold for M consecutive frames and the number of effective beams in each corresponding frame is not lower than the preset minimum number of beams for normal update, it switches to normal update mode.

9. The DVL quality-adaptive underwater navigation method according to claim 8, characterized in that, The output of the underwater robot's navigation status includes selecting an observation matrix based on the total number of DVL beams and the number of effective beams when the DVL update mode is normal update mode, reduced-weight update mode, or weak update mode; constructing a full-beam observation matrix when the number of effective beams is equal to the total number of DVL beams; and constructing a dimension-reduced observation matrix based on the set of effective beams when the number of effective beams is less than the total number of DVL beams but not less than the preset minimum number of effective beams for weak update. The corresponding mode coefficients are applied to the dynamic DVL measurement noise matrix according to the DVL update mode, and the Kalman gain is calculated based on the full beam observation matrix or the reduced-dimensional observation matrix, the covariance matrix after vertical constraint, and the dynamic DVL measurement noise matrix that matches the effective beam set. The position and velocity in the vertically constrained navigation state are corrected and updated using Kalman gain and effective beam observation residuals to obtain the posterior navigation state. When the DVL update mode is DVL loss mode, skip the DVL update and output the navigation state after vertical constraints as the posterior navigation state. The effective beam set satisfies: ， in, For the set of effective beams in the k-th frame, This represents the beam state of the i-th beam in the k-th frame. The beam quality of the i-th beam in the k-th frame. To preset the quality threshold, Let be the beam signal-to-noise ratio of the i-th beam in the k-th frame. To preset the signal-to-noise ratio threshold, For the k-th frame i Beam distance of each beam As a preset lower limit of distance, This is the preset maximum distance.

10. A DVL quality-adaptive underwater navigation system for performing the method of claim 1, characterized in that, include: The data acquisition module is used to acquire IMU data, DVL data, and depth sensor data of the underwater robot. The time synchronization module is used to align the DVL data and depth sensor data with the IMU timestamp as the master clock. The state prediction module is used to predict the state based on IMU data and obtain the prior navigation state. The DVL quality analysis module is used to extract DVL multidimensional quality information from time-aligned DVL data and calculate the velocity residual by combining DVL velocity observations with predicted velocities in prior navigation states. The comprehensive credibility calculation module is used to filter the effective beam set and determine the number of effective beams, and construct the DVL comprehensive credibility that characterizes the reliability of the current DVL observation; The dynamic measurement noise construction module is used to dynamically adjust the initial DVL measurement noise matrix based on the DVL comprehensive credibility to obtain the dynamic DVL measurement noise matrix. The vertical position constraint module is used to apply vertical position constraints to the prior navigation state based on depth sensor data, and obtain the vertically constrained navigation state and its covariance matrix. The mode switching and filter update module is used to determine the DVL update mode based on the DVL comprehensive reliability and the number of effective beams, and to filter and update the navigation state and its covariance matrix after vertical constraint based on the DVL update mode, the effective beam set and the dynamic DVL measurement noise matrix. The navigation status output module is used to output the navigation status of the underwater robot.

Images