Unmanned underwater vehicle speed measuring and calculating method based on hydrodynamic model constraint

Abstract

The invention relates to an unmanned underwater vehicle speed measuring and calculating method based on hydrodynamic model constraint, and the method comprises the following steps: 1, setting an error threshold value: setting a speed error threshold value T of a hydrodynamic model according to the water flow speed of an operation water area of an unmanned underwater vehicle; and 2, theoretical speed calculation: based on the hydrodynamic model, calculating the theoretical speed of the unmanned underwater vehicle by using the rotating speed of a propeller and rudder angle information. According to the invention, the Doppler log can be prevented from outputting abnormal speed information to interfere with navigation and operation of the unmanned underwater vehicle, and the speed measurement and navigation precision and environmental adaptability of the unmanned underwater vehicle are improved.

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 submersibles can operate autonomously underwater without manual control, and play an important role in the exploration and development of marine resources, military applications and other fields. They have been vigorously developed by many countries in the world. Since unmanned submersibles cannot receive positioning information from the global navigation satellite system when navigating underwater, how to achieve high-precision underwater navigation has become a key issue in the development of unmanned submersibles. At present, unmanned submersibles often use a navigation system composed of an inertial navigation system + a Doppler VelocityLog (DVL). This navigation system relies on the speed information output by the Doppler VelocityLog to complete the navigation status update. However, as an acoustic device, the Doppler VelocityLog is easily interfered by external acoustic signals or its own multiple echo signals, which may produce abnormal speed output, resulting in serious deviations in the navigation of the unmanned submersible. 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 submersible 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 submersible, and improving the accuracy and environmental adaptability of the speed measurement and navigation of the unmanned submersible.

[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 flow velocity of the unmanned underwater vehicle 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 unmanned underwater vehicle is calculated using the 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: Measure the speed reception, receive the measured speed v output by the Doppler speed meter carried by 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 estimated speed of the unmanned underwater vehicle; v DVL is the measured speed 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 which has a simple process and is easy to implement, effectively avoiding the interference of abnormal speed information of a Doppler odometer, and improving the accuracy and environmental adaptability of the speed calculation 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 Flow chart 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 present invention provides a navigation track comparison between the speed measured by a Doppler odometer and the speed comprehensively calculated by 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 unmanned submersible is equipped with a 600kHz phased array Doppler speed meter, a propeller speed sensor, a rudder angle sensor, and a main control board. The Doppler speed meter communicates with the main control board through the UDP network protocol, and the propeller speed sensor and the rudder angle sensor communicate with the main control board through CAN. The hydrodynamic model program and the speed comprehensive calculation program run in the main control board. The hydrodynamic model program receives the propeller speed and rudder angle information and outputs the theoretical speed. The speed comprehensive calculation program receives the hydrodynamic model speed error threshold, the Doppler speed meter measured speed, and the hydrodynamic model theoretical speed to complete the speed comprehensive 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 right 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 measurement 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 right theoretical velocity output by the hydrodynamic model; is the upward theoretical velocity output by the hydrodynamic model; is the forward measured speed output by the Doppler log; 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 calculation

[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; The right-hand integrated speed of the unmanned underwater vehicle; It is the comprehensive upward speed of the unmanned underwater vehicle; is the forward theoretical velocity output by the hydrodynamic model; is the right theoretical velocity output by the hydrodynamic model; is the upward theoretical velocity output by the hydrodynamic model; is the forward measured speed output by the Doppler log; 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 are 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 is positive clockwise; the pitch angle is positive when the bow is raised and negative when the bow is lowered; the roll angle is positive clockwise when observed 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 currently used forward speed 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 unmanned underwater vehicle; 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 of 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 yoke; 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 entry point of the unmanned submersible are 109.770491E, 18.111848N; the latitude and longitude of the actual exit point are 109.779706E, 18.108086N. Substituting the speed measured by the Doppler log into the navigation program, the latitude and longitude of the exit point of the unmanned submersible are 109.772812E, 18.113713N, and the navigation error is 960.4m.

[0051] Substituting the comprehensive speed calculation of the present invention into the dead reckoning navigation program, the longitude and latitude of the unmanned submersible's water exit point are obtained to be 109.781809E, 18.108913N, and the navigation error is 240.1m. The results show that the present invention can effectively avoid the interference of abnormal speed information of the Doppler speed log, and improve the accuracy and environmental adaptability of the unmanned submersible's speed calculation and navigation.

[0052] It can be seen that the process of the present invention is simple and easy to implement, and it can effectively avoid 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 is only an explanation of the preferred embodiments of the present invention, which should not be construed as a limitation on the claims. Any equivalent process changes made using the present invention specification 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 in that: The following steps are included: Step 1: Error threshold setting: according to the water flow velocity of the unmanned underwater vehicle 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 unmanned underwater vehicle is calculated using the 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: Measure the speed reception, receive the measured speed v output by the Doppler log carried by 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 estimated speed of the unmanned underwater vehicle; v DVL is the measured speed output by the Doppler log; v Model is the theoretical velocity output by the hydrodynamic model.

Images