Improved path planning method based on raster map fusion cost function

Abstract

The invention discloses an improved path planning method based on a raster map fusion cost function, and belongs to the technical field of path planning. Comprising the following steps: establishing a topological model of a to-be-planned area by using a grid method, dividing grid nodes into a main track and a sub-track, and mapping the state of each grid node to the topological model; establishing a node cost function for each grid node based on the main track and the sub-tracks; based on a Dijkstra algorithm, adding a node cost function in the weight to form an improved Dijkstra algorithm; according to the starting point and the ending point of the to-be-planned path, a planned path is obtained based on an improved Dijkstra algorithm; and maintaining a time window for each path in the preceding path, checking whether the planned path and the preceding path have an intersection or not, and if so, carrying out path planning again until no intersection exists. By optimizing the weight design of the path nodes, the sub-tracks are preferentially selected for passing, and the complexity of path planning is reduced.

Description

Technical Field

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

[0002] The dense storage vertical warehouse is the core component of the modern intelligent warehousing system. Its development is accompanied by the continuous improvement of logistics and supply chain management's demand for efficient storage and rapid response. The traditional warehousing model has problems such as low space utilization, high labor management costs and low cargo dispatching efficiency. The dense storage vertical warehouse effectively solves these pain points through three-dimensional design and intelligent dispatching.

[0003] With the rapid development of automation technology, the Internet of Things, and artificial intelligence, intensive warehouses are gradually transforming from traditional static storage to dynamic intelligent management. Its characteristics include high utilization of vertical space, multi-vehicle collaborative operations, and flexible response to diverse logistics needs. In terms of hardware, the application of advanced equipment such as four-way vehicles, multi-layer shuttle vehicles, and stackers has significantly improved the efficiency of cargo storage and retrieval; in terms of software, the optimization of path planning algorithms and multi-vehicle scheduling strategies has further promoted the level of system intelligence.

[0004] The development of dense storage vertical warehouses not only improves the utilization rate of storage space, but also effectively reduces manual intervention and improves the stability and reliability of system operation. It has shown great application potential, especially in e-commerce, workshops, factory logistics and other fields that require high access efficiency and accuracy. However, the path planning algorithm of dense storage vertical warehouses also has some defects, which limit its application efficiency and flexibility in complex environments. The main defects include the following aspects:

[0005] First, the efficiency of path planning is low. Traditional path planning algorithms, especially when faced with multi-vehicle collaborative operations, often have difficulty in taking into account both global optimization and real-time response. Although some algorithms can work effectively in small-scale warehousing systems, they often require a long calculation time when faced with large-scale, multi-vehicle and complex paths, resulting in a slow response speed for path planning, affecting the overall scheduling efficiency of the system.

[0006] Second, path conflicts and blockages. In dense warehouses, vehicle path conflicts and blockages often occur, especially when multiple vehicles are working together. Path overlaps, traffic jams, or time window conflicts may occur between different vehicles. Traditional path planning methods cannot take into account the scheduling and coordination of multiple workshops in real time, resulting in unsatisfactory path planning results, and even multiple vehicles occupying the same track at the same time, affecting work efficiency and safety.

[0007] Third, the adaptability to dynamically changing environments is poor. The dense warehouse environment is often dynamically changing, including factors such as cargo handling needs, equipment failures, and changes in cargo location. These factors will affect the effectiveness of the original path planning, making it difficult for traditional path planning methods to adapt to the real-time changing warehouse environment. Existing algorithms usually perform path planning based on static warehouse layouts, 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 the scenario of multi-vehicle collaborative operation, 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, ignoring the collaboration and task allocation between vehicles, and fail to achieve the 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] In view of the defects of the prior art, the present invention proposes a path planning method based on the fusion of Dijkstra algorithm and cost function. By assigning weights to each path node, paths with higher traffic efficiency are preferentially selected to reduce path overlap and vehicle conflicts, thereby achieving global optimal path planning.

[0010] In order to achieve the above object, the present 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 topological 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 to the topological model;

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

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

[0014] (4) According to the starting point and the end point of the path to be planned, the planned path is obtained based on step (3);

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

[0016] Furthermore, the main track is a channel that runs transversely through the area to be path planned, including a plurality of grid nodes, allowing transverse passage, and allowing turning in four directions at each node;

[0017] All grid nodes except the main track in the topological model are sub-tracks. The sub-tracks only allow longitudinal passage and cannot change lanes. The sub-tracks can be used for occupation and longitudinal passage.

[0018] Furthermore, the step (1) is specifically as follows: the physical space of the area to be path planned is divided into units of fixed size, each unit corresponds to a grid node, and each grid node is marked as a main track or a sub-track, as well as the grid node status.

[0019] Furthermore, the states of the grid nodes include idle and occupied.

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

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

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

[0023] Cost(x0, y0) = max_weight

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

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

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

[0027] Among them: Cost(x0, y0) is the cost value of the main track grid node; Cost(x, y) is the cost value of the sub-track grid node; step is the number of steps from the main track to the sub-track, indicating the Manhattan distance from the sub-track grid node to the main track grid node; unit_weight is the cost value reduced for each step unit;

[0028] (2.2) Modify the cost value according to the grid node status;

[0029] When the grid node state is occupied, its cost value is ∞, and if the state is restored to idle, the initial value is restored;

[0030] (2.3) Correct the grid node cost value passed by the path in the previous process;

[0031] When there is a previous travel path, for the grid node that the previous travel path is passing through, the cost value of the grid node is increased to the set value Cost0;

[0032] For the grid nodes passed by the previous task, the cost value is calculated according to the time of leaving the grid node:

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

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

[0035] If the calculated cost value is higher than its initial value, the calculated cost value is used and updated 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 longer updated.

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

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

[0038] Wherein: k1 and k2 are weighted 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 the Dijkstra algorithm; new_w(u, v) is the weight from grid node u to v in the improved Dijkstra algorithm.

[0039] Furthermore, the weighting coefficient is determined by an entropy method;

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

[0041] Normalizing the matrix to eliminate the influence of the dimension and the range of index values;

[0042]

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

[0044]

[0045] Where: p ij is the normalized weight ratio;

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

[0047]

[0048] Furthermore, the step (3) is specifically as follows:

[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 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 by backtracking.

[0055] Further, the specific content of step (4) is 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 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 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;

[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] Beneficial effects of the present invention:

[0061] 1. Improve path planning efficiency: By optimizing the weight design of path nodes, sub-tracks are given priority to reduce the complexity of path planning.

[0062] 2. Reduce path conflicts: By accurately calculating the time window, path conflicts can be discovered and resolved in a timely manner, avoiding blockages in warehousing operations.

[0063] 3. Improve system scheduling capabilities: The entropy method is used to allocate scheme weights, which can effectively evaluate the pros and cons of different path planning, thereby achieving global optimal scheduling. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 The figure is a flow chart of an improved path planning method based on a grid map fusion cost function according to an embodiment of the present invention.

[0065] Figure 2 Schematic diagram of the path planning algorithm structure of an embodiment of the present invention.

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

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

[0068] Figure 5 This is a distribution diagram of the initial grid map node cost values ​​of an embodiment of the present invention.

[0069] Figure 6 This is a distribution diagram of node cost values ​​of a dynamic grid map according to an embodiment of the present invention. DETAILED DESCRIPTION

[0070] The specific implementation modes of the present invention are further described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0071] like Figure 3 As shown, the present invention takes the path planning of a four-way vehicle in a dense warehouse as an example. The warehouse is equipped with equipment such as a hoist, a forklift, and a four-way vehicle. The forklift is used to transport goods to the entrance of the warehouse, the hoist is used to switch between different floors, and the four-way vehicle is used to transport goods on the warehouse. The embodiment of the present invention only considers path planning on a single floor, so the switching between different floors is ignored.

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

[0073] S101, using a grid method to establish a topological model of the area to be planned, dividing grid nodes into a main track and a sub-track, and mapping the state of each grid node to the topological model.

[0074] like Figure 4 As shown in the figure, dense storage vertical warehouses are usually divided into fixed-size units, which are composed of main tracks and sub-tracks. Their layout presents a regular grid topology. The main track is a channel that runs horizontally through the vertical warehouse and contains multiple grid nodes. The main track is only used for horizontal transportation and can turn in four directions. The edge can be used as a storage point. All grid nodes except the main track are sub-tracks. The sub-tracks only allow longitudinal movement and cannot change lanes. At the same time, the sub-tracks can be used as cargo spaces.

[0075] The vertical warehouse in the embodiment of the present invention has two main tracks.

[0076] After establishing the topological model, the occupancy status of each grid node needs to be mapped to the grid map, such as idle or occupied.

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

[0078] The introduction of node cost values ​​can help divert vehicles and achieve load balancing. Here, we need to define cost values ​​based on the main track and sub-track types; we also consider four-way vehicles and goods as mobile cost values ​​to make the global cost map dynamic; at the same time, we also need to consider that the four-way shuttle vehicle travels on the track and leaves pheromones on the path it passes through to form a negative feedback mechanism, which can guide the diversion of vehicles while ensuring a safe distance from other vehicles.

[0079] The specific steps are as follows:

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

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

[0082] Cost(x0, y0) = max_weight

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

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

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

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

[0087] Among them: Cost(x0, y0) is the cost value of the main track grid node; Cost(x, y) is the cost value of the sub-track grid node; step is the number of steps from the main track to the sub-track, which represents the Manhattan distance from the sub-track grid node to the main track grid node; unit_weight is the cost value reduced for each step unit.

[0088] (2) Dynamically modify the cost value according to the grid node status.

[0089] When the grid node state is occupied, its cost value is ∞, and if the state returns to idle, it returns to the initial value.

[0090] When goods are stored on a grid node, the grid node is occupied, so this position cannot be occupied by other vehicles, which means that the goods on the cargo space will become an obstacle unless the goods are taken away or moved.

[0091] (3) Dynamically correct the grid node cost values ​​passed by the path in the previous process.

[0092] When there is a preceding travel path, the cost value of the grid node that the preceding travel path is passing through is increased to the set value Cost0.

[0093] For the grid nodes passed by the previous task, its cost value is calculated according to the time of leaving the grid node.

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

[0095] Where: α is the linear decay rate.

[0096] If the calculated cost value is higher than its initial value, the calculated cost value is used and updated 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 longer updated.

[0097] Here we mainly consider the situation of multi-vehicle collaborative operation. When planning the path, there may be other vehicles in advance. The traditional path planning method cannot take into account the scheduling and coordination of multiple workshops in real time, resulting in unsatisfactory path planning results, affecting work efficiency and safety. Therefore, when planning the path, it is necessary to track the current position of the preceding vehicle in real time to avoid passing through these positions to avoid collision or interference. When the preceding four-way vehicle enters a grid, the cost value of this position will increase, which means that other vehicles should give priority to other path points until the vehicle leaves. At the same time, the four-way shuttle will release negative feedback pheromones to mark the paths or nodes that have been traveled, so that the cost of the paths or nodes that have been traveled will increase. The intensity of the negative feedback pheromones decreases over time, forming a certain time dissipation process. The more pheromones there are, the higher the cost value, which means that the path or node is more congested.

[0098] The above factors will be combined to form a comprehensive node cost value, and dynamically adjusted through the algorithm to achieve path optimization, such as Figure 6 shown.

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

[0100] The roadbed planning algorithm of the embodiment of the present invention is based on the Dijkstra algorithm, and introduces the node cost function when calculating the weight. The specific method is as follows:

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

[0102] (2) Take the grid node u that is closest to the starting point from the set of unvisited nodes, add u to the set of visited grid nodes S, and 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 weighted coefficients; cost(v) is the cost value of grid node v; w(u, v) is the path cost weight from grid node u to v in the Dijkstra algorithm; new_w(u, v) is the weight from grid node u to v in the improved Dijkstra algorithm.

[0106] In the embodiments of the present invention, the weighting coefficients k1 and k2 can be determined by the entropy 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, and 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 each path segment between grid nodes 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 being advanced 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 re-perform path planning 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 do not limit the protection scope of the present invention. Various deformations or modifications that can be made by those of ordinary skill in the art without creative labor within the scope of the appended claims 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 steps include: (1) Use the grid method to establish a topological 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 to the topological model; (2) Establishing a node cost value for each grid node based on the status of the main track, sub-tracks, and grid nodes; (3) Based on the Dijkstra algorithm, the node cost value is added to the weight to form an improved Dijkstra algorithm; (4) According to the starting point and the end point of the path to be planned, the planned path is obtained based on step (3); (5) Maintain a time window for each path in the previous process, check whether the planned path has an intersection with the previous path, and if so, re-plan the path 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 that runs transversely through the area to be path planned, including multiple grid nodes, allowing transverse passage, and allowing turning in four directions at each node; All grid nodes except the main track in the topological model are sub-tracks. The sub-tracks only allow longitudinal passage and cannot change lanes. The sub-tracks can be used for occupation and longitudinal passage.

3. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that: The step (1) specifically comprises: dividing the physical space of the area to be path planned into units of fixed size, each unit corresponding to a grid node, and marking each grid node as a main track or a sub-track, as well as the grid node status.

4. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that: The states of the grid nodes include idle and occupied.

5. The improved path planning method based on grid map fusion cost function according to claim 4 is characterized in that: The node cost function in step (2) is: (2.1) Initialize the cost value of each grid node; Assign the maximum weight to each grid node on the main track, and the cost value of the main track is: Cost(x0, y0) = max_weight The cost value of the grid node on the sub-track decreases linearly as the distance from the main track increases; Cost(x,y)=Cost(x0,y0)-step×unit_weight step=|x-x0|+|y-y0| Among them: Cost(x0, y0) is the cost value of the main track grid node; Cost(x, y) is the cost value of the sub-track grid node; step is the number of steps from the main track to the sub-track, indicating the Manhattan distance from the sub-track grid node to the main track grid node; unit_weight is the cost value reduced for each step unit; (2.2) Modify the cost value according to the grid node status; When the grid node state is occupied, its cost value is ∞, and if the state is restored to idle, the initial value is restored; (2.3) Correct the grid node cost value passed by the path in the previous process; When there is a previous travel path, for the grid node that the previous travel path is passing through, the cost value of the grid node is increased to the set value Cost0; For the grid nodes passed by the previous task, the cost value is calculated according to the time of leaving the grid node: Cost(t)=Cost0-αt Where: α is the linear decay rate; If the calculated cost value is higher than its initial value, the calculated cost value is used and updated 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 longer updated.

6. The improved path planning method based on grid map fusion cost function according to claim 1 is characterized in that: The weight in the improved Dijkstra algorithm is as follows: new_w(u, v) = k1 × w(u, v) + k2 × cost(v) 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 the Dijkstra algorithm; new_w(u, v) is the weight from grid node u to v in the improved Dijkstra algorithm.

7. The improved path planning method based on grid map fusion cost function according to claim 6 is characterized in that: The weighting coefficients are determined by the entropy method; Collect the path costs and node costs of all nodes to construct an n*m-dimensional matrix X; Perform normalization processing on the matrix to eliminate the influence of dimension and index value range; Calculate the information entropy E for each indicator j J ; Where: p ij is the normalized weight ratio; Calculate the importance weight k of each indicator based on information entropy j :

8. The improved path planning method based on grid map fusion cost function according to claim 6 is characterized in that: The specific content of step (3) is as follows: (3.1) Set the distance of the path starting point s to 0, that is, dist[s] = 0, initialize the distances of all other grid nodes to ∞, and add the starting point s to the set S of visited nodes, and add the remaining grid nodes to the set T of unvisited nodes; (3.2) Take out the grid node u closest to 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: new_dist[v] = dist[u] + new-w(u, v) If new_dist[v] < dist[v], 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 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.

9. The improved path planning method based on grid map fusion cost function according to claim 1, characterized in that: The specific content of step (4) is as follows: (4.1) Obtain the planned path based on step (3) according to the starting point and ending point of the path to be planned; (4.2) Maintain a time window [start_time, end_time] for each path segment between grid nodes in each path that is in progress, indicating the start time and end time of passing through this path segment; (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 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; (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.

Images