UUV anomaly measurement correction distance measurement method based on motion feature coupling

Abstract

The invention discloses a UUV anomaly measurement correction distance measurement method and device based on motion feature coupling. The method comprises the following steps: calculating an innovation value; performing innovation transient anomaly detection on single measurement data of the sensor based on the innovation value and a theoretical innovation value conforming to the motion characteristics of the UUV, and tracking an abrupt change point in an obtained historical innovation value sequence based on a result of the innovation transient anomaly detection; obtaining a corrected observation value based on the sum of the plurality of previous observation values and the respective random disturbance factors; the method comprises the following steps: acquiring an observation value of the current moment, calculating a corrected innovation value based on the corrected observation value, weighting the calculated and corrected innovation value based on Kalman gain, adding the sum of state prediction values at the current moment to obtain a new state prediction at the current moment, and performing distance measurement by using the new state prediction. While the misjudgment rate is reduced, the calculation amount is reasonably controlled, and the tracking robustness in a complex environment is remarkably improved.

Description

Technical Field

[0001] The present application relates to the technical field of underwater vehicles in the field of marine engineering, and more specifically to a method and device for UUV abnormal measurement and correction ranging based on motion feature coupling. Background Art

[0002] With the rapid development of marine resource exploration and underwater infrastructure construction, the application of unmanned underwater vehicles (UUVs) in fields such as submarine pipeline inspection and ecological monitoring continues to expand, leading to a significant increase in the frequency of multi-UUV collaborative operations in complex waters. Due to interference factors such as strong turbulence, multipath acoustic propagation, and low-frequency sensor drift in the underwater environment, UUV pose estimation of static or slow-moving underwater targets is susceptible to non-Gaussian noise. Especially during close-range precision operations, measurement deviations can cause excessive cumulative tracking errors. Because the traditional Kalman filter framework lacks an effective mechanism to suppress Doppler measurement surges, when underwater suspended objects obstruct the sonar beam or turbulence causes Doppler log (DVL) data to jump, the conventional chi-square detection threshold method is prone to misjudgment due to fluid dynamics hysteresis. Directly rejecting anomalous measurements leads to rank deficiency in the system observability matrix, which in turn causes state estimation divergence.

[0003] To improve the reliability of underwater tracking systems, it is often necessary to fuse data from multiple sensors, such as Doppler odometers (DVLs) and sonar. Existing methods often use extended Kalman filters (EKFs) or cubic Kalman filters (CKFs) for state estimation. While these methods can reduce computational complexity through linear approximation, they are highly sensitive to sudden changes in measurements and can easily cause estimation bias due to abnormal data. Nonlinear processing methods based on particle filters (PFs), while highly fault-tolerant, struggle with computational complexity and the limited computing power available underwater, making them difficult to meet real-time requirements. Summary of the Invention

[0004] This application aims to solve at least one of the technical problems existing in the prior art. To this end, a method for correcting ranging of abnormal measurements of UUV based on motion feature coupling is proposed in the first aspect of this application, including: calculating the difference between the observed value and the predicted observed value based on the predicted value of the state at the current moment to obtain the innovation value; performing innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value conforming to the motion characteristics of the UUV, and tracking and obtaining mutation points in the historical innovation value sequence based on the results of the innovation transient anomaly detection; obtaining multiple observed values before the mutation point, and obtaining the corrected observed value based on the sum of the previous multiple observed values and their respective random perturbation factors, where the random perturbation factor is dynamically optimized according to the change trend of the observed value; calculating the corrected innovation value based on the corrected observed value, and weighting the corrected innovation value based on the Kalman gain to obtain a weighted term, using the sum of the weighted term and the predicted value of the state at the current moment to obtain the new predicted state at the current moment, and using the new predicted state to measure the distance between the target and the UUV.

[0005] Optionally, before calculating the difference between the observed value and the predicted observed value based on the predicted value of the state at the current moment, the method further includes: processing the state estimation of the UUV at the previous moment based on the non-linear state transition function to obtain the predicted state at the current moment, where the state of the UUV at the previous moment includes the radial velocity and the relative azimuth angle between the target and the UUV observation platform.

[0006] Optionally, before calculating the corrected innovation value based on the corrected observed value and weighting the corrected innovation value based on the Kalman gain, the method further includes: calculating the prior error covariance matrix at the current moment based on the preset prior error covariance matrix calculation formula; determining the Jacobian matrix and the measurement noise covariance matrix based on the predicted state at the current moment and the preset measurement function, and calculating the Kalman gain using the Kalman gain calculation formula constructed based on the Jacobian matrix, the measurement noise covariance matrix and the prior error covariance matrix.

[0007] Optionally, before measuring the distance between the target and the UUV using the new predicted state, the method further includes: correcting the posterior error covariance matrix based on the new predicted state, and determining the confidence level of the state estimation based on the corrected posterior error covariance matrix. If the confidence level exceeds the preset value, the distance between the target and the UUV is measured using the new predicted state, otherwise the confidence level is recalculated until it exceeds the preset value.

[0008] Optionally, the innovation covariance calculation formula is:

[0009]

[0010] where H kis the Jacobian matrix of the measurement function, R k is the measurement noise covariance matrix, P k|k-1 is the prior error covariance matrix.

[0011] Optionally, the calculation formula of the prior error covariance matrix is:

[0012] P k∣k-1 = FP k-1∣k-1 F T + GQG T

[0013] where F is the feature of the system dynamic model at the prediction point, G is the driving matrix, and Q is the influence factor of external disturbance and modeling error in the underwater environment on the system.

[0014] Optionally, the innovation transient anomaly detection for the single measurement data of the sensor based on the innovation value and the theoretical innovation value conforming to the UUV motion characteristics includes: calculating the innovation value ΔE at the current moment; if |ΔE k |≥λ·μ, then mark the innovation value at the current moment as an innovation transient anomaly, where λ represents a multiple and μ represents the theoretical innovation value conforming to the UUV motion characteristics.

[0015] To achieve the above object, the second aspect of the present application further provides a UUV abnormal measurement correction ranging device based on motion feature coupling, including: a matrix construction module, configured to calculate the difference between the observed value and the predicted observed value based on the state prediction value at the current moment to obtain an innovation value; a mutation point detection module, configured to perform innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value conforming to the UUV motion characteristics, and track and obtain mutation points in the historical innovation value sequence based on the results of the innovation transient anomaly detection; a data update module, configured to obtain multiple observed values before the mutation point, and obtain the corrected observed value based on the sum of the previous multiple observed values and their respective random disturbance factors, where the random disturbance factor is dynamically optimized according to the change trend of the observed value; a ranging module, configured to calculate the corrected innovation value based on the corrected observed value, and weight the corrected innovation value based on the Kalman gain to obtain a weighted term, and use the sum of the weighted term and the state prediction value at the current moment to obtain the new state prediction at the current moment, and use the new state prediction to measure the distance between the target and the UUV.

[0016] To achieve the above object, a third aspect of the present application further provides an electronic device, the electronic device includes a processor and a memory, and at least one instruction, at least one program, a code set or an instruction set is stored in the memory, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by the processor to implement the method for correcting ranging of UUV abnormal measurement based on motion feature coupling provided by any one of the foregoing method embodiments.

[0017] To achieve the above object, a fourth aspect of the present application further provides a computer-readable storage medium, and at least one instruction, at least one program, a code set or an instruction set is stored in the storage medium, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by a processor to implement the method for correcting ranging of UUV abnormal measurement based on motion feature coupling provided by any one of the foregoing method embodiments.

[0018] The embodiments of the present application provide a method and device for correcting ranging of UUV abnormal measurement based on motion feature coupling. Compared with the prior art, the beneficial effects are as follows: By calculating the difference between the observed value and the predicted observed value based on the predicted value of the state at the current moment, the innovation value is obtained; Based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics, the single measurement data of the sensor is subjected to innovation transient anomaly detection, and the mutation points in the obtained historical innovation value sequence are tracked based on the results of the innovation transient anomaly detection; Obtain multiple observed values before the mutation point, and based on the sum of the multiple observed values before and their respective random perturbation factors, obtain the corrected observed value, where the random perturbation factor is dynamically optimized according to the change trend of the observed value; Calculate the corrected innovation value based on the corrected observed value, and weight the corrected innovation value based on the Kalman gain to obtain a weighted term. Use the sum of the weighted term and the predicted value of the state at the current moment to obtain the new predicted value of the state at the current moment, and use the new predicted value to measure the distance between the target and the UUV. The present application constructs a coupling constraint model based on the motion model and the underwater environment to perform dynamic threshold detection on the radial velocity measurement: When the measured value breaks through the prediction interval based on the historical motion state, use the historical measured value plus a reasonable perturbation to replace the abnormal point, and generate virtual observed values in combination with the propulsion system dynamics equation to maintain the continuity of filtering. Compared with the traditional method, this strategy uses a dual verification mechanism of physical constraint and data-driven to reduce the false positive rate while reasonably controlling the calculation amount, and significantly improves the tracking robustness in complex environments. This strategy uses a dual verification mechanism of physical constraint and data-driven to reduce the false positive rate while reasonably controlling the calculation amount, and significantly improves the tracking robustness in complex environments. Description of the Drawings

[0019] To more clearly illustrate the technical solutions of this application, the following will briefly introduce the accompanying drawings required in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description are only some embodiments of this application. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can also be obtained based on these drawings.

[0020] Figure 1 It is the construction flowchart of the UUV abnormal measurement correction ranging algorithm of this application;

[0021] Figure 2 It is the comparison diagram of the true azimuth angle and the measured azimuth angle of the target relative to the observation point;

[0022] Figure 3 It is the comparison situation of manually adding radial velocity measurement noise;

[0023] Figure 4 It is the radial velocity comparison diagram under the measurement correction mechanism;

[0024] Figure 5 It is the tracking results and ranging error situation before and after measurement correction. Specific embodiments

[0025] The following will clearly and completely describe the technical solutions in the embodiments of this application with reference to the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are only some embodiments of this application, rather than all embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of this application.

[0026] This specification provides the method operation steps as described in the embodiments or the flowchart, but based on routine or non-creative labor, it can include more or fewer operation steps. When the actual system or server product is executed, it can be executed in the order shown in the embodiments or the drawings or in parallel (for example, in an environment of parallel processors or multi-threaded processing).

[0027] In the underwater target tracking task, the observation platform needs to measure and obtain information such as the azimuth angle and radial velocity of an unmanned underwater vehicle (UUV) through sensors, and estimate its relative distance based on this. However, due to the complex underwater environment, sensor measurements are easily affected by water flow disturbances, bubble interference, and system noise, resulting in sudden changes in measurement data, especially in the observation of radial velocity. Such abnormal measurement values will seriously interfere with the update process of the extended Kalman filter (EKF), causing the accumulation of state estimation errors and even leading to tracking failure or distorted target distance estimation. To improve the ranging stability and accuracy in the underwater environment, it is urgent to construct an efficient and robust measurement anomaly detection and correction mechanism.

[0028] Aiming at the problems existing in the prior art, this application proposes an abnormal measurement double-threshold correction mechanism based on dynamic prior. This method deeply explores the motion characteristics of the UUV in viscous fluids: dominated by fluid resistance, its acceleration has an upper threshold, and the change in radial velocity has an inertial hysteresis effect; at the same time, limited by the response bandwidth of the thruster, the measurement increment should satisfy the continuity constraint within a fixed sampling period. By constructing a coupling constraint model based on the motion model and the underwater environment, dynamic threshold detection is performed on the radial velocity measurement: when the measurement value breaks through the prediction interval based on the historical motion state, the abnormal point is replaced by the historical measurement value plus a reasonable perturbation, and virtual observables are generated in combination with the dynamic equation of the propulsion system to maintain the continuity of filtering. Compared with traditional methods, this strategy uses a double-verification mechanism of physical constraints and data-driven to reduce the false positive rate while reasonably controlling the computational amount, and significantly improves the tracking robustness in complex environments.

[0029] The technical solution of this application is: aiming at the problems of sudden change in radial velocity and displacement anomaly of the UUV during underwater operation, a measurement anomaly rejection and correction strategy based on motion change constraints is proposed. Based on the low-speed and slow-changing motion characteristics of the UUV itself, this method establishes a constraint increment limit condition for the change in radial velocity, and makes real-time judgments on the observation data before measurement update:

[0030] Abnormal detection mechanism: When the change amplitude of the measurement at the current moment exceeds the preset threshold range, it is determined that the measurement is abnormal data.

[0031] Adaptive correction mechanism: The data determined to be abnormal does not directly participate in the filter update, but instead uses the measurement value of the previous moment for a small adjustment and then replaces the current observation to construct a smooth measurement input, reducing the impact of sudden anomalies on the system state estimation.

[0032] Continuity guarantee mechanism: Combining the innovation value and the historical measurement trend, further improve the judgment ability of measurement stability, and achieve the suppression and repair of short-term interference.

[0033] Through this strategy, the interference of abnormal data on the EKF update process can be effectively reduced, the tracking accuracy of the UUV for the target in a complex underwater environment and the overall robustness of the system can be improved, providing a reliable guarantee for high-precision tracking and ranging in subsequent underwater operations. The process flow of its construction is as Figure 1 shown

[0034] Refer to Figure 1 , the UUV abnormal measurement correction ranging method based on motion feature coupling may include the following execution process:

[0035] S10. Based on the state prediction value at the current moment, calculate the difference between the observed value and the predicted observed value to obtain the innovation value.

[0036] In an embodiment of the present application, before calculating the difference between the observed value and the predicted observed value based on the state prediction value at the current moment, the UUV abnormal measurement correction ranging method based on motion feature coupling further includes the following execution process:

[0037] Process the state estimation of the UUV at the previous moment based on the non-linear state transition function to obtain the state prediction at the current moment, where the state of the UUV at the previous moment includes the radial velocity and the relative azimuth angle between the target and the UUV observation platform.

[0038] It should be noted that, before this, the method further includes: in the underwater target distance estimation task, the processor first defines the state vector X0 and its covariance matrix P0. The state vector includes physical quantities such as the relative azimuth angle between the target and the observation platform, and the radial velocity. According to the sensor performance and the expected target characteristics, set the standard deviation matrix R0 of the observation error, where the error distribution is usually assumed to be a Gaussian distribution with a mean of zero and a variance of σ 2 . In addition, to improve the estimation accuracy of the initial state, the system process noise covariance matrix Q0 needs to be set in the initialization stage. This matrix reflects the uncertainty between the system model under ideal conditions and the actual environment, and needs to be comprehensively set in combination with the influence of the underwater environment on the UUV movement, such as factors like water flow disturbance and noise.

[0039] Then, the processor obtains the state estimation result at the previous moment according to the motion characteristics of the underwater target, and processes the state estimation result at the previous moment through the non-linear state transition function to obtain the state prediction at the current moment.

[0040] Among them, the state prediction formula at the current moment is:

[0041]

[0042] F is the state transition function matrix, represents the prediction of the system state at time k, Represents the state estimation result at time k - 1.

[0043] In an embodiment of the present application, the calculation formula of the prior error covariance matrix is:

[0044] P k∣k-1 = FPF k-1∣k-1 F T + GQG T

[0045] Wherein, F is the feature of the system dynamic model at the prediction point, G is the drive matrix, and Q is the influence factor of external disturbances and modeling errors in the underwater environment on the system.

[0046] In the present application, the processor can calculate the prior error covariance matrix at the current moment according to the state prediction result. The prior error covariance matrix is as shown above. Under the conditions of strong nonlinearity or significant environmental disturbances, Q should be appropriately adjusted to ensure that the prediction error is within a controllable range and provide stable support for the subsequent filtering process.

[0047] The processor can calculate the Kalman gain based on the current prediction state and the measurement model. The Kalman gain is used to adjust the fusion weight between the predicted value and the actual observation. After obtaining new measurement information, the processor can calculate the Kalman gain based on the current prediction state and the measurement model. The Kalman gain is used to adjust the fusion weight between the predicted value and the actual observation. The calculation formula of the Kalman gain is:

[0048]

[0049] Wherein, H k is the Jacobian matrix of the measurement function, and R k is the measurement noise covariance matrix.

[0050] The construction of the innovation is a mathematical expression of the difference between the measurement and the prediction, and is also the basic basis for the system to distinguish abnormal observations. In the present application, the innovation is not only used as an input item for state correction, but is further incorporated into the core feature quantity of anomaly monitoring to drive the adaptive correction mechanism.

[0051] The difference between the observed value and the predicted observed value is used as the innovation value. The expression of the innovation value is:

[0052]

[0053] In another embodiment of the present application, the calculation formula of the innovation covariance is:

[0054]

[0055] Wherein, H k is the Jacobian matrix of the measurement function, and R kis the measurement noise covariance matrix, P k|k-1 is the prior error covariance matrix. Among them, this formula is one of the terms in the parentheses of the Kalman gain calculation formula.

[0056] The processor can comprehensively evaluate the stability and continuity of the measurement value in the time dimension by performing real-time statistical analysis and dynamic monitoring on the innovation sequence. The processor calculates the magnitude of the innovation in real time and constructs a comprehensive criterion for measurement anomalies. Once any index exceeds the preset threshold range, the processor can determine that there is an abnormal risk in the current observation and trigger the multi-level anomaly detection mechanism in the sixth step. At the same time, to enhance the system's adaptability to sudden disturbances and dynamic changes, the processor will call the adaptive correction strategy and use historical measurement smoothing to replace the measurement value, thereby enhancing the filter's anti-sensitivity to abnormal observations and ensuring the stability, continuity, and convergence of the overall state estimation. Among them, the innovation covariance calculation is the basic support for anomaly discrimination.

[0057] S20. Perform innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics, and track and obtain the mutation points in the historical innovation value sequence based on the results of the innovation transient anomaly detection.

[0058] In an embodiment of the present application, the performing innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics may include the following execution process:

[0059] Calculate the innovation value ΔE at the current moment.

[0060] If |ΔE k |≥λ·μ, then mark the innovation value at the current moment as an innovation transient anomaly, where λ represents a multiple and μ represents the theoretical innovation value that conforms to the UUV motion characteristics.

[0061] Specifically, this detection mechanism integrates criteria such as innovation transient fluctuation detection and historical trend deviation analysis to construct a multi-level anomaly discrimination system with real-time and forward-looking characteristics. The processor can not only accurately capture isolated anomaly points but also identify trend drifts and observational structural disorders. As the key link in the transformation from "passive divergence" to "active convergence" in the entire filtering process, the detection results of this step will be directly input into the adaptive correction module to achieve a closed-loop control from recognition to response, thereby effectively enhancing the anti-interference ability and robustness of the system under non-ideal observation conditions.

[0062] After the processor identifies potential abnormal signs in the innovation sequence, to ensure that the filtering system still has high-precision and high-reliability state estimation capabilities in complex underwater environments, this application introduces a multi-dimensional verification measurement anomaly detection mechanism as an important step in the front-end information processing of the filter. The processor systematically and hierarchically identifies and analyzes the measurement innovation, effectively avoiding the risk of error propagation caused by sudden noise, abnormal observations, or intermittent sensor failures, and ensuring that only reliable and stable measurement information is fused during the state update process.

[0063] The goal of the multi-level measurement anomaly detection mechanism is the innovation transient anomaly detection. The detection goal of the processor is to capture the sudden anomaly of a single measurement (such as instantaneous sensor interference). The method used by the processor is based on the statistical distribution characteristics of the innovation to determine whether the current innovation exceeds the reasonable fluctuation range. Among them, the determination condition is that if |ΔE k |≥λ·μ, it is marked as a measurement anomaly at this moment. After obtaining the measurement anomaly, the processor can track the historical innovation sequence of the measurement, dynamically detect the mutation point, and then smoothly replace the abnormal mutation point of the measurement value.

[0064] S30. Obtain multiple observation values before the mutation point, and based on the sum of the previous multiple observation values and their respective random perturbation factors, obtain the corrected observation value, where the random perturbation factor is dynamically optimized according to the change trend of the observation value.

[0065] After the processor completes the multi-level discrimination and classification annotation of the measurement innovation anomaly, this step, as the core anti-interference module of the improved extended Kalman filter (EKF) algorithm, introduces an adaptive measurement correction and compensation mechanism with dynamic response and linkage adjustment capabilities. This mechanism realizes real-time response and directional replacement of the abnormal state of the observation data through deep coupling with the foregoing detection results, ensuring that the measurement information has time continuity, statistical consistency, and physical credibility before entering the state correction link. Specifically, based on the discrimination of the anomaly level, this mechanism uses historical smoothed data to replace the abnormal data, thereby suppressing the spread of filtering errors caused by problems such as mutation interference and observation drift. Its ultimate goal is to construct an observation fusion channel that is still stable and available in complex underwater environments, provide reliable input for subsequent state estimation, and comprehensively improve the anti-sensitivity and environmental adaptability of the system.

[0066] Exemplarily, the above steps can be executed through the following steps. Input key data and calculations in this core anti-interference module. Then, the processor can annotate the anomaly flag bit and calculate the corrected data according to the previous historical measurement window. Exemplarily, the historical measurement window can be the measurement values {Z K-N ,Z K-N+1 ,……Z K-1} at N moments.

[0067] For example, if a transient or historical mutation anomaly is detected, adaptive dynamic compensation is performed on the observed data according to the following formula:

[0068] z k =z k-1 +ε

[0069] Among them, after in-depth analysis of the continuity characteristics of historical measurement information, this application can dynamically adjust the random disturbance factor ε. The value of the random disturbance factor ε can be dynamically optimized according to the change trend of the measurement data monitored in real time to further enhance the anti-disturbance ability of the filtering process to abnormal conditions.

[0070] Through the above-mentioned multi-level discrimination and correction mechanism, when the processor detects typical abnormal situations such as transient over-limit and trend mutation, it can intelligently adjust the input of the observation value according to the actual disturbance characteristics, effectively avoiding the direct interference of abnormal measurement on the state update process. The corrected measurement information is not only more stable in terms of numerical value, but also meets the filtering fusion requirements under the Gaussian assumption in a statistical sense, thereby significantly improving the convergence speed and robustness of the improved EKF algorithm under non-ideal working conditions. As the central node connecting anomaly detection and state update, the output of the above steps will directly enter the state correction link, that is, S40, to ensure that the entire system forms a closed-loop control chain of "identification-correction-tracking", and ultimately achieves robust estimation and dynamic perception of the target state.

[0071] S40. Calculate the corrected new interest value based on the corrected observation value, and weight the corrected new interest value based on the Kalman gain to obtain the weighted term. Use the sum of the weighted term and the state prediction value at the current moment to obtain the new state prediction at the current moment. Use the new state prediction to measure the distance between the target and the UUV.

[0072] In one embodiment of the present application, before calculating the difference between the observed value and the predicted observed value based on the current state prediction value, the UUV abnormal measurement and correction ranging method based on motion feature coupling further includes the following execution process:

[0073] The state estimate of the UUV at the last moment is determined based on the radial velocity and the relative azimuth angle between the target and the UUV observation platform.

[0074] The state estimation of the UUV at the previous moment is processed based on the nonlinear state transfer function to obtain the state prediction at the current moment.

[0075] It should be noted that state correction is the core part of the improved extended Kalman filter (EKF). The goal of state correction is to generate an optimal posterior state estimate by fusing the prior state prediction value and the current measurement information. Specifically, the processor weights the innovation value (i.e., the difference between the measurement value and the prediction value) using the Kalman gain, dynamically adjusts the prediction result, thereby reducing noise interference and improving the accuracy of the target state estimate. This step directly determines the convergence and robustness of the filtering algorithm. Especially when there are abnormal measurements, through the previous anomaly detection and correction mechanism (such as innovation smoothing replacement), the negative impact of mutation errors on state estimation can be significantly suppressed.

[0076] The processor can first calculate the Kalman gain. As mentioned before, the Kalman gain can be calculated from the measurement noise covariance matrix, the Jacobian matrix of the measurement function, and the prior error covariance matrix.

[0077] It is worth noting that after multi-level anomaly detection, if abnormal data is detected, the processor can correct and replace the abnormal data according to historical data to obtain a smoothed innovation value, which can be the corrected measurement innovation.

[0078] Exemplarily, the calculation formula for the corrected measurement innovation can be:

[0079]

[0080] Among them, the Kalman gain determines the contribution weight of the innovation value to state correction. If the measurement information is reliable (such as small covariance), the Kalman gain increases, and the impact of the Kalman gain on state correction is stronger. Conversely, the gain decreases, and the algorithm relies more on the current prediction value.

[0081] In an embodiment of the present application, before using the new state prediction to measure the distance between the target and the UUV, the UUV abnormal measurement correction ranging method based on motion feature coupling may further include the following execution process:

[0082] Correct the posterior error covariance matrix based on the new state prediction, and determine the confidence level of the state estimate based on the corrected posterior error covariance matrix. If the confidence level exceeds the preset value, use the new state prediction to measure the distance between the target and the UUV; otherwise, recalculate the confidence level until it exceeds the preset value.

[0083] Finally, the processor can perform posterior error covariance matrix correction. Posterior error covariance matrix correction is used to quantify the uncertainty of state estimation and reflect the effect of information fusion in the filtering process. By updating the covariance matrix, the confidence of the current estimate can be dynamically evaluated, and a basis for error propagation can be provided for the prediction at the next moment. The posterior error covariance matrix correction is directly related to the stability and convergence speed of the filter. Especially in a complex underwater environment, reasonable correction of the covariance is the key to suppressing error accumulation and avoiding filter divergence.

[0084] When performing posterior error covariance matrix correction, the processor can first calculate the Kalman gain, and the calculation formula is the same as that in the state correction step. Then, the processor can calculate the measurement Jacobian matrix, which is used to describe the non-linear relationship between the measurement model and the state. Finally, the processor can calculate the prior error covariance matrix, where the prior error covariance matrix is determined based on the error propagation result of the state prediction step.

[0085] The calculation formula for the posterior error covariance matrix can be:

[0086] P k∣k =(I-K k H k+1 )P k∣k-1

[0087] Through the interaction between the Kalman gain and the measurement model, this formula can update the covariance matrix to reduce the uncertainty brought by the new measurement information. If the measurement information is relatively reliable, P k|k will shrink significantly, indicating an increase in the confidence of state estimation.

[0088] To verify the robustness and practical effect of the proposed measurement anomaly detection and correction mechanism in the case of sudden interference, this application conducts data processing experiments for typical observation anomaly scenarios. The target tracking trajectory mainly includes two segments. The first segment is from time 39 to 106, and the second segment is from time 175 to 275. In this ranging problem, the key measurement information obtained by the observation platform mainly includes the azimuth angle and the radial velocity, and the radial velocity has a particularly significant impact on the distance estimation accuracy. Therefore, at times 50 to 70 of the first segment of the target trajectory, Gaussian white noise with a variance of 400 is artificially added to the radial velocity observation data to simulate the situation of sudden measurement anomalies, so as to test the anomaly recognition and correction ability of this algorithm in the face of sudden measurement input, and to test the maintenance effect of the relative distance estimation accuracy of the target.

[0089] Figure 1 is the construction flow chart of the UUV anomaly measurement correction ranging algorithm of this application. The comparison between the true azimuth angle of the target relative to the observation point and the measured azimuth angle is as Figure 2 shown: The comparison of manually adding radial velocity measurement noise is as Figure 3The comparison chart of radial velocity under the measurement correction mechanism is as shown in Figure 4 shown. The tracking results and ranging error conditions before and after measurement correction are as shown in Figure 5 shown. The comparison of average ranging errors is shown in Table 1.

[0090] Table 1 Comparison of ranging errors before and after based on the radial velocity constraint mechanism

[0091]

[0092]

[0093] As can be seen from the above table, for the azimuth-radial velocity observation and tracking method based on the radial velocity constraint mechanism set for the UUV, the average error can be reduced by 10.45% in the case of a sharp increase in measurement, which can effectively reduce the ranging error.

[0094] Based on the above method embodiments, the present application further provides a UUV abnormal measurement correction ranging device based on motion feature coupling, which is used to solve the same technical problems as the method embodiments. The device may include a matrix construction module, a mutation point detection module, a data update module, and a ranging module. Among them, the matrix construction module is used to calculate the difference between the observed value and the predicted observed value based on the state prediction value at the current moment to obtain the innovation value. The mutation point detection module is used to perform innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics, and track and obtain the mutation points in the historical innovation value sequence based on the results of the innovation transient anomaly detection. The data update module is used to obtain multiple observed values before the mutation point, and based on the sum of the previous multiple observed values and their respective random perturbation factors, obtain the corrected observed values, where the random perturbation factor is dynamically optimized according to the change trend of the observed value. The ranging module is used to calculate the corrected innovation value based on the corrected observed value, and weight the corrected innovation value based on the Kalman gain to obtain a weighted term. Using the sum of the weighted term and the state prediction value at the current moment, obtain the new state prediction at the current moment, and use the new state prediction to measure the distance between the target and the UUV.

[0095] In another embodiment provided by the present application, a device is further provided. The device includes a processor and a memory. The memory stores at least one instruction, at least one program, a code set, or an instruction set. The at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor to implement the UUV abnormal measurement correction ranging method based on motion feature coupling described in the embodiments of the present application.

[0096] In yet another embodiment provided by the present application, a computer-readable storage medium is further provided. At least one instruction, at least one program, a code set, or an instruction set is stored in the storage medium. The at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by a processor to implement the method for correcting ranging of UUV abnormal measurement based on motion feature coupling described in the embodiments of the present application.

[0097] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes a plurality of computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions described in the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from a website, a computer, a server, or a data center to another website, computer, server, or data center by wire (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer, or a data storage device such as a server or a data center that includes a plurality of available media integrated. The available medium can be a magnetic medium (such as a floppy disk, a hard disk, a magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as a solid state disk (SSD)).

[0098] It should be noted that in this article, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including", or any other variant thereof is intended to cover a non-exclusive inclusion, such that a process, method, article, or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article, or device. Without further limitation, an element defined by the statement "including a..." does not exclude the existence of additional identical elements in the process, method, article, or device including the element.

[0099] Each embodiment in this specification is described in a related manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and reference can be made to the corresponding parts of the method embodiments for the relevant content.

[0100] The above description is only for the preferred embodiments of the present application and is not intended to limit the protection scope of the present application. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present application are all included in the protection scope of the present application.

Claims

1. A method for correcting ranging of abnormal measurements of UUV based on coupling of motion characteristics, characterized in that Including: Based on the state prediction value at the current moment, calculate the difference between the observed value and the predicted observed value to obtain the innovation value; Based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics, perform innovation transient anomaly detection on the single measurement data of the sensor, and based on the result of the innovation transient anomaly detection, track the mutation points in the obtained historical innovation value sequence; Obtain multiple observed values before the mutation point, and based on the sum of the previous multiple observed values and their respective random perturbation factors, obtain the corrected observed value, where the random perturbation factor is dynamically optimized according to the change trend of the observed value; Calculate the corrected innovation value based on the corrected observed value, and weight the corrected innovation value based on the Kalman gain to obtain a weighted term. Use the sum of the weighted term and the state prediction value at the current moment to obtain the new state prediction at the current moment, and use the new state prediction to measure the distance between the target and the UUV.

2. The method for correcting ranging of abnormal measurements of UUV based on coupling of motion characteristics according to claim 1, wherein, Before calculating the difference between the observed value and the predicted observed value based on the state prediction value at the current moment, the method further includes: Based on the radial velocity and the relative azimuth angle between the target and the UUV observation platform, determine the state estimation of the UUV at the previous moment; Process the state estimation of the UUV at the previous moment based on the non-linear state transition function to obtain the state prediction at the current moment.

3. A method for correcting ranging of abnormal measurements of a UUV based on coupling of motion characteristics according to claim 1, characterized in that Before calculating the corrected innovation value based on the corrected observed value and weighting the corrected innovation value based on the Kalman gain, the method further includes: Based on the preset prior error covariance matrix calculation formula, calculate the prior error covariance matrix at the current moment; Based on the state prediction at the current moment and the preset measurement function, determine the Jacobian matrix and the measurement noise covariance matrix; Calculate the Kalman gain using the Kalman gain calculation formula constructed based on the Jacobian matrix, the measurement noise covariance matrix, and the prior error covariance matrix.

4. The UUV abnormal measurement correction ranging method based on motion feature coupling according to claim 1, wherein Before using the new state prediction to measure the distance between the target and the UUV, the method further includes: Correct the posterior error covariance matrix based on the new state prediction; Determine the confidence level of the state estimation based on the corrected posterior error covariance matrix; If the confidence level exceeds the preset value, use the new state prediction to measure the distance between the target and the UUV, otherwise recalculate the confidence level until it exceeds the preset value.

5. A method for correcting abnormal measurement ranging of UUV based on motion feature coupling according to claim 1, characterized in that, The calculation formula of the prior error covariance matrix is: P k∣k-1 = FP k-1∣k-1 F T + GQG T Where F is the characteristic of the system dynamic model at the prediction point, G is the driving matrix, and Q is the influence factor of external disturbance and modeling error in the underwater environment on the system.

6. The UUV abnormal measurement correction ranging method based on motion feature coupling according to claim 1, characterized in that The innovation transient anomaly detection of the single measurement data of the sensor based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics includes: Calculate the innovation value ΔE at the current moment; If |ΔE k | ≥ λ·μ, then mark the innovation value at the current moment as an innovation transient anomaly, where λ represents a multiple and μ represents the theoretical innovation value that conforms to the UUV motion characteristics.

7. An abnormal measurement correction ranging device for UUV based on coupling of motion characteristics, characterized in that Including: A matrix construction module for calculating the difference between the observed value and the predicted observed value based on the state prediction value at the current moment to obtain the innovation value; A mutation point detection module for performing innovation transient anomaly detection on the single measurement data of the sensor based on the innovation value and the theoretical innovation value that conforms to the UUV motion characteristics, and tracking the mutation points in the obtained historical innovation value sequence based on the result of the innovation transient anomaly detection; A data update module, configured to obtain multiple observation values before the mutation point, and obtain the corrected observation values based on the sum of the previous multiple observation values and their respective random perturbation factors, wherein the random perturbation factor is dynamically optimized according to the change trend of the observation values; A ranging module, configured to calculate a corrected innovation value based on the corrected observation values, weight the corrected innovation value based on the Kalman gain to obtain a weighted term, and use the sum of the weighted term and the state prediction value at the current moment to obtain a new state prediction at the current moment, and use the new state prediction to measure the distance between the target and the UUV.

8. An electronic device, characterized in that, The electronic device includes a processor and a memory, and at least one instruction, at least one program, a code set or an instruction set is stored in the memory, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by the processor to implement the method for correcting ranging of abnormal measurements of UUV based on motion feature coupling according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, At least one instruction, at least one program, a code set or an instruction set is stored in the storage medium, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by a processor to implement the method for correcting ranging of abnormal measurements of UUV based on motion feature coupling according to any one of claims 1-6.

Images