INS / DVL ocean current estimation method and system for underwater unmanned vehicles in mid-waters

Abstract

The invention discloses an INS / DVL current estimation method and system for underwater unmanned vehicles in mid-waters, belonging to the field of underwater unmanned vehicle navigation and positioning technology. In order to solve the problem that the ocean current estimation method cannot obtain the ocean current velocity when the underwater unmanned vehicle in mid-waters lacks accurate navigation information and the water depth is greater than the DVL bottom exploration depth. The present invention records DVL and INS data when the underwater unmanned vehicle performs a plane rotation motion, and solves the motion trajectory position of the underwater unmanned vehicle without the influence of the ocean current and the motion trajectory position of the underwater unmanned vehicle under the influence of the ocean current; then estimates the center of the navigation trajectory without the influence of the ocean current, adjusts the coordinate system according to the deviation between the direction of the ocean current and the direction of the coordinate axis, and then fits the center straight line, and judges the quadrant where the ocean current vector is located and adjusts it through the change of the position of the perpendicular foot point, and finally calculates the distance between each adjacent perpendicular foot point, finds the mean of the distance, and divides it by the time interval between the adjacent centers when estimating the center of the circle to obtain the ocean current velocity.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater unmanned vehicle navigation and positioning, and relates to an INS / DVL ocean current estimation method and system for an underwater unmanned vehicle in a mid-water area. Background Art

[0002] Navigation and positioning technology provides accurate position, speed and attitude information for underwater unmanned vehicles, which is the key to 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. The inertial navigation system (INS) can provide complete navigation parameters without relying on external information. However, the error of INS will accumulate over time, so additional sensors need to be introduced to correct the error during long-term navigation. The Doppler velocimeter (DVL) emits sound waves to the seabed to measure the reference speed of the carrier based on the Doppler effect. Inertial navigation / Doppler (INS / DVL) combined navigation is the most commonly used method in underwater navigation. Combined 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 bottom depth of DVL, DVL obtains the reference speed of the carrier relative to the ocean current. Directly using DVL data without compensating for the ocean current speed will reduce the navigation performance of INS / DVL or even make it invalid. Therefore, when underwater unmanned vehicles perform missions in mid-water areas, it is necessary to estimate the ocean current speed.

[0003] A Chinese patent with publication number CN112729291A disclosed a method for estimating ocean current velocity of a deep-diving and long-endurance submersible INS / DVL based on specific trajectory motion on April 30, 2021. This method relies on the carrier moving along a specific trajectory, but in the absence of accurate navigation information, it is difficult for the carrier to complete the specific trajectory motion through closed-loop control against the action of the ocean current.

[0004] In summary, there is an urgent need for a method that can estimate ocean current parameters when an underwater unmanned vehicle only has INS / DVL navigation information and cannot resist the effects of ocean currents through closed-loop control, while ensuring that the method has good accuracy. Summary of the invention

[0005] The present invention aims 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 mid-water area and the water depth is greater than the DVL bottom exploration depth.

[0006] A mid-water underwater unmanned vehicle INS / DVL ocean current estimation method comprises the following steps:

[0007] S1: Initial alignment of the inertial navigation system;

[0008] S2: Control the underwater unmanned vehicle to make a planar rotation motion and record the DVL and INS data during the motion;

[0009] Determining the convection velocity of underwater unmanned vehicles during their motion based on DVL data The convection velocity is converted to the navigation coordinate system, and the motion trajectory position P of the underwater unmanned vehicle without the influence of the ocean current is calculated based on the convection velocity in the navigation coordinate system. dvl ;

[0010] At the same time, the speed V of the underwater unmanned vehicle in the navigation coordinate system is calculated based on INS. INS , and solve the motion trajectory position P of the underwater unmanned vehicle under the influence of the ocean current INS ;

[0011] S3: Select three track points from the underwater unmanned vehicle navigation track obtained by DVL to estimate the center of the navigation track without the influence of ocean current;

[0012] At the same time, in the navigation trajectory of the underwater unmanned vehicle obtained by INS, the time interval T g Slide and collect trajectory points, group the collected trajectory points so that each group has three trajectory points, the first trajectory point in each group is the last trajectory point in the previous group, and the remaining two trajectory points are newly taken points in this group; estimate the center of the circle where each group of points is located, that is, the center of the circle sliding under the influence of the current;

[0013] A total of N circle center coordinates c are obtained i (x i ,y i ),i=1,2…,N; where i=1,2…,N-1 is the center of the circle of the navigation track obtained by INS;

[0014] S4: Calculate the variance σ of the coordinates of the N circle centers in the horizontal direction x 2 The variance σ with respect to the vertical axis y 2 ;

[0015] If σ x 2 ≤σ min 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis;

[0016] If σ x 2 >σ min 2 And σy 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis;

[0017] If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction;

[0018] where σ min 2 is the variance threshold;

[0019] If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system, and the coordinate value of the center of the circle after rotation in the new coordinate system is calculated.

[0020] If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ) remains unchanged, and the required center coordinates are obtained

[0021]

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

[0023] S6: Determine the foot point of the perpendicular line from the center of the circle sliding with the current to the center of the circle through geometric relationships

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

[0025] Then, the rotation angle compensation is performed according to whether the counterclockwise rotation is performed in step S4. If the center coordinate is rotated counterclockwise in step S4, the current direction angle θ is compensated by the rotation angle θ=θ-45°; if the current direction angle θ is less than 0° after the rotation compensation, θ=θ+360°;

[0026] S8: Calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The mean of the distance Divide by the time interval T between adjacent circle centers when estimating the circle center d Get the current speed.

[0027] Furthermore, the time interval T in S3 g The value is T c / 6 to T c / 4, where T c is the circular motion period of the underwater unmanned vehicle.

[0028] Furthermore, in the process of calculating the motion trajectory of the underwater unmanned vehicle without the influence of the ocean current in S2, the convection velocity V in the navigation coordinate system is calculated. n dvl The integral is used to obtain the motion trajectory of the underwater unmanned vehicle without the influence of ocean current.

[0029] Furthermore, if σ x 2 ≤σ min 2 And σ y 2 ≤σ min 2 , the estimated coordinates of the circle center are close, the ocean current has no effect on the movement of the underwater unmanned vehicle, and the ocean current speed is considered to be 0, then the ocean current estimation is exited.

[0030] Furthermore, the coordinate value of the center of the circle in the new coordinate system after rotation is Determined by:

[0031]

[0032] Where R is the rotation matrix of the coordinate system rotating 45° counterclockwise.

[0033] Furthermore, in S5, a linear fit is performed on the circle center obtained in step S4 by using the least square method.

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

[0035] Obtain the center coordinates of the navigation track from INS Get the equation of the line, where i = 1, 2…, N-1, and according to the geometric relationship, get the coordinates of the foot of the perpendicular from the center of the circle to the line as follows:

[0036]

[0037]

[0038] Furthermore, the time interval T between the adjacent circle centers is d=2T g .

[0039] Furthermore, the distance between each adjacent perpendicular foot point is calculated and the mean of the distance is obtained. The process includes the following steps:

[0040] The foot coordinates of the perpendicular line from the center of the circle to the center of the circle based on sliding with the ocean current are:

[0041] calculate The distance between two points d1:

[0042]

[0043] Use the same method to calculate p1 and p2, p2 and p3, ..., p N-2 With p N-1 The distance between the vertical feet is d i ,i=1,2…,N-2; mean distance between vertical feet

[0044] An INS / DVL ocean current estimation system for an underwater unmanned vehicle in a mid-water area, comprising a navigation information acquisition unit, a geometry calculation unit, and an ocean current estimation unit;

[0045] Navigation information acquisition unit: Determine the convection velocity of the underwater unmanned vehicle during its movement based on DVL data The convection velocity is converted to the navigation coordinate system, and the motion trajectory position P of the underwater unmanned vehicle without the influence of the ocean current is calculated based on the convection velocity in the navigation coordinate system. dvl ; At the same time, the speed V of the underwater unmanned vehicle in the navigation coordinate system is calculated based on INS INS , and solve the motion trajectory position P of the underwater unmanned vehicle under the influence of the ocean current INS ;

[0046] Geometric calculation unit: Based on the motion trajectory solved by DVL and INS when the underwater unmanned vehicle performs a plane rotation motion, the center of the circle is determined by the three-point circle center determination method; then the variance of the coordinates of the center of the circle in the horizontal and vertical axes is calculated to determine the deviation between the direction of the ocean current and the direction of the coordinate axis and adjust the coordinate system accordingly; then the processed center of the circle is fitted into a straight line by the least squares method, and the foot of the perpendicular from the center of the circle sliding with the ocean current to the center of the circle fitting straight line is calculated, and the quadrant of the ocean current vector is determined by the change in the position of the foot of the perpendicular point;

[0047] The process of calculating the variance of the coordinates of the center of the circle in the horizontal and vertical directions, determining the deviation between the direction of the current and the direction of the coordinate axis, and adjusting the coordinate system accordingly includes the following steps:

[0048] Calculate the variance σ of the coordinates of the N circle centers in the horizontal directionx 2 The variance σ with respect to the vertical axis y 2 ;

[0049] If σ x 2 ≤σ min 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis;

[0050] If σ x 2 >σ min 2 And σ y 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis;

[0051] If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction;

[0052] where σ min 2 is the variance threshold;

[0053] If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system, and the coordinate value of the center of the circle after rotation in the new coordinate system is calculated.

[0054] If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ) remains unchanged, and the required center coordinates are obtained

[0055]

[0056] The process of calculating the foot of the perpendicular from the center of the circle sliding with the ocean current to the fitting straight line at the center of the circle and judging the quadrant of the ocean current vector by the change of the position of the foot of the perpendicular point includes the following steps:

[0057] 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 line and b is the intercept;

[0058] Determine the foot point of the perpendicular to the straight line from the center of the circle sliding with the ocean current through geometric relationships

[0059] 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 re-judge;

[0060] Then, the rotation angle compensation is performed according to whether the counterclockwise rotation is performed. If the center coordinate is rotated counterclockwise in step S4, the current direction angle θ is compensated by the rotation angle θ=θ-45°; if the current direction angle θ is less than 0° after the rotation compensation, θ=θ+360°;

[0061] Current estimation unit: calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The mean of the distance Divide by the time interval T between adjacent circle centers when estimating the circle center d Get the current speed.

[0062] The beneficial effects of the present invention are:

[0063] The present invention provides an INS / DVL ocean current estimation method and system for an underwater unmanned vehicle in mid-waters. Based on the fixed steering angle constraint, the present invention uses DVL and INS to solve the motion trajectory of the underwater unmanned vehicle in the absence of ocean current and with ocean current, and obtains the ocean current speed and direction during the motion of the underwater unmanned vehicle through geometric relationships. The method does not require additional sensor equipment or hydroacoustic communication equipment, and the motion constraint is simple to implement. It can effectively obtain the ocean current speed when the underwater unmanned vehicle in mid-waters lacks accurate navigation information. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 Implementation flow chart for the underwater unmanned vehicle current estimation method.

[0065] Figure 2 This is the structural block diagram of the underwater unmanned vehicle current estimation system.

[0066] Figure 3 The following are the motion trajectories of underwater unmanned vehicles in conditions with and without ocean currents.

[0067] Figure 4 This is the center estimation result and the center fitting straight line graph. DETAILED DESCRIPTION

[0068] Specific implementation method 1: Combination Figure 1 To explain this embodiment,

[0069] This embodiment is a method for estimating ocean currents of an underwater unmanned vehicle INS / DVL in mid-waters, comprising the following steps:

[0070] S1: Start the ocean current estimation task, perform the initial alignment of the inertial navigation system, i.e. INS, and start the DVL.

[0071] Initial alignment means 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 velocimeter. 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.

[0072] S2: The underwater unmanned vehicle executes the motion control strategy. The depth and speed controllers work in a closed loop to keep the vehicle at a constant depth and speed. The underwater unmanned vehicle controls the rudder to deflect the fixed rudder angle to drive the underwater unmanned vehicle to perform a planar rotation. The DVL and INS data are recorded during the movement. After the convection velocity obtained by the DVL is integrated, the motion trajectory of the underwater unmanned vehicle without the influence of the current is obtained; the motion trajectory of the underwater unmanned vehicle under the influence of the current is obtained by solving the INS.

[0073] Specifically, in step S2, the underwater unmanned vehicle performs a constant depth uniform plane rotation motion. If there is no influence of the ocean current, the motion trajectory of the underwater unmanned vehicle is a circle, and its angular velocity is ω, such as Figure 3 If there is an influence of ocean current, the motion trajectory of the underwater unmanned vehicle can be approximated as a sliding circle, as shown in the circular curve in the figure. Figure 3 As shown in the spiral curve in the figure, the sliding direction and speed of the circle reflect the direction and speed of the ocean current.

[0074] The angular velocity ω is determined by the speed and deflection angle of the underwater unmanned vehicle. It is easy to get the circular motion period T of the underwater unmanned vehicle c :

[0075]

[0076] Record DVL and INS data during movement.

[0077] The data obtained by DVL is the convection velocity during the movement of the underwater unmanned vehicle. Assuming that the DVL coordinate system coincides with the underwater unmanned vehicle body coordinate system, no coordinate transformation is required. The convection velocity obtained by DVL is converted to the navigation coordinate system:

[0078]

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

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

[0081]

[0082] Among them, V n body is the projection of the underwater unmanned vehicle speed in the navigation coordinate system, V n cur It is the projection of the ocean current velocity in the navigation coordinate system. Therefore, the convection velocity V in the navigation coordinate system n dvl By integrating, we can obtain the motion trajectory of the underwater unmanned vehicle without the influence of ocean currents:

[0083]

[0084] Where P dvl =[λ n λ e ] T is the position of the underwater unmanned vehicle solved by DVL, λ n and λ e They are the north and east positions of the underwater unmanned vehicle in the navigation coordinate system without the influence of ocean currents. When calculating the position, the initial position is uniformly provided by the inertial navigation system.

[0085] 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 north and east 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 They are respectively the north and east positions of the underwater unmanned vehicle in the navigation coordinate system under the influence of ocean currents.

[0086] S3: In the navigation track of the underwater unmanned vehicle obtained by DVL, three track points with a certain interval are selected. Through these three points, the center of the navigation track without the influence of the current can be estimated.

[0087] In the navigation trajectory of the underwater unmanned vehicle obtained by INS, the time interval T g Slide and collect trajectory points, group the collected trajectory points so that each group has three trajectory points, including the last point in the previous group and two newly collected points in this group. Estimate the center of the circle where each group of points is located, that is, the center of the circle sliding under the influence of the current.

[0088] Specifically, in step S3, the interval between the track 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. Tests have found that when the time interval between the track points is T g In T c / 6 to T c / 4, the estimated center position is more accurate. Figure 4 The estimated result of the circle center and the fitted straight line diagram of the circle center. N circle center coordinates c are estimated i (x i ,y i ),i=1,2…,N. Where c i (x i ,y i ), i = 1, 2…, N-1 are the coordinates of the center of the circle obtained by INS. The order of i represents the time sequence of the center of the circle corresponding to the navigation trajectory, such as Figure 4 As shown in the mark. N (x N ,y N ) is the coordinate of the center of the DVL navigation track, such as Figure 4 As shown in the mark.

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

[0090] Assume that three non-collinear trajectory points A(x a ,y a ), B(x b ,y b ), C(x c ,y c ), the center position O(x o ,y o ).in:

[0091]

[0092]

[0093] S4: Determine whether the direction of the ocean current is close to the direction of the coordinate axis through the variance of the coordinates of the center of the circle. If the direction of the ocean current is close to the direction of the coordinate axis, the coordinate axis is rotated 45° counterclockwise around the origin of the coordinate system to form a new coordinate system, and the coordinate value of the center of the circle after the rotation in the new coordinate system is calculated; if the ocean current has no effect on the underwater unmanned vehicle, the ocean current size is considered to be 0, and the ocean current estimation task is exited; if the direction of the ocean current deviates from the direction of the coordinate axis, the obtained coordinates of the center of the circle are kept unchanged.

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

[0095] From step S3, we can know 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 horizontal axis and the vertical axis of the center coordinate:

[0096]

[0097]

[0098]

[0099]

[0100] Variance σ x 2 With σ y 2 They respectively reflect the degree of dispersion of the coordinates of the center of the circle in the horizontal and vertical directions. The smaller the variance, the closer the horizontal (vertical) coordinates of the centers of the circles are.

[0101] Set the threshold σ min 2 , by taking the variance σ x 2 , σ y 2 With the threshold σ min 2 Compare and determine the relationship between the direction of the ocean current and the direction of the coordinate axis:

[0102] If σ x 2 ≤σ min 2 And σ y 2 ≤σ min 2 , then the estimated coordinates of the center of the circle are close, and the ocean current has no effect on the movement of the underwater unmanned vehicle. Therefore, it is considered that the ocean current speed is 0;

[0103] If σ x 2 ≤σmin 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis;

[0104] If σ x 2 >σ min 2 And σ y 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis;

[0105] If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction.

[0106] According to the above judgment results, the coordinates of the circle center are processed:

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

[0108] If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system. The coordinate value of the center of the circle after rotation in the new coordinate system is calculated, which is the required coordinate value of the center of the circle.

[0109] Among them, the superscript T represents transposition, and R is the rotation matrix of the coordinate system rotating 45° counterclockwise:

[0110]

[0111] If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ),i=1,2…,N remains unchanged, and the required center coordinates are obtained

[0112]

[0113] S5: Perform a straight line fitting on the circle center obtained in step S4 by the least square method to obtain a straight line equation of the circle center fitting.

[0114] The straight line fitting method is as follows:

[0115] From step S4, we know that the required center coordinates are The equation of the fitted straight line is:

[0116] y=kx+b (14)

[0117] Among them, k and b are calculated by the least squares method:

[0118]

[0119]

[0120] Figure 3 The center estimation result and the center fitting straight line are shown in Figure 2. Figure 3 Straight line shown.

[0121] S6: Through geometric relationships, determine the foot point of the perpendicular line from the center of the circle sliding with the ocean current to the center of the circle.

[0122] The process of determining the foot point is as follows:

[0123] From steps S4 and S5, we can know that the coordinates of the center of the navigation track obtained by INS are: The equation of the line is (14). According to the geometric relationship, the coordinates of the foot of the perpendicular from the center of the circle to the line are: The calculation formula is:

[0124]

[0125]

[0126] S7: Calculate the direction angle of the ocean current by the change in the foot point position and the slope of the fitted straight line at the center of the circle.

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

[0128] The equation of the center fitting line obtained in step S5 is equation (14), where the slope of the line is k. The foot coordinates can be obtained in step S6 make:

[0129]

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

[0131] θ=90°-arctan(k) (20)

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

[0133] θ=270°-arctan(k) (21)

[0134] When m=0, that is, the threshold setting in step S4 is unreasonable, and the proximity between the current direction and the coordinate axis cannot be correctly judged, it is necessary to return to step S4, modify the variance threshold and make a new judgment.

[0135] If the center coordinates are rotated counterclockwise in step S4, the current direction angle θ is compensated for by the rotation angle:

[0136] θ=θ-45° (22)

[0137] If the current direction angle θ after rotation compensation is less than 0°, then:

[0138] θ=θ+360° (23)

[0139] S8: Calculate the distance between each adjacent perpendicular foot point and find the average of the distances. Divide the average of the distances by the time interval between the adjacent centers of the circle when estimating the center of the circle to obtain the magnitude of the current speed.

[0140] The process of calculating ocean current speed is as follows:

[0141] From step S6, the foot coordinate of the perpendicular line from the center of the circle sliding with the ocean current to the center of the circle is Calculate the distance between adjacent perpendicular foot points;

[0142] Calculate Take the distance d1 between two points as an example:

[0143]

[0144] The calculated distance between the two points is d1. This method is used to calculate the distance between p1 and p2, p2 and p3, ..., p N-2 With p N-1 The distance between the vertical feet is d i ,i=1,2…,N-2. The mean distance between the vertical feet for:

[0145]

[0146] Based on the navigation track calculated by INS in step S3, every time T g Collect a trajectory point. In the collected trajectory points, group them with the idea of ​​sliding. Each group has three trajectory points. Two new points are added each time, and two older points are discarded. Estimate the center of the circle where each group of points is located. Therefore, the time interval T between adjacent circle centers can be obtained. d :

[0147] Td =2T g (26)

[0148] From this, we can get the estimated value of the ocean current speed v c :

[0149]

[0150] Specific implementation method 2: Combination Figure 2 To explain this embodiment,

[0151] The present embodiment is a mid-water underwater unmanned vehicle INS / DVL ocean current estimation system, which includes a motion control unit, a navigation information collection unit, a geometry calculation unit, and an ocean current estimation unit.

[0152] 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 at a constant depth and speed. The underwater unmanned vehicle controls the rudder to deflect the fixed rudder angle to drive the vehicle to perform a planar rotation motion, thus realizing the motion constraints required by the current estimation method.

[0153] Navigation information acquisition unit: Determine the convection velocity of the underwater unmanned vehicle during its movement based on DVL data The convection velocity is converted to the navigation coordinate system, and the motion trajectory position P of the underwater unmanned vehicle without the influence of the ocean current is calculated based on the convection velocity in the navigation coordinate system. dvl ; Calculate the speed V of the underwater unmanned vehicle in the navigation coordinate system based on INS INS , and solve the motion trajectory position P of the underwater unmanned vehicle under the influence of the ocean current INS ;

[0154] Geometric calculation unit: Based on the motion trajectory solved by DVL and INS, the center of the circle is determined by the three-point center determination method; then the variance of the coordinates of the center of the circle in the horizontal and vertical directions is calculated to determine the deviation between the current direction and the coordinate axis direction and adjust the coordinate system accordingly; then the processed center of the circle is fitted into a straight line by the least squares method, and the foot of the perpendicular from the center of the circle sliding with the current to the center of the circle fitting straight line is calculated, and the quadrant of the current vector is determined by the change in the position of the foot of the perpendicular point;

[0155] The process of determining the center of a circle by the three-point method includes the following steps:

[0156] In the navigation track of the underwater unmanned vehicle obtained by DVL, three track points are selected to estimate the center of the navigation track without the influence of the current;

[0157] At the same time, in the navigation trajectory of the underwater unmanned vehicle obtained by INS, the time interval T gSlide and collect trajectory points, group the collected trajectory points so that each group has three trajectory points, the first trajectory point in each group is the last trajectory point in the previous group, and the remaining two trajectory points are newly taken points in this group; estimate the center of the circle where each group of points is located, that is, the center of the circle sliding under the influence of the current;

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

[0159] The process of calculating the variance of the coordinates of the center of the circle in the horizontal and vertical directions, determining the deviation between the direction of the current and the direction of the coordinate axis, and adjusting the coordinate system accordingly includes the following steps:

[0160] Calculate the variance σ of the coordinates of the N circle centers in the horizontal direction x 2 The variance σ with respect to the vertical axis y 2 ;

[0161] If σ x 2 ≤σ min 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis;

[0162] If σ x 2 >σ min 2 And σ y 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis;

[0163] If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction;

[0164] where σ min 2 is the variance threshold;

[0165] If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system, and the coordinate value of the center of the circle after rotation in the new coordinate system is calculated.

[0166] If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ) remains unchanged, and the required center coordinates are obtained

[0167]

[0168] The process of calculating the foot of the perpendicular from the center of the circle sliding with the ocean current to the fitting straight line at the center of the circle and judging the quadrant of the ocean current vector by the change of the position of the foot of the perpendicular point includes the following steps:

[0169] 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 line and b is the intercept;

[0170] Determine the foot point of the perpendicular to the straight line from the center of the circle sliding with the ocean current through geometric relationships

[0171] 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 re-judge;

[0172] Then, rotation angle compensation is performed according to whether counterclockwise rotation is performed. If the center coordinates of the circle are rotated counterclockwise in step S4, rotation angle compensation θ is performed on the current direction angle θ, θ=θ-45°; if the current direction angle θ is less than 0° after rotation compensation, θ=θ+360° is set.

[0173] Current estimation unit: calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The mean of the distance Divide by the time interval T between adjacent circle centers when estimating the circle center d Get the current speed.

[0174] Example

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

[0176] The underwater unmanned vehicle performs a rotation motion with a radius of 100m at a speed of 2m / s. The starting point is at a longitude of 120° and a latitude of 35°. The ocean current speed is set to 0.22m / s. The direction is 63.44° north-east of the geographic coordinate system. The simulation duration is 1000s. The sampling frequency of the inertial device is 10HZ, and the sampling frequency of the DVL is 1HZ.

[0177] Gyroscope zero bias 0.0035° / h, gyroscope angle random walk Accelerometer zero bias 25μg, accelerometer random walk DVL speed measurement accuracy is 0.2%±2mm / s.

[0178] The center of the circle and the fitted straight line estimated by the method of this embodiment are as follows: Figure 4 As shown. The simulation time is set to 1000s. The current direction deviates from the coordinate axis according to the coordinate variance of the center of the circle sliding with the current. The foot of the perpendicular from the center of the circle sliding with the current to the center of the circle is calculated. According to the change of the foot of the perpendicular, the quadrant of the current vector is determined to be the first quadrant. Combined with the slope of the fitting line, the current direction angle is estimated to be 61.77° north-east of the geographic coordinate system. According to the distance and time difference between adjacent perpendiculars, the current velocity value is estimated to be 0.23m / s. The errors of the eastward and northward current velocities are 0.003m / s and 0.008m / s respectively, and the error of the estimated current velocity is small.

[0179] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for estimating ocean currents of an underwater unmanned vehicle INS / DVL in mid-waters, characterized in that: The following steps are involved: S1: Initial alignment of the inertial navigation system; S2: Control the underwater unmanned vehicle to make a planar rotation motion and record the DVL and INS data during the motion; Determining the convection velocity of underwater unmanned vehicles during their motion based on DVL data The convection velocity is converted to the navigation coordinate system, and the motion trajectory position P of the underwater unmanned vehicle without the influence of the ocean current is calculated based on the convection velocity in the navigation coordinate system. dvl ; At the same time, the speed V of the underwater unmanned vehicle in the navigation coordinate system is calculated based on INS. INS , and solve the motion trajectory position P of the underwater unmanned vehicle under the influence of the ocean current INS ; S3: Select three track points from the underwater unmanned vehicle navigation track obtained by DVL to estimate the center of the navigation track without the influence of ocean current; At the same time, in the navigation trajectory of the underwater unmanned vehicle obtained by INS, the time interval T g Slide and collect trajectory points, group the collected trajectory points so that each group has three trajectory points, the first trajectory point in each group is the last trajectory point in the previous group, and the remaining two trajectory points are newly taken points in this group; estimate the center of the circle where each group of points is located, that is, the center of the circle sliding under the influence of the current; A total of N circle center coordinates c are obtained i (x i ,y i ),i=1,2…,N; where i=1,2…,N-1 is the center of the circle of the navigation track obtained by INS; S4: Calculate the variance σ of the coordinates of the N circle centers in the horizontal direction x 2 The variance σ with respect to the vertical axis y 2 ; If σ x 2 ≤σ min 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis; If σ x 2 >σ min 2 And σ y 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis; If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction; where σ min 2 is the variance threshold; If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system, and the coordinate value of the center of the circle after rotation in the new coordinate system is calculated. If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ) remains unchanged, and the required center coordinates are obtained S5: Perform a straight line fitting on the circle center obtained in step S4 to obtain a straight line equation y=kx+b, where k is the slope of the straight line and b is the intercept; S6: Determine the foot point of the perpendicular line from the center of the circle sliding with the 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 compensation is performed according to whether the counterclockwise rotation is performed in step S4. If the center coordinate is rotated counterclockwise in step S4, the current direction angle θ is compensated by the rotation angle θ=θ-45°; if the current direction angle θ is less than 0° after the rotation compensation, θ=θ+360°; S8: Calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The mean of the distance Divide by the time interval T between adjacent circle centers when estimating the circle center d Get the current speed.

2. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 1, characterized in that: The time interval T in S3 g The value is T c / 6 to T c / 4, where T c is the circular motion period of the underwater unmanned vehicle.

3. The method for estimating ocean currents of an INS / DVL in mid-water area according to claim 2, characterized in that: In the process of solving the motion trajectory of the underwater unmanned vehicle without the influence of the ocean current in S2, the convection velocity V in the navigation coordinate system is calculated. n dvl The integral is used to obtain the motion trajectory of the underwater unmanned vehicle without the influence of ocean current.

4. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 1, 2 or 3, characterized in that: In S4, if σ x 2 ≤σ min 2 And σ y 2 ≤σ min 2 , the estimated coordinates of the circle center are close, the ocean current has no effect on the movement of the underwater unmanned vehicle, and the ocean current speed is considered to be 0, then the ocean current estimation is exited.

5. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 4, characterized in that: The coordinate value of the center of the circle in the new coordinate system after rotation Determined by: Where R is the rotation matrix of the coordinate system rotating 45° counterclockwise.

6. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 5, characterized in that: In S5, a linear fit is performed on the center of the circle obtained in step S4 by the least square method.

7. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 6, characterized in that: The process of determining the foot point of S6 includes the following steps: Obtain the center coordinates of the navigation track from INS Get the equation of the line, where i = 1, 2…, N-1, and according to the geometric relationship, get the coordinates of the foot of the perpendicular from the center of the circle to the line as follows:

8. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 7, characterized in that: The time interval T between the adjacent circle centers d =2T g .

9. The method for estimating ocean currents of an INS / DVL in mid-waters according to claim 8, characterized in that: Calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The process includes the following steps: The foot coordinates of the perpendicular line from the center of the circle to the center of the circle based on sliding with the ocean current are: calculate The distance between two points d1: Use the same method to calculate p1 and p2, p2 and p3, ..., p N-2 With p N-1 The distance between the vertical feet is d i ,i=1,2…,N-2; mean distance between vertical feet 10. An INS / DVL ocean current estimation system for mid-water underwater unmanned vehicles, characterized in that: It includes navigation information collection unit, geometry calculation unit and ocean current estimation unit; Navigation information acquisition unit: Determine the convection velocity of the underwater unmanned vehicle during its movement based on DVL data The convection velocity is converted to the navigation coordinate system, and the motion trajectory position P of the underwater unmanned vehicle without the influence of the ocean current is calculated based on the convection velocity in the navigation coordinate system. dvl ; At the same time, the speed V of the underwater unmanned vehicle in the navigation coordinate system is calculated based on INS INS , and solve the motion trajectory position P of the underwater unmanned vehicle under the influence of the ocean current INS ; Geometric calculation unit: Based on the motion trajectory solved by DVL and INS when the underwater unmanned vehicle performs a plane rotation motion, the center of the circle is determined by the three-point circle center determination method; then the variance of the coordinates of the center of the circle in the horizontal and vertical axes is calculated to determine the deviation between the direction of the ocean current and the direction of the coordinate axis and adjust the coordinate system accordingly; then the processed center of the circle is fitted into a straight line by the least squares method, and the foot of the perpendicular from the center of the circle sliding with the ocean current to the center of the circle fitting straight line is calculated, and the quadrant of the ocean current vector is determined by the change in the position of the foot of the perpendicular point; The process of calculating the variance of the coordinates of the center of the circle in the horizontal and vertical directions, determining the deviation between the direction of the current and the direction of the coordinate axis, and adjusting the coordinate system accordingly includes the following steps: Calculate the variance σ of the coordinates of the N circle centers in the horizontal direction x 2 The variance σ with respect to the vertical axis y 2 ; If σ x 2 ≤σ min 2 And σ y 2 >σ min 2 , then the estimated horizontal coordinates of the circle center are close, and the current direction is close to the vertical coordinate axis; If σ x 2 >σ min 2 And σ y 2 ≤σ min 2 , then the estimated ordinates of the circle center are close, and the direction of the current is close to the abscissa axis; If σ x 2 >σ min 2 And σ y 2 >σ min 2 , then the estimated discreteness of the circle center coordinates meets the requirements, and the current direction deviates from the coordinate axis direction; where σ min 2 is the variance threshold; If the direction of the ocean current is close to the horizontal axis or the vertical axis, the coordinate axis is rotated 45° counterclockwise around the origin to form a new coordinate system, and the coordinate value of the center of the circle after rotation in the new coordinate system is calculated. If the direction of the current deviates from the direction of the coordinate axis, the center coordinate c is maintained. i (x i ,y i ) remains unchanged, and the required center coordinates are obtained The process of calculating the foot of the perpendicular from the center of the circle sliding with the ocean current to the fitting straight line at the center of the circle and judging the quadrant of the ocean current vector by the change of the position of the foot of the perpendicular point 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 line and b is the intercept; Determine the foot point of the perpendicular to the straight line from the center of the circle sliding with the ocean current 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 re-judge; Then, the rotation angle compensation is performed according to whether the counterclockwise rotation is performed. If the center coordinates of the circle are rotated counterclockwise, the current direction angle θ is compensated by the rotation angle θ=θ-45°; if the current direction angle θ is less than 0° after the rotation compensation, θ=θ+360°; Current estimation unit: calculate the distance between each adjacent perpendicular foot point and find the mean of the distance The mean of the distance Divide by the time interval T between adjacent circle centers when estimating the circle center d Get the current speed.

Images