Unmanned surface vehicle path planning method based on improved ant colony algorithm and navigational speed keeping

Abstract

The invention provides an unmanned surface vehicle path planning method based on an improved ant colony algorithm and navigational speed keeping, which is used for planning the path of a high-speed unmanned surface vehicle and comprises the following steps: S1, establishing an environment map model of a to-be-planned area; s2, performing global path planning on the environment map model by using a conventional ant colony algorithm to obtain a first planned path; s3, performing global optimization on the first planning path based on an improved ant colony algorithm to obtain a second planning path; and S4, performing steering optimization operation based on navigational speed keeping on each steering point on the second planning path to obtain an optimal planning path of the to-be-planned area. According to the route planned by the method provided by the invention, the unmanned surface vehicle can maintain a relatively high navigational speed in the navigation process, so that the whole route can be completed in the shortest time.

Description

Technical Field

[0001] The present application belongs to the technical field of path planning, 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] In the autonomous navigation of unmanned surface vehicles (USVs), path planning is a key technology. 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 boats, 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 boats.

[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 converge to the local optimal solution in the end, 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 speed reduction and acceleration not only increases the energy consumption of the unmanned boat, but also leads to an extension of the navigation time, so that sailing along the shortest path cannot achieve the minimum navigation time. 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 boat 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 boat 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 map, and the MAKLINK map includes at least one obstacle area and a plurality of 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] Further, 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, cyclically 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 real-time pheromone updates on 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 below:

[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 globally updating the pheromones on the second planned path.

[0030] Further, 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 the two 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 boat when passing the turning point at its estimated speed;

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

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

[0036] S430, connecting the sub-paths obtained through the steering optimization in sequence according to the navigation order of the unmanned surface vehicle to obtain the optimal planned path.

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

[0038] Preferably, the expansion operation described in step S423 is specifically: starting from the turning point, continuously performing the extension-rotation operation to obtain multiple expansion points, and then obtaining a sub-path that has undergone the expansion operation by connecting each expansion point in sequence, 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, 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 at 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 the navigation while maintaining a relatively high speed, each sharp-angle turning segment combination in the globally optimized path is blunted, and finally the optimal planned path that can complete the navigation in the shortest navigation time is obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A flow chart of a method for path planning of an unmanned surface boat 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 provided according to an embodiment of the present application;

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

[0050] Fig.10 is a schematic diagram of a conflict with an obstacle after a shear operation in some embodiments;

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

[0052] Fig.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] Fig.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", "inside", "outside", etc. indicate an orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship in which the products of the embodiments of the present application are usually placed when in use, it is only for the convenience of describing the present application and simplifying the description, and does 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, the words first, second, etc. 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, and their names may be different in the detailed description and claims of the present application.

[0056] The present application provides a method for planning a path for a surface unmanned boat based on an improved ant colony algorithm and speed maintenance through an embodiment. The method is used to plan a path for 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 comprises 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, firstly, a planned area for the surface unmanned boat to navigate is established through step S1, and then a first planned path is obtained through step S2; further, the first planned path is used as a suboptimal path, and a second planned path is obtained by performing global optimization on it through the improved ant colony algorithm in step S3; the second planned 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 planned path, and finally the optimal planned path that can complete the navigation in the shortest navigation time is obtained.

[0062] The specific implementation of each of the above steps is 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 graphs, link graphs, etc. Among them, MAKLINK, as a common link graph format, can represent the sea surface and obstacles with nodes and link lines between nodes. Compared with the grid graph, 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 steps based on the ant colony algorithm, the environmental map model uses the MAKLINK graph 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 freely navigated), 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 is a heuristic algorithm that simulates the foraging behavior of ants in nature. It is widely used in path planning, optimization problems, and other combinatorial optimization problems. It is based on the exchange and collaboration of information between ants. It guides path search by simulating the pheromone left by ants in the process of looking for food. The higher the pheromone concentration, the greater the probability of being selected by ants, thus forming an optimized path. Its basic steps are to let multiple "ants" move on a feasible path. At each step, the ants decide the selected path based on the pheromone concentration and the heuristic function, and update the pheromone on the path after completing the path. The successfully found path will increase the pheromone concentration, while the unselected or poor path will gradually reduce the pheromone concentration through the pheromone volatilization mechanism. Through multiple iterations, the pheromone is continuously updated, gradually guiding the ants to a better path. As the iteration proceeds, the path with a high pheromone concentration will be more favored, forming an optimal solution that tends to converge.

[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 result of executing the conventional ant colony search algorithm. The green lines represent the three first planning paths obtained by the search. The starting points of each route are the same, but the end points are different. They represent 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 line 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 the local extreme value trap and fail to obtain the 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, cyclically 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 real-time pheromone updates on 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 below:

[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 globally updating the pheromones on the second planned path.

[0091] The following steps S3 are described in detail

[0092] 1) Node segmentation

[0093] In the above steps, first, step S31 is used to further segment the link line (i.e., the first link line) where the first planned path is located 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 parameters of the algorithm, including setting the ant population M, the upper limit of the number of cycles N, the initial values ​​of the heuristic information and pheromone (generally, pheromone includes 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 set.

[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 line in turn according to whether the random number q exceeds the selection threshold pheThres, and connect them to obtain a new path, wherein when q≤pheThres, argmax{|τ i,j | α ·|δ i,j | β} Sequentially select the split node number that jumps from the previous first link line to the next first link line. This method directly selects the split node with the largest pheromone and heuristic value. In the embodiment of the present application, it is called the pheromone-heuristic value method; when q>pheThres, the conventional roulette method is used to randomly determine the split node number that jumps to the next first link line. In this way, deterministic superposition random disturbances can be continuously generated on the basis of the path obtained in step S2, effectively ensuring that the optimization result can jump out of 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 relatively close 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 updating is performed in two steps: after each ant completes a path search, the obtained path is updated with pheromones in real time; after all ants in the ant population complete a round of path search, the shortest path is updated with global pheromones.

[0104] In some specific embodiments, the real-time pheromone update is 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 the two adjacent first link lines, μ is the pheromone volatility concentration and 0<μ<1, τ 0 is the fixed parameter of 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, the 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 line segment are both segmentation nodes of the first link line (which may be the midpoint of the first link line, or may be other segmentation 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 a surface unmanned boat with a higher speed, 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), it is bound to need to decelerate when turning at the turning point of each sub-path, and then re-accelerate after passing the turning point. Such frequent acceleration and deceleration operations will not only waste the power of 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 the two 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 boat when passing the turning point at its estimated speed;

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

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

[0119] S430, connecting the sub-paths obtained through the steering optimization in sequence according to 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. For the case where the angle is greater than 60° and the lengths of the two second sub-paths are relatively long, a shearing operation is directly performed in step S422 to transform the angle α of the two second sub-paths into a larger turning angle by generating a shearing sub-path. Specifically, Figure 8 As shown, the shearing 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 from 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 a cutting subpath after the cutting operation.

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

[0122] Specifically, Fig. 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 ( Fig. 9Then, multiple sub-paths that have undergone expansion operations are obtained by connecting each expansion point in sequence ( Fig. 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 by the optimal path while maintaining the original speed, in the expansion operation of step S423, as shown in FIG. Fig. 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 boat can enter sub-path 4 from sub-path 3 with the minimum turning radius it can achieve without slowing down.

[0124] E. Expansion operation of obstacles.

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

[0126] Specifically, Fig.11 As shown in the figure, 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 a new obstacle boundary (virtual boundary), by using this expansion operation, each turning point 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, Fig.12 As shown in FIG. 1 , the cut segment only intersects the virtual boundary and does not conflict with the real boundary.

[0127] Fig.13 The optimal planning path from the starting point to the three target points is finally obtained after the above-mentioned improved ant colony algorithm and global steering optimization operation. Fig.13 It can be seen that the path planned by 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 Fig.13 The lengths and navigation times of the three planned paths obtained by the algorithm provided by the present application are compared.

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

[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, so when the speed of the surface unmanned boat is high, it is necessary to frequently slow down and accelerate, thereby reducing the average speed of the entire route; and the optimal planning path of the surface unmanned ship 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 may 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 method for planning a path for a surface unmanned boat based on an improved ant colony algorithm and speed maintenance, which is used to plan the path of a high-speed unmanned boat, and is 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.

2. The method for path planning of 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 map, which includes at least one obstacle area and multiple free areas.

3. The path planning method for unmanned surface boat based on 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 method for path planning of an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 1, 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 method for path planning of unmanned surface boat based on improved ant colony algorithm and speed maintenance according to claim 4 is characterized in that: Step S3 further comprises the following steps: S31, performing a node splitting operation on the first link line based on the following formula: 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; 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, cyclically 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, generate a random number q 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: 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 method is the roulette method, and the transition probability p of the roulette method is i,j As shown below: 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; 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 globally updating the pheromones on the second planned path.

6. The method for path planning of unmanned surface boat based on improved ant colony algorithm and speed maintenance according to claim 1 is characterized in that: Step S4 further comprises 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 length and whether the angle between the two is greater than 60 degrees, if yes, execute step S422, if not, execute step S423, wherein the lower limit of length is twice the turning radius of the unmanned surface vehicle when passing through the turning point at its expected speed; S422, performing a shearing operation on two second sub-paths connected to the turning point to optimize the turning point; S423, performing an expansion operation on two second sub-paths connected to the turning point to optimize the turning point; S430, connecting the sub-paths obtained through the steering optimization in sequence according to the navigation order of the unmanned surface vehicle to obtain the optimal planned path.

7. The method for path planning of an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 6 is characterized in that: The clipping operation described in step S422 is specifically: taking the turning point as the starting point, respectively cutting a length less than length on the two second sub-paths connected thereto to obtain two clipping points, deleting the turning point and connecting the two clipping points, thereby obtaining a sub-path that has undergone the clipping operation.

8. The method for path planning of an unmanned surface vehicle based on an improved ant colony algorithm and speed maintenance according to claim 6 is characterized in that: The expansion operation described in step S423 is specifically: starting from the turning point, continuously performing the extension-rotation operation to obtain multiple expansion points, and then obtaining a sub-path that has undergone the expansion operation by connecting each expansion point in sequence, 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.

9. The method for path planning of 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 at its expected speed.

Images