Path planning method and device based on important node pre-sampling

Abstract

The invention relates to the field of path planning, in particular to a path planning method and device based on important node pre-sampling. According to the method, important nodes are locked by pre-sampling the map, and a multi-source shortest path tree among the important nodes is pre-constructed. When a path planning task is received, the initial path is generated based on the shortest path tree, the limitation of blind point scattering sampling in a super-large map by a traditional RRT algorithm is broken through, and the generation requirement of a large number of redundant nodes caused by large search space and complex obstacles is reduced, so that the path search time is shortened, and the planning efficiency is improved. According to the method, the RRT * node tree is constructed on the basis of the initial path, and the path is deeply optimized by means of node expansion, so that the finally planned path is shorter and smoother, and the path quality is improved. Finally, the technical bottleneck of path planning of a traditional RRT algorithm in complex scenes such as large factories is effectively overcome, and a powerful solution is provided for transportation requirements.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and in particular to a path planning method and device based on important node pre-sampling. Background Art

[0002] The automation level of large factories continues to improve, and automated transportation equipment such as AGVs, unmanned forklifts, and overhead cranes are widely used in material handling, product transportation, and production and processing. However, large factories (such as shipyards) cover a huge area, which can reach tens or even millions of square meters, posing a severe challenge to the path planning of automated transportation equipment. Although traditional RRT algorithms perform well in scenarios such as warehousing and logistics and intelligent services, when applied to ultra-large map environments, they are constrained by factors such as large search spaces and complex obstacles, resulting in long path search times and poor path quality. This makes it difficult to meet the efficient and accurate transportation needs of large factories, and there is an urgent need for optimization and improvement.

[0003] Specifically, in terms of path search time, due to the large factory space and complex obstacles, the RRT algorithm needs to generate a large number of nodes to find a feasible path. Each time a new node is generated, connectivity calculation and comparison with existing sampling points must be performed. The iteration time increases with the number of iterations, resulting in inefficient path planning and difficulty in adapting to the rapid response requirements of equipment in the factory. In terms of path quality, the traditional RRT algorithm expansion process lacks directional control, and the final path is prone to detours and twists, which cannot guarantee the shortest and smoothest path, increasing the energy consumption and time of equipment transportation and reducing the overall efficiency of the production logistics link. Summary of the Invention

[0004] In view of this, an embodiment of the present invention provides a path planning method and device based on pre-sampling of important nodes to solve the technical problems of long path search time and poor path quality caused by the large search space and complex obstacles when the traditional RRT algorithm is used for path planning of automated transportation equipment in a large-scale factory map environment.

[0005] In a first aspect, an embodiment of the present invention provides a path planning method based on pre-sampling of important nodes, the method comprising:

[0006] Obtaining the shortest paths between important nodes in the map topology, and constructing a multi-source shortest path tree using the shortest paths between the important nodes;

[0007] Acquire a path planning task, wherein the path planning task includes a path starting point and a path end point;

[0008] Search the multi-source shortest path tree to obtain an initial path between the path starting point and the path end point, and construct an RRT* node tree based on the initial path;

[0009] Node expansion is performed on the RRT* node tree to obtain an optimized path between the path starting point and the path end point.

[0010] Furthermore, obtaining the shortest path between important nodes in the map topology structure includes:

[0011] Obtaining an original map and a complex situation of the original map;

[0012] Performing discrete sampling on the original map using the complex situation to obtain a plurality of discrete nodes, and determining the degree centrality corresponding to each discrete node;

[0013] Sampling discrete nodes in the map topology structure based on the point degree centrality to obtain multiple important nodes;

[0014] Get the shortest path between multiple important nodes.

[0015] Furthermore, determining the degree centrality corresponding to each discrete node includes:

[0016] Obtaining an adjacency matrix corresponding to the discrete nodes, wherein an element in the adjacency matrix is ​​used to indicate whether the i-th discrete node is connected to the j-th discrete node;

[0017] For each discrete node, count the number of non-zero elements in the corresponding row or column in the adjacency matrix;

[0018] The degree centrality is determined based on the number of non-zero elements corresponding to the discrete nodes.

[0019] Furthermore, the discrete nodes in the map topology structure are sampled based on the point degree centrality to obtain multiple important nodes, including:

[0020] Determining a sampling weight corresponding to the degree centrality based on a correspondence between the preset degree centrality and the preset sampling weight, wherein the sampling weight is used to represent the probability of a discrete node being selected;

[0021] The discrete nodes in the map topology structure are sampled based on the sampling weights to obtain a plurality of non-repeated important nodes.

[0022] Furthermore, the shortest paths between the important nodes are used to construct a multi-source shortest path tree, including:

[0023] Construct an undirected graph using the locations of important nodes and map information;

[0024] Based on the undirected graph, initialize the path length matrix and the predecessor node matrix between any two points, wherein the path length matrix is ​​used to record the path length of the node pair in the current iteration, and the predecessor node matrix is ​​used to record the predecessor information of the nodes in the shortest path;

[0025] For each pair of nodes, an intermediate node is introduced. If the path passing through the intermediate node is shorter, the length in the path length matrix is ​​updated, and the predecessor information in the predecessor node matrix is ​​updated at the same time. The shortest path length and predecessor information between all node pairs including important nodes are obtained through gradual iterative optimization.

[0026] Based on the updated path length matrix and predecessor node matrix, the shortest paths between multiple important nodes are determined.

[0027] Furthermore, searching the multi-source shortest path tree to obtain an initial path between the path starting point and the path end point includes:

[0028] In the graph associated with the multi-source shortest path tree, searching for important nodes that can be directly connected to the path starting point and the path ending point respectively as target points;

[0029] Detecting whether there is a shortest main path between the path starting point, the path ending point and the target point;

[0030] If it exists, the main path is used as the initial path; or, if the main path does not exist, two random exploration trees are initialized with the path starting point and the path end point as root nodes respectively, and expansion is performed based on the random exploration trees until an initial path is generated in which important nodes on the path can connect the path starting point and the path end point.

[0031] Furthermore, the step of performing node expansion on the RRT* node tree to obtain an optimized path between the path starting point and the path end point includes:

[0032] Guided by the initial path, starting from the nodes involved in the initial path, randomly sampling new nodes in the map, connecting the new nodes with the nodes in the constructed path tree, and generating extended nodes;

[0033] The node expansion operation is continuously repeated to update the connection relationship and path information between the nodes until the node expansion operation is performed a certain number of times, thereby obtaining an optimized path between the path starting point and the path end point.

[0034] In a second aspect, an embodiment of the present invention provides a path planning device based on pre-sampling of important nodes, the device comprising:

[0035] A construction module, configured to obtain the shortest paths between important nodes in a map topology structure, and construct a multi-source shortest path tree using the shortest paths between the important nodes;

[0036] An acquisition module, configured to acquire a path planning task, wherein the path planning task includes a path starting point and a path end point;

[0037] A processing module, configured to search the multi-source shortest path tree, obtain an initial path between the path starting point and the path end point, and construct an RRT* node tree based on the initial path;

[0038] The optimization module is used to perform node expansion on the RRT* node tree to obtain an optimized path between the path starting point and the path end point.

[0039] In a third aspect, an embodiment of the present invention provides a computer device comprising: a memory and a processor, the memory and the processor being communicatively connected to each other, computer instructions being stored in the memory, and the processor executing the method of the first aspect or any corresponding embodiment thereof by executing the computer instructions.

[0040] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to enable a computer to execute the method of the first aspect or any corresponding embodiment thereof.

[0041] This application solution accurately locks important nodes by pre-sampling the map, and pre-builds a multi-source shortest path tree between important nodes. When a path planning task is received, the initial path can be quickly generated based on the shortest path tree, fundamentally breaking the limitation of the traditional RRT algorithm in blindly scattering sampling in ultra-large maps, greatly reducing the need to generate a large number of redundant nodes caused by the large search space and complex obstacles, thereby significantly shortening the path search time and improving planning efficiency. Not only that, building an RRT* node tree with the initial path as a solid foundation, and deeply optimizing the path with the help of node expansion can ensure that the final planned path is not only shorter in length, but also smoother and more fluent, thereby improving the path quality. Ultimately, it effectively overcomes the technical bottleneck of traditional RRT algorithms in path planning in complex scenarios such as large factories, and provides a powerful adaptation solution for efficient and accurate transportation needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0043] Figure 1 is a flowchart of a path planning method based on important node pre-sampling according to some embodiments of the present invention;

[0044] Figure 2 is a schematic diagram of important node sampling results according to some embodiments of the present invention;

[0045] Figure 3 is a schematic diagram of the results of the Floyd algorithm according to some embodiments of the present invention;

[0046] Figure 4 is a schematic diagram of the RRT path optimization principle according to some embodiments of the present invention;

[0047] Figure 5 is a schematic diagram of an initial path and an optimized path according to some embodiments of the present invention;

[0048] Figure 6 is a structural block diagram of a path planning device based on important node pre-sampling according to an embodiment of the present invention;

[0049] Figure 7 Schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0050] To make the purpose, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative efforts shall fall within the scope of protection of the present invention.

[0051] According to an embodiment of the present invention, a path planning method and apparatus based on pre-sampling of important nodes are provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0052] In this embodiment, a path planning method based on pre-sampling of important nodes is provided. Figure 1 is a flow chart of a path planning method based on important node pre-sampling according to an embodiment of the present invention. Figure 1 As shown, the process includes the following steps:

[0053] Step S101 : obtaining the shortest paths between important nodes in the map topology structure, and constructing a multi-source shortest path tree using the shortest paths between important nodes.

[0054] In an embodiment of the present application, obtaining the shortest path between important nodes in a map topology structure based on a path planning task includes the following steps A1-A4:

[0055] Step A1: Obtain the original map and the complex situation of the original map.

[0056] Specifically, the original map and the complexity of the original map can be obtained from channels such as geographic information systems and map databases. At the same time, the complexity of the original map is analyzed, such as whether the terrain is flat or mountainous, the distribution density of buildings or obstacles, the complexity of the road network, and other information. These contents will provide a basis for subsequent discrete sampling and other operations to ensure that subsequent processing adapts to the actual characteristics of the map.

[0057] In step A2, the original map is discretely sampled using complex situations to obtain multiple discrete nodes, and the degree centrality corresponding to each discrete node is determined.

[0058] Specifically, determining the degree centrality corresponding to each discrete node includes: obtaining the adjacency matrix corresponding to the discrete node, wherein the elements in the adjacency matrix are used to indicate whether the i-th discrete node and the j-th discrete node are connected; for each discrete node, counting the number of non-zero elements in the corresponding row or column in the adjacency matrix; and determining the degree centrality based on the number of non-zero elements corresponding to the discrete node.

[0059] After completing discrete sampling of the map to obtain a number of discrete nodes, it is necessary to construct an adjacency matrix that reflects the connectivity relationships between the nodes. Specifically, it is necessary to first determine the indexes of all discrete nodes (for example, numbering the nodes in sampling order), and then determine one by one whether each two discrete nodes (the i-th and the j-th) are connected in the original map scene. The basis for connectivity can be based on geographic spatial relationships (such as the absence of obstacles and being in a traversable area).

[0060] If the i-th discrete node is connected to the j-th discrete node, the elements at positions (i, j) and (j, i) in the adjacency matrix (undirected graph scenario) are set to 1; if they are not connected, they are set to 0. In this way, all node pairs are traversed to finally construct a complete adjacency matrix for subsequent analysis of the connection between nodes.

[0061] For the constructed adjacency matrix, since the adjacency matrix of an undirected graph is symmetric, rows and columns reflect the same node connection information, so you can choose to count the corresponding rows or columns. Taking the i-th discrete node as an example, find the i-th row (or i-th column) in the adjacency matrix, then iterate through the elements in that row (or column) one by one, counting the number of elements with a value of 1. This number is the number of non-zero elements in the corresponding row or column of the adjacency matrix for the i-th discrete node. This number essentially represents the number of edges directly connecting this node to other discrete nodes.

[0062] Degree centrality is used to measure the local importance of a node in a network. After obtaining the number of nonzero elements in the row or column corresponding to each discrete node, this number is directly used as the degree centrality of the discrete node. Because the core logic of degree centrality is to count the number of direct connections between nodes, the number of nonzero elements obtained here through adjacency matrix statistics precisely reflects the direct connectivity of the discrete node with other nodes in the map topology. Therefore, it can be directly used as the quantitative result of degree centrality for subsequent path planning operations such as screening important nodes.

[0063] In step A3, discrete nodes in the map topology are sampled based on degree centrality to obtain multiple important nodes.

[0064] Specifically, discrete nodes in the map topology are sampled based on degree centrality to obtain multiple important nodes, including: determining the sampling weight corresponding to the degree centrality based on the correspondence between the preset degree centrality and the preset sampling weight, wherein the sampling weight is used to represent the probability of the discrete node being selected; and sampling the discrete nodes in the map topology based on the sampling weight to obtain multiple non-repeated important nodes.

[0065] First, define the mapping rules between degree centrality and sampling weights (e.g., linear positive correlation, normalization, etc.). Assuming the default rule is "sampling weight is proportional to degree centrality," first collect the degree centrality values ​​of all discrete nodes, calculate the sum of these values, and then divide each node's degree centrality by the sum to obtain the normalized sampling weights. Ensure that the sum of all weights is 1, so that each weight value accurately represents the probability of the corresponding discrete node being selected during the sampling process.

[0066] Using the determined sampling weights, a weighted random sampling algorithm (such as the roulette wheel sampling method) is used to sample discrete nodes. In the specific operation, the sampling weight of each node is mapped to the size of the sector area on the roulette wheel. By randomly generating a value between 0 and 1, it is determined which sector area the value falls into, thereby determining the selected node. After each sampling, the selected node is marked as unselectable to avoid repeated sampling until a preset number of nodes (such as Figure 2 shown).

[0067] Step A4: Obtain the shortest paths between multiple important nodes.

[0068] Specifically, a multi-source shortest path tree is constructed using the shortest paths between important nodes. This involves constructing an undirected graph using important nodes and their connections on the map. Based on this undirected graph, a path length matrix and a predecessor node matrix are initialized between any two points. The path length matrix records the path length of each node pair in the current iteration, while the predecessor node matrix records the predecessor information of the nodes in the shortest path.

[0069] For each pair of nodes, an intermediate node is introduced. If the path passing through the intermediate node is shorter, the length in the path length matrix is ​​updated, and the predecessor information in the predecessor node matrix is ​​also updated. This iterative optimization process gradually obtains the shortest path lengths and predecessor information between all pairs of nodes in all important nodes. Based on the updated path length matrix and predecessor node matrix, the shortest paths between multiple important nodes are determined.

[0070] First, construct a set V using key nodes. Then, the connectivity of any two nodes (i, j) in the set is determined one by one: If the two nodes are directly accessible in the original map (e.g., no obstacles separate them and they comply with traffic rules), an undirected edge (i, j) is added between the nodes, with the edge weight ωij set to the physical distance (e.g., Euclidean distance) or the cost of the two nodes. If the two nodes are not accessible, no edge is added or the edge weight is set to infinity. Ultimately, an undirected graph G = (V, E, ω) containing the key nodes is constructed, where the undirected nature of the edge indicates bidirectional traffic between the nodes.

[0071] Based on the undirected graph G, create two matrices of dimension |V|×|V| (|V| is the total number of nodes). In the path length matrix L, initially, if nodes i and j are directly connected, then L[i][j] = ωij; otherwise, L[i][j] = +∞, and the diagonal elements L[i][i] = 0. In the predecessor node matrix Pre, initially, if nodes i and j are directly connected, then Pre[i][j] = i (indicating that j's predecessor is i); otherwise, Pre[i][j] = -1 (indicating that there is no direct predecessor).

[0072] like Figure 3As shown, traverse all possible intermediate nodes k∈V, then traverse all node pairs (i,j)∈V×V, and determine whether the path length L[i][k]+L[k][j] passing through the intermediate node k is less than the currently recorded L[i][j]. If it is shorter, update L[i][j]=L[i][k]+L[k][j], and update Pre[i][j] to Pre[k][j] (indicating that the predecessor node of j becomes the predecessor of k). Through three-layer loop iterative optimization, the final path length matrix L stores the shortest path length of any node pair, and the predecessor matrix Pre can be used to backtrack the specific path.

[0073] In an embodiment of the present application, the shortest paths between important nodes are used to construct a multi-source shortest path tree, including: first, the Floyd-Warshall algorithm is used to calculate the shortest paths between all node pairs in the topological graph composed of important nodes, and a matrix that records the predecessor relationship of the shortest path of each node is simultaneously generated; then, each important node is used as the source node, and its shortest path to all other nodes is traced back through the predecessor node matrix, and parent-child node connections are established according to the predecessor-successor relationship of the nodes on the path (such as in the shortest path from node A to node B, if node C is the predecessor node of node B, then C is set as the parent node of B), forming a single-source shortest path tree; finally, all single-source shortest path trees are topologically merged, and the shortest path connection relationship between each node and multiple source nodes is retained, and finally a multi-source shortest path tree containing all important nodes is constructed to ensure that the path from any node in the tree to any source node is the global shortest path.

[0074] Step S102: Acquire a path planning task, wherein the path planning task includes a path starting point and a path end point.

[0075] In an embodiment of the present application, when carrying out path planning, the path planning task must first be obtained. The task clearly contains key information, namely the path starting point and the path end point. They serve as the basis for all subsequent path planning operations, defining where to start planning the path and the path end point to be reached. The subsequent map processing, discrete sampling, path search and optimization and other processes are all gradually unfolded around finding a reasonable and optimal travel path between these two locations.

[0076] Step S103 : searching the multi-source shortest path tree to obtain an initial path between the path start point and the path end point, and constructing an RRT* node tree based on the initial path.

[0077] In the embodiment of the present application, searching the multi-source shortest path tree to obtain the initial path between the path starting point and the path end point includes the following steps B1-B3:

[0078] Step B1: In the graph associated with the multi-source shortest path tree, an important node that can be directly connected to the path starting point and the path ending point is searched as a target point.

[0079] First, define the nodes and edges in the undirected graph G = (V, E, ω) on which the multi-source shortest path tree is based. The node set V consists of all important nodes resampled from discrete points, and the edge set E represents the traversable connections between nodes. Next, traverse all adjacent nodes in the graph directly connected to the path start point to form a candidate start point connected set. Similarly, traverse all adjacent nodes directly connected to the path end point to form a candidate end point connected set T.

[0080] Finally, a node directly connected to the starting point of the path is selected from the candidate starting point connected set as target point 1, and a node directly connected to the candidate terminal is selected from the candidate terminal connected set T as target point 2, ensuring that the two target points have direct edge connections with the starting point and terminal of the path respectively.

[0081] Step B2: Detect whether there is a shortest main path between the path starting point, the path ending point and the target point.

[0082] Based on the two obtained destination points, the shortest path length from destination point 1 to destination point 2 is queried in the path length matrix of the multi-source shortest path tree. The overall path from the path's starting point, through destination point 1, to destination point 2, and then to the path's end point is calculated. If a path exists that passes through important nodes and connects the intermediate nodes to each other through a clear shortest path in the multi-source shortest path tree, a primary path is determined to exist; otherwise, no primary path exists. If multiple primary paths exist, the path with the lowest overall cost is selected as the primary path.

[0083] In step B3, if the main path exists, the main path is used as the initial path; or, if the main path does not exist, two random exploration trees with the starting point and the end point as the root nodes are initialized, and then the random exploration trees are expanded until an initial path is generated that can connect the starting point and the end point through important nodes.

[0084] If a main path exists: directly concatenate the shortest paths from the path start point to target point 1, from target point 1 to target point 2, and from target point 2 to the path end point to form the initial path.

[0085] If no main path exists: Initialize two random exploration trees (RRTs) with the starting and ending points as root nodes. Then, randomly sample new nodes from the map and find the node closest to the new node in the tree. If the new node is accessible to the nearest node, add the new node to the tree and update the connection relationship. Continue to expand the two trees until the newly added nodes (new point 1 and new point 2) in the two trees can be connected through important nodes (target point 1 and target point 2). Finally, generate and link paths from the starting point to new point 1, from new point 1 to target point 1, from target point 1 to target point 2, from target point 2 to new point 2, and from new point 2 to the end point, forming the initial path.

[0086] Step S104: Expand the RRT* node tree to obtain an optimized path between the path start point and the path end point.

[0087] In the embodiment of the present application, the RRT* node tree is expanded to obtain an optimized path between the path starting point and the path end point, including the following steps C1-C2:

[0088] In step C1, guided by the initial path, starting from the nodes involved in the initial path, new nodes are randomly sampled in the map, and the new nodes are connected with the nodes in the constructed path tree to generate extended nodes.

[0089] First, the node set involved in the initial path (such as the path starting point, end point and intermediate key nodes) is determined and used as the initial nodes of the path tree. Then, random sampling is performed in the map to generate new candidate nodes.

[0090] Next, the node closest to the candidate node is found from the constructed path tree, and an attempt is made to establish a connection between the nearest node and the candidate node: if the path between the two points is free of obstacles and complies with the traffic rules, the candidate node is added to the path tree as a child of the nearest node, the connection relationship between the nodes is updated, and the path information (such as edge weight and path length) from the nearest node to the candidate node is recorded; if the connection cannot be made, a new node is resampled and the above process is repeated until a valid extended node is generated.

[0091] Step C2, continuously repeating the node expansion operation to update the connection relationship and path information between nodes until the node expansion operation is performed a certain number of times to obtain the optimized path between the path starting point and the path end point.

[0092] After generating an expansion node, the newly generated expansion node is used as the basis for a new round of expansion, and new candidate nodes are randomly sampled from the map's discrete node set. The node closest to the candidate node is then searched from the currently constructed path tree, and an attempt is made to establish a connection between the two. If the connection path is obstacle-free and meets the traffic rules, the candidate node is added to the path tree as a child of the nearest node. At the same time, the connection relationship between the nodes is updated, and the path information from the nearest node to the candidate node is recorded, including edge weights, path lengths, and other data.

[0093] If the connection fails, the candidate nodes are resampled and the connection operation is repeated; the above process of expanding nodes, attempting connections, and updating path tree information is repeated in a continuous cycle. As the node tree continues to expand, the path information is continuously optimized until the node expansion operation is executed a certain number of times, and an optimized path from the path starting point to the path end point is obtained.

[0094] For example, Figure 4 Figure 2 shows the path optimization process based on RRT* (Rapid Random Tree Retrieval), including the initial path and optimization operations. The left side shows the initial path, extending from the starting point through several nodes to the destination. The right side uses different examples to illustrate two common optimization operations: one is "adjusting the predecessor nodes of a new node." When a new node is generated, if the path to the target node through it is better, the predecessor of a node in the original path is replaced with the new node. The other is "adjusting the predecessor nodes of existing nodes." For existing nodes, if the new path can shorten the length or optimize the structure, its predecessor is also replaced. Through these predecessor update operations, the path is gradually optimized, making the overall path closer to the optimal one.

[0095] This application solution accurately locks important nodes by pre-sampling the map, and pre-builds a multi-source shortest path tree between important nodes. When a path planning task is received, the initial path can be quickly generated based on the shortest path tree, fundamentally breaking the limitation of the traditional RRT algorithm in blindly scattering sampling in ultra-large maps, greatly reducing the need to generate a large number of redundant nodes caused by the large search space and complex obstacles, thereby significantly shortening the path search time and improving planning efficiency. Not only that, building an RRT* node tree with the initial path as a solid foundation, and deeply optimizing the path with the help of node expansion can ensure that the final planned path is not only shorter in length, but also smoother and more fluent, thereby improving the path quality. Ultimately, it effectively overcomes the technical bottleneck of traditional RRT algorithms in path planning in complex scenarios such as large factories, and provides a powerful adaptation solution for efficient and accurate transportation needs.

[0096] As a complete example of this application, the improved RRT path planning algorithm based on important node pre-sampling specifically includes the following steps:

[0097] Step 1: Pre-search important nodes.

[0098] (1.1) Map discretization.

[0099] For a given n1×n2 map, discrete sampling is performed according to the complexity of the map to obtain N (N=N1×N2) discrete points V0.

[0100] Calculate the adjacency matrix A0 and the corresponding edge weight matrix W0 of N points, where a i,j Indicates whether the i-th node is connected to the j-th node, w i,j Represents the weight of the edge between two nodes. When two nodes are connected, a i,j is 1, otherwise a i,j is 0.

[0101] V0=v i =(x i ,y i ),i∈1:N(1)

[0102]

[0103] (1.2) Discrete point weight sampling.

[0104] The degree centrality C of the node can be calculated based on the adjacency matrix A. Using P as the weight, M non-repeated sample points are sampled in V0 as the important node set V of the map.

[0105] C=[c1,c2,…,c N ](3)

[0106]

[0107] P=[p1,p2,…,p N ](5)

[0108] P∝C(6)

[0109] Node V and the edges E between nodes and their weights ω can form a complete weighted undirected graph G = (V, E, ω). V = {v1, v2, ..., v M} represents the set of all nodes, is a set of edges, each edge is an ordered node pair (u, v), and ω is the weight of the edge. E and ω can be further expressed as the adjacency matrix A and the edge weight matrix W, as shown in the formula. When the i-th node is connected to the j-th node, w i,j is the physical distance between the two points, otherwise it is an infinite value.

[0110]

[0111]

[0112] Step 2: Floyd algorithm generates the initial random tree.

[0113] (2.1) Dynamic programming iteration.

[0114] Run the Floyd algorithm for the graph G = (V, E, ω). The Floyd algorithm is a multi-source algorithm that attempts to gradually introduce intermediate nodes between two points to determine whether the path between the two points can be shortened by using these intermediate nodes.

[0115] First, initialize the path length matrix L and the predecessor node matrix Pre between any two points. i,j Indicates the path length between the i-th node and the j-th node in the current iteration. i,j The predecessor node of node j that represents the shortest path from node i to node j.

[0116]

[0117]

[0118] For each pair of nodes (i, j), try to add the middle node k. If l i,k +l k,j <l i,j , then update l i,j for l i,k +l k,j , and update pre i,j For pre k,j .

[0119] (2.2) Backtracking path

[0120] Restore the shortest path of all node pairs. Take the shortest path from node i to node j as an example. If Pre i,j ≠Null, then there is a path between i and j. Using the predecessor node matrix Pre, we can trace back from the end point j until we reach the starting point i. The length of this path is l i,j .

[0121] Step 3: RRT path search and optimization.

[0122] (3.1) Initial path search.

[0123] For any pair of path starting points u s and the path end point u t , first find the point in the graph that can be directly connected to the starting point and the end point of the path Find the main path that makes the overall path length the shortest s,e , the full path is path init ={us ,path s,e ,u t}.

[0124]

[0125] If the main path does not exist s,e Make v s 、v e Respectively with u s and u e Directly connected, initialize two random exploration trees and Its root nodes are u s and u e , expand the two exploration trees respectively, and each time a new node u is added k All try to connect to the nodes in the graph G = (V, E, ω) and expand to the nodes and If it is reachable, an initial path can be generated, which is expressed as in Depend on generate, Depend on Generate Node Respectively with v s 、v t Can be connected directly.

[0126] (3.2) RRT* path progressive optimization.

[0127] With path path init The initial exploration tree with u s Node or u e The node is the source node, and the path tree is randomly sampled and extended until a certain number of iterations are reached (such as Figure 5 To achieve the effect of gradual optimization, a new leaf node u is generated in each iteration. new When the node is found, the predecessor node of the node is updated, and the node is tried as the predecessor node of other nodes until the optimal path is finally obtained.

[0128]

[0129] l s,i =min{pre(u i ),l s,new +c(u new ,u i )}(15)

[0130] This application significantly improves path planning performance by combining important node sampling, Floyd algorithm and RRT algorithm: in terms of path search speed, a random exploration tree is initialized based on the Floyd path, which enhances the global exploration capability of RRT, reduces the local exploration range, and speeds up path acquisition; in terms of computational efficiency, an important node pre-sampling strategy is used, and multiple path trees can be constructed in one Floyd calculation, avoiding the repeated construction of random exploration trees by RRT when the map environment remains unchanged, thereby reducing repeated calculations; in terms of path quality, representative points are selected for important node pre-sampling, and Floyd ensures the optimal trunk path. Combined with the progressive optimization of the RRT* algorithm, the overall path is close to the theoretical shortest path, which comprehensively solves the problems of traditional RRT algorithm in path planning of large factories.

[0131] In this embodiment, a path planning device based on important node pre-sampling is also provided, which is used to implement the above-mentioned embodiments and preferred embodiments, and the details that have been described will not be repeated. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, implementation by hardware, or a combination of software and hardware, is also possible and conceivable.

[0132] This embodiment provides a path planning device based on pre-sampling of important nodes, such as Figure 6 As shown, including:

[0133] Construction module 601 is used to obtain the shortest paths between important nodes in the map topology structure, and use the shortest paths between the important nodes to construct a multi-source shortest path tree;

[0134] An acquisition module 602 is configured to acquire a path planning task, wherein the path planning task includes a path starting point and a path end point;

[0135] Processing module 603 is used to search the multi-source shortest path tree, obtain the initial path between the path starting point and the path end point, and construct the RRT* node tree based on the initial path;

[0136] The optimization module 604 is used to expand the nodes of the RRT* node tree to obtain an optimized path between the path starting point and the path end point.

[0137] In an embodiment of the present application, construction module 601 is used to obtain an original map and a complex situation of the original map; use the complex situation to discretely sample the original map to obtain multiple discrete nodes, and determine the degree centrality corresponding to each discrete node; sample the discrete nodes in the map topology structure based on the degree centrality to obtain multiple important nodes; and obtain the shortest path between the multiple important nodes.

[0138] In an embodiment of the present application, a construction module 601 is used to obtain an adjacency matrix corresponding to a discrete node, wherein the elements in the adjacency matrix are used to indicate whether the i-th discrete node is connected to the j-th discrete node; for each discrete node, the number of non-zero elements in the corresponding row or column in the adjacency matrix is ​​counted; and the degree centrality is determined based on the number of non-zero elements corresponding to the discrete node.

[0139] In an embodiment of the present application, a construction module 601 is used to determine a sampling weight corresponding to a degree centrality based on a correspondence between a preset degree centrality and a preset sampling weight, wherein the sampling weight is used to represent the probability of a discrete node being selected; and based on the sampling weight, discrete nodes in the map topology structure are sampled to obtain multiple non-repeating important nodes.

[0140] In an embodiment of the present application, construction module 601 is used to construct an undirected graph using the positions of important nodes and map information; based on the undirected graph, a path length matrix and a predecessor node matrix between any two points are initialized, wherein the path length matrix is ​​used to record the path length of the node pair in the current iteration, and the predecessor node matrix is ​​used to record the predecessor information of the node in the shortest path; for each pair of nodes, an intermediate node is introduced. If the path passing through the intermediate node is shorter, the length in the path length matrix is ​​updated, and the predecessor information in the predecessor node matrix is ​​updated at the same time, and the shortest path length and predecessor information between all node pairs including the important nodes are obtained by gradual iterative optimization; based on the updated path length matrix and predecessor node matrix, the shortest path between multiple important nodes is determined.

[0141] In an embodiment of the present application, the processing module 603 is used to search for important nodes that can be directly connected to the path starting point and the path end point respectively as target points in a graph associated with a multi-source shortest path tree; detect whether there is a shortest main path between the path starting point, the path end point and the target point; if so, use the main path as the initial path; or, if there is no main path, initialize two random exploration trees with the path starting point and the path end point as root nodes respectively, and expand based on the random exploration trees until an initial path is generated in which the important nodes can connect the path starting point and the path end point.

[0142] In an embodiment of the present application, processing module 603 is used to randomly sample new nodes in the map starting from the nodes involved in the initial path, using the initial path as a guide, connecting the new nodes with the nodes in the constructed path tree, and generating extended nodes; continuously repeating the node expansion operation, updating the connection relationship and path information between the nodes, until the node expansion operation is performed a certain number of times, and obtaining the optimized path between the path starting point and the path end point.

[0143] See also Figure 7 , Figure 7is a structural diagram of a computer device provided by an optional embodiment of the present invention, such as Figure 7 As shown, the computer device includes: one or more processors 10, memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components utilize different buses to communicate with each other and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in the memory or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Equally, multiple computer devices can be connected, and each device provides part of the necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system).

[0144] The processor 10 may be a central processing unit, a network processor, or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be an application-specific integrated circuit, a programmable logic device, or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic, or any combination thereof.

[0145] The memory 20 stores instructions that can be executed by at least one processor 10, so that the at least one processor 10 executes the method shown in the above embodiment.

[0146] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created based on the use of a computer device for displaying a small program landing page, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely located relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0147] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid-state drive; the memory 20 may also include a combination of the above types of memory.

[0148] The computer device further includes a communication interface 30 for the computer device to communicate with other devices or a communication network.

[0149] The embodiment of the present invention also provides a computer-readable storage medium. The above-mentioned method according to the embodiment of the present invention can be implemented in hardware, firmware, or implemented as a computer code that can be recorded in a storage medium, or implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and downloaded through a network and will be stored in a local storage medium, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state drive, etc.; further, the storage medium can also include a combination of the above-mentioned types of memory. It can be understood that a computer, a processor, a microprocessor controller or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor or hardware, the method shown in the above embodiment is implemented.

[0150] Although the embodiments of the present invention have been described with reference to the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention. Such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A path planning method based on pre-sampling of important nodes, characterized in that: The method comprises: Obtaining the shortest paths between important nodes in the map topology, and constructing a multi-source shortest path tree using the shortest paths between the important nodes; Acquire a path planning task, wherein the path planning task includes a path starting point and a path end point; Search the multi-source shortest path tree to obtain an initial path between the path starting point and the path end point, and construct an RRT* node tree based on the initial path; Node expansion is performed on the RRT* node tree to obtain an optimized path between the path starting point and the path end point.

2. The method according to claim 1, characterized in that The method of obtaining the shortest path between important nodes in the map topology structure includes: Obtaining an original map and a complex situation of the original map; Performing discrete sampling on the original map using the complex situation to obtain a plurality of discrete nodes, and determining the degree centrality corresponding to each discrete node; Sampling discrete nodes in the map topology structure based on the point degree centrality to obtain multiple important nodes; Get the shortest path between multiple important nodes.

3. The method according to claim 2, characterized in that Determining the degree centrality corresponding to each discrete node includes: Obtaining an adjacency matrix corresponding to the discrete nodes, wherein an element in the adjacency matrix is ​​used to indicate whether the i-th discrete node is connected to the j-th discrete node; For each discrete node, count the number of non-zero elements in the corresponding row or column in the adjacency matrix; The degree centrality is determined based on the number of non-zero elements corresponding to the discrete nodes.

4. The method according to claim 2, characterized in that The discrete nodes in the map topology structure are sampled based on the point degree centrality to obtain multiple important nodes, including: Determining a sampling weight corresponding to the degree centrality based on a correspondence between the preset degree centrality and the preset sampling weight, wherein the sampling weight is used to represent the probability of a discrete node being selected; The discrete nodes in the map topology structure are sampled based on the sampling weights to obtain a plurality of non-repeated important nodes.

5. The method according to claim 1, wherein The obtaining of the shortest paths between multiple important nodes includes: Construct an undirected graph using the locations of important nodes and map information; Based on the undirected graph, initialize the path length matrix and the predecessor node matrix between any two points, wherein the path length matrix is ​​used to record the path length of the node pair in the current iteration, and the predecessor node matrix is ​​used to record the predecessor information of the nodes in the shortest path; For each pair of nodes, an intermediate node is introduced. If the path passing through the intermediate node is shorter, the length in the path length matrix is ​​updated, and the predecessor information in the predecessor node matrix is ​​updated at the same time. The shortest path length and predecessor information between all node pairs including important nodes are obtained through gradual iterative optimization. Based on the updated path length matrix and predecessor node matrix, the shortest paths between multiple important nodes are determined.

6. The method according to claim 1, characterized in that The searching the multi-source shortest path tree to obtain an initial path between the path starting point and the path end point includes: In the graph associated with the multi-source shortest path tree, searching for important nodes that can be directly connected to the path starting point and the path ending point respectively as target points; Detecting whether there is a shortest main path between the path starting point, the path ending point and the target point; If it exists, the main path is used as the initial path; or, if the main path does not exist, two random exploration trees are initialized with the path starting point and the path end point as root nodes respectively, and expansion is performed based on the random exploration trees until an initial path is generated in which important nodes on the path can connect the path starting point and the path end point.

7. The method according to claim 6, characterized in that The step of performing node expansion on the RRT* node tree to obtain an optimized path between the path starting point and the path end point includes: Guided by the initial path, starting from the nodes involved in the initial path, randomly sampling new nodes in the map, connecting the new nodes with the nodes in the constructed path tree, and generating extended nodes; The node expansion operation is continuously repeated to update the connection relationship and path information between the nodes until the node expansion operation is performed a certain number of times, thereby obtaining an optimized path between the path starting point and the path end point.

8. A path planning device based on pre-sampling of important nodes, characterized in that: The device comprises: A construction module, configured to obtain the shortest paths between important nodes in a map topology structure, and construct a multi-source shortest path tree using the shortest paths between the important nodes; An acquisition module, configured to acquire a path planning task, wherein the path planning task includes a path starting point and a path end point; A processing module, configured to search the multi-source shortest path tree, obtain an initial path between the path starting point and the path end point, and construct an RRT* node tree based on the initial path; The optimization module is used to perform node expansion on the RRT* node tree to obtain an optimized path between the path starting point and the path end point.

9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method according to any one of claims 1 to 7.

Images