A robot formation path planning method and related device

Abstract

The application discloses a kind of robot formation path planning method and related equipment, belong to the formation path planning problem of multiple robots in the grid map with obstacle, the application is carried out on two-dimensional grid map, first, select master robot, according to the position of the rest of the robot participating in formation relative to master robot, the obstacle is expanded (the part of expansion is all the feasible position of the rest of the robot participating in formation), the feasible grid in map is the feasible area of master robot at this time;After obtaining better path by using ant colony algorithm multiple iterations, in turn, using three kinds of optimization strategies of deleting redundant grid, triangle pruning, polyline curve, the best path scheme of master robot is obtained;Finally, the part of obstacle expansion is rejected, and the path planning of entire robot formation is obtained.

Description

Technical Field

[0001] This invention belongs to the field of robot path planning technology, specifically a robot formation path planning method and related equipment. Background Technology

[0002] Robot formation technology, especially in the field of mobile robotics, has become a research hotspot in recent years. With the widespread application of intelligent robots in logistics, military, environmental monitoring, and other fields, research on robot formation has become particularly important. Each robot in the formation needs to achieve autonomous navigation in a dynamic environment while ensuring the stability of the formation and the optimization of the path. Path planning, as one of the core issues in robot formation technology, is directly related to the efficiency and safety of completing the formation task.

[0003] Robot swarming path planning technology refers to the collaborative planning of an efficient and safe path by multiple robots working together, based on their relative positions and environmental perception information. This technology requires robots to avoid obstacles to optimize overall movement trajectories and reduce energy consumption. It has wide applications and significant implications in fields such as autonomous vehicles and drone swarms.

[0004] Multi-robot formation path planning techniques can be categorized into five types: graph-based path planning methods, optimization-based path planning methods, swarm behavior-based path planning methods, game theory-based path planning methods, and adaptive path planning methods. Currently, one of the most widely used path planning methods is graph-based path planning. This method represents the environment by constructing a graph model and calculates the shortest path between robots to achieve path planning. Common graph algorithms include classic shortest path calculation methods that can find efficient paths for each robot and are suitable for task planning in static environments. However, graph-based methods may suffer from high computational complexity, especially in large-scale environments, where the real-time performance of path planning may be affected, particularly when the number of nodes is large, leading to significant computational overhead and impacting path planning efficiency. Optimization-based path planning methods construct optimization models and use mathematical methods to solve for the optimal robot paths. These methods typically optimize paths based on the global objectives of the multi-robot system, such as minimizing total path cost or energy consumption. Common optimization algorithms include genetic algorithms, particle swarm optimization, and ant colony optimization. Optimization-based methods perform particularly well in multi-robot collaboration, effectively achieving global path optimization. However, these methods are computationally expensive, especially when dealing with a large number of robots, where the solution process can become very complex. They also have poor adaptability to dynamic environments and should be applied with caution in tasks with high real-time requirements. Path planning methods based on swarm behavior, such as the Boids model, ant colony optimization, and particle swarm optimization, simulate collective behavior in nature.[7] By simulating collaboration and information sharing among robots, path planning problems in multi-robot systems can be effectively solved. This type of method is highly adaptable to multi-robot collaborative tasks and can flexibly handle dynamic environments. However, in complex environments, it may face problems such as slow path convergence and local optima. Especially in complex tasks or environments with high uncertainty, robots may struggle to find the optimal path quickly and require a long period of exploration and adjustment. Game theory-based path planning methods treat each robot as an independent agent, with robots deciding their path choices through game theory. Game theory methods can optimize the overall performance of multi-robot systems by simulating interactions between robots. Nash equilibrium is a widely used concept in game theory, using game models to optimize path planning and ensure that robots choose the optimal strategy in a given environment. While this method is suitable for tasks with game-like characteristics, such as collision avoidance or resource allocation problems, its limitation is that the game process may lead to inefficient collaboration in multi-robot systems, especially in complex and dynamically changing environments. [9] Game theory methods require significant computational resources to handle the decision-making of multiple robots, and in dynamic environments, they may be limited by the game strategy, affecting path planning efficiency. Adaptive path planning methods, on the other hand, emphasize real-time adjustments by the robot based on environmental changes, enabling it to adapt to dynamic obstacles and unknown environments. Common adaptive methods include extended Kalman filtering and reinforcement learning. Through real-time perception and feedback control, the robot can continuously optimize its path selection while performing tasks. This approach is suitable for complex and dynamic scenarios, providing effective path adjustment strategies when the robot faces uncertain environments. However, adaptive methods may suffer from lower path planning accuracy and higher computational resource consumption, especially in multi-robot collaboration where a large amount of real-time information needs to be coordinated. Furthermore, they may be affected by model incompleteness and environmental complexity, leading to less-than-expected path planning results. Summary of the Invention

[0005] This invention provides a robot formation path planning method to solve the problems in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A robot formation path planning method includes:

[0008] Build a raster map and expand the obstacles and boundaries of the raster map;

[0009] Determine the initial pheromone matrix and the initial potential field heuristic function for the raster map;

[0010] Ants select the movement node for each step based on the initial pheromone matrix and the initial potential field heuristic function, using a roulette wheel approach, to obtain the ant's crawling route and route length. Based on the ant's crawling route and route length, a preliminary path planning is obtained.

[0011] The initial path planning is optimized, and extended obstacles and boundaries are removed to obtain the robot formation path.

[0012] Preferably, the steps for expanding the obstacles and boundaries of the raster map are as follows:

[0013] Based on the starting point of the main robot on the grid map, determine the positional relationship between the main robot and other formation robots. Then, calculate the inverse vector of the displacement vector from the main robot to other formation robots, and expand the obstacles and boundaries based on the inverse vector.

[0014] Preferably, the steps for determining the initial pheromone matrix and the initial potential field heuristic function of the raster map are as follows:

[0015] The raster map is converted into a distance matrix between neighboring rasters. The distance matrix describes the connection relationship and distance between rasters. The initial pheromone matrix is ​​obtained based on the connection relationship and distance between rasters. The initial potential field heuristic function is determined based on the distance between rasters.

[0016] Preferably, the formula for obtaining the initial potential field heuristic function is:

[0017]

[0018] in, and These are the coordinates of the neighboring grid cells. It is the potential field constant. Let be the heuristic function for the potential field.

[0019] Preferably, the steps for selecting the movement node at each step based on roulette wheel selection and obtaining the ant's crawling path and path length are as follows:

[0020] First, the ant selects its next moving node using a roulette wheel method. It then checks if the next moving node is in a dead end. If it is, it returns to the previous node and adds that grid to the taboo list. It then selects its next moving node again using the roulette wheel method. If it is not in a dead end, it does not return to the previous grid. Instead, it directly selects its next moving node using the roulette wheel method, updates the local pheromones of the route it has traveled, and continues walking until it reaches its destination.

[0021] Repeat the above steps to record the crawling route and route length of each ant in each generation, update the global optimal route, and obtain the preliminary path planning.

[0022] Preferably, optimizing the preliminary path planning specifically involves:

[0023] The initial path planning was optimized by sequentially removing redundant grids, triangular pruning, and polyline curve transformation.

[0024] Preferably, deleting redundant rasters involves finding consecutive path nodes. and yes The neighboring grid, if Then the original continuous path raster is deleted. The new continuous path grid is ;

[0025] Triangle pruning is: if multiple consecutive nodes in a path form a polyline, and the direct connection between one node and another node does not pass through any obstacles, then replace the polyline from one node to another with a straight line segment between the two nodes.

[0026] The process of transforming a line into a curve is as follows: Select a node and determine whether it is an inflection point of the line; if so, replace the line with a curve; otherwise, jump to the next inflection point until the destination is reached.

[0027] A robot formation path planning system, comprising:

[0028] Builder module: Used to build raster maps and expand the obstacles and boundaries of the raster map;

[0029] Initialization module: used to determine the initial pheromone matrix and initial potential field heuristic function of the raster map;

[0030] Preliminary planning module: This module is used by the ant to select the movement node at each step based on the initial pheromone matrix and the initial potential field heuristic function, obtain the ant's crawling route and route length, and obtain preliminary path planning based on the ant's crawling route and route length.

[0031] Formation Path Acquisition Module: Used to optimize the initial path planning and remove extended obstacles and boundaries to obtain the robot formation path.

[0032] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of a robot formation path planning method.

[0033] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of a robot formation path planning method.

[0034] Compared with existing technologies, this invention has the following advantages: This invention provides a robot formation path planning method. This method operates on a two-dimensional grid map. First, a grid map is constructed, and then obstacles and boundaries are added to the grid map. At this point, the feasible grids in the map represent the feasible areas for the main robot. Then, an ant colony algorithm is used for multiple iterations to obtain a preliminary path plan. This preliminary path is then optimized to obtain the optimal path scheme for the main robot. Finally, the extended obstacle portion is removed, resulting in the path plan for the entire robot formation. Using a grid map for path planning fully utilizes the advantages of grid maps in environmental modeling. Grid maps can decompose complex environments into discrete grids, transforming the path planning problem into a graph search problem, thus conveniently and effectively solving the path coordination and cooperation problems between robots. The visualization and discretization characteristics of grid maps make the understanding of the environment clearer, helping to improve the accuracy and reliability of path planning.

[0035] Furthermore, obstacle extension technology avoids collisions between robots and obstacles. By appropriately extending the obstacle area, it ensures that the robot not only avoids single obstacles when planning its path but also adapts to various dynamic environmental changes. This method effectively improves the safety of path planning and the stability of robot formations, guarantees the feasibility of formations in dynamic environments, and reduces the risk of collisions in complex environments.

[0036] Furthermore, by performing a three-step geometric optimization process instead of directly converting the path to a polyline curve, the computational complexity is significantly reduced. Traditional polyline curve conversion methods can generate a large amount of computational work and complexity, while this invention simplifies the calculation process and improves the execution efficiency of the algorithm by optimizing the geometry of the path. Especially in large-scale calculations, it can effectively reduce the burden on the computer.

[0037] Furthermore, the path of this invention is a smooth curve, which can effectively reduce the problem of sudden turning during robot movement. Compared with a broken line path, a curved path is more natural and smooth, which can improve the robot's motion stability. Especially in scenarios that require continuous and smooth movement, a curved path can significantly improve the comfort and accuracy of robot movement, avoiding efficiency losses or path deviations caused by the robot turning too sharply. Attached Figure Description

[0038] Figure 1 This is a flowchart of a robot formation path planning method according to the present invention;

[0039] Figure 2 This invention expands the obstacle flowchart;

[0040] Figure 3This is a flowchart of the ant colony algorithm of the present invention;

[0041] Figure 4 This is a flowchart of the geometry optimization process of the present invention;

[0042] Figure 5 This is the original grid map with obstacles in an embodiment of the present invention;

[0043] Figure 6 A grid map that extends the obstacle outline for embodiments of the present invention;

[0044] Figure 7 This is the robot formation path after applying the ant colony algorithm in an embodiment of the present invention;

[0045] Figure 8 This is a robot formation path in an embodiment of the present invention after removing redundant grids;

[0046] Figure 9 This is a robot formation path that has undergone triangular pruning in an embodiment of the present invention;

[0047] Figure 10 The robot formation path in this embodiment of the invention is a polygonal curve.

[0048] Figure 11 This is the complete path of the diagonal queue in an embodiment of the present invention;

[0049] Figure 12 This is a block diagram of a robot formation path planning system according to the present invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0051] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0052] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0053] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0054] like Figure 1 As shown, the present invention provides a robot formation path planning method, including:

[0055] S101 constructs a raster map and expands the obstacles and boundaries of the raster map;

[0056] S102 determines the initial pheromone matrix and the initial potential field heuristic function of the raster map;

[0057] Ant S103 selects the movement node for each step based on the initial pheromone matrix and the initial potential field heuristic function, and obtains the ant's crawling route and route length. Based on the ant's crawling route and route length, a preliminary path planning is obtained.

[0058] S104 optimizes the initial path planning and removes extended obstacles and boundaries to obtain the robot formation path.

[0059] The detailed steps are as follows:

[0060] Step 1: Build a raster map.

[0061] Step 1.2 divides the map into multiple grids for discretization. Each grid should have a unique number and coordinates to facilitate subsequent route planning.

[0062] Step 1.2: Determine the boundary dimensions of the raster map and calculate the number of rows and columns. Each raster should be the same size to ensure consistency in path calculations.

[0063] Step 1.3 Calculate the number of each grid node using a linear sequence to ensure that each grid node has a unique identifier, which facilitates the calculation and comparison of subsequent paths.

[0064] Step 2: Determining obstacles and boundaries, such as Figure 2 As shown.

[0065] Step 2.1 Based on the location of the starting point, calculate the horizontal and vertical distances respectively, and determine the length of the boundary to be added to ensure that the expansion of obstacles will not interfere with path planning.

[0066] Step 2.2 Using the first robot as the master robot, add corresponding obstacles and boundaries according to the negative vector of the relative positions of the robots in the queue, so that the path planning only needs to focus on the position of the master robot.

[0067] Step 3: Initialize the pheromone matrix and heuristic function, such as... Figure 3As shown.

[0068] Step 3.1 Convert the map into a distance matrix between adjacent graticles to describe the connection relationships and distances between graticles for subsequent path selection.

[0069] Step 3.2 Initialize the pheromone matrix to ensure the feasibility of path planning. Pheromone plays a guiding role in the ant's path selection process.

[0070] Step 3.3 Initialize the potential field heuristic function to provide ants with heuristic information for path selection, helping ants to search for the shortest path more effectively.

[0071] Step 4: Ant path selection and update.

[0072] Step 4.1 Based on the current pheromone concentration and heuristic information, the ant selects its next grid cell to move to using a roulette wheel strategy. Through this strategy, the ant tends to choose paths with higher pheromone concentrations or better heuristic information, thereby increasing the probability of finding the optimal path.

[0073] Step 4.2 Update the ant's state and record its path to ensure the accuracy and feasibility of the path planning. Each time the ant moves, its path is updated, and newly added grid cells are added to the tabu list. The tabu list prevents ants from repeatedly traversing already explored grid cells, reducing repetition in path selection and enhancing the diversity of path search.

[0074] Step 4.3 Check if the ant has entered a dead end. If it has, return to the previous step and choose a new path; otherwise, continue along the path.

[0075] Step 5: Identify dead ends and update the path

[0076] Step 5.1 Determine if the ant has entered a dead end. If the ant encounters a dead end (i.e., there is no feasible path to continue), it will return to the previous grid cell and choose a new path, returning to Step 4 to continue path selection. This mechanism ensures that the ant is not trapped when it encounters an infeasible path, thus allowing it to continue exploring other feasible paths.

[0077] Step 5.2 If the ant does not enter a dead end, it updates the local pheromone level of the path it has traversed and continues moving towards the target grid until it reaches the target. Each time an ant traverses a path, it adds pheromone to the grid cell along that path, encouraging other ants to choose their own paths, increasing the global pheromone concentration, and thus helping to find the optimal path.

[0078] Step 6: Update the global pheromone matrix

[0079] Step 6.1 After all ants have reached the target grid, update the global pheromone matrix, including pheromone evaporation and newly added pheromone. At this point, the global pheromone matrix provides the basis for the next round of path selection. The pheromone evaporation mechanism ensures that the pheromone concentration of paths that have not been selected for a long time gradually decreases, thus avoiding the trap of local optima.

[0080] Step 7: Iteration and optimal path update, such as Figure 4 As shown.

[0081] Step 7.1 Repeat steps 4 through 6, recording the path and route length for each ant in each generation. After each iteration, the globally optimal path is updated to find the shortest and best path. The optimal path for each generation is saved for comparison and optimization until the algorithm's convergence condition is met or the maximum number of iterations is reached.

[0082] Step 7.2 Ensure the feasibility of path planning by continuously updating the globally optimal route to ultimately determine the shortest path. The algorithm iterates multiple times to gradually improve the accuracy of path planning and reduce redundant parts of the path, thus ultimately obtaining the shortest and most feasible path.

[0083] Step 8: Path geometry optimization

[0084] Step 8.1 After all iterations are completed, output the preliminary optimized path results, including the shortest path distance, path number, and path coordinates. This path is obtained through ant colony optimization, but it can be further optimized geometrically to improve the path quality and computational efficiency.

[0085] Step 8.2 Performs geometric optimization on the path obtained by the algorithm, removing redundant grids and smoothing the path. The goal of this stage is to make the path smoother and less susceptible to obstacles.

[0086] Step 8.3 uses triangulation and polyline curve shaping techniques to further optimize the path, reduce redundant points, and increase its feasibility and execution efficiency. Triangulation and polyline curve shaping calculate the curvature of the path and perform corresponding optimizations to ensure that the final path is as simple and smooth as possible.

[0087] Step 8.4 In the presence of obstacles, calculate the perpendicular component of the vector product between the obstacle and the path's starting point and direction. If the component is only positive or only negative, the obstacle does not interfere with the path, and the current node can be deleted and the process returned to Step 8.5; if the component has both positive and negative values, the path is affected by the obstacle, the node is retained, and the process jumps to the next node.

[0088] Step 8.5 First, determine if it is a polyline inflection point; record all inflection points and calculate the included angle of each inflection point. The angle information at the inflection points helps in subsequent curve processing, avoiding overly sharp paths.

[0089] Step 8.6 Calculate the reasonable radius at the inflection point and calculate the trajectory of the curve based on the included angle at the inflection point. By curving the path, the number of straight segments is reduced, making the overall path smoother and more natural, and avoiding unnecessary sharp turns by the robot during movement.

[0090] Step 9: Delete extended obstacles, draw the main robot's path, and add paths for other robots to obtain the final formation path.

[0091] Step 9.1 Remove extended obstacles to eliminate potential interference factors during path planning. The geometrically optimized path will provide a more efficient and safer travel route for the master robot and other robots.

[0092] Step 9.2 Draw the main robot's path, and based on the main robot's path, add the paths of other robots to obtain the final formation path. The paths of all robots will take into account the spacing between them and the formation requirements to ensure that the entire robot queue can run smoothly, avoid collisions, and maintain the formation.

[0093] The path planning method of this invention, by combining ant colony optimization and geometric optimization, can plan the shortest, safest, and most feasible path for robot formations in complex environments. By pre-extending obstacles and avoiding complex terrain obstacles, collisions between robots are prevented, thus improving the stability and adaptability of robot formations.

[0094] This invention also simplifies the path structure through geometric optimization techniques, reducing computational complexity and ensuring efficient generation of optimized paths in real-time calculations. Compared to traditional path planning methods, the path planning of this invention not only improves the driving efficiency of robot platoons but also enables them to cope with changes in obstacles in dynamic environments, providing stronger emergency response capabilities.

[0095] Another embodiment of the present invention provides a robot formation path planning method, including:

[0096] 1. Construct a raster map, such as Figure 5 As shown, the black grid represents the original obstacles, and the white grid represents the feasible area;

[0097] 2. Calculate the horizontal direction based on the starting point position of the main robot. Distance in the vertical direction And determine the length of the boundary that needs to be added, using the first robot as the master robot, add obstacles and boundary size limits according to the negative vector of the relative distance between the queues, such as... Figure 6 As shown, the gray grid represents extended obstacles to ensure that we only need to pay attention to the main robot's path.

[0098]

[0099] in, These are the horizontal and vertical coordinates of the robots participating in the formation. These are the horizontal and vertical coordinates of the main robot.

[0100] 3. Convert the map into a distance matrix between adjacent grid cells. It describes the connections and distances between grid cells, and obtains the initial pheromone matrix based on the connections and distances between grid cells. To ensure the feasibility of path planning, an initial potential field heuristic function is obtained based on the distance between grid cells. It provides heuristic information for path selection.

[0101]

[0102] in, and These are the coordinates of the neighboring grid cells. It is the potential field constant.

[0103] 4. Ants initialize the pheromone matrix. and the initialization potential field heuristic function The ant selects the next node to move based on a roulette wheel approach, updates the ant's status and records, and ensures the feasibility of the path planning.

[0104]

[0105] Here, PP is the probability of moving to a neighboring grid cell.

[0106] 5. Determine if the ant has entered a dead end. If so, return to the previous node and add the cell to the taboo list, then proceed to step 4. If the ant has not entered a dead end, update the local pheromone of the ant's path. And continue until the destination is reached;

[0107] 6. Update the global pheromone matrix, including pheromone volatilization and newly added components.

[0108] 7. Repeat steps 4-6, recording the crawling route and route length for each ant in each generation, updating the globally optimal route, and obtaining a preliminary path plan (e.g., Figure 7 (as shown)

[0109] 8. Perform geometric route optimization, following these three steps in sequence:

[0110] Remove redundant rasters: Find consecutive path nodes and yes The neighboring grid, if Then the original continuous path raster is deleted. The new continuous path grid is ,like Figure 8 As shown;

[0111] Triangle pruning: If multiple consecutive nodes in a path form a polyline, and the direct connection between nodes does not pass through any obstacles, then the multiple polyline segments from node to node can be replaced with the straight line segment between nodes. The effect is as follows: Figure 9 ;

[0112] Polyline to Curve Conversion: Select a node and determine if it is an inflection point of a polyline; if so, replace the polyline with a curve; otherwise, jump to the next inflection point until the destination is reached. The effect is as follows: Figure 10 .

[0113] 9. Delete the grid of obstacle extensions, draw the main robot's path, add other robots and draw their paths, finally obtaining the formation path, the effect of which is as follows. Figure 11 ;

[0114] like Figure 12 As shown, the present invention provides a robot formation path planning system, comprising:

[0115] Builder module: Used to build raster maps and expand the obstacles and boundaries of the raster map;

[0116] Initialization module: used to determine the initial pheromone matrix and initial potential field heuristic function of the raster map;

[0117] Preliminary planning module: This module is used by the ant to select the movement node at each step based on the initial pheromone matrix and the initial potential field heuristic function, obtain the ant's crawling route and route length, and obtain preliminary path planning based on the ant's crawling route and route length.

[0118] Formation Path Acquisition Module: Used to optimize the initial path planning and remove extended obstacles and boundaries to obtain the robot formation path.

[0119] An embodiment of the present invention provides a terminal device. This terminal device includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the various method embodiments described above. Alternatively, when the processor executes the computer program, it implements the functions of each module / unit in the various device embodiments described above.

[0120] The computer program can be divided into one or more modules / units, which are stored in the memory and executed by the processor to complete the present invention.

[0121] The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0122] The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0123] The memory can be used to store the computer program and / or module. The processor implements various functions of the terminal device by running or executing the computer program and / or module stored in the memory and calling the data stored in the memory.

[0124] If the modules / units integrated into the terminal device are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0125] Although embodiments of the present invention have been described above in conjunction with the accompanying drawings, the present invention is not limited to the specific embodiments and application fields described above. The specific embodiments described above are merely illustrative and instructive, and not restrictive. Those skilled in the art, guided by the specification, can make many other modifications without departing from the scope of the claims of the present invention, and all of these modifications are within the scope of protection of the present invention.

Claims

1. A method for robot formation path planning, characterized in that, The application relates to a robot formation path planning method based on ant colony optimization, which comprises the following steps: constructing a grid map, extending obstacles and boundaries of the grid map; determining an initial pheromone matrix and an initial potential field heuristic function of the grid map; an ant selects a moving node at each step based on a roulette wheel mode according to the initial pheromone matrix and the initial potential field heuristic function, obtains a crawling route and a route length of the ant, and obtains a preliminary path planning based on the crawling route and the route length of the ant; the preliminary path planning is optimized, the extended obstacles and boundaries are deleted, and a robot formation path is obtained; the step of extending the obstacles and the boundaries of the grid map is specifically as follows: according to the position of a starting point of a host robot on the grid map, the position relationship between the host robot and other formation robots is determined, the distances in horizontal and vertical directions are calculated, the boundary length to be added is determined, the inverse vector of a displacement vector of the host robot to the other formation robots is calculated, and corresponding obstacles and boundaries are added according to the inverse vector; the step that the ant selects a moving node at each step based on a roulette wheel mode according to the initial pheromone matrix and the initial potential field heuristic function is specifically as follows: the ant selects a grid to be moved at the next step through a roulette wheel mode according to current pheromone concentration and heuristic information, and through the roulette wheel strategy, the ant tends to select a path with higher pheromone concentration or better heuristic information, so that the probability of searching for an optimal path is increased; the state of the ant is updated and the path is recorded, the accuracy and feasibility of path planning are ensured, the path of the ant is updated every time the ant moves, and meanwhile, the newly added grid is put into a taboo table, the taboo table is used for avoiding the ant from repeatedly walking through an already explored grid, reducing the repeatability in path selection, and enhancing the diversity of path search; whether the ant enters a dead end is checked, if the ant enters the dead end, the ant returns to the last step and reselects a path, otherwise, the ant continues to advance along the path; whether the ant enters the dead end is judged, if the ant encounters the dead end, the ant returns to the last grid and reselects a path, and the path selection is continued, the mechanism ensures that the ant is not trapped when encountering an infeasible path, so that other feasible paths can be continuously explored; if the ant does not enter the dead end, the local pheromone of the path walked through by the ant is updated, and the ant continues to advance to a target until the target is reached, the pheromone of the grid of the path walked through by the ant is increased every time the ant walks through a path, the path selection of other ants is promoted, the global pheromone concentration is increased, and the optimal path is helped to be found; after all the ants reach the target grid, a global pheromone matrix is updated, including volatilization and newly added parts, at this time, the global pheromone matrix provides a basis for the next round of path selection, the volatilization mechanism of the pheromone ensures that the pheromone concentration of a path which is not selected for a long time is gradually reduced, so that the trap of a local optimal solution is avoided.

2. The method of claim 1, wherein, the step of determining the initial pheromone matrix and the initial potential field heuristic function of the grid map is specifically as follows: The grid map is converted into a distance matrix between adjacent grids, the connection relationship and distance between grids are described according to the distance matrix, the initial pheromone matrix is obtained according to the connection relationship and distance between grids, and the initial potential field heuristic function is determined according to the distance between grids.

3. The method of claim 2, wherein, The formula for obtaining the initial potential field heuristic function is: wherein, with is the coordinate of the adjacent grid, is the potential field constant, is the potential field heuristic function.

4. The method of claim 1, wherein, The preliminary path planning is optimized in the following steps: The preliminary path planning is optimized by deleting redundant grids, triangular pruning and line curve in turn.

5. The robot formation path planning method according to claim 4, characterized in that, Delete redundant grid is: find the continuous path node and is neighbor grid, if , the original continuous path grid is deleted , and the new continuous path grid is ; The triangular pruning is that if a plurality of continuous nodes in the path form a multi-segment line, and the direct connection path between one node and another node does not pass through any obstacle, then the multi-segment line from the one node to the other node is replaced by a straight line segment between the two nodes; The line curve is that a node is selected, and it is judged whether the node is a line inflection point; if yes, the line is replaced by a curve; if no, the next inflection point is jumped to until the destination.

6. A robotic formation path planning system, characterized by, The robot formation path planning method comprises: a construction module for constructing a grid map and extending obstacles and boundaries of the grid map; an initialization module for determining an initial pheromone matrix and an initial potential field heuristic function of the grid map; a preliminary planning module for selecting a moving node at each step based on a roulette wheel method according to the initial pheromone matrix and the initial potential field heuristic function, obtaining a crawling route and a route length of an ant, and obtaining a preliminary path planning based on the crawling route and the route length of the ant; a formation path obtaining module for optimizing the preliminary path planning, deleting the extended obstacles and boundaries, and obtaining a robot formation path.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the steps of the robot formation path planning method according to any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, the computer program comprising instructions that, when executed by a computer, cause the computer to perform the method of any one of claims 1 to 7. The computer program is executed by the processor to implement the steps of the robot formation path planning method according to any one of claims 1 to 5.

Images