A method for underwater dead reckoning based on polarization / DVL considering ocean current interference

Abstract

The application discloses a kind of underwater track extrapolation method based on polarization / DVL considering ocean current interference, belong to underwater polarization autonomous integrated navigation positioning field.The method includes: combining DVL speed information and gyroscope output angular velocity information, design underwater unmanned system position and attitude recursive updating algorithm;Considering the influence of external environment interference and ocean current velocity on DVL speed measurement, establish DVL speed error equation containing scale factor error and ocean current interference;For the cumulative error caused by long time navigation, establish attitude correction model with refraction polarization error;Design state vector containing scale factor, ocean current velocity and polarization error, derive track extrapolation system dynamic model for multi-sensor information fusion, complete underwater track extrapolation navigation based on polarization / DVL.The application has the advantages of strong autonomy, high reliability, strong engineering practicability, etc., and can be used for unfamiliar rejection ocean environment underwater unmanned system autonomous navigation.

Description

Technical Field

[0001] This invention belongs to the field of underwater polarization autonomous integrated navigation and positioning, specifically involving an underwater track estimation method based on polarization / DVL that takes into account ocean current interference. Background Technology

[0002] In recent years, the strategic importance of the marine economy and maritime battlefield has been increasing, and underwater unmanned systems (UUVs), as a major carrier for marine development and utilization, have received increasing attention. Navigation and positioning are the core and key technologies for the development of underwater UUVs. Unlike the terrestrial environment, underwater radio signals are naturally blocked, information is scarce, and there are no specific landmarks, posing a severe challenge to the reliability and trustworthiness of underwater UUV navigation and positioning. Polarization navigation is an autonomous navigation technology that learns, understands, simulates, and realizes the perception capabilities of biological navigation / environment / targets. It features fast response, passive autonomy, and non-accumulating errors, making it a research hotspot in the field of autonomous navigation on land / air / underwater. DVL (Depth-Velocity Measurement) is an instrument that performs high-precision velocity measurement based on the Doppler effect. Its advantage lies in the fact that velocity errors do not accumulate over time, making it suitable for high-precision acquisition of velocity information for underwater UUVs. By organically combining polarization navigation and DVL, an efficient and practical method for underwater UUV trajectory estimation navigation and positioning can be developed, effectively improving the reliability and trustworthiness of underwater UUV navigation and positioning.

[0003] The published paper "A Method for Track Estimation of Small Underwater Unmanned Vehicles" is based on the vehicle's kinematic model and a special fusion of navigation information data. However, obtaining the vehicle model parameters is difficult, and the method cannot suppress the divergence of accumulated errors. Existing track estimation navigation and positioning methods, such as "An Underwater Navigation and Positioning Method Based on Acoustic Positioning / Dead Estimation," improve the underwater navigation and positioning accuracy and the reliability of the navigation system when acoustic system data is abnormal by combining dead estimation information based on compass and DVL with an acoustic positioning system. However, its application requires the pre-deployment of transponder devices and the transmission and reception of signals. Chinese invention patent ZL202110111390.X (An Integrated Acoustic Positioning and Tracking Control Method for Autonomous Underwater Vehicles) addresses the uncertainties in acoustic positioning and environmental interference such as ocean currents by proposing a system state estimation algorithm based on extended Kalman filtering. This solves the problems of difficult positioning and susceptibility to ocean currents in the underwater environment for AUVs. However, this method requires vehicle model information, and parameter acquisition is difficult.

[0004] However, although the above methods achieve underwater navigation and positioning or integrated navigation based on track calculation, they often rely on the dynamic model of the inertial navigation system or ignore the influence of ocean currents and other disturbances on the attitude and position of track calculation. Furthermore, they rely on navigation means that need to be preset (such as underwater acoustic systems) to achieve navigation and positioning, which cannot be applied to unfamiliar sea environments. The stability and reliability of navigation in actual marine environments are difficult to guarantee. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides an underwater trajectory estimation method based on polarization / DVL, considering ocean current interference, and realizes underwater autonomous integrated navigation and positioning based on polarization / DVL. Addressing the issue of limited marine environmental information sources and difficulty in positioning, this invention combines DVL velocity information with gyroscope output angular velocity information to design a recursive update algorithm for the position and attitude of the underwater unmanned system. Considering the influence of external environmental interference and ocean current velocity on DVL velocity measurement, a DVL velocity error equation incorporating scale factor error and ocean current information is established. To address the cumulative errors caused by long-term navigation, an attitude correction model with refraction and polarization errors is established. A state vector incorporating scale factor, ocean current velocity, and polarization error is designed, and a dynamic model of the integrated navigation system is derived for multi-sensor information fusion.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for underwater track estimation based on polarization / DVL, considering ocean current interference, includes:

[0008] Step (1): Obtain the current Time-based DVL measurement of velocity and gyroscope output angular velocity In combination with the current situation Latitude of Underwater Unmanned System ,longitude , calculation Real-time attitude and position of the underwater unmanned system;

[0009] Step (2): Considering external environmental disturbances and the influence of ocean current velocity, establish an error calculation method containing the DVL scale factor. and ocean current speed The DVL velocity error equation;

[0010] Step (3): Considering the cumulative position error caused by heading drift in the trajectory calculation, the underwater polarized light field vector is obtained using a polarization sensing system. Deriving the underwater polarized light field vector With attitude misalignment angle error The relationship between them is established, including refraction interference error. Underwater polarization measurement model;

[0011] Step (4): Construct a system that considers DVL scale factor error Ocean current speed and refraction interference error state vector ,in Indicates the error in latitude, longitude, and altitude. Indicates 3D gyroscope drift, superscript The transpose of the matrix is ​​used to establish a dynamic model of the navigation system based on the selected state vector, thereby completing the information fusion of the trajectory estimation system.

[0012] The beneficial effects of this invention are as follows:

[0013] (1) In response to the influence of external environmental interference and ocean current speed, this invention establishes the DVL velocity error equation of DVL scale factor error and ocean current speed, and directly uses DVL velocity in the attitude and position update process, which can suppress the velocity accumulation error in inertial navigation strapdown and effectively improve the accuracy of trajectory estimation attitude and position calculation.

[0014] (2) To address the cumulative error in heading during trajectory estimation, an underwater polarization measurement equation containing refraction interference error was established. Subsequently, the DVL scale factor error, ocean current velocity, and refraction interference error were extended into the state vector, thereby improving the accuracy and robustness of information fusion. Attached Figure Description

[0015] Figure 1 This is a flowchart of an underwater trajectory estimation method based on polarization / DVL that takes into account ocean current interference, according to the present invention.

[0016] Figure 2 This is a simulation example of the position tracking result of an underwater trajectory estimation method based on polarization / DVL that takes into account ocean current interference, according to the present invention.

[0017] Figure 3 This is a simulation example of the heading estimation result of an underwater trajectory estimation method based on polarization / DVL that takes into account ocean current interference, according to the present invention. Detailed Implementation

[0018] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0019] like Figure 1 As shown, the present invention provides a method for underwater trajectory estimation based on polarization / DVL, considering ocean current interference, comprising the following steps:

[0020] Step (1): Obtain the current Time-based DVL measurement of velocity and gyroscope output angular velocity In combination with the current situation Latitude of Underwater Unmanned System ,longitude , calculation Real-time attitude and position of the underwater unmanned system;

[0021] Step (2): Considering external environmental disturbances and the influence of ocean current velocity, establish an error calculation method containing the DVL scale factor. and ocean current speed The DVL velocity error equation;

[0022] Step (3): Considering the cumulative position error caused by heading drift in the trajectory calculation, the underwater polarized light field vector is obtained using a polarization sensing system. Deriving the underwater polarized light field vector With attitude misalignment angle error The relationship between them is established, including refraction interference error. Underwater polarization measurement model;

[0023] Step (4): Construct a system that considers DVL scale factor error Ocean current speed and refraction interference error state vector ,in Indicates the error in latitude, longitude, and altitude. Indicates 3D gyroscope drift, superscript The transpose of the matrix is ​​used to establish a dynamic model of the navigation system based on the selected state vector, thereby completing the information fusion of the trajectory estimation system.

[0024] Specifically, step (1) includes:

[0025] The navigation system is defined as the northeast-central coordinate system (n-system); the upper right-front coordinate system is defined as the carrier system (b-system); the inertial coordinate system is defined as the i-system, with its Z-axis parallel to the Earth's rotation axis and its X-axis pointing to the mean vernal equinox, and its Y-axis determined by the right-hand rule; the geocentric-earth-fixed system is defined as the e-system, with its Z-axis parallel to the Earth's rotation axis and its X-axis pointing to the intersection of the 0° meridian and the equator, and its Y-axis determined by the right-hand rule. Based on the working principle of DVL velocity measurement and coordinate transformation relationships, and based on the current... DVL measurement of velocity in time b frame Get the current The DVL velocity at time n is:

[0026] ;

[0027] in, This represents the coordinate transformation matrix from the b-system to the n-system.

[0028] Known current Latitude of Underwater Unmanned System and longitude Based on the principle of trajectory position estimation, the following is calculated: Time and location information:

[0029] ;

[0030] in, and They represent Latitude and longitude of the underwater unmanned system at all times; , and They represent The x, y, and z-axis components of the DVL velocity at time n in the frame; and These represent the radii of curvature of the Earth's meridian and circumference, respectively. express Time's up Time interval;

[0031] According to the quaternion update algorithm, the differential equation for attitude update under quaternions is:

[0032] ;

[0033] in, Represents attitude quaternions, Let represent the angular velocity of the b-system relative to the n-system, and we have:

[0034] ;

[0035] In the formula, Let be the rotational angular velocity of system b relative to system i in system b; Let be the angular velocity of the e-system relative to the i-system in the n-system. This represents the angular velocity of n relative to e in the n-frame. Specifically, Calculation is key to addressing attitude error divergence; therefore, the calculation of rotational angular velocity is crucial. Utilizing DVL speed and current Latitude of Underwater Unmanned System Then, the rotational angular velocity based on the DVL velocity is:

[0036] ;

[0037] Then, the pose is updated based on the pose quaternion. and Indicates DVL speed Components on the y-axis and x-axis.

[0038] Specifically, step (2) includes:

[0039] In actual measurements, changes in the external environment (such as water temperature and salinity) can introduce deviations in DVL measurements. We assume that the resulting error is attributable to calibration factor error. This indicates that, on the other hand, considering that DVL may be unable to conduct terrestrial observations and can only conduct convection observations when encountering deep waters such as trenches, ocean current velocity will affect the measurement of actual velocity information. Assuming the ocean current is irrotational, the ocean current velocity under the carrying system is... .

[0040] Combined with scale factor error and ocean current speed The actual speed of DVL under the b series is:

[0041] ;

[0042] in, This indicates the actual speed of the b-series DVL. This represents the theoretical velocity of DVL in the b-system.

[0043] Considering attitude misalignment angle error n-series DVL actual speed for:

[0044] ;

[0045] in, Represents the identity matrix. Indicates an antisymmetric matrix operator, ignoring information about... , and For higher-order small quantities, the actual speed of DVL is:

[0046] .

[0047] Contains DVL scale factor error and ocean current speed The DVL velocity error equation is expressed as:

[0048] ;

[0049] in, This represents the theoretical velocity of DVL in the n-system.

[0050] Specifically, step (3) includes:

[0051] Heading is a key factor affecting trajectory estimation performance. Considering the cumulative position error caused by heading drift during trajectory estimation, a polarization sensing system is used to obtain underwater polarized light field information. The underwater polarized light field vector in the b-system is calculated by solving the polarization analytical equation. ,Right now ,in, This represents the analytical equation for polarization.

[0052] Under ideal Rayleigh scattering, the solar vector and the observed polarized light field vector in the same coordinate system satisfy the following spatial relationship. :

[0053] ;

[0054] in, Indicate the solar vector in the navigation system; assume the refraction interference error caused by underwater refraction. satisfy And there are The actual underwater polarized light field vector is... Actual spatial relationships for:

[0055] ;

[0056] Considering attitude misalignment angle error underwater polarized light field vector With attitude misalignment angle error The relationship between them is:

[0057] .

[0058] Thus, an error term containing refraction interference was established. Underwater polarization measurement model:

[0059] .

[0060] Specifically, step (4) includes:

[0061] Combining the DVL velocity error equation in step (2) And the underwater polarization measurement model in step (3) Construct DVL scale factor error Ocean current speed and refraction interference error State vector:

[0062] ;

[0063] The position update equation is calculated based on the trajectory in step (1), ignoring the denominator. and For second-order and higher-order terms, the position error equation in trajectory calculation simplifies to:

[0064] ;

[0065] in, , and They represent The x, y, and z axis components; , and These represent the latitude, longitude, and altitude errors, respectively, and the superscript (·) indicates the first-order time derivative.

[0066] Combining the DVL velocity error equation The position dynamic equation of the trajectory estimation system is:

[0067] ;

[0068] in, and The parameter matrix is ​​represented; the attitude dynamic equation of the trajectory estimation system is expressed as:

[0069] ;

[0070] in, Indicates the platform command angular velocity; and This represents the parameter matrix.

[0071] In summary, the system dynamic equations in trajectory estimation are as follows:

[0072] ;

[0073] in, Represents the state transition matrix. For system process noise, Representing the state vector, combined with the underwater polarization measurement model By selecting a suitable linear Kalman filter, system information fusion can be completed to achieve autonomous positioning based on polarization / DVL trajectory extrapolation in unfamiliar marine environments, taking into account ocean current velocity.

[0074] Example

[0075] This example uses a polarization- and DVL-based trajectory estimation system to simulate and verify the feasibility and effectiveness of the proposed method. The simulation parameter settings are shown in Table 1, and these parameters are limited to a specific simulation environment for a polarization- and DVL-based trajectory estimation system.

[0076] Table 1

[0077]

[0078] This simulation verifies the position tracking and heading estimation performance of the invented method. The position comparison and heading estimation results are as follows: Figure 2 and Figure 3As shown in the figure. Clearly, the method of this invention can track the changing trends of navigation information in real time for both heading estimation and position tracking, proving the feasibility and effectiveness of the method in heading and position acquisition. Statistics show that the entire simulation experiment covered a distance of approximately 13.75 km, and the root mean square error of the position using the method of this invention was 0.40 km, accounting for 2.91% of the distance. Regarding the heading angle, the average error, standard deviation, and root mean square error of the heading angle using the method of this invention can reach 0.47°, 0.76°, and 0.90°, respectively. The results indicate that the proposed method can achieve heading estimation and position tracking based on track extrapolation even when both ocean current interference and refraction interference are present.

[0079] On the other hand, the present invention also provides an underwater trajectory estimation system based on polarization / DVL that takes into account ocean current interference, the system comprising:

[0080] Calculation unit, used to obtain the current Time-based DVL measurement of velocity and gyroscope output angular velocity In combination with the current situation Latitude of Underwater Unmanned System ,longitude , calculation Real-time attitude and position of the underwater unmanned system;

[0081] Error cells are used to account for external environmental disturbances and the influence of ocean current velocity, establishing an error containing DVL scale factors. and ocean current speed The DVL velocity error equation;

[0082] The modeling unit is used to account for the cumulative position error caused by heading drift in trajectory estimation, and uses a polarization sensing system to obtain the underwater polarized light field vector. Deriving the underwater polarized light field vector With attitude misalignment angle error The relationship between them is established, including refraction interference error. Underwater polarization measurement model;

[0083] Fusion unit, used to construct a system that takes into account DVL scale factor error. Ocean current speed and refraction interference error state vector ,in Indicates the error in latitude, longitude, and altitude. Indicates 3D gyroscope drift, superscript The transpose of the matrix is ​​used to establish a dynamic model of the navigation system based on the selected state vector, thereby completing the information fusion of the trajectory estimation system.

[0084] Thirdly, the present invention provides an electronic device, comprising: one or more processors; and a memory for storing one or more programs; wherein, when the one or more programs are executed by the one or more processors, the one or more processors implement the aforementioned method for calculating underwater tracks based on polarization / DVL considering ocean current interference.

[0085] Fourthly, the present invention provides a computer-readable storage medium having executable instructions stored thereon, which, when executed by a processor, enable the processor to implement the aforementioned method for underwater track estimation based on polarization / DVL, taking into account ocean current interference.

[0086] 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 descriptions are merely specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for underwater track estimation based on polarization / DVL considering ocean current interference, characterized in that, Includes the following steps: Step (1): Obtain the current Time-based DVL measurement of velocity and gyroscope output angular velocity In combination with the current situation Latitude of Underwater Unmanned System and longitude , calculation Real-time attitude and position of the underwater unmanned system; Step (2): Considering external environmental disturbances and the influence of ocean current velocity, establish an error calculation method containing the DVL scale factor. and ocean current speed The DVL velocity error equation; Step (3): Considering the cumulative position error caused by heading drift in the trajectory calculation, the underwater polarized light field vector is obtained using a polarization sensing system. Deriving the underwater polarized light field vector With attitude misalignment angle error The relationship between them is established, including refraction interference error. Underwater polarization measurement model; Step (4): Construct a system that considers DVL scale factor error Ocean current speed and refraction interference error state vector ,in Indicates the error in latitude, longitude, and altitude. Indicates 3D gyroscope drift, superscript The transpose of the matrix is ​​used to establish a dynamic model of the navigation system based on the selected state vector, thereby completing the information fusion of the trajectory estimation system.

2. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 1, characterized in that, Step (1) includes taking the northeast-sky coordinate system as the navigation system, i.e., the n system; the right-front-upper coordinate system as the carrier system, i.e., the b system; the inertial coordinate system as the i system, whose Z-axis is parallel to the Earth's rotation axis and whose X-axis points to the average vernal equinox, and whose Y-axis is determined by the right-hand rule; and the geocentric-earth-fixed system as the e system, whose Z-axis is parallel to the Earth's rotation axis and whose X-axis points to the intersection of the 0° meridian and the equator, and whose Y-axis is determined by the right-hand rule. Based on the working principle of DVL velocity measurement and coordinate transformation relationship, and based on the current... DVL measurement of velocity in time b frame Get the current The velocity measured by DVL at time n is: ； in, This represents the coordinate transformation matrix from the b-system to the n-system. Known current Latitude of Underwater Unmanned System and longitude Based on the principle of trajectory position estimation, the following is calculated: Horizontal position information at any time: ； in, and They represent Latitude and longitude of the underwater unmanned system at all times; , and They represent The x, y, and z-axis components of the DVL velocity at time n in the frame; and These represent the radii of curvature of the Earth's meridian and circumference, respectively. express Time's up Time interval.

3. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 2, characterized in that, Step (1) further includes using an attitude quaternion update algorithm, wherein the quaternion differential equation is: ； in, Represents attitude quaternions, Representing attitude quaternions The first derivative, Let represent the angular velocity of the b-system relative to the n-system, and we have: ； In the formula, Let be the rotational angular velocity of system b relative to system i in system b; Let be the angular velocity of the e-system relative to the i-system in the n-system. Represent the angular velocity of n relative to e in the n-frame system, and calculate the angular velocity of n. Utilizing DVL speed and current Latitude of Underwater Unmanned System Then, the rotational angular velocity based on the DVL velocity is: ； Then, the pose is updated based on the pose quaternion. and Indicates DVL speed Components on the y-axis and x-axis.

4. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 1, characterized in that, In step (2), the DVL scale factor error is set. The ocean currents are irrotational, and the velocity of the lower ocean currents in the b series is... ; Combined with scale factor error and ocean current speed The actual speed of DVL under the b series is: ； in, This indicates the actual speed of the b-series DVL. This represents the theoretical velocity of DVL in the b-series. Considering attitude misalignment angle error n-series DVL actual speed for: ； in, Represents the identity matrix. Indicates an antisymmetric matrix operator, ignoring information about... , and For higher-order small quantities, the actual speed of n-series DVL can be rewritten as: 。 5. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 4, characterized in that, Step (2) includes DVL scale factor error. and ocean current speed The DVL velocity error equation is expressed as: ； in, This represents the theoretical velocity of DVL in the n-system.

6. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference according to claim 1, characterized in that, Step (3) includes acquiring underwater polarized light field information using a polarization sensing system. The underwater polarized light field vector in the b-system is calculated by solving the polarization analytical equation. ,Right now ,in, This represents the analytical equation for polarization.

7. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 6, characterized in that, Step (3) further includes, under ideal Rayleigh scattering, the solar vector and the observed polarized light field vector in the same coordinate system satisfy the following spatial relationship. : ； in, Indicate the solar vector in the navigation system; assume the refraction interference error caused by underwater refraction. satisfy And there are The actual underwater polarized light field vector is... Actual spatial relationships for: ； Considering attitude misalignment angle error underwater polarized light field vector With attitude misalignment angle error The relationship between them is: 。 8. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference according to claim 7, characterized in that, Step (3) also includes establishing a system containing refraction interference error. The underwater polarization measurement model is as follows: 。 9. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference as described in claim 1, characterized in that, Step (4) includes: Ignore the relevant terms in the denominator of the position update equation in step (1). and For second-order and higher-order terms, the equation simplifies to: ； in, , and They represent The x, y, and z axis components; , and These represent latitude and longitude errors, respectively, and the superscript (·) indicates the first time derivative; Combining the DVL velocity error equation The position dynamic equation of the trajectory estimation system is: ； in, and The parameter matrix is ​​represented; the attitude dynamic equation of the trajectory estimation system is expressed as: ； in, Indicates the platform command angular velocity; and This represents the parameter matrix.

10. The underwater trajectory estimation method based on polarization / DVL considering ocean current interference according to claim 9, characterized in that, Step (4) further includes the following: the system dynamic equation is: ； in, Represents the state transition matrix. For system process noise, Let represent the state vector, and we have: ； Combined with underwater polarization measurement model By selecting a suitable linear Kalman filter, system information fusion can be completed to achieve autonomous positioning based on polarization / DVL trajectory extrapolation in unfamiliar marine environments, taking into account ocean current velocity.

Images