A method and system for estimating the depth of an underwater vehicle in a near-surface operating condition

Abstract

This invention discloses a method and system for estimating the depth of an underwater vehicle in near-surface operating conditions. The method includes the following steps: establishing a mathematical model of the underwater vehicle's depth channel; designing a state observer based on the mathematical model; calculating the average depth measurement value for each cycle; calculating the rate of change of the state observer's gain; updating the gain and performing adaptive adjustment; and updating the depth estimate. This invention designs a method for estimating the near-surface depth of an underwater vehicle under wave disturbance conditions based on the principle of the state observer, namely, adaptively adjusting the observer gain to complete the data filtering estimation of the depth channel.

Description

Technical Field

[0001] This invention relates to the field of underwater vehicle technology, and in particular to a method and system for estimating the depth of an underwater vehicle in near-surface operating conditions. Background Technology

[0002] When navigating near the surface, underwater vehicles are disturbed by near-surface waves due to the undulations of the sea surface, causing periodic fluctuations in depth measurements (pressure values). These pressure fluctuations are essentially measurement disturbances, not actual depth changes. Depth signals with periodic disturbances adversely affect control, as the controller attempts to suppress these fluctuations, resulting in actual depth fluctuations for the torpedo.

[0003] Due to the influence of waves, disturbance forces and torques (such as...) are generated on underwater vehicles. Figure 1 (As shown). These disturbance forces and moments act simultaneously in the X, Y, and Z directions of the torpedo system.

[0004] Specifically, when underwater vehicles navigate in shallow water, they are affected by ocean waves in two main ways:

[0005] 1. Dynamic effects

[0006] The average wave force experienced by a torpedo near the water surface is not zero. In addition to the oscillating force, there is also a component of steady force. The vertical force and moment, i.e., the wave-absorbing force, have the greatest impact on the vertical plane of the torpedo. This force increases as the travel depth decreases, making it difficult for the torpedo to maintain its depth and causing it to tend to be thrown towards the water surface.

[0007] 2. Kinematic effects

[0008] When a torpedo is navigating near the water's surface, its altitude should be physically kept constant. However, waves can still cause variations in pressure (depth measurement). Using this periodically changing depth measurement for feedback control would affect depth channel performance, potentially causing the torpedo to surface during rough seas. Summary of the Invention

[0009] The purpose of this invention is to observe the depth signal to estimate the true depth value, thereby providing an accurate input value for the longitudinal attitude controller (i.e., the depth channel) of the underwater vehicle, based on the influence of ocean wave fluctuations on the lift / lift torque of the underwater vehicle's ascent and descent.

[0010] To achieve the objectives of this invention, on one hand, this invention provides a depth estimation method for underwater vehicles operating near the water surface. This method utilizes an established underwater vehicle control model and wave model to design an adaptive state observer, which can effectively suppress first-order wave forces (high frequency). Specific steps include:

[0011] Step 1: Establish a mathematical model for the depth channel of the underwater vehicle;

[0012] Step 2: Design the state observer based on the mathematical model;

[0013] Step 3: Constant offset estimation. Calculate the average depth measurement of the first N cycles for each cycle (N≥150, approximately 1.5s).

[0014] Step 4: Calculate the observer gain for each cycle. The rate of change;

[0015] Step 5: Update gain every cycle To make adaptive adjustments;

[0016] Step 6: Update the depth estimate every period.

[0017] This invention designs a method for estimating the near-surface depth of an underwater vehicle under wave disturbance conditions based on the state observer principle. Specifically, it adaptively adjusts the observer gain to complete the data filtering estimation of the depth channel. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below.

[0019] Figure 1 A diagram illustrating the impact of ocean waves;

[0020] Figure 2 This is a schematic diagram of the state observer structure;

[0021] Figure 3 A schematic flowchart of a method for estimating the depth of an underwater vehicle in near-water surface conditions, provided in an embodiment of the present invention;

[0022] Figure 4 A schematic diagram illustrating the principle of the mathematical model for the depth channel of an underwater vehicle.

[0023] Figure 5 A schematic diagram of the simulation results for shallow water depth - Case 1;

[0024] Figure 6 A schematic diagram of the simulation results for shallow water depth - Case 2;

[0025] Figure 7 This is a schematic diagram of the simulation results for shallow water depth - use case 3. Detailed Implementation

[0026] The following detailed description of specific embodiments, in conjunction with examples, further illustrates the above-mentioned content of the present invention, but it should not be construed as limiting the scope of the subject matter of the present invention to the following examples. Various substitutions or modifications made based on ordinary technical knowledge and conventional methods in the art without departing from the above-described spirit of the present invention should be included within the scope of the present invention.

[0027] This invention provides a specific implementation method for online depth estimation when an underwater vehicle is navigating near water and there is a significant discrepancy between the raw data collected by the pressure sensor and the actual depth channel data due to the influence of ocean waves.

[0028] Figure 2 This is a schematic diagram of a state observer. When unobservable states exist in the state equations, estimation is required. The device (computer program) that estimates or observes the state variables is called a state observer, or simply an observer. The observer estimates the state variables based on the output measurement and control variables. Consider the following system state equations:

[0029] (1)

[0030] To perfectly fit the system's state equations, a dynamic model is established, assuming... Depend on If we approximate this, then the following dynamic model is the observer. For state variables, For control signals, For output signal, for A constant coefficient state matrix, for The input matrix with constant coefficients, for Constant coefficient output matrix

[0031] (2)

[0032] The input to the state observer is and The output is , This is the correction term, i.e., the output of the state equation. With estimated output The difference between them, matrix This is the weight matrix, i.e., the gain. The correction term monitors the state variables. When there are differences between the actual system (Equation 1) and the dynamic model (Equation 2), the correction term will reduce the impact of the differences.

[0033] Subtracting equation (2) from equation (1), we get the observer error equation.

[0034] (3)

[0035] Assume that the error bias vector

[0036] (4)

[0037] Substituting equation (4) into equation (3), we get

[0038] (5)

[0039] (6)

[0040] if , then, when hour, ,at this time, .

[0041] This invention, based on the aforementioned state observer principle, designs an algorithm for estimating the near-surface depth of underwater vehicles under wave disturbance conditions, i.e., adjusting the observer gain. Adaptive adjustments are made to complete the data filtering estimation for the depth channel.

[0042] Figure 3 This is a schematic flowchart illustrating a method for estimating the depth of an underwater vehicle in near-surface operating conditions, provided as an embodiment of the present invention. Figure 3 As shown, the method includes steps S101-S106:

[0043] Step S101: Establish a mathematical model for the depth channel of the underwater vehicle.

[0044] Specifically, the equation describing the variation of the longitudinal motion parameters of an underwater vehicle is called the longitudinal motion equation: According to the kinematic equation of the depth channel of the underwater vehicle, we know that:

[0045] (7)

[0046] in, These are depth sensor measurements. For longitudinal velocity, Vertical velocity, The pitch angle.

[0047] Based on actual conditions, such as the inherent noise of the pressure sensor, unknown vertical velocity, and random and uncontrollable factors such as near-surface wave disturbance, the embodiments of the present invention create the following measurement model of the system according to the above formula (7):

[0048] (8)

[0049] in, This is the measurement value from the depth sensor (pressure gauge output). This is a depth estimate. Output values ​​for the system measurement model; Disturbed by the waves, For wave amplitude, For a period of time, This is the initial phase; For the accumulation of system error bias, i.e. , ; It is a constant.

[0050] Step S102: Design a depth channel state observer. The observer estimates the state variables based on the output measurement and control variables.

[0051] In equation (7), the measurable quantity is and The immeasurable quantity is , It is measurable, but it is greatly disturbed by ocean waves, meaning the depth measured by the pressure sensor is not the actual depth. This causes inaccuracies in the depth readings of underwater vehicles. It is in an unpredictable state.

[0052] Therefore, according to equation (7), a state observer is introduced, which uses known quantities. , and The estimated precise depth of the underwater vehicle is:

[0053] (9)

[0054] Step S103: Estimate the average depth measurement for the first N cycles using the following formula (N≥150, approximately 1.5s).

[0055] (10)

[0056] in, This is the depth constant offset estimate, i.e., the average depth measurement, where N is the number of depth sampling periods. It is the measurement value from the depth sensor (pressure gauge output). It is a sampling period count. It refers to the current moment.

[0057] Step S104, calculate the gain rate of change ;

[0058] (11)

[0059] in, It is a constant;

[0060] In this embodiment of the invention, .

[0061] Step S105: Update the gain every cycle. , It will make adaptive adjustments.

[0062] (12)

[0063] In this embodiment of the invention, .

[0064] Step S106: Update the depth estimate every cycle.

[0065] Specifically, using the calculation results of steps S102 and S104, the observer output of step S102 is updated every cycle to obtain the depth adaptive estimate.

[0066] (13)

[0067] (14)

[0068] The shallow water navigation controller design integrates pitch angle and depth sensor measurements, employing an adaptive state observation method to suppress wave disturbances to depth information. Therefore, simulations use sine waves of different frequencies, amplitudes, and phases to model both the actual depth and the depth measurements after wave disturbance, comparing the effectiveness of the algorithm in suppressing wave noise.

[0069] The following simulations verify the effects of wave disturbances at different frequencies:

[0070] Use Case 1 (Simulation results are as follows) Figure 5 (As shown)

[0071] Simulation conditions:

[0072] 1) Actual depth: 0.1Hz;

[0073] 2) Wave disturbance: frequency 0.5Hz, amplitude 1m

[0074] (1) Case 2 (simulation results are as follows) Figure 6 (As shown)

[0075] Simulation conditions:

[0076] 1) Actual depth: 0.5Hz;

[0077] 2) Wave disturbance: frequency 0.1Hz, amplitude 1.5m

[0078] (2) Case 3 (Simulation results are as follows) Figure 7 (As shown)

[0079] Simulation conditions:

[0080] 1) Actual depth: 0.2Hz;

[0081] 2) Wave disturbance: frequencies of 0.2Hz and 0.8Hz superimposed, with amplitudes of 1m and 0.2m respectively.

[0082] Simulation Result Analysis:

[0083] Simulation results of wave disturbances at different frequencies show that the effect of periodic disturbances can be effectively suppressed. The effect is better when the wave disturbance noise frequency is higher than the actual depth channel frequency, but there is also a significant effect when the wave frequency is lower.

[0084] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for estimating the depth of an underwater vehicle in near-surface operating conditions, characterized in that, Includes the following steps: Establish a mathematical model for the depth channel of an underwater vehicle; Design a state observer based on a deep channel mathematical model; Calculate the average depth measurement for each cycle; Calculate the rate of change of the state observer gain; Update the gain and make adaptive adjustments; Update the depth estimate.

2. The method according to claim 1, characterized in that, In the step of establishing the mathematical model of the depth channel of the underwater vehicle, the equation for the variation law of the longitudinal motion parameters of the underwater vehicle becomes the longitudinal motion equation: According to the kinematic equation of the depth channel of the underwater vehicle, it can be known that: in, These are depth sensor measurements. For longitudinal velocity, Vertical velocity, It is the pitch angle.

3. The method according to claim 2, characterized in that, A measurement model is created based on the described kinematic approach: in, These are depth sensor measurements. For longitudinal velocity, Where K is the pitch angle and K is the gain. This is a depth estimate. This is for the accumulation of system error bias; The output value of the system measurement model, Disturbed by the waves, For wave amplitude, For a period of time, For the first phase, It is the sampling period count.

4. The method according to claim 2, characterized in that, The step of designing a state observer based on a deep channel mathematical model includes: using known quantities , and The estimated precise depth of the underwater vehicle is: in, These are depth sensor measurements. For longitudinal velocity, Where K is the pitch angle and K is the gain. This is a depth estimate. For the accumulation of system error bias, It refers to the current moment.

5. The method according to claim 1, characterized in that, The step of calculating the average depth measurement for each cycle includes: estimating the average depth measurement for the first N cycles, where N ≥ 150. in, It is the depth constant offset estimate, i.e., the average depth measurement; N is the number of depth sampling periods; These are depth sensor measurements. It is a sampling period count; It refers to the current moment.

6. The method according to claim 5, characterized in that, The step of calculating the rate of change of the state observer gain includes: calculating the gain. rate of change ; in, It is a constant. It is the depth constant offset estimate, i.e., the average depth measurement; N is the number of depth sampling periods; These are depth sensor measurements. It is a sampling period count; It is the current moment; =0.0125.

7. The method according to claim 6, characterized in that, The gain update step includes: Gain updated every cycle , , 。 8. The method according to claim 7, characterized in that, The step of updating the depth estimate includes: updating the state observer output based on the average depth measurement and the rate of change of the state observer gain to obtain an adaptive depth estimate; in, For longitudinal velocity, Where K is the pitch angle and K is the gain. These are depth sensor measurements. This is a depth estimate. For the accumulation of system error bias, It refers to the current moment.

9. A depth estimation system for underwater vehicles operating near the water surface, characterized in that, include: The first processing unit is used to establish a mathematical model of the depth channel of the underwater vehicle. The second processing unit is used to design a state observer based on the deep channel mathematical model; The third processing unit is used to calculate the average depth measurement value for each cycle. The fourth processing unit is used to calculate the rate of change of the state observer gain; The fifth processing unit is used to update the gain and make adaptive adjustments. The sixth processing unit is used to update the depth estimate.

10. The system according to claim 9, characterized in that, The second processing unit is used to update the state observer output based on the average depth measurement and the rate of change of the state observer gain, so as to obtain the adaptive depth estimate.

Images