CN116839589A - Method and system for estimating INS/DVL ocean currents of underwater unmanned aircraft in middle water area - Google Patents

Abstract

The underwater unmanned vehicle INS/DVL ocean current estimation method and system in mid-water areas belongs to the field of underwater unmanned vehicle navigation and positioning technology. In order to solve the problem that the current estimation method cannot obtain the current velocity when the underwater unmanned vehicle lacks accurate navigation information in the middle waters and the water depth is greater than the DVL bottoming depth. This invention records DVL and INS data when the underwater unmanned vehicle performs planar rotational motion, and calculates the motion trajectory position of the underwater unmanned vehicle without the influence of sea currents and the motion trajectory position of the underwater unmanned vehicle under the influence of sea currents; and then Estimate the center of the navigation trajectory without the influence of ocean currents, adjust the coordinate system according to the deviation between the ocean current direction and the direction of the coordinate axis, and then fit the straight line at the center of the circle. Through the change of the vertical foot point position, determine the quadrant of the ocean current vector and adjust it, and finally calculate each Calculate the distance between adjacent vertical foot points, find the mean value of the distance, and divide it by the time interval between adjacent circle centers when estimating the circle center to obtain the ocean current velocity.

Description

Underwater unmanned vehicle INS/DVL ocean current estimation method and system in mid-water areas

Technical field

The invention belongs to the technical field of underwater unmanned vehicle navigation and positioning, and relates to an underwater unmanned vehicle INS/DVL ocean current estimation method and system in mid-water areas.

Background technique

Navigation and positioning technology provides accurate position, speed and attitude information for underwater unmanned vehicles, which is the key to determining whether underwater unmanned vehicles can accurately reach the predetermined location, successfully complete the mission and return safely. Radio waves are severely attenuated in seawater, and the Global Navigation Satellite System (GNSS) is not suitable for underwater navigation. Inertial navigation systems (INS) can provide complete navigation parameters without relying on external information. However, INS errors will accumulate over time, so additional sensors need to be introduced to correct the errors during long-term voyages. The Doppler Velocimeter (DVL) emits sound waves to the seafloor to measure the reference velocity of the carrier based on the Doppler effect. Inertial navigation/Doppler (INS/DVL) integrated navigation is the most commonly used method in underwater navigation. Integrated navigation is achieved by fusing the data collected by INS with the relative speed collected by DVL. However, when the water depth is greater than the bottoming depth of DVL, what DVL obtains is the reference velocity of the carrier relative to the ocean current. Direct use of DVL data without compensating ocean current speed will cause INS/DVL navigation performance to degrade or even fail. Therefore, when underwater unmanned vehicles perform tasks in mid-water areas, it is necessary to estimate the speed of ocean currents.

The Chinese patent with the publication number CN112729291A disclosed a method for estimating the ocean current speed of the deep-diving long-range submersible INS/DVL based on specific trajectory motion on April 30, 2021. This method relies on the carrier to move along a specific trajectory. However, in the absence of precise navigation information, it is difficult for the carrier to resist the action of ocean currents through closed-loop control to complete the specific trajectory movement.

In summary, there is an urgent need for a method that can estimate ocean current parameters when the underwater unmanned vehicle only has INS/DVL navigation information and cannot resist the action of ocean currents through closed-loop control. At the same time, it is necessary to ensure that the method has good Accuracy.

Contents of the invention

The purpose of this invention is to solve the problem that the current ocean current estimation method cannot obtain the ocean current velocity when the underwater unmanned vehicle lacks accurate navigation information in mid-water areas and the water depth is greater than the DVL bottom sounding depth.

An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas includes the following steps:

S1: Initial alignment of the inertial navigation system;

S2: Control the underwater unmanned vehicle to perform planar rotation and record the DVL and INS data during the movement;

Determining the convection velocity during the motion of underwater unmanned vehicle based on DVL data Convert the convection speed to the navigation coordinate system, and calculate the motion trajectory position P _dvl of the underwater unmanned vehicle without the influence of ocean currents based on the convection speed in the navigation coordinate system;

At the same time, the speed V _INS of the underwater unmanned vehicle in the navigation coordinate system is calculated based on the INS, and the motion trajectory position P _INS of the underwater unmanned vehicle under the influence of ocean currents is calculated;

S3: From the underwater unmanned vehicle navigation trajectory obtained by DVL, select three trajectory points and estimate the center of the navigation trajectory without the influence of sea currents;

At the same time, in the underwater unmanned vehicle navigation trajectory obtained by the INS, trajectory points are collected slidingly at the time interval T _g , and the collected trajectory points are grouped so that each group has three trajectory points, and the first trajectory in each group The point is the last trajectory point in the previous group, and the remaining two trajectory points are the newly taken points in this group; the center of the circle where each group of points is located is estimated, that is, the center of the circle sliding under the influence of ocean currents;

A total of N center coordinates c _i (x _i ,y _i ) are obtained, i=1,2...,N; where i=1,2...,N-1 is the center of the circle for the navigation trajectory obtained by the INS;

S4: Calculate the variance σ _x ² in the horizontal axis direction and the variance σ _y ² in the vertical axis direction of the coordinates of the N center centers;

If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis;

If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis;

If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discrete degree of the circle center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis;

where σ _min ² is the variance threshold;

If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system, and calculate the coordinate value of the center of the circle in the new coordinate system after rotation.

If the direction of the ocean current deviates from the direction of the coordinate axis, keep the circle center coordinate c _i (x _i , y _i ) unchanged and obtain the required circle center coordinate

S5: Perform straight line fitting on the center of the circle obtained in step S4, and obtain the straight line equation y=kx+b for the center of the circle, where k is the slope of the straight line and b is the intercept;

S6: Determine the vertical foot point of the fitted straight line from the center of the circle sliding with the ocean current to the center of the circle through geometric relationships.

S7: order When m>0, the current direction angle θ=90°-arctan(k); when m<0, the current direction angle θ=270°-arctan(k);

Then, the rotation angle is compensated according to whether the counterclockwise rotation is performed in step S4. If the circle center coordinate is counterclockwise rotated in step S4, the rotation angle compensation θ=θ-45° is performed for the sea current direction angle θ=θ-45°; if after rotation compensation If the ocean current direction angle θ <0°, then let θ = θ + 360°;

S8: Calculate the distance between adjacent vertical foot points and find the mean value of the distance Convert the mean of the distance/> Divide by the time interval T _d between adjacent circle centers when estimating the circle center to obtain the ocean current velocity.

Further, the time interval T _g in S3 takes a value between T _c /6 and T _c /4, where T _c is the circular motion period of the underwater unmanned vehicle.

Further, in the process of solving the motion trajectory of the underwater unmanned vehicle without the influence of ocean currents in S2, the convection velocity V _n ^dvl in the navigation coordinate system is integrated to obtain the motion trajectory of the underwater unmanned vehicle without the influence of ocean currents.

Furthermore, in S4, if σ _x ² ≤ _{σ min} ² and σ _y ² ≤ _{σ min} ² , the estimated center coordinates are similar, and the ocean current has no effect on the motion of the underwater unmanned vehicle. The ocean current velocity is considered to be 0, so Then exit the current estimation.

Furthermore, the coordinate value of the center of the circle in the new coordinate system after rotation Determine it as follows:

Among them, R is the rotation matrix that rotates the coordinate system counterclockwise by 45°.

Further, in S5, the least squares method is used to perform straight line fitting on the center point of the circle obtained in step S4.

Further, the process of determining the vertical foot point in S6 includes the following steps:

Obtain the center coordinates of the navigation trajectory from INS Obtain the equation of the straight line, where i=1,2...,N-1. According to the geometric relationship, the vertical coordinates from the center of the circle to the straight line can be obtained/> as follows:

Further, the time interval between adjacent circle centers is T _d =2T _g .

Further, calculate the distance between adjacent vertical foot points and find the mean value of the distance. The process includes the following steps:

The vertical coordinates on the fitted straight line from the center of the circle to the center of the circle based on sliding with the ocean current are

calculate The distance d ₁ between two points:

Use the same method to calculate the distance between p ₁ and p ₂ , p ₂ and p ₃ ,..., p _N-2 and p _N-1 , and obtain the distance between vertical feet d _i ,i=1,2...,N-2 ;Mean distance between vertical feet

An underwater unmanned vehicle INS/DVL ocean current estimation system in mid-water areas, including a navigation information collection unit, a geometric calculation unit, and an ocean current estimation unit;

Navigation information collection unit: Determine the convection speed during the movement of the underwater unmanned vehicle based on DVL data Convert the convection speed to the navigation coordinate system, and calculate the motion trajectory position P _dvl of the underwater unmanned vehicle without the influence of ocean currents based on the convection speed in the navigation coordinate system; at the same time, calculate the underwater unmanned navigation in the navigation coordinate system based on the INS vehicle speed V _INS , and solve the motion trajectory position P _INS of the underwater unmanned vehicle under the influence of ocean currents;

Geometric calculation unit: Based on the motion trajectories calculated by DVL and INS when the underwater unmanned vehicle performs planar rotation, the center of the circle is determined through the three-point centering method; then the variance of the horizontal and vertical axis directions of the coordinates of the center of the circle is calculated. , determine the deviation between the direction of the ocean current and the direction of the coordinate axis and adjust the coordinate system accordingly; then fit the processed circle center into a straight line through the least squares method, calculate the vertical foot from the center of the circle sliding with the ocean current to the center of the circle fitting straight line, through the vertical foot point Changes in position determine the quadrant of the current vector;

The process of calculating the variance of the coordinates of the center of the circle in the horizontal axis direction and the vertical axis direction, determining the deviation of the ocean current direction from the coordinate axis direction, and adjusting the coordinate system accordingly includes the following steps:

Calculate the variance σ _x ² in the horizontal axis direction and the variance σ _y ² in the vertical axis direction of the coordinates of the N circle centers;

If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis;

If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis;

If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discrete degree of the circle center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis;

where σ _min ² is the variance threshold;

If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system, and calculate the coordinate value of the center of the circle in the new coordinate system after rotation.

If the direction of the ocean current deviates from the direction of the coordinate axis, keep the circle center coordinate c _i (x _i , y _i ) unchanged and obtain the required circle center coordinate

Calculate the vertical foot of the fitted straight line from the center of the circle to the center of the circle sliding with the ocean current. Through the change of the position of the vertical foot point, the process of determining the quadrant of the current vector includes the following steps:

Remember that the equation of the straight line fitted to the center of the circle is y=kx+b, where k is the slope of the straight line and b is the intercept;

Determine the vertical foot point of the straight line fitting from the center of the circle sliding with the ocean current to the center of the circle through geometric relationships.

make When m>0, the current direction angle θ=90°-arctan(k), when m<0, the current direction angle θ=270°-arctan(k), when m=0, return to modify the variance threshold and restart make judgments;

Then, the rotation angle is compensated according to whether the counterclockwise rotation is performed. If the circle center coordinate is rotated counterclockwise in step S4, the rotation angle compensation θ=θ-45° is performed on the sea current direction angle θ=θ-45°; if the sea current direction angle is corrected after rotation θ<0°, then let θ=θ+360°;

Current estimation unit: Calculate the distance between adjacent vertical foot points and find the mean value of the distance Convert the mean of the distance/> Divide by the time interval T _d between adjacent circle centers when estimating the circle center to obtain the ocean current velocity.

The beneficial effects of the present invention are:

The invention provides a method and system for estimating ocean currents for an underwater unmanned vehicle INS/DVL in mid-water areas. Based on the fixed control rudder angle constraint, this invention uses DVL and INS to calculate the movement trajectory of the underwater unmanned vehicle without and with currents, and obtains the speed and direction of the sea current during the movement of the underwater unmanned vehicle through geometric relationships. This method does not require additional sensor equipment or hydroacoustic communication equipment, and is simple to implement motion constraints. It can effectively obtain ocean current speed when underwater unmanned vehicles in mid-water areas lack precise navigation information.

Description of the drawings

Figure 1 is a flow chart for the implementation of the ocean current estimation method for underwater unmanned vehicles.

Figure 2 is a structural block diagram of the underwater unmanned vehicle current estimation system.

Figure 3 shows the motion trajectory of the underwater unmanned vehicle under conditions of no current and presence of current.

Figure 4 is a graph of the circle center estimation results and the circle center fitting straight line.

Detailed ways

Specific implementation mode one: This implementation mode will be described with reference to Figure 1.

This implementation mode is an underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas, which includes the following steps:

S1: Start the ocean current estimation task, perform the initial alignment of the inertial navigation system, or INS, and start DVL.

Initial alignment refers to making the coordinate system defined by the inertial navigation system consistent with the navigation coordinate system, providing correct initial conditions for the normal operation of the navigation computer. DVL is a Doppler velocity meter. The seabed measurement range of DVL is about 300-400 meters. When the water depth exceeds this range, DVL automatically switches to the convection velocity measurement mode. The present invention is also carried out in this mode.

S2: The underwater unmanned vehicle executes a motion control strategy, and the depth and speed controllers work in a closed loop to keep the vehicle sailing at a constant depth and speed. The underwater unmanned vehicle controls the rudder to deflect at a fixed rudder angle to drive the underwater unmanned vehicle. Make a plane rotational motion. Record DVL and INS data during exercise. After integrating the convective velocity obtained by DVL, the movement trajectory of the underwater unmanned vehicle without the influence of sea currents is obtained; the movement trajectory of the underwater unmanned vehicle under the influence of sea currents is obtained by INS solution.

Specifically, in step S2, the underwater unmanned vehicle performs a constant-depth and uniform plane rotation motion. If there is no influence of ocean currents, the motion trajectory of the underwater unmanned vehicle is a circle, and its angular velocity is ω, as shown in the circular curve in Figure 3. If affected by ocean currents, the motion trajectory of the underwater unmanned vehicle can be approximated as a sliding circle, as shown in the spiral curve in Figure 3. The sliding direction and speed of the circle reflect the direction and speed of the ocean current.

The angular velocity ω is determined by the speed and deflection angle of the underwater unmanned vehicle. It is easy to obtain the circular motion period T _c of the underwater unmanned vehicle:

Record DVL and INS data during exercise.

The data obtained by DVL is the convection velocity during the movement of underwater unmanned vehicles. It is assumed that the DVL coordinate system coincides with the underwater unmanned vehicle body coordinate system, and no coordinate transformation is required. Convert the convection velocity obtained by DVL into the navigation coordinate system:

in, It is the coordinate transformation matrix from the underwater unmanned vehicle body coordinate system to the navigation coordinate system, which is obtained through the attitude angle measured by the INS.

In the navigation coordinate system, it is easy to get:

Among them, V _n ^body is the projection of the underwater unmanned vehicle speed in the navigation coordinate system, and V _n ^cur is the projection of the ocean current speed in the navigation coordinate system. Therefore, by integrating the convection velocity V _n ^dvl in the navigation coordinate system, the motion trajectory position of the underwater unmanned vehicle without the influence of ocean currents can be obtained:

Where P _dvl = [λ _n λ _e ] ^T is the position of the underwater unmanned vehicle solved by DVL, λ _n and λ _e are respectively the north direction and the east direction of the underwater unmanned vehicle under the navigation coordinate system without the influence of ocean currents. s position. When performing position calculation, the initial position is uniformly provided by the inertial navigation system.

The speed of the underwater unmanned vehicle in the navigation coordinate system calculated by INS is V _INS =[v _n v _e ] ^T , where v _n and v _e are the northward and eastward speeds of the underwater unmanned vehicle in the navigation coordinate system respectively; The calculated motion trajectory position is P _INS = [L _n L _e ] ^T , L _n and L _e are respectively the northward and eastward positions of the underwater unmanned vehicle in the navigation coordinate system under the influence of ocean currents.

S3: From the underwater unmanned vehicle navigation trajectory obtained by DVL, select three trajectory points with a certain interval. Through these three points, the center of the circle of the navigation trajectory without the influence of ocean currents can be estimated.

In the navigation trajectory of the underwater unmanned vehicle obtained by the INS, the trajectory points are collected slidingly at the time interval T _g , and the collected trajectory points are grouped so that each group has three trajectory points, including the last one in the previous group. point, and two newly taken points in this group. From this, the center of the circle where each set of points is located is estimated, that is, the center of the circle sliding under the influence of ocean currents.

Specifically, in step S3, the interval between trajectory points is determined by the speed and rudder angle of the underwater unmanned vehicle, and the proximity of the points on the arc will affect the estimation of the center of the circle. The test found that when the time interval T _g of the trajectory points is between T _c /6 and T _c /4, the estimated center position of the circle is more accurate. Figure 4 is a graph of the circle center estimation results and the circle center fitting straight line. It is estimated that N circle center coordinates c _i (x _i , y _i ), i=1,2...,N. Among them, c _i (x _i , y _i ), i = 1, 2..., N-1 are the center coordinates of the navigation trajectory obtained by the INS, and the order of i represents the time sequence of the navigation trajectory corresponding to the center of the circle, as shown in the marks in Figure 4 . c _N (x _N ,y _N ) is the center coordinate of the navigation trajectory obtained by DVL, as marked in Figure 4.

The process of estimating the center of a circle is as follows:

Assume that three non-collinear trajectory points A(x _a ,y _a ), B(x _b ,y _b ), C(x _c ,y _c ) are selected, and the circle center position O(x _o ,y _o ) can be obtained through calculation. . in:

S4: Determine whether the direction of the sea current is close to the direction of the coordinate axis through the variance of the center coordinates. If the direction of the sea current is close to the direction of the coordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system. Calculate the position of the center of the circle in the new coordinate system after the rotation. The coordinate value in ; if the ocean current has no impact on the underwater unmanned vehicle, the ocean current size is considered to be 0 and the ocean current estimation task is exited; if the ocean current direction deviates from the coordinate axis direction, the obtained circle center coordinates remain unchanged.

The method to determine whether the direction of the ocean current is close to the direction of the coordinate axis is as follows:

It can be known from step S3 that the coordinates of the center of the circle are c _i (x _i , y _i ), i=1,2...,N. Calculate the variance of the circle center coordinates in the horizontal axis direction and the vertical axis direction:

The variances σ _x ² and σ _y ² reflect the degree of dispersion of the circle center coordinates in the horizontal and vertical axis directions respectively. The smaller the variance, the closer the horizontal (vertical) coordinates of each circle center are.

Set the threshold σ _min ² and compare the variances σ _x ² and σ _y ² with the threshold σ _min ² to determine the relationship between the direction of the ocean current and the direction of the coordinate axis:

If σ _x ² ≤ σ _min ² and σ _y ² ≤ σ _min ² , then the estimated center coordinates are similar and the ocean current has no effect on the motion of the underwater unmanned vehicle. Therefore, the ocean current velocity is considered to be 0;

If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis;

If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis;

If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discretization degree of the center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis.

Process the circle center coordinates based on the above judgment results:

If the ocean current velocity is 0, exit the ocean current estimation task.

If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system. Calculate the coordinate value of the center of the circle in the new coordinate system after rotation, which is the required center coordinate.

Among them, the superscript T represents the transposition, and R is the rotation matrix that rotates the coordinate system counterclockwise by 45°:

If the direction of the sea current deviates from the direction of the coordinate axis, keep the circle center coordinates c _i (x _i , y _i ), i = 1, 2..., N unchanged, and obtain the required circle center coordinates

S5: Use the least squares method to perform straight line fitting on the circle center obtained in step S4, and obtain the straight line equation of the circle center fitting.

The straight line fitting method is as follows:

From step S4, we can know that the required circle center coordinates are The straight line equation obtained by fitting is:

y＝kx+b (14)

Among them, k and b can be calculated by the least squares method:

Figure 3 is a graph of the circle center estimation results and the circle center fitting straight line. The circle center fitting straight line is shown as a straight line in Figure 3.

S6: Through geometric relations, determine the vertical foot point of the straight line fitting from the center of the circle sliding with the ocean current to the center of the circle.

The process of determining the vertical foot point is as follows:

It can be seen from steps S4 and S5 that the center coordinates of the navigation trajectory obtained by the INS are The equation of the straight line is equation (14). According to the geometric relationship, the vertical coordinates from the center of the circle to the straight line can be obtained as The calculation formula is:

S7: Calculate the direction angle of the ocean current through the change of the vertical foot point position and the slope of the straight line fitted to the center of the circle.

The process of calculating the current direction angle is as follows:

The equation expression of the circle center fitting straight line obtained in step S5 is formula (14), where the slope of the straight line is k. From step S6, the vertical coordinates can be obtained make:

When m>0, that is, when the ocean current vector is in the first or fourth quadrant, the ocean current direction angle θ is:

θ＝90°-arctan(k) (20)

When m<0, that is, when the ocean current vector is located in the second or third quadrant, the ocean current direction angle θ is:

θ＝270°-arctan(k) (21)

When m=0, that is, the threshold setting in step S4 is unreasonable, and the proximity of the current direction to the coordinate axis cannot be correctly judged. You need to return to step S4, modify the variance threshold and make a new judgment.

If the center coordinate of the circle is rotated counterclockwise in step S4, the rotation angle compensation is performed for the sea current direction angle θ:

θ＝θ-45° (22)

If the current direction angle θ <0° after rotation compensation, then:

θ＝θ+360° (23)

S8: Calculate the distance between adjacent vertical foot points and find the average value of the distance. Divide the mean distance by the time interval between adjacent circle centers when estimating the circle center to obtain the ocean current velocity.

The process of calculating ocean current speed is as follows:

From step S6, the vertical coordinates on the fitting straight line from the center of the circle sliding with the ocean current to the center of the circle are: Calculate the distance between adjacent vertical foot points;

to calculate Take the distance d ₁ between two points as an example:

The calculated distance between the two points is d ₁ . In this way, the distance between p ₁ and p ₂ , p ₂ and p ₃ ,..., p _N-2 and p _N-1 is calculated, and the distance between vertical feet d _i is obtained. i=1,2...,N-2. Mean distance between vertical feet for:

Based on the navigation trajectory solved by the INS in step S3, a trajectory point is collected every time T _g . The collected trajectory points are grouped based on the idea of sliding. Each group has three trajectory points. Two new points are added each time and two older points are discarded. From this, the center of the circle where each set of points is located is estimated. Therefore, the time interval T _d between adjacent circle centers can be obtained:

T _d =2T _g (26)

From this, we can get the estimated value v _c of ocean current velocity:

Specific implementation mode two: This implementation mode will be described with reference to Figure 2.

This embodiment is an underwater unmanned vehicle INS/DVL ocean current estimation system in mid-water areas, including a motion control unit, a navigation information collection unit, a geometric calculation unit, and an ocean current estimation unit.

Motion control unit: used for motion control of underwater unmanned vehicles. During motion control, the depth and speed controllers work in a closed loop to keep the vehicle sailing at a constant depth and speed. The underwater unmanned vehicle controls the rudder deflection at a fixed rudder angle to drive the vehicle to perform planar rotational motion to achieve the motion constraints required by the ocean current estimation method. .

Navigation information collection unit: Determine the convection speed during the movement of the underwater unmanned vehicle based on DVL data Convert the convection speed to the navigation coordinate system, and calculate the motion trajectory position P _dvl of the underwater unmanned vehicle without the influence of ocean currents based on the convection speed in the navigation coordinate system; calculate the underwater unmanned vehicle in the navigation coordinate system based on INS Speed V _INS , and calculate the motion trajectory position P _INS of the underwater unmanned vehicle under the influence of ocean currents;

Geometric calculation unit: Based on the motion trajectories solved by DVL and INS, the center of the circle is determined through the three-point centering method; then the variance of the coordinates of the center of the circle in the horizontal axis direction and the vertical axis direction is calculated to determine and correspond to the deviation between the direction of the ocean current and the direction of the coordinate axis. Adjust the coordinate system; then fit the processed circle center into a straight line through the least squares method, calculate the vertical foot from the center of the circle sliding with the ocean current to the straight line fitted to the center of the circle, and determine the quadrant of the current vector through the change in the position of the vertical foot point;

The process of determining the center of a circle through the three-point centering method includes the following steps:

In the navigation trajectory of the underwater unmanned vehicle obtained by DVL, three trajectory points are selected to estimate the center of the navigation trajectory without the influence of sea currents;

At the same time, in the underwater unmanned vehicle navigation trajectory obtained by the INS, trajectory points are collected slidingly at the time interval T _g , and the collected trajectory points are grouped so that each group has three trajectory points, and the first trajectory in each group The point is the last trajectory point in the previous group, and the remaining two trajectory points are the newly taken points in this group; the center of the circle where each group of points is located is estimated, that is, the center of the circle sliding under the influence of ocean currents;

A total of N center coordinates c _i (x _i , y _i ) are obtained, i=1,2...,N; where i=1,2...,N-1 is the center of the circle for the navigation trajectory obtained by the INS.

The process of calculating the variance of the coordinates of the center of the circle in the horizontal axis direction and the vertical axis direction, determining the deviation of the ocean current direction from the coordinate axis direction, and adjusting the coordinate system accordingly includes the following steps:

Calculate the variance σ _x ² in the horizontal axis direction and the variance σ _y ² in the vertical axis direction of the coordinates of the N circle centers;

If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis;

If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis;

If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discrete degree of the circle center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis;

where σ _min ² is the variance threshold;

If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system, and calculate the coordinate value of the center of the circle in the new coordinate system after rotation.

If the direction of the ocean current deviates from the direction of the coordinate axis, keep the circle center coordinate c _i (x _i , y _i ) unchanged and obtain the required circle center coordinate

Calculate the vertical foot of the fitted straight line from the center of the circle to the center of the circle sliding with the ocean current. Through the change of the position of the vertical foot point, the process of determining the quadrant of the current vector includes the following steps:

Remember that the equation of the straight line fitted to the center of the circle is y=kx+b, where k is the slope of the straight line and b is the intercept;

Determine the vertical foot point of the straight line fitting from the center of the circle sliding with the ocean current to the center of the circle through geometric relationships.

make When m>0, the current direction angle θ=90°-arctan(k), when m<0, the current direction angle θ=270°-arctan(k), when m=0, return to modify the variance threshold and restart make judgments;

Then, the rotation angle is compensated according to whether the counterclockwise rotation is performed. If the circle center coordinate is rotated counterclockwise in step S4, the rotation angle compensation θ=θ-45° is performed on the sea current direction angle θ=θ-45°; if the sea current direction angle is corrected after rotation θ<0°, then let θ=θ+360°.

Current estimation unit: Calculate the distance between adjacent vertical foot points and find the mean value of the distance Convert the mean of the distance/> Divide by the time interval T _d between adjacent circle centers when estimating the circle center to obtain the ocean current velocity.

Example

The method of the present invention is simulated and verified, and the simulation verification conditions are set as:

The underwater unmanned vehicle performs rotational motion with a radius of 100m at a speed of 2m/s. The starting point longitude is 120°, the latitude is 35°, the ocean current speed is set to 0.22m/s, and the direction is 63.44° north by east in the geographical coordinate system. , the simulation time is 1000s, the inertial device sampling frequency is 10HZ, and the DVL sampling frequency is 1HZ;

Gyroscope zero bias 0.0035°/h, gyroscope angle random walk The accelerometer bias is 25μg, and the accelerometer walks randomly/> DVL speed measurement accuracy is 0.2%±2mm/s.

The circle center estimated by the method of this embodiment and its fitting straight line are shown in Figure 4. The simulation time is set to 1000s, which is determined based on the coordinate variance of the center of the circle sliding with the ocean current, and the ocean current direction deviates from the coordinate axis. Calculate the vertical foot position of the fitted straight line from the center of the circle to the center of the circle sliding with the ocean current. Based on the change of the vertical foot position, determine the ocean current vector quadrant as the first quadrant. Combined with the slope of the fitted straight line, the ocean current direction angle is estimated to be north by east in the geographical coordinate system. 61.77°. Based on the distance and time difference between adjacent vertical legs, the ocean current velocity is estimated to be 0.23m/s. The eastward and northward ocean current velocity errors are 0.003m/s and 0.008m/s respectively, and the estimated ocean current velocity error is small.

The above-mentioned calculation examples of the present invention are only to explain the calculation model and calculation process of the present invention in detail, but are not intended to limit the implementation of the present invention. For those of ordinary skill in the art, other changes or modifications in different forms can be made on the basis of the above description. It is impossible to exhaustively enumerate all the implementation modes here. All the technical solutions that belong to the present invention are derived. Obvious changes or modifications are still within the protection scope of the present invention.

Claims (10)

1. A method for estimating ocean currents by underwater unmanned vehicle INS/DVL in mid-water areas, which is characterized by including the following steps: S1: Initial alignment of the inertial navigation system; S2: Control the underwater unmanned vehicle to perform planar rotation and record the DVL and INS data during the movement; Determining the convection velocity during the motion of underwater unmanned vehicle based on DVL data Convert the convection speed to the navigation coordinate system, and calculate the motion trajectory position P _dvl of the underwater unmanned vehicle without the influence of ocean currents based on the convection speed in the navigation coordinate system; At the same time, the speed V _INS of the underwater unmanned vehicle in the navigation coordinate system is calculated based on the INS, and the motion trajectory position P _INS of the underwater unmanned vehicle under the influence of ocean currents is calculated; S3: From the underwater unmanned vehicle navigation trajectory obtained by DVL, select three trajectory points and estimate the center of the navigation trajectory without the influence of sea currents; At the same time, in the underwater unmanned vehicle navigation trajectory obtained by the INS, trajectory points are collected slidingly at the time interval T _g , and the collected trajectory points are grouped so that each group has three trajectory points, and the first trajectory in each group The point is the last trajectory point in the previous group, and the remaining two trajectory points are the newly taken points in this group; the center of the circle where each group of points is located is estimated, that is, the center of the circle sliding under the influence of ocean currents; A total of N center coordinates c _i (x _i ,y _i ) are obtained, i=1,2...,N; where i=1,2...,N-1 is the center of the circle for the navigation trajectory obtained by the INS; S4: Calculate the variance σ _x ² in the horizontal axis direction and the variance σ _y ² in the vertical axis direction of the coordinates of the N center centers; If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis; If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis; If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discrete degree of the circle center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis; where σ _min ² is the variance threshold; If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system, and calculate the coordinate value of the center of the circle in the new coordinate system after rotation. If the direction of the ocean current deviates from the direction of the coordinate axis, keep the circle center coordinate c _i (x _i , y _i ) unchanged and obtain the required circle center coordinate S5: Perform straight line fitting on the center of the circle obtained in step S4, and obtain the straight line equation y=kx+b for the center of the circle, where k is the slope of the straight line and b is the intercept; S6: Determine the vertical foot point of the fitted straight line from the center of the circle sliding with the ocean current to the center of the circle through geometric relationships. S7: order When m>0, the current direction angle θ=90°-arctan(k); when m<0, the current direction angle θ=270°-arctan(k); Then, the rotation angle is compensated according to whether the counterclockwise rotation is performed in step S4. If the circle center coordinate is counterclockwise rotated in step S4, the rotation angle compensation θ=θ-45° is performed for the sea current direction angle θ=θ-45°; if after rotation compensation If the ocean current direction angle θ <0°, then let θ = θ + 360°; S8: Calculate the distance between adjacent vertical foot points and find the mean value of the distance Convert the mean of the distance/> Divide by the time interval T _d between adjacent circle centers when estimating the circle center to obtain the ocean current velocity. 2. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas according to claim 1, characterized in that the time interval T _g in S3 is between T _c /6 and T _c /4. time, where T _c is the circular motion period of the underwater unmanned vehicle. 3. A method for estimating ocean currents for underwater unmanned vehicles in mid-water areas INS/DVL according to claim 2, characterized in that, in the process of calculating the motion trajectory of the underwater unmanned vehicles without the influence of ocean currents in S2, The convection velocity V _n ^dvl in the navigation coordinate system is integrated to obtain the motion trajectory of the underwater unmanned vehicle without the influence of ocean currents. 4. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas according to claim 1, 2 or 3, characterized in that, in S4, if σ _x ² ≤ σ _min ² and σ _y ² ≤ σ _min ² , then the estimated center coordinates are similar, and the ocean current has no effect on the motion of the underwater unmanned vehicle. The ocean current speed is considered to be 0, and the ocean current estimation is exited at this time. 5. A method for estimating ocean currents for underwater unmanned vehicle INS/DVL in mid-water areas according to claim 4, characterized in that the coordinate value of the center of the circle in the new coordinate system after rotation Determine it as follows: Among them, R is the rotation matrix that rotates the coordinate system counterclockwise by 45°. 6. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas according to claim 5, characterized in that, in S5, the least squares method is used to perform straight line fitting on the center of the circle obtained in step S4. 7. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water waters according to claim 6, characterized in that the process of determining the vertical foot point in S6 includes the following steps: Obtain the center coordinates of the navigation trajectory from INS Obtain the equation of the straight line, where i=1,2...,N-1. According to the geometric relationship, the vertical coordinates from the center of the circle to the straight line can be obtained/> as follows: 8. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas according to claim 7, characterized in that the time interval between adjacent circle centers is T _d =2T _g . 9. An underwater unmanned vehicle INS/DVL ocean current estimation method in mid-water areas according to claim 8, characterized in that the distance between adjacent vertical foot points is calculated and the mean value of the distance is obtained. The process includes the following steps: The vertical coordinates on the fitted straight line from the center of the circle to the center of the circle based on sliding with the ocean current are calculate The distance d ₁ between two points: Use the same method to calculate the distance between p ₁ and p ₂ , p ₂ and p ₃ ,..., p _N-2 and p _N-1 , and obtain the distance between vertical feet d _i ,i=1,2...,N-2 ;Mean distance between vertical feet 10. A mid-water unmanned underwater vehicle INS/DVL ocean current estimation system, characterized by including a navigation information collection unit, a geometric calculation unit, and an ocean current estimation unit; Navigation information collection unit: Determine the convection speed during the movement of the underwater unmanned vehicle based on DVL data Convert the convection speed to the navigation coordinate system, and calculate the motion trajectory position P _dvl of the underwater unmanned vehicle without the influence of ocean currents based on the convection speed in the navigation coordinate system; at the same time, calculate the underwater unmanned navigation in the navigation coordinate system based on the INS vehicle speed V _INS , and solve the motion trajectory position P _INS of the underwater unmanned vehicle under the influence of ocean currents; Geometric calculation unit: Based on the motion trajectories calculated by DVL and INS when the underwater unmanned vehicle performs planar rotation, the center of the circle is determined through the three-point centering method; then the variance of the horizontal and vertical axis directions of the coordinates of the center of the circle is calculated. , determine the deviation between the direction of the ocean current and the direction of the coordinate axis and adjust the coordinate system accordingly; then fit the processed circle center into a straight line through the least squares method, calculate the vertical foot from the center of the circle sliding with the ocean current to the center of the circle fitting straight line, through the vertical foot point Changes in position determine the quadrant of the current vector; The process of calculating the variance of the coordinates of the center of the circle in the horizontal axis direction and the vertical axis direction, determining the deviation of the ocean current direction from the coordinate axis direction, and adjusting the coordinate system accordingly includes the following steps: Calculate the variance σ _x ² in the horizontal axis direction and the variance σ _y ² in the vertical axis direction of the coordinates of the N circle centers; If σ _x ² ≤σ _min ² and σ _y ² >σ _min ² , then the abscissa coordinate of the estimated center of the circle is similar, and the direction of the ocean current is close to the ordinate axis; If σ _x ² >σ _min ² and σ _y ² ≤σ _min ² , then the estimated ordinate of the center of the circle is similar, and the direction of the ocean current is close to the abscissa axis; If σ _x ² > σ _min ² and σ _y ² > σ _min ² , then the estimated discrete degree of the circle center coordinates meets the requirements, and the direction of the ocean current deviates from the direction of the coordinate axis; where σ _min ² is the variance threshold; If the direction of the ocean current is close to the abscissa or ordinate axis, rotate the coordinate axis 45° counterclockwise around the coordinate origin to form a new coordinate system, and calculate the coordinate value of the center of the circle in the new coordinate system after rotation. If the direction of the ocean current deviates from the direction of the coordinate axis, keep the circle center coordinate c _i (x _i , y _i ) unchanged and obtain the required circle center coordinate Calculate the vertical foot of the fitted straight line from the center of the circle to the center of the circle sliding with the ocean current. Through the change of the position of the vertical foot point, the process of determining the quadrant of the current vector includes the following steps: Remember that the equation of the straight line fitted to the center of the circle is y=kx+b, where k is the slope of the straight line and b is the intercept; Determine the vertical foot point of the straight line fitting from the center of the circle sliding with the ocean current to the center of the circle through geometric relationships. make When m>0, the current direction angle θ=90°-arctan(k), when m<0, the current direction angle θ=270°-arctan(k), when m=0, return to modify the variance threshold and restart make judgments; Then, the rotation angle is compensated according to whether the counterclockwise rotation is performed. If the circle center coordinate is rotated counterclockwise in step S4, the rotation angle compensation θ=θ-45° is performed on the sea current direction angle θ=θ-45°; if the sea current direction angle is corrected after rotation θ<0°, then let θ=θ+360°; Ocean current estimation unit: Calculate the distance between adjacent vertical points and find the mean value d of the distance; divide the mean value d of the distance by the time interval T _d between adjacent circle centers when estimating the center of the circle to obtain the ocean current velocity.