A robot path planning method, system, storage medium and program product

Abstract

The application provides a robot path planning method, system, storage medium and program product. The method is executed under an iteration framework of an A* algorithm, includes: initializing a search environment with a starting point and an ending point; searching nodes in the search environment until the search ends; and backtracking the nodes to generate a path or restart the search. The search environment at least includes: a to-be-explored set storing all discovered but not completely expanded nodes; an explored set storing completely expanded nodes; and a control region set consisting of one or more control regions. In the process of searching the nodes, conflict detection is performed on selected nodes, and the total cost of the nodes is dynamically adjusted according to a preset conflict processing rule, so as to adjust the path. In this way, the path changing and avoidance between robots can be avoided as much as possible, and the work efficiency is improved.

Description

Technical Field

[0001] This application belongs to the field of path planning technology, specifically relating to a robot path planning method, system, storage medium, and program product. Background Technology

[0002] Currently, when material handling robots (including different types such as side-mounted and stealth robots) perform material handling operations in warehousing or factory settings, their paths are usually pre-set, and the operations of different types of robots in the same aisle or control area are mutually exclusive. For example, when type A robot enters a certain aisle, if type B robot also enters the same aisle, it can easily cause congestion, route changes, retreats, or blockages, severely impacting operational efficiency.

[0003] However, existing path planning methods often only consider the geometric shortest path or the scheduling of a single type of robot, failing to incorporate the spatial mutual exclusion relationships between multiple robot types into the planning process. Therefore, in actual operation, conflicts are only discovered during the execution phase, leading to numerous temporary route changes and avoidance operations, reduced operational efficiency, increased collision risks and safety hazards, and even causing multiple robots to wait for each other or even lock up in confined areas.

[0004] Therefore, there is an urgent need for a path planning method that enables multiple types of robots to work together efficiently and safely in complex shared spaces. Summary of the Invention

[0005] To address the aforementioned issues, the purpose of this application is to provide a robot path planning method, system, storage medium, and program product that can minimize the need for robots to change paths and avoid collisions, thereby improving operational efficiency.

[0006] This application provides a robot path planning method, which is executed within the iterative framework of the A* algorithm. The method includes: initializing a search environment based on a known start and end point; cyclically searching for nodes in the search environment until the search ends; and backtracking nodes to generate a path or restarting the search. The search environment includes at least: a set to be explored, storing all discovered but not fully expanded nodes; an explored set, storing nodes that have been fully expanded; and a control region set, consisting of one or more control regions. During the cyclical search of nodes, conflict detection is performed on selected nodes, and the total cost of the nodes is dynamically adjusted according to preset conflict handling rules, thereby adjusting the path.

[0007] Alternatively, in this application, initializing the search environment includes: initializing the set to be explored, the set already explored, and the set of control regions; and adding the starting point to the set to be explored, and calculating the total cost, actual cost, and heuristic cost of the starting point.

[0008] Alternatively, in this application, the search ends when the set to be explored is empty.

[0009] Alternatively, in this application, the conflict detection may include at least area conflict detection, which detects whether the area where the robot is located is a control area, and the conflict handling rules may include at least preventing different types of robots from occupying the same control area at the same time.

[0010] Alternatively, in this application, the process of iteratively searching for nodes may include: selecting the node n with the minimum total cost from the set to be explored; traversing all neighboring nodes m of node n and performing the aforementioned regional conflict detection on neighboring nodes m; calculating the total cost of neighboring nodes m based on the results of the regional conflict detection; and updating neighboring nodes m based on the total cost.

[0011] Alternatively, in this application, when node n is the endpoint, the search ends by tracing back the path through the parent and child nodes.

[0012] Alternatively, in this application, when the result of the regional conflict detection is that the neighboring node m is not in the control area set, or the neighboring node m is in the control area set but there are no other types of robots in the control area set, the total cost of the neighboring node m is calculated; when the result of the regional conflict detection is that the neighboring node m is not in the control area set and there are other types of robots in the control area set, the neighboring node m is discarded.

[0013] Alternatively, in this application, when calculating the total cost of a neighboring node m, the total cost consists of three parts: the actual cost, which represents the cumulative path cost from the starting point to the neighboring node m; the heuristic cost, which represents the estimated cost from node n to the neighboring node m; and the adjustment cost, which is dynamically adjusted based on the result of conflict detection of the neighboring node m.

[0014] Alternatively, in this application, the conflict detection may include at least: steering detection for detecting whether a turn occurs in the path; point-occupancy conflict detection for detecting whether multiple robots are competing for the same point; and reverse edge conflict detection for whether multiple robots are traveling in opposite directions; the conflict handling rules may include at least the adjustment cost of adding conflict avoidance to the neighboring node m.

[0015] Alternatively, in this application, the adjustment cost may include at least: a steering cost representing the penalty for turning in the path; a point-occupancy conflict cost representing the penalty for multiple robots competing for the same point; and a reverse edge conflict cost representing the penalty for multiple robots traveling in opposite directions.

[0016] Alternatively, in this application, during the turning detection, it is detected whether a change of direction occurs from node n to neighboring node m. If a turning is required between node n and neighboring node m, the turning cost is added to the adjustment cost. The turning cost is 3 × dist(n, m), where dist(n, m) represents the basic cost between node n and neighboring node m.

[0017] Alternatively, in this application, if the number of other robots that have occupied or plan to occupy a neighboring node m within a specified time is detected in the occupation conflict detection, and if the number of other robots detected is crossNodeNum, then the occupation conflict cost is added to the adjustment cost, and the occupation conflict cost is log(∑crossNodeNum + 1), where crossNodeNum > 0.

[0018] Alternatively, in this application, when the cost of the occupation point conflict exceeds a preset threshold, the neighboring node m is discarded to avoid entering the high-congestion area.

[0019] Alternatively, in this application, if the reverse edge conflict detection detects the number of other robots whose travel path within a specified time includes a reverse edge [m, n] opposite to the current edge [n, m], and if the detected robot's edgeNum is added to the adjustment cost, the reverse edge conflict cost is increased to 10 × edgeNum × dist(n, m), where dist(n, m) represents the basic cost between node n and neighboring node m.

[0020] Alternatively, in this application, when updating the neighboring node m based on the total cost, if the neighboring node m is in the explored set and the total cost decreases, then the parent node of the neighboring node m is updated.

[0021] If the neighboring node m is in the set to be explored, then skip the neighboring node m; if the neighboring node m is not in either the explored set or the set to be explored, then set the parent node of the neighboring node m to node n, and add the neighboring node m to the set to be explored.

[0022] Alternatively, in this application, after the search is completed, a valid path is generated through parent and child nodes. If no valid path is generated, the search is restarted after a preset time interval.

[0023] Effects of the Invention: The robot path planning method provided in this application introduces regional mutual exclusion control and dynamic cost calculation mechanisms into the A* algorithm framework. During the planning phase, it detects whether different types of robots will conflict in the same area, discarding conflicting paths in advance and avoiding frequent route changes and retreats during execution. Simultaneously, it dynamically adjusts multiple dimensions in real time, including turning penalties, point-occupancy conflict costs, and reverse edge conflict costs, making path planning between similar and different types of robots more aligned with actual operational needs, reducing congestion, opposing conflicts, and deadlock risks. Through this method, multiple types of robots can achieve efficient, safe, and collaborative operation in a shared space, significantly improving the overall operational efficiency and stability of the system. Attached Figure Description

[0024] Figure 1 This is an overall flowchart of the path planning method according to one embodiment of this application;

[0025] Figure 2 yes Figure 1 A partial flowchart of step S1 in the process;

[0026] Figure 3 yes Figure 2 A partial flowchart of step SA in the process;

[0027] Figure 4 yes Figure 2 A partial flowchart of step SB in the process;

[0028] Figure 5 yes Figure 2 A partial flowchart of step SC in the process;

[0029] Figure 6 yes Figure 2 A partial flowchart of steps SD in the process;

[0030] Figure 7 yes Figure 1 A partial flowchart of step S2 in the process;

[0031] Figure 8 yes Figure 1 A detailed flowchart of the path planning method. Detailed Implementation

[0032] The present application is further described below with reference to the following embodiments. It should be understood that the following embodiments are for illustrative purposes only and are not intended to limit the present application. The same or corresponding reference numerals in the figures denote the same parts, and repeated descriptions are omitted.

[0033] Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0034] This application provides a robot path planning method, system, readable storage medium, and program product based on the iterative framework of the A* algorithm. For clarity and understanding, some technical terms and / or technical expressions used in this application will be explained or defined.

[0035] The "iterative framework of the A* algorithm" mentioned in this application is a general algorithmic framework for path planning that progressively expands nodes in the search space to find the optimal path from the starting point to the destination. The A* algorithm is an ordered search algorithm, characterized by its definition of the evaluation function.

[0036] The “open_set” mentioned in this application is one of the three core data structures contained in the iterative framework of the A* algorithm, used to store the nodes to be explored, i.e., the set to be explored.

[0037] The “close_set” mentioned in this application is one of the three core data structures contained in the iterative framework of the A* algorithm, used to store the explored nodes, i.e., the explored set.

[0038] The term “constraint_set” as used in this application refers to a set consisting of one or more control regions, i.e., a set of control regions.

[0039] The “x” mentioned in this application n " represents the coordinates of node n on the x-axis.

[0040] The "y" mentioned in this application n " represents the y-coordinate of node n.

[0041] The term "toNode" as used in this application refers to a unique identifier for the current node, such as its coordinates (x, y) or status number.

[0042] The term "fromNode" mentioned in this application refers to the parent node from which the current node is extended. The parent-child relationship is formed through fromNode→toNode to trace the path back.

[0043] The terms "node p, n, m" mentioned in this application do not always represent a specific node, but change with the iteration cycle. They are used more to distinguish the expressions to avoid confusion. For example, when node p represents the currently expanded node, node n represents a neighboring node of node p. Similarly, when node n represents the currently expanded node, node p represents the parent node of node n, and node m represents a neighboring node of node n.

[0044] The “h value” or “h(n)” mentioned in this application refers to the heuristic estimated cost from the current node n to the target node, i.e., the heuristic cost.

[0045] The “g value” or “g(n)” mentioned in this application refers to g(n) = g(p) + dist(p, n) + cost(p, n), where g(n) represents the actual cost from the starting point to the current node n, such as the distance traveled or the number of steps. Node n represents the node that is currently being expanded, and node p represents the parent node of node n.

[0046] In this application, "cost(n, m)" refers to the additional adjustment cost between two nodes (node ​​n → node m), where node n is the node currently being expanded, and node m is a neighboring node of node n. Similarly, in this application, "cost(p, n)" refers to the additional adjustment cost between two nodes (node ​​p → node n), where node p is the parent node of the current node n, and node n is the node being expanded.

[0047] It should be understood that the significance of distinguishing between cost(p, n) and cost(n, m) in this application is to illustrate that during the path search process, when expanding from the parent node p to the current node n, cost(p, n) is used to calculate the increment of the g value, while when expanding from the current node n to the neighboring node m, cost(n, m) is used to calculate the actual cost of extending the candidate path, thereby dynamically updating the total cost f(m) of the neighboring nodes. Therefore, the two expressions have different logical positions in the A* search structure, but there is no essential difference. They are only used to emphasize the current algorithm logic, and the same applies below.

[0048] In this application, "dist(n, m)" represents the basic cost between two nodes (node ​​n → node m), where node n is the node currently being expanded, and node m is a neighboring node of node n. Similarly, in this application, "dist(p, n)" represents the basic cost between two nodes (node ​​p → node n), where node p is the parent node of the current node n, and node n is the node being expanded.

[0049] The “f value” or “f(n)” mentioned in this application refers to the total cost used to comprehensively evaluate the node priority, which combines the actual cost and the estimated cost. f(n) = g(n) + h(n), where node n represents the node currently being expanded, and node p is the parent node of node n. The smaller the f(n) value, the more priority node n is given to expansion.

[0050] The term "path" as used in this application refers to the path with the minimum total cost f(n) between the starting point and the ending point, found using the A* algorithm.

[0051] Furthermore, those skilled in the art should understand that multiple types of robots and the central control system form a multi-level integrated structure via a communication network. The central control system, acting as the global scheduling center, is responsible for receiving transport tasks from the upper-level system (e.g., the coordinates of the starting and ending points from upper-level instructions), collecting real-time data on the positions, statuses, and occupancy of various areas of the multiple robot types, and performing unified path planning and dynamic adjustments within the A* iterative framework. Different types of robots (such as lurking and side-mounted transport vehicles) act as execution units, completing transport operations according to the path instructions issued by the central control system and continuously feeding back real-time data through the communication network.

[0052] Furthermore, in this application, the data of the nodes (such as f value, g value, h value, etc.) are stored in the node information record structure. During the node expansion process, the data of the current node n is retrieved from the set to be explored (open_set) or the node information table.

[0053] The following, in conjunction with the appendix Figures 1-8 The robot path planning method according to one embodiment of this application will be described. Figure 1 This is an overall flowchart of the path planning method according to one embodiment of this application. Figure 1 As shown, in this embodiment, the path planning method includes steps S0 to S2. Figure 2 This is a flowchart showing step S1. Figures 3-6 Steps SA through SD are then broken down and shown step by step. Figure 7 The flowchart for step S2 is shown in detail. Finally, Figure 8 In this embodiment, all steps are roughly integrated into a single flowchart to facilitate a comprehensive understanding of the path planning method from the overall picture to the details.

[0054] In step S0, the algorithm is initialized and the search environment is prepared.

[0055] For example, in step S01, open_set, close_set, and constraint_set are initialized.

[0056] Next, in step S02, the starting point is placed into open_set, and the f, g, and h values ​​of the starting point are calculated.

[0057] In step S1, a loop search is performed, continuously selecting the optimal node from the open_set for expansion until the loop terminates. The optimal node is the node with the lowest total cost.

[0058] First, select the node n that will be best expanded from the nodes to be explored (i.e., step SA).

[0059] For example, in step S11, it is determined whether open_set is empty.

[0060] If open_set is empty (yes in step S11), the algorithm ends and proceeds to step S2. An empty open_set indicates that the algorithm has no expandable nodes.

[0061] If open_set is not empty (not in step S11), then proceed to step S12.

[0062] In step S12, the node n with the smallest f value is selected from open_set. This indicates that the algorithm selects node n as the optimal path.

[0063] In detail, `open_set` is a set of nodes to be explored, where each node contains information such as `toNode`, `fromNode`, `f`, `g`, and `h`, which are used to wait for expansion. When `open_set` is empty, it means there are no candidate nodes that can be expanded, and the current search cannot find a path under the existing constraints. Therefore, this round of search ends, and the process proceeds to the processing branch of step S3 (detailed later). When `open_set` is not empty, the node `n` with the smallest `f` value is selected as the node that is more worthy of being expanded first, that is, the node with higher priority. As the value of f, f(n) = g(n) + h(n), where g(n) = g(p) + dist(p, n) + cost(p, n), node n represents the node currently being expanded, node p represents the parent node of node n, dist(p, n) represents the basic cost from node p (in the initial loop phase, node p can be, for example, the starting point) to node n, and cost(p, n) represents the adjustment cost from node p to node n. Data such as g(p) of node p can be retrieved from the algorithm's node data structure (such as a dictionary, hash table, node class instance, etc.), and the calculated data (such as the f value, g value, etc.) will also be stored corresponding to node n.

[0064] In step S13, it is determined whether node n is the endpoint.

[0065] If node n is the endpoint (yes in step S13), then proceed to step S131.

[0066] In step S131, based on the parent-child node relationship, the algorithm backtracks from the endpoint to the starting point to generate the result path, and then proceeds to step S2, at which point the algorithm ends.

[0067] If node n is not the endpoint (no in step S13), then proceed to step S14.

[0068] In step S14, it is determined whether there is an unsearched node n in open_set.

[0069] If there is no unsearched node n in open_set (not found in step S14), then proceed to step S2, and the algorithm ends. Specifically, if there is no unsearched node n in open_set, it means there are no more nodes to expand upon before reaching the destination, making path planning impossible, and the algorithm terminates.

[0070] If there is an unsearched node n in open_set (yes in step S14), then proceed to step S15.

[0071] In step S15, a state transition is performed on node n. That is, node n is removed from the open_set and added to the close_set. In detail, the current node n has already been selected in step S12 because it has the smallest f value, which means that the current path up to node n is optimal. Removing n from the open_set can prevent node n from being repeatedly expanded, thus maintaining search efficiency. Moreover, adding node n to the close_set can prevent the algorithm from going into an infinite loop or path conflict.

[0072] Next, traverse all neighboring nodes m of node n to confirm the region conflict of neighboring node m (i.e., step SB).

[0073] In detail, in path planning implementation, the map is usually abstracted into a graph or grid. Each point (location) in the graph or grid is a node n, such as each cell in the map grid, an intersection in the path network, or a key location that the robot may reach, etc. A neighboring node (m) is a node that is directly connected to the current node n or is reachable in one step. Traversing all neighboring nodes m of node n means visiting each node m adjacent to node n in turn and processing it. In the A* algorithm, for example, starting from the current node n, all neighboring nodes m are found, the f value is calculated for each neighboring node m, and by comparing the f values, the optimal node for the next step is selected to continue exploration. In this implementation, the iterative architecture of the A* algorithm is also based, and the traversal is implemented through the loop method described later.

[0074] For example, in step S16, we explore a neighboring node m of node n. That is, we can select a neighboring node m for processing based on known rules such as priority order.

[0075] However, when multiple robots of different types exist in a given scenario and each performs its own path planning, mutual exclusion conflicts may occur within the same passage or control area on a unified map or grid. For example, if two robots are moving towards each other in a passage according to their respective optimal paths, one robot may have to exit the passage to allow the other to pass, significantly reducing operational efficiency. To address this, this implementation performs regional conflict detection on the selected neighboring node m to confirm its conflict status within the control area.

[0076] For example, in step S161, it is determined whether the neighboring node m is in the constraint_set. In this embodiment, the control area that makes up the constraint_set can be, for example, a lane, an intersection, a task area, a loading and unloading area, etc. Furthermore, in this embodiment, the constraint_set can be preset, can be a fixed area, or can its range be dynamically adjusted according to the actual scenario.

[0077] If the neighboring node m is in the constraint_set (yes in step S161), proceed to step S1611. This indicates that the neighboring node m is within the control range, and further checks on the occupancy within that region are needed. Furthermore, when the constraint_set contains multiple control regions, it specifically refers to the control region where the neighboring node m is located.

[0078] For example, in step S1611, it is determined whether other types of robots exist within the control area (in the art, a quantity limit is set for the same type of robot within the work area, but no conflict limit is set). In this embodiment, for example, the real-time status of constraint_set can be queried to confirm whether other types of robots already exist there. Furthermore, in this embodiment, the status of constraint_set is dynamically updated during path planning.

[0079] If other types of robots exist within the controlled area (yes in step S1611), proceed to step S1612.

[0080] In step S1612, the neighboring node m is discarded (without expansion), and then the process proceeds to step S19. Therefore, in this embodiment, different types of robots cannot enter the same control area simultaneously. If another type of robot is detected already existing in the control area where the neighboring node m is located, the current robot cannot enter that neighboring node m. Since different types of robots have different sizes, turning radii, speeds, and control logic, they may interfere with each other in the same area or road segment. Therefore, the algorithm prohibits different types of robots from entering the same control area simultaneously. Furthermore, in this embodiment, different types of robots can be, for example, trolley transport vehicles, side-mounted transport vehicles, etc. In addition, during path planning, multiple robots of the same type can usually enter the same control area simultaneously because they have consistent sizes, speeds, and control logic, and can achieve cooperative control (such as platooning, automatically maintaining distance, following other vehicles), resulting in low safety risks and preventing interference between robots of different structures. However, for robots of the same type, although they can travel together on the same path (without mutual exclusion), considering the limited space in the work area, multiple robots working simultaneously can easily cause congestion and reduce efficiency. Therefore, it is possible to control the number of robots of the same type entering the work area (such as loading and unloading points, shelving areas, transfer ports, etc.) at the same time. For example, if the number of robots of the same type in the controlled area reaches a certain limit, then the adjacent node m is discarded.

[0081] Therefore, this implementation determines whether a neighboring node m is worth expanding based on the regional situation at the initial stage of exploring neighboring nodes m. In other words, before calculating the f value for neighboring node m, it first determines whether the neighboring node m to be expanded next is in a constrained control area, and whether that control area has been occupied by other types of robots. If there is a constraint and it is occupied, the neighboring node m will be discarded. If there is a constraint but it is not occupied, it indicates that the neighboring node m can be expanded. Therefore, this implementation provides a shared constraint control space for path planning of multiple types of robots, enabling different types of robots to cooperate safely and efficiently in complex scenarios. This not only avoids route changes and retreats during the execution phase, thereby improving work efficiency, but also eliminates nodes that are not worth expanding as early as possible, thereby significantly improving planning efficiency.

[0082] In step S19, it is determined whether there are any untested neighboring nodes m.

[0083] If there are still untested neighboring nodes m around node n (yes in step S19), then proceed to step S191.

[0084] In step S191, select any one of the untested neighboring nodes m around node n, proceed to step S161, and repeat the subsequent steps to ensure that all neighboring nodes m of node n can be traversed.

[0085] If there are no unchecked neighboring nodes m around node n (no in step S19), proceed to step S11. This indicates that all neighboring nodes m around node n have been checked and all have conflicts that prevent further expansion, making it impossible to proceed with the next path planning step. Therefore, return to step S11 and reselect the next optimal node n from open_set for expansion.

[0086] If there are no other types of robots within the control area (no in step S1611), then proceed to step SC as described later.

[0087] If the neighboring node m is not in the constraint_set (which is not the case in step S161), proceed directly to step SC described later. Not being in the constraint_set indicates that the neighboring node m is not subject to the constraints of the control region.

[0088] Next, the cost is calculated for the neighboring nodes m that have no regional conflicts after the above screening (i.e., step SC).

[0089] In step SC, the cost is calculated based on the relationship between node n and neighboring node m, that is, the f value of neighboring node m is calculated. An exemplary method for calculating the f value of neighboring node m is described in detail below.

[0090] First, in step SC1, the heuristic cost h(m) is calculated.

[0091] In this embodiment, h(m) = |x n -x m | + |y n -y m |. That is, calculating the Manhattan distance between node n and its neighboring node m is a local heuristic. In this embodiment, the calculation of h(m) is not specifically limited. For example, Manhattan distance can be used for four-adjacent grids, and Euclidean distance can be used for eight-adjacent or continuous coordinates, as long as it is consistent with the global strategy.

[0092] Next, in step SC2, the adjustment cost cost(n, m) is calculated. For example, the initial value cost(n, m) can be set to 0.

[0093] In this embodiment, the adjustment term cost(n, m) is optimized to improve path planning. That is, penalties (i.e., increased costs) are added for turning, crossNodeNum, and edgeNum. Therefore, in this embodiment, the direction is set as one of the node's attributes, which is data that needs to be stored and maintained together with the node, for example, by adding a parent pointer to the node structure.

[0094] Calculation of turning costs.

[0095] First, the direction of the neighboring node m is obtained, and it is determined whether a turn occurs between node n and neighboring node m. In this embodiment, for example, the judgment can be made based on node information such as node coordinates. If a change in direction occurs from node n to neighboring node m, i.e., a turn is required, then the adjustment cost cost(n, m) = cost(n, m) + 3 × dist(n, m). Here, dist(n, m) is the basic cost between node n and neighboring node m, constituting the underlying distance factor for calculating the g value, turning penalty, reverse edge penalty, etc. In this embodiment, dist(n, m) is not only the distance in physical space, but also includes physical constraints and spatial layout during robot movement. Here, 3 × dist(n, m) can be regarded as the turning cost, indicating that the penalty value is proportional to the basic movement cost, and in this embodiment, it is amplified by a coefficient of 3. By increasing the turning cost, the penalty for turning is much higher than that for straight movement, thereby avoiding turning as much as possible, allowing the algorithm to prioritize smooth straight paths and reduce turning, but not completely prohibiting turning, which is especially suitable for multi-robot scenarios such as warehouses and factories. However, the coefficient is set to 3 in this embodiment, but it is not limited to this. It can be adjusted appropriately according to the specific situation. For example, if the site is large and easy to turn, it can be reduced appropriately; if the alley is narrow and difficult to turn, it can be increased appropriately.

[0096] Calculation of the cost of point capture conflicts.

[0097] In multi-type mobile robot path planning scenarios, a node m (e.g., an alleyway, intersection, or loading / unloading position) may be occupied or about to be occupied by multiple robots simultaneously. If these real-time conflicts are not considered, the algorithm will only choose the geometrically shortest path, easily leading to multiple robots entering the same point simultaneously (congestion, collision) or two or more robots waiting for each other, causing system stagnation (deadlock). Therefore, in this implementation, to avoid the above problems, when expanding neighboring nodes m, the algorithm needs to detect how many other robots plan to occupy node m at the current and future times, and reflect this in the search process through a congestion cost function, enabling the algorithm to actively avoid congested areas.

[0098] First, the number of other robots at node m, crossNodeNum, is detected at a specified time. When calculating the number of adjacent nodes m that other robots plan to occupy, since different robots have different speeds, the occupancy status can be comprehensively judged based on the time dimension, rather than just the current position. For example, in this embodiment, the number crossNodeNum includes at least the total number of robots that are already at node m when this robot arrives at node m, or are planned to arrive at node m at the same time, or arrive at node m during this robot's stay.

[0099] Next, the cost of capturing nodes is calculated using the formula: cost = log(∑crossNodeNum + 1). For example, with one robot, log(1+1)≈0.693; with two robots, log(2+1)≈1.099; with three robots, log(3+1)≈1.386; and with four robots, log(4+1)≈1.609, and so on. Thus, the cost increases slowly, avoiding infinitely large penalties from a small number of robots. When there is only slight congestion, the algorithm can still consider proceeding; as congestion increases and the cost rises, the algorithm tends to detour.

[0100] If crossNodeNum > 0, the calculated occupation conflict cost is added to the adjustment cost cost(n,m) = cost(n,m) + cost. If crossNodeNum = 0, no occupation conflict cost is added, and the adjustment cost cost(n,m) remains unchanged.

[0101] Therefore, by calculating crossNodeNum and applying log compression penalties, the algorithm can dynamically avoid congested areas during path planning, enabling multi-robot systems to operate efficiently, safely, and collaboratively in a shared space, thereby reducing collisions, deadlocks, and task delays. Furthermore, in this embodiment, a weight w can be set to further determine the robot's sensitivity to capture point conflicts. In this case, the capture point conflict cost is calculated as cost = log(∑crossNodeNum + 1) × w. For example, in this embodiment, for robots of the same type, weight w can be set separately based on their respective states (e.g., idle with task, heavily loaded with task, faulty robot, no task, etc.). self For example, robots with heavy workloads have a higher weight, robots with idle workloads have a medium weight, and robots without tasks have a lower weight. Furthermore, different penalty weights can be set for different types of robots. type For example, heavily loaded robots have a larger weight, while lightly loaded robots have a smaller weight. Furthermore, in this embodiment, neighboring nodes m can be discarded directly when the log value exceeds a certain threshold, further avoiding waste of computational resources.

[0102] Calculation of the conflict cost of the reverse edge.

[0103] When robot A prepares to move from node n to neighboring node m, if another robot B is expected to arrive at the same time and is moving in the opposite direction (from neighboring node m to node n), this constitutes a conflict between opposing sides. In path planning, conflicts between opposing sides are generally more likely to reduce robot efficiency than turning or point capture, especially in narrow alleys or single-lane roads, where robot braking, yielding, or getting stuck also poses significant safety risks. Therefore, in this implementation, to avoid these problems, the cost of the reverse edge is estimated, allowing the algorithm to select paths without conflict as much as possible.

[0104] First, the number of other robots (edgeNum) whose travel paths include edge [m, n] (the opposite edge to the current robot's travel path [n, m], i.e., the reverse edge) within a specified time window is detected, including robots that are currently or plan to reach node m. The travel path includes the actual travel path or the planned travel path. Therefore, the specified time window can be, for example, the time interval during which the current robot travels on edge [n, m]. Thus, if the time interval during which other robots travel on edge [m, n] overlaps with the specified time window, it indicates that there may be a conflict between them, and therefore, the other robots are included in edgeNum.

[0105] When there are opposing robots in conflict, the adjustment cost is calculated as cost(n, m) = cost(n, m) + 10 × edgeNum × dist(n, m). In this implementation, 10 × edgeNum × dist(n, m) can be regarded as the cost of opposing edge conflict. The coefficient 10 is much larger than the turning penalty coefficient, indicating that the algorithm tries to exclude opposing conflicts from the planning and prioritizes safer and smoother passage.

[0106] In summary, in this implementation, the adjustment cost cost(n, m) = initial value (= 0) + turning cost (= 3 × dist(n, m)) + point occupation conflict cost (= log(∑crossNodeNum + 1)) + reverse edge conflict cost (= 10× edgeNum × dist(n, m)).

[0107] Furthermore, in this embodiment, the conflict cost of same-direction edges can also be considered. For example, when multiple types of robots travel along the same edge [m, n] in the same direction, queue congestion may occur due to different speeds or insufficient distance, resulting in frequent acceleration and deceleration or even rear-end collisions. Therefore, for example, the number of other robots edgeNum on edge [m, n] and the safe distance in front and behind can be detected within a fixed time window. If edgeNum > 0 and the safe distance in front and behind is less than a specified threshold, the conflict cost of same-direction edges is increased or the node is discarded.

[0108] Next, in step SC3, the actual cost g(m) is calculated. In this embodiment, g(m) = g(n) + dist(n,m) + cost(n, m). Here, g(n) is the actual cost of the currently expanding node n, representing "the cumulative cost from the starting point to node n".

[0109] Next, in step SC4, the total cost f(m) = g(m) + h(m) is calculated for subsequent evaluation, sorting, and expansion.

[0110] At this point, the cost calculation for the neighboring node m is complete.

[0111] Next, based on the cost calculation results (i.e., f(m)) and the expansion state of the neighboring node m, the node is updated to achieve path adjustment (i.e. step SD).

[0112] For example, in step S17, it is determined whether the neighboring node m is in close_set.

[0113] If the neighboring node m is in the close_set (yes in step S17), then proceed to step S171. Its existence in the close_set indicates that the neighboring node m has been fully expanded; in other words, the neighboring node m has been removed from the open_set and undergone complete cost calculation. That is, the neighboring node m already has an f value representing the optimal path.

[0114] In step S171, it is determined whether the calculated f value (also called the new f value) of the neighboring node m is less than the previously known f value (also called the old f value). The new f value is the path cost to the neighboring node m calculated in step SD, and the old f value is the path cost to the neighboring node m recorded in close_set, indicating that it is the optimal path cost to the neighboring node m at that time.

[0115] If the calculated f value is less than the originally known f value (yes in step S171), then proceed to step S1711. This indicates that a better path to the neighboring node m has been found.

[0116] In step S1711, the parent node of the neighboring node m is updated, and then the process proceeds to step S19, repeating the subsequent steps. Thus, when a better path is found, the path is dynamically adjusted by updating the parent node, ensuring the optimal path is always maintained.

[0117] If the calculated f value is not less than the originally known f value (which was not the case in step S171), then proceed to step S1712. This indicates that the stored path to the neighboring node m is still the optimal path.

[0118] In step S1712, the neighboring node m is skipped, that is, the original state is maintained, and then the process proceeds to step S19 to repeat the subsequent steps.

[0119] If the neighboring node m is not in close_set (which was not in step S17), then proceed to step S18.

[0120] In step S18, it is determined whether the neighboring node m is in open_set.

[0121] If the neighboring node m is in open_set (yes in step S18), then proceed to step S181.

[0122] In step S181, the neighboring node m is skipped, i.e., it remains unchanged, and then the process proceeds to step S19 to repeat the subsequent steps. The neighboring node m in open_set indicates that the neighboring node m has been discovered and stored by the algorithm, but has not yet been fully expanded. Therefore, it does not need to be added again at this time, but waits for subsequent exploration, such as expansion at an appropriate time.

[0123] If the neighboring node m is not in the open_set (which was not the case in step S18), then proceed to step S182. This indicates that the neighboring node m is a completely new and undiscovered node that needs to be formally included in the set of nodes to be explored.

[0124] In step S182, the parent node of the neighboring node m is set to node n, that is, parent(m) = n. Thus, parent(m) = n records the node m obtained by expanding from a certain parent node n, so that the path can be reconstructed during subsequent backtracking.

[0125] Next, in step S183, the neighboring node m is added to the open_set, and then the process proceeds to step S19, repeating the subsequent steps. Specifically, the neighboring node m is added to the open_set and its f, g, and h values ​​are stored. Furthermore, for example, the open_set can be automatically sorted according to the f values ​​of the neighboring nodes m using a min-heap or priority queue, allowing the neighboring node m to participate in the next round of selection along with other nodes to be explored.

[0126] In step S2, the search ends (i.e., the algorithm ends), and the path is generated or the algorithm is restarted.

[0127] For example, in step S20, the algorithm is terminated. This step, as a node or boundary, is to uniformly manage termination, waiting, and resetting, ensuring that all branch steps complete the loop, guaranteeing search closure, and supporting retries in dynamic environments.

[0128] In step S21, it is determined whether the path is empty.

[0129] If the path is empty (yes in step S21), proceed to step S211. path = This indicates that there is no feasible path under the current constraints.

[0130] In step S211, after waiting for a fixed time interval T, the process proceeds to step S0 (more precisely, step S01) to reinitialize the data and continue path planning in the next round. Thus, by delaying the restart of the algorithm after completion, local conflicts can be mitigated, and unnecessary frequent route changes can be reduced. Furthermore, in this embodiment, the time interval T can be preset or dynamically adjusted based on real-time system data. For example, if the surrounding environment changes frequently (e.g., fast cargo turnover), the time interval T can be set to a smaller value to quickly respond to environmental changes and improve real-time performance. For example, if the surrounding environment is crowded (e.g., with many robots working), the time interval T can be set to a medium value to allow other robots sufficient time to clear space. For example, if the surrounding environment has high safety requirements (e.g., handling fragile items), the time interval T can be set to a larger value to minimize conflicts and allow for retreats, prioritizing safety.

[0131] If the path is not empty (not empty in step S21), proceed to step S212. path ≠ This indicates that a feasible path has been found.

[0132] In step S212, the path is output.

[0133] After receiving a valid path, the robot first parses and verifies the path's validity. If the path is valid, it switches to execution mode and proceeds step by step according to the path, while simultaneously providing real-time feedback on its current location and execution status to the central control system or scheduling system for dynamic monitoring and path adjustment.

[0134] In summary, the robot path planning method according to one embodiment of this application, by incorporating mutual exclusion control and dynamic cost adjustment mechanisms for different types of robot areas into the planning stage within the A* iterative algorithm framework, can effectively avoid path changing and collision avoidance, improve operational efficiency, and achieve path planning that takes into account different types of robots while maintaining the same type of robot.

[0135] Specifically, this application avoids conflicts during the planning phase. Before expanding to a new node, it checks whether other types of robots are located in the controlled area (such as alleyways, intersections, loading and unloading areas, etc.). If different types of robots already exist in the area, the node is immediately abandoned to avoid retreat during later execution. Thus, inaccessible areas are eliminated during the planning phase, reducing conflicts and route-changing operations during the execution phase.

[0136] Furthermore, this application can dynamically adjust costs and optimize path selection. By introducing multiple additional cost factors, path planning becomes more aligned with real-world operational needs. Specifically, the turning cost is used to avoid frequent turns, prioritizing straight paths and improving smoothness and speed. The point-of-occupancy conflict cost detects anticipated congestion at the same node and automatically avoids highly congested points based on the number of other robots (crossNodeNum). The reverse edge conflict cost detects opposing travel conflicts, imposing a high penalty on reverse edges to ensure the algorithm prioritizes paths without conflicts. This improves overall operational smoothness, reduces the risk of collisions and deadlocks, and enhances safety and efficiency.

[0137] Furthermore, this application improves the overall efficiency of the system. Through real-time detection and dynamic cost calculation, multiple types of robots can collaborate in a shared space, reducing stagnation caused by path conflicts. If no feasible path is found after the algorithm finishes, a timed restart mechanism is supported to avoid frequent and ineffective route-changing attempts. This effectively improves the task completion rate.

[0138] Furthermore, this application exhibits excellent adaptability. The additional costs in the algorithm are flexibly adjustable, supporting not only different scenarios (such as narrow alleyways and wide open spaces) but also various types of robots, allowing for customized settings.

[0139] For those skilled in the art, any process or method description in the flowchart or otherwise described herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order according to the functions involved, as should be understood by those skilled in the art to which the embodiments of this application pertain.

[0140] Furthermore, those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this application.

[0141] Furthermore, this application also provides a computer-readable storage medium storing instructions that, when executed on a terminal device, cause the terminal device to perform the planning method described above. It is understood that a computer-readable storage medium may include: any entity or device capable of carrying a computer program, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), and a software distribution medium, etc. A computer program includes computer program code. Computer program code may be in the form of source code, object code, an executable file, or some intermediate form, etc. A computer-readable storage medium may include: any entity or device capable of carrying computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), and a software distribution medium, etc.

[0142] Furthermore, the present invention also provides a computer program product comprising computer instructions stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the methods provided in various optional implementations of the above embodiments.

[0143] The above detailed embodiments further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above are merely one specific embodiment of this application and are not limited to the scope of protection of this application. This application can be embodied in various forms without departing from its fundamental characteristics. Therefore, the embodiments described in this application are for illustrative purposes only and not for limitation. Since the scope of this application is defined by the claims rather than the description, and all variations falling within the scope defined by the claims, or their equivalents, should be understood to be included in the claims. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A robot path planning method, characterized in that, The method is executed within the iterative framework of the A* algorithm, including: initializing the search environment given the start and end points; cyclically searching for nodes in the search environment until the search ends; and backtracking to generate paths for nodes or restarting the search. The search environment includes at least: a set to be explored, which stores all discovered but not fully expanded nodes; an explored set, which stores nodes that have been fully expanded; and a set of control regions, which consists of one or more control regions. During the iterative search for nodes, conflict detection is performed on selected nodes, and the total cost of the nodes is dynamically adjusted according to preset conflict handling rules, thereby adjusting the path. The conflict detection includes at least regional conflict detection, which checks whether the area where the robot is located is a control area. The conflict handling rules include at least preventing different types of robots from occupying the same control area simultaneously. The process of searching for nodes in a loop includes: Select the node n with the minimum total cost from the set to be explored; Traverse all neighboring nodes m of node n, and perform the aforementioned region conflict detection on neighboring nodes m; Based on the results of the regional conflict detection, calculate the total cost of neighboring node m; and Based on the total cost, update the neighboring node m. When the result of the regional conflict detection is that the neighboring node m is not in the control region set, or the neighboring node m is in the control region set but there are no other types of robots in the control region set, calculate the total cost of the neighboring node m. When the result of the regional conflict detection is that the neighboring node m is in the control area set and there are other types of robots in the control area set, the neighboring node m is discarded.

2. The robot path planning method according to claim 1, characterized in that, Initialize the search environment, including: Initialize the set to be explored, the set already explored, and the set of controlled regions; and Add the starting point to the set to be explored, and calculate the total cost, actual cost, and heuristic cost of the starting point.

3. The robot path planning method according to claim 1, characterized in that, The search ends when the set to be explored is empty.

4. The robot path planning method according to claim 1, characterized in that, When node n is the endpoint, the search ends by tracing back the path through the parent and child nodes.

5. The robot path planning method according to claim 1, characterized in that, When calculating the total cost of neighboring node m, the total cost consists of three parts: Actual cost, which represents the cumulative path cost from the starting point to the neighboring node m; Heuristic cost, which represents the estimated cost from node n to neighboring node m; and The adjustment cost is dynamically adjusted based on the results of collision detection of neighboring nodes m.

6. The robot path planning method according to claim 5, characterized in that, The collision detection includes at least: Steering detection used to detect whether a path is turning; Used for detecting point-occupancy conflict detection when multiple robots are competing for the same location; and Used for detecting reverse edge conflicts when multiple robots are traveling in opposite directions; The conflict handling rules include at least the adjustment cost of adding conflict avoidance to the neighboring node m.

7. The robot path planning method according to claim 6, characterized in that, The adjustment costs include at least: The turning cost used to represent the penalty for turning in a path; The point capture conflict cost is used to represent the penalty for multiple robots competing for the same point; and The reverse edge conflict cost is used to represent the penalty for multiple robots traveling in opposite directions.

8. The robot path planning method according to claim 7, characterized in that, In the steering detection, it is detected whether a change of direction occurs from node n to neighboring node m. If a steering is required between node n and neighboring node m, the steering cost is added to the adjustment cost. The turning cost is 3 × dist(n, m), where dist(n, m) represents the basic cost between node n and its neighboring node m.

9. The robot path planning method according to claim 7, characterized in that, If, in the occupation conflict detection, the number of other robots that have already occupied or plan to occupy a neighboring node m within a specified time is detected, and if the number of other robots detected is crossNodeNum, then the occupation conflict cost is increased on the adjustment cost. The cost of the point capture conflict is log(∑crossNodeNum + 1), where crossNodeNum > 0.

10. The robot path planning method according to claim 9, characterized in that, When the cost of the occupation point conflict exceeds a preset threshold, the neighboring node m is discarded to avoid entering the high congestion area.

11. The robot path planning method according to claim 7, characterized in that, If, in the reverse edge conflict detection, the number of other robots whose travel paths within a specified time include a reverse edge [m, n] opposite to the current edge [n, m] is detected, and if the detected robot's value is edgeNum, then the reverse edge conflict cost is added to the adjustment cost. The reverse edge conflict cost is 10 × edgeNum × dist(n, m), where dist(n, m) represents the basic cost between node n and its neighboring node m.

12. The robot path planning method according to claim 1, characterized in that, When updating neighboring node m based on the total cost, If the neighboring node m is in the explored set and the total cost decreases, then update the parent node of the neighboring node m. If a neighboring node m is in the set to be explored, then skip neighboring node m; If the neighboring node m is not in the explored set or the unexplored set, then set the parent node of the neighboring node m to node n, and add the neighboring node m to the unexplored set.

13. The robot path planning method according to claim 1, characterized in that, After the search is complete, a valid path is generated based on the parent and child nodes. If no valid path is generated, the search will restart after a preset time interval.

14. A robot path planning system, comprising one or more memories and one or more processors coupled to the one or more memories, characterized in that, The processor is configured to perform the robot path planning method as described in any one of claims 1 to 13.

15. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the robot path planning method according to any one of claims 1 to 13.

16. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the robot path planning method according to any one of claims 1 to 14.

Images