CN116465425A - Heuristic path planning method for local optimization and bidirectional calculation - Google Patents
Heuristic path planning method for local optimization and bidirectional calculation Download PDFInfo
- Publication number
- CN116465425A CN116465425A CN202310523302.6A CN202310523302A CN116465425A CN 116465425 A CN116465425 A CN 116465425A CN 202310523302 A CN202310523302 A CN 202310523302A CN 116465425 A CN116465425 A CN 116465425A
- Authority
- CN
- China
- Prior art keywords
- node
- path
- shortest path
- heuristic
- path planning
- 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
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000004364 calculation method Methods 0.000 title claims abstract description 23
- 230000002457 bidirectional effect Effects 0.000 title claims abstract description 17
- 238000005457 optimization Methods 0.000 title claims abstract description 16
- 230000008859 change Effects 0.000 claims abstract description 11
- 230000008569 process Effects 0.000 claims description 6
- 239000004973 liquid crystal related substance Substances 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical group [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 description 1
- 239000010931 gold Substances 0.000 description 1
- 229910052737 gold Inorganic materials 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- KDYFGRWQOYBRFD-UHFFFAOYSA-N succinic acid Chemical compound OC(=O)CCC(O)=O KDYFGRWQOYBRFD-UHFFFAOYSA-N 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Abstract
The invention discloses a heuristic path planning method for local optimization and bidirectional calculation, which comprises the following steps of structuring path data, finding out the shortest path by calculating the current node starting function and the weight from the current node to the end point, and completing initial path planning; judging whether the shortest path changes in real time, if so, dividing the changed shortest path part, setting endpoints, checking all nodes affected by the change, establishing parent-child node relation, searching and comparing child nodes corresponding to the parent node s' to find out child nodes of the optimal path, and calculating the node starting distance g * (s); calculating a predicted value rhs(s) based on the g (s ') value of the parent node s'; find g * (s) the same node as rhs(s) as the shortest path node; and the intersected shortest path node is found to be used as a connection path point, and the initial path planning is updated, so that the real-time path planning efficiency is improved.
Description
Technical Field
The invention relates to the technical field of geographic information development, in particular to a heuristic path planning method for local optimization and bidirectional calculation.
Background
Path planning on maps is an important topic in many industries, especially in robots, GPS navigation and electronic maps. Real-time path planning is somewhat challenging for a number of reasons. First, some planning methods must deal with the effects of dynamic environmental changes. For example, traffic jam may occur when accidents occur on roads or traffic flow is large, and path change is affected. Second, the path planning algorithm must be as fast as possible. For example, the original journey should be changed for various emergency situations during the driving on the road, and the route needs to be re-planned in a short time, otherwise, the situation of repeated planning is easy to happen.
The invention discloses a heuristic path searching method based on A, which is to integrate local optimization and bidirectional calculation, wherein the original A is to add a heuristic function on the basis of Dijkstra algorithm, and find out the shortest path by calculating the distance weight from the current node to the starting point and the estimated cost from the current node to the end point. The heuristic search method is expected to find the shortest path of the path planning problem faster than the non-information search method, and the target direction can be determined faster through the heuristic search instead of the non-directional traversal search.
Disclosure of Invention
The invention aims to provide a heuristic path planning method for local optimization and bidirectional calculation.
The aim of the invention can be achieved by the following technical scheme:
a heuristic path planning method for local optimization and bidirectional computation comprises the following steps:
s1, structuring path data, constructing an adjacent matrix, and setting the distance between path nodes as a weight value;
s2, integrating the priority f (S) of the current node S by calculating the heuristic function g (S) of the current node S and the weight h (S) from the current node S to the terminal point, traversing all path nodes, finding out the shortest path according to the priority, and completing initial path planning;
s3, judging whether the shortest path is changed in real time, and executing a step S4 if the shortest path is changed;
s4, dividing the shortest path part with change, setting end points, checking all nodes affected by the change, establishing parent-child node relation, searching and comparing child nodes corresponding to the parent node S' to find child nodes of the optimal path, and calculating the node starting distance g * (s)；
S5, calculating a predicted value rhs (S) based on the g (S ') value of the parent node S';
s6, finding g * (s) the same node as rhs(s) as the shortest path node;
and S7, searching from two end point directions by utilizing a bidirectional calculation mode, finding out the intersected shortest path node as a connection path point, and updating the initial path planning.
As a further scheme of the invention: in step S2, route planning is performed by adopting a bidirectional calculation manner when the initial route planning is completed.
As a further scheme of the invention: in step S4, node start distance g * The calculation formula of(s) is as follows:
wherein pred(s) is the parent node, d (s ', s) is the distance connecting s' and s, g * (s') parent node start distance.
As a further scheme of the invention: in step S5, the calculation formula of the predicted value rhs (S) is as follows:
wherein g (s') is a parent node heuristic.
As a further scheme of the invention: the process of judging whether the current node can be used as the shortest path node comprises the following steps:
setting the weight of the current point s as k(s), wherein k(s) comprises k 1(s) and k 2(s);
wherein: k (k) 1 (s) is a heuristic function, k, from the current point s to the start and end points 2 (s) is the distance weight from the current point s to the starting point,
judging whether k(s) is less than or equal to k (s'), if yes, enabling the current node to serve as a shortest path node, otherwise, enabling the current node not to serve as the shortest path node;
wherein, the liquid crystal display device comprises a liquid crystal display device,
the invention has at least one of the following beneficial effects:
(1) Structuring path information and dividing path weights by combining actual conditions;
(2) Compared with the traditional path algorithm, the method has directivity, combines a bidirectional calculation mode, and improves the path searching efficiency;
(3) The invention carries out real-time adjustment on the shortest path by comparing the weights of the father node and the child node, carries out local re-planning on the path which changes each time, multiplexes the last search result of part and improves the efficiency of planning the path in real time.
Drawings
The invention is further described below with reference to the accompanying drawings.
FIG. 1 is a flow chart of a heuristic path planning method of the present invention for local optimization and bi-directional computation;
FIG. 2 is a schematic diagram of a heuristic path planning method of the present invention for local optimization and bi-directional computation;
FIG. 3 is a relationship of the off heuristic functions of parent and child nodes of the present invention;
fig. 4 is a schematic diagram of a bi-directionally computed shortest path of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Referring to fig. 1-4, the present invention is a heuristic path planning method for local optimization and bidirectional computation, which divides and constructs road data, sets path length as path weight, optimizes the weight relationship of nodes by establishing parent-child relationships among nodes on the basis of heuristic path planning, thereby finding out the next node on the shortest path more quickly, and improving the planning efficiency of the shortest path by adopting a bidirectional computation mode.
The method comprises the following steps:
s1, structuring path data, constructing an adjacent matrix, and setting the distance between path nodes as a weight value;
s2, integrating the priority f (S) of the current node S by calculating the heuristic function g (S) of the current node S and the weight h (S) from the current node S to the terminal point, traversing all path nodes, finding out the shortest path according to the priority, and completing initial path planning. In initial path planning, the present invention uses an a-algorithm to calculate each node s x by the following function s ,y s ]The weight calculation process of the a-algorithm is as follows: f(s) =g(s) +h(s).
Where f(s) is the integrated priority of node s. When we choose the next node to traverse, we always choose the node with the highest comprehensive priority (the smallest weight of the function calculation).
g(s) is the weight of the node s from the start point, and is related to the distance.
h(s) is the predicted weight of the node s from the endpoint, which is a heuristic function of the a-algorithm.
In the operation process, the node with the smallest f(s) value (highest priority) is selected from the priority queue each time and used as the next node to be traversed. In addition, the algorithm a uses two sets to represent the node to be traversed, and the node to be traversed is open_set and close_set respectively, so as to avoid repeated traversing of the same node, and the heuristic function in the invention can be represented by Euclidean distance, wherein the current node is s [ x ] s ,y s ]The target node is gold [ x ] g ,y g ]The formula is as follows:
specifically, in step S2, route planning is performed by adopting a bidirectional calculation manner when the initial path planning is completed.
S3, judging whether the shortest path is changed in real time, and executing a step S4 if the shortest path is changed;
s4, dividing the shortest path part with change, setting end points, checking all nodes affected by the change, establishing parent-child node relation, searching and comparing child nodes corresponding to the parent node S' to find child nodes of the optimal path, and calculating the node starting distance g * (s)。
It should be explained that, except the start node and the target node, the present invention sets a previous generation (parent) node pred(s) and a next generation (child) node succ(s) for each node s, wherein the node with any one edge pointing to s is the previous generation of s;
any node that leads from s to an edge is a successor node to s. In the following description, these two terms refer only to the immediate parent (the previous generation) and offspring (the next generation), and not to the parent of the parent or the offspring of the offspring. FIG. 3 is a graph of the relationship of the parent and child nodes.
Specifically, the shortest path occursWhen the path is changed, real-time path planning is carried out, the shortest path of the changed part is searched again, the father-son node relation is established, the son node of the optimal path is found out by searching and comparing the son nodes corresponding to the father node, and the node starting distance g is calculated * (s). The invention maintains two estimated values for the starting distance g(s) of each node, and the estimated weight value is 0 when s is the starting point: g * (s) is the initial distance from the initial point to the target point, g(s) is the target point * The estimated weights of(s) will be closer to g(s) as the calculation proceeds.
Wherein pred(s) is the parent node, d (s ', s) is the distance connecting s' and s, g * (s') parent node start distance.
S5, calculating a predicted value rhs (S) based on the g (S ') value of the parent node S';
specifically, in step S5, the calculation formula of the predicted value rhs (S) is as follows:
wherein g (s') is a parent node heuristic. g * (s) is continuously changed along with the algorithm solving process, when the distance from the starting point to the target is the same as the estimated weight, i.e. g(s) =g * When(s), the value of g(s) is the actual shortest distance to the starting point, and rhs(s) =g(s).
S6, finding g * (s) the same node as rhs(s) as the shortest path node;
specifically, step 4 and step 5 are repeated, and g is compared * Values of(s) and rhs(s) up to g * (s) is the same as rhs(s), and the estimated value g(s) is the same as the starting distance g * (s) are identical, thereby finding the shortest path node.
The shortest path searching mode is optimized by establishing a relationship between father and child nodes and a heuristic function, wherein the relationship establishing mode comprises the steps of calculating the distance weights of a starting point and an ending point through a current point, and calculating and comparing reference weights when the nodes are selected. The comparison mode of the weights is as follows:
the weight of the current point s is set to be k(s), and the current point s is divided into two types of k 1(s) and k 2(s), and the smaller the k(s), the more suitable the node is as a shortest path.
k(s)＝[k1(s)；k2(s)]
Wherein:
k 1 (s) is a heuristic function, k, from the current point s to the start and end points 2 (s) is the distance weight from the current point s to the starting point. The next node judgment criterion of whether the node s is the shortest path is as follows:
it should be noted that, the two upper and lower rows in the formula may be satisfied, where the sum in the downlink formula is the sum, that is, the sum needs to be satisfied simultaneously.
And S7, searching from two end point directions by utilizing a bidirectional calculation mode, finding out the intersected shortest path node as a connection path point, and updating the initial path planning. Specifically, route planning is performed from the start point and the end point of the map at the same time, and the intersection point is found to serve as a connecting path point on the basis of the calculation mode, so that path planning is completed. Setting open_set and close_set in two directions, wherein the first search is the same as the first search, the shortest path is searched from the end point and the start point respectively, the second search is performed on the nodes on the shortest path in a local optimization mode, the algorithm performs path planning again between the two endpoints by calculating the two endpoints of the changed path, and the path intersection point in the departure direction of the two endpoints is found to realize the shortest path planning. The manner of bi-directionally calculating the shortest path is shown in fig. 4.
It should be explained that the minimum value of all g (s ')+d (s ', s), where s ' [ x1, y1] is the parent of s [ x2, y2], and d (s ', s) is the cost of connecting s ' and s edges, the distance cost can be expressed in terms of Euclidean distance in an actual map navigation scenario.
For the actual shortest path length g * (s n ) With shortest path s 1 ,s 2 ,s 3 ,……s n The method comprises the following calculation processes:
for a start node start, the following is always true:
rhs(start)＝g(start)＝0
if rhs(s) is equal to g(s), then node s is said to be locally consistent. If all nodes are locally identical, the shortest path may be determined using an a-x algorithm. But when the edge cost changes, local agreement needs to be re-established with the nodes associated with the path.
An embodiment of the invention also specifically discloses a method example, which comprises the following steps:
(1) Setting path weight configuration, wherein the distance between path nodes is used as a weight value, and the structured map path is an adjacent matrix.
(2) And (3) performing path planning for the first time, setting a queue open_set to store nodes according to priority, calculating weights g(s) and h(s) from a path node s to a starting point and a finishing point, integrating the weights of the current nodes as f(s), and comparing the values of f(s) to find and store the optimal nodes.
(3) Performing a second and subsequent path planning, partitioning the affected shortest path portion, checking all nodes affected by the change, i.e. all nodes where one change edge ends (if one edge can traverse in both directions and the change affects both directions, checking the two nodes connected by the edge):
1) The rhs(s) value of the node is updated.
2) Nodes that have become locally consistent will be removed from the queue. Nodes that have become locally inconsistent will be added to the queue.
3) The keys of the nodes that remain locally inconsistent are updated. Thereafter, the node continues to expand until an end condition is reached.
(4) After node expansion is completed (i.e., the exit condition is satisfied), the shortest path is calculated. If the cost of the target is infinite, there is no finite cost path from the beginning to the target. Otherwise, the shortest path may be determined by moving backward.
From the target, move to the parent s ' of the current node s, where g (s ')+d (s ', s) is lowest (if the lowest score is shared by multiple nodes, each node is valid, any of which can be arbitrarily selected).
The previous step is repeated until intersecting the path searched for in the other direction.
The invention improves the searching efficiency of the path through a heuristic algorithm, sets the path weight, calculates the weights from the node to the starting point and the ending point to serve as the node evaluation index, searches the optimal node by comparing the weights between the father node and the child node, and completes the shortest path planning through a bidirectional path searching mode. The invention has the following advantages: (1) And structuring path information and dividing path weights by combining actual conditions. (2) Compared with the traditional path algorithm, the method has directivity, combines a bidirectional calculation mode, and improves the path searching efficiency. (3) The invention carries out real-time adjustment on the shortest path by comparing the weights of the father node and the child node, carries out local re-planning on the path which changes each time, multiplexes the last search result of part and improves the efficiency of planning the path in real time.
The foregoing describes one embodiment of the present invention in detail, but the description is only a preferred embodiment of the present invention and should not be construed as limiting the scope of the invention. All equivalent changes and modifications within the scope of the present invention are intended to be covered by the present invention.
Claims (5)
1. A heuristic path planning method for local optimization and bi-directional computation, comprising the steps of:
s1, structuring path data, constructing an adjacent matrix, and setting the distance between path nodes as a weight value;
s2, integrating the priority f (S) of the current node S by calculating the heuristic function g (S) of the current node S and the weight h (S) from the current node S to the terminal point, traversing all path nodes, finding out the shortest path according to the priority, and completing initial path planning;
s3, judging whether the shortest path is changed in real time, and executing a step S4 if the shortest path is changed;
s4, dividing the shortest path part with change, setting end points, checking all nodes affected by the change, establishing parent-child node relation, searching and comparing child nodes corresponding to the parent node S' to find child nodes of the optimal path, and calculating the node starting distance g * (s)；
S5, calculating a predicted value rhs (S) based on the g (S ') value of the parent node S';
s6, finding g * (s) the same node as rhs(s) as the shortest path node;
and S7, searching from two end point directions by utilizing a bidirectional calculation mode, finding out the intersected shortest path node as a connection path point, and updating the initial path planning.
2. The heuristic path planning method of local optimization and bi-directional computation according to claim 1, wherein in step S2, route planning is performed by bi-directional computation when initial path planning is completed.
3. The heuristic path planning method of local optimization and bi-directional computation according to claim 1, wherein in step S4, the node starting distance g * The calculation formula of(s) is as follows:
wherein pred(s) is the parent node, d (s ', s) is the distance connecting s' and s, g * (s') parent node start distance.
4. The heuristic path planning method of local optimization and bi-directional computation according to claim 1, wherein in step S5, the calculation formula of the predicted value rhs (S) is as follows:
wherein g (s') is a parent node heuristic.
5. The heuristic path planning method of local optimization and bi-directional computation of claim 1, wherein the process of determining whether the current node can act as a shortest path node comprises:
setting the weight of the current point s as k(s), wherein k(s) comprises k 1(s) and k 2(s);
wherein: l (L) 1 (s) is a heuristic function, k, from the current point s to the start and end points 2 (s) is the distance weight from the current point s to the starting point,
judging whether k(s) is less than or equal to k (s'), if yes, enabling the current node to serve as a shortest path node, otherwise, enabling the current node not to serve as the shortest path node;
wherein, the liquid crystal display device comprises a liquid crystal display device,
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310523302.6A CN116465425A (en) | 2023-05-10 | 2023-05-10 | Heuristic path planning method for local optimization and bidirectional calculation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310523302.6A CN116465425A (en) | 2023-05-10 | 2023-05-10 | Heuristic path planning method for local optimization and bidirectional calculation |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116465425A true CN116465425A (en) | 2023-07-21 |
Family
ID=87177101
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310523302.6A Pending CN116465425A (en) | 2023-05-10 | 2023-05-10 | Heuristic path planning method for local optimization and bidirectional calculation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116465425A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117017488A (en) * | 2023-10-10 | 2023-11-10 | 华中科技大学同济医学院附属协和医院 | Puncture arm path planning method comprising non-autonomous motion compensation |
-
2023
- 2023-05-10 CN CN202310523302.6A patent/CN116465425A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117017488A (en) * | 2023-10-10 | 2023-11-10 | 华中科技大学同济医学院附属协和医院 | Puncture arm path planning method comprising non-autonomous motion compensation |
| CN117017488B (en) * | 2023-10-10 | 2024-01-09 | 华中科技大学同济医学院附属协和医院 | Puncture arm path planning method comprising non-autonomous motion compensation |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US5272638A (en) | Systems and methods for planning the scheduling travel routes | |
| CN109506669B (en) | Dynamic path planning method, device, system and storage medium | |
| US20090125229A1 (en) | Corridor mapping with alternative routes | |
| JP5661782B2 (en) | Additional map generation, refinement and expansion using GPS trajectories | |
| EP2075537B1 (en) | Navigation apparatus and program | |
| CN103544291A (en) | Mobile object continuous k-nearest neighbor (CKNN) query method based on road based road networks tree (RRN-Tree) in road network | |
| CN105758410A (en) | Method for quickly planning and mixing paths on basis of A-star algorithms | |
| WO2018033957A1 (en) | Route searching method and route searching device | |
| JP6298322B2 (en) | Route search device, route search method and program | |
| CN116465425A (en) | Heuristic path planning method for local optimization and bidirectional calculation | |
| CN104949678A (en) | Method and device for determining navigation end point in navigation system, and navigation equipment | |
| CN103256934A (en) | Road information providing apparatus | |
| CN107917716A (en) | Fixed circuit air navigation aid, device, terminal and computer-readable recording medium | |
| CN113295177B (en) | Dynamic path planning method and system based on real-time road condition information | |
| JP6912859B2 (en) | Map update device, map update method, computer program, and recording medium on which the computer program is recorded. | |
| US20210333112A1 (en) | Route search system and route search program | |
| JP5132694B2 (en) | DATA GENERATION DEVICE, DATA GENERATION METHOD, AND ROUTE SEARCH DEVICE | |
| CN115183789A (en) | Navigation route determination method and device | |
| CN114383617A (en) | Navigation route calculation method, device, equipment and storage medium based on map neural network | |
| JP5288239B2 (en) | Route search method | |
| JPH0527679A (en) | Route search method of on-vehicle navigation system and map data storage medium | |
| Hedderich et al. | Optimization of a Park Spot Route based on the A* Algorithm | |
| JPH0914985A (en) | Method and device for searching route | |
| JP6307270B2 (en) | Route search device | |
| JP4677767B2 (en) | Navigation device and information presentation method |
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 |