Full-coverage path planning algorithm for unmanned sailboat

Abstract

The invention discloses a full-coverage path planning algorithm for an unmanned sailboat. The path planning algorithm comprises the following steps: detecting and sensing a real wind speed through a wind speed sensor; setting the wind direction at the beginning of executing the full-coverage task as 0 degree, and rasterizing the map along the directions parallel and vertical to the wind direction; assigning each node on the grid map, and further judging whether a windward area exists on the grid map or not; when a windward area exists on the grid map, the grid map goes to a relatively maximum windward position, the grid map is re-assigned according to the track, and a sailing mark is left along the way; when the dead zone exists on the grid map, the unmanned sailboat is separated from the dead zone, and the steps are repeated; when no dead zone exists in the grid map, the unmanned sailboat goes to the node with the maximum value in the surrounding feasible grids, the grid map is re-assigned, and a sailing mark is left; and judging whether the unmanned sailboat passes through all target areas or not, and completing a full-coverage path planning task of the unmanned sailboat.

Description

Technical field:

[0001] The invention relates to a full coverage path planning algorithm for an unmanned sailboat. Background technology:

[0002] Unmanned sailboats are mainly driven by wind and solar energy and are suitable for long-term and long-distance monitoring tasks. With the increasing demand for marine environmental information, unmanned sailboats have become an important marine monitoring platform and are applied to marine data collection and environmental monitoring.

[0003] The main tasks of unmanned sailboats include ocean monitoring, security patrols, search and rescue missions, scientific research and resource exploration, etc. They can be used to monitor environmental changes, pollutant diffusion and ecosystem status in real time, collect data and generate maps. Security patrols can detect illegal activities or suspicious targets and enhance marine safety. As a prerequisite for unmanned sailboats to perform tasks, path planning of unmanned sailboats is an indispensable and important link. However, the existing path planning algorithm will be affected by the motion environment, the overall calculation process is too simple, and is subject to more conditions and constraints, which cannot be fully applicable to the functional characteristics of unmanned sailboats. Summary of the invention:

[0004] The embodiment of the present invention provides a full coverage path planning algorithm for an unmanned sailboat. The method is reasonably designed. The map is rasterized according to the wind direction when coverage starts, so that the wind direction when coverage starts points from the top of the grid map to the bottom. The wind direction is then divided into different areas according to the polar coordinate diagram of the speed of the unmanned sailboat, and the unnavigable directions in the eight grids around the current grid are disabled, so that the planned path changes with the change of wind direction, avoiding the influence of the telecontrol environment and the constraints of external conditions. It can be fully applicable to the functional characteristics of the unmanned sailboat and solves the problems existing in the prior art.

[0005] The technical solution adopted by the present invention to solve the above technical problems is:

[0006] A full coverage path planning algorithm for an unmanned sailboat, the path planning algorithm comprising the following steps:

[0007] S1, detecting and sensing the actual wind speed through a wind speed sensor;

[0008] S2, setting the wind direction at the start of the full coverage mission to 0°, rasterizing the map in directions parallel and perpendicular to the wind direction, and then disabling some of the eight grids around the unmanned sailboat according to the unmanned sailboat VPP curve graph;

[0009] S3, assigning a value to each node on the grid map, and then determining whether there is an upwind area on the grid map;

[0010] S4, when there is an upwind area on the grid map, go to the relative maximum upwind position, reassign the grid map according to the track, and leave a navigation mark along the way; when there is no upwind area on the grid map, determine whether there is a dead zone on the grid map;

[0011] S5, when there is a dead zone on the grid map, the unmanned sailboat leaves the dead zone and repeats step S3; when there is no dead zone on the grid map, the unmanned sailboat goes to the node with the largest value in the surrounding feasible grids, reassigns the grid map, and leaves a navigation mark;

[0012] S6, judging whether the unmanned sailboat has passed through all target areas, and completing the full coverage path planning task of the unmanned sailboat.

[0013] Assigning a value to each node on the grid map includes the following steps:

[0014] S3.1, according to the polar coordinate diagram of the speed of the unmanned sailboat, the wind direction is divided into different areas, and the grids in the unnavigable direction among the eight grids around the current grid are disabled;

[0015] S3.2, the wind direction pointing downward from the top of the map is set to 0°, and the counterclockwise direction is set as the positive direction, thereby dividing the wind direction into eight directions and determining eight main wind directions in the grid map;

[0016] S3.3, when the wind direction θ is between [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], it is necessary to assign values ​​to the nodes in the grid map before coverage begins and after each step during coverage, and define the values ​​at the obstacle position:

[0017] f(x, y) = -1

[0018] The assignment formula at the non-obstacle position is:

[0019]

[0020] In the formula: x, y represent the horizontal and vertical coordinates of the map; X is the total number of grids counted along the main wind direction in the map; a is the sequence number of the current grid in the grids counted along the main wind direction in the map; Y is the total number of grids in the continuous grid string counted along the direction perpendicular to the main wind direction and not separated by obstacles, navigated grids, and the current sailboat position; b is the sequence number of the current grid in the continuous grid string that is not separated by obstacles, navigated grids, and the current sailboat position along the direction perpendicular to the wind direction; after the sailboat enters a grid, the current grid value is reduced by 1;

[0021] S3.4, when the wind direction θ changes to an area other than [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], if the area where the previous wind direction was located is adjacent to the current area, the value is assigned according to the previous assignment method; if not, the value is assigned randomly according to the assignment method of one of the two adjacent areas.

[0022] Use the improved APF algorithm to reach the relative maximum upwind position. In the improved APF algorithm, the potential field is divided into the global potential field and the local potential field. In each movement process, the node with the lowest total potential energy among the eight surrounding nodes is selected;

[0023] The global potential field includes the attractive potential field generated by the target point, and the repulsive potential field generated by obstacles and covered areas;

[0024] In order to attract the unmanned sailboat to the target point, the following linear potential field P is established for each grid node P on the map: g :

[0025] P g =G g ·dist(P,P goal )

[0026] G g is the attraction gradient dist(P,P goal ) is point to point P goal The Euclidean distance of

[0027] In order to make the unmanned sailboat avoid obstacles, at the obstacle node P obst Add a repulsive potential field P o ;

[0028]

[0029] Where k1 is an adjustable parameter

[0030] In order to prevent the unmanned sailboat from entering the covered area as much as possible, the covered node P covered Add a repulsive potential field P c ;

[0031]

[0032] Where: k2 is an adjustable parameter.

[0033] The local potential field is used to make the planned path conform to the movement law of the unmanned sailboat. The local potential field includes a headwind potential field and a hysteresis potential field. The headwind potential field is used to prevent the unmanned sailboat from sailing in a restricted navigation area. The hysteresis potential field is used to reduce the turning of the unmanned sailboat during its advancement, so that the unmanned sailboat can reach the target point faster.

[0034] The headwind potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat, and changes direction with the change of wind direction; any point P in the navigation restriction area w Wind potential energy P up The size of P w Proportional to the distance to point P; outside the restricted navigation area, the wind potential energy is 0:

[0035]

[0036] Among them, G up is the gradient of the wind potential energy in the navigation restricted area, and φ represents the true wind direction angle;

[0037] The hysteresis potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat's position, and changes direction with the change of the sailboat's heading Ψ; the hysteresis potential field is set to be 0 in the same direction as the current heading, and any point P in the rest of the area w Hysteresis potential energy P h The size of P w The distance to point P is proportional to:

[0038]

[0039] Among them, G h It is the parameter that limits the turning of the sailboat.

[0040] Use the breadth-first algorithm to find the nearest grid with a gradient value greater than 0; first, create a queue and put the current node into the queue; then, take a node from the queue and check its adjacent nodes, which are the nodes directly adjacent to the current node, and add them to the queue, but do not add them to the queue when encountering obstacles; repeat this process, continuously take out nodes from the queue and explore their adjacent nodes, and use the improved APF method to go to the target grid after finding it.

[0041] When the unmanned sailboat approaches the covered area, the heading vector is decomposed into a direction parallel to the main wind direction used for assignment and a direction perpendicular to the main wind direction. If there are other grids to travel, the track that crosses the navigated route in the same direction as the covered route in the direction perpendicular to the main wind direction is not used.

[0042] The present invention adopts the above structure, by setting the wind direction at the beginning of the full coverage task to 0°, rasterizing the map in directions parallel and perpendicular to the wind direction, and then disabling some of the eight grids around the unmanned sailboat according to the VPP curve diagram of the unmanned sailboat; by assigning a value to each node on the grid map, and then judging whether there is an upwind area on the grid map; by dividing the wind direction into different areas according to the polar coordinate diagram of the speed of the unmanned sailboat, disabling the grids in the non-navigable direction in the eight grids around the current grid; by judging whether the unmanned sailboat has passed through all the target areas, the full coverage path planning task of the unmanned sailboat is completed, and if not completed, the actual wind speed is sensed by the wind speed sensor to cycle the previous operation steps, which has the advantages of flexibility, practicality, safety and reliability. Description of the drawings:

[0043] Figure 1 is a polar coordinate diagram of the speed of a sailboat provided by an embodiment of the present invention;

[0044] Figure 2 Schematic diagram of disabling grids according to wind direction provided by an embodiment of the present invention

[0045] Figure 3 is a flow chart of an algorithm provided by an embodiment of the present invention;

[0046] Figure 4 Schematic diagram of wind direction division provided by an embodiment of the present invention

[0047] Figure 5 This is a schematic diagram of the algorithm assignment provided by the embodiment of the present invention

[0048] Figure 6 This is a schematic diagram of the gradient change of the assignment process provided by the embodiment of the present invention Specific implementation method:

[0049] In order to clearly illustrate the technical features of the present solution, the present invention is described in detail below through specific implementation methods and in conjunction with the accompanying drawings.

[0050] like Figure 1-6 As shown in , a full coverage path planning algorithm for an unmanned sailboat, the path planning algorithm comprises the following steps:

[0051] S1, detecting and sensing the actual wind speed through a wind speed sensor;

[0052] S2, setting the wind direction at the start of the full coverage mission to 0°, rasterizing the map in directions parallel and perpendicular to the wind direction, and then disabling some of the eight grids around the unmanned sailboat according to the unmanned sailboat VPP curve graph;

[0053] S3, assigning a value to each node on the grid map, and then determining whether there is an upwind area on the grid map;

[0054] S4, when there is an upwind area on the grid map, go to the relative maximum upwind position, reassign the grid map according to the track, and leave a navigation mark along the way; when there is no upwind area on the grid map, determine whether there is a dead zone on the grid map;

[0055] S5, when there is a dead zone on the grid map, the unmanned sailboat leaves the dead zone and repeats step S3; when there is no dead zone on the grid map, the unmanned sailboat goes to the node with the largest value in the surrounding feasible grids, reassigns the grid map, and leaves a navigation mark;

[0056] S6, judging whether the unmanned sailboat has passed through all target areas, and completing the full coverage path planning task of the unmanned sailboat.

[0057] Assigning a value to each node on the grid map includes the following steps:

[0058] S3.1, according to the polar coordinate diagram of the speed of the unmanned sailboat, the wind direction is divided into different areas, and the grids in the unnavigable direction among the eight grids around the current grid are disabled;

[0059] S3.2, the wind direction pointing downward from the top of the map is set to 0°, and the counterclockwise direction is set as the positive direction, thereby dividing the wind direction into eight directions and determining eight main wind directions in the grid map;

[0060] S3.3, when the wind direction θ is between [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], it is necessary to assign values ​​to the nodes in the grid map before coverage begins and after each step during coverage, and define the values ​​at the obstacle position:

[0061] f(x, y) = -1

[0062] The assignment formula at the non-obstacle position is:

[0063]

[0064] In the formula: x, y represent the horizontal and vertical coordinates of the map; X is the total number of grids counted along the main wind direction in the map; a is the sequence number of the current grid in the grids counted along the main wind direction in the map; Y is the total number of grids in the continuous grid string counted along the direction perpendicular to the main wind direction and not separated by obstacles, navigated grids, and the current sailboat position; b is the sequence number of the current grid in the continuous grid string that is not separated by obstacles, navigated grids, and the current sailboat position along the direction perpendicular to the wind direction; after the sailboat enters a grid, the current grid value is reduced by 1;

[0065] S3.4, when the wind direction θ changes to an area other than [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], if the area where the previous wind direction was located is adjacent to the current area, the value is assigned according to the previous assignment method; if not, the value is assigned randomly according to the assignment method of one of the two adjacent areas.

[0066] Use the improved APF algorithm to reach the relative maximum upwind position. In the improved APF algorithm, the potential field is divided into the global potential field and the local potential field. In each movement process, the node with the lowest total potential energy among the eight surrounding nodes is selected;

[0067] The global potential field includes the attractive potential field generated by the target point, and the repulsive potential field generated by obstacles and covered areas;

[0068] In order to attract the unmanned sailboat to the target point, the following linear potential field P is established for each grid node P on the map: g :

[0069] P g =G g ·dist(P,P goal )

[0070] G g is the attraction gradient d ist(P,P goal ) is point to point P goal The Euclidean distance of

[0071] In order to make the unmanned sailboat avoid obstacles, at the obstacle node P obst Add a repulsive potential field P o ;

[0072]

[0073] Where k1 is an adjustable parameter

[0074] In order to prevent the unmanned sailboat from entering the covered area as much as possible, the covered node P covered Add a repulsive potential field P c ;

[0075]

[0076] Where: k2 is an adjustable parameter.

[0077] The local potential field is used to make the planned path conform to the movement law of the unmanned sailboat. The local potential field includes a headwind potential field and a hysteresis potential field. The headwind potential field is used to prevent the unmanned sailboat from sailing in a restricted navigation area. The hysteresis potential field is used to reduce the turning of the unmanned sailboat during its advancement, so that the unmanned sailboat can reach the target point faster.

[0078] The headwind potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat, and changes direction with the change of wind direction; any point P in the navigation restriction area w Wind potential energy P up The size of P w Proportional to the distance to point P; outside the restricted navigation area, the wind potential energy is 0:

[0079]

[0080] Among them, G up is the gradient of the wind potential energy in the navigation restricted area, and φ represents the true wind direction angle;

[0081] The hysteresis potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat's position, and changes direction with the change of the sailboat's heading Ψ; the hysteresis potential field is set to be 0 in the same direction as the current heading, and any point P in the rest of the area w Hysteresis potential energy P h The size of P w The distance to point P is proportional to:

[0082]

[0083] Among them, G h It is the parameter that limits the turning of the sailboat.

[0084] Use the breadth-first algorithm to find the nearest grid with a gradient value greater than 0; first, create a queue and put the current node into the queue; then, take a node from the queue and check its adjacent nodes, which are the nodes directly adjacent to the current node, and add them to the queue, but do not add them to the queue when encountering obstacles; repeat this process, continuously take out nodes from the queue and explore their adjacent nodes, and use the improved APF method to go to the target grid after finding it.

[0085] When the unmanned sailboat approaches the covered area, the heading vector is decomposed into a direction parallel to the main wind direction used for assignment and a direction perpendicular to the main wind direction. If there are other grids to travel, the track that crosses the navigated route in the same direction as the covered route in the direction perpendicular to the main wind direction is not used.

[0086] The working principle of a full coverage path planning algorithm for an unmanned sailboat in an embodiment of the present invention is as follows: the map is rasterized according to the wind direction when coverage starts, so that the wind direction when coverage starts points from the top of the grid map to the bottom, and then the wind direction is divided into different areas according to the polar coordinate diagram of the unmanned sailboat speed, and the unnavigable directions in the eight grids around the current grid are disabled, so that the planned path changes with the change of wind direction, avoiding the influence of the telecontrol environment and the constraints of external conditions, and can be fully applicable to the functional characteristics of the unmanned sailboat.

[0087] In the overall scheme, the path planning algorithm includes the following steps: detecting and sensing the real wind speed through the wind speed sensor; setting the wind direction when starting to perform the full coverage task to 0°, gridding the map in the direction parallel and perpendicular to the wind direction, and then disabling some of the eight grids around the unmanned sailboat according to the unmanned sailboat VPP curve diagram; assigning a value to each node on the grid map, and then judging whether there is an upwind area on the grid map; when there is an upwind area on the grid map, going to the relative maximum upwind position, and reassigning the grid map according to the track, and leaving a navigation mark along the way; when there is no upwind area on the grid map, judging whether there is a dead zone on the grid map; when there is a dead zone on the grid map, the unmanned sailboat leaves the dead zone and repeats step S3; when there is no dead zone on the grid map, the unmanned sailboat goes to the node with the largest median value of the surrounding feasible grids, and reassigns the grid map, leaving a navigation mark; judging whether the unmanned sailboat has passed all target areas, and completing the full coverage path planning task of the unmanned sailboat.

[0088] The core innovation of this application is to rasterize the map according to the wind direction at the beginning of coverage, so that the wind direction at the beginning of coverage points from the top of the grid map to the bottom, divide the wind direction into different areas according to the polar coordinate diagram of the sailboat speed, and disable the grids in the non-navigable direction in the eight grids around the current grid; set the wind direction from the top of the map to the bottom to 0°, and take the counterclockwise direction as the positive direction. If the wind direction θ is in [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], assign values ​​according to the following rules.

[0089] In the sailboat full coverage algorithm, the nodes in the map need to be assigned values ​​before the coverage begins and after each step during the coverage process; the values ​​at the obstacle position are:

[0090] f(x, y) = -1

[0091] The assignment formula at the non-obstacle position is:

[0092]

[0093] In the formula: x, y represent the horizontal and vertical coordinates of the map; X is the total number of grids counted along the main wind direction in the map; a is the sequence number of the current grid in the grids counted along the main wind direction in the map; Y is the total number of grids in the continuous grid string counted along the direction perpendicular to the main wind direction and not separated by obstacles, navigated grids, and the current sailboat position; b is the sequence number of the current grid in the continuous grid string that is not separated by obstacles, navigated grids, and the current sailboat position along the direction perpendicular to the wind direction; after the sailboat enters a grid, the current grid value is reduced by 1;

[0094] When the wind direction θ changes to areas outside the areas of [-30°, 30°], [60°, 120°], [150°, 210°], and [240°, 300°], if the area where the previous wind direction was located is adjacent to the current area, the value is assigned according to the previous assignment method; if not, the value is randomly assigned according to the assignment method of one of the two adjacent areas.

[0095] During the coverage process, since the movement of the unmanned sailboat is restricted by the wind direction and cannot move directly in the opposite direction of the wind, it needs to go to a position suitable for full coverage tasks in order to reduce the double coverage rate; once the unmanned sailboat determines that there is an uncovered area in the upwind direction, it will find a relatively suitable position in the upwind direction as the target and go to that position. This point is called the relatively upwind position. Depending on the wind direction, the criteria for determining the existence of upwind uncovered areas are different.

[0096] Since the unmanned sailboat can only select the next target grid from the eight grids around the current grid at a time, when there is no area to be covered around the unmanned sailboat, but only covered grids or obstacles, the sailboat is considered to have entered a deadlock; specifically in the algorithm implementation, when the f(x, y) of the adjacent grids around the unmanned sailboat are all less than 0, the unmanned sailboat is considered to have entered a deadlock.

[0097] In order to escape from the deadlock, the present invention uses a breadth-first algorithm to find the nearest grid with a gradient value greater than 0; first, create a queue and put the current node into the queue; then, take a node from the queue and check its adjacent nodes; these adjacent nodes are the nodes directly adjacent to the current node, and they are added to the queue, but they are not added to the queue when obstacles are encountered; repeat this process, continuously take out the nodes in the queue, and explore their adjacent nodes. If the node has been visited, it will not be added to the queue, and the exploration is carried out layer by layer until the target point is found.

[0098] Specifically, in order to escape from the deadlock and move to the relatively most upwind position, it is necessary to use the improved artificial potential field method to reach the relatively most upwind position. In the improved APF algorithm, the potential field is divided into the global potential field and the local potential field. In each movement process, the node with the lowest total potential energy among the eight surrounding nodes is selected;

[0099] The global potential field includes the attractive potential field generated by the target point, and the repulsive potential field generated by obstacles and covered areas;

[0100] In order to attract the unmanned sailboat to the target point, the following linear potential field P is established for each grid node P on the map: g :

[0101] P g =G g ·dist(P,P goal )

[0102] G g is the attraction gradient d ist(P,P goal ) is point to point P goal The Euclidean distance of

[0103] In order to make the unmanned sailboat avoid obstacles, at the obstacle node P obst Add a repulsive potential field P o ;

[0104]

[0105] Where k1 is an adjustable parameter

[0106] In order to prevent the unmanned sailboat from entering the covered area as much as possible, the covered node P covered Add a repulsive potential field P c ;

[0107]

[0108] Where: k2 is an adjustable parameter.

[0109] Furthermore, the local potential field is used to make the planned path conform to the movement law of the unmanned sailboat, and the local potential field includes a headwind potential field and a hysteresis potential field. The headwind potential field is used to prevent the unmanned sailboat from sailing in the restricted navigation area; the hysteresis potential field is used to reduce the turning of the unmanned sailboat during the forward movement, so that the unmanned sailboat reaches the target point faster.

[0110] The headwind potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat, and changes direction with the change of wind direction; any point P in the navigation restriction area w Wind potential energy P up The size of P wProportional to the distance to point P; outside the restricted navigation area, the wind potential energy is 0:

[0111]

[0112] Among them, G up is the gradient of the wind potential energy in the navigation restricted area, and φ represents the true wind direction angle;

[0113] The hysteresis potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat's position, and changes direction with the change of the sailboat's heading Ψ; the hysteresis potential field is set to be 0 in the same direction as the current heading, and any point P in the rest of the area w Hysteresis potential energy P h The size of P w The distance to point P is proportional to:

[0114]

[0115] Among them, G h It is the parameter that limits the turning of the sailboat.

[0116] During the unmanned sailboat coverage process, in order to reduce the number of times the sailboat crosses the covered area, a heading mark is left on the sailed route; when approaching the covered area, the heading vector is decomposed into a direction parallel to the main wind direction used in the assignment method and a direction perpendicular to the main wind direction. If there are other grids to walk on, the track that crosses the sailed route in the direction perpendicular to the main wind direction and the same heading direction as the covered route is not used.

[0117] Further, the contents of this application are described in conjunction with the contents of the accompanying drawings; Figure 1 It can be seen that when the actual wind direction angle is between -30° and 30°, there are areas where the sailboat has a low speed or even cannot navigate. Therefore, in path planning, considering the movement law of the unmanned sailboat, the wind direction is divided into different areas, and the grids in the unnavigable direction in the eight grids around the current grid are disabled; the wind direction pointing from the top of the map to the bottom is set to 0°, and the counterclockwise direction is taken as the positive direction.

[0118] For example Figure 2 As shown in the figure, when the true wind blows towards the sailboat from the direction of 295°, the direction from 265° to 325° is the navigation restriction area; corresponding to the grid map, as Figure 2 As shown, the grids to the right and to the upper right are restricted areas. During the movement, the unmanned sailboat will select the next direction of action from the directions outside the restricted areas.

[0119] The algorithm flow is shown as Figure 3As shown, at the beginning, the map is converted into a grid map according to the wind direction, and the grid map is assigned; according to the current position of the unmanned sailboat, it is determined whether there is an upwind area to be covered, if so, it goes to the relatively most upwind position, and the map is reassigned once each time it moves, and a heading mark is left in the area along the way; after reaching the relatively most upwind position, it is determined whether it is in a deadlock, if there is no upwind area to be covered, it is determined whether it is currently in a deadlock; if it is a deadlock, it escapes from the deadlock; if it is not a deadlock, the grid with the largest gradient value among the selectable grids in the eight nodes around the current position is selected according to the wind direction and the navigated path, and the grid map is reassigned after moving, and a heading mark is left.

[0120] After moving, determine whether the coverage is completed. If completed, exit the loop. If not, determine the wind direction and continue the loop.

[0121] like Figure 4 As shown, the wind direction at the beginning of the full coverage mission is selected as 0°, and the map is rasterized according to the wind direction; the direction of the assignment is changed according to the change of wind direction during the coverage mission. The wind direction is divided into eight directions and eight main wind directions are determined in the grid map.

[0122] For the assignment example of this application, in Figure 5 and Figure 6 has been shown; in Figure 5 In the figure, black grids represent obstacles. The wind direction is from top to bottom; for example, at node (1, 3), the total number of grids X counted along the main wind direction is 5, and the sort number a of the current grid along the wind direction is 3.

[0123] like Figure 6 As shown, wind direction and obstacles are Figure 5 The sailboat moves from position (2,1) to (5,4). In the third row, there are two continuous grid strings separated by obstacles and the navigated path, namely [(5,3)] and [(1,3), (2,3)]. Their Y values ​​are 1 and 2 respectively, and the b of grid (2,3) is 2. In the figure, the counting order of b is from left to right, and the counting direction can also be from right to left, which does not affect the final assignment of the grid.

[0124] During the coverage process, since the movement of the unmanned sailboat is restricted by the wind direction and cannot move directly in the opposite direction of the wind, it is necessary to go to a position suitable for full coverage tasks in order to reduce the double coverage rate; once the unmanned sailboat determines that there is an uncovered area in the upwind direction, it finds a relatively suitable position in the upwind direction as the target and goes to that position. Depending on the wind direction, the criteria for determining the existence of upwind uncovered areas are different. With the sailboat as the center, the wind direction from top to bottom is defined as 0°, and counterclockwise is positive. The method for determining the existence of upwind uncovered areas is shown in Table 1. If there is an upwind uncovered area in the coordinates after the current position of the sailboat is added to the value in the judgment position, it is considered that there is an upwind area to be covered; if there is an upwind area to be covered, the grid with the largest gradient value in the direction of the judgment position at the current position is found as the relative upwind position, and the improved APF method is used to go to the node. For example, when the wind direction is [75°, 105°], if it is determined that there is an upwind area to be covered, traverse the grid from the current position toward the (0, 1) direction and find the grid with the largest f(x, y) value as the target point; 60＜ When θ≤75, if it is determined that there is an uncovered upwind area, the grid is traversed from the current position toward the (0,1) and (1,1) directions to find the grid with the largest f(x,y) value.

[0125] Table 1 Determination location of upwind area to be covered under different wind directions

[0126]

[0127] It is particularly noted that since the unmanned sailboat can only select the next target grid from the eight grids around the current grid at a time, when there is no area to be covered around the unmanned sailboat, only covered grids or obstacles, the sailboat is considered to have entered a deadlock; specifically in the algorithm implementation, when the f(x,y) of the adjacent grids around the unmanned sailboat is less than 0, the sailboat is considered to have entered a deadlock. In order to escape the deadlock. In this paper, the breadth-first algorithm is used to find the nearest grid with a gradient value greater than 0, and the grid is reached through the improved APF method.

[0128] In summary, a full coverage path planning algorithm for an unmanned sailboat in an embodiment of the present invention rasterizes the map according to the wind direction when coverage starts, so that the wind direction when coverage starts points from the top of the grid map to the bottom, and then divides the wind direction into different areas according to the unmanned sailboat speed polar coordinate diagram, and disables the unnavigable directions in the eight grids around the current grid, so that the planned path changes with the change of wind direction, avoids the influence of the telecontrol environment and the constraints of external conditions, and can be fully applicable to the functional characteristics of the unmanned sailboat.

[0129] The specific implementation manner cannot be used as a limitation to the protection scope of the present invention. For those skilled in the art, any substitution, improvement or change made to the implementation manner of the present invention shall fall within the protection scope of the present invention.

[0130] The matters not described in detail in the present invention are all known technologies to those skilled in the art.

Claims

1. A full coverage path planning algorithm for an unmanned sailboat, characterized in that: The path planning algorithm comprises the following steps: S1, detecting and sensing the actual wind speed through a wind speed sensor; S2, setting the wind direction at the start of the full coverage mission to 0°, rasterizing the map in directions parallel and perpendicular to the wind direction, and then disabling some of the eight grids around the unmanned sailboat according to the unmanned sailboat VPP curve graph; S3, assigning a value to each node on the grid map, and then determining whether there is an upwind area on the grid map; S4, when there is an upwind area on the grid map, go to the relative maximum upwind position, reassign the grid map according to the track, and leave a navigation mark along the way; when there is no upwind area on the grid map, determine whether there is a dead zone on the grid map; S5, when there is a dead zone on the grid map, the unmanned sailboat leaves the dead zone and repeats step S3; when there is no dead zone on the grid map, the unmanned sailboat goes to the node with the largest value in the surrounding feasible grids, reassigns the grid map, and leaves a navigation mark; S6, judging whether the unmanned sailboat has passed through all target areas, and completing the full coverage path planning task of the unmanned sailboat.

2. A full coverage path planning algorithm for an unmanned sailboat according to claim 1, characterized in that: Assigning a value to each node on the grid map includes the following steps: S3.1, according to the polar coordinate diagram of the speed of the unmanned sailboat, the wind direction is divided into different areas, and the grids in the unnavigable direction among the eight grids around the current grid are disabled; S3.2, the wind direction pointing downward from the top of the map is set to 0°, and the counterclockwise direction is set as the positive direction, thereby dividing the wind direction into eight directions and determining eight main wind directions in the grid map; S3.3, when the wind direction θ is between [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], it is necessary to assign values ​​to the nodes in the grid map before coverage begins and after each step during coverage, and define the values ​​at the obstacle position: f(x, y) = -1 The assignment formula at the non-obstacle position is: In the formula: x, y represent the horizontal and vertical coordinates of the map; X is the total number of grids counted along the main wind direction in the map; a is the sequence number of the current grid in the grids counted along the main wind direction in the map; Y is the total number of grids in the continuous grid string counted along the direction perpendicular to the main wind direction and not separated by obstacles, navigated grids, and the current sailboat position; b is the sequence number of the current grid in the continuous grid string that is not separated by obstacles, navigated grids, and the current sailboat position along the direction perpendicular to the wind direction; after the sailboat enters a grid, the current grid value is reduced by 1; S3.4, when the wind direction θ changes to an area other than [-30°, 30°], [60°, 120°], [150°, 210°], [240°, 300°], if the area where the previous wind direction was located is adjacent to the current area, the value is assigned according to the previous assignment method; if not, the value is assigned randomly according to the assignment method of one of the two adjacent areas.

3. The full coverage path planning algorithm for an unmanned sailboat according to claim 1, characterized in that: Use the improved APF algorithm to reach the relative maximum upwind position. In the improved APF algorithm, the potential field is divided into the global potential field and the local potential field. In each movement process, the node with the lowest total potential energy among the eight surrounding nodes is selected; The global potential field includes the attractive potential field generated by the target point, and the repulsive potential field generated by obstacles and covered areas; In order to attract the unmanned sailboat to the target point, the following linear potential field P is established for each grid node P on the map: g : P g =G g ·dist(P,P goal ) G g is the attraction gradient dist(P,P goal ) is point to point P goal The Euclidean distance of In order to make the unmanned sailboat avoid obstacles, at the obstacle node P obst Add a repulsive potential field P o ; Where k1 is an adjustable parameter In order to prevent the unmanned sailboat from entering the covered area as much as possible, the covered node P covered Add a repulsive potential field P c ; Where: k2 is an adjustable parameter.

4. The full coverage path planning algorithm for an unmanned sailboat according to claim 3, characterized in that: The local potential field is used to make the planned path conform to the movement law of the unmanned sailboat. The local potential field includes a headwind potential field and a hysteresis potential field. The headwind potential field is used to prevent the unmanned sailboat from sailing in a restricted navigation area. The hysteresis potential field is used to reduce the turning of the unmanned sailboat during its advancement, so that the unmanned sailboat can reach the target point faster. The headwind potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat, and changes direction with the change of wind direction; any point P in the navigation restriction area w Wind potential energy P up The size of P w Proportional to the distance to point P; outside the restricted navigation area, the wind potential energy is 0: Among them, G up is the gradient of the wind potential energy in the navigation restricted area, and φ represents the true wind direction angle; The hysteresis potential field is centered at the current point P of the sailboat, moves with the movement of the sailboat's position, and changes direction with the change of the sailboat's heading Ψ; the hysteresis potential field is set to be 0 in the same direction as the current heading, and any point P in the rest of the area w Hysteresis potential energy P h The size of P w The distance to point P is proportional to: Among them, G h It is the parameter that limits the turning of the sailboat.

5. The full coverage path planning algorithm for an unmanned sailboat according to claim 1, characterized in that: Use the breadth-first algorithm to find the nearest grid with a gradient value greater than 0; first, create a queue and put the current node into the queue; then, take a node from the queue and check its adjacent nodes, which are the nodes directly adjacent to the current node, and add them to the queue, but do not add them to the queue when encountering obstacles; repeat this process, continuously take out nodes from the queue and explore their adjacent nodes, and use the improved APF method to go to the target grid after finding it.

6. The full coverage path planning algorithm for an unmanned sailboat according to claim 3, characterized in that: When the unmanned sailboat approaches the covered area, the heading vector is decomposed into a direction parallel to the main wind direction used for assignment and a direction perpendicular to the main wind direction. If there are other grids to travel, the track that crosses the navigated route in the same direction as the covered route in the direction perpendicular to the main wind direction is not used.

Images