CN113494926A - Path finding method, device and equipment - Google Patents
Path finding method, device and equipment Download PDFInfo
- Publication number
- CN113494926A CN113494926A CN202111039750.6A CN202111039750A CN113494926A CN 113494926 A CN113494926 A CN 113494926A CN 202111039750 A CN202111039750 A CN 202111039750A CN 113494926 A CN113494926 A CN 113494926A
- Authority
- CN
- China
- Prior art keywords
- node
- current node
- alternative
- current
- candidate
- 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.)
- Pending
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
Abstract
The application provides a way-finding method, which comprises the following steps: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path. By excluding negative nodes by a priori factors before performing priority calculation, calculation time is saved, and the chance of reverse traveling of a way-finding result is reduced. The application also provides a way finding device and equipment.
Description
Technical Field
The present application relates to a path optimization technology, and in particular, to a path finding method. The application also provides a way finding device and equipment.
Background
The way-finding algorithm is widely applied in actual life, and the most basic application is to plan the optimal path in the city. The dijkstra algorithm and the a algorithm are commonly used in the current routing algorithm.
The Dijkstra algorithm specifically operates by setting an S set in which the shortest path has been solved and a U set in which the shortest path has not been solved. And acquiring a map with fine labels, and selecting a starting point of a path from the map. And putting the starting point into the S set, and putting all the other nodes into the set U. And (4) setting the last solved node in the S set as S, if the node in the U set is connected with S, recording the connecting edge distance as the recording distance, and recording the node distance which is not connected with S as infinity.
Selecting the node with the shortest distance S from the U as x, adding the node into the S, and removing the x from the U;
searching a direct connection point by taking x as a current node, calculating connection weight, and updating the node distance in the U;
and repeating the steps until all the nodes are traversed, and searching the distance value of the target node from the S at the moment, wherein the distance is the shortest distance from the starting point to the target node.
The operation step of the A-algorithm is to carry out cost estimation on the current state to the target state by combining the known information and the initial state.
Firstly, an OPEN table and a CLOSE table are set, wherein the OPEN table records nodes which are not subjected to traversal operation, and the CLOSE table records nodes which are traversed.
After initializing the OPEN table and the CLOSE table, the start node of the path is placed in the OPEN table, and only one initial node is in the OPEN table.
The smallest head node, i.e. the node that was added last, is taken from the OPEN table. At the beginning, the OPEN table only has a starting node, so the starting node is taken out as the current node, the father node of the starting node is added into the CLOSE node, and if no father node exists, the father node is not added.
Deleting the current node from the OPEN table, judging whether the current node is the terminal point, and finding the optimal path from the starting point to the terminal point through the parent pointer of the current node if the current node is the terminal point.
If not, searching the current node connection point excluding the node in the CLOSE table, judging whether the connection point is in the OPEN table, if not, pointing the father pointer of the connection point to the current node, and putting the father pointer into the OPEN table, if so, calculating a first distance from the connection point to the father node of the connection point and a second distance from the connection point to the father node of the connection point through the current node. And if the first distance is greater than the second distance, pointing the parent pointer of the connection point to the current node.
And repeating the steps until the end point is found, and then finding the optimal path between the starting point and the end point according to the parent pointer.
In the above two optimal path planning methods, the following problems also exist:
1. the Dijkstra algorithm is applied to a map with few path nodes, and can accurately find the optimal road stiffness, but in some maps with many path nodes, the calculation amount is greatly increased due to huge data amount, and the Dijkstra algorithm cannot adapt to real-time calculation.
2. The a-x algorithm cannot provide an estimate of all factors when the map is complex, and cannot estimate the optimal path.
Disclosure of Invention
The application discloses a path searching method, a path searching device and path searching equipment, which are used for solving the problems that the path searching calculation is delayed, so that the real-time path searching cannot be carried out and the path searching error is overlarge.
The application provides a way-finding method, which comprises the following steps:
step one, defining a path starting point as a current node;
step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node;
step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node;
and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path.
Optionally, obtaining the prior factor includes:
determining a first connection line of the current node pointing to the alternative node according to the positions of the current node and the alternative node;
and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
Optionally, the method includes:
adding the traversed current node to a CLOSE table, and judging whether the alternative node is in an OPEN table;
if not, the father pointer of the alternative node points to the current node and is added into the OPEN table;
if yes, calculating a first distance between the candidate node and a father node of the candidate node and a second distance between the candidate node and the father node of the candidate node through the current node, and if the first distance is larger than the second distance, pointing a father pointer of the candidate node to the current node.
Optionally, the calculating the priority of the candidate node includes:
calculating a first optimal step number from the alternative node to a terminal point and a second optimal step number from the alternative node to a current node;
and taking the sum of the first optimal step number and the second optimal step number as a priority parameter.
Optionally, the priority parameter further includes a traffic light factor:
calculating a traffic light factor according to the number of traffic lights between the current node and the alternative node, the waiting time of the traffic lights and the traveling speed on the path, wherein the formula is as follows:
Optionally, the position information of the starting point is obtained according to a node ID of the starting point, and the node ID is recorded in an intersection information table and a road information table.
Optionally, the returning the optimal path includes: return road ID and lane ID.
Optionally, returning the optimal path further includes: and path finding failure information.
The present application further provides a way finding device, including:
the defining module is used for defining the starting point of the path as the current node;
the traversal module is used for judging whether the current node is the end point or not, and if not, traversing the new connection node of the current node to obtain an alternative node;
the updating module is used for calculating the priority of the alternative node after eliminating the negative node according to the prior factor, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node into a new current node;
and the result module is used for repeating the operation steps of the traversal module and the updating module until the current node is judged to be the end point, and taking a path traced back to the starting point by the end point according to the direction of the parent pointer as an optimal path.
Optionally, the update module further includes:
a priori factor unit, configured to determine, according to the positions of the current node and the candidate node, a first connection line that the current node points to the candidate node;
and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
Optionally, the update module further includes:
the resetting unit is used for adding the traversed current node to a CLOSE table and judging whether the alternative node is in an OPEN table;
if not, the father pointer of the alternative node points to the current node and is added into the OPEN table;
if yes, calculating a first distance between the candidate node and a father node of the candidate node and a second distance between the candidate node and the father node of the candidate node through the current node, and if the first distance is larger than the second distance, pointing a father pointer of the candidate node to the current node.
Optionally, the update module further includes:
the priority computing unit is used for computing a first optimal step number from the candidate node to a terminal point and a second optimal step number from the candidate node to a current node;
and taking the sum of the first optimal step number and the second optimal step number as a priority parameter.
Optionally, the update module further includes:
a traffic light factor unit, configured to calculate a traffic light factor according to the number of traffic lights between the current node and the candidate node, the waiting time of the traffic lights, and the traveling speed on the path, where the formula is as follows:
The present application further provides a way-finding device, including:
a display for displaying the optimal path;
a memory; used for storing the way-finding program;
a processor for executing a retrieve routing program from the memory, comprising: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path.
The advantages of the present application over the prior art are as follows:
the application provides a way-finding method, which comprises the following steps: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path. By excluding negative nodes by a priori factors before performing priority calculation, calculation time is saved, and the chance of reverse traveling of a way-finding result is reduced.
Drawings
FIG. 1 is a flow chart of a way-finding method in the present application.
Fig. 2 is a flow chart of alternative node screening in the present application.
Fig. 3 is a schematic diagram of a traffic light factor optimization path according to the present application.
Fig. 4 is a schematic diagram of a way-finding device in the present application.
Detailed Description
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present application. This application is capable of implementation in many different ways than those herein set forth and of similar import by those skilled in the art without departing from the spirit of this application and is therefore not limited to the specific implementations disclosed below.
The application mainly provides a way-finding method, which comprises the following steps: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path. And the negative nodes are eliminated through the prior factors, so that the calculation amount is reduced, and the path searching response delay and the path searching error are effectively reduced while the optimal result is ensured.
For clarity of description of the aspects of the present application, reference is made to the following detailed description taken in conjunction with the accompanying drawings.
FIG. 1 is a flow chart of a way-finding method in the present application.
Referring to fig. 1, step S101 is to define a path starting point as a current node;
the starting point is a starting point from a certain place and another place, and the starting point is actually road traffic data which is road and road network data stored in an OpenDrive file.
Reading Opendrive data from the Opendrive file, analyzing the Opendrive data by an XML file, and storing the analyzed data into a database. According to the method and the device, Road intersection data and Road information data in the Opendrive data are recorded in a Junction table and a Road table respectively, the Road intersection is the intersection of the Road, and the Road information comprises information such as signal lamps and lanes on the Road.
The node has a unique ID, the starting point and the end point are extracted according to the node ID, and the ID records the coordinates of the starting point and the end point.
And after the starting point is obtained, defining the starting point as a current node.
S102, judging whether the current node is an end point or not, and traversing a new connection node of the current node to obtain an alternative node if the current node is not the end point;
after the information of the point and the end point is acquired, a routing algorithm is called to execute routing calculation from the starting point to the end point.
First, an OPEN table in which non-traversed nodes are recorded and a CLOSE table in which traversed nodes are recorded need to be initialized in advance.
After the initialization is completed, the start point is added to the OPEN table, which is just completed by the initialization, so only the start point.
And judging whether the OPEN table is empty or not, wherein the OPEN table is not empty because the starting point is added, and then taking the starting point out of the OPEN table and loading the starting point into the CLOSE table, namely taking the current node out of the OPEN table and loading the current node into the CLOSE table.
In another scenario, no node exists in the OPEN table, that is, the OPEN table is empty, and at this time, a path finding failure message is returned, whereas if there are multiple nodes, the node to be extracted is a head node, and the head node, that is, a parent node, is known and a child node is unknown.
When the current node is loaded into the CLOSE table, indicating that the current node has or is undergoing a traversal process, the current node is removed from the OPEN table and then subsequently processed.
And the current node stores the nodes in a CLOSE table, then traverses the current node, finds each node connected with the current node, and takes all traversed nodes as alternative nodes, wherein the alternative nodes do not contain the nodes loaded in the CLOSE table.
S103, calculating the priority of the alternative node after eliminating the negative node according to the prior factor, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node;
the above steps have selected the alternative nodes connected with the current node, and all the alternative nodes can be used as child nodes of the current node.
It should be clear to those skilled in the art that an optimal path is usually impossible to connect in opposite directions to each other, and the candidate node in the opposite direction is the negative node. Therefore, in the first step of the method, an included angle between a first connecting line of the candidate node and the current node and a second connecting line of the current node and the terminal point is used as a screening condition to screen the candidate node. Determining a first connection line of the current node pointing to the alternative node according to the positions of the current node and the alternative node; and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
Fig. 2 is a flow chart of alternative node screening in the present application.
Referring to fig. 2, the current node 2 is connected to the candidate node 1, the candidate node 3, and the candidate node 4 to form a first connection line q, a first connection line w, and a first connection line e, respectively. A second connection r between said current node 2 and the end point 7. Respectively calculating included angles of the second connecting line r, the first connecting line q, the first connecting line w and the first connecting line e, wherein the formula is as follows:
wherein A is the current node 2, B is an alternative node 1, 3 or 4, E is the end point 7,
is the angle between the two connecting lines,
is the angle between the connecting line of the two points and the X axis.
And after calculating the included angle between the connecting line of the current node 2 and the alternative node 1, 3 or 4 and the connecting line of the current node 2 and the terminal 7, screening the alternative node 1, 3 or 4 according to the included angle.
And deleting the corresponding alternative node 1 according to the rule of the prior factor, wherein the angle between the relevant alternative node 1 and the alternative node 3 is less than 90 degrees, the alternative node is reserved, and the next step of processing is carried out.
Through the steps, the application already carries out preliminary screening on the alternative nodes and screens out the alternative nodes capable of being processed in the next step, so that the calculation amount of the alternative nodes is greatly reduced.
And calculating the priority of the alternative nodes, namely selecting the nodes of the optimal path from the current node to the end point.
It has already been mentioned above that the current node is already loaded in the CLOSE table, and it is then determined whether the candidate node is in the OPEN table. Specifically, if the current node is a starting point, the parent node of each candidate node is the current node, if the current node is converted from a candidate node, the candidate node connected to the current node may be also connected to the previous current node, and if the current node is converted from a candidate node, the candidate node is already loaded in the OPEN table.
If the candidate node is not in the OPEN table, it can be determined that the candidate node is directly connected to the current node, and the parent pointer of the candidate node is pointed to the current node, and the candidate node is loaded into the OPEN table.
If the candidate node is in the OPEN table, it can be determined that the candidate node and the parent node thereof are connected in two ways. One is that the candidate node is connected with its father node through the current node, and the distance from the candidate node to the current node and the sum of the distance from the current node to the father node of the candidate node are marked as a second distance. And secondly, the alternative node is directly connected with the father node, and the distance from the alternative node to the father node is marked as a first distance.
And judging the relationship between the first distance and the second distance, and if the first distance is greater than the second distance, replacing the father node of the alternative node with the current node.
Through the above setting, all father nodes of the candidate nodes traversed by the current node are set as the current node, and then priority judgment is performed, including: and calculating a first optimal step number from the candidate node to the terminal point and a second optimal step number from the candidate node to the current node.
The first optimal step number is the shortest distance from the current node to the candidate node or the minimum step number, and the second optimal step is the shortest distance from the candidate node to the end point or the minimum step number. And adding the first optimal step number and the second optimal step number to obtain a result which is a priority parameter, wherein the candidate node with the lowest priority is the candidate node with the highest priority.
The optimal distance or the step number is calculated and obtained through the Manhattan distance, and the formula is as follows:
wherein, the
Is a priority parameter, said
And
the distribution represents the distance of the candidate node to the current node and the end point.
On the other hand, the present application needs to optimize the priority, so that the reflection of the road condition information can be actually integrated by the priority.
The waiting time of the traffic lights is integrated into the priority judgment. One or more traffic lights may be arranged on a road, and each traffic light may need a certain waiting time, so the number of the traffic lights and the waiting time actually have important influence on traveling, and the influence needs to be judged according to the traveling speed.
Fig. 3 is a schematic diagram of a traffic light factor optimization path.
Please refer to fig. 3. A traffic light i is arranged on a path from the current node 2 to the alternative node 4, a traffic light o is arranged from the alternative node 4 to the node 5, and a traffic light p is arranged from the node 6 to the terminal point 7. And optimizing according to the traffic light, wherein the obtained path is the path from the current node to the alternative node 3, then to the node 5 and then to the terminal 7.
The factor of the traffic light response priority is referred to as a traffic light factor in the application, and the calculation method of the traffic light factor is as follows:
wherein the content of the first and second substances,
is a traffic light factor, said
Is the product of the waiting time and the travel speed of the traffic light, said
Is the number of traffic lights between the current node and the candidate node.
And directly adding the traffic light factor and the Manhattan distance to obtain the following formula:
wherein
Is an optimized priority parameter, said
Comprising the formula
。
Through the formula, the candidate node with the highest priority can be obtained, and the selected candidate node is set as the current node.
And S104, repeating the second step and the third step until the current node is judged to be the end point, and taking the path traced back from the end point to the starting point according to the direction of the parent pointer as an optimal path.
And then, taking out the current node from the OPEN table, judging whether the current node is an end point, repeating the steps of S102-S103 until the current node is judged to be the end point, and explaining a route searching process.
And the end point is patrolled to the starting point through the parent pointer, and the path traced back to the starting point by the end point according to the direction of the parent pointer is used as an optimal path. The information of the optimal path includes a road ID and a lane ID.
Corresponding to the path searching method, the application also provides a path searching device.
Fig. 4 is a schematic diagram of a way-finding device in the present application.
Referring to fig. 4, a defining module 401 is configured to define a path starting point as a current node;
the starting point is a starting point from a certain place and another place, and the starting point is actually road traffic data which is road and road network data stored in an OpenDrive file.
Reading Opendrive data from the Opendrive file, analyzing the Opendrive data by an XML file, and storing the analyzed data into a database. According to the method and the device, Road intersection data and Road information data in the Opendrive data are recorded in a Junction table and a Road table respectively, the Road intersection is the intersection of the Road, and the Road information comprises information such as signal lamps and lanes on the Road.
The node has a unique ID, the starting point and the end point are extracted according to the node ID, and the ID records the coordinates of the starting point and the end point.
And after the starting point is obtained, defining the starting point as a current node.
A traversal module 402, configured to determine whether the current node is an end point, and if not, traverse a new connection node of the current node to obtain an alternative node;
after the information of the point and the end point is acquired, a routing algorithm is called to execute routing calculation from the starting point to the end point.
The traversal module 402 comprises:
first, an OPEN table in which non-traversed nodes are recorded and a CLOSE table in which traversed nodes are recorded need to be initialized in advance.
After the initialization is completed, the start point is added to the OPEN table, which is just completed by the initialization, so only the start point.
And judging whether the OPEN table is empty or not, wherein the OPEN table is not empty because the starting point is added, and then taking the starting point out of the OPEN table and loading the starting point into the CLOSE table, namely taking the current node out of the OPEN table and loading the current node into the CLOSE table.
In another scenario, no node exists in the OPEN table, that is, the OPEN table is empty, and at this time, a path finding failure message is returned, whereas if there are multiple nodes, the node to be extracted is a head node, and the head node, that is, a parent node, is known and a child node is unknown.
When the current node is loaded into the CLOSE table, indicating that the current node has or is undergoing a traversal process, the current node is removed from the OPEN table and then subsequently processed.
And the current node stores the nodes in a CLOSE table, then traverses the current node, finds each node connected with the current node, and takes all traversed nodes as alternative nodes, wherein the alternative nodes do not contain the nodes loaded in the CLOSE table.
An updating module 403, configured to calculate the priority of the candidate node after excluding the negative node according to the prior factor, point the parent pointer of the candidate node with the highest priority to the current node, and update the candidate node as a new current node;
the above steps have selected the alternative nodes connected with the current node, and all the alternative nodes can be used as child nodes of the current node.
It should be clear to those skilled in the art that an optimal path is usually impossible to connect in opposite directions to each other, and the candidate node in the opposite direction is the negative node. Therefore, the update module of the present application further includes: a priori factor unit, configured to determine, according to the positions of the current node and the candidate node, a first connection line that the current node points to the candidate node; and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
The first step is to take the included angle between the first connecting line of the candidate node and the current node and the second connecting line of the current node and the terminal point as the screening condition to screen the candidate nodes. Determining a first connection line of the current node pointing to the alternative node according to the positions of the current node and the alternative node; and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
As shown in fig. 2, the current node 2 is connected to the candidate node 1, the candidate node 3, and the candidate node 4 to form a first connection line q, a first connection line w, and a first connection line e, respectively. A second connection r between said current node 2 and the end point 7. Respectively calculating included angles of the second connecting line r, the first connecting line q, the first connecting line w and the first connecting line e, wherein the formula is as follows:
wherein A is the current node 2, B is an alternative node 1, 3 or 4, E is the end point 7,
is the angle between the two connecting lines,
is the angle between the connecting line of the two points and the X axis.
And after calculating the included angle between the connecting line of the current node 2 and the alternative node 1, 3 or 4 and the connecting line of the current node 2 and the terminal 7, screening the alternative node 1, 3 or 4 according to the included angle.
And deleting the corresponding alternative node 1 according to the rule of the prior factor, wherein the angle between the relevant alternative node 1 and the alternative node 3 is less than 90 degrees, the alternative node is reserved, and the next step of processing is carried out.
Through the steps, the application already carries out preliminary screening on the alternative nodes and screens out the alternative nodes capable of being processed in the next step, so that the calculation amount of the alternative nodes is greatly reduced.
And calculating the priority of the alternative nodes, namely selecting the nodes of the optimal path from the current node to the end point.
As mentioned above, the current node is already loaded in the CLOSE table, and the updating module 403 further includes:
the resetting unit is used for adding the traversed current node to a CLOSE table and judging whether the alternative node is in an OPEN table;
specifically, if the current node is a starting point, the parent node of each candidate node is the current node, if the current node is converted from a candidate node, the candidate node connected to the current node may be also connected to the previous current node, and if the current node is converted from a candidate node, the candidate node is already loaded in the OPEN table.
If not, the father pointer of the alternative node points to the current node and is added into the OPEN table. If the candidate node is not in the OPEN table, it can be determined that the candidate node is directly connected to the current node, and the parent pointer of the candidate node is pointed to the current node, and the candidate node is loaded into the OPEN table.
If yes, calculating a first distance between the candidate node and a father node of the candidate node and a second distance between the candidate node and the father node of the candidate node through the current node, and if the first distance is larger than the second distance, pointing a father pointer of the candidate node to the current node. If the candidate node is in the OPEN table, it can be determined that the candidate node and the parent node thereof are connected in two ways. One is that the candidate node is connected with its father node through the current node, and the distance from the candidate node to the current node and the sum of the distance from the current node to the father node of the candidate node are marked as a second distance. And secondly, the alternative node is directly connected with the father node, and the distance from the alternative node to the father node is marked as a first distance.
And judging the relationship between the first distance and the second distance, and if the first distance is greater than the second distance, replacing the father node of the alternative node with the current node.
The update module 403 further includes:
the priority computing unit is used for computing a first optimal step number from the candidate node to a terminal point and a second optimal step number from the candidate node to a current node;
setting all father nodes of the alternative nodes traversed by the current node as current nodes, and then judging the priority, wherein the judging comprises the following steps: and calculating a first optimal step number from the candidate node to the terminal point and a second optimal step number from the candidate node to the current node.
The first optimal step number is the shortest distance from the current node to the candidate node or the minimum step number, and the second optimal step is the shortest distance from the candidate node to the end point or the minimum step number. And adding the first optimal step number and the second optimal step number to obtain a result which is a priority parameter, wherein the candidate node with the lowest priority is the candidate node with the highest priority.
And taking the sum of the first optimal step number and the second optimal step number as a priority parameter. The optimal distance or the step number is calculated and obtained through the Manhattan distance, and the formula is as follows:
wherein, the
Is a priority parameter, said
And
the distribution represents the distance of the candidate node to the current node and the end point.
The update unit further includes:
and the traffic light factor unit is used for calculating the traffic light factor according to the number of the traffic lights between the current node and the alternative node, the waiting time of the traffic lights and the traveling speed on the path.
The waiting time of the traffic lights is integrated into the priority judgment. One or more traffic lights may be arranged on a road, and each traffic light may need a certain waiting time, so the number of the traffic lights and the waiting time actually have important influence on traveling, and the influence needs to be judged according to the traveling speed.
The factor of the traffic light response priority is referred to as a traffic light factor in the application, and the calculation method of the traffic light factor is as follows:
wherein the content of the first and second substances,
is a traffic light factor, said
Is the product of the waiting time and the travel speed of the traffic light, said
Is the number of traffic lights between the current node and the candidate node.
And directly adding the traffic light factor and the Manhattan distance to obtain the following formula:
wherein
Is an optimized priority parameter, said
Comprising the formula
。
Through the formula, the candidate node with the highest priority can be obtained, and the selected candidate node is set as the current node.
And a result module 404, configured to repeat the operation steps of the traversal module and the update module until the current node is determined to be an end point, and use a path traced back from the end point to the start point according to the direction of the parent pointer as an optimal path.
And then, taking out the current node from the OPEN table, judging whether the current node is an end point, repeating the processing of the modules 402-403 until the current node is judged to be the end point, and explaining a route searching process.
And the end point is patrolled to the starting point through the parent pointer, and the path traced back to the starting point by the end point according to the direction of the parent pointer is used as an optimal path. The information of the optimal path includes a road ID and a lane ID.
The present application further provides a way-finding device, including:
a display for displaying the optimal path;
a memory; used for storing the way-finding program;
a processor for executing a retrieve routing program from the memory, comprising: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path.
Claims (14)
1. A way-finding method, comprising:
step one, defining a path starting point as a current node;
step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node;
step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node;
and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path.
2. The way-finding method according to claim 1, wherein obtaining the prior factor comprises:
determining a first connection line of the current node pointing to the alternative node according to the positions of the current node and the alternative node;
and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
3. The way-finding method according to claim 2, characterized by comprising:
adding the traversed current node to a CLOSE table, and judging whether the alternative node is in an OPEN table;
if not, the father pointer of the alternative node points to the current node and is added into the OPEN table;
if yes, calculating a first distance between the candidate node and a father node of the candidate node and a second distance between the candidate node and the father node of the candidate node through the current node, and if the first distance is larger than the second distance, pointing a father pointer of the candidate node to the current node.
4. The way-finding method according to claim 2, wherein said calculating the candidate node priority comprises:
calculating a first optimal step number from the alternative node to a terminal point and a second optimal step number from the alternative node to a current node;
and taking the sum of the first optimal step number and the second optimal step number as a priority parameter.
5. The way-finding method according to claim 4, wherein the priority parameter further comprises a traffic light factor:
calculating a traffic light factor according to the number of traffic lights between the current node and the alternative node, the waiting time of the traffic lights and the traveling speed on the path, wherein the formula is as follows:
6. The way-finding method according to claim 1, wherein the position information of the starting point is obtained from a node ID of the starting point, the node ID being recorded in an intersection information table and a road information table.
7. The way-finding method according to claims 1-6, characterized in that returning the optimal path comprises: return road ID and lane ID.
8. The way-finding method according to claims 1-6, characterized in that returning the optimal path further comprises: and path finding failure information.
9. A way-finding device, comprising:
the defining module is used for defining the starting point of the path as the current node;
the traversal module is used for judging whether the current node is the end point or not, and if not, traversing the new connection node of the current node to obtain an alternative node;
the updating module is used for calculating the priority of the alternative node after eliminating the negative node according to the prior factor, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node into a new current node;
and the result module is used for repeating the operation steps of the traversal module and the updating module until the current node is judged to be the end point, and taking a path traced back to the starting point by the end point according to the direction of the parent pointer as an optimal path.
10. The way-finding device according to claim 9, wherein the updating module further comprises:
a priori factor unit, configured to determine, according to the positions of the current node and the candidate node, a first connection line that the current node points to the candidate node;
and calculating an included angle between the first connecting line and a second connecting line of the current node pointing to the terminal point, and taking the included angle larger than 90 degrees as a prior factor.
11. The way-finding device according to claim 10, wherein the updating module further comprises:
the resetting unit is used for adding the traversed current node to a CLOSE table and judging whether the alternative node is in an OPEN table;
if not, the father pointer of the alternative node points to the current node and is added into the OPEN table;
if yes, calculating a first distance between the candidate node and a father node of the candidate node and a second distance between the candidate node and the father node of the candidate node through the current node, and if the first distance is larger than the second distance, pointing a father pointer of the candidate node to the current node.
12. The way-finding device according to claim 10, wherein the updating module further comprises:
the priority computing unit is used for computing a first optimal step number from the candidate node to a terminal point and a second optimal step number from the candidate node to a current node;
and taking the sum of the first optimal step number and the second optimal step number as a priority parameter.
13. The way-finding device according to claim 12, wherein the updating module further comprises:
a traffic light factor unit, configured to calculate a traffic light factor according to the number of traffic lights between the current node and the candidate node, the waiting time of the traffic lights, and the traveling speed on the path, where the formula is as follows:
14. A way-finding device, comprising:
a display for displaying the optimal path;
a memory; used for storing the way-finding program;
a processor for executing a retrieve routing program from the memory, comprising: step one, defining a path starting point as a current node; step two, judging whether the current node is an end point, if not, traversing a new connecting node of the current node to obtain an alternative node; step three, calculating the priority of the alternative nodes after eliminating negative nodes according to prior factors, pointing the parent pointer of the alternative node with the maximum priority to the current node, and updating the alternative node to be a new current node; and step four, repeating the step two and the step three until the current node is judged to be the end point, and taking the path traced back to the starting point by the end point according to the direction of the parent pointer as the optimal path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111039750.6A CN113494926A (en) | 2021-09-06 | 2021-09-06 | Path finding method, device and equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111039750.6A CN113494926A (en) | 2021-09-06 | 2021-09-06 | Path finding method, device and equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113494926A true CN113494926A (en) | 2021-10-12 |
Family
ID=77997213
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111039750.6A Pending CN113494926A (en) | 2021-09-06 | 2021-09-06 | Path finding method, device and equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113494926A (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1480867A (en) * | 2002-10-16 | 2004-03-10 | 黄珏华 | Method for preparing electronic maps and display method |
| CN102506849A (en) * | 2011-09-28 | 2012-06-20 | 浙江大学 | Method for optimizing shortest path with restraint |
| CN102927990A (en) * | 2012-10-29 | 2013-02-13 | 苏州两江科技有限公司 | Method for determining urban road optimal path of automobile by geographic information system |
| CN103226581A (en) * | 2013-04-02 | 2013-07-31 | 浙江大学 | Heuristic shortest path search method based on direction optimization |
| CN104914862A (en) * | 2015-04-21 | 2015-09-16 | 电子科技大学 | Path planning algorithm based on target direction constraint |
| US20160035221A1 (en) * | 2014-04-29 | 2016-02-04 | Maxwell Consulting, LLC | Systems and Methods for Traffic Guidance Nodes and Traffic Navigating Entities |
| CN106530779A (en) * | 2016-12-29 | 2017-03-22 | 长沙理工大学 | Path planning method and system based on urban traffic control signal lights |
| CN113110507A (en) * | 2021-05-13 | 2021-07-13 | 北京鼎翰科技有限公司 | Path planning method for autonomous obstacle avoidance |
-
2021
- 2021-09-06 CN CN202111039750.6A patent/CN113494926A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1480867A (en) * | 2002-10-16 | 2004-03-10 | 黄珏华 | Method for preparing electronic maps and display method |
| CN102506849A (en) * | 2011-09-28 | 2012-06-20 | 浙江大学 | Method for optimizing shortest path with restraint |
| CN102927990A (en) * | 2012-10-29 | 2013-02-13 | 苏州两江科技有限公司 | Method for determining urban road optimal path of automobile by geographic information system |
| CN103226581A (en) * | 2013-04-02 | 2013-07-31 | 浙江大学 | Heuristic shortest path search method based on direction optimization |
| US20160035221A1 (en) * | 2014-04-29 | 2016-02-04 | Maxwell Consulting, LLC | Systems and Methods for Traffic Guidance Nodes and Traffic Navigating Entities |
| CN104914862A (en) * | 2015-04-21 | 2015-09-16 | 电子科技大学 | Path planning algorithm based on target direction constraint |
| CN106530779A (en) * | 2016-12-29 | 2017-03-22 | 长沙理工大学 | Path planning method and system based on urban traffic control signal lights |
| CN113110507A (en) * | 2021-05-13 | 2021-07-13 | 北京鼎翰科技有限公司 | Path planning method for autonomous obstacle avoidance |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109506669B (en) | Dynamic path planning method, device, system and storage medium | |
| US8681635B2 (en) | Computer-implemented systems and methods for planning a route | |
| US20170010101A1 (en) | Hybrid road network and grid based spatial-temporal indexing under missing road links | |
| CN113191550B (en) | Map matching method and device | |
| WO2021164708A1 (en) | Method and device for detecting map calibration error | |
| CN113155141A (en) | Map generation method and device, electronic equipment and storage medium | |
| WO2018058888A1 (en) | Street view image recognition method and apparatus, server and storage medium | |
| KR20170031745A (en) | Maintaining point of interest data using wireless access points | |
| CN111488414B (en) | Road task matching method, device and equipment | |
| CN111664864A (en) | Dynamic planning method and device based on automatic driving | |
| WO2023071029A1 (en) | Map data processing method and apparatus, and electronic device and storage medium | |
| CN113850996B (en) | Road section bus line repetition coefficient data calculation method and device and storage medium | |
| CN112307151B (en) | Navigation data processing method and device | |
| CN111380540A (en) | Map matching method and device, medium and terminal | |
| CN113867356A (en) | Robot path planning method and device and robot | |
| CN112269848A (en) | Crowdsourcing trajectory data fusion method and device | |
| CN113494926A (en) | Path finding method, device and equipment | |
| CN111666359A (en) | POI candidate arrival point mining method, device and equipment | |
| CN111858785A (en) | Method, device and system for matching discrete elements of map and storage medium | |
| CN107270919B (en) | Bus route grading method and device and bus route navigation method and device | |
| CN114323037A (en) | Road section position matching and navigation method, device and storage medium | |
| CN113048988B (en) | Method and device for detecting change elements of scene corresponding to navigation map | |
| CN113701768A (en) | Path determination method and device and electronic equipment | |
| CN105488192A (en) | Point cloud data K neighborhood search method | |
| CN112949358A (en) | Road description file generation method and equipment |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20211012 |
|
| RJ01 | Rejection of invention patent application after publication |