A method for rapid route planning of unmanned surface vessels / ships (USVs)

Abstract

This invention relates to a rapid planning method for unmanned surface vessels (USVs) swarm formations. Based on a planned reference route and swarm formation parameters, this invention quickly generates swarm member routes for multi-track guidance and tracking control, while also providing situational awareness for command and control personnel, thus improving the computational efficiency of swarm mission planning. The advantage of this invention lies in its direct and rapid calculation of swarm member routes using a reference route for guidance and tracking control. Compared to single-track guidance and tracking, this method provides a more intuitive swarm situational reference, enabling swarm members to maintain formation even after losing leader movement reference information, thereby effectively improving the robustness of the swarm system.

Description

Technical Field

[0001] This invention belongs to the field of unmanned surface vessel (USV) path planning technology, and in particular, a method for rapid planning of unmanned surface vessel (USV) formation routes. Background Technology

[0002] In the highly dynamic and uncertain marine environment, individual unmanned surface vessels (USVs) often face limitations in their operational capabilities and low efficiency when facing various complex tasks. Marine vehicles are showing a trend towards intelligentization, networking, and clustering. Collaborating multiple USVs to achieve clustered operations can greatly improve operational efficiency and enable the execution of various more complex tasks.

[0003] The key to achieving coordinated swarm operations among multiple unmanned surface vessels (USVs) lies in the rapid planning of swarm routes. Methods for solving this problem generally fall into two categories: coupled and decoupled planning methods. Coupled path planning treats the swarm of multiple USVs as a single entity, performing search and planning. Common search methods include Artificial Potential Field (APF), Probabilistic Route Map (PRM), and Random Tree Quick Search (RRT). These methods typically offer high computational efficiency but neglect the attitude changes of individual members and their relative motions. Decoupled path planning, on the other hand, plans independent paths for each robot and then adjusts and modifies these independent paths through conflict point coordination. Common methods include heuristic algorithms, conflict-based search algorithms, and methods based on reserved regions. These algorithms are more effective at resolving conflicts within the swarm, but they are computationally intensive and complex, increasing the computational burden of coordination among swarm members. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method for rapid route planning of unmanned surface vessels / ships.

[0005] The above-mentioned objective of the present invention is achieved through the following technical solution:

[0006] A method for rapid route planning of unmanned surface vessels / ships (USVs) formations, characterized by the following steps:

[0007] Step 1: Specify a reference route and formation parameters, where the formation parameters include, but are not limited to, the number of routes to be generated, the route spacing, the formation type, and the azimuth.

[0008] Step 2: Perform a reference route verification to check whether the route crosses a navigation obstruction zone or restricted waters; if so, modify the original route and replan; otherwise, proceed to Step 3.

[0009] Step 3: Read the reference route and display parameters to obtain the chart display scale and projection method;

[0010] Step 4: Perform coordinate transformation on the list of waypoints included in the reference route, convert them to screen coordinates, and store the coordinates into the new list of waypoints in sequence;

[0011] Step 5: Determine whether the number of waypoints N0 in the screen coordinate reference route waypoint list is greater than or equal to 2. If yes, proceed to step 6; otherwise, output that the reference route is incomplete and modify the reference route specified in step 1.

[0012] Step 6: Determine if N0 is greater than or equal to 3; otherwise, proceed to Step 7; otherwise, proceed to Step 8.

[0013] Step 7: Calculate and generate a single-segment formation route based on all the formation parameters specified in Step 1, and store the generated single-segment formation route in the form of a waypoint list;

[0014] Step 8: Based on the formation parameters specified in Step 1, calculate and generate multi-segment formation routes, and store the generated multi-segment formation routes in the form of a waypoint list;

[0015] Step 9: Based on the parameters obtained in Step 3, perform coordinate transformation on the waypoint list obtained in Steps 7 and 8, convert the screen coordinates into latitude and longitude coordinates, and save them to the new latitude and longitude coordinate form of the waypoint list.

[0016] Step 10: Perform formation route verification. Check in turn whether the route of each member crosses a navigation obstruction area or restricted waters. If so, prompt the monitoring personnel to modify the route; otherwise, proceed to Step 11.

[0017] Step 11: Output formation flight path.

[0018] Moreover, the specific implementation method of step 1 is as follows: the cluster monitoring personnel manually plan or automatically generate a reference flight path through the remote control terminal. The formation parameters to be generated include the number of flight paths, the flight path interval, the formation type, and the azimuth angle, which are specified by the monitoring personnel through the remote control terminal.

[0019] Furthermore, step 2 uses the existing route analysis and verification module to perform initial route verification to determine whether the route crosses a navigation obstruction zone or restricted waters. Among these, navigation obstruction zones or restricted waters include, but are not limited to, various static obstacles marked on nautical charts: land, shoals, dry reefs, no-navigation zones, and fishing areas.

[0020] Furthermore, in step 3, the reference route is read by sequentially reading waypoints in latitude and longitude form, and the waypoints in latitude and longitude form are saved to a list; the display parameters read in step 3 include scale, projection method, and monitor specifications.

[0021] Furthermore, based on the latitude and longitude format waypoint list, scale, and display specification parameters obtained in step 4, the waypoints in the reference route waypoint list are subjected to coordinate transformation, and the screen coordinates of each waypoint are sequentially stored into the new reference route screen coordinate waypoint list.

[0022] Furthermore, step 7 includes the following sub-steps:

[0023] Step 7.1: Based on all the formation parameters specified in Step 1, determine whether the number of routes N to be generated is even. If so, calculate L = N-1 and R = N; otherwise, L = N and R = N-1. Please provide the meaning of the parameters.

[0024] Step 7.2: Based on the formation type and formation parameters set in Step 1 (please explain if needed), calculate the starting point of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route; the calculation process is as follows:

[0025] If the formation type is horizontal, azimuth, or V-formation, then the starting points of the l-th and r-th routes are calculated sequentially according to formulas (1) to (9); where (x 0,1 ,y 0,1 (x) represents the coordinates of the first waypoint of the reference route being translated. l,1 ,y l,1 ), l=1,...,L represents the coordinates of the first waypoint (origin) of the l-th formation route on the inner or upper side, where l is an odd number; (x r,1 ,y r,1 ), r=2,...,R are the coordinates of the first waypoint of the r-th route on the right or bottom side, where r is an even number; d is the distance from the reference route to the adjacent member route; α1 represents the angle between the first segment of the reference route and the x-axis; β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship.

[0026]

[0027]

[0028]

[0029]

[0030]

[0031] x l,1 =x 0,1 -ldsinα1+ldtan(βl -π / 2)cosα1 (6)

[0032] y l,1 =y 0,1 +ldcosα1+ldtan(β l -π / 2)sinα1 (7)

[0033] x r,1 =x 0,1 +rdsinα1-rdtan(β r -π / 2)cosα1 (8)

[0034] y r,1 =y 0,1 -rdcosα1-rdtan(β r -π / 2)sinα1 (9)

[0035] If the formation is in column, then the starting points of each route are calculated sequentially according to formulas (10) to (11); where, (x 0,1 ,y 0,1 (x) represents the coordinates of the first waypoint of the reference route being translated. n,1 ,y n,1 ), n=1,...,N are the coordinates of the first waypoint, i.e., the starting point, of the nth formation route;

[0036] x n,1 =x 0,1 -ndsinα1 (10)

[0037] y n,1 =y 0,1 +ndcosα1 (11)

[0038] Step 7.3: Based on the formation type and formation parameters specified in Step 1 (please explain if needed), calculate the endpoint of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route; the calculation process is as follows:

[0039] If the formation type is horizontal, azimuth, or V-formation, then the endpoints of the l-th and r-th routes are calculated sequentially according to formulas (12) to (15). Wherein, (x 0,2 ,y 0,2 (x) represents the coordinates of the endpoint of the translated reference route. l,2 ,y l,2 ), l=1,...,L are the coordinates of the endpoint of the l-th formation route on the inner or upper side, where l is an odd number. (x r,2 ,y r,2Let r = 2, ..., R be the coordinates of the r-th endpoint of the right or lower route, where r is an even number. d is the distance from the reference route to the adjacent member route, and α1 represents the angle between the first segment of the reference route and the x-axis. β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the right-hand member ship and the reference ship's route is the azimuth angle from the left-hand member ship to the reference ship.

[0040] x l,2 =x 0,2 -ldsinα1+ldtan(β l -π / 2)cosα1 (12)

[0041] y l,2 =y 0,2 +ldcosα1+ldtan(β l -π / 2)sinα1 (13)

[0042] x r,2 =x 0,2 +rdsinα1-rdtan(β r -π / 2)cosα1 (14)

[0043] y r,2 =y 0,2 -rdcosα1-rdtan(β r -π / 2)cosα1 (15)

[0044] If the formation is in column, then the destination of each route is calculated sequentially according to formulas (16) to (17); where, (x 0,2 ,y 0,2 (x) represents the coordinates of the endpoint of the reference route being translated. n,2 ,y n,2 ), n=1,...,N are the coordinates of the second waypoint, i.e., the destination, of the nth formation route;

[0045] x n,2 =x 0,2 -nd sinα1 (16)

[0046] y n,2 =y 0,2 +ndcosα1 (17)

[0047] Furthermore, step 8 is characterized by including the following sub-steps:

[0048] Step 8.1: Based on the formation parameters specified in Step 1 and the formation type to be generated, determine whether the number of routes n to be generated is even. If so, calculate L = N-1 and R = N; otherwise, L = N and R = N-1.

[0049] Step 8.2: Based on the formation type and formation parameters specified in Step 1, proceed to Step 7.2, and use formulas (1)-(9) to calculate the starting point of each translation-generated formation route in sequence, and store it in the corresponding route waypoint list;

[0050] Step 8.3: Based on the formation type and formation parameters specified in Step 1, calculate the formation line inflection points generated by each translation reference line in sequence, and store them in the corresponding waypoint list. The calculation process is as follows:

[0051] If the formation type is horizontal formation, azimuth formation, or V-formation, then calculate the screen coordinates of each turning point of the l-th and r-th routes in sequence according to formulas (18)-(27);

[0052]

[0053]

[0054]

[0055]

[0056]

[0057] d li,0i =d ri,0i =d / cos((α) i -α i-1 () / 2), i=2,...,N0-1 (23)

[0058] x l,i =x 0,i -d li,0i sin((α i-1 +α i () / 2), i=2,...,N L -1,l=1,...,L (24)

[0059] y l,i =y 0,i +d li,0i cos((α i-1 +α i () / 2), i=2,...,N L -1,l=1,...,L (25)

[0060] x r,i =x 0,i+d ri,0i sin((α i-1 +α i () / 2), i=2,...,N L -1, r = 2, ..., R (26)

[0061] y r,i =y 0,i -d ri,0i cos((α i-1 +α i () / 2), i=2,...,N R -1, r = 2, ..., R (27)

[0062] Among them, (x 0,i ,y 0,i ), i = 2, ..., N0 are the coordinates of the i-th waypoint of the reference route being translated, i.e., the coordinates of the (i-1)-th inflection point of the reference route; (x l,i ,y l,i ), i = 2, ..., N L ,l=1,...,L represents the coordinates of the i-th endpoint of the l-th route on the inner or upper side, i.e., the coordinates of the (i-1)-th inflection point of the l-th member route, where l is an odd number; (x r,i ,y r,i ), i = 2, ..., N R R = 2, ..., R represents the coordinates of the i-th waypoint of the r-th route on the right or bottom side, i.e., the coordinates of the (i-1)-th inflection point of the r-th member route, where r is an even number; d is the distance from the reference route to the adjacent member route, α i These represent the angles between the i-th segment of the reference route and the x-axis, respectively; d li,0i d ri,0i Let represent the distances from the i-th segment of the reference route to the i-th segment of the formation member route of the l-th route, and the distances from the i-th segment of the reference route to the i-th segment of the formation member route of the r-th route, respectively; d is the distance between the corresponding ends of the broken lines of the reference route and the generated member route; α i-1 ,i=2,...,N0 represents the angle between the (i-1)th segment of the reference route and the x-axis; β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship.

[0063] If the formation type is column, then calculate the screen coordinates of each turning point of the l-th and r-th routes according to formulas (27)-(28). Where i = 2,...,N L -1;

[0064] xn,i =x 0,i (28)

[0065] y n,i =y 0,i (29)

[0066] Step 8.4: Based on the formation type and formation parameters specified in Step 1, calculate the endpoint of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route. The calculation process is as follows:

[0067] If the formation type is horizontal, azimuth, or V-formation, then the endpoints of the l-th and r-th routes are calculated sequentially according to formulas (30) to (33). The coordinates of the endpoint of the reference route being translated; The coordinates are the endpoints of the l-th formation route on the inner or upper side, where l is an odd number; d represents the coordinates of the r-th line endpoint on the right or bottom side, where r is an even number; d is the distance from the reference line to the adjacent member line. These represent the angles between the N0-1 segment of the reference route and the x-axis, respectively; β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship.

[0068]

[0069]

[0070]

[0071]

[0072] If the formation is in column, then the destination of each route is calculated sequentially according to formulas (34) to (35); where, (x 0,N ,y 0,N (x) represents the coordinates of the endpoint of the reference route being translated. n,N ,y n,N ),n=1,…,N are the coordinates of the Nth waypoint, i.e., the destination, of the nth formation route;

[0073]

[0074]

[0075] The advantages and positive effects of this invention are as follows:

[0076] 1. This invention rapidly generates formation member routes based on planned reference routes and formation-related parameters for multi-track guided tracking and control, while also providing situational reference for command and control personnel, thus improving the efficiency of swarm mission planning and calculation. Compared to single-track guided tracking, this method provides a more intuitive swarm situational reference, enabling swarm members to maintain formation even after losing leader movement reference information, thereby effectively improving the robustness of the swarm system.

[0077] 2. Based on the idea of ​​decoupled planning method, this invention can quickly calculate and generate cluster routes by referencing route and formation parameters. The generated cluster routes can avoid collisions caused by multiple unmanned surface vessels arriving at the same position at the same time. At the same time, it can reduce the computational burden of coordinating and replanning conflict points among the members of each formation, and effectively solve the planning conflict problem that is prone to occur within the cluster. Attached Figure Description

[0078] Figure 1 This is a flowchart of the present invention;

[0079] Figure 2 This is a schematic diagram of an embodiment of the present invention. Detailed Implementation

[0080] The structure of the present invention will be further described below with reference to the accompanying drawings and embodiments. It should be noted that these embodiments are descriptive and not limiting.

[0081] For a dynamic safety trajectory planning method for unmanned surface vessels considering dynamic water depth, please refer to [link / reference]. Figure 1 The inventive point is as follows: it includes the following steps:

[0082] Step 1: Specify a reference flight path and formation parameters, where the formation parameters include, but are not limited to, the number of flight paths to be generated, the flight path spacing, the formation type, and the azimuth. Specifically:

[0083] Cluster monitoring personnel can manually plan or automatically generate a reference flight path (reference flight path) through a remote terminal. The number of flight paths, flight path spacing, formation type, azimuth angle and other formation parameters to be generated are specified by the monitoring personnel through the remote terminal.

[0084] Step 2: Perform a reference route check to see if the route crosses any obstructed navigation areas or restricted waters. If so, modify the original route and replan; otherwise, proceed to Step 3. Specifically:

[0085] The existing route analysis and verification module is used to perform initial route verification to determine whether the route crosses navigation obstruction areas or restricted waters. These navigation obstruction areas or restricted waters include, but are not limited to, various static obstacles marked on nautical charts: land, shoals, dry reefs, no-navigation zones, and fishing areas.

[0086] Step 3: Read the reference route and display parameters to obtain the chart display scale and projection method. Specifically:

[0087] The reference route is read by sequentially reading waypoints in latitude and longitude form and saving the waypoints in latitude and longitude form to a list; the display parameters read in step 3 include scale, projection method, and monitor specifications.

[0088] Step 4: Perform coordinate transformation on the list of waypoints included in the reference route, converting them to screen coordinates, and then store the coordinates sequentially into the new waypoint list. Specifically:

[0089] Based on the latitude and longitude format waypoint list, scale, and display specification parameters obtained in step 4, the waypoints in the reference route waypoint list are converted to coordinates, and the screen coordinates of each waypoint are sequentially stored into the new reference route screen coordinate waypoint list.

[0090] Step 5: Determine if the number of waypoints N0 in the screen coordinate reference route waypoint list is greater than or equal to 2. If yes, proceed to step 6; otherwise, output that the reference route is incomplete and modify the reference route specified in step 1.

[0091] Step 6: Determine if N0 is greater than or equal to 3; otherwise, proceed to Step 7; otherwise, proceed to Step 8.

[0092] Step 7: Calculate and generate a single-segment formation route based on all the formation parameters specified in Step 1, and store the generated single-segment formation route in the form of a waypoint list.

[0093] Step 7.1: Based on the formation parameters specified in Step 1 and the formation type to be generated, determine whether the number of routes N to be generated is even. If so, calculate L = N-1 and R = N; otherwise, L = N and R = N-1.

[0094] Step 7.2: Based on the formation type and formation parameters set in Step 1, calculate the starting point of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route. The calculation process is as follows:

[0095] If the formation type is horizontal, azimuth, or V-formation, then the starting points of the l-th and r-th routes are calculated sequentially according to formulas (1) to (9). Where, (x 0,1 ,y 0,1 (x) represents the coordinates of the first waypoint of the reference route being translated. l,1 ,y l,1 ), l=1,…,L represents the coordinates of the first waypoint (origin) of the l-th formation route on the inner or upper side, where l is an odd number. (x r,1 ,y r,1), r=2,…,R are the coordinates of the first waypoint of the r-th route on the right or bottom side, where r is an even number. d is the distance from the reference route to the adjacent member route, α1 represents the angle between the first segment of the reference route and the x-axis, β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the right-hand member ship and the reference ship's route is the azimuth angle from the left-hand member ship to the reference ship.

[0096]

[0097]

[0098]

[0099]

[0100]

[0101] x l,1 =x 0,1 -ldsinα1+ldtan(β l -π / 2)cosα1 (6)

[0102] y l,1 =y 0,1 +ldcosα1+ldtan(β l -π / 2)sinα1 (7)

[0103] x r,1 =x 0,1 +rdsinα1-rdtan(β r -π / 2)cosα1 (8)

[0104] y r,1 =y 0,1 -rdcosα1-rdtan(β r -π / 2)sinα1 (9)

[0105] If the formation is in column, then the starting points of each route are calculated sequentially according to formulas (10) to (11). Where, (x 0,1 ,y 0,1 (x) represents the coordinates of the first waypoint of the reference route being translated. n,1 ,y n,1 ), n=1,…,N are the coordinates of the first waypoint, i.e., the starting point, of the nth formation route.

[0106] x n,1 =x 0,1 -ndsinα1 (10)

[0107] y n,1 =y 0,1 +ndcosα1 (11)

[0108] Step 7.3: Based on the formation type and formation parameters specified in Step 1, calculate the endpoint of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route. The calculation process is as follows:

[0109] If the formation type is horizontal, azimuth, or V-formation, then the endpoints of the l-th and r-th routes are calculated sequentially according to formulas (12) to (15). Wherein, (x 0,2 ,y 0,2 (x) represents the coordinates of the endpoint of the translated reference route. l,2 ,y l,2 ), l=1,…,L are the coordinates of the endpoint of the l-th formation route on the inner or upper side, where l is an odd number. (x r,2 ,y r,2 Let r = 2, ..., R be the coordinates of the r-th endpoint of the right or lower route, where r is an even number. d is the distance from the reference route to the adjacent member route, and α1 represents the angle between the first segment of the reference route and the x-axis. β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the right-hand member ship and the reference ship's route is the azimuth angle from the left-hand member ship to the reference ship.

[0110] x l,2 =x 0,2 -ldsinα1+ldtan(β l -π / 2)cosα1 (12)

[0111] y l,2 =y 0,2 +ldcosα1+ldtan(β l -π / 2)sinα1 (13)

[0112] x r,2 =x 0,2 +rdsinα1-rdtan(β r -π / 2)cosα1 (14)

[0113] y r,2 =y 0,2 -rdcosα1-rdtan(β r -π / 2)cosα1 (15)

[0114] If the formation is in column, then the destinations of each route are calculated sequentially according to formulas (16) to (17). Where, (x 0,2 ,y 0,2 (x) represents the coordinates of the endpoint of the reference route being translated. n,2 ,y n,2 ), n=1,…,N are the coordinates of the second waypoint, i.e., the destination, of the nth formation route.

[0115] x n,2 =x 0,2 -ndsinα1 (16)

[0116] y n,2 =y 0,2 +ndcosα1 (17)

[0117] Step 8: Based on the formation type and formation parameters specified in Step 1, calculate and generate multi-segment formation routes, and store the generated multi-segment formation routes in the form of a waypoint list.

[0118] Step 8.1: Based on the formation parameters specified in Step 1 and the formation type to be generated, determine whether the number of routes n to be generated is even. If so, calculate L = N-1 and R = N; otherwise, L = N and R = N-1.

[0119] Step 8.2: Based on the formation type and formation parameters specified in Step 1, proceed to Step 7.2, and use formulas (1)-(9) to calculate the starting point of each translation-generated formation route in sequence, and store it in the corresponding route waypoint list.

[0120] Step 8.3: Based on the formation type and formation parameters specified in Step 1, calculate the formation line inflection points generated by each translation reference line in sequence, and store them in the corresponding waypoint list. The calculation process is as follows:

[0121] If the formation type is horizontal, azimuth, or V-formation, then calculate the screen coordinates of each turning point of the l-th and r-th routes in sequence according to formulas (18)-(27).

[0122]

[0123]

[0124]

[0125]

[0126]

[0127] d li,0i =d ri,0i =d / cos((α) i-α i-1 ) / 2), i=2,…,N0-1 (23)

[0128] x l,i =x 0,i -d li,0i sin((α i-1 +α i () / 2), i=2,…,N L -1,l=1,...,L (24)

[0129] y l,i =y 0,i +d li,0i cos((α i-1 +α i () / 2), i=2,…,N L -1,l=1,…,L (25)

[0130] x r,i =x 0,i +d ri,0i sin((α i-1 +α i () / 2), i=2,...,N L -1, r = 2, ..., R (26)

[0131] y r,i =y 0,i -d ri,0i cos((α i-1 +α i () / 2), i=2,...,N R -1, r = 2, ..., R (27)

[0132] Among them, (x 0,i ,y 0,i ), i = 2, ..., N0 are the coordinates of the i-th waypoint of the translated reference route, i.e., the coordinates of the (i-1)-th inflection point of the reference route. (x l,i ,y l,i ), i = 2, ..., N L Let l = 1, ..., L be the coordinates of the i-th endpoint of the l-th route on the inner or upper side, i.e., the coordinates of the (i-1)-th inflection point of the l-th member route, where l is an odd number. (x r,i ,y r,i ), i = 2, ..., N R Let r = 2, ..., R be the coordinates of the i-th waypoint of the r-th route on the right or bottom side, i.e., the coordinates of the (i-1)-th inflection point of the r-th member route, where r is an even number. d is the distance from the reference route to the adjacent member route, and α... i d represents the angle between the i-th segment of the reference route and the x-axis.li,0i d ri,0i Let α represent the distance from the i-th segment of the reference route to the i-th segment of the formation member route of the l-th route, and d represent the distance from the i-th segment of the reference route to the i-th segment of the formation member route of the r-th route, respectively. i-1 Let i = 2, ..., N0 represent the angle between the (i-1)th segment of the reference route and the x-axis. β l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the right-hand member ship and the reference ship's route is the azimuth angle from the left-hand member ship to the reference ship.

[0133] If the formation type is column, then calculate the screen coordinates of each turning point of the l-th and r-th routes according to formulas (27)-(28). Where i = 2,...,N L -1.

[0134] x n,i =x 0,i (28)

[0135] y n,i =y 0,i (29)

[0136] Step 8.4: Based on the formation type and formation parameters specified in Step 1, calculate the endpoint of each formation route generated by the translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route. The calculation process is as follows:

[0137] If the formation type is horizontal, azimuth, or V-formation, then the endpoints of the l-th and r-th routes are calculated sequentially according to formulas (30) to (33). The coordinates of the endpoint of the reference route being translated. The coordinates are the endpoints of the l-th formation route on the inner or upper side, where l is an odd number. Let be the coordinates of the r-th line endpoint on the right or bottom side, where r is an even number. d is the distance from the reference line to the adjacent member line. β represents the angle between the broken line of the N0-1 segment of the reference route and the x-axis. l β is the angle between the line connecting the reference ship to the port crew member and the reference ship's course, i.e., the bearing angle from the port crew member to the reference ship. r The angle between the line connecting the reference ship to the right-hand member ship and the reference ship's route is the azimuth angle from the left-hand member ship to the reference ship.

[0138]

[0139]

[0140]

[0141]

[0142] If the formation is in column, then the destination of each route is calculated sequentially according to formulas (34) to (35). Where, (x 0,N ,y 0,N (x) represents the coordinates of the endpoint of the reference route being translated. n,N ,y n,N ), n=1,...,N are the coordinates of the Nth waypoint, i.e., the destination, of the nth formation route.

[0143]

[0144]

[0145] Step 9: Based on the parameters obtained in Step 3, perform coordinate transformation on the waypoint list obtained in Steps 7 and 8, convert the screen coordinates into latitude and longitude coordinates, and save them to the new latitude and longitude coordinate form of the waypoint list.

[0146] Step 10: Perform formation route verification. Check in turn whether the route of each member crosses a navigation obstruction area or restricted waters. If so, prompt the monitoring personnel to modify the route; otherwise, proceed to Step 11.

[0147] Step 11: Output formation flight path.

[0148] The following is an example:

[0149] Set the formation type to line formation, the route interval to 500m, the number of routes to 7, and the alignment angle to 90°. Input a reference route test01. The route includes 10 waypoints with the following coordinates: WP1 (40°13.024′N, 120°52.250′), WP2 (40°13.198′N, 120°55.0785′), WP3 (40°13.173′N, 120°59.044′), WP4 (40°11.639 ... °00.909′), WP5 (40°11.739′N, 121°05.655′), WP6 (40°10.817′N, 121°08.484′), WP7 (40°11.066′N, 121°14.2 70′), WP8 (40°09.470′N, 121°20.707′), WP9 (40°07.649′N, 121°21.551′), WP10 (40°04.828′N, 121°22.300′).

[0150] Using the method described in this invention, the generated cluster member routes are calculated as test01-1, test01-2, test01-3, test01-4, test01-5, test01-6, and test01-6, with the following results: Figure 2 As shown.

[0151] Although embodiments and drawings of the present invention have been disclosed for illustrative purposes, those skilled in the art will understand that various substitutions, alterations and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the contents disclosed in the embodiments and drawings.

Claims

1. A method for rapid route planning in unmanned surface vessel / ship convoys, characterized in that: Includes the following steps: Step 1: Specify a reference route and formation parameters, where the formation parameters include the number of routes to be generated, route spacing, formation type, and bearing. Step 2: Perform a reference route verification to check whether the route crosses a navigation obstruction zone or restricted waters; if so, modify the original route and replan; otherwise, proceed to Step 3. Step 3: Read the reference route and display parameters to obtain the chart display scale and projection method; Step 4: Perform coordinate transformation on the list of waypoints included in the reference route, convert them to screen coordinates, and store the coordinates into the new waypoint list in sequence; Step 5: Determine the number of waypoints in the screen coordinate reference route waypoint list. If the value is greater than or equal to 2, proceed to step 6; otherwise, output that the reference route is incomplete and modify the reference route specified in step 1. Step 6: Determine Is it greater than or equal to 3? If not, proceed to step 7; otherwise, proceed to step 8. Step 7: Calculate and generate a single-segment formation route based on the formation parameters specified in Step 1, and store the generated single-segment formation route in the form of a waypoint list; Step 8: Based on the formation parameters specified in Step 1, calculate and generate multi-segment formation routes, and store the generated multi-segment formation routes in the form of a waypoint list; Step 9: Based on the parameters obtained in Step 3, perform coordinate transformation on the waypoint list obtained in Steps 7 and 8, convert the screen coordinates into latitude and longitude coordinates, and save them to the new latitude and longitude coordinate form of the waypoint list. Step 10: Perform formation route verification. Check in turn whether the route of each member crosses the navigation obstruction area or restricted waters. If so, prompt the monitoring personnel to modify the route; otherwise, proceed to step 11. Step 11: Output the formation flight path; Step 7 includes the following sub-steps: Step 7.1: Determine the number of flight paths to be generated based on all the formation parameters specified in Step 1. If the number is even, then calculate the number of routes on the left. and the number of routes on the right ,in ,otherwise ; Step 7.2: Based on the formation parameters set in Step 1, calculate the starting point of the formation route generated by each translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route; the calculation process is as follows: If the formation type is horizontal, directional, or V-formation, then the number of steps is calculated sequentially according to formulas (1) to (9). Article and Section The starting point of the routes; among them, The coordinates of the first waypoint of the reference route being translated. For the inner or upper side The first waypoint of the formation's route is the coordinate of the starting point. It is an odd number; The right or bottom side The coordinates of the first waypoint of the route, It is an even number; The distance from the reference route to the routes of adjacent members, These respectively represent the broken line of the first segment of the reference route and Angle between axes The angle between the line connecting the reference vessel to the port-side crew member vessel and the reference vessel's course is the bearing angle from the port-side crew member to the reference vessel. The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship. (1) (2) (3) (4) (5) (6) (7) (8) (9) If the formation is in column, then the starting points of each route are calculated sequentially according to formulas (10) to (11); where, The coordinates of the first waypoint of the reference route being translated. For the first The first waypoint of the formation route is the coordinate of the starting point; (10) (11) Step 7.3: Based on the formation parameters specified in Step 1, calculate the endpoint of the formation route generated by each translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route; the calculation process is as follows: If the formation type is horizontal, directional, or V-formation, then the number of steps is calculated sequentially according to formulas (12) to (15). Article and Section The destination of the route; among them, The coordinates of the endpoint of the reference route being translated; For the inner or upper side The coordinates of the end point of the formation's route. It is an odd number; The right or bottom side The coordinates of the destination of the route. It is an even number; The distance from the reference route to the routes of adjacent members, These respectively represent the broken line of the first segment of the reference route and Angle between axes; The angle between the line connecting the reference vessel to the port-side crew member vessel and the reference vessel's course is the bearing angle from the port-side crew member to the reference vessel. The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship. (12) (13) (14) (15) If the formation is in column, then the destinations of each route are calculated sequentially according to formulas (16) and (17); among them, The coordinates of the endpoint of the reference route being translated. For the first The coordinates of the second waypoint of the formation's route, which is also the destination. (16) (17)。 2. The method for unmanned surface vessel / ship convoy route planning according to claim 1, characterized in that, The specific implementation method of step 1 is as follows: the cluster monitoring personnel manually plan or automatically generate a reference flight path through the remote control terminal. The formation parameters to be generated include the number of flight paths, the flight path interval, the formation type, and the azimuth angle, which are specified by the monitoring personnel through the remote control terminal.

3. The method for unmanned surface vessel / ship convoy route planning according to claim 1, characterized in that: Step 2: Perform initial route verification using the existing route analysis and verification module to determine whether the route crosses navigation obstruction zones or restricted waters. These zones or restricted waters include, but are not limited to, various static obstacles marked on nautical charts: land, shoals, dry reefs, no-navigation zones, and fishing areas.

4. The method for unmanned surface vessel / ship convoy route planning according to claim 1, characterized in that: In step 3, the reference route is read by sequentially reading waypoints in latitude and longitude form, and the waypoints in latitude and longitude form are saved to the waypoint list; the display parameters read in step 3 include scale, projection method, and monitor specifications.

5. The method for unmanned surface vessel / ship convoy route planning according to claim 1, characterized in that: Based on the latitude and longitude waypoint list, scale, and display specifications obtained in step 4, the waypoints in the reference route waypoint list are converted to coordinates, and the screen coordinates of each waypoint are sequentially stored into the new reference route waypoint list.

6. The method for unmanned surface vessel / ship convoy route planning according to claim 1, characterized in that, Step 8: Includes the following sub-steps: Step 8.1: Based on the formation parameters specified in Step 1 and the formation type to be generated, determine whether the number of routes n to be generated is even. If so, calculate... ,otherwise ; Step 8.2: Based on the formation parameters specified in Step 1, proceed to Step 7.2, and use formulas (1) to (9) to calculate the starting point of each translation-generated formation route in sequence, and store it in the corresponding route waypoint list; Step 8.3: Based on the formation parameters specified in Step 1, calculate the formation line inflection points generated by each translation reference line in sequence, and store them in the corresponding waypoint list; the calculation process is as follows: If the formation type is horizontal, azimuth, or V-formation, then calculate the number of steps in sequence according to formulas (18) to (27). Article and Section Screen coordinates of each turning point on the route; (18) (19) (20) (21) (22) (23) (24) (25) (26) (27) in, The reference route that was translated The coordinates of the waypoint, i.e., the reference route's first waypoint. The coordinates of the inflection point; For the inner or upper side Route No. The coordinates of the endpoint, i.e. the first endpoint. Member routes The coordinates of the inflection point It is an odd number; The right or bottom side Route No. The coordinates of the nth waypoint, i.e. Member routes The coordinates of the inflection point It is an even number; The distance from the reference route to the routes of adjacent members, These represent the reference route number. Segmented broken line and Angle between axes; , These represent the reference route number. Segment of the route to the first Route formation members Route number Distance of segment route and reference route Segment of the route to the first Route formation members Route number The distance of the route segment The distance between the reference route and the corresponding ends of the generated member routes; Indicates the reference route number Segmented broken line and Angle between axes; The angle between the line connecting the reference vessel to the port-side crew member vessel and the reference vessel's course is the bearing angle from the port-side crew member to the reference vessel. The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship. If the formation type is column, then calculate the number of columns in sequence according to formulas (27)-(28). Article and Section The screen coordinates of each turning point on the route; among them, ; (28) (29) Step 8.4: Based on the formation parameters specified in Step 1, calculate the endpoint of the formation route generated by each translation reference route in sequence, and store the calculation results in the waypoint list of the corresponding route; the calculation process is as follows: If the formation type is horizontal, directional, or V-formation, then the number of steps is calculated sequentially according to formulas (30) to (33). Article and Section The destination of the route; among them, The coordinates of the endpoint of the reference route being translated; For the inner or upper side The coordinates of the end point of the formation's route. It is an odd number; The right or bottom side The coordinates of the destination of the route. It is an even number; The distance from the reference route to the routes of adjacent members, These represent the reference route number. Segmented broken line and Angle between axes; The angle between the line connecting the reference vessel to the port-side crew member vessel and the reference vessel's course is the bearing angle from the port-side crew member to the reference vessel. The angle between the line connecting the reference ship to the starboard crew member and the route of the reference ship is the azimuth angle from the left crew member to the reference ship. （30） （31） （32） （33） If the formation is in column, then the destinations of each route are calculated sequentially according to formulas (34) and (35); among them, The coordinates of the endpoint of the reference route being translated. For the first The first formation route Each waypoint is the coordinate of the destination; （34） （35）。

Images