CN114721370A - Robot rapid optimal path planning method based on double heuristic functions - Google Patents
Robot rapid optimal path planning method based on double heuristic functions Download PDFInfo
- Publication number
- CN114721370A CN114721370A CN202210202965.3A CN202210202965A CN114721370A CN 114721370 A CN114721370 A CN 114721370A CN 202210202965 A CN202210202965 A CN 202210202965A CN 114721370 A CN114721370 A CN 114721370A
- Authority
- CN
- China
- Prior art keywords
- node
- path
- heuristic
- target
- adjacent
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004364 calculation method Methods 0.000 claims description 12
- 230000009977 dual effect Effects 0.000 claims description 11
- 230000009191 jumping Effects 0.000 claims description 10
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000011156 evaluation Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000019637 foraging behavior Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000013139 quantization Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a robot rapid optimal path planning method based on double heuristic functions, two path searching modes are formulated, eight-neighborhood guided search can search a target node at the fastest speed and in the shortest path, when a large-range obstacle is encountered, four-neighborhood obstacle detouring search is adopted, the large-range obstacle can be rapidly detoured, and the searching range is effectively reduced. Meanwhile, in order to solve the problem of quickest and optimal, the invention introduces two heuristic functions, wherein the optimal path heuristic function is used for ensuring that a shorter path can be found, and the target guide heuristic function is used for guiding the path to expand towards the target direction, thereby reducing useless path attempts. And after the primary path is obtained, extracting inflection point information of the path, performing path re-planning among inflection points, optimizing the path with the minimum cost, and obtaining the shortest path from the initial node to the target node.
Description
Technical Field
The invention relates to the technical field of robot path planning, in particular to a robot rapid optimal path planning method based on double heuristic functions.
Background
Path planning techniques are an important branch of the field of robotic research. The path planning problem of the robot is to search a path which can avoid the obstacle from the initial state to the target state in the working space of the robot. The path planning can be divided into global path planning based on prior complete information and local path planning based on sensor information according to the degree of confidence of the environmental information. The planning mode may be random, or an optimal path from the initial state to the target state for avoiding the obstacle may be searched in the working space according to some optimal criteria (e.g., minimum working cost, shortest walking route, shortest walking time, etc.). Obviously, obstacle avoidance is a key problem in path planning, and meanwhile, as the moving speed of the robot is continuously improved and the cooperative motion of multiple robots is increasingly complex, the planning speed also becomes an important index for measuring the quality of a planning algorithm.
In the prior art, common path planning methods include a fast random search tree method, an artificial potential field method, an ant colony algorithm, an a-star algorithm based on a grid map, and the like. Wherein:
the fast random search tree method adopts a random path search mode, the search coverage is large, the obtained path is not the optimal path, the path is relatively tortuous, and the path is generally required to be smoothly operated.
The artificial potential field method is a virtual force method, which simulates the movement of an object under the repulsive force of an attractive force. The planned path is smooth and safe and is simple to describe, but the problem of local optimization exists, and the design of the gravitational field is complex and is the key for successful application of the algorithm.
The idea of the ant colony algorithm comes from the exploration of the foraging behavior of the ant colony, the algorithm achieves the aim by simulating the foraging behavior of the ant colony through iteration, has good global optimization capability, is easy to realize by a computer, but has large calculation amount and is easy to fall into a local optimal solution.
The grid map-based A-x algorithm represents a map by using coded grids, and marks grids containing obstacles as obstacle grids, and conversely, the grids are free grids. The method can obtain the optimal path under the guidance of the heuristic function, the algorithm is easy to realize by a computer, but the path is greatly influenced by the obstacles, so that the planning time is overlong.
Disclosure of Invention
In view of the above problems, the present invention aims to provide an optimal path planning method with small calculation amount, short planning time and easy implementation.
The purpose of the invention is realized by adopting the following technical scheme:
in a first aspect, the invention provides a robot fast optimal path planning method based on a double heuristic function, which comprises the following steps:
s1 defines a heuristic function, wherein the heuristic function comprises an optimal path heuristic function S (P, P)s) And a target-directing heuristic function G (P, P)g) Wherein the optimal path heuristic function S (P, P)s) Representing the minimum cost from the starting node to the current node, and is used for establishing a parent-child relationship between adjacent nodes; target guide heuristic function G (P, P)g) Representing heuristic evaluation cost from a current node to a target node, wherein the heuristic evaluation cost is used for guiding the path to expand towards the target direction; wherein P issRepresenting the coordinates of the start node, P, in a grid mapgRepresenting target node coordinates; p represents the current node coordinates;
s2, initializing an open set and a closed set, wherein the open set is used for storing the nodes to be detected, and the closed set is used for storing the searched nodes; adding the starting node into the open set, and designating a father node of the starting node as a virtual node, wherein the target guidance heuristic function value of the father node is infinite;
s3 selecting a target guideline heuristic function value G (P, P) from the open setg) Judging whether the node is the target point or not by the smallest node, and if so, jumping to the step S11; otherwise, further judging the node G (P, P)g) If the value is smaller than that of the parent node, jumping to step S4 and executing eight-neighborhood guided search; otherwise, rejecting nodes which are not directly adjacent to the barrier in the open set, jumping to the step S8, and executing obstacle detour search of the four adjacent domains;
s4, searching eight neighborhood spaces of the current node, and if a certain adjacent node is not an obstacle and is not in a closed set, calculating an optimal path heuristic function value and a target guidance heuristic function value of the adjacent node;
s5, updating the parent-child relationship between the current node and the adjacent node according to the optimal path enlightenment function value; if a certain adjacent node does not establish a parent-child relationship, the parent node of the adjacent node is the current node, and the adjacent node is added into an open set; if a certain adjacent node is in the set, namely the parent-child relationship is established with other nodes, the parent-child relationship with a smaller optimal path heuristic function value is reserved;
s6, adding the current node into the closed set, and returning to the step S3;
s7 selecting a target guideline heuristic function value G (P, P) from the open setg) Judging whether the node is the target point or not by the smallest node, and if so, jumping to the step S11; otherwise, further judging G (P, P) of the nodeg) If the value is smaller than the parent node, clearing the open set, skipping to the step S4, and executing eight-neighborhood guided search; otherwise, jumping to step S8, and executing obstacle detour search of the four adjacent domains;
s8, searching the four-neighbor space of the current node, and if a certain adjacent node is directly or indirectly adjacent to the barrier, calculating the optimal path heuristic function value and the target guidance heuristic function value of the adjacent node;
s9, updating the parent-child relationship between the current node and the adjacent node according to the optimal path enlightenment function value, if the parent-child relationship is not established in a certain adjacent node, the parent node of the adjacent node is the current node, and the adjacent node is added into the open set; if a certain adjacent node is in the set, namely the adjacent node establishes a parent-child relationship with other nodes, keeping the parent-child relationship with a smaller optimal path heuristic function value;
s10, adding the current node into the closed set, and returning to the step S7;
s11, starting from the target node, backtracking to the initial node according to the parent-child relationship between the nodes to obtain a preliminary path;
s12, acquiring all inflection points in the preliminary path;
s13 follows the preliminary path direction, traversing all inflection points: for the currently traversed inflection point, finding a non-adjacent inflection point closest to the inflection point to replan a path between the two inflection points, and if the new path between the two inflection points is shorter than the original path, replacing the original path between the two inflection points with the new path;
s14 repeats the thirteenth step until the path length no longer changes, resulting in an optimal path.
Preferably, the grid map includes a two-dimensional grid map or a three-dimensional grid map.
Preferably, in step S1, the optimal path heuristic function S (P, P)s) The calculation method) is as follows:
in the formula
A neighbor node representing a current node;
target-directing heuristic function G (P, P)g) The calculation method is as follows:
G(P,Pg)＝(R-Rg)2+(C-Cg)2
wherein, (R, C) represents the current node coordinate P (R, C); (R)g,Cg) Representing target node coordinates Pg(Rg,Cg)。
Preferably, in step S4, if the neighboring node is an obstacle or is already in the closed set, no processing is performed on it.
Preferably, in step S8, if the neighboring node is not directly or indirectly adjacent to the obstacle, no processing is performed on the neighboring node.
Preferably, in step S11, the parent-child relationship between the nodes is from the parent node to the child node.
Preferably, in step S12, when the three adjacent nodes in the initial path are not on the same straight line, the node at the middle position of the three nodes is referred to as an inflection point, and the start node and the target node are also referred to as inflection points.
Preferably, in step S13, when the path between two inflection points is re-planned, the re-planned search distance does not exceed the distance between the two inflection points in the preliminary path.
In a second aspect, the present invention provides a robot fast optimal path planning apparatus based on dual heuristic functions, which is used to implement the robot fast optimal path planning method based on dual heuristic functions as described in any of the embodiments of the first aspect above.
The invention has the beneficial effects that:
1. the optimal path heuristic function is introduced to shorten the planned path, and the target guide heuristic function is introduced to guide the planning process to expand to the target node and reduce useless path attempts. The problem that quantization units are constrained mutually does not exist in the two independent heuristic functions, each heuristic function can use the most accurate cost calculation mode and calculation measurement, the calculation amount is small, and the heuristic effect is more ideal.
2. And judging whether the current search process encounters a large-range obstacle or not through the target guidance heuristic function values of the father node and the son node. When the target guidance heuristic function value of the child node is larger than that of the parent node, the situation that a large-range obstacle is encountered needs to be bypassed is shown. Otherwise, the situation that the obstacle does not meet the large-range obstacle or the obstacle is already bypassed is indicated, and the accuracy of the most-path planning is improved.
3. Meanwhile, two search modes of eight-neighborhood guided search and four-neighborhood obstacle detour search are adopted. The eight-neighborhood guided search can search a target node at the highest speed and in the shortest path, when a large-range obstacle is encountered, four-neighborhood obstacle detouring search is adopted, only the nodes directly adjacent to the obstacle are processed, the large-range obstacle can be rapidly detoured, and the search range is effectively reduced.
4. And extracting inflection point information in the preliminary path, and performing path re-planning between inflection points on a certain inflection point and a non-adjacent inflection point closest to the certain inflection point, so that the path can be optimized at the minimum cost, and the shortest path from the starting node to the target node is obtained.
Drawings
The invention is further illustrated by means of the attached drawings, but the embodiments in the drawings do not constitute any limitation to the invention, and for a person skilled in the art, other drawings can be obtained on the basis of the following drawings without inventive effort.
FIG. 1 is a schematic diagram of a two-dimensional grid map according to an embodiment of the present invention;
FIG. 2 is a schematic flow chart of a robot rapid optimal path planning method based on a dual heuristic function according to the present invention;
FIG. 3 is a schematic diagram of a neighboring node according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a current node neighborhood shown in the embodiment of the present invention;
FIG. 5 is a schematic diagram of a preliminary path according to an embodiment of the present invention;
Detailed Description
The invention is further described in connection with the following application scenarios.
The scheme of the invention can be used in a two-dimensional or three-dimensional grid map, and for convenience of description, the embodiment further illustrates the robot rapid optimal path planning method based on the dual heuristic functions, which is provided by the invention, by taking a possible two-dimensional grid map as an example.
In the distance two-dimensional grid map shown in fig. 1, white squares are free spaces, black squares are obstacles, white arrows represent initial position nodes, black flags represent target position nodes, and black anchor points represent current position nodes. Wherein the grid map takes rows and columns as coordinates, the coordinate values are positive integers, and the initial node coordinate is Ps(Rs,Cs) Target node coordinate is Pg(Rg,Cg) The current node coordinates are P (R, C), where R and C are the coordinates of the grid map, and may define the origin of coordinates as the upper left corner of the map, R represents the row coordinates, and C represents the column coordinates.
Referring to fig. 2, a flow diagram of the robot fast optimal path planning method based on the dual heuristic function in the embodiment is shown, wherein in order to avoid performing excessive useless path search, two path search modes are formulated in the embodiment, eight-neighborhood guided search can search a target node at the fastest speed and in the shortest path, when a large-range obstacle is encountered, four-neighborhood obstacle detouring search is adopted, the large-range obstacle can be bypassed fast, and the search range is effectively reduced. In order to solve the fastest and optimal problems, the embodiment introduces two heuristic functions, the optimal path heuristic function is used for ensuring that a shorter path can be found, and the target guidance heuristic function is used for guiding the path to expand towards the target direction, so that useless path attempts are reduced. And after the primary path is obtained, extracting inflection point information of the path, performing path re-planning among inflection points, optimizing the path with the minimum cost, and obtaining the shortest path from the initial node to the target node. The method comprises the following specific steps:
in a first step, a heuristic function is defined. The present embodiment uses two heuristic functions to guide the path planning process, including: optimal path heuristic function S (P, P)s) A target guide heuristic function G (P, P)g). Optimal path heuristic function S (P, P)s) Represents the minimum cost from the starting node to the current node to establish the parent-child relationship between adjacent nodes to ensure that a shorter path can be found. For example, referring to FIG. 3, the neighbor nodes of the current node are
As is apparent from the grey squares in fig. 3, the cost of the current node reaching the directly neighboring nodes is lower than that of the indirectly neighboring nodes, and the optimal path heuristic function S (P, P)s) The calculation is as follows:
target guide heuristic function G (P, P)g) Representing a heuristically evaluated cost from a current node to a target node
The guiding path is expanded towards the target direction, and useless path attempts are reduced. It is defined as follows:
G(P,Pg)＝(R-Rg)2+(C-Cg)2
and secondly, initializing an open set and a closed set, wherein the open set is used for storing the nodes to be detected, and the closed set is used for storing the searched nodes to avoid repeated searching. The start node is added to the open set and the parent of the start node is designated as a virtual node whose target direction heuristic function value is infinite (e.g., when programming, the maximum value of the data type may be used).
Thirdly, selecting a target guide heuristic function value G (P, P) from the open setg) And judging whether the node is the target point or not by the smallest node, and if so, starting from the eleventh step. Otherwise, further judging the node G (P, P)g) If the value is smaller than the parent node, if so, starting from the fourth step, executing eight neighborhood guided search. Otherwise, the current planning process encounters a large-range obstacle, needs to detour to avoid useless search, firstly eliminates nodes which are not directly adjacent to the obstacle in the set, and executes obstacle detour search in the four adjacent domains from the eighth step.
And fourthly, searching an eight-neighborhood space (such as a gray area shown in fig. 4 (a)) of the current node, and if a certain adjacent node is an obstacle or is already in a closed set (namely, is already searched), not processing the adjacent node. Otherwise, calculating the optimal path heuristic function value and the target guidance heuristic function value of the adjacent nodes.
And fifthly, updating the parent-child relationship between the current node and the adjacent node according to the optimal path heuristic function value. If a certain adjacent node does not establish a parent-child relationship, the parent node of the adjacent node is the current node, and the adjacent node is added into an open set; if a certain adjacent node is already in the open set, namely the parent-child relationship is established with other nodes, the parent-child relationship with the smaller optimal path heuristic function value is reserved at the moment.
And sixthly, adding the current node into the closed set, and returning to the third step.
Seventhly, selecting a target guide heuristic function value G (P, P) from the open setg) And judging whether the node is the target point or not by the smallest node, and if so, starting from the eleventh step. Otherwise, further judging G (P, P) of the nodeg) If the value is smaller than the parent node, if so, the current planning process is indicated to bypassAn obstacle, at which point the open set is emptied, and from the fourth step, an eight neighborhood guided search is performed. Otherwise, starting from the eighth step, a four-neighborhood obstacle detour search is performed.
And eighthly, searching a four-neighbor space (such as a gray area shown in fig. 4 (b)) of the current node, and if a certain adjacent node is directly or indirectly adjacent to the obstacle, calculating an optimal path heuristic function value and an objective guidance heuristic function value of the adjacent node. Otherwise, it is not processed at all.
And ninthly, updating the parent-child relationship between the current node and the adjacent node according to the optimal path heuristic function value. If a certain adjacent node does not establish a parent-child relationship, the parent node of the adjacent node is the current node, and the adjacent node is added into an open set; if a certain adjacent node is already in the open set, namely the parent-child relationship is established with other nodes, the parent-child relationship with the smaller optimal path heuristic function value is reserved at the moment.
And step ten, adding the current node into the closed set, and returning to the step seventh.
In the tenth step, as shown in fig. 5, the black arrows indicate the parent-child relationship between the nodes, pointing from the parent node to the child nodes. Therefore, a preliminary path is obtained by backtracking to the initial node from the target node according to the parent-child relationship between the nodes.
And step ten, finding all inflection points in the preliminary path. As shown by the black dots in fig. 5, when three adjacent nodes in the initial path are not on the same straight line, the node at the middle position of the three nodes is called as an inflection point, and the starting node and the target node are also called as inflection points.
And thirteenth, traversing all the inflection points along the direction of the preliminary path, and finding the nearest non-adjacent inflection point to a certain inflection point (as shown in fig. 5, for the starting node, the inflection point adjacent to the certain inflection point is the inflection point shown by the dashed circle box, and the nearest non-adjacent inflection point is the inflection point shown by the dashed square box). Re-planning a path between the two inflection points, and if the new path between the two inflection points is shorter than the original path, replacing the original path between the two inflection points with the new path; otherwise, the next inflection point is processed continuously. The re-planning search distance is regulated not to exceed the distance of the two inflection points in the preliminary path, because the purpose of re-planning is to find a more optimal path, if the search length exceeds the original path, the search is not needed to be continued, the search range can be effectively reduced, and the planning time is saved.
And fourteenth, repeating the thirteenth step until the path length is not changed any more, and obtaining the optimal path.
In the above-described embodiments of the present invention,
firstly, an optimal path heuristic function is introduced to shorten a planned path, and a target guide heuristic function is introduced to guide a planning process to expand towards a target node so as to reduce useless path attempts. The problem that quantization units are constrained mutually does not exist in the two independent heuristic functions, each heuristic function can use the most accurate cost calculation mode and calculation measurement, and the heuristic effect is more ideal.
Secondly, judging whether the current searching process meets a large-range obstacle or not through the target guidance heuristic function values of the father node and the son node. When the target guidance heuristic function value of the child node is larger than that of the parent node, the situation that a large-range obstacle is encountered needs to be bypassed is shown. Otherwise, it indicates that the obstacle in the large range is not encountered or has been bypassed.
Thirdly, two search modes of eight-neighborhood guided search and four-neighborhood obstacle detour search are adopted simultaneously. The eight-neighborhood guided search can search a target node at the highest speed and in the shortest path, when a large-range obstacle is encountered, four-neighborhood obstacle detouring search is adopted, only the nodes directly adjacent to the obstacle are processed, the large-range obstacle can be rapidly detoured, and the search range is effectively reduced.
Fourthly, extracting inflection point information in the preliminary path, and performing path replanning between inflection points on a certain inflection point and a non-adjacent inflection point closest to the certain inflection point, so that the path can be optimized at the minimum cost, and the shortest path from the starting node to the target node is obtained.
Meanwhile, corresponding to the robot fast optimal path planning method based on the double heuristic functions, the embodiment of the invention also provides a robot fast optimal path planning device based on the double heuristic functions, wherein the device is used for realizing the robot fast optimal path planning method based on the double heuristic functions and the specific embodiments corresponding to the steps of the method, and the invention is not repeated here.
It should be noted that, functional units/modules in the embodiments of the present invention may be integrated into one processing unit/module, or each unit/module may exist alone physically, or two or more units/modules are integrated into one unit/module. The integrated unit/module may be implemented in the form of hardware, or may also be implemented in the form of a software functional unit/module.
From the above description of embodiments, it is clear for a person skilled in the art that the embodiments described herein can be implemented in hardware, software, firmware, middleware, code or any appropriate combination thereof. For a hardware implementation, the processor may be implemented in one or more of the following units: an Application Specific Integrated Circuit (ASIC), a Digital Signal Processor (DSP), a Digital Signal Processing Device (DSPD), a Programmable Logic Device (PLD), a Field Programmable Gate Array (FPGA), a processor, a controller, a microcontroller, a microprocessor, other electronic units designed to perform the functions described herein, or a combination thereof. For a software implementation, some or all of the procedures of an embodiment may be performed by a computer program instructing associated hardware. In practice, the program may be stored on or transmitted over as one or more instructions or code on a computer-readable medium. Computer-readable media includes both computer storage media and communication media including any medium that facilitates transfer of a computer program from one place to another. A storage media may be any available media that can be accessed by a computer. Computer-readable media can include, but is not limited to, RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer.
Finally, it should be noted that the above embodiments are only used for illustrating the technical solutions of the present invention, and not for limiting the protection scope of the present invention, although the present invention is described in detail with reference to the preferred embodiments, it should be analyzed by those skilled in the art that modifications or equivalent substitutions can be made on the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.
Claims (9)
1. The robot rapid optimal path planning method based on the double heuristic functions is characterized by comprising the following steps:
s1 defines a heuristic function, wherein the heuristic function includes an optimal path heuristic function S (P, P)s) And a target-directing heuristic function G (P, P)g) Wherein the optimal path heuristic function S (P, P)s) Representing the minimum cost from the starting node to the current node, and is used for establishing a parent-child relationship between adjacent nodes; target-directing heuristic function G (P, P)g) Representing heuristic evaluation cost from a current node to a target node, wherein the heuristic evaluation cost is used for guiding the path to expand towards the target direction; wherein P issRepresenting the coordinates of the start node, P, in a grid mapgRepresenting target node coordinates; p represents the current node coordinates;
s2, initializing an open set and a closed set, wherein the open set is used for storing the nodes to be detected, and the closed set is used for storing the searched nodes; adding the starting node into an open set, and designating a father node of the starting node as a virtual node, wherein the target guidance heuristic function value of the father node is infinite;
s3 selecting a target guideline heuristic function value G (P, P) from the open setg) Judging whether the node is the target point or not by the smallest node, and if so, jumping to the step S11; otherwise, further judging the node G (P, P)g) If the value is smaller than that of the parent node, jumping to step S4 and executing eight-neighborhood guided search; otherwise, removing nodes which are not directly adjacent to the barrier in the open set, jumping to the step S8, and executing four-adjacent-domain barrier detour search;
s4, searching the eight neighborhood spaces of the current node, and if a certain adjacent node is not an obstacle and is not in a closed set, calculating the optimal path heuristic function value and the target guidance heuristic function value of the adjacent node;
s5, updating the parent-child relationship between the current node and the adjacent node according to the optimal path heuristic function value; if a certain adjacent node does not establish a parent-child relationship, the parent node of the adjacent node is the current node, and the adjacent node is added into an open set; if a certain adjacent node is in the set, namely the parent-child relationship is established with other nodes, the parent-child relationship with a smaller optimal path heuristic function value is reserved;
s6, adding the current node into the closed set, and returning to the step S3;
s7 selecting a target guideline heuristic function value G (P, P) from the open setg) Judging whether the node is the target point or not by the smallest node, and if so, jumping to the step S11; otherwise, further judging G (P, P) of the nodeg) If the value is smaller than the parent node, clearing the open set, skipping to the step S4, and executing eight-neighborhood guided search; otherwise, jumping to step S8, and executing obstacle detour search of the four adjacent domains;
s8, searching the four-neighbor space of the current node, and if a certain adjacent node is directly or indirectly adjacent to the barrier, calculating the optimal path heuristic function value and the target guidance heuristic function value of the adjacent node;
s9, updating the parent-child relationship between the current node and the adjacent node according to the optimal path enlightenment function value, if the parent-child relationship is not established in a certain adjacent node, the parent node of the adjacent node is the current node, and the adjacent node is added into the open set; if a certain adjacent node is in the set, namely the adjacent node establishes a parent-child relationship with other nodes, the parent-child relationship with a smaller optimal path heuristic function value is reserved;
s10, adding the current node into the closed set, and returning to the step S7;
s11, tracing back to the initial node from the target node according to the parent-child relationship between the nodes to obtain a preliminary path;
s12, acquiring all inflection points in the preliminary path;
s13 follows the preliminary path direction, traversing all inflection points: for the currently traversed inflection point, finding a non-adjacent inflection point closest to the inflection point to replan a path between the two inflection points, and if the new path between the two inflection points is shorter than the original path, replacing the original path between the two inflection points with the new path;
s14 repeats the thirteenth step until the path length no longer changes, resulting in an optimal path.
2. The dual heuristic function based robot fast optimal path planning method of claim 1, wherein the grid map comprises a two-dimensional grid map or a three-dimensional grid map.
3. The method for rapid optimal path planning for robot based on dual heuristic functions of claim 1, wherein in step S1, the optimal path heuristic function S (P, P)s) The calculation method) is as follows:
in the formula
A neighbor node representing a current node;
target-directing heuristic function G (P, P)g) The calculation method is as follows:
G(P,Pg)＝(R-Rg)2+(C-Cg)2
wherein, (R, C) represents the current node coordinates P (R, C); (R)g,Cg) Representing target node coordinates Pg(Rg,Cg)。
4. The method for robot fast optimal path planning based on dual heuristic functions of claim 1, wherein in step S4, if the neighboring node is an obstacle or is already in the closed set, it is not processed any more.
5. The method for robot fast optimal path planning based on dual heuristic functions of claim 1, wherein in step S8, if the neighboring node is not directly or indirectly adjacent to the obstacle, it is not processed any more.
6. The method for robot fast optimal path planning based on dual heuristic functions of claim 1, wherein in step S11, the parent-child relationship between nodes is from parent node to child node.
7. The method for robot fast optimal path planning based on dual heuristic functions of claim 1, wherein in step S12, when three adjacent nodes in the initial path are not on the same straight line, the node at the middle position of the three nodes is called an inflection point, and the start node and the target node are also called inflection points.
8. The method for fast optimal path planning for robot according to claim 1, wherein in step S13, when the path between two inflection points is re-planned, the re-planned search distance does not exceed the distance between the two inflection points in the preliminary path.
9. The robot fast optimal path planning device based on the double heuristic functions is characterized by being used for realizing the robot fast optimal path planning method based on the double heuristic functions as claimed in any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210202965.3A CN114721370B (en) | 2022-03-02 | 2022-03-02 | Robot rapid optimal path planning method based on double heuristic functions |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210202965.3A CN114721370B (en) | 2022-03-02 | 2022-03-02 | Robot rapid optimal path planning method based on double heuristic functions |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114721370A true CN114721370A (en) | 2022-07-08 |
| CN114721370B CN114721370B (en) | 2024-10-15 |
Family
ID=82236268
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210202965.3A Active CN114721370B (en) | 2022-03-02 | 2022-03-02 | Robot rapid optimal path planning method based on double heuristic functions |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114721370B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116484798A (en) * | 2023-04-27 | 2023-07-25 | 北京容汇科技有限公司 | PCB wiring acceleration optimization method based on segmentation parallel search |
| CN118092361A (en) * | 2024-04-17 | 2024-05-28 | 哈尔滨工业大学（威海） | Heterogeneous multi-robot task scheduling method and system for textile industry |
| CN118654682A (en) * | 2024-08-20 | 2024-09-17 | 杭州麒云数据科技有限公司 | Unmanned aerial vehicle path planning method apparatus, device and medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105844364A (en) * | 2016-04-08 | 2016-08-10 | 上海派毅智能科技有限公司 | Service robot optimal path program method based on heuristic function |
| WO2017173990A1 (en) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | Method for planning shortest path in robot obstacle avoidance |
| CN111504325A (en) * | 2020-04-29 | 2020-08-07 | 南京大学 | Global path planning method based on weighted A-algorithm for expanding search neighborhood |
-
2022
- 2022-03-02 CN CN202210202965.3A patent/CN114721370B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017173990A1 (en) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | Method for planning shortest path in robot obstacle avoidance |
| CN105844364A (en) * | 2016-04-08 | 2016-08-10 | 上海派毅智能科技有限公司 | Service robot optimal path program method based on heuristic function |
| CN111504325A (en) * | 2020-04-29 | 2020-08-07 | 南京大学 | Global path planning method based on weighted A-algorithm for expanding search neighborhood |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116484798A (en) * | 2023-04-27 | 2023-07-25 | 北京容汇科技有限公司 | PCB wiring acceleration optimization method based on segmentation parallel search |
| CN116484798B (en) * | 2023-04-27 | 2024-05-03 | 苏州容汇科技有限公司 | PCB wiring acceleration optimization method based on segmentation parallel search |
| CN118092361A (en) * | 2024-04-17 | 2024-05-28 | 哈尔滨工业大学（威海） | Heterogeneous multi-robot task scheduling method and system for textile industry |
| CN118092361B (en) * | 2024-04-17 | 2024-10-15 | 哈尔滨工业大学（威海） | Heterogeneous multi-robot task scheduling method and system for textile industry |
| CN118654682A (en) * | 2024-08-20 | 2024-09-17 | 杭州麒云数据科技有限公司 | Unmanned aerial vehicle path planning method apparatus, device and medium |
| CN118654682B (en) * | 2024-08-20 | 2024-11-01 | 杭州麒云数据科技有限公司 | Unmanned aerial vehicle path planning method apparatus, device and medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114721370B (en) | 2024-10-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114721370A (en) | Robot rapid optimal path planning method based on double heuristic functions | |
| CN110887484B (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| CN109059924B (en) | Accompanying robot incremental path planning method and system based on A-x algorithm | |
| CN114510056B (en) | Method for planning steady moving global path of indoor mobile robot | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| CN111504325A (en) | Global path planning method based on weighted A-algorithm for expanding search neighborhood | |
| CN109163722A (en) | A kind of anthropomorphic robot paths planning method and device | |
| KR100994075B1 (en) | Method for planning an optimal path in a biped robot | |
| CN115167474A (en) | Mobile robot path planning optimization method | |
| CN111427341B (en) | Robot shortest expected time target searching method based on probability map | |
| Desai et al. | Efficient kinodynamic multi-robot replanning in known workspaces | |
| CN115469662A (en) | Environment exploration method, device and application | |
| Mahida et al. | Comparision of pathfinding algorithms for visually impaired people in IoT based smart buildings | |
| CN117008589A (en) | Mobile robot autonomous exploration mapping method based on improved RRT algorithm | |
| CN114879660A (en) | Robot environment sensing method based on target driving | |
| Jarvis | Distance transform based path planning for robot navigation | |
| CN117804455A (en) | Unmanned surface vessel path planning method based on improved A-Star algorithm | |
| Zafar et al. | Mine detection and route planning in military warfare using multi agent system | |
| Hernandez et al. | A path planning algorithm for an AUV guided with homotopy classes | |
| WO2019239680A1 (en) | Information processing device and information processing method | |
| CN114577217B (en) | Route planning method, device, equipment and storage medium based on Von Lonouh graph | |
| Ng | An analysis of mobile robot navigation algorithms in unknown environments | |
| CN113311843A (en) | Unmanned ship path planning method based on safety distance constraint and LOS sight judgment | |
| CN118068854B (en) | Unmanned aerial vehicle obstacle avoidance path planning method based on artificial potential field | |
| Chahal | Generating an adaptive path using rrt sampling and potential functions with directional nearest neighbors |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |