Submersible navigation trajectory gross error detection and elimination method based on sliding window and standard deviation

Abstract

The invention discloses a submersible navigation track gross error detection and elimination method based on a sliding window and a standard deviation, belongs to the technical field of underwater robot navigation and motion control, is used for submersible navigation track gross error detection and elimination, and comprises the steps of data preparation and initialization processing, and dynamic configuration of the sliding window. Calculating spatial position statistics and kinematics characteristics of the data; judging an abnormal gross error value by sliding multiple windows; setting a multi-mechanism gross error elimination strategy, and performing hierarchical interpolation restoration of the data and optimization after data restoration; and performing iterative processing on the repaired submersible trajectory data, and setting an adaptive termination threshold. Compared with the prior art, the method has the advantages that track abrupt change points caused by sensor noise or environmental interference are accurately recognized and eliminated, and high robustness and real-time performance are achieved; the method can adapt to a complex underwater environment, the track smoothness and reliability are improved, and high-precision track guarantee is provided for autonomous operation of the underwater submersible.

Description

Technical Field

[0001] The invention discloses a method for detecting and eliminating gross errors in a submarine navigation trajectory based on a sliding window and standard deviation, and belongs to the technical field of underwater robot navigation and motion control. Background Art

[0002] Nearly 49% of Earth's surface is deep sea, rich in biological and mineral resources. Deep-sea manned submersibles are essential equipment for deep-sea surveys, ocean mapping, and sampling. Submersible position and trajectory are derived through multi-sensor measurement fusion. However, underwater navigation is subject to sensor noise (such as Doppler logs and inertial navigation systems), complex water flow disturbances, environmental noise (temporal and spatial errors in sound speed caused by temperature, salinity, and depth variations), and unstable communication links. These factors can easily lead to gross errors in trajectory data, sometimes even continuous errors. These errors manifest as sudden changes in trajectory coordinates or deviations from the true path, severely impacting analysis of submersible operational data and sample point coordinate analysis.

[0003] Traditional trajectory filtering methods (such as Kalman filtering and median filtering) are insensitive to gross errors. They may have poor adaptability to dynamic environments due to fixed windows, or lose true features due to excessive smoothing. Furthermore, manual post-processing of gross errors is inefficient and difficult to meet real-time requirements. Summary of the Invention

[0004] The purpose of the present invention is to provide a method for detecting and eliminating gross errors in the navigation trajectory of a submarine based on a sliding window and standard deviation, so as to solve the problems in the prior art of insufficient noise recognition sensitivity of underwater submarine sensors and low efficiency of manual post-processing of gross errors.

[0005] The method for detecting and eliminating gross errors in the submersible's navigation trajectory based on sliding windows and standard deviation includes:

[0006] S1. Prepare and initialize data based on shipborne positioning data and manned submersible navigation and positioning data, dynamically configure the sliding window for local precision data error detection and elimination;

[0007] S2. Calculate the spatial position statistics of the data and calculate the kinematic characteristics based on the characteristics of the manned diving voyage;

[0008] S3. Mark outliers through single-point anomaly detection and data context association detection, and use sliding multi-windows to determine outlier gross errors;

[0009] S4. Set a multi-mechanism gross error elimination strategy to eliminate global gross errors, perform hierarchical interpolation repair of data to make the data continuous and smooth, and perform optimization after data repair;

[0010] S5. Iteratively process the repaired submersible trajectory data, set an adaptive termination threshold, and stop the iteration when the condition is met.

[0011] S1 includes, S1.1. The original trajectory data of the submersible's multi-sensor fusion includes the three-dimensional coordinate sequence information P output by the inertial navigation system, Doppler log, and ultra-short baseline positioning system:

[0012] P={p1,p2,…,p n},p i =(x i ,y i ,z i ,t i );

[0013] Where p i The coordinates of the original trajectory data output by the multi-sensor fusion of the submersible, x i is the x-axis coordinate at time i, y i is the y-axis coordinate at time i, z i is the z-axis coordinate at time i, t i is the timestamp of time i;

[0014] Convert the original trajectory data coordinates of the multi-sensor fusion of the submersible into the northeast celestial coordinate system;

[0015] S1.2. Preprocess missing data values. If the coordinates corresponding to a timestamp are missing, use forward filling and backward filling strategies, preferably using linear interpolation based on the motion trends of adjacent valid points.

[0016] S1 includes, S1.3. Dynamic configuration of sliding window, according to the maximum movement speed v of the submersible max and data sampling frequency f s Calculate the initial window length L0:

[0017]

[0018] Where, T cover is the time span expected to be covered, d sample is the maximum displacement of a single point, the window sliding step S is fixed to 1, and point-by-point detection is performed;

[0019] Establish a dynamic window adjustment mechanism to monitor the gross error rate η within the window in real time:

[0020]

[0021] Where N is the number of gross error points and L is the window size;

[0022] If η>10%, it is determined to be a high noise environment and the window L will be updated.new Extended to L new =L+2, maximum window L max No more than 20;

[0023] If η<2%, it is considered a stable environment and the window L will be updated. new Shrink to L new =L-1, minimum window L min Not less than 5.

[0024] S2 includes, S2.1. Perform multi-dimensional statistical analysis and feature extraction in the sliding window, calculate the statistics of the spatial position, including the calculation of the mean and standard deviation, and calculate the sliding window W. k ={p k ,p k+1 ,…,p k+L-1}Inside, p k is the three-dimensional coordinate of the kth data point, and the mean μ of each coordinate component is calculated separately x 、μ y 、μ z and standard deviation σ x , σ y , σ z ;

[0025] Calculate the joint spatial deviation and use the 3D Mahalanobis distance method to detect outliers in the integrated space:

[0026]

[0027] Where D j is the three-dimensional Mahalanobis distance, p j is the three-dimensional coordinate of the jth data point, μ is the mean of each coordinate, μ=(μ x ,μ y ,μ z ), ∑ is the covariance matrix, D j Anything outside the quantile of 11.34 is marked as an anomaly.

[0028] S2 includes, S2.2. Analyze and calculate the kinematic characteristics of the submersible underwater based on the navigation trajectory, sensor information, environmental information and hydrological information, including instantaneous velocity and acceleration, and calculate the velocity v of the jth data point based on the time difference Δt between adjacent points. j and acceleration a j for:

[0029] Δt=t j+1 -t j ;

[0030]

[0031] Where, t jis the time of the jth data point;

[0032] S2.3. Setting the motion smoothness index δ v Define the rate of change of velocity within the window:

[0033] δ v =max(v j )-min(v j );

[0034] Motion smoothness index and standard deviation of acceleration σ a , used to assist in the determination of gross errors.

[0035] S3 includes, S3.1. By setting the gross error judgment rules, the gross error points of the trajectory after multi-source data fusion are detected. The first step is to set the hard threshold, the second step is to set the soft threshold, and the third step is to set the multi-window voting mechanism;

[0036] S3.2. Set hard thresholds. Hard thresholds are used for single-point anomaly detection, including setting spatial thresholds and kinematic thresholds.

[0037] The spatial threshold is if a point satisfies |x j -μ x |>3σ x or |y j -μ y |>3σ y or |z j -μ z |>3σ z , triggering the primary exception flag, x j 、y j 、z j is the three-dimensional coordinate of the j-th data point;

[0038] The kinematic threshold is if v j >1.5v max or a j >2a max , marked as movement abnormality, v max is the maximum design speed of the submersible, a max is the maximum design acceleration of the submersible.

[0039] S3 includes, S3.3. Setting soft constraints, where the soft constraints are context association detection, including time continuity verification and trajectory curvature consistency verification;

[0040] Temporal continuity verification is that if a point is marked as abnormal, the abnormal status of m adjacent points is detected, where m is 2. If m+1 consecutive points are abnormal, it is determined to be a true gross error, otherwise it is considered as transient noise.

[0041] The trajectory curvature consistency verification is to calculate the curvature κ of the front and back ends of the outlier point:

[0042]

[0043] Where v is the velocity of the front and rear ends of the outlier point, and a is the acceleration of the front and rear ends of the outlier point. If the curvature mutation exceeds twice the historical average value, it is strengthened into an outlier judgment;

[0044] S3.4. Set up a multi-window voting mechanism to prevent a point from being detected by multiple windows due to the overlap of sliding windows, and set the voting threshold V th , V th The value is 2. If a point is exceeded by V th If a window is judged to be abnormal, it is confirmed as a gross error point.

[0045] S4 includes, S4.1. After identifying gross errors in S3, performing gross error elimination and adaptive trajectory repair, including gross error elimination strategy, hierarchical interpolation repair and post-repair optimization;

[0046] S4.2. Gross error elimination strategies include single point elimination and continuous segment elimination;

[0047] Single point elimination is to directly delete the points confirmed as gross errors and record the index of the vacant position;

[0048] The continuous segment is eliminated if the number of continuous gross error points N bad ≥3, it is considered as communication packet loss or sensor failure, the entire segment is removed and the time interval is recorded;

[0049] S4.3. Perform hierarchical interpolation repair. If N bad ≤2, short gap repair is performed, including linear interpolation and curve smoothing;

[0050] Linear interpolation is to directly connect the previous and next valid points:

[0051]

[0052] Where p(t) is the coordinate of the interpolation point, p prev is the coordinate of the previous valid point, p next is the coordinate of the subsequent effective point, t is the linear interpolation time, t prev is the time of the previous effective point, t next The time of the next valid point;

[0053] Curve smoothing generates smooth transitions by introducing control points to avoid sudden changes in broken lines.

[0054] S4 includes, S4.4. Perform hierarchical interpolation repair, if N bad >2, long gap repair, including dynamic model interpolation and external data fusion;

[0055] The dynamic model interpolation predicts the trajectory based on the vehicle's kinematic model:

[0056]

[0057] Where p0 is the initial coordinate, v0 is the initial velocity, and a0 is the initial acceleration. v0 and a0 are obtained by fitting the historical data before and after the gap. The kinematic model of the submersible is a constant acceleration model.

[0058] External data fusion is to perform multi-source data Kalman filtering fusion if there is valid data from Doppler log and depth meter sensors during the vacancy period;

[0059] S4.5. Perform post-repair optimization, including curvature smoothing and energy minimization constraints;

[0060] Curvature smoothing is to apply Savitzky-Golay filter to the repaired trajectory segment to eliminate high-frequency jitter while preserving motion characteristics;

[0061] The energy minimization constraint is to construct the optimization objective function J to minimize the integral of the acceleration change of the trajectory:

[0062]

[0063] Where t1 is the starting time, t2 is the ending time, and the gradient descent method is used to solve the optimal smooth path.

[0064] S5 includes, S5.1. performing iterative optimization of the algorithm and parameter adaptation, including iterative detection and parameter self-learning;

[0065] S5.2. Set up an iterative detection mechanism and re-execute steps S1 to S4 on the repaired trajectory, looping until any of the following conditions is met:

[0066] Condition 1: The rate of decrease in the number of gross errors in two consecutive iterations is less than or equal to 5%;

[0067] Condition 2: Reach the maximum number of iterations N max =5;

[0068] S5.3. Set the parameter self-learning strategy, adaptively adjust the threshold, and dynamically adjust the sensitivity coefficient based on historical detection results:

[0069]

[0070] Where, α new is the adjusted sensitivity coefficient, α old is the sensitivity coefficient before adjustment, η FP is the false alarm rate, η FN is the false negative rate, γ is the learning rate;

[0071] Record the optimal window length under different environments and establish a lookup table for quick call in subsequent tasks.

[0072] Compared with the existing technology, the present invention has the following beneficial effects: by dynamically adjusting the window length and standard deviation thresholds, combining multi-dimensional position, kinematic characteristics and contextual association analysis, it can accurately identify and eliminate trajectory mutation points caused by sensor noise or environmental interference, and at the same time use hierarchical interpolation to repair the trajectory, with both high robustness and real-time performance; compared with traditional fixed threshold or single-dimensional detection methods, it can adapt to complex underwater environments, improve trajectory smoothness and reliability, and provide high-precision trajectory protection for autonomous operation of underwater submersibles. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a technical flow chart of the present invention;

[0074] Figure 2 It is the result map of USBL original position data converted into east direction;

[0075] Figure 3 Convert USBL original position data into north direction result map;

[0076] Figure 4 This is a comparison chart of the USBL original position data converted into sky direction and depth meter data;

[0077] Figure 5 This is a comparison chart of the results before and after removing gross errors in the east direction;

[0078] Figure 6 This is a comparison chart of the results before and after removing the gross errors in the north direction;

[0079] Figure 7 This is the result of smooth interpolation comparison in the east direction;

[0080] Figure 8 This is the comparison result of smooth interpolation in the north direction. DETAILED DESCRIPTION

[0081] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention are described clearly and completely below. Obviously, the embodiments described are only some of the embodiments of the present invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0082] A method for detecting and eliminating gross errors in the trajectory of a submersible based on sliding windows and standard deviation, such as Figure 1 ,include:

[0083] S1. Prepare and initialize data based on shipborne positioning data and manned submersible navigation and positioning data, dynamically configure the sliding window for local precision data error detection and elimination;

[0084] S2. Calculate the spatial position statistics of the data and calculate the kinematic characteristics based on the characteristics of the manned diving voyage;

[0085] S3. Mark outliers through single-point anomaly detection and data context association detection, and use sliding multi-windows to determine outlier gross errors;

[0086] S4. Set a multi-mechanism gross error elimination strategy to eliminate global gross errors, perform hierarchical interpolation repair of data to make the data continuous and smooth, and perform optimization after data repair;

[0087] S5. Iteratively process the repaired submersible trajectory data, set an adaptive termination threshold, and stop the iteration when the condition is met.

[0088] S1 includes, S1.1. The original trajectory data of the submersible's multi-sensor fusion includes the three-dimensional coordinate sequence information P output by the inertial navigation system, Doppler log, and ultra-short baseline positioning system:

[0089] P={p1,p2,…,p n},p i =(x i ,y i ,z i ,t i );

[0090] Where p i The coordinates of the original trajectory data output by the multi-sensor fusion of the submersible, x i is the x-axis coordinate at time i, y i is the y-axis coordinate at time i, z i is the z-axis coordinate at time i, t i is the timestamp of time i;

[0091] Convert the original trajectory data coordinates of the multi-sensor fusion of the submersible into the northeast celestial coordinate system;

[0092] S1.2. Preprocess missing data values. If the coordinates corresponding to a timestamp are missing, use forward filling and backward filling strategies, preferably using linear interpolation based on the motion trends of adjacent valid points.

[0093] S1 includes, S1.3. Dynamic configuration of sliding window, according to the maximum movement speed v of the submersible max and data sampling frequency f s Calculate the initial window length L0:

[0094]

[0095] Where, T cover is the time span expected to be covered, d sample is the maximum displacement of a single point, the window sliding step S is fixed to 1, and point-by-point detection is performed;

[0096] Establish a dynamic window adjustment mechanism to monitor the gross error rate η within the window in real time:

[0097]

[0098] Where N is the number of gross error points and L is the window size;

[0099] If η>10%, it is determined to be a high noise environment and the window L will be updated. new Extended to L new =L+2, maximum window L max No more than 20;

[0100] If η<2%, it is considered a stable environment and the window L will be updated. new Shrink to L new =L-1, minimum window L min Not less than 5.

[0101] S2 includes, S2.1. Perform multi-dimensional statistical analysis and feature extraction in the sliding window, calculate the statistics of the spatial position, including the calculation of the mean and standard deviation, and calculate the sliding window W. k ={p k ,p k+1 ,…,p k+L-1}Inside, p k is the three-dimensional coordinate of the kth data point, and the mean μ of each coordinate component is calculated separately x 、μ y 、μ z and standard deviation σ x , σ y , σ z ;

[0102] Calculate the joint spatial deviation and use the 3D Mahalanobis distance method to detect outliers in the integrated space:

[0103]

[0104] Where D j is the three-dimensional Mahalanobis distance, p j is the three-dimensional coordinate of the jth data point, μ is the mean of each coordinate, μ=(μ x ,μ y ,μ z ), ∑ is the covariance matrix, Dj Anything outside the quantile of 11.34 is marked as an anomaly.

[0105] S2 includes, S2.2. Analyze and calculate the kinematic characteristics of the submersible underwater based on the navigation trajectory, sensor information, environmental information and hydrological information, including instantaneous velocity and acceleration, and calculate the velocity v of the jth data point based on the time difference Δt between adjacent points. j and acceleration a j for:

[0106] Δt=t j+1 -t j ;

[0107]

[0108] Where, t j is the time of the jth data point;

[0109] S2.3. Setting the motion smoothness index δ v Define the rate of change of velocity within the window:

[0110] δ v =max(v j )-min(v j );

[0111] Motion smoothness index and standard deviation of acceleration σ a , used to assist in the determination of gross errors.

[0112] S3 includes, S3.1. By setting the gross error judgment rules, the gross error points of the trajectory after multi-source data fusion are detected. The first step is to set the hard threshold, the second step is to set the soft threshold, and the third step is to set the multi-window voting mechanism;

[0113] S3.2. Set hard thresholds. Hard thresholds are used for single-point anomaly detection, including setting spatial thresholds and kinematic thresholds.

[0114] The spatial threshold is if a point satisfies |x j -μ x |>3σ x or |y j -μ y |>3σ y or |z j -μ z |>3σ z , triggering the primary exception flag, x j 、y j 、z j is the three-dimensional coordinate of the j-th data point;

[0115] The kinematic threshold is if v j >1.5vmax or a j >2a max , marked as movement abnormality, v max is the maximum design speed of the submersible, a max is the maximum design acceleration of the submersible.

[0116] S3 includes, S3.3. Setting soft constraints, where the soft constraints are context association detection, including time continuity verification and trajectory curvature consistency verification;

[0117] Temporal continuity verification is that if a point is marked as abnormal, the abnormal status of m adjacent points is detected, where m is 2. If m+1 consecutive points are abnormal, it is determined to be a true gross error, otherwise it is considered as transient noise.

[0118] The trajectory curvature consistency verification is to calculate the curvature κ of the front and back ends of the outlier point:

[0119]

[0120] Where v is the velocity of the front and rear ends of the outlier point, and a is the acceleration of the front and rear ends of the outlier point. If the curvature mutation exceeds twice the historical average value, it is strengthened into an outlier judgment;

[0121] S3.4. Set up a multi-window voting mechanism to prevent a point from being detected by multiple windows due to the overlap of sliding windows, and set the voting threshold V th , V th The value is 2. If a point is exceeded by V th If a window is judged to be abnormal, it is confirmed as a gross error point.

[0122] S4 includes, S4.1. After identifying gross errors in S3, performing gross error elimination and adaptive trajectory repair, including gross error elimination strategy, hierarchical interpolation repair and post-repair optimization;

[0123] S4.2. Gross error elimination strategies include single point elimination and continuous segment elimination;

[0124] Single point elimination is to directly delete the points confirmed as gross errors and record the index of the vacant position;

[0125] The continuous segment is eliminated if the number of continuous gross error points N bad ≥3, it is considered as communication packet loss or sensor failure, the entire segment is removed and the time interval is recorded;

[0126] S4.3. Perform hierarchical interpolation repair. If N bad ≤2, short gap repair is performed, including linear interpolation and curve smoothing;

[0127] Linear interpolation is to directly connect the previous and next valid points:

[0128]

[0129] Where p(t) is the coordinate of the interpolation point, p prev is the coordinate of the previous valid point, p next is the coordinate of the subsequent effective point, t is the linear interpolation time, t prev is the time of the previous effective point, t next The time of the next valid point;

[0130] Curve smoothing generates smooth transitions by introducing control points to avoid sudden changes in broken lines.

[0131] S4 includes, S4.4. Perform hierarchical interpolation repair, if N bad >2, long gap repair, including dynamic model interpolation and external data fusion;

[0132] The dynamic model interpolation predicts the trajectory based on the vehicle's kinematic model:

[0133]

[0134] Where p0 is the initial coordinate, v0 is the initial velocity, and a0 is the initial acceleration. v0 and a0 are obtained by fitting the historical data before and after the gap. The kinematic model of the submersible is a constant acceleration model.

[0135] External data fusion is to perform multi-source data Kalman filtering fusion if there is valid data from Doppler log and depth meter sensors during the vacancy period;

[0136] S4.5. Perform post-repair optimization, including curvature smoothing and energy minimization constraints;

[0137] Curvature smoothing is to apply Savitzky-Golay filter to the repaired trajectory segment to eliminate high-frequency jitter while preserving motion characteristics;

[0138] The energy minimization constraint is to construct the optimization objective function J to minimize the integral of the acceleration change of the trajectory:

[0139]

[0140] Where t1 is the starting time, t2 is the ending time, and the gradient descent method is used to solve the optimal smooth path.

[0141] S5 includes, S5.1. performing iterative optimization of the algorithm and parameter adaptation, including iterative detection and parameter self-learning;

[0142] S5.2. Set up an iterative detection mechanism and re-execute steps S1 to S4 on the repaired trajectory, looping until any of the following conditions is met:

[0143] Condition 1: The rate of decrease in the number of gross errors in two consecutive iterations is less than or equal to 5%;

[0144] Condition 2: Reach the maximum number of iterations N max =5;

[0145] S5.3. Set the parameter self-learning strategy, adaptively adjust the threshold, and dynamically adjust the sensitivity coefficient based on historical detection results:

[0146]

[0147] Where, α new is the adjusted sensitivity coefficient, α old is the sensitivity coefficient before adjustment, η FP is the false alarm rate, η FN is the false negative rate, γ is the learning rate;

[0148] Record the optimal window length under different environments and establish a lookup table for quick call in subsequent tasks.

[0149] The original position data of the ultra-short baseline underwater acoustic positioning system USBL is converted into the east direction result as follows Figure 2 As shown, the horizontal axis time is time, the unit is minutes, the vertical axis E is the east direction position, the unit is m; USBL original position data is converted to the north direction result Figure 3 As shown, the vertical coordinate N is the north direction position, the unit is m; the comparison results of USBL original data and depth meter data are plotted on the depth map as shown in Figure 4 As shown, the blue curve represents the conversion of USBL original position data into sky direction data, the orange curve represents the depth data of the depth meter, and the vertical coordinate Deep is the sky direction position in meters; Figure 2 It can be seen that a large gross error value appears at about 180 minutes. In order to obtain a better fitting smooth curve, it is necessary to eliminate the large gross error points.

[0150] By using the above-mentioned gross error elimination method to conduct a gross error elimination experiment, the results of the east and north directions after gross error elimination are compared with those before gross error elimination. Figure 5 、 Figure 6 As shown, through Figure 5 、 Figure 6 The comparison of the results before and after gross error removal shows that the blue curve represents the result before gross error removal, and the orange curve represents the result after gross error removal. The gross errors around 180 minutes have been successfully removed, and the small gross errors have also been well removed. Based on the results after gross error removal, data smoothing and interpolation processing is performed, and the results of the changes in the east and north directions of the underwater submersible after smooth interpolation are as follows: Figure 7 、 Figure 8 As shown. Figure 7 、 Figure 8 From the comparison results of smooth interpolation, we can see that the blue curve represents the original USBL east and north direction data, the orange curve represents the result after gross error elimination, the yellow curve represents the result after smoothing, and the purple short line represents the result after smooth interpolation.

[0151] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents, and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for detecting and eliminating gross errors in submersible navigation trajectories based on sliding windows and standard deviations, characterized in that: include: S1. Prepare and initialize data based on shipborne positioning data and manned submersible navigation and positioning data, dynamically configure the sliding window for local precision data error detection and elimination; S2. Calculate the spatial position statistics of the data and calculate the kinematic characteristics based on the characteristics of the manned diving voyage; S3. Mark outliers through single-point anomaly detection and data context association detection, and use sliding multi-windows to determine outlier gross errors; S4. Set a multi-mechanism gross error elimination strategy to eliminate global gross errors, perform hierarchical interpolation repair of data to make the data continuous and smooth, and perform optimization after data repair; S5. Iteratively process the repaired submersible trajectory data, set an adaptive termination threshold, and stop the iteration when the condition is met.

2. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 1 is characterized in that: S1 includes, S1.

1. The original trajectory data of the submersible's multi-sensor fusion includes the three-dimensional coordinate sequence information P output by the inertial navigation system, Doppler log, and ultra-short baseline positioning system: P={p1,p2,…,p n },p i =(x i ,y i ,z i ,t i ); Where p i The coordinates of the original trajectory data output by the multi-sensor fusion of the submersible, x i is the x-axis coordinate at time i, y i is the y-axis coordinate at time i, z i is the z-axis coordinate at time i, t i is the timestamp of time i; Convert the original trajectory data coordinates of the multi-sensor fusion of the submersible into the northeast celestial coordinate system; S1.

2. Preprocess missing data values. If the coordinates corresponding to a timestamp are missing, use forward filling and backward filling strategies, preferably using linear interpolation based on the motion trends of adjacent valid points.

3. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 2, characterized in that: S1 includes, S1.

3. Dynamic configuration of sliding window, according to the maximum movement speed v of the submersible max and data sampling frequency f s Calculate the initial window length L0: Where, T cover is the time span expected to be covered, d sample is the maximum displacement of a single point, the window sliding step S is fixed to 1, and point-by-point detection is performed; Establish a dynamic window adjustment mechanism to monitor the gross error rate η within the window in real time: Where N is the number of gross error points and L is the window size; If η>10%, it is determined to be a high noise environment and the window L will be updated. new Extended to L new =L+2, maximum window L max No more than 20; If η<2%, it is considered a stable environment and the window L will be updated. new Shrink to L new =L-1, minimum window L min Not less than 5.

4. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 3 is characterized in that: S2 includes, S2.

1. Perform multi-dimensional statistical analysis and feature extraction in the sliding window, calculate the statistics of the spatial position, including the calculation of the mean and standard deviation, and calculate the sliding window W. k ={p k ,p k+1 ,…,p k+L-1 }Inside, p k is the three-dimensional coordinate of the kth data point, and the mean μ of each coordinate component is calculated separately x 、μ y 、μ z and standard deviation σ x , σ y , σ z ; Calculate the joint spatial deviation and use the 3D Mahalanobis distance method to detect outliers in the integrated space: Where D j is the three-dimensional Mahalanobis distance, p j is the three-dimensional coordinate of the jth data point, μ is the mean of each coordinate, μ=(μ x ,μ y ,μ z ), ∑ is the covariance matrix, D j Anything outside the quantile of 11.34 is marked as an anomaly.

5. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 4 is characterized in that: S2 includes, S2.

2. Analyze and calculate the kinematic characteristics of the submersible underwater based on the navigation trajectory, sensor information, environmental information and hydrological information, including instantaneous velocity and acceleration, and calculate the velocity v of the jth data point based on the time difference Δt between adjacent points. j and acceleration a j for: Δt=t j+1 -t j ; Where, t j is the time of the jth data point; S2.

3. Setting the motion smoothness index δ v Define the rate of change of velocity within the window: d v =max(v j )-min(v j ); Motion smoothness index and standard deviation of acceleration σ a , used to assist in the determination of gross errors.

6. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 5, characterized in that: S3 includes, S3.

1. By setting the gross error judgment rules, the gross error points of the trajectory after multi-source data fusion are detected. The first step is to set the hard threshold, the second step is to set the soft threshold, and the third step is to set the multi-window voting mechanism; S3.

2. Set hard thresholds. Hard thresholds are used for single-point anomaly detection, including setting spatial thresholds and kinematic thresholds. The spatial threshold is if a point satisfies |x j -μ x |>3σ x or |y j -μ y |>3σ y or |z j -μ z |>3σ z , triggering the primary exception flag, x j 、y j 、z j is the three-dimensional coordinate of the j-th data point; The kinematic threshold is if v j >1.5v max or a j >2a max , marked as movement abnormality, v max is the maximum design speed of the submersible, a max is the maximum design acceleration of the submersible.

7. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 6, characterized in that: S3 includes, S3.

3. Setting soft constraints, where the soft constraints are context association detection, including time continuity verification and trajectory curvature consistency verification; Temporal continuity verification is that if a point is marked as abnormal, the abnormal status of m adjacent points is detected, where m is 2. If m+1 consecutive points are abnormal, it is determined to be a true gross error, otherwise it is considered as transient noise. The trajectory curvature consistency verification is to calculate the curvature κ of the front and back ends of the outlier point: Where v is the velocity of the front and rear ends of the outlier point, and a is the acceleration of the front and rear ends of the outlier point. If the curvature mutation exceeds twice the historical average value, it is strengthened into an outlier judgment; S3.

4. Set up a multi-window voting mechanism to prevent a point from being detected by multiple windows due to the overlap of sliding windows, and set the voting threshold V th , V th The value is 2. If a point is exceeded by V th If a window is judged to be abnormal, it is confirmed as a gross error point.

8. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 7, characterized in that: S4 includes, S4.

1. After identifying gross errors in S3, performing gross error elimination and adaptive trajectory repair, including gross error elimination strategy, hierarchical interpolation repair and post-repair optimization; S4.

2. Gross error elimination strategies include single point elimination and continuous segment elimination; Single point elimination is to directly delete the points confirmed as gross errors and record the index of the vacant position; The continuous segment is eliminated if the number of continuous gross error points N bad ≥3, it is considered as communication packet loss or sensor failure, the entire segment is removed and the time interval is recorded; S4.

3. Perform hierarchical interpolation repair. If N bad ≤2, short gap repair is performed, including linear interpolation and curve smoothing; Linear interpolation is to directly connect the previous and next valid points: Where p(t) is the coordinate of the interpolation point, p prev is the coordinate of the previous valid point, p next is the coordinate of the subsequent effective point, t is the linear interpolation time, t prev is the time of the previous effective point, t next The time of the next valid point; Curve smoothing generates smooth transitions by introducing control points to avoid sudden changes in broken lines.

9. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 8, characterized in that: S4 includes, S4.

4. Perform hierarchical interpolation repair, if N bad >2, long gap repair, including dynamic model interpolation and external data fusion; The dynamic model interpolation predicts the trajectory based on the vehicle's kinematic model: Where p0 is the initial coordinate, v0 is the initial velocity, and a0 is the initial acceleration. v0 and a0 are obtained by fitting the historical data before and after the gap. The kinematic model of the submersible is a constant acceleration model. External data fusion is to perform multi-source data Kalman filtering fusion if there is valid data from Doppler log and depth meter sensors during the vacancy period; S4.

5. Perform post-repair optimization, including curvature smoothing and energy minimization constraints; Curvature smoothing is to apply Savitzky-Golay filter to the repaired trajectory segment to eliminate high-frequency jitter while preserving motion characteristics; The energy minimization constraint is to construct the optimization objective function J to minimize the integral of the acceleration change of the trajectory: Where t1 is the starting time, t2 is the ending time, and the gradient descent method is used to solve the optimal smooth path.

10. The method for detecting and eliminating gross errors in a submersible's navigation trajectory based on a sliding window and standard deviation according to claim 9, characterized in that: S5 includes, S5.

1. performing iterative optimization of the algorithm and parameter adaptation, including iterative detection and parameter self-learning; S5.

2. Set up an iterative detection mechanism and re-execute steps S1 to S4 on the repaired trajectory, looping until any of the following conditions is met: Condition 1: The rate of decrease in the number of gross errors in two consecutive iterations is less than or equal to 5%; Condition 2: Reach the maximum number of iterations N max =5; S5.

3. Set the parameter self-learning strategy, adaptively adjust the threshold, and dynamically adjust the sensitivity coefficient based on historical detection results: Where, α new is the adjusted sensitivity coefficient, α old is the sensitivity coefficient before adjustment, η FP is the false alarm rate, η FN is the false negative rate, γ is the learning rate; Record the optimal window length under different environments and establish a lookup table for quick call in subsequent tasks.

Images