CN116465425A - Heuristic path planning method for local optimization and bidirectional calculation - Google Patents

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

Heuristic path planning method for local optimization and bidirectional calculation

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) ₁ (s) is a heuristic function, k, from the current point s to the start and end points ₂ (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 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 ₁ (s) is a heuristic function, k, from the current point s to the start and end points ₂ (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 ₁ ,s ₂ ,s ₃ ,……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) ₁ (s) is a heuristic function, k, from the current point s to the start and end points ₂ (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,,