Path planning method for unmanned surface vehicle based on improved ant colony algorithm and speed maintenance

Abstract

The present application provides a surface unmanned boat path planning method based on an improved ant colony algorithm and speed maintenance, which is used to plan the path of a high-speed unmanned boat, including the following steps: S1, establishing an environmental map model of the area to be planned; S2, using a conventional ant colony algorithm to perform global path planning on the environmental map model to obtain a first planned path; S3, performing global optimization on the first planned path based on the improved ant colony algorithm to obtain a second planned path; S4, performing a steering optimization operation based on speed maintenance on each turning point on the second planned path to obtain the optimal planned path for the area to be planned. The route planned by the method provided by the present application can enable the surface unmanned boat to maintain a high speed during navigation, thereby completing the entire route in the shortest time.

Description

Technical Field

[0001] The present application belongs to the field of path planning technology, and specifically relates to a path planning method for a surface unmanned boat based on an improved ant colony algorithm and speed maintenance. Background Art

[0002] Path planning is a key technology in the autonomous navigation of unmanned surface vehicles (USVs). USVs usually need to navigate autonomously in complex marine environments, avoid obstacles, and reach the target area by the shortest path or the optimized path according to specific mission requirements.

[0003] For the path planning problem of surface unmanned vehicles, the main research algorithms currently include genetic algorithms, ant colony algorithms and particle swarm algorithms. Among them, the particle swarm algorithm is an optimization algorithm based on swarm intelligence. This algorithm simulates the foraging behavior of biological groups such as bird flocks and fish schools, and uses the collaboration and information sharing mechanism between individuals in the group to quickly find the optimal solution. It has been widely used in the path planning of surface unmanned vehicles.

[0004] However, since the traditional ant colony algorithm is essentially an algorithm with a positive feedback mechanism, and the initial search of the ant colony algorithm is random, if the solution searched at the beginning is a local optimal solution and other adjacent solutions are obviously inferior to the local optimal solution, the ant colony algorithm is likely to eventually converge to the local optimal solution, resulting in the inability to search for the global optimal solution; in addition, the traditional ant colony algorithm generates multiple broken lines based on the shortest path goal. When the angle between two broken lines is an acute angle, the unmanned boat (especially the high-speed unmanned boat) generally needs to slow down when passing the intersection of the two broken lines to avoid the risk of capsizing caused by sharp turns at high speed. Frequent deceleration and acceleration not only increases the energy consumption of the unmanned boat, but also leads to an extension of the navigation time, making it impossible to achieve the shortest navigation time by sailing along the shortest path. For high-speed unmanned boats, the above problems are more obvious. Summary of the Invention

[0005] The purpose of this application is to provide a path planning method for a surface unmanned vehicle based on an improved ant colony algorithm and speed maintenance. By optimizing the traditional ant colony algorithm, a global optimal path solution is searched to keep the unmanned vehicle at a high speed. The method includes the following steps:

[0006] S1, establish an environmental map model of the area to be planned;

[0007] S2, performing global path planning using a conventional ant colony algorithm on the environment map model to obtain a first planned path;

[0008] S3, performing global optimization on the first planned path based on an improved ant colony algorithm to obtain a second planned path;

[0009] S4, performing a steering optimization operation based on maintaining the speed on each turning point on the second planned path to obtain an optimal planned path for the area to be planned.

[0010] Preferably, the environment map model is a MAKLINK graph, and the MAKLINK graph includes at least one obstacle area and multiple free areas.

[0011] Preferably, the first planned path is composed of at least two straight first sub-paths connected in sequence, and each turning point of the first planned path is the midpoint of a link line in the MAKLINK graph.

[0012] The second planned path is composed of at least two straight second sub-paths connected in sequence, and each turning point of the second planned path is located on the link line where each turning point in the first planned path is located in a one-to-one correspondence.

[0013] Furthermore, step S3 includes the following steps:

[0014] S31, performing a node splitting operation on the first link line based on the following formula:

[0015]

[0016] Wherein, the first link line is the link line where each turning point in the first planned path is located, L is any link line, H is the total number of equal divisions of L, and h is the sequence number of the division node;

[0017] S32, performing initialization operation of the improved ant colony algorithm, setting the ant colony size M, the upper limit of the number of cycles N, and the initial values ​​of the heuristic information and pheromone;

[0018] S33, repeatedly execute steps S331 to S336 until the upper limit N of the number of cycles is reached:

[0019] S331, select any ant in the ant colony;

[0020] S332, randomly setting the split node number of the ant on the first link line;

[0021] S333, generate a random number q between [0,1];

[0022] S334, traverse each first link line, and determine the ant's movement from the current first link line to the next one based on the following method

[0023] The path of the first link line and the real-time pheromone update of the path:

[0024]

[0025] Among them, i and j are the serial numbers of the split nodes where the ant is located on the current and next first link line respectively, τ i,j , δ i,j are the pheromone concentration and heuristic value of the path from point i to point j, α and β are the control coefficients of pheromone concentration and heuristic value, pheThres is the selection threshold, roulette methoud is the roulette method, and the transition probability p of the roulette method is i,j As shown in the following formula:

[0026]

[0027] Among them, S is the set of split nodes on the next first link line, τ i,s , δ i,s are the pheromone and heuristic values ​​of the path from point i to point s respectively;

[0028] S335, reselect an ant and return to step S332 until all M ants in the ant colony have been selected;

[0029] S336 , selecting a shortest path from the paths passed by all the M ants as a second planned path, and performing a global update on the pheromones on the second planned path.

[0030] Furthermore, step S4 includes the following steps:

[0031] S410, traverse each turning point in the second planned path, and execute steps S421 to S423 for each turning point:

[0032] S421, determine whether the lengths of the two second sub-paths connected by the turning point are both greater than length and whether the angle between them is greater than 60 degrees. If yes, execute step S422; if not, execute step S423, where the lower limit of length is

[0033] twice the turning radius of the unmanned surface vehicle when passing the turning point at its expected speed;

[0034] S422, performing a shearing operation on two second subpaths connected to the turning point to optimize the turning point;

[0035] S423, performing an expansion operation on two second subpaths connected to the turning point to optimize the turning point;

[0036] S430: Connect the sub-paths obtained through the steering optimization end to end in accordance with the navigation order of the unmanned surface vehicle to obtain the optimal planned path.

[0037] Preferably, the shearing operation described in step S422 is specifically as follows: taking the turning point as the starting point, cutting a length less than length on the two second sub-paths connected thereto to obtain two shearing points, deleting the turning point and connecting the two shearing points, thereby obtaining the sub-path that has undergone the shearing operation.

[0038] Preferably, the expansion operation in step S423 is specifically as follows: starting from the turning point, continuously performing an extension-rotation operation to obtain multiple expansion points, and then sequentially connecting each expansion point to obtain a sub-path that has undergone the expansion operation, wherein the first expansion point is located on the heading of one of the second sub-paths connected to the turning point, and in each extension-rotation operation, the extension length is not less than length.

[0039] Preferably, between steps S1 and S2, a step of expanding the obstacle area is further included, wherein the expansion distance is the turning radius of the unmanned surface boat when turning according to its expected speed.

[0040] The embodiment of the present application provides a surface unmanned boat path planning method based on an improved ant colony algorithm and speed maintenance. A marine environment map model is established through MAKLINK graph theory, thereby improving the environmental adaptability and accuracy of path planning. The search results of the traditional ant colony algorithm are globally optimized through the improved ant colony algorithm to solve the local extreme value problem. Furthermore, in order to ensure that the surface unmanned boat can complete navigation while maintaining a high speed, each acute-angle turning segment combination in the globally optimized path is blunted, and finally the optimal planned path that can complete navigation in the shortest navigation time is obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 This is a flow chart of a path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to an embodiment of the present application;

[0042] Figure 2 A schematic diagram of an environment map model provided according to an embodiment of the present application;

[0043] Figure 3 A schematic diagram of optional paths in an environment map model provided according to an embodiment of the present application;

[0044] Figure 4 A flowchart of a conventional ant colony search algorithm provided according to an embodiment of the present application;

[0045] Figure 5 A schematic diagram of a first planned path provided according to an embodiment of the present application;

[0046] Figure 6A flowchart of global optimization of a first planned path based on an improved ant colony algorithm according to an embodiment of the present application;

[0047] Figure 7 A schematic diagram of node segmentation and searching for a second planned path according to an embodiment of the present application;

[0048] Figure 8 A schematic diagram of a shearing operation according to an embodiment of the present application;

[0049] Figure 9 A schematic diagram of an expansion operation provided according to an embodiment of the present application;

[0050] Figure 10 is a schematic diagram of a collision with an obstacle after a shear operation in some embodiments;

[0051] Figure 11 A schematic diagram of an expansion operation on an obstacle according to an embodiment of the present application;

[0052] Figure 12 A schematic diagram of the principle of avoiding shear segment conflicts after performing an expansion operation on an obstacle according to an embodiment of the present application;

[0053] Figure 13 Schematic diagram of the optimal planning path obtained according to an embodiment of the present application. DETAILED DESCRIPTION

[0054] Hereinafter, the present application will be further described based on preferred embodiments with reference to the accompanying drawings.

[0055] In the description of the embodiments of the present application, it should be noted that if the terms "upper", "lower", "inner", "outer" and the like indicate an orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or are the orientation or positional relationship in which the products of the embodiments of the present application are usually placed when in use, they are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application. In addition, in the description of the present application, in order to distinguish different units, words such as first and second are used in this specification, but these are not limited by the order of manufacture, nor can they be understood as indicating or implying relative importance. Their names may be different in the detailed description and claims of the present application.

[0056] The present application provides a surface unmanned boat path planning method based on an improved ant colony algorithm and speed maintenance through an embodiment. The method is used to plan the path of a high-speed unmanned boat (for example, an unmanned boat with a continuous cruising speed greater than or equal to 25 knots, or a maximum speed of 50 knots). Figure 1 The implementation flow chart of this method is as follows: Figure 1 As shown, the method includes the following steps:

[0057] S1, establish an environmental map model of the area to be planned;

[0058] S2, performing global path planning using a conventional ant colony algorithm on the environment map model to obtain a first planned path;

[0059] S3, performing global optimization on the first planned path based on an improved ant colony algorithm to obtain a second planned path;

[0060] S4, performing a steering optimization operation based on maintaining the speed on each turning point on the second planned path to obtain an optimal planned path for the area to be planned.

[0061] In each of the above steps, first, a planning area for the surface unmanned boat to navigate is established through step S1, and then a first planning path is obtained through step S2; further, the first planning path is used as a suboptimal path, and the improved ant colony algorithm of step S3 is used to perform global optimization to obtain a second planning path; the second planning path after global optimization is composed of multiple broken lines. In order to ensure that the surface unmanned boat can complete the navigation while maintaining a high speed, finally, step S4 is used to perform a steering optimization process on each turning point in the second planning path, and finally the optimal planning path that can complete the navigation in the shortest navigation time is obtained.

[0062] The specific implementation of each of the above steps will be described in detail below with reference to the accompanying drawings.

[0063] A. Construction of map model.

[0064] In an embodiment of the present application, step S1 can construct an environmental map model of the area to be planned based on pre-acquired satellite remote sensing imaging data, geographic information data, etc.

[0065] The environmental map model can be constructed using various formats known to those skilled in the art, such as grid maps, link maps, etc. Among them, MAKLINK, as a common link map format, can represent the sea surface and obstacles with nodes and link lines between nodes. Compared with the grid map, the data volume is small and it is conducive to obstacle avoidance. In the preferred embodiment of the present application, in order to facilitate the implementation of the subsequent path planning step based on the ant colony algorithm, the environmental map model uses the MAKLINK map format.

[0066] Figure 2In a specific embodiment, an environmental map model constructed using a MAKLINK graph is shown. As shown in the figure, the MAKLINK graph includes at least one obstacle area (the black part in the figure represents obstacles such as islands and sea surface facilities) and multiple free areas (the white part in the figure represents the sea surface that can be navigated freely), wherein each obstacle is represented by a polygon, and the intersection of any two polygons represents a node. Furthermore, the free area is divided by linking multiple boundary nodes passing through the planning area and the nodes of each obstacle. The above-mentioned method of constructing an environmental map model of the planning area through a MAKLINK graph is already known to those skilled in the art and will not be repeated here.

[0067] B. Preliminary path planning.

[0068] In an embodiment of the present application, step S2 searches in the above MAKLINK graph through a conventional ant colony algorithm, thereby obtaining a first planned path as a suboptimal path, providing a basis for further optimization.

[0069] The Ant Colony Search Algorithm (ACA) is a heuristic algorithm that mimics the foraging behavior of ants in nature and is widely used in path planning, optimization, and other combinatorial optimization problems. It relies on the exchange of information and collaboration between ants, simulating the pheromones left by ants during their foraging process to guide path search. Paths with higher pheromone concentrations are more likely to be chosen by ants, resulting in an optimized path. The basic process involves multiple "ants" moving along feasible paths. At each step, the ants determine the path they choose based on pheromone concentration and a heuristic function. After completing a path, they update the pheromone concentration along the path. Successfully found paths increase their pheromone concentration, while unselected or inferior paths gradually decrease their concentration through pheromone volatilization. Through multiple iterations, the pheromone concentration is continuously updated, gradually guiding the ants toward more optimal paths. As iterations proceed, paths with higher pheromone concentrations become more favored, leading to a convergence towards an optimal solution.

[0070] Specifically, in the embodiment of the present application, when using the MAKLINK graph to perform path search, it can be as follows: Figure 3 As shown, first connect the midpoints of each chain line to obtain various optional paths, and then pass through them. Figure 4 The conventional ant colony search algorithm shown obtains a first planning path. Figure 5 Shows the Figure 3 The results of executing the conventional ant colony search algorithm. The green lines represent the three first-planned paths obtained by the search. Each route has the same starting point but different end points, representing the paths starting from the same starting point and reaching three different end points.

[0071] As shown in the figure, the first planned path obtained in step S2 is composed of at least two straight sub-paths (in this application, each sub-path in the first planned path is referred to as a first sub-path) connected in sequence, and each turning point of the first planned path is the midpoint of a link line in the MAKLINK diagram.

[0072] C. Global optimization based on the shortest path.

[0073] In an embodiment of the present application, in order to solve the problem that the conventional ant colony search algorithm may fall into a local extreme value trap, resulting in the inability to obtain a global optimal solution, after executing step S2, the first planned path is globally optimized through the improved ant colony search algorithm of step S3. The goal of global optimization is to obtain the shortest planned path. In an embodiment of the present application, this path is also referred to as the second planned path.

[0074] In some preferred embodiments, Figure 6 As shown, step S3 further includes the following steps:

[0075] S31, performing a node splitting operation on the first link line based on the following formula:

[0076]

[0077] Wherein, the first link line is the link line where each turning point in the first planned path is located, L is any link line, H is the total number of equal divisions of L, and h is the sequence number of the division node;

[0078] S32, performing initialization operation of the improved ant colony algorithm, setting the ant colony size M, the upper limit of the number of cycles N, and the initial values ​​of the heuristic information and pheromone;

[0079] S33, repeatedly execute steps S331 to S336 until the upper limit N of the number of cycles is reached:

[0080] S331, select any ant in the ant colony;

[0081] S332, randomly setting the split node number of the ant on the first link line;

[0082] S333, generate a random number q between [0,1];

[0083] S334, traverse each first link line, and determine the ant's movement from the current first link line to the next one based on the following method

[0084] The path of the first link line and the real-time pheromone update of the path:

[0085]

[0086] Among them, i and j are the serial numbers of the split nodes where the ant is located on the current and next first link line respectively, τ i,j , δ i,j are the pheromone concentration and heuristic value of the path from point i to point j, α and β are the control coefficients of pheromone concentration and heuristic value, pheThres is the selection threshold, roulette methoud is the roulette method, and the transition probability p of the roulette method is i,j As shown in the following formula:

[0087]

[0088] Among them, S is the set of split nodes on the next first link line, τ i,s , δ i,s are the pheromone and heuristic values ​​of the path from point i to point s respectively;

[0089] S335, reselect an ant and return to step S332 until all M ants in the ant colony have been selected;

[0090] S336 , selecting a shortest path from the paths passed by all the M ants as a second planned path, and performing a global update on the pheromones on the second planned path.

[0091] The following steps S3 are described in detail

[0092] 1) Node splitting

[0093] In each of the above steps, the link line (i.e., the first link line) where the first planned path is located is first further segmented through step S31 to obtain the segmentation nodes thereon. The subsequent global optimization is to select segmentation nodes that are better than the midpoint from each first link line, so that the planned path is further shortened.

[0094] In some preferred embodiments, H may be set to 10, ie, the first link line is divided into 10 equal parts.

[0095] 2) Algorithm initialization

[0096] Step S32 initializes the algorithm parameters, including the ant population size M, the upper limit of the number of loops N, the initial values ​​of the heuristic information and pheromones (generally, pheromones include information calculation parameters and pheromone update parameters). Pheromones can be stored in the format of a pheromone matrix. In addition, a search result record matrix and an evolution process record matrix can be further configured.

[0097] In some preferred embodiments, M and N can be set to 10 and 2000 respectively, the heuristic information of each selectable path can be set to the inverse of its distance, and the pheromone calculation parameter setting and the pheromone update parameter can be set to 2 and 0.1 respectively.

[0098] 3) Global search optimization

[0099] Steps S331 to S336 are executed cyclically, wherein for each ant in each cycle, different strategies are used to reselect split nodes on each first link in turn according to whether the random number q exceeds the selection threshold pheThres, and connect them to obtain a new path. When q≤pheThres, argmax{|τ i,j | α ·|δ i,j | β Sequentially select the split node number for jumping from the previous first link to the next first link. This method directly selects the split node with the largest pheromone and heuristic value. In the embodiments of this application, this is referred to as the pheromone-heuristic value method. When q > pheThres, a conventional roulette wheel method is used to randomly determine the split node number for jumping to the next first link. In this way, deterministic and random perturbations can be continuously generated based on the path obtained in step S2, effectively ensuring that the optimization result can escape the local convergence value.

[0100] Figure 7 The red broken line in FIG. 1 shows a schematic diagram of various search paths generated during the global search optimization process in a specific embodiment.

[0101] By setting the value of pheThres, the tendency between the deterministic jump mode and the random jump mode can be adjusted. In some preferred embodiments, pheThres is greater than 0.5 and closer to 1. For example, pheThres is set to 0.8 to tilt the optimization result toward a better pheromone-inspiration value.

[0102] 4) Pheromone Update

[0103] In the embodiment of the present application, pheromone update is performed in two steps: after each ant completes a path search, the pheromone of the path it obtains is updated in real time; after all ants in the ant population complete a round of path search, the pheromone of the shortest path is updated globally.

[0104] In some specific embodiments, real-time pheromone updates are shown as follows:

[0105] τ i,j =(1-μ)·τ i,j+μ·τ0,

[0106] Among them, τ i,j is the pheromone on the line segment connected by the split nodes i and j on two adjacent first link lines, μ is the volatile concentration of the pheromone and 0<μ<1, and τ0 is the fixed parameter of the pheromone.

[0107] In some specific embodiments, the global pheromone update is shown as follows:

[0108]

[0109] Among them, minlen is the length of the shortest path currently searched.

[0110] Through the above step S3, a path after global optimization using the improved ant colony algorithm will be obtained. In the embodiment of the present application, this path is called the second planned path. Obviously, the second planned path is composed of at least two straight sub-paths connected in sequence (correspondingly, each sub-path in the second planned path is called a second sub-path), and the starting and ending points of each straight segment are both split nodes of the first link line (which may be the midpoint of the first link line or other split nodes on the first link line), that is: each turning point of the second planned path is located one-to-one on the link line where each turning point in the first planned path is located, and the total length of the second planned path is less than or equal to the first planned path.

[0111] D. Path optimization based on speed maintenance.

[0112] As analyzed above, although the second planned path further shortens the length of the entire path based on the first planned path, it is still a broken line composed of multiple straight sub-paths. For surface unmanned boats with higher speeds, when the angle between two adjacent sub-paths is small (for example, the angle between the two sub-paths is close to a right angle, or even a sharper acute angle), when it turns at the turning point of each sub-path, it is bound to need to decelerate and then re-accelerate after passing the turning point. This frequent acceleration and deceleration operation will not only cause a waste of power for the unmanned boat, but also make it impossible for the surface unmanned boat that needs to maintain a high speed to complete the navigation in the shortest navigation time when sailing according to the above-mentioned second planned path.

[0113] To this end, in an embodiment of the present application, a global steering optimization operation is performed through step S4. Specifically, step S4 further includes the following steps:

[0114] S410, traverse each turning point in the second planned path, and execute steps S421 to S423 for each turning point:

[0115] S421, determine whether the lengths of the two second sub-paths connected by the turning point are both greater than length and whether the angle between them is greater than 60 degrees. If yes, execute step S422; if not, execute step S423, where the lower limit of length is

[0116] twice the turning radius of the unmanned surface vehicle when passing the turning point at its expected speed;

[0117] S422, performing a shearing operation on two second subpaths connected to the turning point to optimize the turning point;

[0118] S423, performing an expansion operation on two second subpaths connected to the turning point to optimize the turning point;

[0119] S430: Connect the sub-paths obtained through the steering optimization end to end in accordance with the navigation order of the unmanned surface vehicle to obtain the optimal planned path.

[0120] In each of the above steps, the turning point situation is further refined in S421. In the case where the included angle is greater than 60° and the lengths of the two second sub-paths are long, a shearing operation is directly performed in step S422 to transform the included angle α of the two second sub-paths into a larger turning angle by generating a shearing sub-path. Specifically, Figure 8 As shown, the cutting operation in step S422 is specifically as follows: starting from the turning point, the two second sub-paths ( Figure 8 A length less than length is intercepted on subpath 1 and subpath 2 in the subpath to obtain two cutting points (cutting point 1 and cutting point 2), the turning point is deleted and the two cutting points are connected to obtain the cutting subpath after the cutting operation.

[0121] If the angle α between the two second sub-paths is less than 60°, for example Figure 9 The two second subpaths (subpath 3 and subpath 4) shown in the figure indicate that the unmanned ship will make a sharp turn here. At this time, a shearing operation is used. In order to ensure that the length of the shearing subpath is long enough to achieve high-speed turning, the distance from the shearing point to the original turning point needs to be greatly extended. However, since such a sharp-angle turning point generally appears when the planned subpath described later is close to an obstacle, this will increase the probability of the shearing subpath colliding with the obstacle. Therefore, it is necessary to expand the two second subpaths connected by such a sharp-angle turning point to ensure that the unmanned ship passes the turning point at a high speed.

[0122] Specifically, if Figure 9 As shown, the expansion operation in step S423 is: starting from the turning point, continuously performing the extension-rotation operation to obtain multiple expansion points ( Figure 9Then, multiple subpaths that have undergone expansion operations are obtained by connecting each expansion point in sequence ( Figure 9 , wherein the first expansion point is located on the heading of one of the second subpaths (subpath 3) connected to the turning point, and in each extension-rotation operation, the extension length is not less than length.

[0123] In some preferred embodiments, in order to achieve the high-speed unmanned boat returning to the original second sub-path along the optimal path while maintaining the original speed, in the expansion operation of step S423, as shown in FIG. Figure 9 As shown, the number of expansion points and expansion sub-segments is 4, the rotation angle of the second expansion sub-path relative to the first expansion sub-path is 90°-α (α is the angle between the two second sub-paths, i.e., sub-path 3 and sub-path 4), the rotation angle of the third expansion sub-path relative to the second expansion sub-path and the rotation angle of the fourth expansion sub-path relative to the third expansion sub-path are both 90°, and the fourth expansion point coincides with the original turning point. With the above settings, the fourth expansion sub-path can be kept perpendicular to the original sub-path 4, so that the high-speed unmanned vehicle can enter sub-path 4 from sub-path 3 with its minimum achievable turning radius without slowing down.

[0124] E. Expansion operation of obstacles.

[0125] In some embodiments, the second planned path obtained in step S2 may have two adjacent second sub-paths that are close to the edge of the obstacle. In this case, the angle between them is generally an acute turning point. If a shearing operation is performed directly on them, such as Figure 10 As shown, the clipping sub-path may conflict with the obstacle. Therefore, in some preferred embodiments, between steps S1 and S2, a step of expanding the obstacle area is also included, wherein the expansion distance is the turning radius of the surface unmanned boat when turning according to its expected speed.

[0126] Specifically, if Figure 11 As shown, the turning radius of the high-speed unmanned boat when turning according to its expected speed can be set as the expansion distance D safe , and then expand the boundary of the obstacle outward D safe As the new obstacle boundary (virtual boundary), by using this expansion operation, the turning points obtained in the subsequent steps S2 and S3 will be far away from the real boundary, so as to ensure that when the shearing operation is performed in step S4, Figure 12 As shown in the figure, the cut segment only intersects the virtual boundary and does not conflict with the real boundary.

[0127] Figure 13 The optimal planning path from the starting point to the three target points is obtained after the above improved ant colony algorithm and global steering optimization operation. Figure 13 It can be seen that the path planned through the above steps obviously takes into account the path length and the speed during turning, so that the surface unmanned boat can travel from the starting point to the end point without slowing down throughout the whole process.

[0128] Table 1 and Table 2 respectively Figure 5 The traditional ant colony algorithm shown and Figure 13 The lengths and navigation times of the three planned paths obtained by the algorithm provided by this application are compared.

[0129] Table 1 Comparison of path lengths before and after optimization

[0130]

[0131] Table 2 Comparison of navigation time before and after optimization of each route

[0132]

[0133] By comparison, it can be found that when the ant colony algorithm is used alone for path planning, the obtained path is a multi-segment broken line, which requires frequent deceleration and acceleration when the speed of the surface unmanned boat is high, thereby reducing the average speed of the entire route; and the optimal planning path of the surface unmanned boat obtained by the method provided by the present application, compared with the traditional ant colony algorithm, not only shortens the total length of the entire route, but also optimizes the turning points formed by connecting each straight line sub-path, so that the surface unmanned boat can complete the entire route while maintaining a high speed, thereby significantly shortening the navigation time.

[0134] The above is a detailed introduction to the specific implementation methods of the present application. For those skilled in the art, several improvements and modifications can be made to the present application without departing from the principles of the present application. These improvements and modifications also fall within the scope of protection of the claims of the present application.

Claims

1. A surface unmanned boat path planning method based on an improved ant colony algorithm and speed maintenance, used for planning the path of a high-speed unmanned boat, characterized in that: The following steps are involved: S1, establish an environmental map model of the area to be planned; S2, performing global path planning using a conventional ant colony algorithm on the environment map model to obtain a first planned path; S3, performing global optimization on the first planned path based on an improved ant colony algorithm to obtain a second planned path; S4, performing a steering optimization operation based on maintaining the speed on each turning point on the second planned path to obtain an optimal planned path for the area to be planned; Step S4 further includes the following steps: S410, traverse each turning point in the second planned path, and execute steps S421 to S423 for each turning point: S421, determine whether the lengths of the two second sub-paths connected by the turning point are both greater than And whether the angle between the two is greater than 60 degrees, if yes, then execute step S422, if not, then execute step S423, wherein, The lower limit is twice the turning radius of the unmanned surface vehicle when it passes the turning point at its expected speed; S422, performing a shearing operation on two second subpaths connected to the turning point to optimize the turning point; S423, performing an expansion operation on two second subpaths connected to the turning point to optimize the turning point; S430, connecting the sub-paths obtained through the steering optimization end to end in accordance with the navigation order of the unmanned surface vehicle to obtain the optimal planned path; The specific cutting operation in step S422 is: starting from the turning point, cutting a smaller than to obtain two cutting points, delete the turning point and connect the two cutting points to obtain the subpath after the cutting operation; The expansion operation described in step S423 is specifically as follows: starting from the turning point, continuously performing the extension-rotation operation to obtain multiple expansion points, and then sequentially connecting each expansion point to obtain a sub-path that has undergone the expansion operation, wherein the first expansion point is located on the heading of one of the second sub-paths connected to the turning point, and in each extension-rotation operation, the extension length is not less than ; The number of expansion points and expansion subpaths is 4, and the rotation angle of the second expansion subpath relative to the first expansion subpath is ,in is the angle between the two second sub-paths, the rotation angle of the third expansion sub-path relative to the second expansion sub-path and the rotation angle of the fourth expansion sub-path relative to the third expansion sub-path are both 90°, and the fourth expansion point coincides with the original turning point.

2. The path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 1 is characterized in that: The environment map model is a MAKLINK graph, which includes at least one obstacle area and multiple free areas.

3. The path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 2 is characterized in that: The first planned path is composed of at least two straight first sub-paths connected in sequence, and each turning point of the first planned path is the midpoint of a link line in the MAKLINK graph.

4. The path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 1 is characterized in that: The second planned path is composed of at least two straight second sub-paths connected in sequence, and each turning point of the second planned path is located on the link line where each turning point in the first planned path is located in a one-to-one correspondence.

5. The path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 4 is characterized in that: Step S3 further includes the following steps: S31, performing a node splitting operation on the first link line based on the following formula: ， The first link line is the link line where each turning point in the first planned path is located. For any link line, For The total number of equal divisions, is the sequence number of the split node; S32, performing initialization operation of the improved ant colony algorithm, setting the ant colony size M, the upper limit of the number of cycles N, and the initial values ​​of the heuristic information and pheromone; S33, repeatedly execute steps S331 to S336 until the upper limit N of the number of cycles is reached: S331, select any ant in the ant colony; S332, randomly setting the split node number of the ant on the first link line; S333, generates a random number between [0,1] ; S334: traverse each first link line, determine the path of the ant moving from the current first link line to the next first link line based on the following method, and perform real-time pheromone update on the path: ， in, 、 are the sequence numbers of the split nodes where the ant is located on the current and next first link lines, 、 Points Solstice The pheromone concentration and heuristic value of the path, 、 are the control coefficients of pheromone concentration and heuristic value, To select the threshold, is a roulette method, the transition probability of the roulette method is As shown in the following formula: ， in, is the set of split nodes on the next first link line, 、 Points Solstice The pheromone and heuristic values ​​of the path; S335, reselect an ant and return to step S332 until all M ants in the ant colony have been selected; S336 , selecting a shortest path from the paths passed by all the M ants as a second planned path, and performing a global update on the pheromones on the second planned path.

6. The path planning method for an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 2, characterized in that: Between steps S1 and S2, there is also included a step of expanding the obstacle area, wherein the expansion distance is the turning radius of the unmanned surface boat when turning according to its expected speed.

Images