A three-dimensional underwater motion trajectory interpolation method and system considering the influence of ocean currents

Abstract

The present invention relates to the technical field of ship navigation operations, and provides a three-dimensional underwater motion trajectory interpolation method and system that takes into account the influence of ocean currents. The method includes calculating the turning rate based on the heading difference and time difference between two adjacent motion trajectory points to be interpolated, and calculating the heading of the interpolation point based on the turning rate; calculating the horizontal velocity and vertical velocity of the interpolation point based on acceleration; fusing ocean current information based on time to obtain the horizontal flow velocity, current direction, and vertical flow velocity of the interpolation point, inferring the forward and reverse horizontal position and forward and reverse vertical position of the interpolation point, and calculating the horizontal position and vertical position of the interpolation point; and completing the interpolation between two adjacent motion trajectory points based on the interpolation point time and the horizontal and vertical position of the interpolation point. The present invention accurately establishes the corresponding relationship between time and trajectory points, forming a motion trajectory with a uniform time series, and provides a reliable data foundation for subsequent applications such as trajectory fusion and playback.

Description

Technical Field

[0001] The present invention relates to the technical field of ship navigation operations, and in particular to a three-dimensional underwater motion trajectory interpolation method and system considering the influence of ocean currents. Background Art

[0002] In scenarios such as underwater target identification or underwater navigation status playback analysis, it is often necessary to obtain dense underwater motion trajectory information that is close to the actual motion state. This information is an important basis for achieving accurate target positioning, navigation status assessment, and fault analysis. However, during actual underwater navigation, problems such as abnormal positioning signals or equipment failures will lead to incomplete collected motion trajectory data and uneven distribution of track points. Some underwater vehicles have limited storage space, resulting in sparse track point storage. Therefore, interpolation is required, that is, restoring missing track points from historical trajectory point data and establishing a relationship between time and trajectory points to supplement the complete motion trajectory information.

[0003] Most existing trajectory interpolation methods fail to fully consider the impact of ocean currents on the motion of underwater vehicles, and rarely incorporate the vehicle's own motion characteristics, such as heading changes and speed changes. This results in deviations between the interpolated trajectory points and the actual motion process, making it difficult to meet the trajectory accuracy requirements of scenarios such as underwater target identification and navigation status playback analysis. Summary of the Invention

[0004] The present invention aims to address at least one of the technical problems existing in the related art. To this end, the present invention provides a method and system for interpolating three-dimensional underwater motion trajectories that consider the influence of ocean currents. By considering the motion characteristics of ocean currents and the speed and heading of underwater vehicles during the interpolation process, the present invention restores missing trajectory points from historical trajectory point data, establishes a relationship between time and trajectory points, and forms a uniform time series of motion trajectories, providing a reliable data foundation for subsequent applications such as trajectory fusion and playback.

[0005] The present invention provides a three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents, comprising:

[0006] S1: Determine two adjacent motion trajectory points that need to be interpolated according to the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculate the number of interpolation points and the interpolation point time;

[0007] S2: Calculate the turning rate between the two adjacent motion trajectory points that need to be interpolated based on the heading difference and time difference between the two adjacent motion trajectory points that need to be interpolated, and calculate the heading of the interpolation point based on the turning rate;

[0008] S3: Calculate the horizontal velocity and vertical velocity of the interpolation point based on the acceleration of the two adjacent motion trajectory points to be interpolated;

[0009] S4: fusing the horizontal flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal flow velocity of the interpolation point, fusing the ocean current directions of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the ocean current direction of the interpolation point, and fusing the vertical flow velocities of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical flow velocity of the interpolation point;

[0010] S5: decomposing the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposing the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, calculating the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and calculating the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point;

[0011] S6: Calculating the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculating the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point;

[0012] S7: Repeat steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and complete the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points.

[0013] Furthermore, in step S1,

[0014] Calculate the time difference between two adjacent motion trajectory points. If the time difference between two adjacent motion trajectory points is less than or equal to the interpolation time interval, no interpolation is required.

[0015] If the time difference between two adjacent motion trajectory points is greater than the interpolation time interval, the time difference is divided by the interpolation time interval and rounded up to obtain the number of interpolation points;

[0016] The calculation expression of the interpolation point time is:

[0017]

[0018] in, For the interpolation point time, is the time of the first motion trajectory point, is the interpolation time interval.

[0019] Furthermore, step S2 includes:

[0020] S21: Calculate the heading difference between two adjacent motion trajectory points that need to be interpolated. If the heading difference is less than or equal to , then the heading does not need to be interpolated;

[0021] If the heading difference is greater than , then the turning rate between two adjacent motion trajectory points is obtained by dividing the heading difference by the time difference;

[0022] S22: Calculate the heading of the interpolation point based on the turning rate between two adjacent motion trajectory points. The calculation expression is:

[0023]

[0024] in, For the The heading of the interpolation points, is the heading of the first motion trajectory point, is the turning rate between two adjacent motion trajectory points, is the interpolation time interval;

[0025] like , then reduce ,like , then add .

[0026] Furthermore, step S3 includes:

[0027] S311: According to The horizontal speed at the moment and Calculate the positive horizontal acceleration from the horizontal velocity at the moment;

[0028] S312: According to the positive horizontal acceleration and The horizontal velocity at the moment is calculated to obtain the horizontal velocity of the forward interpolation point;

[0029] S313: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the positive vertical acceleration;

[0030] S314: According to the positive vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the forward interpolation point.

[0031] Furthermore, step S3 also includes:

[0032] S321: According to The horizontal speed at the moment and Calculate the reverse horizontal acceleration from the horizontal velocity at the moment;

[0033] S322: According to the reverse horizontal acceleration and The horizontal velocity at the moment is calculated to obtain the horizontal velocity of the reverse interpolation point;

[0034] S323: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the reverse vertical acceleration;

[0035] S324: According to the reverse vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the reverse interpolation point.

[0036] Furthermore, the calculation expression of the horizontal velocity of the interpolation point is:

[0037]

[0038] in, For the The horizontal velocity of the interpolation point, is the time difference between two adjacent motion trajectory points, For the positive The horizontal velocity of the interpolation point, For the reverse The horizontal velocity of the interpolation point;

[0039] The calculation expression of the vertical velocity of the interpolation point is:

[0040]

[0041] in, For the The vertical velocity of the interpolation point, For the positive The vertical velocity of the interpolation point; For the reverse The vertical velocity of the interpolation point.

[0042] Furthermore, in step S4,

[0043] The calculation expression of the horizontal flow velocity at the interpolation point is:

[0044]

[0045] in, For the The horizontal flow velocity at the interpolation point, for The horizontal flow velocity at time, is the time difference between two adjacent motion trajectory points, is the interpolation time interval, for Horizontal flow velocity at the moment;

[0046] The calculation expression of the ocean current direction at the interpolation point is:

[0047]

[0048] in, For the The current direction of the interpolation point, for The direction of the ocean current at all times, for The direction of the ocean current at every moment;

[0049] The calculation expression of the vertical flow velocity at the interpolation point is:

[0050]

[0051] in, For the The vertical velocity at the interpolation point, for The vertical flow velocity at the moment, for Vertical flow velocity at the moment.

[0052] Furthermore, in step S5,

[0053] The calculation expression for the positive horizontal position of the interpolation point is:

[0054]

[0055]

[0056] in, For the The positive x-direction component of the horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The x-direction component of the horizontal velocity of the interpolation point, For the The x-direction component of the horizontal velocity at the interpolation point, is the interpolation time interval; For the The positive horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment, For the The positive y-direction component of the horizontal velocity of the interpolation point, For the The y-direction component of the horizontal flow velocity at the interpolation point;

[0057] The calculation expression for the positive vertical position of the interpolation point is:

[0058]

[0059] in, For the The positive vertical position of the interpolation point, for The vertical position at the moment, For the The vertical velocity of the interpolation point, For the The vertical velocity of the interpolation point;

[0060] The calculation expression for the reverse horizontal position of the interpolation point is:

[0061]

[0062]

[0063] in, For the The x-direction component of the reverse horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The reverse horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment;

[0064] The calculation expression for the reverse vertical position of the interpolation point is:

[0065]

[0066] in, For the The reverse vertical position of the interpolation point, for The vertical position at the moment.

[0067] Furthermore, in step S6,

[0068]

[0069]

[0070]

[0071] in, For the The x-direction component of the horizontal position of the interpolation point, For the The y-direction component of the horizontal position of the interpolation point, For the The vertical position of the interpolation point.

[0072] The present invention further provides a three-dimensional underwater motion trajectory interpolation system considering the influence of ocean currents, which is used to execute the above-mentioned three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents, comprising:

[0073] A trajectory interpolation point calculation module, which determines two adjacent motion trajectory points that need to be interpolated based on the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculates the number of interpolation points and the interpolation point time;

[0074] An interpolation point heading calculation module is configured to calculate a turning rate between two adjacent motion trajectory points to be interpolated based on a heading difference and a time difference between the two adjacent motion trajectory points to be interpolated, and calculate a heading of the interpolation point based on the turning rate;

[0075] An interpolation point velocity calculation module, which calculates the horizontal velocity and the vertical velocity of the interpolation point according to the accelerations of two adjacent motion trajectory points to be interpolated;

[0076] an interpolation point ocean current parameter calculation module, which fuses the horizontal flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal flow velocity of the interpolation point, fuses the ocean current directions of two adjacent motion trajectory points that need to be interpolated according to time to obtain the ocean current direction of the interpolation point, and fuses the vertical flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical flow velocity of the interpolation point;

[0077] an interpolation point position estimation module, the interpolation point position estimation module decomposing the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposing the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, estimating the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and estimating the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point;

[0078] an interpolation point position calculation module, the interpolation point position calculation module calculating the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculating the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point;

[0079] The interpolation module repeats steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and completes the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points.

[0080] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0081] During the interpolation process, the present invention fully considers ocean currents, a key influencing factor in the underwater environment, and combines the underwater vehicle's own motion characteristics such as speed and heading. This allows the interpolated trajectory points to more realistically reflect the motion state of the underwater vehicle in the actual ocean environment, greatly reducing trajectory deviations caused by ignoring factors such as ocean currents. Through heading interpolation, speed interpolation, flow rate interpolation, two-way position estimation and fusion, the corresponding relationship between time and trajectory points is accurately established, forming a motion trajectory with a uniform time series, providing a reliable data foundation for subsequent applications such as trajectory fusion and playback.

[0082] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0084] Figure 1 This is a flow chart of a three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents provided by the present invention.

[0085] Figure 2 This is a structural diagram of a three-dimensional underwater motion trajectory interpolation system considering the influence of ocean currents provided by the present invention.

[0086] Reference numerals:

[0087] 101. Trajectory interpolation point calculation module; 102. Interpolation point heading calculation module; 103. Interpolation point speed calculation module; 104. Interpolation point ocean current parameter calculation module; 105. Interpolation point position calculation module; 106. Interpolation point position calculation module; 107. Interpolation module. DETAILED DESCRIPTION

[0088] To make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. Obviously, the embodiments described are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0089] In the description of the embodiments of the present invention, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiments of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0090] The following combination Figure 1 and Figure 2 The present invention describes a three-dimensional underwater motion trajectory interpolation method and system taking into account the influence of ocean currents.

[0091] The underwater vehicle is equipped with a positioning sensor that can measure and send out motion information such as longitude, latitude, depth, horizontal speed, heading, vertical speed, and time. The installed current sensor can measure the current information of the current point, and can measure the current direction, horizontal flow rate, and vertical flow rate.

[0092] underwater vehicles Time location , , for Longitude at the moment, for The latitude of time, for The depth of the moment, The horizontal velocity at time , The vertical velocity at time , The heading at the moment is , The horizontal flow velocity at time , The vertical flow velocity at the moment is , Current direction at all times ;

[0093] Time location , , for Longitude at the moment, for The latitude of time, for The depth of the moment, The horizontal velocity at time , The vertical velocity at time , The heading at the moment is , The horizontal flow velocity at time , The vertical flow velocity at the moment is , The current direction at this moment is ;

[0094] Time location , , for Longitude at the moment, for The latitude of time, for The depth of the moment, The horizontal velocity at time , The vertical velocity at time , The heading at the moment is , The horizontal flow velocity at time , The vertical flow velocity at the moment is , The current direction at this moment is ;

[0095] Time location , , for Longitude at the moment, for The latitude of time, for The depth of the moment, Horizontal speed at time , Vertical speed at the moment , The direction of the moment , Horizontal flow velocity at the moment , Vertical flow velocity at the moment , Current direction at all times .

[0096] like Figure 1 As shown in FIG, a three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents includes:

[0097] S1: Determine two adjacent motion trajectory points that need to be interpolated according to the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculate the number of interpolation points and the interpolation point time;

[0098] Calculate the time difference between two adjacent motion trajectory points. If the time difference between two adjacent motion trajectory points is less than or equal to the interpolation time interval, no interpolation is required.

[0099] If the time difference between two adjacent motion trajectory points is greater than the interpolation time interval, interpolation is required. The time difference is divided by the interpolation time interval and rounded up to obtain the number of interpolation points.

[0100] The calculation expression for the number of interpolation points is:

[0101]

[0102] in, is the number of interpolation points, is the time difference between two adjacent motion trajectory points, is the interpolation time interval, is the ceiling function;

[0103] The calculation expression of the interpolation point time is:

[0104]

[0105] in, For the interpolation point time, is the time of the first motion trajectory point.

[0106] In some specific embodiments of the present invention,

[0107] assumed time, The time difference between the moments is , no interpolation is required; time, The time difference between the moments is , no interpolation is required;

[0108] time, Interpolation between moments , , interpolation is required.

[0109] The geographic coordinates of the underwater vehicle at each moment are converted to three-dimensional rectangular Cartesian coordinates. The conversion formula is as follows:

[0110] Convert longitude to radians and the calculation expression is:

[0111]

[0112] in, is the longitude information, is the longitude;

[0113] Convert latitude to radians, the calculation expression is:

[0114]

[0115] in, is the latitude information, is latitude;

[0116] Equatorial radius :

[0117] Polar radius :

[0118] The calculation expression of flattening is:

[0119]

[0120] in, is the flatness rate;

[0121] The calculation expression of the first eccentricity is:

[0122]

[0123] in, is the first eccentricity;

[0124] The calculation expression of the curvature radius of the unitary circle of the reference ellipsoid is:

[0125]

[0126] in, is the radius of curvature of the reference ellipsoid, To find the square root;

[0127]

[0128]

[0129]

[0130] in, In the rectangular coordinate system coordinate, In the rectangular coordinate system coordinate, In the rectangular coordinate system coordinate, is the depth information, is the latitude information, Longitude information.

[0131] S2: Calculate the turning rate between the two adjacent motion trajectory points that need to be interpolated based on the heading difference and time difference between the two adjacent motion trajectory points that need to be interpolated, and calculate the heading of the interpolation point based on the turning rate;

[0132] S21: Calculate the heading difference between two adjacent motion trajectory points that need to be interpolated. If the heading difference is less than or equal to , then the heading does not need to be interpolated;

[0133] If the heading difference is greater than , then the turning rate between two adjacent motion trajectory points is obtained by dividing the heading difference by the time difference. The calculation expression is:

[0134]

[0135] in, for Moment and The turning rate between moments, is the heading difference;

[0136] In some specific embodiments of the present invention, .

[0137] S22: Calculate the heading of the interpolation point based on the turning rate between two adjacent motion trajectory points. The calculation expression is:

[0138]

[0139] in, For the The heading of the interpolation points, is the heading of the first motion trajectory point, is the turning rate between two adjacent motion trajectory points, is the interpolation time interval;

[0140] like , then reduce ,like , then add .

[0141] S3: Calculate the horizontal velocity and vertical velocity of the interpolation point based on the acceleration of the two adjacent motion trajectory points to be interpolated;

[0142] The interpolation of velocity is achieved by interpolating the acceleration of two adjacent points within the time period. When it is large, using only one side's acceleration calculation may produce a large deviation. Therefore, the acceleration at both ends is calculated, and the horizontal velocity of the interpolation point and the vertical velocity of the interpolation point are calculated forward and reverse respectively.

[0143] S311: According to The horizontal speed at the moment and The horizontal velocity at the moment is used to calculate the positive horizontal acceleration. The calculation expression is:

[0144]

[0145] in, is the positive horizontal acceleration, for The horizontal velocity at the moment, for Horizontal velocity at a moment;

[0146] S312: According to the positive horizontal acceleration and The horizontal velocity at the moment is used to calculate the horizontal velocity of the forward interpolation point. The calculation expression is:

[0147]

[0148] in, For the positive The horizontal velocity of the interpolation point;

[0149] S313: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the positive vertical acceleration. The calculation expression is:

[0150]

[0151] in, for Vertical acceleration at any moment, for The vertical speed at the moment, for Vertical speed at the moment;

[0152] S314: According to the positive vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the forward interpolation point. The calculation expression is:

[0153]

[0154] in, For the positive The vertical velocity of the interpolation point;

[0155] S321: According to The horizontal speed at the moment and The horizontal velocity at the moment is used to calculate the reverse horizontal acceleration. The calculation expression is:

[0156]

[0157] in, is the reverse horizontal acceleration, for The horizontal velocity at the moment, for Horizontal velocity at a moment;

[0158] S322: According to the reverse horizontal acceleration and The horizontal velocity at the moment is used to calculate the horizontal velocity of the reverse interpolation point. The calculation expression is:

[0159]

[0160] in, For the reverse The horizontal velocity of the interpolation point;

[0161] S323: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the reverse vertical acceleration. The calculation expression is:

[0162]

[0163] in, is the reverse vertical acceleration, for The vertical speed at the moment, for Vertical speed at the moment;

[0164] S324: According to the reverse vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the reverse interpolation point. The calculation expression is:

[0165]

[0166] in, For the reverse The vertical velocity of the interpolation point;

[0167] The calculation expression of the horizontal velocity of the interpolation point is:

[0168]

[0169] in, For the The horizontal velocity of the interpolation point;

[0170] The calculation expression of the vertical velocity of the interpolation point is:

[0171]

[0172] in, For the The vertical velocity of the interpolation point.

[0173] S4: fusing the horizontal flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal flow velocity of the interpolation point, fusing the ocean current directions of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the ocean current direction of the interpolation point, and fusing the vertical flow velocities of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical flow velocity of the interpolation point;

[0174] The calculation expression of the horizontal flow velocity at the interpolation point is:

[0175]

[0176] in, For the The horizontal flow velocity at the interpolation point, for The horizontal flow velocity at time, for Horizontal flow velocity at the moment;

[0177] The calculation expression of the ocean current direction at the interpolation point is:

[0178]

[0179] in, For the The current direction of the interpolation point, for The direction of the ocean current at all times, for The direction of the ocean current at every moment;

[0180] The calculation expression of the vertical flow velocity at the interpolation point is:

[0181]

[0182] in, For the The vertical velocity at the interpolation point, for The vertical flow velocity at the moment, for Vertical flow velocity at the moment.

[0183] S5: decomposing the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposing the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, calculating the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and calculating the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point;

[0184] The horizontal velocity of the interpolation point is decomposed according to the heading of the interpolation point, and the calculation expression is:

[0185]

[0186]

[0187] in, is the x-direction component of the horizontal velocity of the interpolation point, is the y-direction component of the horizontal velocity of the interpolation point;

[0188] The horizontal velocity at the interpolation point is decomposed according to the current direction at the interpolation point, and the calculation expression is:

[0189]

[0190]

[0191] in, For the The x-direction component of the horizontal velocity at the interpolation point, For the The y-direction component of the horizontal flow velocity at the interpolation point;

[0192] The calculation expression for the positive horizontal position of the interpolation point is:

[0193]

[0194]

[0195] in, For the The positive x-direction component of the horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The x-direction component of the horizontal velocity of the interpolation point, is the interpolation time interval; For the The positive horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment, For the The positive y-direction component of the horizontal velocity of the interpolation point;

[0196] The calculation expression for the positive vertical position of the interpolation point is:

[0197]

[0198] in, For the The positive vertical position of the interpolation point, for The vertical position at the moment, For the The vertical velocity of the interpolation point, For the The vertical velocity of the interpolation point;

[0199] The calculation expression for the reverse horizontal position of the interpolation point is:

[0200]

[0201]

[0202] in, For the The x-direction component of the reverse horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The reverse horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment;

[0203] The calculation expression for the reverse vertical position of the interpolation point is:

[0204]

[0205] in, For the The reverse vertical position of the interpolation point, for The vertical position at the moment.

[0206] S6: Calculating the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculating the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point;

[0207]

[0208]

[0209]

[0210] in, For the The x-direction component of the horizontal position of the interpolation point, For the The y-direction component of the horizontal position of the interpolation point, For the The vertical position of the interpolation point, is the positive weight, It is an inverse weight, the closer the time is, the greater the weight is.

[0211] S7: Repeat steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and complete the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points;

[0212] Repeat steps S2 to S6 according to the number of interpolation points to solve the horizontal and vertical positions of all interpolation points and convert them into geographic coordinates.

[0213] Convert three-dimensional Cartesian coordinates to geographic coordinates using the following conversion formula:

[0214]

[0215] in, is the distance auxiliary variable, To find the square root;

[0216]

[0217] in, is the initial latitude, To calculate the arc tangent function of two variables;

[0218]

[0219] in, is the second eccentricity;

[0220]

[0221] in, is the inverse tangent function;

[0222]

[0223]

[0224] .

[0225] Between two adjacent motion trajectory points that need to be interpolated, the interpolation point time as well as the horizontal and vertical positions of the interpolation points are filled in, thereby realizing a complete and continuous interpolation process between two adjacent motion trajectory points and constructing a smooth and accurate motion trajectory.

[0226] like Figure 2 As shown, a three-dimensional underwater motion trajectory interpolation system considering the influence of ocean currents is used to perform a three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents, including:

[0227] The trajectory interpolation point calculation module 101 determines two adjacent motion trajectory points that need to be interpolated according to the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculates the number of interpolation points and the interpolation point time;

[0228] The interpolation point heading calculation module 102 calculates the turning rate between the two adjacent motion trajectory points to be interpolated based on the heading difference and time difference between the two adjacent motion trajectory points to be interpolated, and calculates the heading of the interpolation point based on the turning rate;

[0229] The interpolation point velocity calculation module 103 calculates the horizontal velocity and the vertical velocity of the interpolation point according to the accelerations of the two adjacent motion trajectory points to be interpolated;

[0230] The interpolation point current parameter calculation module 104 fuses the horizontal velocity of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal velocity of the interpolation point, fuses the current direction of two adjacent motion trajectory points that need to be interpolated according to time to obtain the current direction of the interpolation point, and fuses the vertical velocity of two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical velocity of the interpolation point;

[0231] The interpolation point position estimation module 105 decomposes the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposes the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, estimates the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and estimates the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point;

[0232] The interpolation point position calculation module 106 calculates the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculates the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point;

[0233] The interpolation module 107 repeats steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and completes the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points.

[0234] Through the collaborative work of the above modules, the interpolation process fully considers the ocean current, a key influencing factor in the underwater environment. At the same time, combined with the underwater vehicle's own motion characteristics such as speed and heading, the interpolated trajectory points can more realistically reflect the motion state of the underwater vehicle in the actual ocean environment, greatly reducing the trajectory deviation caused by ignoring factors such as ocean currents. Through heading interpolation, speed interpolation, flow rate interpolation, two-way position estimation and fusion, the correspondence between time and trajectory points is accurately established, forming a uniform motion trajectory in time series, providing a reliable data foundation for subsequent applications such as trajectory fusion and playback.

[0235] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents, characterized in that: include: S1: Determine two adjacent motion trajectory points that need to be interpolated according to the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculate the number of interpolation points and the interpolation point time; S2: Calculate the turning rate between the two adjacent motion trajectory points that need to be interpolated based on the heading difference and time difference between the two adjacent motion trajectory points that need to be interpolated, and calculate the heading of the interpolation point based on the turning rate; S3: Calculate the horizontal velocity and vertical velocity of the interpolation point based on the acceleration of the two adjacent motion trajectory points to be interpolated; S4: fusing the horizontal flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal flow velocity of the interpolation point, fusing the ocean current directions of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the ocean current direction of the interpolation point, and fusing the vertical flow velocities of the two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical flow velocity of the interpolation point; S5: decomposing the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposing the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, calculating the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and calculating the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point; S6: Calculating the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculating the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point; S7: Repeat steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and complete the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points.

2. A three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 1, characterized in that: In step S1, Calculate the time difference between two adjacent motion trajectory points. If the time difference between two adjacent motion trajectory points is less than or equal to the interpolation time interval, no interpolation is required. If the time difference between two adjacent motion trajectory points is greater than the interpolation time interval, the time difference is divided by the interpolation time interval and rounded up to obtain the number of interpolation points; The calculation expression of the interpolation point time is: in, For the interpolation point time, is the time of the first motion trajectory point, is the interpolation time interval.

3. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 1 is characterized in that: Step S2 includes: S21: Calculate the heading difference between two adjacent motion trajectory points that need to be interpolated. If the heading difference is less than or equal to , then the heading does not need to be interpolated; If the heading difference is greater than , then the turning rate between two adjacent motion trajectory points is obtained by dividing the heading difference by the time difference; S22: Calculate the heading of the interpolation point based on the turning rate between two adjacent motion trajectory points. The calculation expression is: in, For the The heading of the interpolation points, is the heading of the first motion trajectory point, is the turning rate between two adjacent motion trajectory points, is the interpolation time interval; like , then reduce ,like , then add .

4. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 2 is characterized in that: The S3 steps include: S311: According to The horizontal speed at the moment and Calculate the positive horizontal acceleration from the horizontal velocity at the moment; S312: According to the positive horizontal acceleration and The horizontal velocity at the moment is calculated to obtain the horizontal velocity of the forward interpolation point; S313: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the positive vertical acceleration; S314: According to the positive vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the forward interpolation point.

5. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 4 is characterized in that: The S3 step also includes: S321: According to The horizontal speed at the moment and Calculate the reverse horizontal acceleration from the horizontal velocity at the moment; S322: According to the reverse horizontal acceleration and The horizontal velocity at the moment is calculated to obtain the horizontal velocity of the reverse interpolation point; S323: According to The vertical speed at the moment and The vertical velocity at the moment is used to calculate the reverse vertical acceleration; S324: According to the reverse vertical acceleration and The vertical velocity at the moment is used to calculate the vertical velocity of the reverse interpolation point.

6. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 5 is characterized in that: The calculation expression of the horizontal velocity of the interpolation point is: in, For the The horizontal velocity of the interpolation point, is the time difference between two adjacent motion trajectory points, For the positive The horizontal velocity of the interpolation point, For the reverse The horizontal velocity of the interpolation point; The calculation expression of the vertical velocity of the interpolation point is: in, For the The vertical velocity of the interpolation point, For the positive The vertical velocity of the interpolation point; For the reverse The vertical velocity of the interpolation point.

7. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 1 is characterized in that: In step S4, The calculation expression of the horizontal flow velocity at the interpolation point is: in, For the The horizontal flow velocity at the interpolation point, for The horizontal flow velocity at time, is the time difference between two adjacent motion trajectory points, is the interpolation time interval, for Horizontal flow velocity at the moment; The calculation expression of the ocean current direction at the interpolation point is: in, For the The current direction of the interpolation point, for The direction of the ocean current at all times, for The direction of the ocean current at every moment; The calculation expression of the vertical flow velocity at the interpolation point is: in, For the The vertical velocity at the interpolation point, for The vertical flow velocity at the moment, for Vertical flow velocity at the moment.

8. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 1 is characterized in that: In step S5, The calculation expression for the positive horizontal position of the interpolation point is: in, For the The positive x-direction component of the horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The x-direction component of the horizontal velocity of the interpolation point, For the The x-direction component of the horizontal velocity at the interpolation point, is the interpolation time interval; For the The positive horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment, For the The positive y-direction component of the horizontal velocity of the interpolation point, For the The y-direction component of the horizontal flow velocity at the interpolation point; The calculation expression for the positive vertical position of the interpolation point is: in, For the The positive vertical position of the interpolation point, for The vertical position at the moment, For the The vertical velocity of the interpolation point, For the The vertical velocity of the interpolation point; The calculation expression for the reverse horizontal position of the interpolation point is: in, For the The x-direction component of the reverse horizontal position of the interpolation point, for The x-direction component of the horizontal position at the moment, For the The reverse horizontal position y-direction component of the interpolation point, for The y-direction component of the horizontal position at the moment; The calculation expression for the reverse vertical position of the interpolation point is: in, For the The reverse vertical position of the interpolation point, for The vertical position at the moment.

9. The three-dimensional underwater motion trajectory interpolation method considering the influence of ocean currents according to claim 8, characterized in that: In step S6, in, For the The x-direction component of the horizontal position of the interpolation point, For the The y-direction component of the horizontal position of the interpolation point, For the The vertical position of the interpolation point.

10. A three-dimensional underwater motion trajectory interpolation system considering the influence of ocean currents, characterized in that: A method for interpolating a three-dimensional underwater motion trajectory considering the influence of ocean currents as claimed in any one of claims 1 to 9, comprising: A trajectory interpolation point calculation module, which determines two adjacent motion trajectory points that need to be interpolated based on the time difference between the two adjacent motion trajectory points and the interpolation time interval, and calculates the number of interpolation points and the interpolation point time; An interpolation point heading calculation module is configured to calculate a turning rate between two adjacent motion trajectory points to be interpolated based on a heading difference and a time difference between the two adjacent motion trajectory points to be interpolated, and calculate a heading of the interpolation point based on the turning rate; An interpolation point velocity calculation module, which calculates the horizontal velocity and the vertical velocity of the interpolation point according to the accelerations of two adjacent motion trajectory points to be interpolated; an interpolation point ocean current parameter calculation module, which fuses the horizontal flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the horizontal flow velocity of the interpolation point, fuses the ocean current directions of two adjacent motion trajectory points that need to be interpolated according to time to obtain the ocean current direction of the interpolation point, and fuses the vertical flow velocities of two adjacent motion trajectory points that need to be interpolated according to time to obtain the vertical flow velocity of the interpolation point; an interpolation point position estimation module, the interpolation point position estimation module decomposing the horizontal velocity of the interpolation point according to the heading of the interpolation point, decomposing the horizontal flow velocity of the interpolation point according to the current direction of the interpolation point, estimating the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point according to the decomposed horizontal velocity and the decomposed horizontal flow velocity, and estimating the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point according to the vertical velocity of the interpolation point and the vertical flow velocity of the interpolation point; an interpolation point position calculation module, the interpolation point position calculation module calculating the horizontal position of the interpolation point according to the forward horizontal position of the interpolation point and the reverse horizontal position of the interpolation point, and calculating the vertical position of the interpolation point according to the forward vertical position of the interpolation point and the reverse vertical position of the interpolation point; The interpolation module repeats steps S2 to S6 according to the number of interpolation points to obtain the horizontal and vertical positions of all interpolation points, and completes the interpolation between two adjacent motion trajectory points according to the interpolation point time and the horizontal and vertical positions of the interpolation points.

Images