Robot formation path planning method and related equipment

Abstract

The invention discloses a robot formation path planning method and related equipment, and belongs to the problem of formation path planning of multiple robots in a grid map with obstacles, the method is carried out on a two-dimensional grid map, firstly, a main robot is selected, and according to the positions of the other robots participating in formation relative to the main robot, the main robot is selected; the obstacles are expanded (the expanded parts are all feasible positions of other robots participating in formation), and at the moment, feasible grids in the map are feasible areas of the main robot; after an optimal path is obtained through multiple iterations of the ant colony algorithm, the optimal path scheme of the main robot is obtained through three optimization strategies of redundant grid deletion, triangular pruning and broken line curvification in sequence; and finally, eliminating the expanded part of the obstacle to obtain the path planning of the whole robot formation.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and specifically relates to a robot formation path planning method and related equipment. Background Art

[0002] Robot formation technology, especially in the field of mobile robots, has become a hot topic in recent years. With the widespread application of intelligent robots in logistics, military, environmental monitoring and other fields, the 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 of robot formation technology, is directly related to the efficiency and safety of the completion of the formation task.

[0003] The path planning technology of robot formation refers to the collaborative planning of an effective and safe path by multiple robots based on their relative positions and environmental perception information. Path planning technology requires robots to avoid obstacles to optimize the overall motion trajectory and reduce energy consumption. This technology has wide applications and important significance in the fields of autonomous vehicles, drone groups, etc.

[0004] The path planning technology of multi-robot formation can be divided into five categories: graph-based path planning method, optimization-based path planning method, group behavior-based path planning method, game theory-based path planning method and adaptive path planning method. At present, one of the most widely used path planning methods is the graph-based path planning method, which represents the environment by constructing a graph model and realizes path planning by calculating the shortest path between robots. Common graph algorithms include some classic shortest path calculation methods, which can find efficient paths for each robot and are suitable for task planning in static environments. However, graph-based methods may have the problem of high computational complexity, especially in large-scale environments, the real-time performance of path planning may be affected, especially when the number of nodes is large, the computational overhead is large, affecting the efficiency of path planning. Optimization-based path planning methods construct optimization models and use mathematical methods to solve the optimal solution of the robot path. These methods can usually optimize the path according to the global goals of the multi-robot system, such as minimizing the total path cost or energy consumption. Common optimization algorithms include genetic algorithms, particle swarm optimization and ant colony optimization. Optimization-based methods are particularly outstanding in multi-robot collaboration and can achieve global path optimization well. However, these methods have high computational overhead, especially when there are a large number of robots. The solution process may become very complicated, and they are less adaptable to dynamic environments. They need to be used with caution in tasks with high real-time requirements. Path planning methods based on group behavior simulate collective behaviors in nature, such as the Boids model, ant colony algorithm, and particle swarm optimization.[7] . By simulating the collaboration and information sharing between robots, the path planning problem in the multi-robot system can be effectively solved. This type of method has good adaptability when dealing with multi-robot collaborative tasks and can flexibly cope with dynamic environments, but it may face the problems of slow path convergence and local optimal solutions in complex environments. Especially when the task is complex or the environment is full of uncertainty, it may be difficult for the robot to quickly find the optimal path, and it takes a long time to explore and adjust. The path planning method based on game theory regards each robot as an independent intelligent agent, and the robot determines its own path selection through game. Game theory methods can optimize the overall performance of the multi-robot system by simulating the interaction between robots. Nash equilibrium is a widely used concept in game theory. The game model is used to optimize path planning to ensure that the robot chooses the optimal strategy in a given environment. Although this method is suitable for tasks with game characteristics, such as collision avoidance or resource allocation problems, its limitation is that the game process may cause the collaboration of the multi-robot system to be inefficient, especially in environments with complex tasks and dynamic changes. [9] . Game theory methods require a lot of computing resources to process the game decisions of multiple robots, and in dynamic environments, they may be limited by game strategies, affecting the efficiency of path planning. Adaptive path planning methods emphasize that robots make real-time adjustments based on environmental changes and can adapt to dynamic obstacles and unknown environments. Common adaptive methods include extended Kalman filtering and reinforcement learning. Through real-time perception and feedback control, robots can continuously optimize path selection when performing tasks. This method is suitable for complex and dynamic scenarios and can provide effective path adjustment strategies when robots face uncertain environments. However, adaptive methods may face problems such as low path planning accuracy and high consumption of computing resources, especially when multiple robots collaborate and need to coordinate a large amount of real-time information. They may also be affected by incomplete models and environmental complexity, resulting in less than expected path planning results. Summary of the invention

[0005] The present invention provides a robot formation path planning method to solve the problems in the background technology.

[0006] To achieve the above object, the present invention provides the following technical solutions: A robot formation path planning method, comprising: Build a grid map and expand the obstacles and boundaries of the grid map; Determine the initialization pheromone matrix and initialization potential field heuristic function of the grid map; According to the initialized pheromone matrix and the initialized potential field heuristic function, the ants select the mobile nodes for each step based on the roulette wheel method, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; The preliminary path planning is optimized, and the extended obstacles and boundaries are deleted to obtain the robot formation path.

[0007] Preferably, the steps of extending obstacles and boundaries of the grid map are specifically as follows: According to the position of the starting point of the main robot on the grid map, the positional relationship between the main robot and other formation robots is determined, and then the inverse vector of the displacement vector from the main robot to other formation robots is calculated, and obstacles and boundaries are expanded according to the inverse vector.

[0008] Preferably, the steps of determining the initialization pheromone matrix and initialization potential field heuristic function of the grid map are specifically: 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 initialization pheromone matrix is ​​obtained according to the connection relationship and distance between grids, and the initialization potential field heuristic function is determined according to the distance between grids.

[0009] Preferably, the formula for obtaining the initialization potential field heuristic function is:

[0010] in, and are the coordinates of the neighboring grid, is the potential field constant, is the potential field inspiration function.

[0011] Preferably, the steps of selecting the mobile node at each step based on the roulette wheel method and obtaining the crawling route and route length of the ant are specifically as follows: First, the ant selects the next moving node based on the roulette method, and determines whether the next moving node is in a dead end. If it is in a dead end, it returns to the previous node and puts the grid into the taboo table, and re-selects the next moving node based on the roulette method. If it is not in a dead end, it does not return to the previous grid, but directly selects the next moving node based on the roulette method, updates the local pheromone of the route, and continues walking until it reaches the destination; Repeat the above steps, record the crawling route and route length of each ant in each generation, update the global optimal route, and obtain the preliminary path planning.

[0012] Preferably, the optimization of the preliminary path planning is specifically as follows: The initial path planning is optimized by deleting redundant grids, triangle pruning and polyline curvature.

[0013] Preferably, deleting redundant grids is as follows: finding continuous path nodes and yes The neighbor grid of , then the original continuous path grid is deleted , the new continuous path grid is ; Triangle pruning is as follows: if multiple consecutive nodes in the path form a multi-segment polyline, and the direct connection path between one node and another node does not pass through any obstacles, then replace the multi-segment polyline from one node to another node with a straight line segment between the two nodes; The broken line is converted to a curve as follows: select a node and determine whether it is a broken line inflection point; if so, replace the broken line with a curve; if not, jump to the next inflection point until the destination.

[0014] A robot formation path planning system, comprising: Construction module: used to construct grid maps and expand obstacles and boundaries of grid maps; Initialization module: used to determine the initialization pheromone matrix and initialization potential field heuristic function of the grid map; Preliminary planning module: It is used for ants to select the mobile node for each step based on the roulette wheel method according to the initialization pheromone matrix and the initialization potential field heuristic function, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; Formation path acquisition module: used to optimize the preliminary path planning, delete extended obstacles and boundaries, and obtain the robot formation path.

[0015] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of a robot formation path planning method are implemented.

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

[0017] Compared with the prior art, the present invention has the following beneficial effects: the present invention provides a robot formation path planning method, which is performed on a two-dimensional grid map. First, a grid map is constructed, and then the obstacles and boundaries of the grid map are expanded. At this time, the feasible grid in the map is the feasible area of ​​the main robot; after using the ant colony algorithm to iterate multiple times to obtain a better path, a preliminary path planning is obtained, and then the preliminary path is optimized to obtain the best path plan for the main robot. Finally, the part where the obstacle is extended is eliminated to obtain the path planning of the entire robot formation. The grid map is used for path planning, and the advantages of the grid map in environmental modeling are fully utilized. The grid map can decompose the complex environment into discrete grids, so that the path planning problem is converted into a graph search problem, thereby conveniently and effectively solving the path coordination and collaboration problems between robots. The visualization and discretization characteristics of the grid map make the understanding of the environment clearer, which helps to improve the accuracy and reliability of path planning.

[0018] Furthermore, the obstacle expansion technology avoids collisions between robots and obstacles. By appropriately expanding the obstacle area, it ensures that the robot not only avoids a single obstacle when planning a path, but also adapts to a variety of dynamic environmental changes. This method effectively improves the safety of path planning and the stability of the robot formation, ensures the feasibility of the formation in a dynamic environment, and reduces the risk of collision in a complex environment.

[0019] Furthermore, by performing a three-step geometric optimization process instead of directly converting the path into a broken line curve, the computational complexity is significantly reduced. The traditional broken line curve conversion method may generate a lot of computational work and complexity, while the present invention simplifies the computational process and improves the execution efficiency of the algorithm by optimizing the geometric shape of the path, which can effectively reduce the burden on the computer, especially in large-scale calculations.

[0020] Furthermore, the path of the present invention is a smooth curve, which can effectively reduce the problem of sudden turns during the movement of the robot. Compared with the broken line path, the curved path is more natural and smooth, which can improve the movement stability of the robot. Especially in the scene that requires continuous and smooth driving, the curved path can greatly improve the comfort and accuracy of the robot's movement, and avoid the efficiency loss or path deviation caused by the robot turning too quickly. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This is a flow chart of a robot formation path planning method of the present invention; Figure 2 Expand the obstacle flow chart for the present invention; Figure 3 This is the flow chart of the ant colony algorithm of the present invention; Figure 4This is a flow chart of geometry optimization of the present invention; Figure 5 The original grid map with obstacles in the embodiment of the present invention; Figure 6 A grid map of obstacle contours expanded for an embodiment of the present invention; Figure 7 The robot formation path after the ant colony algorithm is used in the embodiment of the present invention; Figure 8 The robot formation path of the embodiment of the present invention after deleting redundant grids; Fig. 9 The robot formation path after triangle pruning according to an embodiment of the present invention; Fig.10 The robot formation path in the embodiment of the present invention is converted into a broken line curve; Fig.11 It is the complete path of the diagonal queue in the embodiment of the present invention; Fig.12 This is a block diagram of a robot formation path planning system of the present invention. DETAILED DESCRIPTION

[0022] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0023] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0024] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0025] In order 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.

[0026] like Figure 1 As shown, the present invention provides a robot formation path planning method, comprising: S101 constructs a grid map and expands obstacles and boundaries of the grid map; S102 determines the initialization pheromone matrix and initialization potential field heuristic function of the grid map; S103 The ants select the mobile nodes for each step based on the roulette wheel method according to the initialized pheromone matrix and the initialized potential field heuristic function, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; S104 optimizes the preliminary path planning and deletes the extended obstacles and boundaries to obtain the robot formation path.

[0027] The detailed steps are: Step 1: Build a raster map.

[0028] Step 1.2: Divide the map into multiple grids to discretize the map. Each grid should have an independent number and coordinates to facilitate subsequent path planning.

[0029] Step 1.2 determines the bounding size of the grid map and calculates the number of rows and columns of the map. The size of each grid should be uniform to ensure consistency in path calculation.

[0030] Step 1.3 uses a linear sequence to calculate the number of each grid node, ensuring that each grid node has a unique identifier to facilitate the calculation and comparison of subsequent paths.

[0031] Step 2: Determination of obstacles and boundaries, such as Figure 2 shown.

[0032] Step 2.1: Calculate the horizontal and vertical distances based on the position of the starting point, and determine the length of the boundary to be added to ensure that the extension of the obstacle does not interfere with the path planning.

[0033] Step 2.2 uses the first robot as the master robot, and adds corresponding obstacles and boundaries according to the negative vector of the relative position of the robots in the queue to ensure that path planning only focuses on the position of the master robot.

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

[0035] Step 3.1 converts the map into a distance matrix between adjacent grids to describe the connection relationship and distance between grids for subsequent path selection.

[0036] Step 3.2 initializes the pheromone matrix to ensure the feasibility of path planning. Pheromones guide ants to choose during the path search process.

[0037] Step 3.3: Initialize the potential field heuristic function to provide ants with path selection heuristic information and help them search for the shortest path more effectively.

[0038] Step 4: Ants’ path selection and update.

[0039] Step 4.1 The ants choose the grid to move to next by roulette according to the current pheromone concentration and heuristic information. Through the roulette strategy, the ants tend to choose the path with higher pheromone concentration or better heuristic information, thereby increasing the probability of searching for the optimal path.

[0040] Step 4.2 Update the ant's status and record the path to ensure the accuracy and feasibility of path planning. Each time the ant moves, the path will be updated, and the newly added grid will be put into the taboo table. The taboo table is used to prevent the ant from repeatedly walking through the grid that has been explored, reduce the repetitiveness in path selection, and enhance the diversity of path search.

[0041] Step 4.3 Check whether the ant has entered a dead end. If so, return to the previous step and reselect a path; otherwise, continue to move forward along the path.

[0042] Step 5: Determine dead ends and update paths Step 5.1 Determine whether 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 and reselect a path, returning to step 4 to continue path selection. This mechanism ensures that the ant will not be trapped when encountering an infeasible path, so that it can continue to explore other feasible paths.

[0043] Step 5.2 If the ant does not enter a dead end, it updates the local pheromone of the path it has walked and continues to move towards the target grid until it reaches the target. Every time an ant walks through a path, it adds pheromones to the grid of the path, promoting the path selection of other ants and increasing the global pheromone concentration, thereby helping to find the optimal path.

[0044] Step 6: Update the global pheromone matrix Step 6.1 After all ants reach the target grid, update the global pheromone matrix, including the volatilization of pheromones and the newly added parts. At this point, the global pheromone matrix provides the basis for the next round of path selection. The volatilization mechanism of pheromones ensures that the pheromone concentration of paths that have not been selected for a long time gradually decreases, thus avoiding the trap of local optimal solutions.

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

[0046] Step 7.1 Repeat steps 4 to 6 and record the path and route length of each ant in each generation. After each iteration, the global optimal path is updated to find the shortest and most optimal path. The optimal path of each generation is saved for comparison and optimization until the convergence condition of the algorithm is met or the maximum number of iterations is reached.

[0047] Step 7.2 Ensure the feasibility of path planning by continuously updating the global optimal route and finally determining the shortest path. The algorithm gradually improves the accuracy of path planning and reduces the redundant parts of the path through multiple iterations, thus finally obtaining a shortest and feasible path.

[0048] Step 8: Path Geometry Optimization Step 8.1 After all iterations are completed, output the preliminary optimal path results, including the shortest path distance, path number and path coordinates. The path is obtained by ant colony algorithm optimization, but it can be further processed by geometric optimization to improve the quality and computational efficiency of the path.

[0049] Step 8.2: Optimize the geometry of the path obtained by the algorithm, remove redundant grids, and smooth the path. The goal of this stage is to make the path smoother and less susceptible to obstacles.

[0050] Step 8.3 Use triangle pruning and polyline curvature technology to further optimize the path, reduce redundant points on the path, and increase the feasibility and execution efficiency of the path. Triangle pruning and polyline curvature processing calculates the curvature of the path and performs corresponding optimization to ensure that the final path is as simple and smooth as possible.

[0051] Step 8.4: If there is an obstacle, calculate the vertical component of the vector product between the obstacle and the path 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 return to step 8.5; if the component is both positive and negative, the path is affected by the obstacle, and the node is retained and the next node is jumped.

[0052] Step 8.5 First determine whether it is a broken line inflection point; record all inflection points and calculate the angles of all inflection points. The angle information at the inflection point helps with subsequent curve processing to avoid the path being too sharp.

[0053] Step 8.6 Calculate the reasonable radius at the inflection point, and calculate the trajectory of the curve based on the angle of the inflection point. By making the curve, the straight line segments in the path are reduced, making the overall path smoother and more natural, and avoiding unnecessary sharp turns during the movement of the robot.

[0054] Step 9: Delete the extended obstacles, draw the route of the main robot and add the routes of other robots to finally get the path of the formation Step 9.1 Delete the extended obstacles to eliminate the interference factors that may be caused in the path planning process. The geometrically optimized path will provide a more efficient and safe driving route for the main robot and other robots.

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

[0056] The path planning method of the present invention combines the ant colony algorithm with the geometric optimization method to plan the shortest, safest and feasible path for the robot queue in a complex environment. By pre-expanding obstacles, avoiding complex terrain obstacles and avoiding collisions between robots, the stability and adaptability of the robot formation are improved.

[0057] The present invention also simplifies the path structure through geometric optimization technology, reduces the complexity of calculation, and ensures that the optimized path can be efficiently generated in real-time calculation. Compared with traditional path planning methods, the path planning of the present invention not only improves the driving efficiency of the robot fleet, but also can cope with the changes of obstacles in a dynamic environment, and has stronger emergency handling capabilities.

[0058] Another embodiment of the present invention provides a robot formation path planning method, comprising: 1. Build a grid map, such as Figure 5 As shown, the black grid is the original obstacle and the white grid is the feasible area; 2. Calculate the horizontal direction based on the starting point of the main robot. The vertical distance , and determine the length of the boundary to be added. Take the first robot as the main robot and add obstacles and boundary size limits according to the negative vector of the relative distance of the queue, such as Figure 6 As shown in the figure, the gray grid is the extended obstacle to ensure that we only need to pay attention to the route of the main robot.

[0059]

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

[0061] 3. Convert the map into a distance matrix between adjacent grids , describes the connection relationship and distance between grids, and obtains the initialization pheromone matrix based on the connection relationship and distance between grids , to ensure the feasibility of path planning, obtain the initialization potential field heuristic function based on the distance between grids , providing heuristic information for path selection.

[0062]

[0063] in, and are the coordinates of the neighboring grid, is the potential field constant.

[0064] 4. Ants initialize the pheromone matrix and initialize the potential field heuristic function ,The node to move next is selected based on the roulette method, and the ant’s status and records are updated to ensure the feasibility of path planning.

[0065]

[0066] Where PP is the probability of walking towards the adjacent grid.

[0067] 5. Determine whether the ant has entered a dead end. If so, return to the previous node and put the grid into the taboo table and jump to step 4; if the ant has not entered a dead end, update the local pheromone of the route the ant has walked. , and continue until the destination is reached; 6. Update the global pheromone matrix, including pheromone volatilization and newly added parts 7. Repeat steps 4 to 6, and record the crawling route and route length of each ant in each generation, update the global optimal route, and obtain the preliminary path planning (such as Figure 7 shown); 8. To optimize the geometric route, perform the following three steps in sequence: Remove redundant grids: Find consecutive path nodes and yes The neighbor grid of , then the original continuous path grid is deleted , the new continuous path grid is ,like Figure 8 As shown; Triangle pruning: If multiple consecutive nodes in the path form a multi-segment polyline, and the direct connection path between and does not pass through any obstacles, the multiple polyline segments from to can be replaced by the straight line segments between and . The effect is as follows Fig. 9 ; Convert a broken line to a curve: Select a node and determine whether it is a broken line inflection point; if so, replace the broken line with a curve; if not, jump to the next inflection point until the destination. The effect is as follows: Fig.10 .

[0068] 9. Delete the grid extended by the obstacle, draw the path of the main robot, add other robots and draw their paths, and finally get the path of the formation. The effect is as follows: Fig.11 ; like Fig.12 As shown, the present invention provides a robot formation path planning system, comprising: Construction module: used to construct grid maps and expand obstacles and boundaries of grid maps; Initialization module: used to determine the initialization pheromone matrix and initialization potential field heuristic function of the grid map; Preliminary planning module: It is used for ants to select the mobile node for each step based on the roulette wheel method according to the initialization pheromone matrix and the initialization potential field heuristic function, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; Formation path acquisition module: used to optimize the preliminary path planning, delete extended obstacles and boundaries, and obtain the robot formation path.

[0069] A terminal device is provided in one embodiment of the present invention. The terminal device of this embodiment 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, the steps in the above-mentioned method embodiments are implemented. Alternatively, when the processor executes the computer program, the functions of the modules / units in the above-mentioned device embodiments are implemented.

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

[0071] The terminal device may be a computing device such as a desktop computer, a notebook, a PDA, a cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

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

[0073] The memory may be used to store the computer program and / or module, and 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.

[0074] If the module / unit integrated in the terminal device is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the computer-readable medium can be appropriately increased or decreased 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 electric carrier signals and telecommunication signals.

[0075] Although the embodiments of the present invention are described above in conjunction with the accompanying drawings, the present invention is not limited to the above-mentioned specific embodiments and application fields, and the above-mentioned specific embodiments are only illustrative and instructive, rather than restrictive. Under the guidance of the specification, a person skilled in the art can also make many forms without departing from the scope of protection of the claims of the present invention, all of which belong to the scope of protection of the present invention.

Claims

1. A robot formation path planning method, characterized in that: include: Build a grid map and expand the obstacles and boundaries of the grid map; Determine the initialization pheromone matrix and initialization potential field heuristic function of the grid map; According to the initialized pheromone matrix and the initialized potential field heuristic function, the ants select the mobile nodes for each step based on the roulette wheel method, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; The preliminary path planning is optimized, and the extended obstacles and boundaries are deleted to obtain the robot formation path.

2. A robot formation path planning method according to claim 1, characterized in that: The steps to extend the obstacles and boundaries of the grid map are as follows: According to the position of the starting point of the main robot on the grid map, the positional relationship between the main robot and other formation robots is determined, and then the inverse vector of the displacement vector from the main robot to other formation robots is calculated, and obstacles and boundaries are expanded according to the inverse vector.

3. A robot formation path planning method according to claim 1, characterized in that: The specific steps for determining the initialization pheromone matrix and initialization potential field heuristic function of the grid map are: 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 initialization pheromone matrix is ​​obtained according to the connection relationship and distance between grids, and the initialization potential field heuristic function is determined according to the distance between grids.

4. A robot formation path planning method according to claim 3, characterized in that: The formula for obtaining the initialization potential field heuristic function is: in, and are the coordinates of the neighboring grid, is the potential field constant, is the potential field inspiration function.

5. A robot formation path planning method according to claim 1, characterized in that: The steps of selecting the mobile node at each step based on the roulette wheel method and obtaining the crawling route and route length of the ant are as follows: First, the ant selects the next moving node based on the roulette method, and determines whether the next moving node is in a dead end. If it is in a dead end, it returns to the previous node and puts the grid into the taboo table, and re-selects the next moving node based on the roulette method. If it is not in a dead end, it does not return to the previous grid, but directly selects the next moving node based on the roulette method, updates the local pheromone of the route, and continues walking until it reaches the destination; Repeat the above steps, record the crawling route and route length of each ant in each generation, update the global optimal route, and obtain the preliminary path planning.

6. A robot formation path planning method according to claim 1, characterized in that: The optimization of the preliminary path planning is as follows: The initial path planning is optimized by deleting redundant grids, triangle pruning and polyline curvature.

7. A robot formation path planning method according to claim 6, characterized in that: Delete redundant grids: Find continuous path nodes and yes The neighbor grid of , then the original continuous path grid is deleted , the new continuous path grid is ; Triangle pruning is as follows: if multiple consecutive nodes in the path form a multi-segment polyline, and the direct connection path between one node and another node does not pass through any obstacles, then replace the multi-segment polyline from one node to another node with a straight line segment between the two nodes; The broken line is converted to a curve as follows: select a node and determine whether it is a broken line inflection point; if so, replace the broken line with a curve; if not, jump to the next inflection point until the destination.

8. A robot formation path planning system, characterized in that: include: Construction module: used to construct grid maps and expand obstacles and boundaries of grid maps; Initialization module: used to determine the initialization pheromone matrix and initialization potential field heuristic function of the grid map; Preliminary planning module: It is used for ants to select the mobile node for each step based on the roulette wheel method according to the initialization pheromone matrix and the initialization potential field heuristic function, obtain the crawling route and route length of the ants, and obtain the preliminary path planning based on the crawling route and route length of the ants; Formation path acquisition module: used to optimize the preliminary path planning, delete the extended obstacles and boundaries, and obtain the robot formation path.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of a robot formation path planning method as described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of a robot formation path planning method as described in any one of claims 1 to 7 are implemented.

Images