Adaptive environment weighted data fusion navigation and positioning method for manned submersible in polar region

Abstract

The invention discloses an adaptive environment weighted data fusion navigation and positioning method for a manned submersible in a polar region, which belongs to the technical field of ocean navigation and positioning, is used for navigation and positioning of the submersible in the polar region, and comprises the following steps: acquiring data of IMU (Inertial Measurement Unit), acoustics, optics, environment sensors and Doppler sensors, and carrying out de-noising, time synchronization and coordinate conversion; modeling the polar region environment variable, calculating the adaptive weight, and dynamically adjusting the contribution degree of the sensor by adopting an exponential decay function; introducing an environment factor to correct the weight, and performing nonlinear state estimation by adopting an EKF (Extended Kalman Filter); performing error modeling by using historical data, and optimizing a weight adjustment strategy in combination with machine learning; and filtering optimization is performed on the fusion result, and multi-scale correction is performed in combination with prior environment information. Compared with the prior art, the influence of a single sensor error on a final positioning result is reduced, the positioning precision of the submersible in a polar region complex environment is improved, and the stable navigation capability is kept under the condition that sensor data suddenly changes or is lost.

Description

Technical Field

[0001] The invention discloses a polar region manned submersible adaptive environment weighted data fusion navigation and positioning method, belonging to the technical field of ocean navigation and positioning. Background Art

[0002] With the continuous deepening of polar scientific exploration, marine resource development and deep-sea engineering, the navigation and positioning of polar manned submersibles in extreme environments has received increasing attention. The commonly used navigation and positioning systems currently rely mainly on inertial measurement units (IMUs), acoustic positioning devices, optical sensors and Doppler velocimeters, etc., and achieve positioning through multi-sensor data fusion. However, the polar sea environment has harsh conditions such as low temperature, high pressure, ice interference, multipath effects and complex seabed topography. These factors can easily lead to increased sensor data noise, error accumulation and signal shielding, which seriously affects positioning accuracy and system stability.

[0003] Existing technologies usually use preset noise models and fixed weights for data fusion. This method cannot dynamically respond to changes in sensor performance in polar environments, lacks real-time feedback and closed-loop correction mechanisms, and is difficult to ensure high accuracy and robustness of navigation positioning in complex environments. At the same time, traditional methods fail to fully utilize the impact of real-time environmental parameters such as water temperature, salinity, and ice interference on the reliability of sensor data, resulting in a sharp drop in the overall navigation system performance when some sensor data fails or errors increase under extreme conditions.

[0004] Therefore, there is an urgent need for a navigation and positioning technology that can dynamically adjust the data fusion weight according to real-time environmental parameters and sensor status to improve the positioning accuracy and stability of polar manned submersibles in complex ocean environments. The prior art has not yet provided a comprehensive solution that takes into account environmental adaptability and sensor dynamic performance feedback, which provides the necessary technical improvement basis for the proposal of the present invention. Summary of the invention

[0005] The purpose of the present invention is to provide a polar manned submersible adaptive environment weighted data fusion navigation and positioning method to solve the problems of low navigation and positioning accuracy in polar waters and great influence by the environment in the prior art.

[0006] A method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion, comprising:

[0007] S1. Obtain IMU, acoustic, optical, environmental sensor and Doppler sensor data, perform denoising, time synchronization and coordinate conversion to ensure that each data source is used for fusion calculation;

[0008] S2. Model polar environmental variables, combine sensor measurement residuals, calculate adaptive weights, and use exponential decay functions to dynamically adjust the contribution of different sensors;

[0009] S3. Based on weighted data fusion, environmental factors are introduced to correct weights, and extended Kalman filter (EKF) is used for nonlinear state estimation to achieve precise positioning.

[0010] S4. Through the error feedback mechanism, historical data is used for error modeling, and the weight adjustment strategy is optimized in combination with machine learning methods to achieve closed-loop correction and improve long-term navigation accuracy;

[0011] S5. Filter and optimize the fusion results, and perform multi-scale corrections based on prior environmental information to ensure that navigation solutions can be provided in extreme environments.

[0012] S1 includes S1.1. In polar environments, manned submersibles are equipped with sensors including IMU, acoustic positioning system, optical sensor, environmental sensor and deep-water Doppler velocimeter. The data collected by IMU include acceleration a(t) and angular velocity ω(t). The data collected by the acoustic positioning system include long baseline LBL or ultra-short baseline USBL relative coordinates (x a ,y a ,z a ) and signal-to-noise ratio SNRa, the data collected by the optical sensor includes environmental images, point cloud data and feature matching displacement ΔP opt The data collected by the environmental sensors include real-time water temperature T, salinity S, ice thickness h ice , sound speed c, the data collected by the deep-water Doppler velocimeter includes speed and ranging information.

[0013] S1 includes:

[0014] S1.2. Perform data preprocessing on each sensor, synchronize timestamps, and use hardware clock to align all data to make the timing consistent;

[0015] S1.3. Use zero bias compensation and Kalman filtering for IMU data, and adaptive threshold denoising for acoustic data, and Kalman filtering to remove sensor errors:

[0016]

[0017] In the formula, x filtered is the data using Kalman filtering to remove sensor errors, x raw is the raw data of the sensor, is the predicted value, and K is the Kalman gain.

[0018] S1 includes S1.4. Coordinate transformation includes converting IMU data to the navigation coordinate system (X n ,Y n ,Z n ):

[0019]

[0020] Where R is the rotation matrix and T is the coordinate offset;

[0021] The acoustic data uses the least squares method to correct the ranging error:

[0022] d corr =d meas -Δd multi -Δd temp ;

[0023] Where, d meas is the mean value of acoustic data, Δd multi is the multipath error correction term, Δd temp is the temperature-dependent correction term.

[0024] S2 includes S2.1. Sensor adaptive weighted calculation. The performance of each sensor in the polar environment fluctuates with the change of environmental factors, including temperature, salinity, and ice thickness. The sensor weight Wi(k) at time k is calculated using residuals and environmental influencing factors:

[0025]

[0026] In the formula, R i (k) is the residual of the ith sensor at time k, α is the parameter that adjusts the residual effect, and R j (k) is the residual error of j sensors at time k, E i (k) is the environmental impact factor;

[0027] Calculate R i (k) is:

[0028]

[0029] In the formula, Z i (k) is the observation value of the i-th sensor at time k, is the predicted value obtained by state estimation;

[0030] Calculate E i (k) is:

[0031]

[0032] In the formula, E ref is the ideal environment value; E i is the current environmental measurement value, including sound speed and temperature; β is the adjustment parameter that controls the sensitivity of environmental factors.

[0033] S2 includes S2.2. The navigation solution after weighted fusion of the weights of each sensor is:

[0034] X f (k)=∑ i W i (k)X i (k);

[0035] V f (k)=∑ i W i (k)V i (k);

[0036] Where, X f (k) is the final position estimate, X i (k) is the estimated position of sensor i at time k, V f (k) is the final velocity estimate, V i (k) is the velocity estimate of sensor i at time k.

[0037] S3 includes S3.1. Using EKF to update the state, further optimize the navigation accuracy, state prediction, assuming that the state vector X at time k k for:

[0038] X k =[x,y,z,v x ,v y ,v z ,φ,θ,ψ] T ;

[0039] Where x, y and z are the three-dimensional positions of the submersible, v x 、v y and v z is the three-dimensional velocity of the submersible, φ is the pitch angle, θ is the roll angle, and ψ is the yaw angle;

[0040] The state transfer equation is:

[0041] X k+1 =FX k +GU k +W k ;

[0042] Where F is the state transfer matrix, which is defined by the physical motion model; G is the control input matrix, which includes propulsion force and environmental disturbance terms; U k is the control input, including the thrust of the propeller and the steering control amount; W k is the process noise.

[0043] S3 includes S3.2. Using the measured value Z of the sensor k Update the state and observe the updated measurement equation:

[0044] Z k=HX k +V k ;

[0045] Where H is the observation matrix, which defines the mapping of sensor measurements to state variables, V k is the measurement noise, which is affected by sensor errors and the environment;

[0046] Update Kalman gain K k for:

[0047] K k =P k H T (HP k H T +R k ) -1 ;

[0048] Where P k is the state covariance matrix, R k is the measurement noise covariance matrix;

[0049] The final status update is:

[0050]

[0051] In the formula, is the predicted state, is the updated status, is the predicted state covariance matrix, is the updated state covariance matrix.

[0052] S4 includes S4.1. Introducing a real-time feedback mechanism to improve the robustness of the navigation system, perform error closed-loop correction, and calculate the navigation error e k :

[0053] e k =‖X f (k)-X ref (k) ‖;

[0054] Where, X ref (k) is the reference position, which is the known starting point or target position. If e k If the threshold is exceeded, the feedback mechanism is activated to readjust Wi(k) and recalculate the state estimate to correct the system deviation.

[0055] S4 includes S4.2. Based on statistical analysis, using Mahalanobis distance D M Determine abnormal data and perform sensor anomaly detection:

[0056]

[0057] Where μ is the mean of the observations, ∑ is the covariance matrix, which describes the measurement noise. If DM is greater than the set threshold, the i-th sensor data is discarded to ensure the accuracy of data fusion.

[0058] Compared with the prior art, the present invention has the following beneficial effects: by introducing environmental factors to dynamically adjust sensor weights, and adopting an adaptive weighted fusion algorithm based on measurement residuals and environmental influencing factors, the influence of single sensor errors on the final positioning results is effectively reduced, thereby improving the positioning accuracy of the submersible in the complex environment of the polar regions, and maintaining stable navigation capabilities in the event of sudden changes or loss of sensor data. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a technical flow chart of the present invention. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] The technical flow chart of the present invention is as follows Figure 1 As shown in the figure, an adaptive environment weighted data fusion navigation and positioning method for polar manned submersibles is divided into five steps: sensor data preprocessing, adaptive weight calculation, precise positioning based on EKF, error modeling and weight optimization, and fusion result optimization. Sensor data preprocessing includes collecting multi-source sensor data, denoising, time synchronization and coordinate conversion. Adaptive weight calculation includes establishing an environmental variable model, combining measurement residuals, and using exponential decay function to calculate and adjust weights. EKF-based precise positioning includes introducing environmental factors to correct weights on the basis of weighted fusion and using EKF for nonlinear state estimation. Error modeling and weight optimization include building an error model using historical data through an error feedback mechanism, optimizing weight strategies in combination with machine learning algorithms, and realizing closed-loop correction. Fusion result optimization includes filtering optimization and multi-scale correction combined with prior environmental information.

[0062] A method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion, comprising:

[0063] S1. Obtain IMU, acoustic, optical, environmental sensor and Doppler sensor data, perform denoising, time synchronization and coordinate conversion to ensure that each data source is used for fusion calculation;

[0064] S2. Model polar environmental variables, combine sensor measurement residuals, calculate adaptive weights, and use exponential decay functions to dynamically adjust the contribution of different sensors;

[0065] S3. Based on weighted data fusion, environmental factors are introduced to correct weights, and extended Kalman filter (EKF) is used for nonlinear state estimation to achieve precise positioning.

[0066] S4. Through the error feedback mechanism, historical data is used for error modeling, and the weight adjustment strategy is optimized in combination with machine learning methods to achieve closed-loop correction and improve long-term navigation accuracy;

[0067] S5. Filter and optimize the fusion results, and perform multi-scale corrections based on prior environmental information to ensure that navigation solutions can be provided in extreme environments.

[0068] S1 includes S1.1. In polar environments, manned submersibles are equipped with sensors including IMU, acoustic positioning system, optical sensor, environmental sensor and deep-water Doppler velocimeter. The data collected by IMU include acceleration a(t) and angular velocity ω(t). The data collected by the acoustic positioning system include long baseline LBL or ultra-short baseline USBL relative coordinates (x a ,y a ,z a ) and signal-to-noise ratio SNRa, the data collected by the optical sensor includes environmental images, point cloud data and feature matching displacement ΔP opt The data collected by the environmental sensors include real-time water temperature T, salinity S, ice thickness h ice , sound speed c, the data collected by the deep-water Doppler velocimeter includes speed and ranging information.

[0069] S1 includes:

[0070] S1.2. Perform data preprocessing on each sensor, synchronize timestamps, and use hardware clock to align all data to make the timing consistent;

[0071] S1.3. Use zero bias compensation and Kalman filtering for IMU data, and adaptive threshold denoising for acoustic data, and Kalman filtering to remove sensor errors:

[0072]

[0073] In the formula, x filtered is the data using Kalman filtering to remove sensor errors, x raw is the raw data of the sensor, is the predicted value, and K is the Kalman gain.

[0074] S1 includes S1.4. Coordinate transformation includes converting IMU data to the navigation coordinate system (Xn ,Y n ,Z n ):

[0075]

[0076] Where R is the rotation matrix and T is the coordinate offset;

[0077] The acoustic data uses the least squares method to correct the ranging error:

[0078] d corr =d meas -Δd multi -Δd temp ;

[0079] Where, d meas is the mean value of acoustic data, Δd multi is the multipath error correction term, Δd temp is the temperature-dependent correction term.

[0080] S2 includes S2.1. Sensor adaptive weighted calculation. The performance of each sensor in the polar environment fluctuates with the change of environmental factors, including temperature, salinity, and ice thickness. The sensor weight Wi(k) at time k is calculated using residuals and environmental influencing factors:

[0081]

[0082] In the formula, R i (k) is the residual of the ith sensor at time k, α is the parameter that adjusts the residual effect, and R j (k) is the residual error of j sensors at time k, E i (k) is the environmental impact factor;

[0083] Calculate R i (k) is:

[0084]

[0085] In the formula, Z i (k) is the observation value of the i-th sensor at time k, is the predicted value obtained by state estimation;

[0086] Calculate E i (k) is:

[0087]

[0088] In the formula, E ref is the ideal environment value; E i is the current environmental measurement value, including sound speed and temperature; β is the adjustment parameter that controls the sensitivity of environmental factors.

[0089] S2 includes S2.2. The navigation solution after weighted fusion of the weights of each sensor is:

[0090] X f (k)=∑ i W i (k)X i (k);

[0091] V f (k)=∑ i W i (k)V i (k);

[0092] Where, X f (k) is the final position estimate, X i (k) is the estimated position of sensor i at time k, V f (k) is the final velocity estimate, V i (k) is the velocity estimate of sensor i at time k.

[0093] S3 includes S3.1. Using EKF to update the state, further optimize the navigation accuracy, state prediction, assuming that the state vector X at time k k for:

[0094] X k =[x,y,z,v x ,v y ,v z ,φ,θ,ψ] T ;

[0095] Where x, y and z are the three-dimensional positions of the submersible, v x 、v y and v z is the three-dimensional velocity of the submersible, φ is the pitch angle, θ is the roll angle, and ψ is the yaw angle;

[0096] The state transfer equation is:

[0097] X k+1 =FX k +GU k +W k ;

[0098] Where F is the state transfer matrix, which is defined by the physical motion model; G is the control input matrix, which includes propulsion force and environmental disturbance terms; U k is the control input, including the thrust of the propeller and the steering control amount; W k is the process noise.

[0099] S3 includes S3.2. Using the measured value Z of the sensork Update the state and observe the updated measurement equation:

[0100] Z k =HX k +V k ;

[0101] Where H is the observation matrix, which defines the mapping of sensor measurements to state variables, V k is the measurement noise, which is affected by sensor errors and the environment;

[0102] Update Kalman gain K k for:

[0103] K k =P k H T (HP k H T +R k ) -1 ;

[0104] Where P k is the state covariance matrix, R k is the measurement noise covariance matrix;

[0105] The final status update is:

[0106]

[0107] In the formula, is the predicted state, is the updated status, is the predicted state covariance matrix, is the updated state covariance matrix.

[0108] S4 includes S4.1. Introducing a real-time feedback mechanism to improve the robustness of the navigation system, perform error closed-loop correction, and calculate the navigation error e k :

[0109] e k =‖X f (k)-X ref (k) ‖;

[0110] Where, X ref (k) is the reference position, which is the known starting point or target position. If e k If the threshold is exceeded, the feedback mechanism is activated to readjust Wi(k) and recalculate the state estimate to correct the system deviation.

[0111] S4 includes S4.2. Based on statistical analysis, using Mahalanobis distance D M Determine abnormal data and perform sensor anomaly detection:

[0112]

[0113] Where μ is the mean of the observations, and v is the covariance matrix, which describes the measurement noise. If DM is greater than the set threshold, the i-th sensor data is discarded to ensure the accuracy of data fusion.

[0114] The system of the present invention integrates multiple sensors such as IMU, acoustic positioning device, optical / laser sensor and Doppler velocimeter, and collects environmental parameters such as water temperature, salinity, ice interference and so on in real time. After the preprocessing module filters, denoises and normalizes the data of each sensor, an adaptive weight calculation formula combining residual and environmental influencing factor is adopted, and the weight of each sensor is dynamically adjusted through an exponential decay function, so as to reduce the adverse effects caused by environmental interference or error increase, and output high-precision positioning information. The feedback correction module adjusts the parameters in real time according to the overall positioning error to form a closed-loop adaptive mechanism, which further improves the system stability and positioning accuracy.

[0115] 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, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features may be replaced by 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 navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion, characterized in that: include: S1. Obtain IMU, acoustic, optical, environmental sensor and Doppler sensor data, perform denoising, time synchronization and coordinate conversion to ensure that each data source is used for fusion calculation; S2. Model polar environmental variables, combine sensor measurement residuals, calculate adaptive weights, and use exponential decay functions to dynamically adjust the contribution of different sensors; S3. Based on weighted data fusion, environmental factors are introduced to correct weights, and extended Kalman filter (EKF) is used for nonlinear state estimation to achieve precise positioning. S4. Through the error feedback mechanism, historical data is used for error modeling, and the weight adjustment strategy is optimized in combination with machine learning methods to achieve closed-loop correction and improve long-term navigation accuracy; S5. Filter and optimize the fusion results, and perform multi-scale corrections based on prior environmental information to ensure that navigation solutions can be provided in extreme environments.

2. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 1, characterized in that: S1 includes S1.

1. In polar environments, manned submersibles are equipped with sensors including IMU, acoustic positioning system, optical sensor, environmental sensor and deep-water Doppler velocimeter. The data collected by IMU include acceleration a(t) and angular velocity ω(t). The data collected by the acoustic positioning system include long baseline LBL or ultra-short baseline USBL relative coordinates (x a ,y a ,z a ) and signal-to-noise ratio SNRa, the data collected by the optical sensor includes environmental images, point cloud data and feature matching displacement ΔP opt The data collected by the environmental sensors include real-time water temperature T, salinity S, ice thickness h ice , sound speed c, the data collected by the deep-water Doppler velocimeter includes speed and ranging information.

3. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 2, characterized in that: S1 includes: S1.

2. Perform data preprocessing on each sensor, synchronize timestamps, and use hardware clock to align all data to make the timing consistent; S1.

3. Use zero bias compensation and Kalman filtering for IMU data, and adaptive threshold denoising for acoustic data, and Kalman filtering to remove sensor errors: In the formula, x filtered is the data using Kalman filtering to remove sensor errors, x raw is the raw data of the sensor, is the predicted value, and K is the Kalman gain.

4. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 3 is characterized in that: S1 includes S1.

4. Coordinate transformation includes converting IMU data to the navigation coordinate system (X n ,Y n ,Z n ): Where R is the rotation matrix and T is the coordinate offset; The acoustic data uses the least squares method to correct the ranging error: d corr =d meas -Δd multi -Δd temp ; Where, d meas is the mean value of acoustic data, Δd multi is the multipath error correction term, Δd temp is the temperature-dependent correction term.

5. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 4, characterized in that: S2 includes S2.

1. Sensor adaptive weighted calculation. The performance of each sensor in the polar environment fluctuates with the change of environmental factors, including temperature, salinity, and ice thickness. The sensor weight Wi(k) at time k is calculated using residuals and environmental influencing factors: In the formula, R i (k) is the residual of the ith sensor at time k, α is the parameter that adjusts the residual effect, and R j (k) is the residual error of j sensors at time k, E i (k) is the environmental impact factor; Calculate R i (k) is: In the formula, Z i (k) is the observation value of the i-th sensor at time k, is the predicted value obtained by state estimation; Calculate E i (k) is: In the formula, E ref is the ideal environment value; E i is the current environmental measurement value, including sound speed and temperature; β is the adjustment parameter that controls the sensitivity of environmental factors.

6. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 5, characterized in that: S2 includes S2.

2. The navigation solution after weighted fusion of the weights of each sensor is: X f (k)=∑ i W i (k)X i (k); V f (k)=∑ i W i (k)V i (k); Where, X f (k) is the final position estimate, X i (k) is the estimated position of sensor i at time k, V f (k) is the final velocity estimate, V i (k) is the velocity estimate of sensor i at time k.

7. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 6, characterized in that: S3 includes S3.

1. Using EKF to update the state, further optimize the navigation accuracy, state prediction, assuming that the state vector X at time k k for: X k =[x,y,z,v x ,v y ,v z ,φ,θ,ψ] T ; Where x, y and z are the three-dimensional positions of the submersible, v x 、v y and v z is the three-dimensional velocity of the submersible, φ is the pitch angle, θ is the roll angle, and ψ is the yaw angle; The state transfer equation is: X k+1 =FX k +GU k +W k ; Where F is the state transfer matrix, which is defined by the physical motion model; G is the control input matrix, which includes propulsion force and environmental disturbance terms; U k is the control input, including the thrust of the propeller and the steering control amount; W k is the process noise.

8. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 7, characterized in that: S3 includes S3.

2. Using the measured value Z of the sensor k Update the state and observe the updated measurement equation: Z k =HX k +V k ; Where H is the observation matrix, which defines the mapping of sensor measurements to state variables, V k is the measurement noise, which is affected by sensor errors and the environment; Update Kalman gain K k for: K k =P k H T (HP k H T +R k ) -1 ; Where P k is the state covariance matrix, R k is the measurement noise covariance matrix; The final status update is: In the formula, is the predicted state, is the updated status, is the predicted state covariance matrix, is the updated state covariance matrix.

9. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 8, characterized in that: S4 includes S4.

1. Introducing a real-time feedback mechanism to improve the robustness of the navigation system, perform error closed-loop correction, and calculate the navigation error e k : e k =‖X f (k)-X ref (k)‖; Where, X ref (k) is the reference position, which is the known starting point or target position. If e k If the threshold is exceeded, the feedback mechanism is activated to readjust Wi(k) and recalculate the state estimate to correct the system deviation.

10. The method for navigation and positioning of polar manned submersibles by adaptive environment weighted data fusion according to claim 9, characterized in that: S4 includes S4.

2. Based on statistical analysis, using Mahalanobis distance D M Determine abnormal data and perform sensor anomaly detection: Where μ is the mean of the observations, ∑ is the covariance matrix, which describes the measurement noise. If DM is greater than the set threshold, the i-th sensor data is discarded to ensure the accuracy of data fusion.

Images