An unmanned underwater vehicle speed estimation method based on water dynamics model constraint

Abstract

The application relates to a kind of unmanned underwater vehicle speed measurement methods based on water dynamics model constraint, comprising the following steps, step one: error threshold setting, according to the flow velocity of unmanned underwater vehicle operation water area, the velocity error threshold T of water dynamics model is set;Step two: theoretical speed calculation, based on water dynamics model, using propeller speed and rudder angle information to calculate the theoretical speed of unmanned underwater vehicle.The application can avoid the interference of abnormal speed information output by Doppler log on unmanned underwater vehicle navigation and operation, and improve the precision and environmental adaptability of unmanned underwater vehicle speed measurement and navigation.

Description

Technical field:

[0001] The present invention belongs to the technical field of unmanned underwater vehicle speed calculation, and in particular relates to an unmanned underwater vehicle speed calculation method based on hydrodynamic model constraints. Background technology:

[0002] Unmanned underwater vehicles (UUVs) can operate autonomously underwater without human control, playing a vital role in marine resource exploration and development, military applications, and are being actively developed in many countries around the world. Because UUVs cannot receive positioning information from global navigation satellite systems (GNSS) while navigating underwater, achieving high-precision underwater navigation has become a key issue in their development. Currently, UUVs often use a navigation system consisting of an inertial navigation system and a Doppler velocity log (DVL). This navigation system relies on velocity information output by the Doppler velocity log to update navigation status. However, as an acoustic device, the Doppler velocity log is susceptible to interference from external acoustic signals or its own multiple echo signals, potentially generating abnormal velocity outputs and causing serious deviations in the UUV's navigation. Summary of the invention:

[0003] The technical problem to be solved by the present invention is to provide a method for measuring the speed of an unmanned underwater vehicle based on hydrodynamic model constraints. This method adds hydrodynamic model constraints to the measured speed output by a Doppler log to determine the validity of the measured speed, thereby avoiding the interference of abnormal speed information output by the Doppler log with the navigation and operation of the unmanned underwater vehicle, thereby improving the accuracy and environmental adaptability of the unmanned underwater vehicle's speed measurement and navigation.

[0004] The technical solution of the present invention is to provide a method for calculating the speed of an unmanned underwater vehicle based on hydrodynamic model constraints, comprising the following steps:

[0005] Step 1: Error threshold setting: according to the water velocity of the UUV operating waters, the speed error threshold T of the hydrodynamic model is set;

[0006] Step 2: Theoretical speed calculation: Based on the hydrodynamic model, the theoretical speed of the UUV is calculated using propeller speed and rudder angle information:

[0007] v Model =F(n, angles)

[0008] Where: v Model is the theoretical speed output by the hydrodynamic model; F(·) is the hydrodynamic model, whose input is the propeller speed and rudder angle, and the output is the theoretical speed; n is the propeller speed of the unmanned underwater vehicle; angles is the rudder angle of the unmanned underwater vehicle;

[0009] Step 3: Receive the speed measurement, receive the speed measurement output by the Doppler log on the unmanned underwater vehicle DVL ;

[0010] Step 4: Speed ​​error calculation, calculate the error between the measured speed and the theoretical speed

[0011] err=E(v Model , v DVL )

[0012] Where: err is the error between the measured speed and the theoretical speed, E(·) is the error calculation function, whose input is the theoretical speed and the measured speed, and the output is the error; v Model is the theoretical velocity output by the hydrodynamic model; v DVL is the measured speed output by the Doppler log;

[0013] Step 5: Comprehensive speed calculation. If the error err is less than the speed error threshold T of the hydrodynamic model, the comprehensive speed of the unmanned underwater vehicle is the measured speed output by the Doppler log; otherwise, the comprehensive speed of the unmanned underwater vehicle is determined to be the theoretical speed output by the hydrodynamic model.

[0014]

[0015] Where: v UUV is the comprehensive calculated speed of the unmanned underwater vehicle; v DVL is the measured velocity output by the Doppler log; v Model is the theoretical velocity output by the hydrodynamic model.

[0016] Compared with the prior art, the present invention has the following advantages:

[0017] The present invention provides a method for calculating the speed of an unmanned underwater vehicle based on hydrodynamic model constraints with a simple process and convenient implementation, which effectively avoids the interference of abnormal speed information of a Doppler log and improves the accuracy and environmental adaptability of the speed measurement and navigation of the unmanned underwater vehicle. Description of the drawings:

[0018] Figure 1 This is an information flow diagram of an embodiment of the present invention.

[0019] Figure 2 Flowchart of the method of the present invention.

[0020] Figure 3 is the forward measurement speed of the Doppler speed meter in the embodiment.

[0021] Figure 4 It is the forward comprehensive calculation speed of the present invention in the embodiment.

[0022] Figure 5 The navigation trajectory is compared using a Doppler odometer to measure speed and the comprehensive speed calculation method of the present invention. Specific implementation method:

[0023] The present invention will be further described below with reference to the accompanying drawings:

[0024] like Figure 1 The UUV is equipped with a 600kHz phased array Doppler speed log, a propeller speed sensor, a rudder angle sensor, and a main control board. The Doppler speed log communicates with the main control board via the UDP network protocol, while the propeller speed sensor and rudder angle sensor communicate with the main control board via CAN. The main control board runs a hydrodynamic model program and a speed calculation program. The hydrodynamic model program receives propeller speed and rudder angle information and outputs the theoretical speed. The speed calculation program receives the hydrodynamic model speed error threshold, the Doppler speed log measured speed, and the hydrodynamic model theoretical speed to complete the speed calculation.

[0025] like Figure 2-5 As shown, a method for calculating the speed of an unmanned underwater vehicle based on hydrodynamic model constraints includes the following steps:

[0026] (1) Error threshold setting

[0027] According to the water flow velocity in the operating waters of the unmanned underwater vehicle, the operator sets the speed error threshold T = 0.5m / s of the hydrodynamic model, and the speed comprehensive measurement program receives the threshold information.

[0028] (2) Theoretical speed calculation

[0029] The hydrodynamic model program receives the propeller speed and rudder angle information fed back by the propeller speed sensor and rudder angle sensor, and calculates the theoretical speed of the unmanned underwater vehicle based on the hydrodynamic model:

[0030]

[0031] Where: is the forward theoretical velocity output by the hydrodynamic model; is the rightward theoretical velocity output by the hydrodynamic model; is the upward theoretical speed output by the hydrodynamic model; F(·) is the hydrodynamic model, whose input is the propeller speed and rudder angle, and the output is the theoretical speed; n is the propeller speed of the unmanned underwater vehicle; angle is the rudder angle of the unmanned underwater vehicle.

[0032] (3) Measurement speed reception

[0033] The speed comprehensive calculation program receives the forward measurement speed output by the Doppler log carried by the unmanned underwater vehicle Rightward measured speed Upward measurement speed

[0034] (4) Speed ​​error calculation

[0035] The speed comprehensive calculation program calculates the error between the measured speed and the theoretical speed

[0036]

[0037] Where: err is the error between the measured speed and the theoretical speed; is the forward theoretical velocity output by the hydrodynamic model; is the rightward theoretical velocity output by the hydrodynamic model; is the upward theoretical velocity output by the hydrodynamic model; is the forward speed measured by the Doppler log output; is the rightward measured speed output by the Doppler log; It is the upward speed measured by the Doppler log output.

[0038] (5) Comprehensive speed measurement

[0039] The speed comprehensive calculation program performs comprehensive calculation: if the error err is less than the hydrodynamic model speed error threshold T, the comprehensive calculated speed of the unmanned underwater vehicle is the measured speed output by the Doppler log; otherwise, the comprehensive calculated speed of the unmanned underwater vehicle is determined to be the theoretical speed output by the hydrodynamic model.

[0040]

[0041] Where: The forward integrated estimated speed of the unmanned underwater vehicle; is the rightward integrated speed of the unmanned underwater vehicle; The upward integrated speed of the unmanned underwater vehicle; is the forward theoretical velocity output by the hydrodynamic model; is the rightward theoretical velocity output by the hydrodynamic model; is the upward theoretical velocity output by the hydrodynamic model; is the forward speed measured by the Doppler log output; is the rightward measured speed output by the Doppler log; It is the upward speed measured by the Doppler log output.

[0042] The speed measured by the Doppler log and the speed calculated by the present invention were respectively substituted into the dead reckoning navigation program of the unmanned underwater vehicle to compare the navigation accuracy. The dead reckoning method is as follows:

[0043] Receive the heading angle yaw, pitch angle pitch, and roll angle roll output by the attitude sensor of the unmanned underwater vehicle. The heading angle is 0° with due north, and clockwise is positive; the pitch angle is positive with the bow raised and negative with the bow lowered; the roll angle is positive clockwise when viewed from the stern to the bow. In short, sin(yaw) = sy, cos(yaw) = cy, sin(pitch) = sp, cos(pitch) = cp, sin(roll) = sr, cos(roll) = cr. Receive the current forward velocity v forward , right speed v right , upward velocity v up , calculate the eastward speed v of the unmanned underwater vehicle E and northward velocity v N

[0044] v E =(cr*cy-sp*sr*sy)*v right +(-cp*sy)v forward +(cy*sr+cr*sp*sy)*v up

[0045] v N =(cr*sy+cy*sp*sr)*v right +(cp*cy)*v forward +(sr*sy-cr*cy*sp)*v up

[0046] Where: v E is the eastward speed of the UUV; v N is the northbound speed of the UUV.

[0047] The latitude and longitude update equations of the unmanned underwater vehicle are as follows:

[0048]

[0049] Where: lon i-1 is the longitude at the previous moment, lon i is the current longitude, lat i-1 is the latitude of the previous moment, lat i is the current latitude, is the radius of principal curvature of the Earth's circumplex; is the principal radius of curvature of the Earth's meridian. e is the equatorial radius of the Earth, e is the elliptical eccentricity of the Earth, and using the WGS84 coordinate system, then R e =6378137m,e 2 =0.00669437999014132. Δt is the time difference between the current moment and the previous moment.

[0050] The latitude and longitude of the UUV's entry point are 109.770491E, 18.111848N; the actual latitude and longitude of its exit point are 109.779706E, 18.108086N. Substituting the Doppler log's speed into the navigation program yields the UUV's exit point latitude and longitude as 109.772812E, 18.113713N, with a navigation error of 960.4m.

[0051] Substituting the comprehensive speed calculation method into the dead reckoning navigation program, the latitude and longitude of the UUV's water exit point are obtained as 109.781809E, 18.108913N, with a navigation error of 240.1m. These results demonstrate that the method can effectively avoid interference from abnormal speed information from the Doppler speed log, improving the accuracy and environmental adaptability of the UUV's speed measurement and navigation.

[0052] It can be seen that the process of the present invention is simple and easy to implement, and it effectively avoids the interference of abnormal speed information of the Doppler log, thereby improving the accuracy and environmental adaptability of the speed measurement and navigation of the unmanned underwater vehicle.

[0053] The above description is only for the preferred embodiment of the present invention, which should not be understood as limiting the claims. Any equivalent process changes made using the present invention description are included in the patent protection scope of the present invention.

Claims

1. A method for calculating the speed of an unmanned underwater vehicle based on hydrodynamic model constraints, characterized by: The following steps are included: Step 1: Error threshold setting: according to the water velocity of the UUV operating waters, the speed error threshold T of the hydrodynamic model is set; Step 2: Theoretical speed calculation: Based on the hydrodynamic model, the theoretical speed of the UUV is calculated using propeller speed and rudder angle information: v Model =F(n,angles) Where: v Model is the theoretical speed output by the hydrodynamic model; F(·) is the hydrodynamic model, whose input is the propeller speed and rudder angle, and the output is the theoretical speed; n is the propeller speed of the unmanned underwater vehicle; angles is the rudder angle of the unmanned underwater vehicle; Step 3: Receive the speed measurement, receive the speed measurement output by the Doppler log on the unmanned underwater vehicle DVL ; Step 4: Speed ​​error calculation, calculate the error between the measured speed and the theoretical speed err=E(v Model ,v DVL ) Where: err is the error between the measured speed and the theoretical speed, E(·) is the error calculation function, whose input is the theoretical speed and the measured speed, and the output is the error; v Model is the theoretical velocity output by the hydrodynamic model; v DVL is the measured speed output by the Doppler log; Step 5: Comprehensive speed calculation: If the error err is less than the speed error threshold T of the hydrodynamic model, the comprehensive speed of the unmanned underwater vehicle is the measured speed output by the Doppler log; otherwise, the comprehensive speed of the unmanned underwater vehicle is determined to be the theoretical speed output by the hydrodynamic model. Where: v UUV is the comprehensive calculated speed of the unmanned underwater vehicle; v DVL is the measured velocity output by the Doppler log; v Model is the theoretical velocity output by the hydrodynamic model.

Images