CN111928867B - Path planning method, system, equipment and storage medium based on time expansion - Google Patents
Path planning method, system, equipment and storage medium based on time expansion Download PDFInfo
- Publication number
- CN111928867B CN111928867B CN202010842827.2A CN202010842827A CN111928867B CN 111928867 B CN111928867 B CN 111928867B CN 202010842827 A CN202010842827 A CN 202010842827A CN 111928867 B CN111928867 B CN 111928867B
- Authority
- CN
- China
- Prior art keywords
- time
- space
- node
- map
- dimensional space
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
Abstract
The invention provides a path planning method, a system, equipment and a storage medium based on time expansion, wherein the method comprises the following steps: acquiring initial parameters and a two-dimensional space map of path planning; expanding a two-dimensional space map to a three-dimensional space-time map; and sequentially searching next path nodes in the three-dimensional space-time map from the starting place space-time node to obtain the shortest path from the starting place to the destination based on an A Star path planning algorithm. The method can be well applied to multi-vehicle path planning, and meets the constraint of the fastest transportation plan while realizing vehicle collision prevention and deadlock avoidance.
Description
Technical Field
The present invention relates to the field of path planning technologies, and in particular, to a method, a system, a device, and a storage medium for path planning based on time expansion.
Background
Since the Dijkstra algorithm, the development of shortest path algorithms has been a leading edge problem in engineering and graph theory. Due to the wide applicability, the path planning algorithm variation designed separately for various industries is also endless. In order to improve the efficiency of solving the optimal path, researchers successively put forward algorithm optimization schemes aiming at different industrial application fields. The algorithm A Star is one of the more important algorithms, adopts a heuristic search mode, does not look like Dijkstra algorithm blind search, can obviously reduce the search range and improve the navigation efficiency. Bidirectional Search (Bidirectional Search) is also a fast Search method, which employs a strategy of starting a Search from a start point and a target point at the same time, ideally meeting at the midpoint of a path, thereby terminating the Search process. The Hierarchical technology (Hierarchical Methods) adopts a preprocessing method to reduce the dimension of the road network to be searched, thereby achieving the purpose of quick search.
The multi-vehicle path planning rule puts higher requirements on the algorithm, the algorithm needs to ensure that the paths are kept separated from each other under the condition of multi-vehicle coordinated scheduling, the vehicles do not collide in the process of running according to the paths, and the planned paths are not allowed to deadlock. Researchers quickly found that the complexity of the algorithm increased greatly when the number of vehicles increased to many.
Disclosure of Invention
Aiming at the problems in the prior art, the invention aims to provide a path planning method, a system, equipment and a storage medium based on time expansion, which are applied to multi-vehicle path planning, realize vehicle collision prevention and deadlock avoidance and simultaneously meet the constraint of a fastest transportation plan.
The embodiment of the invention provides a path planning method based on time expansion, which comprises the following steps:
acquiring initial parameters of path planning and a two-dimensional space map, wherein the initial parameters comprise a space coordinate of a starting place, a starting time and a space coordinate of a destination, the two-dimensional space map comprises a plurality of space nodes, and each space node has a two-dimensional space coordinate;
expanding a two-dimensional space map to a three-dimensional space-time map, wherein a first dimension and a second dimension are two dimensions of the two-dimensional space map respectively, a third dimension is time, and the time range of the three-dimensional space-time map takes the starting time as a starting point;
determining a starting place space-time node in the three-dimensional space-time map according to the space coordinate of the starting place and the starting time, and determining a destination straight line in the three-dimensional space-time map according to the space coordinate of the destination and the time range of the three-dimensional space-time map;
and sequentially searching next path nodes in the three-dimensional space-time map from the starting place space-time node based on an A Star path planning algorithm to obtain the shortest path from the starting place to the destination.
Optionally, when searching for a next path node in the three-dimensional space-time map, setting a space-time constraint condition for selection of the next path node, where the space-time constraint condition includes: the next space-time node available for searching comprises a space-time node determined according to the space coordinates of the current space-time node and the next moment and a node determined according to the space coordinates of the adjacent nodes on the space of the current space-time node and the next moment.
Optionally, after the two-dimensional space map is expanded to the three-dimensional space-time map, the method further includes the following steps:
calculating an arrival difficulty index of each space node in the three-dimensional space-time map;
and when the next path node is searched in the three-dimensional space-time map, the heuristic function is calculated according to the arrival difficulty index of each space node in the three-dimensional space-time map.
Optionally, the calculating the arrival difficulty index of each spatial node includes the following steps:
sequentially calculating an arrival difficulty index from each space node to an adjacent node in the two-dimensional space map;
and calculating the average value of the arrival difficulty indexes of the space node to each adjacent node thereof as the arrival difficulty index of the space node for each space node.
Optionally, the calculating the arrival difficulty index of each spatial node to its neighboring node includes calculating manhattan distance or travel time of each spatial node to its neighboring node as the corresponding arrival difficulty index.
Optionally, the sequentially calculating the arrival difficulty index from each spatial node to its neighboring node includes the following steps:
finding the two farthest spatial nodes in the two-dimensional spatial map;
and sequentially searching second-most space nodes from the two farthest space nodes by using a depth-first traversal method, and iteratively generating a four-dimensional matrix, wherein a first dimension and a second dimension in the four-dimensional matrix are respectively two dimensions of the two-dimensional space map, a third dimension corresponds to time, and a fourth dimension corresponds to an arrival difficulty index from each space node to an adjacent node.
Optionally, finding the two farthest spatial nodes in the two-dimensional spatial map comprises finding the two spatial nodes with the farthest manhattan distance or the longest travel time in the two-dimensional spatial map.
Optionally, the method is applied to multi-vehicle path planning, and when a next path node is searched in the three-dimensional space-time map, a currently planned vehicle and a planned vehicle cannot pass through the same space node at the same time.
Optionally, the method is applied to multi-vehicle path planning, and when a next path node is searched in the three-dimensional space-time map, a closed list is established by adopting the following steps:
acquiring data of a planned vehicle path according to a preset vehicle path planning sequence;
determining the passing time of the planned vehicle path at each space node;
determining locking nodes according to the passing time of the planned vehicle path at each space node and the space coordinates of the corresponding space node;
and adding the locking node into a closed list.
Optionally, the closed list further includes a preset default locking node, and the time corresponding to the default locking node is the time range of the three-dimensional space-time map.
Optionally, after obtaining the initial parameters of the path plan, the method further includes the following steps:
calculating a travel time for the origin to the destination;
and defining the time range of the three-dimensional space-time map according to the starting time and the travel time.
The embodiment of the invention also provides a path planning system based on time expansion, which is used for realizing the path planning method based on time expansion, and the system comprises:
the system comprises a data acquisition module, a route planning module and a route planning module, wherein the data acquisition module is used for acquiring initial parameters and a two-dimensional space map of the route planning, the initial parameters comprise a space coordinate of a starting place, a starting time and a space coordinate of a destination, the two-dimensional space map comprises a plurality of space nodes, and each space node has a two-dimensional space coordinate;
the three-dimensional expansion module is used for expanding a two-dimensional space map to a three-dimensional space-time map, wherein a first dimension and a second dimension are two dimensions of the two-dimensional space map respectively, a third dimension is time, and the time range of the three-dimensional space-time map takes the starting time as a starting point;
the node determining module is used for determining a starting place space-time node in the three-dimensional space-time map according to the space coordinate of the starting place and the starting time, and determining a destination straight line in the three-dimensional space-time map according to the space coordinate of the destination and the time range of the three-dimensional space-time map;
and the path planning module is used for sequentially searching next path nodes in the three-dimensional space-time map from the starting place space-time node to obtain the shortest path from the starting place to the destination based on an A Star path planning algorithm.
An embodiment of the present invention further provides a path planning device based on time expansion, including:
a processor;
a memory having stored therein executable instructions of the processor;
wherein the processor is configured to perform the steps of the time expansion based path planning method via execution of the executable instructions.
The embodiment of the present invention further provides a computer-readable storage medium, which is used for storing a program, and when the program is executed, the steps of the path planning method based on time expansion are implemented.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the disclosure.
The time expansion-based path planning method, system, equipment and storage medium have the following beneficial effects:
according to the invention, a two-dimensional space map is expanded to a three-dimensional space-time map, the time dimension is increased, a single time point is constrained and released to a straight line of the three-dimensional space, and when a route is planned and searched to any point on the straight line where a destination is located, a jump-out condition is met, so that the method can be well applied to multi-vehicle route planning; the invention carries out path node search by comprehensively considering time and space, realizes the search of the path with space-time consistency while realizing the collision prevention and deadlock avoidance of vehicles, and can meet the constraint of the fastest transportation plan.
Drawings
Other features, objects and advantages of the present invention will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, with reference to the accompanying drawings.
FIG. 1 is a flow chart of a method for time-based expansion path planning according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a three-dimensional spatiotemporal map in accordance with an embodiment of the present invention;
FIG. 3 is a flowchart of calculating an arrival difficulty index for each spatial node according to an embodiment of the present invention;
FIG. 4 is a flow chart of establishing a closed list according to one embodiment of the present invention;
fig. 5 is a schematic structural diagram of a path planning system based on time expansion according to an embodiment of the present invention;
fig. 6 is a schematic structural diagram of a path planning apparatus based on time expansion according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of a computer-readable storage medium according to an embodiment of the present invention.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. Example embodiments may, however, be embodied in many different forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.
Furthermore, the drawings are merely schematic illustrations of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus their repetitive description will be omitted. Some of the block diagrams shown in the figures are functional entities and do not necessarily correspond to physically or logically separate entities. These functional entities may be implemented in the form of software, or in one or more hardware modules or integrated circuits, or in different networks and/or processor devices and/or microcontroller devices.
As shown in fig. 1, an embodiment of the present invention provides a path planning method based on time expansion, including the following steps:
s100: acquiring initial parameters of path planning and a two-dimensional space map, wherein the initial parameters comprise a space coordinate of a starting place, a starting time and a space coordinate of a destination, the two-dimensional space map comprises a plurality of space nodes, and each space node has a two-dimensional space coordinate;
s200: expanding a two-dimensional space map to a three-dimensional space-time map, wherein a first dimension and a second dimension are two dimensions of the two-dimensional space map respectively, a third dimension is time, and the time range of the three-dimensional space-time map takes the starting time as a starting point;
s300: determining a starting place space-time node in the three-dimensional space-time map according to the space coordinate of the starting place and the starting time, and determining a destination straight line in the three-dimensional space-time map according to the space coordinate of the destination and the time range of the three-dimensional space-time map;
s400: and sequentially searching next path nodes in the three-dimensional space-time map from the starting place space-time node based on an A Star path planning algorithm to obtain the shortest path from the starting place to the destination.
In the path planning method based on time expansion of this embodiment, the sequence number of each step is only to distinguish each step, and is not to be taken as a limitation on the specific execution sequence of each step, and the execution sequence between the above steps may be adjusted and changed as needed. For example, it is possible to first obtain a two-dimensional space map as a target search, perform three-dimensional expansion, and then obtain parameters such as space coordinates of a start location and a destination, and the like, and all fall within the scope of the present invention.
By adopting the time expansion-based path planning method, after the two-dimensional space map of the initial parameters and the target is obtained in the step S100, the time dimension is increased for the two-dimensional space map in the step S200, the method can be expanded to a three-dimensional space-time map, the expansion of a search space is realized, and the conditions of collision, deadlock and the like in multi-vehicle path planning can be avoided. The invention releases the single time point constraint to a straight line of a three-dimensional space in the step S300 by increasing the time dimension, and meets the jumping-out condition when the route planning searches any point on the straight line of the destination, thereby being well applied to multi-vehicle route planning.
In the step S400, if the route is planned for one vehicle, a route search is performed in the three-dimensional space-time map based on the a Star route planning algorithm to obtain each route node of the shortest route. And if the route planning is carried out aiming at a plurality of vehicles, sequentially carrying out route search in the three-dimensional space-time map for each vehicle according to a preset vehicle route planning sequence.
As shown in fig. 2, the present invention expands a two-dimensional space map (x, y) to three dimensions (x, y, t) on the basis of a conventional two-dimensional path search through steps S200 and S300, and adds time t to a third dimension. Although the prior art has also proposed the use of time as an extension of the search space, the prior art searches on the basis of a certain time result point, which is difficult to find in a multi-vehicle situation. In contrast, in the present invention, the time point constraint of the single destination is released to a straight line (e.g., a straight line extending in the t-dimension corresponding to the node 7 shown in fig. 3) in the three-dimensional space through step S300, so that when the single-vehicle path planning searches for any point of the straight line, the jump-out condition is satisfied, and the optimization based on the time expansion is further optimized.
In the present invention, a spatial node refers to a two-dimensional spatial node on a two-dimensional spatial map of an original object, each two-dimensional spatial node having a two-dimensional (x, y-direction) spatial coordinate (x, y). The invention expands a two-dimensional space map into a three-dimensional space-time map, increases the dimension t, and the space-time coordinates (x, y, t) of each space-time node in the three-dimensional space-time map comprise the space coordinates (x, y) and the corresponding time t.
When the next path node is sequentially searched from the starting space-time node in the three-dimensional space-time map, not only the two-dimensional space coordinates of the path node but also the time dimension corresponding to the path node need to be considered. Therefore, in the step S400, when searching for the next path node in the three-dimensional space-time map, a space search is required, which is different from the prior art, and since time is added as the third dimension, only the temporal superposition is allowed in the searching process, and the temporal reduction is not allowed, a space-time constraint condition needs to be set for the selection of the next path node, and the space-time constraint condition is based on the convention: the path forward cannot be performed while the time is reversed, and the stop waiting cannot be performed while the time is reversed.
In this embodiment, the spatio-temporal constraints include: the next spatio-temporal nodes available for search include (a) spatio-temporal nodes determined from the spatial coordinates of the current spatio-temporal node and the next time and (b) nodes determined from the spatial coordinates of spatially adjacent nodes of the current spatio-temporal node and the next time. Wherein (a) represents spatio-temporal nodes that may remain in place while time advances; (b) representing the spatio-temporal nodes that are selectable by time advance while the path advances to the next spatial node.
Specifically, the spatiotemporal constraint may be expressed as:
N{Adj}t+1＝Nt+1+N{Adj}t
wherein, N represents a spatio-temporal node used for searching, t represents a current time, and Adj represents an adjacent point set. The next spatio-temporal node N { Adj }available for searchingt+1Comprises (a) according to the current space-time node NtSpace coordinate and space-time node N determined by the next moment t +1t+1And (b) according to the current spatio-temporal node NtAnd the spatial coordinates of the spatially adjacent node N { Adj } determined at the next time instantt. The above formula shows that the next node of the current node can only be searched in the adjacent point set at the time t + 1.
In step S400, a path search is performed in the three-dimensional space-time map based on the a Star path planning algorithm. The a Star path planning algorithm is first introduced here.
The a Star algorithm, a x algorithm, is an optimal priority search, which is formulated based on a weighted graph. The next path node is sequentially searched from an initial ground node of a target map (the initial ground node is an initial space node when a two-dimensional space map is adopted, and the initial ground node is an initial space-time node when a three-dimensional space-time map is adopted in the invention) until the next path node reaches a destination ground node (the destination ground node is a destination space node when the two-dimensional space map is adopted, and the destination ground node is a straight line corresponding to a destination when the three-dimensional space-time map is adopted in the invention).
The a Star algorithm aims to find the path cost (minimum distance traveled, minimum time, etc.) for a given destination node with a minimum value. It does this by maintaining a tree of paths originating from the origin node and extending those paths one at a time until their termination criteria are met. In each iteration of its main loop, a Star needs to determine which paths to expand. The total path cost is estimated according to the cost of the current path and the cost required for extending the path to the target, so as to judge which path is used as the node for next search. Specifically, the path cost of the a Star algorithm may be expressed as the following equation:
f(n)＝g(n)+h(n)
where n represents the next node in the path, g (n) represents the cost of the path from the origin node to node n, and h (n) is a heuristic function for estimating the cost of the shortest path from node n to the destination node. The a Star search terminates when it chooses that the extended path is a path from the origin to the destination or no path can be extended. The heuristic function is problem-specific. If the heuristic function is acceptable, meaning that it never overestimates the actual cost of achieving the goal, the A Star algorithm guarantees that the lowest cost path from the origin to the destination is returned. A Star uses a priority queue to perform a repeatedly selected minimum (estimated) cost node for expansion. In each iteration of the algorithm, the node with the lowest value of f (n) is removed from the queue, its neighbors are updated with the f and g values, respectively, and these neighbors are added to the queue. The algorithm continues until the f-value of the destination node is lower than the f-value of any node in the queue or the queue is empty. The f value of the destination node is the cost of the shortest path because in the heuristic function describing the destination, the h value at the destination node is zero.
And finally, performing reverse search on the whole result queue, starting from the destination and performing reverse search to the starting place, thus obtaining the shortest path. In graph theory, this process is referred to as the augmentation process.
For a general path planning algorithm, the a Star algorithm of the traditional two-dimensional space search can meet most requirements. However, for multi-vehicle path planning, the conventional a Star algorithm has some limitations: (1) when each vehicle is planned independently, it is difficult to ensure that each path does not conflict; (2) the planning of a single path lacks global optimization, and if each vehicle simply waits for a stop, it is difficult to ensure that the completion time (ETA) of horizontal transportation is minimized although the shortest path can be ensured without deadlock.
In this embodiment, the step S200: after the two-dimensional space map is expanded to the three-dimensional space-time map, the method also comprises the following steps:
s210: calculating an arrival difficulty index of each space node in the three-dimensional space-time map;
in the step S400, when a next path node is searched in the three-dimensional space-time map, the heuristic function is calculated according to the arrival difficulty index of each space node in the three-dimensional space-time map.
The heuristic function in the conventional a Star algorithm is calculated based on the manhattan distance of the current node to the destination node. Since in the present invention the destination description is changed from a single point to a straight line, the original manhattan distance will not fit well into the heuristic function.
Specifically, the arrival difficulty indexes of all the space nodes are adopted to replace the Manhattan distance between two nodes in the traditional A Star algorithm, and in a heuristic function, the cost of the shortest path from each space node to a destination node is estimated according to the arrival difficulty indexes of all the space nodes in the three-dimensional space-time map.
Further, in step S210, the calculating the arrival difficulty index of each spatial node includes the following steps:
s211: sequentially calculating an arrival difficulty index from each space node to an adjacent node in the two-dimensional space map;
s212: for each spatial node, an average value of the arrival difficulty indexes of the spatial node to its respective neighboring nodes is calculated, and as the arrival difficulty index of the spatial node, for example, if the arrival difficulty indexes of one spatial node to its four neighboring nodes are 2, 1, 2, and 1, respectively, the arrival difficulty index of the spatial node is (2+1+2+1)/4, which is 1.5.
In this embodiment, the calculating the difficulty index of arrival of each spatial node to its neighboring node includes calculating a manhattan distance or a travel time (travel time) of each spatial node to its neighboring node as the corresponding difficulty index of arrival. Manhattan Distance (Manhattan Distance) is a geometric term used in geometric measurement space to indicate the sum of absolute wheel base of two points on a standard coordinate system. The manhattan distance is suitable for scenes that roads are flat and vertical and are distributed regularly, such as a path planning scene of container trucks at a wharf, and the manhattan distance is suitable for being adopted because the roads at the wharf are flat and vertical. In the path planning scene in the actual urban road, the shape of the urban road is not particularly regular, and the influence of inclined intersection and road speed limit can be caused, so that the method is more suitable for adopting travel time. The travel time may be calculated based on the actual length of each link and the road speed limit for that link.
Faloutsos proposes a Fast Map method, which uses the farthest iteration calculation of the physical distance of a Map to generate a distance Map for each point estimation. Specifically, as shown in fig. 3, the step S211: sequentially calculating the arrival difficulty index from each space node to the adjacent node thereof, comprising the following steps:
s211-1: finding two space nodes with the farthest physical distance in an original two-dimensional space map;
s211-2: sequentially searching second-most spatial nodes by using a depth-first traversal method from the two farthest spatial nodes, and iteratively generating a four-dimensional matrix;
the traditional Fast Map method expands a two-dimensional space Map to a three-dimensional scene, and needs to be promoted to four dimensions because the invention is applied to a three-dimensional space-time Map, wherein a first dimension and a second dimension in a four-dimensional matrix are two dimensions of the two-dimensional space Map respectively, a third dimension corresponds to time, and a fourth dimension corresponds to an arrival difficulty index from each space node to an adjacent node thereof, namely a Fast Map dimension.
The specific implementation manner of step S212 is: after all the space nodes in the original two-dimensional space Map are traversed, the whole four-dimensional scene is compressed back to the three-dimensional space-time Map, and the distance from each space node to each other space node is the average of all the values of the fourth dimension (Fast Map dimension).
In this embodiment, in step S211-1, when two spatial nodes with the longest physical distance are found in the original two-dimensional spatial map, two spatial nodes with the longest manhattan distance or the longest travel time may be found in the two-dimensional spatial map.
In this embodiment, due to the addition of the time dimension, the time expansion-based path planning method is applied to multi-vehicle path planning, and when a next path node is searched in the three-dimensional space-time map, a vehicle path planned later is constrained by a vehicle path planned earlier, that is, the vehicle path planned later cannot pass through the same space node at the same time as the vehicle path planned earlier. The invention ensures that the path planning of each single vehicle does not conflict with other vehicles in the multi-vehicle path planning through the closed list.
As shown in fig. 4, specifically, the path planning method based on time expansion is applied to multi-vehicle path planning, and in the step S400, when a next path node is searched in the three-dimensional space-time map, a closed list is established by adopting the following steps:
s510: acquiring data of a planned vehicle path according to a preset vehicle path planning sequence;
s520: determining the passing time of the planned vehicle path at each space node;
s530: determining locking nodes according to the passing time of the planned vehicle path at each space node and the space coordinates of the corresponding space node;
s540: and adding the locking node into a closed list.
In this embodiment, the closed list further includes a preset default locking node, and the time corresponding to the default locking node is the time range of the three-dimensional space-time map. Such as a node that is known to be impassable for road repair or road faults, etc.
Thus, the closed list can be expressed as the following formula:
wherein L isthisIndicating all the closure nodes in the current closure list, Close indicating the current default locking node,
representing the set of locking nodes determined using step S530 from the data of the planned vehicle path.
Thus, by using the closed list of the present invention, it is not necessary to lock all of the time for each path of the bicycle. That is, the lock corresponding to each physical node on the map is only temporary, and if and only if the vehicle is scheduled to pass through the time period, the node is locked, otherwise, the node is opened. The present invention accomplishes this by adding the result of each way-finding to the closed list of the next way-finding.
In theory, the time range t _ space of the three-dimensional spatiotemporal map may include all times after the start time. However, too large a time range t _ space will result in an increase in the algorithm search space, and too small a time range t _ space will not result in a spatial path because sufficient in-place latency may not be scheduled in a limited (too small) space. Therefore, if the proper size of t _ space is selected, it is also a key factor affecting the algorithm.
In this embodiment, after acquiring the initial parameters of the path planning in step S100, the method further includes the following steps:
calculating a travel time for the origin to the destination;
and defining the time range of the three-dimensional space-time map according to the starting time and the travel time. Specifically, the starting time is used as a starting point of a time range of the three-dimensional space-time map, and a time obtained by adding the travel time to the starting time may be used as an ending point of the time range of the three-dimensional space-time map, i.e., the size of the time range t _ space is equal to the travel time from the starting point to the destination. Or multiplying the travel time by a preset coefficient, adding the result to the starting time to serve as the end point of the time range of the three-dimensional space-time map, and adjusting the size of the coefficient according to requirements.
The pseudo code implementation description that the route planning method based on time expansion can adopt of the invention is as follows:
when the path planning is performed in step S400, a plurality of operation threads may be used simultaneously, and the value of the time range t _ space is substituted into the algorithm for path finding in each operation thread. Once a thread gets a legitimate path, the computation of other threads is stopped.
The present invention may be embodied in the above pseudo code using the C + + language or other languages. To test the feasibility of the algorithm, the inventors constructed a series of test maps and performed algorithm validation on these maps. Here, the 3 most representative maps are selected, where map 1 has 64 nodes, map 2 has 450 nodes, and map 3 has 80000 nodes.
Aiming at different maps, the algorithm of the invention randomly selects two points on the map as the starting place and the destination of the vehicle, iterates 10000 times, calculates the calculation time of each route searching result and takes an average value. Table 1 below shows the calculation results for a single calculation (single vehicle system).
TABLE 1 Single calculation results
| Map with a plurality of maps | Map 1 | Map 2 | Map 3 |
| Mean time of calculation | 10ms | 122ms | 154ms |
| Average number of iteration layers of way finding | 5432 | 11284 | 14538 |
It can be seen from the results of the single-vehicle system that the path planning method of the present invention shows a relatively high convergence rate on the large map, and the average calculation time can still be within an acceptable range after the number of nodes is increased. Meanwhile, the iteration times of the algorithm are also in a reasonable interval.
For multiple calculations (multi-vehicle system), the calculation results of the path planning method of the present invention are as follows:
TABLE 2 results of multiple calculations
| Map with a plurality of maps | Map 1 | Map 2 | Map 3 |
| Mean time of calculation | 30ms | 150ms | 530ms |
| Average number of iteration layers of way finding | 6208 | 63289 | 118275 |
Aiming at the multi-vehicle environment, the algorithm can still obtain higher speed in a medium and small map, and the calculation time of the method is correspondingly increased when the number of vehicles is increased. This is because the locking nodes of multiple vehicles affect the way to the next vehicle. More locking implies more iteration levels and thus more computation time.
As shown in fig. 5, an embodiment of the present invention further provides a path planning system based on time expansion, which is configured to implement the path planning method based on time expansion, and the system includes:
the data acquisition module M100 is configured to acquire an initial parameter and a two-dimensional space map of a path plan, where the initial parameter includes a space coordinate of a starting location, a starting time, and a space coordinate of a destination, the two-dimensional space map includes a plurality of space nodes, and each space node has a two-dimensional space coordinate;
a three-dimensional expansion module M200, configured to expand a two-dimensional space map to a three-dimensional space-time map, where a first dimension and a second dimension are two dimensions of the two-dimensional space map, respectively, a third dimension is time, and a time range of the three-dimensional space-time map takes the starting time as a starting point;
a node determining module M300, configured to determine a starting point spatio-temporal node in the three-dimensional spatio-temporal map according to the spatial coordinate of the starting point and the starting time, and determine a destination straight line in the three-dimensional spatio-temporal map according to the spatial coordinate of the destination and the time range of the three-dimensional spatio-temporal map;
and the path planning module M400 is used for sequentially searching the next path node from the starting place space-time node in the three-dimensional space-time map based on the A Star path planning algorithm so as to obtain the shortest path from the starting place to the destination.
By adopting the path planning system based on time expansion, after the two-dimensional space map of the initial parameters and the target is obtained through the data acquisition module M100, the time dimension is increased for the two-dimensional space map through the three-dimensional expansion module M200, the path planning system can be expanded to a three-dimensional space-time map, the expansion of a search space is realized, and the conditions of collision or deadlock and the like during multi-vehicle path planning can be avoided. According to the invention, by increasing the time dimension, a single time point is constrained to be released to a straight line of a three-dimensional space in the node determining module M300, and when the route planning searches any point on the straight line where the destination is located, the jump-out condition is met, so that the method can be well applied to multi-vehicle route planning.
In the path planning system based on time expansion of the present invention, the functions of each module may be implemented by using the specific implementation of the path planning method based on time expansion as described above. For example, the data acquisition module M100 may implement data acquisition by using the specific implementation of step S100, the three-dimensional expansion module M200 may acquire a three-dimensional spatio-temporal map by using the specific implementation of step S200, the node determination module M300 may determine spatio-temporal positions of a start location and a destination by using the specific implementation of step S300, and the path planning module M400 may implement path planning by using the specific implementation of step S400.
The operating environment of the path planning system of the present invention may be as shown in table 3 below, but the present invention is not limited thereto.
TABLE 3 System operating Environment
| OS | Linux Ubuntu 16.06 |
| CPU | E5-2609V4 8Core 1.7G |
| Chip set | intel C600 |
| Memory | 32G |
The embodiment of the invention also provides a path planning device based on time expansion, which comprises a processor; a memory having stored therein executable instructions of the processor; wherein the processor is configured to perform the steps of the time expansion based path planning method via execution of the executable instructions.
As will be appreciated by one skilled in the art, aspects of the present invention may be embodied as a system, method or program product. Thus, various aspects of the invention may be embodied in the form of: an entirely hardware embodiment, an entirely software embodiment (including firmware, microcode, etc.) or an embodiment combining hardware and software aspects that may all generally be referred to herein as a "circuit," module "or" platform.
An electronic device 600 according to this embodiment of the invention is described below with reference to fig. 6. The electronic device 600 shown in fig. 6 is only an example, and should not bring any limitation to the functions and the scope of use of the embodiments of the present invention.
As shown in fig. 6, the electronic device 600 is embodied in the form of a general purpose computing device. The components of the electronic device 600 may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 that connects the various system components (including the storage unit 620 and the processing unit 610), a display unit 640, and the like.
Wherein the storage unit stores program code, which can be executed by the processing unit 610, to cause the processing unit 610 to perform the steps according to various exemplary embodiments of the present invention described in the above section of the time-based extension path planning method of the present specification. For example, the processing unit 610 may perform the steps as shown in fig. 1.
The storage unit 620 may include readable media in the form of volatile memory units, such as a random access memory unit (RAM)6201 and/or a cache memory unit 6202, and may further include a read-only memory unit (ROM) 6203.
The memory unit 620 may also include a program/utility 6204 having a set (at least one) of program modules 6205, such program modules 6205 including, but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which, or some combination thereof, may comprise an implementation of a network environment.
The electronic device 600 may also communicate with one or more external devices 700 (e.g., keyboard, pointing device, bluetooth device, etc.), with one or more devices that enable a user to interact with the electronic device 600, and/or with any devices (e.g., router, modem, etc.) that enable the electronic device 600 to communicate with one or more other computing devices. Such communication may occur via an input/output (I/O) interface 650. Also, the electronic device 600 may communicate with one or more networks (e.g., a Local Area Network (LAN), a Wide Area Network (WAN), and/or a public network such as the Internet) via the network adapter 660. The network adapter 660 may communicate with other modules of the electronic device 600 via the bus 630. It should be appreciated that although not shown in the figures, other hardware and/or software modules may be used in conjunction with the electronic device 600, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems, among others.
The embodiment of the present invention further provides a computer-readable storage medium, which is used for storing a program, and when the program is executed, the steps of the path planning method based on time expansion are implemented. In some possible embodiments, aspects of the present invention may also be implemented in the form of a program product comprising program code for causing a terminal device to perform the steps according to various exemplary embodiments of the present invention described in the above section of the time-based extension path planning method of this specification, when the program product is executed on the terminal device.
Referring to fig. 7, a program product 800 for implementing the above method according to an embodiment of the present invention is described, which may employ a portable compact disc read only memory (CD-ROM) and include program code, and may be executed on a terminal device, such as a personal computer. However, the program product of the present invention is not limited in this regard and, in the present document, a readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The program product may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
The computer readable storage medium may include a propagated data signal with readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A readable storage medium may also be any readable medium that is not a readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a readable storage medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Program code for carrying out operations for aspects of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C + + or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computing device (e.g., through the internet using an internet service provider).
In summary, by adopting the time expansion-based path planning method, system, device and storage medium of the present invention, the two-dimensional space map is expanded to the three-dimensional space-time map, the time dimension is increased, the single time point is constrained to be released on one straight line of the three-dimensional space, and when the path planning searches for any point on the straight line where the destination is located, the jump-out condition is satisfied, and the method, system, device and storage medium can be well applied to multi-vehicle path planning; the invention carries out path node search by comprehensively considering time and space, realizes the search of space-time consistency paths while realizing the collision prevention and deadlock avoidance of vehicles, and can meet the constraint of the fastest transportation plan; the invention can be applied to the planning of single-vehicle paths, can also be well adapted to the planning of multi-vehicle paths, and meanwhile, the invention can be applied to the planning of the paths of container trucks on wharfs, and can also be applied to the planning of the paths of other vehicles on other roads, such as urban roads, mountain roads and the like.
The foregoing is a more detailed description of the invention in connection with specific preferred embodiments and it is not intended that the invention be limited to these specific details. For those skilled in the art to which the invention pertains, several simple deductions or substitutions can be made without departing from the spirit of the invention, and all shall be considered as belonging to the protection scope of the invention.
Claims (11)
1. A path planning method based on time expansion is characterized by comprising the following steps:
acquiring initial parameters of path planning and a two-dimensional space map, wherein the initial parameters comprise a space coordinate of a starting place, a starting time and a space coordinate of a destination, the two-dimensional space map comprises a plurality of space nodes, and each space node has a two-dimensional space coordinate;
expanding a two-dimensional space map to a three-dimensional space-time map, wherein a first dimension and a second dimension are two dimensions of the two-dimensional space map respectively, a third dimension is time, and the time range of the three-dimensional space-time map takes the starting time as a starting point;
determining a starting place space-time node in the three-dimensional space-time map according to the space coordinate of the starting place and the starting time, and determining a destination straight line in the three-dimensional space-time map according to the space coordinate of the destination and the time range of the three-dimensional space-time map;
searching a next path node in the three-dimensional space-time map in sequence from the starting place space-time node based on an A Star path planning algorithm to obtain a shortest path from the starting place to the destination;
after the two-dimensional space map is expanded to the three-dimensional space-time map, the method also comprises the following steps:
finding the two farthest spatial nodes in the two-dimensional spatial map;
sequentially searching second-most space nodes from the two farthest space nodes by using a depth-first traversal method, and iteratively generating a four-dimensional matrix, wherein a first dimension and a second dimension in the four-dimensional matrix are respectively two dimensions of the two-dimensional space map, a third dimension corresponds to time, and a fourth dimension corresponds to an arrival difficulty index from each space node to an adjacent node;
after all space nodes in the two-dimensional space map are traversed, compressing the whole four-dimensional scene back to the three-dimensional space-time map, wherein the distance from each space node to each other space node is the average of all values of the fourth dimension, and the average is used as an arrival difficulty index of each space node;
and when the next path node is searched in the three-dimensional space-time map, the heuristic function is calculated according to the arrival difficulty index of each space node in the three-dimensional space-time map.
2. The time-expansion-based path planning method according to claim 1, wherein, when searching for a next path node in the three-dimensional space-time map, a space-time constraint is set for selection of the next path node, and the space-time constraint includes: the next space-time node available for searching comprises a space-time node determined according to the space coordinates of the current space-time node and the next moment and a node determined according to the space coordinates of the adjacent nodes on the space of the current space-time node and the next moment.
3. The method of claim 1, wherein the calculating the difficulty of arrival index of each spatial node to its neighboring nodes comprises calculating manhattan distance or travel time of each spatial node to its neighboring nodes as the corresponding difficulty of arrival index.
4. The time-expansion-based path planning method according to claim 1, wherein finding the two farthest spatial nodes in the two-dimensional spatial map comprises finding the two spatial nodes with the longest manhattan distance or the longest travel time in the two-dimensional spatial map.
5. The time-expansion-based path planning method according to claim 1, wherein the method is applied to multi-vehicle path planning, and when a next path node is searched in the three-dimensional space-time map, a currently planned vehicle and a planned vehicle cannot pass through the same spatial node at the same time.
6. The time-expansion-based path planning method according to claim 5, wherein the method is applied to multi-vehicle path planning, and when a next path node is searched in the three-dimensional space-time map, a closed list is established by adopting the following steps:
acquiring data of a planned vehicle path according to a preset vehicle path planning sequence;
determining the passing time of the planned vehicle path at each space node;
determining locking nodes according to the passing time of the planned vehicle path at each space node and the space coordinates of the corresponding space node;
and adding the locking node into a closed list.
7. The time-expansion-based path planning method according to claim 6, wherein the closed list further includes a preset default locking node, and the time corresponding to the default locking node is the time range of the three-dimensional space-time map.
8. The method for path planning based on time expansion according to claim 1, further comprising the following steps after the initial parameters of the path planning are obtained:
calculating a travel time for the origin to the destination;
and defining the time range of the three-dimensional space-time map according to the starting time and the travel time.
9. A time expansion based path planning system for implementing the time expansion based path planning method according to any one of claims 1 to 8, the system comprising:
the system comprises a data acquisition module, a route planning module and a route planning module, wherein the data acquisition module is used for acquiring initial parameters and a two-dimensional space map of the route planning, the initial parameters comprise a space coordinate of a starting place, a starting time and a space coordinate of a destination, the two-dimensional space map comprises a plurality of space nodes, and each space node has a two-dimensional space coordinate;
the three-dimensional expansion module is used for expanding a two-dimensional space map to a three-dimensional space-time map, wherein a first dimension and a second dimension are two dimensions of the two-dimensional space map respectively, a third dimension is time, and the time range of the three-dimensional space-time map takes the starting time as a starting point;
the node determining module is used for determining a starting place space-time node in the three-dimensional space-time map according to the space coordinate of the starting place and the starting time, and determining a destination straight line in the three-dimensional space-time map according to the space coordinate of the destination and the time range of the three-dimensional space-time map;
and the path planning module is used for sequentially searching next path nodes in the three-dimensional space-time map from the starting place space-time node to obtain the shortest path from the starting place to the destination based on an A Star path planning algorithm.
10. A path planning apparatus based on time expansion, comprising:
a processor;
a memory having stored therein executable instructions of the processor;
wherein the processor is configured to perform the steps of the time expansion based path planning method of any one of claims 1 to 8 via execution of the executable instructions.
11. A computer readable storage medium storing a program, wherein the program when executed implements the steps of the time-expansion-based path planning method of any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010842827.2A CN111928867B (en) | 2020-08-20 | 2020-08-20 | Path planning method, system, equipment and storage medium based on time expansion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010842827.2A CN111928867B (en) | 2020-08-20 | 2020-08-20 | Path planning method, system, equipment and storage medium based on time expansion |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111928867A CN111928867A (en) | 2020-11-13 |
| CN111928867B true CN111928867B (en) | 2021-04-30 |
Family
ID=73305885
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010842827.2A Active CN111928867B (en) | 2020-08-20 | 2020-08-20 | Path planning method, system, equipment and storage medium based on time expansion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111928867B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112665592B (en) * | 2020-12-16 | 2023-10-20 | 郑州大学 | Space-time path planning method based on multiple agents |
| CN113091751B (en) * | 2021-04-13 | 2023-10-24 | 西安美拓信息技术有限公司 | Path planning method under partial grid orientation condition in grid space |
| CN113393062A (en) * | 2021-08-17 | 2021-09-14 | 深圳坤湛科技有限公司 | Vehicle scheduling method, program product, system, and storage medium |
| CN113899383B (en) * | 2021-11-22 | 2024-04-19 | 上海西井科技股份有限公司 | Multi-vehicle deadlock prevention method, system, equipment and storage medium based on short path |
| CN114595880B (en) * | 2022-03-03 | 2022-11-25 | 捻果科技(深圳)有限公司 | Intelligent presetting method and system for flight area behavior route |
| CN114353820A (en) * | 2022-03-18 | 2022-04-15 | 腾讯科技（深圳）有限公司 | Path planning method and device, electronic equipment and storage medium |
| CN115526385B (en) * | 2022-09-13 | 2024-04-16 | 成都飞机工业（集团）有限责任公司 | Warehouse logistics distribution path planning method, device, equipment and medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108491971A (en) * | 2018-03-20 | 2018-09-04 | 北京交通大学 | The optimum path planning method of three-dimensional map based on plan of travel |
| CN108759851A (en) * | 2018-06-01 | 2018-11-06 | 上海西井信息科技有限公司 | More vehicle paths planning methods, system, equipment and storage medium based on time window |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
| CN110362083A (en) * | 2019-07-17 | 2019-10-22 | 北京理工大学 | It is a kind of based on multiple target tracking prediction space-time map under autonomous navigation method |
| CN111380526A (en) * | 2018-12-27 | 2020-07-07 | 北京航迹科技有限公司 | System and method for determining path |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9200913B2 (en) * | 2008-10-07 | 2015-12-01 | Telecommunication Systems, Inc. | User interface for predictive traffic |
-
2020
- 2020-08-20 CN CN202010842827.2A patent/CN111928867B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108491971A (en) * | 2018-03-20 | 2018-09-04 | 北京交通大学 | The optimum path planning method of three-dimensional map based on plan of travel |
| CN108759851A (en) * | 2018-06-01 | 2018-11-06 | 上海西井信息科技有限公司 | More vehicle paths planning methods, system, equipment and storage medium based on time window |
| CN111380526A (en) * | 2018-12-27 | 2020-07-07 | 北京航迹科技有限公司 | System and method for determining path |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
| CN110362083A (en) * | 2019-07-17 | 2019-10-22 | 北京理工大学 | It is a kind of based on multiple target tracking prediction space-time map under autonomous navigation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111928867A (en) | 2020-11-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111928867B (en) | Path planning method, system, equipment and storage medium based on time expansion | |
| Taguchi et al. | Online map matching with route prediction | |
| CN109115226B (en) | Route planning method for avoiding multi-robot conflict based on jumping point search | |
| JP5172326B2 (en) | System and method for adaptive path planning | |
| US20210374824A1 (en) | Method and an Apparatus for Searching or Comparing Sites Using Routes or Route Lengths Between Sites and Places Within a Transportation System | |
| Lin et al. | A geo-aware and VRP-based public bicycle redistribution system | |
| CN107103753A (en) | Traffic time prediction system, traffic time prediction method, and traffic model establishment method | |
| US9292800B2 (en) | Statistical estimation of origin and destination points of trip using plurality of types of data sources | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| CN113899383B (en) | Multi-vehicle deadlock prevention method, system, equipment and storage medium based on short path | |
| CN113865589B (en) | Long-distance rapid path planning method based on terrain gradient | |
| CN116136417A (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN108776668B (en) | Path estimation method, system, equipment and storage medium based on road network nodes | |
| CN114120635B (en) | Tensor decomposition-based urban road network linear missing flow estimation method and system | |
| US20150127302A1 (en) | Method and apparatus for optimized routing | |
| US20130218465A1 (en) | Navigation system | |
| US20220300002A1 (en) | Methods and systems for path planning in a known environment | |
| Gochev et al. | Anytime tree-restoring weighted A* graph search | |
| Chen et al. | Research on the shortest path Analysis method in Complex Traffic Environment Based on GIS | |
| CN113701768A (en) | Path determination method and device and electronic equipment | |
| Duan et al. | POP: a passenger-oriented partners matching system | |
| Makulavičius et al. | E-tri: E-vehicle testbed routing infrastructure | |
| CN112465318B (en) | Task allocation method for formation of heterogeneous underwater vehicles | |
| Li et al. | A Depth First Search Algorithm for Optimal Triangulation of Bayesian Networks | |
| Jun et al. | Time-constrained shortest path search algorithm for urban road network |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder | ||
| CP01 | Change in the name or title of a patent holder |
Address after: Room 503-3, 398 Jiangsu Road, Changning District, Shanghai 200050 Patentee after: Shanghai Xijing Technology Co.,Ltd. Address before: Room 503-3, 398 Jiangsu Road, Changning District, Shanghai 200050 Patentee before: SHANGHAI WESTWELL INFORMATION AND TECHNOLOGY Co.,Ltd. |