A real-time position inference method of an autonomous underwater unmanned vehicle

Abstract

The application provides a real-time position deduction method of an autonomous underwater vehicle, and the method comprises the following steps: calculating the shortest distance from the current position of the autonomous underwater vehicle (AUV) to the current section according to the current position of the AUV and the information of the target navigation point; judging whether the shortest distance exceeds the distance threshold, and if yes, inserting a new navigation point on the navigation line; and outputting the longitude, latitude, navigation speed and heading angle information of the AUV at the next moment according to the input query information. The application can solve the technical problem that the current position information of the AUV cannot be obtained in time and accurately when the AUV executes a task and the shore-based equipment cannot communicate with the AUV, so that the real-time position of the AUV can be deduced by the shore-based equipment.

Description

Technical Field

[0001] This invention belongs to the field of underwater unmanned vehicle technology, and specifically relates to a method for real-time position estimation of an autonomous underwater unmanned vehicle. Background Technology

[0002] In the field of marine engineering, the operational process of Autonomous Underwater Vehicles (AUVs) follows a sequence of mission formulation, mission planning, AUV mission reception, AUV mission execution, and AUV recovery. Because AUVs are deep-sea operational equipment, they cannot maintain stable data transmission with shore-based equipment for extended periods during missions. This can result in shore-based equipment being unable to obtain the AUV's current location information for considerable periods, making it impossible to assess the AUV's mission progress. Therefore, it is necessary to simulate the AUV's offline location so that shore-based operators can know the AUV's current location information even when communication with the AUV is unavailable, thus gaining a general understanding of the AUV's mission status. Summary of the Invention

[0003] In view of this, the present invention provides a real-time position estimation method for autonomous underwater vehicles (AUVs) to solve the technical problem that shore-based equipment cannot obtain the current position information of the AUV in a timely and accurate manner when it is unable to communicate with the AUV during the AUV's mission, thereby enabling the shore-based equipment to estimate the real-time position of the AUV.

[0004] To solve the above-mentioned technical problems, the present invention is implemented as follows.

[0005] This invention provides a method for real-time position estimation of an autonomous underwater vehicle, comprising:

[0006] Calculate the shortest distance from the current position of the autonomous underwater vehicle (AUV) to the current route based on the information of the current position and the target waypoint;

[0007] Determine whether the shortest distance exceeds the distance threshold. If it does, insert a new waypoint on the route.

[0008] Based on the input query information, output the AUV's latitude and longitude, speed, and heading angle for the next moment.

[0009] Preferably, the information of the target waypoint includes: the sequence number of the target waypoint;

[0010] The calculation of the shortest distance between the AUV's current position and the current flight segment based on the AUV's current position and the target waypoint information includes:

[0011] Query the waypoint queue in the AUV mission file based on the target waypoint number to determine the latitude and longitude information of the start and end points of the current flight segment;

[0012] Calculate the distance d0 between the current position of the AUV and the starting point of the flight segment;

[0013] Calculate the current azimuth angle of the AUV based on the start of the flight segment.

[0014] Calculate the angle between the current flight segment's starting point and true north.

[0015] According to formula (4), the angle between the current position of the AUV and the current flight segment is calculated based on the starting point of the flight segment.

[0016]

[0017] Calculate the shortest distance d between the current position of the AUV and the current flight segment according to formula (5). min :

[0018]

[0019] Preferably, the calculation of the distance d0 between the current position of the AUV and the starting point of the flight segment includes: calculating according to formula (1):

[0020]

[0021] in,

[0022] lon is the longitude of the AUV's current location;

[0023] lat is the latitude of the current location of the AUV;

[0024] lon0 is the longitude of the starting point of the flight segment;

[0025] lat0 is the latitude of the starting point of the flight segment;

[0026] dx_auv0 is the distance between the current position of the AUV and the starting point of the flight segment along the x-axis.

[0027] dy_auv0 is the distance between the current position of the AUV and the starting point of the flight segment along the y-axis.

[0028] The calculation of the AUV is based on the current azimuth angle at the start of the flight segment. Includes: Calculation according to formula (2):

[0029]

[0030] The calculation involves determining the angle between the current flight segment's starting point and true north. Includes: Calculation according to formula (3):

[0031]

[0032] in:

[0033] lon1 is the longitude of the end point of the flight segment;

[0034] lat1 is the latitude of the end point of the flight segment;

[0035] lon0 is the longitude of the starting point of the flight segment;

[0036] lat0 is the latitude of the starting point of the flight segment;

[0037] dx_k1 is the distance between the end point and the start point of the segment along the x-axis;

[0038] dy_k1 is the distance between the end point and the start point of the segment along the y-axis.

[0039] Preferably, inserting a new waypoint on the route includes: calculating the latitude and longitude of the new waypoint, and inserting the latitude and longitude of the new waypoint before the target waypoint in the waypoint queue of the AUV mission file.

[0040] Preferably, calculating the latitude and longitude of the new waypoint includes:

[0041] Calculate the distance d1 between the current position of the AUV and the end point of the flight segment;

[0042] Calculate the azimuth angle of the AUV based on the end of the flight segment.

[0043] Calculate the angle between the current flight segment and true north at the end of the flight segment.

[0044] The intersection of the extension of the line connecting the current position of the AUV to the end of the segment and the line extending from the current position of the AUV to the perpendicular line of the route with the current segment is taken as the new waypoint. The distance d from the current position of the AUV to the new waypoint is calculated according to formula (9). c :

[0045]

[0046] Calculate the latitude and longitude of the new waypoint according to formula (10):

[0047]

[0048] in,

[0049] lon c The longitude of the new waypoint;

[0050] lat c The latitude of the new waypoint.

[0051] Preferably, the calculation of the distance d1 between the current position of the AUV and the end point of the flight segment includes: calculating according to formula (6):

[0052]

[0053] in,

[0054] lon is the longitude of the AUV's current location;

[0055] lat is the latitude of the current location of the AUV;

[0056] lon1 is the longitude of the end point of the flight segment;

[0057] lat1 is the latitude of the end point of the flight segment;

[0058] dx_auv1 is the distance between the current position of the AUV and the end point of the flight segment along the x-axis.

[0059] dy_auv1 is the distance between the current position of the AUV and the end point of the flight segment along the y-axis.

[0060] The calculation of the AUV is based on the azimuth angle at the end of the flight segment. Includes: Calculation according to formula (7):

[0061]

[0062] The calculation involves determining the angle between the current flight segment's endpoint and true north. Includes: Calculation according to formula (8):

[0063]

[0064] in:

[0065] lon1 is the longitude of the end point of the flight segment;

[0066] lat1 is the latitude of the end point of the flight segment;

[0067] lon0 is the longitude of the starting point of the flight segment;

[0068] lat0 is the latitude of the starting point of the flight segment;

[0069] dx_k1 is the distance between the end point and the start point of the segment along the x-axis;

[0070] dy_k1 is the distance between the end point and the start point of the segment along the y-axis.

[0071] It is the angle between the current flight segment's starting point and due north.

[0072] Preferably, the method further includes: calculating the distance and heading angle of each segment in the waypoint queue according to formula (11):

[0073]

[0074] in,

[0075] i is the waypoint number;

[0076] lon i The longitude of waypoint i;

[0077] lat i Let i be the latitude of waypoint i;

[0078] dx_k i Let be the distance between the start and end points of segment i along the x-axis.

[0079] dy_k i Let be the distance between the start and end points of segment i along the y-axis.

[0080] Let be the heading angle of segment i.

[0081] Preferably, the step of outputting the AUV's latitude, longitude, speed, and heading angle information for the next moment based on the input query information includes:

[0082] Calculate according to formula (12):

[0083]

[0084] in,

[0085] i is the input waypoint number;

[0086] lon i The longitude of waypoint i;

[0087] lat i Let i be the latitude of waypoint i;

[0088] v i Let i be the speed of travel segment i;

[0089] Let be the heading angle of segment i.

[0090] In summary, the method provided by this invention uses a mathematical model to extrapolate the real-time underwater position of an AUV based on waypoint information in the AUV mission file. This solves the problem of not being able to obtain AUV position information in a timely manner during AUV missions, enabling shore-based equipment operators to know the AUV's current position while the AUV is performing underwater missions. The technical solution provided by this invention does not rely on complex equipment and instruments, has good adaptability, and is simple and economical to implement.

[0091] Specifically, by adopting the technical solution provided by this invention, the following beneficial effects can be achieved:

[0092] (1) The real-time position estimation method for autonomous underwater vehicles (AUVs) provided by this invention first calculates the shortest distance between the current position of the AUV and the current flight segment based on the current position of the AUV and the target waypoint information; then it determines whether the shortest distance exceeds a distance threshold, and if it does, inserts a new waypoint on the flight path; if the AUV deviates from the current flight segment but does not exceed the set distance threshold, no new waypoint is inserted. In this way, when the AUV deviates from the current flight segment beyond the set distance threshold, the inserted new waypoint allows the output of the AUV's latitude, longitude, speed, and heading angle information at the next moment based on the input query information in the next step, making the real-time position estimation of the AUV by the shore-based equipment closer to the AUV's actual position, thereby improving the accuracy of the position estimation. However, when the AUV deviates from the current flight segment but does not exceed the set distance threshold, no new waypoint is inserted, and the original waypoint queue in the AUV mission file is still used to estimate the AUV's position.

[0093] (2) This invention determines the latitude and longitude information of the starting point and ending point of the current voyage by querying the waypoint queue in the AUV mission file according to the sequence number of the target waypoint. It realizes the calculation of the real-time underwater position of the AUV by using a mathematical model based on the waypoint information in the AUV mission file, thereby determining the shortest distance between the current position of the AUV and the current voyage. This provides a basis for judging whether a new waypoint needs to be inserted on the route, making the subsequent real-time position calculation more accurate.

[0094] (3) The technical solution provided by the present invention does not rely on complex equipment and instruments, but uses mathematical models to deduce the real-time underwater position of AUV, which has good adaptability and is simple and economical to implement.

[0095] (4) When it is necessary to insert a new waypoint on the route, the present invention calculates the distance d1 between the current position of the AUV and the end point of the segment, and the azimuth angle of the AUV based on the end point of the segment. The angle between the current flight segment and due north at the end of the flight segment. The new waypoint is the intersection of the extension of the line connecting the current AUV's current position to the end of the segment and the extension of the line connecting the current AUV's current position to the perpendicular from the current AUV's current position to the route, and the line connecting the current AUV's current position to the current route. The distance d from the current AUV's current position to the new waypoint is then calculated. c This allows for the calculation of the latitude and longitude of new waypoints, enabling the inserted waypoints to assist in subsequent real-time position projection, thereby improving the accuracy of real-time position projection. Attached Figure Description

[0096] Figure 1 This is a schematic diagram illustrating the principle of a real-time position estimation method for an autonomous underwater vehicle according to an embodiment of this application.

[0097] Figure 2 This is a flowchart illustrating a real-time position estimation method for an autonomous underwater vehicle according to an embodiment of the present invention.

[0098] Figure 3 This is a schematic diagram of the relevant physical quantities of the current position and current flight segment of the vehicle in an embodiment of the present invention.

[0099] Figure 4 This is a schematic diagram of the relevant physical quantities of the current position and current flight segment of the vehicle in an embodiment of the present invention. Detailed Implementation

[0100] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0101] Figure 1 This is a schematic diagram illustrating the principle of a real-time position estimation method for an autonomous underwater vehicle according to an embodiment of this application. The method includes the following steps:

[0102] Step 101: Calculate the shortest distance between the AUV's current location and the current flight segment based on the AUV's current location and the target waypoint information;

[0103] Step 102: Determine whether the shortest distance exceeds the distance threshold. If it does, insert a new waypoint on the route.

[0104] Step 103: Output the latitude, longitude, speed and heading angle of the AUV at the next moment based on the input query information.

[0105] In step 101, the target waypoint information may include: the target waypoint number; thus, the waypoint queue in the AUV mission file can be queried based on the target waypoint number to determine the latitude and longitude information of the start and end points of the current segment, and then the shortest distance from the current position of the AUV to the current segment can be calculated in the following way:

[0106] Calculate the distance d0 between the current position of the AUV and the starting point of the flight segment;

[0107] Calculate the current azimuth angle of the AUV based on the start of the flight segment.

[0108] Calculate the angle between the current flight segment's starting point and true north.

[0109] According to formula (4), the angle between the current position of the AUV and the current flight segment is calculated based on the starting point of the flight segment.

[0110]

[0111] Calculate the shortest distance d between the current position of the AUV and the current flight segment according to formula (5). min :

[0112]

[0113] Regarding d0, and The specific calculations will be combined later. Figure 2 and Figure 3 This will be explained in the description.

[0114] In step 102, when the calculated shortest distance between the current position of the AUV and the current flight segment exceeds a set distance threshold, a new waypoint can be inserted into the flight path to more accurately estimate the real-time position of the AUV. Specifically, inserting a new waypoint into the flight path includes: calculating the latitude and longitude of the new waypoint and inserting the latitude and longitude of the new waypoint before the target waypoint in the waypoint queue of the AUV mission file.

[0115] Specifically, calculating the latitude and longitude of the new waypoints includes:

[0116] Calculate the distance d1 between the current position of the AUV and the end point of the flight segment;

[0117] Calculate the azimuth angle of the AUV based on the end of the flight segment.

[0118] Calculate the angle between the current flight segment and true north at the end of the flight segment.

[0119] The intersection of the extension of the line connecting the current position of the AUV to the end of the segment and the line extending from the current position of the AUV to the perpendicular line of the route with the current segment is taken as the new waypoint. The distance d from the current position of the AUV to the new waypoint is calculated according to formula (9). c :

[0120]

[0121] Calculate the latitude and longitude of the new waypoint according to formula (10):

[0122]

[0123] in,

[0124] lon c The longitude of the new waypoint;

[0125] lat c The latitude of the new waypoint.

[0126] Regarding d1, and The specific calculations will be combined later. Figure 2 and Figure 4 This will be explained in the description.

[0127] Step 103 is a practical application of the present invention. The present invention can output the latitude, longitude, speed, and heading angle of the AUV at the next moment based on the input query information, specifically including:

[0128] Calculate according to formula (12):

[0129]

[0130] in,

[0131] i is the input waypoint number;

[0132] lon i The longitude of waypoint i;

[0133] lat i Let i be the latitude of waypoint i;

[0134] v i Let i be the speed of travel segment i;

[0135] Let be the heading angle of segment i.

[0136] The next moment can be the next second.

[0137] The following is combined Figures 2 to 4 The technical solution of this application will be further described in detail through a specific embodiment.

[0138] Figure 2 This is a flowchart illustrating a real-time position estimation method for an autonomous underwater vehicle according to an embodiment of the present invention. Figure 2 The method shown includes the following steps:

[0139] Step 201: When starting the AUV mission, input the latitude and longitude information of each waypoint in the AUV mission file to form a waypoint queue.

[0140] Waypoints are a series of pre-defined points, and the segments between these points are sequentially linked to form a complete AUV route from the starting point to the ending point. The AUV mission file contains the latitude and longitude information of each waypoint. Based on this information, the AUV departs from the starting point and navigates segment by segment, using each waypoint as the target waypoint, until it reaches the ending point. Each segment consists of two waypoints: a starting waypoint and a target waypoint. The starting waypoint can be called the starting point of the segment, and the target waypoint can be called the ending point.

[0141] Step 202: Determine if communication with the AUV is interrupted. If communication is interrupted, proceed to step 203. If communication is not interrupted, continue communicating with the AUV to obtain its information and continue with the determination in this step. Typically, this step is performed by setting a period or timer, or when needed.

[0142] Step 203: Enter the latitude and longitude information before the AUV communication was interrupted and the sequence number of the target waypoint before the AUV communication was interrupted.

[0143] As described in step 201, the AUV mission file contains the latitude and longitude information of each waypoint. By determining the sequence number of the target waypoint before the AUV communication was interrupted, the relevant information of the current flight segment can be determined, including: the latitude and longitude information of the start and end points of the flight segment, the speed of navigation, the heading angle, etc.

[0144] Step 204: Use the latitude and longitude information before the AUV communication was interrupted as the current position of the AUV, and calculate the shortest distance between the current position of the AUV and the current flight segment.

[0145] Figure 3 This is a schematic diagram illustrating the relevant physical quantities of the vehicle's current position and current flight segment in an embodiment of the present invention. See also... Figure 3 Specifically, you can follow these steps 2041-2045 (for simplicity, Figure 2 (Not shown in the image) Calculate the shortest distance from the current position of the AUV to the current flight segment.

[0146] Step 2041: Calculate the distance d0 between the current position of the AUV and the starting point of the flight segment. For example, it can be calculated according to formula (1) as follows:

[0147]

[0148] in,

[0149] lon is the longitude of the AUV's current location;

[0150] lat is the latitude of the current location of the AUV;

[0151] lon0 is the longitude of the starting point of the flight segment;

[0152] lat0 is the latitude of the starting point of the flight segment;

[0153] dx_auv0 is the distance between the current position of the AUV and the starting point of the flight segment along the x-axis.

[0154] dy_auv0 is the distance between the current position of the AUV and the starting point of the flight segment along the y-axis.

[0155] According to formula (1), the distance dx_auv0 between the current position of the AUV and the starting point of the segment can be calculated in the x-axis direction and the y-axis direction respectively based on the latitude and longitude of the current position of the AUV and the latitude and longitude of the starting point of the segment. Then, the distance d0 between the current position of the AUV and the starting point of the segment can be calculated.

[0156] Step 2042: Calculate the current azimuth angle of the AUV based on the start of the flight segment.

[0157] For example, determining based on the starting point of the flight segment, such as Figure 3 The dashed line indicates due north. Then, calculate the current azimuth of the AUV based on the starting point of the flight segment according to formula (2). as follows:

[0158]

[0159] Step 2043: Calculate the angle between the current flight segment's starting point and true north.

[0160] Here, the determination is also based on the starting point of the flight segment, such as... Figure 3 The dashed line indicates due north. Then, calculate the angle between the current flight segment's starting point and due north using formula (3).

[0161]

[0162] in:

[0163] lon1 is the longitude of the end point of the flight segment;

[0164] lat1 is the latitude of the end point of the flight segment;

[0165] lon0 is the longitude of the starting point of the flight segment;

[0166] lat0 is the latitude of the starting point of the flight segment;

[0167] dx_k1 is the distance between the end point and the start point of the segment along the x-axis;

[0168] dy_k1 is the distance between the end point and the start point of the segment along the y-axis.

[0169] According to formula (3), the distance dx_k1 between the end point and the beginning point of the flight segment can be calculated along the x-axis and the distance dy_k1 along the y-axis, respectively, based on the latitude and longitude of the end point and the beginning point of the flight segment. Then, the following can be calculated:

[0170] Step 2044: Based on the starting point of the flight segment, calculate the angle between the current position of the AUV and the current flight segment. Based on the preceding steps, the current azimuth angle of the AUV based on the start of the flight segment has been calculated. as well as See Figure 3 Using these two angles, the angle between the current position of the AUV and the current flight segment can be calculated according to formula (4).

[0171]

[0172] Step 2045: Calculate the shortest distance d between the current position of the AUV and the current flight segment. min .

[0173] See Figure 3 Based on the previously calculated distance d0 between the current position of the AUV and the starting point of the flight segment, and the angle between the current position of the AUV and the flight path... The shortest distance d between the current position of the AUV and the flight path can be calculated using formula (5). min :

[0174]

[0175] Step 205: Determine whether the shortest distance between the current position of the AUV and the route exceeds the distance threshold. If it exceeds the distance threshold, determine that a new waypoint needs to be inserted on the route and continue to step 206. Otherwise, no new waypoint needs to be inserted and skip to step 208.

[0176] For example, the distance threshold can be set to 50 meters. In practical applications, different threshold values ​​can be set according to the required level of accuracy.

[0177] Step 206: Calculate the latitude and longitude of the new waypoints to be inserted.

[0178] The new waypoints inserted in this invention are used by shore-based equipment to simulate the real-time position of the AUV, and are not actually inserted into the AUV mission file to change or adjust the AUV's navigation.

[0179] Figure 4 This is a schematic diagram illustrating the relevant physical quantities of the vehicle's current position and current flight segment in an embodiment of the present invention. See also... Figure 4 Specifically, you can follow these steps 2061-2065 (for simplicity, Figure 2 (Not shown in the image) Calculate the latitude and longitude of the new waypoints to be inserted.

[0180] Step 2061: Calculate the distance d1 between the current position of the AUV and the end point of the flight segment. For example, it can be calculated according to formula (6) as follows:

[0181]

[0182] in,

[0183] lon is the longitude of the AUV's current location;

[0184] lat is the latitude of the current location of the AUV;

[0185] lon1 is the longitude of the end point of the flight segment;

[0186] lat1 is the latitude of the end point of the flight segment;

[0187] dx_auv1 is the distance between the current position of the AUV and the end point of the flight segment along the x-axis.

[0188] dy_auv1 is the distance between the current position of the AUV and the end point of the flight segment along the y-axis.

[0189] According to formula (6), the distance dx_auv1 between the current position of the AUV and the end point of the flight segment can be calculated in the x-axis direction and the y-axis direction respectively based on the latitude and longitude of the current position of the AUV and the latitude and longitude of the end point of the flight segment. Then, the distance d1 between the current position of the AUV and the end point of the flight segment can be calculated.

[0190] Step 2063: Calculate the AUV's azimuth based on the end point of the flight segment.

[0191] like Figure 4 As shown, the determination is based on the end point of the flight segment. Figure 4 The dashed line indicates due north, and then the current azimuth of the AUV based on the end of the flight segment is calculated according to formula (7). as follows:

[0192]

[0193] Step 2063: Calculate the angle between the current flight segment and true north at the end of the flight segment.

[0194] Here, the determination is also based on the end point of the flight segment. Figure 4 The dashed line indicates due north. Then, calculate the angle between the current flight segment and due north at the end of the segment according to formula (8).

[0195]

[0196] In formula (8), the physical meanings of lon1, lat1, lon0, lat0, dx_k1, and dy_k1 are the same as in formula (3), and will not be repeated here. The calculated... Then, according to the relational formula able to obtain

[0197] Step 2064: The intersection of the extension of the line connecting the current AUV position to the end of the segment and the line extending from the current AUV position to the perpendicular line from the current AUV position to the route with the current segment position is taken as the new waypoint to be inserted. Calculate the distance d from the current AUV position to the new waypoint to be inserted. c For example, the calculation is as follows according to formula (9):

[0198]

[0199] Step 2065: Calculate the latitude and longitude of the new waypoint to be inserted. Based on the previously known conditions, the calculation can be performed according to formula (10):

[0200]

[0201] in,

[0202] lon c The longitude of the new waypoint to be inserted;

[0203] lat c The latitude of the new waypoint to be inserted.

[0204] Step 207: Store the latitude and longitude of the new waypoints to be inserted and the latitude and longitude of subsequent waypoints into the waypoint queue of the AUV mission file, and calculate the distance and heading angle of each segment according to formula (11).

[0205]

[0206] in,

[0207] i is the waypoint number;

[0208] loni The longitude of waypoint i;

[0209] lat i Let i be the latitude of waypoint i;

[0210] dx_k i Let be the distance between the start and end points of segment i along the x-axis.

[0211] dy_k i Let be the distance between the start and end points of segment i along the y-axis.

[0212] Let be the heading angle of segment i.

[0213] Step 208: When the user inputs the current target waypoint number of the AUV, determine the current flight segment of the AUV based on the input target waypoint number, and calculate the latitude and longitude of the AUV for the next second based on the heading angle of the flight segment. For example, it can be calculated as follows according to formula (12):

[0214]

[0215] Among them, v i Let be the speed of segment i. The physical meanings of the other physical quantities involved in formula (12) are as described above and will not be repeated here.

[0216] Step 209: Output the AUV's latitude, longitude, speed, and heading angle for the next second.

[0217] Thus, by employing the method provided by this invention, and using a mathematical model based on waypoint information in the AUV mission file, the real-time underwater position of the AUV can be calculated. This solves the problem of not being able to obtain AUV position information in a timely manner during AUV missions, enabling shore-based equipment operators to know the current position of the AUV while it is performing underwater missions. The technical solution provided by this invention does not rely on complex equipment and instruments, has good adaptability, and is simple and economical to implement.

Claims

1. A method for real-time position estimation of an autonomous underwater vehicle, characterized in that, include: Calculate the shortest distance from the current position of the autonomous underwater vehicle (AUV) to the current route based on the information of the current position and the target waypoint; Determine whether the shortest distance exceeds the distance threshold. If it does, insert a new waypoint on the route. Based on the input query information, output the AUV's latitude and longitude, speed, and heading angle for the next moment; The information of the target waypoint includes: the sequence number of the target waypoint; The calculation of the shortest distance between the AUV's current position and the current flight segment based on the AUV's current position and the target waypoint information includes: Query the waypoint queue in the AUV mission file based on the target waypoint number to determine the latitude and longitude information of the start and end points of the current flight segment; Calculate the distance between the AUV's current position and the start of the flight segment. ; Calculate the current azimuth angle of the AUV based on the start of the flight segment. ; Calculate the angle between the current flight segment's starting point and true north. ; According to formula (4), the angle between the current position of the AUV and the current flight segment is calculated based on the starting point of the flight segment. : Calculate the shortest distance between the current position of the AUV and the current flight segment according to formula (5). : The step of inserting a new waypoint on the route includes: calculating the latitude and longitude of the new waypoint, and inserting the latitude and longitude of the new waypoint before the target waypoint in the waypoint queue of the AUV mission file.

2. The method according to claim 1, characterized in that: The calculation of the distance between the AUV's current position and the start of the flight segment. Includes: Calculation according to formula (1): in, The longitude of the AUV's current location; The latitude of the AUV's current location; Longitude of the starting point of the flight segment; The latitude of the starting point of the flight segment; This represents the distance between the current position of the AUV and the starting point of the flight segment along the x-axis. This represents the distance between the current position of the AUV and the starting point of the flight segment along the y-axis. The calculation of the AUV is based on the current azimuth angle at the start of the flight segment. Includes: Calculation according to formula (2): The calculation involves determining the angle between the current flight segment's starting point and true north. Includes: Calculation according to formula (3): in: The longitude of the end point of the flight segment; The latitude of the end point of the flight segment; Longitude of the starting point of the flight segment; The latitude of the starting point of the flight segment; This represents the distance between the end point and the start point of the flight segment along the x-axis. This represents the distance between the end point and the start point of the flight segment along the y-axis.

3. The method according to claim 1, characterized in that, Calculating the latitude and longitude of the new waypoint includes: Calculate the distance between the AUV's current position and the end of the flight segment. ; Calculate the azimuth angle of the AUV based on the end of the flight segment. ; Calculate the angle between the current flight segment and true north at the end of the flight segment. ; The intersection of the extension of the line connecting the current position of the AUV to the end of the segment and the line extending from the current position of the AUV to the perpendicular line of the route with the current segment is taken as the new waypoint. The distance from the current position of the AUV to the new waypoint is calculated according to formula (9). : Calculate the latitude and longitude of the new waypoint according to formula (10): in, The longitude of the new waypoint; The latitude of the new waypoint.

4. The method according to claim 3, characterized in that: The calculation of the distance between the AUV's current position and the end of the flight segment. Includes: Calculation according to formula (6): in, The longitude of the AUV's current location; The latitude of the AUV's current location; The longitude of the end point of the flight segment; The latitude of the end point of the flight segment; This represents the distance between the current position of the AUV and the end point of the flight segment along the x-axis. This represents the distance between the current position of the AUV and the end point of the flight segment along the y-axis. The calculation of the AUV is based on the azimuth angle at the end of the flight segment. Includes: Calculation according to formula (7): The calculation involves determining the angle between the current flight segment's endpoint and true north. Includes: Calculation according to formula (8): in: The longitude of the end point of the flight segment; The latitude of the end point of the flight segment; Longitude of the starting point of the flight segment; The latitude of the starting point of the flight segment; This represents the distance between the end point and the start point of the flight segment along the x-axis. This represents the distance between the end point and the start point of the flight segment along the y-axis. It is the angle between the current flight segment's starting point and due north.

5. The method according to claim 3, characterized in that, The method further includes: calculating the distance and heading angle of each segment in the waypoint queue according to formula (11): in, i is the waypoint number; The longitude of waypoint i; Let i be the latitude of waypoint i; Let be the distance between the start and end points of segment i along the x-axis. Let be the distance between the start and end points of segment i along the y-axis. Let be the heading angle of segment i.

6. The method according to claim 3, characterized in that, The step of outputting the AUV's latitude, longitude, speed, and heading angle information for the next moment based on the input query information includes: Calculate according to formula (12): in, i is the input waypoint number; The longitude of waypoint i; Let i be the latitude of waypoint i; Let i be the speed of travel segment i; Let be the heading angle of segment i.

Images