An improved path planning method based on grid map fusion cost function

Abstract

The application discloses an improved path planning method based on a grid map fusion cost function and belongs to the technical field of path planning. The method comprises the following steps: a topology model of a region to be planned is established by using a grid method, grid nodes are divided into main tracks and sub-tracks, and the state of each grid node is mapped to the topology model; a node cost function is established for each grid node based on the main tracks and the sub-tracks; an improved Dijkstra algorithm is formed by adding the node cost function in the weight based on the Dijkstra algorithm; a planning path is obtained based on the improved Dijkstra algorithm according to the starting point and the ending point of the path to be planned; a time window is maintained for each path in the prior progress, and it is checked whether the planning path intersects with the prior path, and if there is an intersection, path planning is performed again until there is no intersection. The weight of the path node is optimized, the sub-track is preferentially selected, and the complexity of path planning is reduced.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to an improved path planning method based on a grid map fusion cost function. Background Technology

[0002] Automated storage and retrieval systems (AS / RS) are a core component of modern intelligent warehousing systems, and their development is driven by the ever-increasing demands for efficient storage and rapid response in logistics and supply chain management. Traditional warehousing models suffer from low space utilization, high manual management costs, and low cargo scheduling efficiency, while AS / RS effectively solves these pain points through three-dimensional design and intelligent scheduling.

[0003] With the rapid development of automation technology, the Internet of Things, and artificial intelligence, automated storage and retrieval systems (AS / RS) are gradually transforming from traditional static storage to dynamic intelligent management. Their characteristics include high utilization of vertical space, enabling multi-vehicle collaborative operations, and flexible response to diverse logistics needs. On the hardware side, the application of advanced equipment such as four-way vehicles, multi-level shuttles, and stacker cranes significantly improves the efficiency of goods storage and retrieval; on the software side, the optimization of path planning algorithms and multi-vehicle scheduling strategies further promotes the level of system intelligence.

[0004] The development of automated storage and retrieval systems (AS / RS) has not only improved the utilization rate of storage space but also effectively reduced manual intervention, enhancing the stability and reliability of system operation. It has demonstrated enormous application potential, particularly in e-commerce, workshops, and factory logistics, where high efficiency and accuracy in storage and retrieval are crucial. However, AS / RS path planning algorithms also have some shortcomings, which limit their efficiency and flexibility in complex environments. The main shortcomings include the following:

[0005] First, path planning is inefficient. Traditional path planning algorithms, especially when dealing with multi-vehicle collaborative operations, often struggle to balance global optimization and real-time response. While some algorithms may work effectively in small-scale warehousing systems, they often require significant computation time when dealing with large-scale, multi-vehicle, and complex paths, resulting in slow path planning response and impacting the overall scheduling efficiency of the system.

[0006] Second, path conflicts and congestion issues. In densely packed automated warehouses, path conflicts and congestion are common problems, especially in multi-vehicle collaborative operations. Different vehicles may experience path overlap, traffic congestion, or time window conflicts. Traditional path planning methods cannot take into account the real-time scheduling and coordination of multiple workshops, resulting in unsatisfactory path planning results. In some cases, multiple vehicles may occupy the same track at the same time, affecting operational efficiency and safety.

[0007] Third, it has poor adaptability to dynamically changing environments. Dense automated warehouse environments are often dynamically changing, including factors such as cargo handling needs, equipment malfunctions, and changes in cargo location. These factors affect the effectiveness of existing path planning, making traditional path planning methods difficult to adapt to real-time changes in the warehouse environment. Existing algorithms are typically based on static warehouse layouts for path planning, lacking the ability to adjust in real time and flexibly respond to dynamic changes.

[0008] Fourth, there is a lack of comprehensive scheduling optimization. In multi-vehicle collaborative operation scenarios, in addition to path planning, vehicle scheduling strategies and task allocation are also important factors affecting efficiency. Existing path planning algorithms usually focus on the path optimization of a single vehicle, neglecting the cooperation and task allocation between vehicles, thus failing to achieve an optimal solution for global scheduling. Especially in multi-vehicle collaborative operations, the mutual coordination of vehicles and the reasonable allocation of tasks are crucial to improving the overall efficiency of the system. Summary of the Invention

[0009] To address the shortcomings of existing technologies, this invention proposes a path planning method based on a fusion of Dijkstra's algorithm and a cost function. By assigning weights to each path node, paths with higher traffic efficiency are prioritized to reduce path overlap and vehicle conflicts, thereby achieving globally optimal path planning.

[0010] To achieve the above objectives, this invention provides an improved path planning method based on a grid map fusion cost function, comprising the following steps:

[0011] (1) Use the grid method to establish a topology model of the area to be planned, divide the grid nodes into main tracks and sub-tracks, and map the state of each grid node onto the topology model;

[0012] (2) Establish a node value for each grid node based on the status of the main track, sub-track and grid node;

[0013] (3) Based on Dijkstra's algorithm, the node cost is added to the weight to form an improved Dijkstra's algorithm;

[0014] (4) Based on the starting point and ending point of the path to be planned, obtain the planned path based on step (3);

[0015] (5) Maintain a time window for each path in the prior process, check whether the planned path intersects with the prior path, and if there is an intersection, re-plan the path until there is no intersection.

[0016] Furthermore, the main track is a channel that runs horizontally through the area to be planned, containing multiple grid nodes, allowing horizontal passage, and allowing turning in four directions at each node;

[0017] In the topology model, all grid nodes except the main track are sub-tracks. The sub-tracks only allow longitudinal passage and cannot change lanes. The sub-tracks can be used for both occupation and longitudinal passage.

[0018] Furthermore, step (1) specifically involves dividing the physical space of the area to be planned into units of fixed size, with each unit corresponding to a grid node, and marking each grid node as either a main track or a sub-track, as well as the grid node status.

[0019] Furthermore, the state of the grid node includes idle and occupied.

[0020] Furthermore, the node cost function in step (2) is:

[0021] (2.1) Initialize the value of each grid node;

[0022] Assigning the maximum weight to each grid node on the main track, the cost value of the main track is:

[0023] Cost(x0, y0) = max_weight

[0024] The cost of a grid node on the sub-track decreases linearly as its distance from the main track increases;

[0025] Cost(x,y)=Cost(x0,y0)-step×unit_weight

[0026] step = |x - x0| + |y - y0|

[0027] Where: Cost(x0, y0) is the cost of the main track grid node; Cost(x, y) is the cost of the sub-track grid node; step is the number of steps from the main track to the sub-track, representing the Manhattan distance from the sub-track grid node to the main track grid node; unit_weight is the cost reduction per step unit;

[0028] (2.2) Adjust the cost value based on the grid node status;

[0029] When the grid node is in an occupied state, its cost value is ∞. If the state is restored to idle, the initial value is restored.

[0030] (2.3) Correct the value of the grid nodes traversed by the path in the preceding process;

[0031] When a preceding path exists, for a grid node that is being traversed by the preceding path, increase the value of that grid node to the set value Cost0.

[0032] For each grid node traversed by a prior task, its cost is calculated based on the time it left that grid node:

[0033] Cost(t) = Cost0 - αt

[0034] Where: α is the linear decay rate;

[0035] If the calculated cost is higher than its initial value, the calculated cost is used and updated continuously over time until it is lower than or equal to the initial value; if it is lower than or equal to the initial value, the initial value is used and no further updates are made.

[0036] Furthermore, the weights in the improved Dijkstra algorithm are:

[0037] new_w(u,v)=k1×w(u,v)+k2×cost(v)

[0038] Where: k1 and k2 are weighting coefficients; cost(v) is the cost value of the grid node v; w(u,v) is the path cost weight from grid node u to v in Dijkstra's algorithm; new_w(u,v) is the weight from grid node u to v in the improved Dijkstra's algorithm.

[0039] Furthermore, the weighting coefficients are determined using the entropy method;

[0040] Collect the path cost and node cost of all nodes, and construct an n*m dimensional matrix X;

[0041] The matrix is ​​normalized to eliminate the influence of dimensions and index value ranges;

[0042]

[0043] Calculate the information entropy E for each indicator j. J ;

[0044]

[0045] Where: p ij It is the normalized weighting percentage;

[0046] Calculate the importance weight k of each indicator based on information entropy. j :

[0047]

[0048] Furthermore, step (3) specifically involves:

[0049] (3.1) Set the distance of the path starting point s to 0, i.e., dist[s] = 0, initialize the distances of all other grid nodes to ∞, add the starting point s to the set S of visited nodes, and add all the remaining grid nodes to the set T of unvisited nodes;

[0050] (3.2) Take out the grid node u with the shortest distance from the starting point from the set of unvisited nodes, add u to the set S of visited grid nodes. If the neighbor node v of node u has not been visited, calculate the new distance from u to v:

[0051] new_dist[v] = dist[u] + new_w(u, v)

[0052] If new_dist[v] < dist[v], update dist[v] and set the predecessor of v to u;

[0053] (3.3) Repeat step (3.2) until all grid nodes are processed or the set of unvisited nodes is empty;

[0054] (3.4) After all grid nodes are processed, the shortest path lengths from the starting point s to each node are stored in dist[v]; record the predecessor nodes on the path each time dist[v] is updated, and obtain each path through backtracking.

[0055] Further, the specific steps of step (4) are as follows:

[0056] (4.1) Based on the starting point and end point of the path to be planned, obtain the planned path based on step (3);

[0057] (4.2) Maintain a time window [start_time, end_time] for each path segment between every two grid nodes in each path that is in progress, representing the start time and end time of passing through this path segment;

[0058] (4.3) When the planned path and the paths in progress contain the same path segment, detect whether their time windows overlap. The condition for the time windows to overlap is: start_time_A < end_time_B and end_time_A > start_time_B, where [start_time_A, end_time_A] and [start_time_B, end_time_B] represent the time windows when the planned path and any prior path pass through the same path segment;

[0059] (4.4) If there is an overlap, increase the weight of this path in the improved Dijkstra algorithm by a predefined cost gain factor, and return to step (3) to re - plan the path until there is no intersection.

[0060] The beneficial effects of this invention are:

[0061] 1. Improve path planning efficiency: By optimizing the weight design of path nodes, priority is given to selecting sub-tracks for passage, thereby reducing the complexity of path planning.

[0062] 2. Reduce path conflicts: By accurately calculating time windows, path conflicts can be detected and resolved in a timely manner, avoiding congestion in warehousing operations.

[0063] 3. Enhance system scheduling capabilities: By using the entropy method to allocate scheme weights, the merits of different path plans can be effectively evaluated, thereby achieving global optimal scheduling. Attached Figure Description

[0064] Figure 1 This is a schematic diagram of the improved path planning method based on the grid map fusion cost function according to an embodiment of the present invention.

[0065] Figure 2 This is a schematic diagram of the path planning algorithm structure according to an embodiment of the present invention.

[0066] Figure 3 This is a schematic diagram of a dense storage vertical warehouse according to an embodiment of the present invention.

[0067] Figure 4 This is a schematic diagram of a grid map according to an embodiment of the present invention.

[0068] Figure 5 This is the initial grid map node value distribution map of an embodiment of the present invention.

[0069] Figure 6 This is a dynamic grid map node value distribution map according to an embodiment of the present invention. Detailed Implementation

[0070] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0071] like Figure 3 As shown, this invention uses four-way vehicle path planning in a high-density automated storage and retrieval system (AS / RS) as an example. The AS / RS is equipped with elevators, forklifts, and four-way vehicles. Forklifts are used to transport goods to the AS / RS entrance, elevators are used to switch between different levels, and four-way vehicles are used to transport goods within the AS / RS. This embodiment of the invention only considers path planning on a single level, therefore ignoring switching between different levels.

[0072] like Figure 1 , Figure 2 As shown, this embodiment of the invention provides an improved path planning method based on a grid map fusion cost function, comprising the following steps:

[0073] S101. Use the grid method to establish a topology model of the area to be planned, divide the grid nodes into main tracks and sub-tracks, and map the state of each grid node onto the topology model.

[0074] like Figure 4 As shown, high-density automated storage and retrieval systems (AS / RS) are typically divided into fixed-size units, consisting of intersecting main tracks and sub-tracks, forming a regular grid-like topology. The main track is a transverse channel running through the AS / RS, containing multiple grid nodes. The main track is used only for lateral transport, can turn in four directions, and its edges can serve as entry points. All grid nodes other than the main track are sub-tracks, which are only allowed to move longitudinally and cannot change lanes. Sub-tracks can also be used as storage locations.

[0075] In this embodiment of the invention, the vertical storage unit has two main tracks.

[0076] After establishing the topology model, it is necessary to map the occupancy status of each grid node onto the grid map, such as idle or occupied.

[0077] S102. Establish a node value for each grid node based on the status of the main track, sub-track, and grid node.

[0078] The introduction of node cost values ​​can help divert vehicles and achieve load balancing. Here, the cost value needs to be defined according to the main track and sub-track types; four-way vehicles and goods are also considered as moving cost values, making the global cost map dynamic; at the same time, it is also necessary to consider that the four-way shuttles leave pheromones on the path they travel on the track to form a negative feedback mechanism, which guides vehicle diversion while ensuring a safe distance from other vehicles.

[0079] The specific steps are as follows:

[0080] (1) Initialize the value of each grid node.

[0081] like Figure 5 As shown, if the maximum weight is assigned to each grid node on the main track, then the cost value of the main track is:

[0082] Cost(x0, y0) = max_weight

[0083] Where: max_weight is the predefined maximum weight value.

[0084] The cost of each grid node on the sub-track decreases linearly with increasing distance from the main track:

[0085] Cost(x,y)=Cost(x0,y0)-step×unit_weight

[0086] step = |x - x0| + |y - y0|

[0087] Where: Cost(x0, y0) is the cost of the main track grid node; Cost(x, y) is the cost of the sub-track grid node; step is the number of steps from the main track to the sub-track, representing the Manhattan distance from the sub-track grid node to the main track grid node; unit_weight is the cost reduction per step unit.

[0088] (2) Dynamically adjust the cost value based on the grid node status.

[0089] When a grid node is in an occupied state, its cost is ∞. If the state is restored to idle, the initial value is restored.

[0090] Once goods are stored on a grid node, that grid node is occupied, so this location cannot be occupied by other vehicles. This means that the goods in the storage location will become an obstacle unless the goods are removed or moved.

[0091] (3) Dynamically correct the value of the grid nodes traversed by the path in the preceding process.

[0092] When a preceding path exists, for a grid node that is being traversed by the preceding path, increase the value of that grid node to the set value Cost0.

[0093] For each grid node traversed by a prior task, its cost is calculated based on the time it takes to leave that grid node.

[0094] Cost(t) = Cost0 - αt

[0095] Where α is the linear decay rate.

[0096] If the calculated cost is higher than its initial value, the calculated cost is used and updated continuously over time until it is lower than or equal to the initial value; if it is lower than or equal to the initial value, the initial value is used and no further updates are made.

[0097] This primarily considers multi-vehicle collaborative operations. During path planning, other vehicles may be already in progress. Traditional path planning methods cannot take into account the real-time scheduling and coordination of multiple vehicles, leading to unsatisfactory path planning results and impacting operational efficiency and safety. Therefore, it is necessary to track the current position of preceding vehicles in real time during path planning to avoid passing through these locations and prevent collisions or interference. When a preceding four-way shuttle enters a grid, the cost of that location increases, meaning other vehicles should prioritize alternative path points until the shuttle leaves. Simultaneously, the four-way shuttle releases negative feedback pheromones, marking already traversed paths or nodes, increasing the cost of already traveled paths or nodes. The intensity of the negative feedback pheromones decreases over time, forming a dissipation process. More pheromones mean a higher cost, indicating greater congestion on that path or node.

[0098] These factors will combine to form a comprehensive node cost, which will be dynamically adjusted through an algorithm to optimize the path, such as... Figure 6 As shown.

[0099] S103. Based on Dijkstra's algorithm, a node cost function is added to the weights to form an improved Dijkstra's algorithm.

[0100] The roadbed planning algorithm in this embodiment of the invention is based on Dijkstra's algorithm, and introduces a node cost function during weight calculation. Specifically:

[0101] (1) Set the distance of the path starting point s to 0, i.e. dist[s] = 0, initialize the distance of all other grid nodes to ∞, add the starting point s to the set of visited nodes S, and add the remaining grid nodes to the set of unvisited nodes T.

[0102] (2) Take out the grid node u that is closest to the starting point from the unvisited node set and add u to the visited grid node set S. If the neighbor node v of node u has not been visited, calculate the new distance from u to v.

[0103] new_dist[v]=dist[u]+new_w(u,v)

[0104] new_w(u,v)=k1×w(u,v)+k2×cost(v)

[0105] Where: k1 and k2 are weighting coefficients; cost(v) is the cost of grid node v; w(u,v) is the path cost weight from grid node u to v in Dijkstra's algorithm; new_w(u,v) is the weight from grid node u to v in the improved Dijkstra's algorithm.

[0106] In the embodiments of the present invention, the weighting coefficients k1 and k2 can be determined by the entropy value method.

[0107] Collect the path costs and node costs of all nodes to construct an n*m-dimensional matrix X.

[0108] Normalize the matrix to eliminate the influence of dimension and index value range.

[0109]

[0110] Calculate the information entropy E for each index j J ;

[0111]

[0112] where: p ij is the normalized weight ratio;

[0113] Calculate the importance weight k of each index according to the information entropy j :

[0114]

[0115] If new_dist[v]<dist[v], update dist[v] and set the predecessor of v to u.

[0116] (3) Repeat step (2) until all grid nodes are processed or the set of unvisited nodes is empty.

[0117] (4) After all grid nodes are processed, the shortest path lengths from the starting point s to each node are stored in dist[v]; record the predecessor nodes on the path each time dist[v] is updated, and obtain each path through backtracking.

[0118] S104. Based on the starting point and ending point of the path to be planned, obtain the planned path according to step S103.

[0119] S105. Maintain a time window for each path in progress, check whether there is an intersection between the planned path and the prior path. If there is an intersection, re-plan the path until there is no intersection.

[0120] (1) Maintain a time window [start_time, end_time] for the path segment between each grid node in each path in progress, indicating the start time and end time of passing through this path segment;

[0121] (2) When the planned path and the path in progress contain the same path segment, detect whether their time windows overlap. The condition for time window overlap is: start_time_A < end_time_B and end_time_A > start_time_B, where [start_time_A, end_time_A] and [start_time_B, end_time_B] represent the time windows when the planned path and any prior path pass through the same path segment.

[0122] (3) If there is an overlap, increase the weight of this path in the improved Dijkstra algorithm by a predefined cost gain factor, and return to step S103 to perform path planning again until there is no intersection.

[0123] new_w(u, v) = new_w(u, v) + delta

[0124] where: delta is the cost gain factor.

[0125] The above embodiments are preferred cases of the present invention and are not used to limit the protection scope of the present invention. Various modifications or changes that can be made by those of ordinary skill in the art within the scope of the appended claims without creative efforts still fall within the protection scope of this patent.

Claims

1. An improved path planning method based on grid map fusion cost function, characterized in that, The method comprises the following steps: (1) a topology model of a region to be path planned is established by using a grid method, grid nodes are divided into main tracks and sub-tracks, and a state of each grid node is mapped to the topology model; (2) a node generation value of each grid node is established based on the main tracks, the sub-tracks and the state of the grid node; the state of the grid node comprises idle and occupied; (2.1) each grid node generation value is initialized; a maximum weight is assigned to each grid node on the main tracks, and a generation value of the main track is: a generation value of a grid node on the sub-track linearly decreases with an increase of a distance from the main track; wherein: is the main track grid node generation value; is the sub-track grid node generation value; step is the number of steps from the main track to the sub-track, representing the Manhattan distance from the sub-track grid node to the main track grid node; and unit_weight is the generation value reduced per unit of step length. (2.2) the generation value is corrected according to the state of the grid node; when the state of the grid node is occupied, the generation value is ∞, and if the state returns to idle, the initial value is restored; (2.3) the generation value of a grid node passed through by a previously advanced path is corrected; when there is a previously advanced path, for a grid node being passed through by the previously advanced path, the generation value of the grid node is improved to a set value Cost0; for a grid node passed through by a previously advanced task, a generation value of the grid node is calculated according to a time of leaving the grid node: Cost(t) = Cost0 αt wherein α is a linear decay rate; if the calculated generation value is higher than the initial value, the calculated generation value is used and is constantly updated until the calculated generation value is lower than or equal to the initial value; if the calculated generation value is lower than or equal to the initial value, the initial value is used and is not updated any more; (3) an improved Dijkstra algorithm is formed by adding the node generation value to weights based on the Dijkstra algorithm; the weight in the improved Dijkstra algorithm is: wherein: with is a weighting factor; is the generation value of the grid node v; is the path cost weight of the grid node u to v in Dijkstra's algorithm; is the weight of the grid node u to v in the improved Dijkstra's algorithm; (4) a planned path is obtained based on step (3) according to a starting point and an ending point of the path to be planned; (5) a time window is maintained for each previously advanced path, and whether the planned path intersects with the previously advanced path is checked, and if there is an intersection, path planning is performed again until there is no intersection.

2. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that: the main track is a channel transversely passing through the region to be path planned, comprises a plurality of grid nodes, allows transverse passing, and allows turning in four directions at each node; all grid nodes except the main track in the topology model are sub-tracks, the sub-track only allows longitudinal passing and cannot change lanes, and the sub-track can be used for occupation and longitudinal passing.

3. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that: step (1) is specifically that a physical space of the region to be path planned is divided into units of a fixed size, each unit corresponds to a grid node, and each grid node is marked as a main track or a sub-track and a state of the grid node.

4. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that, the weighting coefficient is determined by an entropy method; Collect the path cost and node cost of all nodes, build a n*m dimensional matrix ; the matrix is normalized to eliminate influences of dimensions and index value ranges; Calculate the information entropy for each indicator j ; wherein: is the normalized weight proportion; According to the information entropy, the importance weight of each index is calculated : 。 5. The improved path planning method based on grid map fusion cost function according to claim 1, wherein, step (3) is specifically: (3.1) a distance of a path starting point s is set as 0, distances of all other grid nodes are initialized as ∞, the starting point s is added to an accessed node set S, and the remaining grid nodes are added to an unaccessed node set T; (3.2) a grid node u closest to the starting point is taken out from the unaccessed node set T, u is added to the accessed node set S, if a neighbor node v of u has not been accessed, a new distance from u to v is calculated: If , update dist[v] and set the predecessor of v to u; (3.3) repeat step (3.2) until all grid nodes are processed or the set of unvisited nodes is empty; (3.4) after all grid nodes are processed, the shortest path length from the start node s to each node is stored in dist[v]; the predecessor node on the path is recorded each time dist[v] is updated, and each path is obtained by backtracking.

6. The improved path planning method based on grid map fusion cost function according to claim 1, wherein, The step (4) is specifically: (4.1) according to the start node and the end node of the path to be planned, the planned path is obtained based on step (3); (4.2) for each path segment between each grid node in each path in prior travel, a time window [start_time, end_time] is maintained, indicating the start time and the end time of passing through the path segment; (4.3) when the planned path and the path in prior travel contain the same path segment, it is detected whether the time windows of them overlap, and the time window overlap condition is: start_time_A < end_time_B and end_time_A > start_time_B, wherein [start_time_A, end_time_A] and [start_time_B, end_time_B] represent the time windows of the planned path and any prior path when passing through the same path segment; (4.4) if there is overlap, the weight of the path in the improved Dijkstra algorithm is increased by a predefined cost gain factor, and step (3) is returned to re-plan the path until there is no intersection.

Images