Fast single-source routing algorithm based on bidirectional search
Patent Information
- Application Number
- CN202510658311.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-05-21
- Publication Date
- 2026-09-04
- Estimated Expiration
- 2045-05-21
AI Technical Summary
[0004]然而,上述的路径规划方式存在以下问题:在仓储环境中出库点通常是固定的,在计算AGV从取货点到出库点的路径时常常由于计算响应慢而出现与其他AGV在时空上的冲突,导致出现多余的等待或绕路操作;同时,不同AGV的速度差异未被纳入规划模型,使得动态环境适应性不足,在路径频繁调整时易引发全局调度效率下降
[0032]本发明利用双向协同搜索提升效率,通过反向Dijkstra预计算各节点到目标的最短距离,正向搜索结合实时转向代价,减少全局计算量,路径生成速度显著优于传统RRA*算法;
Smart Images

Figure CN120538557B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot path planning technology, and in particular to a fast single-source pathfinding algorithm based on bidirectional search. Background Technology
[0002] With the development of automated warehousing and logistics technology, automated guided vehicle (AGV) collaborative scheduling technology has emerged. This technology has the characteristics of realizing multi-vehicle path planning and dynamic obstacle avoidance in complex warehousing environments, which leads to scheduling schemes based on the Multi-Robot Path Planning (MAPF) framework.
[0003] In related technologies, the A* algorithm is used to plan the paths of multiple AGVs in a warehouse environment in sequence. The path of each AGV is regarded as occupied. Path conflict detection and avoidance are realized based on a static grid map, and the passage of multiple vehicles is coordinated by global path priority rules.
[0004] However, the above-mentioned path planning method has the following problems: In the warehousing environment, the outbound point is usually fixed. When calculating the path of AGV from the picking point to the outbound point, there are often conflicts with other AGVs in time and space due to slow calculation response, resulting in unnecessary waiting or detour operations. At the same time, the speed difference of different AGVs is not included in the planning model, which makes the dynamic environment unsuitable and easily leads to a decrease in global scheduling efficiency when the path is frequently adjusted. Summary of the Invention
[0005] To address the shortcomings of existing production technologies, this applicant provides a fast single-source pathfinding algorithm based on bidirectional search, which is applicable to transportation robots in fields such as warehousing and logistics. It can quickly return a high-quality path from any point to the target point and can perform rapid path planning when encountering dynamic obstacles.
[0006] The technical solution adopted in this invention is as follows: a fast single-source pathfinding algorithm based on bidirectional search, comprising the following steps:
[0007] S1. Obtain the task, read the pre-defined workspace map, and obtain the task target point;
[0008] S2. Reverse search and record nodes: Starting from the target point, use Dijkstra's algorithm to perform a reverse search, record the distance cost of the nodes traversed from the target point to the current position of the AGV, and update the distance table.
[0009] S3. Forward search for path expansion: Starting from the current position of the AGV, based on the distance cost of the reverse search and the turning cost, select the node expansion path with the minimum comprehensive cost.
[0010] S4. Handle dynamic obstacles: If a dynamic obstacle is detected during movement, update the path planning and search for feasible nodes again.
[0011] S5. Path generation: The AGV directly determines the next node and moves based on the information from the reverse search record, without having to backtrack the entire path.
[0012] In one embodiment, step S1 specifically includes:
[0013] S11. Read the node set and edge set of the map, and obtain the location information of the task target point;
[0014] S12. Constrain the movement rules of the AGV within discrete time intervals to ensure no positional conflicts and path intersections.
[0015] In one embodiment, step S2 specifically includes:
[0016] S21. Initialize the priority queue starting from the target point and perform a reverse Dijkstra search.
[0017] S22. Record the actual distance cost from all nodes to the target point and store it in the distance table.
[0018] In one embodiment, when the forward search encounters a node q for which the distance cost has not been recorded, the following operations are performed:
[0019] Starting from the nearest recorded node to q, continue executing Dijkstra's algorithm until the cost of q is calculated.
[0020] In one embodiment, the formula for calculating the overall cost in step S3 is:
[0021] Cost = min{Cost_d + Cost_W};
[0022] Where Cost_d is the distance cost of the reverse search record, and Cost_W is the AGV turning cost.
[0023] In one embodiment, the dynamic obstacle handling in step S4 includes:
[0024] S41. When the next node is occupied, quickly evaluate the surrounding nodes based on the existing distance table and select the node with the lowest overall cost as the alternative path.
[0025] In one embodiment, the path generation in step S5 is implemented as follows:
[0026] After each AGV moves to a new node, it immediately determines the next node based on the updated distance table and continues to execute until it reaches the target point.
[0027] In one embodiment, the algorithm updates the distance table in real time in a dynamic environment, specifically including:
[0028] When a path blockage or new obstacle is detected, a reverse search is retried to update the distance cost of the affected nodes.
[0029] In one embodiment, the steering cost Cost_W is calculated based on the difference in steering angle and speed of the AGV, and paths with steering angles less than a preset threshold are preferentially selected.
[0030] In one embodiment, the algorithm uses a combination of reverse search and forward search. The reverse search updates the local node cost as needed, while the forward search dynamically adjusts the path based on real-time information to reduce the global computational load.
[0031] The beneficial effects of this invention are as follows:
[0032] This invention improves efficiency by utilizing bidirectional collaborative search. It pre-calculates the shortest distance from each node to the target through reverse Dijkstra's algorithm, and combines forward search with real-time turning costs to reduce the global computational load. The path generation speed is significantly better than the traditional RRA* algorithm.
[0033] This invention also has the following advantages:
[0034] (1) The present invention has strong adaptability to dynamic environment. When encountering obstacles, it quickly replans locally based on the existing distance table, only updating the affected area, avoiding global path backtracking, and shortening the response time.
[0035] (2) The present invention optimizes the smooth path by introducing Cost_W, which is the difference between steering angle and speed. It prioritizes nodes with small steering angles, reduces the turning frequency of AGV, and improves the smoothness of the path.
[0036] (3) The present invention determines the next node directly based on the distance table during AGV movement, without waiting for the complete path to be generated, thus reducing task processing delay;
[0037] (4) This invention ensures no positional conflicts by constraining AGV movement rules, and reduces the deadlock rate by combining dynamic obstacle updates. Attached Figure Description
[0038] Figure 1 This is a schematic diagram of the process of the present invention.
[0039] Figure 2 This invention provides a comparison between a high-quality path graph generated by the present invention and an existing low-quality path graph; including a display of the high-quality path graph generated by the present invention (right) and a display of the existing low-quality path graph (left).
[0040] Figure 3 The image shows the solution result of the algorithm of this invention under a map model (48×48).
[0041] Figure 4 The image shows the solution result of the algorithm of this invention under another map model (194×194).
[0042] Figure 5 This is a schematic diagram illustrating how the present invention solves an example problem in one specific embodiment. Detailed Implementation
[0043] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. In the description of the present invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "clockwise," "counterclockwise," "axial," "radial," and "circumferential," etc., indicating orientation or positional relationships, are based on the orientation or positional relationships shown in the accompanying drawings and are only for the convenience of describing the present invention and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.
[0044] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.
[0045] In this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components, unless otherwise explicitly limited. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.
[0046] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can mean that the first feature is in direct contact with the second feature, or that the first feature is in indirect contact with the second feature through an intermediate medium. Furthermore, "above," "over," and "on top" of the second feature can mean that the first feature is directly above or diagonally above the second feature, or simply that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "beneath" of the second feature can mean that the first feature is directly below or diagonally below the second feature, or simply that the first feature is at a lower horizontal level than the second feature.
[0047] It should be noted that when an element is referred to as being "fixed to" or "set on" another element, it can be directly on the other element or there may be an intervening element. When an element is considered to be "connected to" another element, it can be directly connected to the other element or there may be an intervening element. The terms "vertical," "horizontal," "upper," "lower," "left," "right," and similar expressions used herein are for illustrative purposes only and do not represent the only possible implementation.
[0048] like Figures 1 to 5 The diagram illustrates a process schematic of a fast single-source pathfinding algorithm based on bidirectional search in one embodiment of this application.
[0049] This application proposes a fast single-source pathfinding algorithm for bidirectional search, comprising the following steps:
[0050] S1. Obtain Task: Read the pre-defined workspace map and obtain the task, which includes the target point that the AGV needs to reach;
[0051] S2. Reverse search and record nodes: Starting from the target point, use Dijkstra's algorithm to perform a reverse search, record the nodes passed from the target point to the current position of the AGV, add them to the Openlist according to the cost (distance), and update the distance table to record the distance of each node to the target point;
[0052] S3. Forward search path expansion: Starting from the current position of the AGV, based on the record information of the reverse search, select the node with the lowest cost from the Openlist for expansion, and quickly build a feasible path to the target point.
[0053] S4. Handling Dynamic Obstacles: If the AGV encounters a dynamic obstacle (such as another vehicle) during its movement, that location will be considered an obstacle, and the path planning will be updated. At this point, the search will begin again from the surrounding feasible nodes, and the new path will be adjusted.
[0054] S5. Path Generation: Path generation does not require waiting for the AGV to reach the target point before backtracking. By reverse-searching the recorded information, the AGV can instantly determine the next node and continue traveling along the path;
[0055] In this embodiment, step S1 further includes:
[0056] S11. Read map information, record information of all nodes and edges, and obtain the location of the task target point;
[0057] S12. In G = (V, E), where V represents the set of all vertices in the graph and E represents the set of all edges in the graph. Assume P... i (t) represents the position of the i-th automated guided vehicle at time t. In any discrete time interval t, the movement of all automated guided vehicles must adhere to the following two constraints:
[0058] First, for any two automated guided vehicles i and j (i ≠ j), if P i If (t) = v, then P must exist. j (t)≠v
[0059] Second, for any two automated guided vehicles i and j (i ≠ j), if P i (t) = v and P i If (t+1) = v', then P must exist. j (t+1)≠v'.
[0060] In this embodiment, step S2 further includes:
[0061] S21. Initialize Openlist, starting from the target point and using the AGV's position as the target point, perform a reverse search using Dijkstra's algorithm.
[0062] S22. During the search, record the actual distance cost from all nodes to the target point and record it.
[0063] In this embodiment, step S3 further includes:
[0064] S31. Start the search from the current position of the AGV to the target point. Use the distance cost Cost_d recorded during the reverse search, and consider the AGV turning cost Cost_W. Select the node with the lowest cost Cost = min{Cost_d + Cost_W} for movement.
[0065] S32. When starting the search from the current position of the AGV to the target point, if a node q that was not recorded in the reverse search is encountered, the Dijkstra algorithm is continued from the node closest to q based on the information recorded during the reverse search, until the cost of q is recorded.
[0066] In this embodiment, step S4 further includes:
[0067] S41. When the next node that the AGV is about to move to is occupied by a dynamic obstacle or another AGV, quickly evaluate other nodes based on the existing information and select the node with the lowest cost to move to.
[0068] In this embodiment, step S5 further includes:
[0069] S51. Path generation does not require backtracking. The AGV moves directly to the next node with the lowest overall cost, saving unnecessary backtracking operations.
[0070] The basic steps of the algorithm in this application are shown in Table 1, which illustrates the algorithm flow.
[0071] Table 1 Algorithm Flow
[0072]
[0073] Table 1 above shows the proposed algorithm flow. The algorithm flow realizes the rapid planning of AGV paths in a dynamic environment through forward search and backward search strategies. Specifically, it includes two stages: forward search and backward search, which are responsible for path expansion and cost update, respectively.
[0074] by Figure 5 For example, Figure 5 This illustration shows a scenario diagram of an example problem in a specific embodiment of the present application, specifically including:
[0075] Read the map, obtain information about nodes and edges, determine the task objective, and begin planning using the algorithm.
[0076] Please continue reading. Figure 1 The flowchart illustrating the algorithm's execution is shown. Specifically, it includes:
[0077] Forward search phase:
[0078] First, the algorithm starts from the AGV's current position and traverses all its adjacent nodes to find expandable paths. For each neighbor, it checks if the distance cost has been calculated. If the distance cost of a neighbor has not been calculated, the algorithm performs a reverse search phase to obtain the cost information of that node. If the distance cost of a neighbor has not been calculated, a reverse search phase is performed to calculate the shortest path and cost from that point to the target point. By comprehensively evaluating the costs of neighboring nodes, the neighbor with the minimum combined distance cost and turning cost is selected as the next target node. Once the neighbor with the minimum cost (denoted as u) is selected, the AGV moves towards that point (s = u).
[0079] Reverse search phase:
[0080] The purpose of reverse search is to calculate the distance cost of all nodes traversed from the target point to the current position of the AGV. Reverse search is performed using Dijkstra's algorithm. For each node, its distance to the target point is initialized to infinity (INF), and an empty priority queue Q is created to handle node expansion. The starting node cost is set to 0, and the queue is traversed while updating the node distances.
[0081] When priority queue Q is not empty, perform the following steps:
[0082] Find the node with the minimum cost in Q: Take the node v with the minimum cost from the queue and mark it as the current node;
[0083] Traverse neighbor nodes vs and update costs: Traverse all neighbor nodes vs of the current node v and calculate their update costs. The calculation formula is: alt = dist[v] + weight(vs,v) where weight(vs,v) represents the weight (cost) of the edge from node v to vs;
[0084] Check and update the distance: If the cost alt of reaching node vs through the current node v is less than the previously recorded dist[vs], then update dist[vs] to alt and add vs to the priority queue Q;
[0085] Repeat the above steps until the current location of the AGV is found. At this point, record all distance costs along the current path.
[0086] Figure 2 The calculated path diagram is shown, including:
[0087] When obtaining a path through forward search, not only the distance cost but also the AGV turning cost must be considered, as turning takes time during actual AGV movement. Therefore, this cost is factored in during movement to minimize AGV turning operations and make the movement as smooth as possible, thereby improving efficiency.
[0088] refer to Figures 3-4 The experimental results, shown in the figure, are compared with the RRA* algorithm, including:
[0089] The experiment used two maps of different sizes for testing. Figure 1 It contains 2304 points, and the ground Figure 2 It contains 13214 points.
[0090] Experiments were conducted on multiple sets of tasks from arbitrary starting points to target points. The results show that the method of this invention outperforms the RRA* algorithm in terms of execution speed. Notably, the advantage of the method of this invention over the RRA* algorithm becomes increasingly significant as the number of tasks increases.
[0091] In summary, this application employs a lazy backward search to calculate the distance cost of nodes to be moved on demand, and then uses a forward search combining distance and turning costs to guide the AGV to always move towards the node with the lowest cost. During path generation, unnecessary backtracking operations are avoided; once the AGV selects the next feasible node with the lowest cost, it immediately begins moving. This method efficiently returns a path from any point to the target point, making it particularly suitable for environments with dynamic obstacles and scenarios with fixed outbound points in warehouse environments.
[0092] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0093] The embodiments described above are merely illustrative of implementation methods of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.
Claims
1. A fast single-source pathfinding algorithm based on bidirectional search, characterized in that, Includes the following steps: S1. Obtain the task, read the pre-defined workspace map, and obtain the task target point; S2. Reverse search and record nodes: Starting from the target point, use Dijkstra's algorithm to perform a reverse search, record the distance cost of the nodes traversed from the target point to the current position of the AGV, and update the distance table. Initialize a priority queue starting from the target point, perform a reverse Dijkstra search, record the actual distance cost from all nodes to the target point, and store it in a distance table; When the forward search encounters a node q whose distance cost is not recorded, start from the nearest recorded node q and continue executing Dijkstra's algorithm until the cost of q is calculated; S3. Forward search for path expansion: Starting from the current position of the AGV, based on the distance cost of the reverse search and the turning cost, select the node expansion path with the minimum overall cost; the formula for calculating the overall cost is: Cost = min{Cost_d + Cost_W}; Where Cost_d is the distance cost of the reverse search record, and Cost_W is the AGV turning cost; S4. Handle dynamic obstacles: If a dynamic obstacle is detected during movement, update the path planning and search for feasible nodes again. S5. Path generation: The AGV directly determines the next node and moves based on the information in the reverse search record, without having to backtrack the entire path. After the AGV moves to a new node, it immediately determines the next node based on the updated distance table and continues to execute until it reaches the target point.
2. The fast single-source pathfinding algorithm based on bidirectional search according to claim 1, characterized in that, Step S1 specifically includes: S11. Read the node set and edge set of the map, and obtain the location information of the task target point; S12. Constrain the movement rules of the AGV within discrete time intervals to ensure no positional conflicts and path intersections.
3. The fast single-source pathfinding algorithm based on bidirectional search according to claim 1, characterized in that, The dynamic obstacle handling in step S4 includes: S41. When the next node is occupied, quickly evaluate the surrounding nodes based on the existing distance table and select the node with the lowest overall cost as the alternative path.
4. The fast single-source pathfinding algorithm based on bidirectional search according to claim 1, characterized in that, The algorithm updates the distance table in real time in a dynamic environment, specifically including: When a path blockage or new obstacle is detected, a reverse search is retried to update the distance cost of the affected nodes.
5. The fast single-source pathfinding algorithm based on bidirectional search according to claim 1, characterized in that, The calculation of the turning cost Cost_W is based on the difference in the turning angle and speed of the AGV, and prioritizes the selection of paths with a turning angle less than a preset threshold.
6. The fast single-source pathfinding algorithm based on bidirectional search according to claim 1, characterized in that, The algorithm works in tandem with reverse search and forward search. Reverse search updates the cost of local nodes as needed, while forward search dynamically adjusts the path based on real-time information to reduce the global computational load.
Citation Information
Patent Citations
AGV (Automated Guide Vehicle) path planning method based on minimum turning cost
CN109542098A
Factory AGV path planning method based on dijkstra algorithm
CN115712289A
Workshop AGV path planning method and system
CN117889860A
