CN115291597A - Path planning method and device based on bidirectional hybrid A-algorithm and terminal - Google Patents

Path planning method and device based on bidirectional hybrid A-algorithm and terminal Download PDF

Info

Publication number
CN115291597A
CN115291597A CN202210603043.3A CN202210603043A CN115291597A CN 115291597 A CN115291597 A CN 115291597A CN 202210603043 A CN202210603043 A CN 202210603043A CN 115291597 A CN115291597 A CN 115291597A
Authority
CN
China
Prior art keywords
node
backward
algorithm
search
path
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
Application number
CN202210603043.3A
Other languages
Chinese (zh)
Inventor
黄超
盛文威
叶玥
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xiantu Intelligent Technology Co Ltd
Original Assignee
Shanghai Xiantu Intelligent Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanghai Xiantu Intelligent Technology Co Ltd filed Critical Shanghai Xiantu Intelligent Technology Co Ltd
Priority to CN202210603043.3A priority Critical patent/CN115291597A/en
Publication of CN115291597A publication Critical patent/CN115291597A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

A path planning method, a device and a terminal based on a bidirectional hybrid A-x algorithm are disclosed, wherein the method comprises the following steps: determining an initial node and a target node of path search in the grid map; from the starting node, generating a forward search sub-node of the current forward node in a traversing manner, and from the target node, generating a backward search sub-node of the current backward node in a traversing manner; for each forward search sub-node, calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm, calculating to obtain a second path length from the current forward node to the target node by adopting an heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node. The method can consider the vehicle kinematic constraint and the obstacle avoidance constraint under the complex scene, thereby improving the accuracy of path planning.

Description

Path planning method and device based on bidirectional hybrid A-algorithm and terminal
Technical Field
The invention relates to the technical field of data processing, in particular to a path planning method and device based on a bidirectional hybrid A-x algorithm and a terminal.
Background
In the unmanned vehicle path planning module, a collision-free safe and feasible path from the current position to the target end point can be searched in a certain area according to the map information, the positioning information and the prediction information and the current state of the vehicle.
However, in a complex scenario based on an unstructured road (e.g., a path planning scenario for automatic parking), a situation where an obstacle exists at an end point is often encountered.
In the prior art, a traditional hybrid a-star algorithm is usually adopted for path planning, however, a Dubins curve or a Reeds-sheets curve is often adopted as a heuristic function in the algorithm, and the heuristic function does not consider the factors of obstacles, so that a path finding failure is easy to occur in a complex scene.
Disclosure of Invention
The invention solves the technical problem of providing a path planning method, a device and a terminal based on a bidirectional hybrid A-x algorithm, which can add an obstacle avoidance effect brought by an obstacle avoidance algorithm, thereby not only considering vehicle kinematics constraint but also considering obstacle avoidance constraint in a complex scene, and further improving the accuracy of path planning.
In order to solve the above technical problem, an embodiment of the present invention provides a path planning method based on a bidirectional hybrid a-x algorithm, including: step A: determining an initial node and a target node of path search in the grid map; and B: from the starting node, generating a forward search sub-node of the current forward node in a traversing manner, and from the target node, generating a backward search sub-node of the current backward node in a traversing manner; and C: for each forward search sub-node, calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm, calculating to obtain a second path length from the current forward node to the target node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node; and/or for each backward search sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the heuristic cost of the current backward node.
Optionally, one or more of the following are satisfied: the obstacle avoidance algorithm is selected from: a, an algorithm, a Voronoi diagram algorithm and an artificial potential field algorithm; a heuristic function curve algorithm different from the obstacle avoidance algorithm is selected from: reeds-sheets curve algorithm and Dubins curve algorithm.
Optionally, the path planning method further includes: for each forward search sub-node and each backward search sub-node, determining the used cost by adopting a first penalty coefficient and/or a second penalty coefficient; wherein the first penalty coefficient is used for representing that a node direction difference value between the forward searching child node and a parent node of the forward searching child node is larger than a first threshold value, or used for representing that a node direction difference value between the backward searching child node and a parent node of the backward searching child node is larger than the first threshold value; the second penalty coefficient is used for representing that the gear direction of the forward searching child node is different from the gear direction between the parent nodes of the forward searching child node, or representing that the gear direction of the backward searching child node is different from the gear direction between the parent nodes of the backward searching child node.
Optionally, the used cost is determined by using the following formula:
G＝G'+(w 1 +w 2 )×dis
wherein G is used for representing the used cost from the starting node to the forward searching child node or representing the used cost from the target node to the backward searching child node, G' is used for representing the used cost from the starting node to the parent node of the forward searching child node or representing the used cost from the target node to the parent node of the backward searching child node, w 1 A preset value, w, for representing said first penalty factor 2 And a preset value used for representing the second penalty coefficient, dis is used for representing the path length from the parent node of the forward search child node to the forward search child node, or is used for representing the path length from the parent node of the backward search child node to the backward search child node.
Optionally, the path planning method further includes: selecting a forward searching sub-node with the minimum total cost value and selecting a backward searching sub-node with the minimum total cost value; the total cost value of the forward search child node is the sum of the heuristic cost and the used cost of the forward search child node, and the total cost value of the backward search child node is the sum of the heuristic cost and the used cost of the backward search child node.
Optionally, the path planning method further includes: if the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value meet any one of the following items, determining that the path planning is successful, otherwise, determining that the path planning is unsuccessful: the distance between the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value is smaller than the first distance, and the forward searching sub-node and the backward searching sub-node are connected by adopting the heuristic function curve algorithm and do not collide with the barrier; the distance between the forward search subnode with the minimum total cost value and the target node is smaller than a first distance, and the forward search subnode is connected with the target node by adopting a heuristic function curve algorithm and does not collide with an obstacle; the distance between the backward search sub-node with the minimum total cost value and the starting node is smaller than the first distance, and the backward search sub-node is connected by adopting the heuristic function curve algorithm and does not collide with the barrier; the forward searching child node with the minimum total cost value is the target node; and the backward searching child node with the minimum total cost value is the starting node.
Optionally, the heuristic function curve algorithm is selected from a Reeds-Sheeps curve algorithm and a Dubins curve algorithm.
Optionally, the path planning method further includes: and D, when the path planning is not successful, discarding the current forward node and the current backward node, returning to the step B, traversing and generating the forward searching sub-node of the current forward node, merging the newly generated forward searching sub-node with the forward searching sub-node which is generated before and is not discarded, traversing and generating the backward searching sub-node of the current backward node, and merging the newly generated backward searching sub-node with the backward searching sub-node which is generated before and is not discarded.
Optionally, the path planning method further includes: and when the path planning is not determined to be successful within the preset time length or the path planning is not determined to be successful within the preset total number of turns of the path planning, modifying the resolution of the grid map, and returning to the step A.
Optionally, during the forward search, the attitude angle θ of the initial pose of the wheel during the path planning is adopted s An attitude angle theta representing the starting node and, when path planning is employed, the ending pose of the wheel e Representing the target node; in the backward search, the attitude angle theta from the forward search is adopted e Inverted attitude angle theta s ' representing the target node, using an attitude angle theta from the time of forward search s Inverted attitude angle theta e ' denotes the start node.
Optionally, the obstacle avoidance algorithm is an a algorithm; in the process of calculating the first path length from the current forward node to the target node by adopting the A-x algorithm, caching each path node and the path length from the path node to the target node from the current forward node when traversing the forward search child node and calculating the total cost value of the forward search child node; and/or caching each path node and the path length from the path node to the starting node from the current backward node when traversing the backward search child node and calculating the total cost value of the backward search child node in the process of calculating the third path length from the current backward node to the starting node by adopting the A-algorithm.
Optionally, the path planning method further includes: if the current forward node or the forward search child node generated by subsequent traversal is a cached node, adopting the cached path length as the heuristic cost based on the A-x algorithm; and/or if the current backward node or the backward search child node generated by the subsequent traversal is a cached node, adopting the cached path length as the heuristic cost based on the A-star algorithm.
Optionally, the cached node is added to the open list of the a-algorithm, and the method further includes: if the current forward node is not added to the open list of the A-algorithm, traversing the forward search child nodes of the current forward node, and updating the open list of the A-algorithm; and/or, if the current backward node is not added to the open list of the A-algorithm, traversing the backward search child nodes of the current backward node and updating the open list of the A-algorithm.
Optionally, the path planning method further includes: after a forward search child node with the minimum total cost value based on the A-star algorithm is determined, backtracking from the target node to the starting node, and updating each path node and the path length from the path node to the target node in a cache; and/or, after each backward search child node with the minimum total cost value based on the A-star algorithm is determined, backtracking from the starting node to the target node, and updating each path node and the path length from the path node to the starting node in a cache; wherein the minimum total cost value based on the A-algorithm is the minimum sum of the heuristic cost and the used cost calculated based on the A-algorithm.
In order to solve the above technical problem, an embodiment of the present invention provides a path planning apparatus based on a bidirectional hybrid a-x algorithm, including: the node determining module is used for determining a starting node and a target node of path search in the raster map; the traversal module is used for generating a forward search sub-node of the current forward node in a traversal mode from the starting node and generating a backward search sub-node of the current backward node in a traversal mode from the target node; the heuristic cost determining module is used for calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm for each forward search sub-node, calculating to obtain a second path length from the current forward node to the target node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node; and/or for each backward search sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the heuristic cost of the current backward node.
To solve the above technical problem, an embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored, where the computer program is executed by a processor to perform the steps of the above path planning method based on the bidirectional hybrid a-x algorithm.
In order to solve the above technical problem, an embodiment of the present invention provides a terminal, including a memory and a processor, where the memory stores a computer program capable of running on the processor, and the processor executes the steps of the path planning method based on the bidirectional hybrid a-x algorithm when running the computer program.
Compared with the prior art, the technical scheme of the embodiment of the invention has the following beneficial effects:
in the embodiment of the invention, by adopting the bidirectional mixed A-star algorithm, the two opposite search directions from the starting point to the end point and from the end point to the starting point can be searched simultaneously, and compared with the condition that the end point is easy to encounter an obstacle during single-direction search, the interference of the obstacle on path search can be reduced. In addition, the method further adopts an obstacle avoidance algorithm and different heuristic function curve algorithms to respectively calculate the path length from the current forward node to the target node and calculate the path length from the current backward node to the starting node, and adopts the larger one as the heuristic cost.
Further, the first penalty coefficient and/or the second penalty coefficient are used for determining the used cost. The first penalty coefficient is used for indicating that the node direction difference between the forward searching child node or the backward searching child node and the parent node thereof is larger than a first threshold value, so that a penalty can be added to frequent gear shifting operation, and the condition of path distortion oscillation searched based on the bidirectional hybrid A-x algorithm can be relieved. The second penalty coefficient is used for representing that the gear direction of the forward searching child node or the backward searching child node is different from the gear direction between the parent nodes of the forward searching child node or the backward searching child node, so that the frequent change of the directions of the front node and the back node on the path can be penalized, and the condition of path distortion oscillation searched based on the bidirectional mixed A-star algorithm can be relieved.
Further, when the forward search sub-node with the minimum total cost value and the backward search sub-node with the minimum total cost value satisfy that the distance between the forward search sub-node with the minimum total cost value and the backward search sub-node with the minimum total cost value is smaller than the first distance, and the heuristic function curve algorithm is adopted to connect and then not collide with the obstacle, the path planning is determined to be successful, so that after each round of path planning calculation, whether the path finding is successful or not can be confirmed by combining the forward search result and the backward search result, and the advantage that the bidirectional hybrid A algorithm can search from two opposite search directions at the same time is fully exerted.
Further, when the distance between the forward search sub-node with the minimum total cost value and the target node or the distance between the backward search sub-node with the minimum total cost value and the starting node is smaller than a first distance and the collision with the obstacle does not occur after the connection of the forward search sub-node and the starting node by adopting a heuristic function curve algorithm, the path planning is determined to be successful, so that whether the path finding is successful or not can be respectively confirmed according to the forward search result and the backward search result after each round of path planning calculation, and the advantage that the bidirectional hybrid A algorithm can search from two directions is further exerted.
And furthermore, when the path planning is not successful, discarding the current forward node and the current backward node, returning to the step B, traversing to generate the forward searching sub-node of the current forward node, merging the newly generated forward searching sub-node with the forward searching sub-node which is generated before and is not discarded, traversing to generate the backward searching sub-node of the current backward node, and merging the newly generated backward searching sub-node with the backward searching sub-node which is generated before and is not discarded. By adopting the scheme of the embodiment of the invention, the possibility of repeatedly traversing the searched nodes can be reduced by abandoning the steps, and the calculation efficiency is effectively improved.
Further, in the forward search, the attitude angle θ of the starting pose of the wheel in the path planning is adopted s An attitude angle theta representing the starting node and the ending pose of the wheel when path planning is adopted e Representing the target node; in the backward search, the attitude angle theta of the forward search is adopted e Inverted attitude angle theta s ' representing the target node, using an attitude angle theta from the time of forward search s Inverted attitude angle theta e ' denotes the start node. By adopting the scheme of the embodiment of the invention, the attitude angles of the starting point and the end point during the backward search are reversed, so that the attitude angle of the path node obtained in the backward search process can meet the correct driving direction of the vehicle.
Further, the obstacle avoidance algorithm is an algorithm A; caching each path node and the path length from the path node to the target node when traversing the forward search child node and calculating the total cost value of the forward search child node; and/or caching each path node and the path length from the path node to the starting node each time the backward search child node is traversed and the total cost value of the backward search child node is calculated. Therefore, when the heuristic cost of each node based on the A-algorithm is calculated in the follow-up process, the situation that the path length from the same node to the end point is repeatedly traversed and calculated is effectively reduced, and the efficiency of calculating the path length based on the A-algorithm is improved. Compared with the prior art that when the position near the end point is searched, a large amount of node expansion is needed, so that the path searching rate is low, the scheme in the embodiment of the invention can improve the calculation efficiency on the basis of increasing the obstacle avoidance effect.
Further, if the current forward node or the forward search child node generated by subsequent traversal is a cached node, the cached path length is used as the heuristic cost based on the a-x algorithm, the confirmed actual path length can be multiplexed when the total cost value is calculated, and the path length is more accurate than the path length obtained by each calculation, so that the accuracy of path planning can be further improved on the basis of improving the calculation efficiency.
Drawings
Fig. 1 is a flowchart of a path planning method based on a bidirectional hybrid a-x algorithm according to an embodiment of the present invention;
fig. 2 is a flow chart of another path planning method based on the bidirectional hybrid a-x algorithm according to an embodiment of the present invention;
fig. 3 is a flow chart of a method of calculating path length using the a-algorithm in an embodiment of the present invention;
fig. 4 is a flow chart of another method for calculating path length using the a algorithm in accordance with an embodiment of the present invention;
fig. 5 is a schematic structural diagram of a path planning apparatus based on a bidirectional hybrid a-x algorithm according to an embodiment of the present invention.
Detailed Description
At present, in the process of planning the route of an unmanned vehicle, the task of planning the route is generally divided into the route planning on a structured road and the route planning on an unstructured road according to different road environment information. The structured road has clear road sign lines, including expressways, urban arterial roads and the like; unstructured roads have no road sign lines and often refer to rural streets, roads in open environments, and the like. The driving speed of the unmanned vehicle on the unstructured road is generally not high, and compared with the high-speed structured road, the scene of the unstructured road is complex, and the situations that obstacles are dense, messy and the range of the vehicle capable of driving is narrow often occur. In a planning module, a planning method needs to consider time factors in addition to factors such as feasibility and safety of a path, and the planning module needs to rapidly plan a suitable path within a limited time so as to enable a downstream control module to execute subsequent tasks.
The inventor of the invention discovers through research that in the existing path planning method, a search space can be dispersed into limited grids to form a connected graph, and then a graph search method is adopted to search paths. In one specific application, the a-algorithm may be used, however, the a-algorithm ignores the dynamics of the vehicle during the search. In another specific application, a hybrid a-algorithm may be adopted, and the hybrid a-algorithm may be regarded as an improvement to the a-algorithm, and the search process takes into account the attitude angle of the vehicle, and the searched path satisfies the constraint of the vehicle dynamics.
However, the inventor of the present invention has found through research that, in a complex scene (for example, a route planning scene for automatic parking) based on an unstructured road, especially when the situation of an obstacle at a start point or near an end point is complex, because the state included in a state space searched by a hybrid a × algorithm is infinite, searching from the start point to the end point is performed by using the hybrid a × algorithm, when the vicinity of the obstacle at the end point is searched, a large amount of node expansion is performed, and a heuristic function used by a conventional hybrid a × algorithm is a Dubins curve or a Reeds-Sheeps curve, which does not consider the obstacle factor, and reduces the route planning accuracy while the route planning complexity is reduced.
In the embodiment of the invention, by adopting the bidirectional hybrid A-x algorithm, the two opposite search directions from the starting point to the end point and from the end point to the starting point can be searched simultaneously, and compared with the situation that the end point is easy to encounter an obstacle during searching in a single direction, the interference of the obstacle on path searching can be reduced. In addition, the method further adopts an obstacle avoidance algorithm and different heuristic function curve algorithms to respectively calculate the path length from the current forward node to the target node and calculate the path length from the current backward node to the starting node, and adopts the larger one as the heuristic cost.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in detail below.
Referring to fig. 1, fig. 1 is a flowchart of a path planning method based on a bidirectional hybrid a-x algorithm according to an embodiment of the present invention. The path planning method based on the bidirectional hybrid a-algorithm may include steps S11 to S13:
step S11: determining an initial node and a target node of path search in the grid map;
step S12: from the starting node, generating a forward searching sub-node of the current forward node in a traversing manner, and from the target node, generating a backward searching sub-node of the current backward node in a traversing manner;
step S13: for each forward search sub-node, calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm, calculating to obtain a second path length from the current forward node to the target node by adopting an heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node; and/or for each backward search sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the heuristic cost of the current backward node.
It will be appreciated that in a specific implementation, the method may be implemented in the form of a software program running on a processor integrated within a chip or chip module.
In a specific implementation of step S11, the map may be first rasterized according to the map information with a certain raster resolution, so as to determine a raster map, and obtain discrete map information.
The starting node and the target node of the path search in the raster map may then be determined.
Further, in the forward search, the attitude angle θ of the starting pose of the wheel in the path planning is adopted s An attitude angle theta representing the starting node and, when path planning is employed, the ending pose of the wheel e Representing the target node; in the backward search, the attitude angle theta from the forward search is adopted e Inverted attitude angle theta s ' representing the target node, using an attitude angle theta from the time of forward search s Inverted attitude angle theta e ' denotes the start node.
Specifically, the starting node (forward starting point for short) in the forward search corresponds to the starting pose (x) of the vehicle in the path planning process s ,y s ,θ s ) The destination node (forward end point for short) in the forward search corresponds to the ending pose (x) of the vehicle in the path planning e ,y e ,θ e )。
Position (x) of starting node (backward starting point for short) in backward search s ’,y s ') can be compared with the position (x) of the forward end point e ,y e ) Same, attitude angle θ of backward starting point s ' attitude angle theta with forward end point e In the reverse direction, i.e. theta s ’＝θ e + π. Similarly, the position of the target node (called backward end point for short) in the backward search may be the same as the position of the forward start point, and the attitude angle θ of the backward end point e ’＝θ s +π。
In the embodiment of the invention, during the forward search, the attitude angle theta of the starting pose of the wheel during the path planning is adopted s An attitude angle theta representing the starting node and the ending pose of the wheel when path planning is adopted e Representing the target node; in the backward search, the attitude angle theta of the forward search is adopted e Inverted attitude angle theta s ' representing the target node, using an attitude angle theta from the time of forward search s Inverted attitude angle theta e ' denotes the start node. By adopting the scheme of the embodiment of the invention, backward search is realizedThe attitude angles of the starting point and the end point are reversed, so that the attitude angle of the path node obtained in the backward searching process can meet the correct driving direction of the vehicle.
Further, a forward search start list and a backward search start list of the bidirectional hybrid a-x algorithm may be preset, and the forward starting point and the forward end point are stored in the forward search start list, and the backward starting point and the backward end point are stored in the backward search start list.
The forward search opening list and the backward search opening list may be updated in subsequent exploration, for example, each forward search child node and each backward search child node are stored, and may be removed from the corresponding opening list after the child nodes are explored.
In a specific implementation of step S12, a forward search child node of the current forward node is generated by traversing from the starting node, and a backward search child node of the current backward node is generated by traversing from the target node.
Specifically, in the forward search of the first round of path planning, each forward search child node generating the start node may be traversed with the start node as a parent node, and, in the backward search of the first round of path planning, each backward search child node generating the target node may be traversed with the target node as a parent node.
In the second round of the forward search of the path plan and the next round, each forward search child node of the current forward node may be generated in a traversal manner with the current forward node as a parent node, and in the second round of the backward search of the path plan, each backward search child node of the current backward node may be generated in a traversal manner with the current backward node as a parent node.
Further, in the process of generating the forward search sub-node and the backward search sub-node in a traversal manner, the forward search sub-node and the backward search sub-node may be generated by using one or more preset sampling step lengths and one or more preset sampling angles, so as to perform screening in advance.
In the specific implementation of the step S13, an obstacle avoidance algorithm and different heuristic function curve algorithms are adopted to calculate the path lengths from the current forward node to the target node, respectively, and the greater of the path lengths is adopted as the heuristic cost; and respectively calculating the path lengths from the current backward node to the initial node, and adopting the larger one as the heuristic cost.
Further, the obstacle avoidance algorithm may be selected from: a-algorithm, voronoi diagram algorithm, and artificial potential field algorithm.
Specifically, the a-Star algorithm is also called an a-Star algorithm or an a-Star algorithm, is a direct search method for solving the shortest path in a static road network, and is also an effective algorithm for solving many search problems. The closer the distance estimate is to the actual value in the algorithm, the faster the final search speed.
The Voronoi Diagram (Voronoi Diagram) algorithm is also called a thiessen polygon algorithm or a Dirichlet Diagram algorithm, and a path in search is a continuous polygon formed by perpendicular bisectors of straight lines connecting two adjacent points.
The artificial potential field path planning algorithm can be regarded as a virtual force method, and the basic idea is that the motion of a robot in the surrounding environment is designed into the motion in an abstract artificial gravitational field, a target point generates attraction to the mobile robot, an obstacle generates repulsion to the mobile robot, and finally the motion of the mobile robot is controlled by solving the resultant force. The path planned by the artificial potential field algorithm is smooth and safe.
Further, heuristic function curve algorithms different from the obstacle avoidance algorithm may be selected from: the Reeds-sheets curve algorithm and the Dubins curve algorithm.
Specifically, the heuristic function adopted by the traditional hybrid A algorithm already adopts a Dubins curve or a Reeds-Sheeps curve, in the bidirectional hybrid A algorithm of the embodiment of the invention, part of the existing technology can be used by multiplexing the Reeds-Sheeps curve algorithm and the Dubins curve algorithm, the development complexity is reduced, and the accuracy and the efficiency of calculation by adopting the heuristic function curve algorithm are improved by utilizing the characteristics that the Reeds-Sheeps curve algorithm and the Dubins curve algorithm have the vehicle kinematic constraint and the high calculation efficiency.
In the embodiment of the invention, by adopting the bidirectional hybrid A-x algorithm, the two opposite search directions from the starting point to the end point and from the end point to the starting point can be searched simultaneously, and compared with the situation that the end point is easy to encounter an obstacle during searching in a single direction, the interference of the obstacle on path searching can be reduced. In addition, the method further adopts an obstacle avoidance algorithm and different heuristic function curve algorithms to respectively calculate the path length from the current forward node to the target node and calculate the path length from the current backward node to the starting node, and adopts the larger one as the heuristic cost.
Further, the path planning method may further include: for each forward search sub-node and backward search sub-node, adopting a first penalty coefficient w 1 And/or a second penalty factor w 2 Determining a used cost; wherein the first penalty coefficient w 1 For indicating that a node direction difference between the forward search child node and a parent node of the forward search child node is greater than a first threshold, or for indicating that a node direction difference between the backward search child node and a parent node of the backward search child node is greater than the first threshold; the second penalty factor w 2 The gear direction used for representing the forward searching child node is different from the gear direction between the father nodes of the forward searching child node, or the gear direction used for representing the backward searching child node is different from the gear direction between the father nodes of the backward searching child node.
Specifically, the first penalty coefficient w may be set in advance 1 A preset value, when the node direction difference between the forward search child node and the parent node of the forward search child node is greater than a first threshold, or the node direction difference between the backward search child node and the parent node of the backward search child node is greater than the first threshold,first penalty factor w 1 Adopting the preset value, otherwise, adopting a first punishment coefficient w 1 Zero or other predetermined value.
Specifically, the second penalty coefficient w may also be set in advance 2 When the gear direction of the forward search child node is different from the gear direction between the parent nodes of the forward search child node or the gear direction of the backward search child node is different from the gear direction between the parent nodes of the backward search child node, the gear direction of the forward search child node is a preset value, and a second penalty coefficient w 2 Using the default value, otherwise, the second penalty factor w 2 Zero or other predetermined value.
Still further, the used cost may be determined using the following formula:
G＝G'+(w 1 +w 2 )×dis
wherein G is used for representing the used cost from the starting node to the forward searching child node or representing the used cost from the target node to the backward searching child node, and G' is used for representing the used cost from the starting node to the parent node of the forward searching child node or representing the used cost from the target node to the parent node of the backward searching child node.
In the embodiment of the invention, the used cost which is obtained by the calculation of the father node can be multiplexed by calculating G' so as to reduce the calculation complexity and improve the calculation efficiency.
Wherein, w 1 A preset value, w, for representing said first penalty factor 2 And the preset value is used for representing the second penalty coefficient. As previously mentioned, when the penalty condition is not satisfied, w 1 And w 2 May be zero or other preset value.
And dis is used for representing the path length from the parent node of the forward search child node to the forward search child node or representing the path length from the parent node of the backward search child node to the backward search child node.
In the embodiment of the invention, the used cost is determined by adopting the first penalty coefficient and/or the second penalty coefficient. The first penalty coefficient is used for indicating that the node direction difference between the forward searching child node or the backward searching child node and the parent node thereof is larger than a first threshold value, so that a penalty can be added to frequent shifting operation, and the condition of path distortion oscillation searched based on the two-way hybrid A-star algorithm can be relieved. The second penalty coefficient is used for representing that the gear direction of the forward searching child node or the backward searching child node is different from the gear direction between the parent nodes of the forward searching child node or the backward searching child node, so that the frequent change of the directions of the front node and the back node on the path can be penalized, and the condition of path distortion oscillation searched based on the bidirectional hybrid A-x algorithm can be relieved.
Further, the path planning method may further include: selecting a forward searching sub-node with the minimum total cost value and selecting a backward searching sub-node with the minimum total cost value; the total cost value of the forward search child node is the sum of the heuristic cost and the used cost of the forward search child node, and the total cost value of the backward search child node is the sum of the heuristic cost and the used cost of the backward search child node.
Specifically, the total cost value may be determined using the following formula:
F＝G+H
wherein, F is used for representing the total cost value of the forward search child node or the total cost value of the backward search child node, G is used for representing the used cost of the forward search child node or the used cost of the backward search child node, and H is used for representing the heuristic cost of the forward search child node or the heuristic cost of the backward search child node.
In the embodiment of the present invention, by selecting the forward search sub-node with the minimum total cost value and selecting the backward search sub-node with the minimum total cost value, the most suitable forward search sub-node and backward search sub-node may be determined in the plurality of forward search sub-nodes or the plurality of backward search sub-nodes generated by traversal based on the bidirectional hybrid a-star algorithm.
Further, the path planning method may further include: if the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value meet any one of the following items, determining that the path planning is successful, otherwise, determining that the path planning is unsuccessful: the distance between the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value is smaller than the first distance, and the forward searching sub-node and the backward searching sub-node are connected by adopting the heuristic function curve algorithm and do not collide with the barrier; the distance between the forward search subnode with the minimum total cost value and the target node is smaller than a first distance, and the forward search subnode is connected with the target node by adopting a heuristic function curve algorithm and does not collide with an obstacle; the distance between the backward search sub-node with the minimum total cost value and the starting node is smaller than the first distance, and the backward search sub-node is connected by adopting the heuristic function curve algorithm and does not collide with the barrier; the forward searching child node with the minimum total cost value is the target node; and the backward searching child node with the minimum total cost value is the starting node.
Specifically, after each round of path planning calculation, the path length between the forward search sub-node with the minimum total cost value and the backward search sub-node with the minimum total cost value is calculated by combining the forward search result and the backward search result, and if the distance between the forward search sub-node and the backward search sub-node is close to the minimum total cost value, the forward search sub-node and the backward search sub-node are connected by adopting a heuristic function curve algorithm, and whether collision with an obstacle does not occur is determined.
Furthermore, the heuristic function curve algorithm can be selected from a Reeds-Sheeps curve algorithm and a Dubins curve algorithm, so that the accuracy and the efficiency of calculation by using the heuristic function curve algorithm are improved by utilizing the characteristics of being in line with vehicle kinematics constraint, high in calculation efficiency and the like.
In a specific implementation, it may be determined whether a curve obtained by connecting the curves by using the heuristic function curve algorithm collides with an obstacle by using a conventional method, which is not limited in the embodiment of the present invention.
In the embodiment of the present invention, when the forward search sub-node with the minimum total cost value and the backward search sub-node with the minimum total cost value satisfy that the distance between the forward search sub-node with the minimum total cost value and the backward search sub-node with the minimum total cost value is smaller than the first distance, and the forward search sub-node and the backward search sub-node with the minimum total cost value are connected by using the heuristic function curve algorithm and do not collide with the obstacle, it is determined that the path planning is successful, so that after each round of path planning calculation, whether the path finding is successful or not can be determined by combining the forward search result and the backward search result, and the advantage that the bidirectional hybrid a algorithm can search from two opposite search directions at the same time is fully exerted.
Further, after each round of path planning calculation, whether the path finding is successful or not can be respectively confirmed according to the forward search result and the backward search result. Specifically, the distance between the forward search subnode with the minimum total cost value and the target node and the distance between the backward search subnode with the minimum total cost value and the starting node may be respectively determined, and if the distances are short and the curves are connected and do not collide with the obstacle, it is determined that the path finding is successful.
When the forward searching sub-node with the minimum total cost value is the target node, or when the backward searching sub-node with the minimum total cost value is the starting node, collision with the barrier is avoided inevitably, and at the moment, success in route searching can be judged.
In the embodiment of the invention, when the distance between the forward search sub-node with the minimum total cost value and the target node or the distance between the backward search sub-node with the minimum total cost value and the starting node is smaller than a first distance and the distance is not collided with an obstacle after the forward search sub-node and the starting node are connected by adopting a heuristic function curve algorithm, the path planning is determined to be successful, so that whether the path is found successfully or not can be respectively determined according to the forward search result and the backward search result after each round of path planning calculation, and the advantage that the bidirectional hybrid A algorithm can search from two directions is further exerted.
Further, in the embodiment of the present invention, the method may further include the step of updating the forward search open list and the backward search open list.
Specifically, the path planning method may further include: and (c) discarding the current forward node and the current backward node each time the path planning is unsuccessful, returning to step B (i.e., step S12 in fig. 1), traversing the forward search child nodes generating the current forward node, merging the newly generated forward search child nodes with the forward search child nodes generated before and not discarded, traversing the backward search child nodes generating the current backward node, and merging the newly generated backward search child nodes with the discarded backward search child nodes generated before and not discarded.
Specifically, the discarding process may be that in one or more subsequent rounds of path planning, the discarded node is not repeatedly explored.
Further, in the case where the forward search open list and the backward search open list are provided, the discarded node may be deleted in the forward search open list and the backward search open list, and may also be marked as being discarded.
The child node generated for each parent node may be subjected to discard processing when a discard condition is satisfied. The abandon condition comprises that the child node is explored, or the path from the parent node to the child node is not in obstacle avoidance.
More specifically, the child node has been explored to search for the child node in a forward direction with the minimum total cost value or search for the child node in a backward direction with the minimum total cost value, which does not satisfy the aforementioned condition, so that the path planning is unsuccessful, and the path from the parent node to the child node is not prevented from colliding with the obstacle after the parent node and the child node are connected by using the heuristic function curve algorithm.
It should be noted that, for the child node generated by each parent node, if the explored child node is already in the forward search open list and the backward search open list, that is, the child node has been explored in the previous exploration, the more appropriate parent node may be selected in a backward direction.
Specifically, for example, in the forward search, the total cost value from the start node to the child node via the current parent node may be calculated, and the total cost value from the start node to the forward search child node via the parent node that was last searched for may be calculated. After each round of path planning and obtaining the forward search child nodes meeting the requirements, the expanded parent nodes can be taken as the abandoned nodes, removed from the forward search open list, and the states of the nodes are recorded as explored. The same way can be used for backward exploration.
In the embodiment of the invention, when the path planning is not successful, the current forward node and the current backward node are abandoned, the step B is returned, the forward searching sub-node of the current forward node is generated in a traversing manner, the newly generated forward searching sub-node is merged with the forward searching sub-node which is generated before and is not abandoned, the backward searching sub-node of the current backward node is generated in a traversing manner, and the newly generated backward searching sub-node is merged with the backward searching sub-node which is generated before and is not abandoned. By adopting the scheme of the embodiment of the invention, the possibility of repeatedly traversing the searched nodes can be reduced by abandoning the steps, and the calculation efficiency is effectively improved.
Further, the path planning method may further include: when the path planning is not determined to be successful within the preset time length or when the path planning is not determined to be successful within the preset total number of turns of the path planning, the resolution of the grid map is modified, and then the step A (namely, the step S11 in FIG. 1) is returned to start again.
In the embodiment of the invention, the long-time invalid search can be avoided by setting the termination condition.
Referring to fig. 2, fig. 2 is a flowchart of another path planning method based on the bidirectional hybrid a-star algorithm in the embodiment of the present invention. The other path planning method based on the bidirectional hybrid a-algorithm may include steps S201 to S225, which are described below.
It is to be noted that steps S202 to S210 are execution steps relating to forward search, steps S212 to S220 are execution steps relating to backward search, and steps S222 to S225 are execution steps relating to bidirectional search.
In step S201, the map is rasterized.
Specifically, the map may be rasterized with a preset resolution.
In step S202, a forward search open list is initialized.
In step S203, the forward search open list is updated.
Further details regarding the forward search open list may be provided with reference to the foregoing and the steps illustrated in fig. 1, and the step of updating the forward search open list may include removing the discarded forward nodes and may also include adding the newly generated forward search child nodes.
In step S204, a forward search child node is selected.
Specifically, the forward search child node with the minimum total cost value can be selected by referring to the steps in the foregoing and fig. 1.
In step S205, it is determined whether the selected forward search child node is the target node, if so, step S206 is continuously executed, otherwise, step S207 is continuously executed.
In step S206, the forward search is successful.
In step S207, it is determined whether the distance between the selected forward search sub-node and the target node is smaller than the threshold, if so, step S208 is continuously performed, otherwise, step S210 is continuously performed.
In step S208, the curves are connected.
Specifically, referring to the foregoing and the steps in fig. 1, a preset heuristic function curve algorithm may be adopted to perform curve connection on the selected forward search sub-node and the target node.
In step S209, it is determined whether a collision occurs between the curve and the obstacle, if so, step S210 is continuously executed, otherwise, step S206 is executed to determine that the forward search is successful.
In step S210, the traversal generates a forward search child node.
Specifically, the forward search child node selected in step S204 may be taken as a parent node, and the forward search child node is generated in a traversal manner.
In the embodiment of the present invention, through steps S202 to S210, a bidirectional hybrid a-star algorithm may be adopted to implement the search from the starting point to the end point.
In step S212, the post-search open list is initialized.
In step S213, the search open list is updated.
In step S214, a backward search child node is selected.
In step S215, it is determined whether the selected backward search child node is the start node, if so, step S216 is executed continuously, otherwise, step S217 is executed continuously.
In step S216, the backward search is successful.
In step S217, it is determined whether the distance between the selected backward search child node and the start node is less than the threshold, if so, step S218 is continuously performed, otherwise, step S230 is continuously performed.
In step S218, the curves are connected.
In step S219, it is determined whether a collision occurs between the curve and the obstacle, if yes, step S230 is continuously performed, otherwise, step S216 is performed, and it is determined that the backward search is successful.
In step S220, the backward search child node is generated by traversal.
Specifically, the backward search child node selected in step S214 may be used as a parent node, and the backward search child node is generated in a traversal manner.
In the embodiment of the present invention, through steps S202 to S210, a bidirectional hybrid a-star algorithm may be adopted to search from the end point to the start point.
Specifically, more contents of step S212 to step S220 can be executed with reference to the foregoing and the contents of step S202 to step S210, and are not described herein again.
In step S222, it is determined whether the distance between the forward search sub-node and the backward search sub-node selected in step S204 and step S214 is smaller than the threshold, and if the determination result is yes, step S223 is continuously executed.
It should be noted that, if the determination result is negative, the steps S210 and S220 may be executed respectively.
In step S223, the curves are connected.
In step S224, it is determined whether a collision occurs between the curve and the obstacle, and if yes, steps S210 and S220 are respectively executed, otherwise step S225 is executed.
In step S225, the bidirectional search is successful.
In the embodiment of the invention, by adopting the bidirectional hybrid A-x algorithm, the two opposite search directions from the starting point to the end point and from the end point to the starting point can be searched simultaneously, and compared with the situation that the end point is easy to encounter an obstacle during searching in a single direction, the interference of the obstacle on path searching can be reduced.
Referring to fig. 3, fig. 3 is a flowchart of a method for calculating a path length using the a-x algorithm according to an embodiment of the present invention. The method for calculating the path length by using the a-algorithm may include steps S31 to S33, and may further include steps S34 to S36, and each step is described below.
It should be noted that steps S31 to S33 are execution steps relating to forward search, and steps S34 to S36 are execution steps relating to backward search.
In step S31, in the process of calculating the first path length from the current forward node to the target node by using the a-x algorithm, each path node and the path length from the path node to the target node are cached from the current forward node every time the forward search child node is traversed and the total cost value of the forward search child node is calculated.
In the embodiment of the invention, the obstacle avoidance algorithm is an a-star algorithm; each time a forward search child node is traversed and a total cost value for the forward search child node is calculated, each way node and its path length to the target node are cached. Therefore, when the heuristic cost of each node based on the A-algorithm is calculated in the follow-up process, the situation that the path length from the same node to the end point is repeatedly traversed and calculated is effectively reduced, and the efficiency of calculating the path length based on the A-algorithm is improved. Compared with the prior art that when the position near the end point is searched, a large amount of node expansion is needed, so that the path searching rate is low, the scheme in the embodiment of the invention can improve the calculation efficiency on the basis of increasing the obstacle avoidance effect.
In step S32, if the current forward node or the forward search child node generated by the subsequent traversal is a node that has already been cached, the cached path length is used as the heuristic cost based on the a-x algorithm.
In the embodiment of the invention, if the current forward node or the forward search child node generated by subsequent traversal is a cached node, the cached path length is used as the heuristic cost based on the A-x algorithm, the confirmed actual path length can be reused when the total cost value is calculated, and the path length is more accurate than the path length obtained by each calculation, so that the accuracy of path planning can be further improved on the basis of improving the calculation efficiency.
In step S33, the forward search child node having the smallest total cost value based on the a-algorithm is determined.
Specifically, the minimum total cost value based on the a-algorithm is the minimum sum of the heuristic cost calculated based on the a-algorithm and the used cost.
In step S34, in the process of calculating the third path length from the current backward node to the start node by using the a-x algorithm, each path node and the path length from the path node to the start node are cached from the current backward node every time the backward search child node is traversed and the total cost value of the backward search child node is calculated.
In step S35, if the current backward node or the backward search child node generated by the subsequent traversal is a node that has already been cached, the cached path length is used as the heuristic cost based on the a-x algorithm.
In step S36, the backward search child node having the smallest total cost value based on the a-algorithm is determined.
For more details of step S34 to step S36, please refer to the description of step S31 to step S33 for execution, which is not repeated herein.
In a specific implementation, an open list of the a-algorithm may be preset, and the cached nodes are added to the open list of the a-algorithm.
The open list of a-algorithm may also be updated in subsequent searches, such as storing each forward search child node and backward search child node, and may be removed from the open list of a-algorithm after the child nodes are explored.
Further, if the current forward node is not added to the open list of the A-algorithm, traversing the forward search child nodes of the current forward node, and updating the open list of the A-algorithm; and/or traversing the backward searching child nodes of the current backward node and updating the open list of the A-algorithm if the current backward node is not added into the open list of the A-algorithm.
In the embodiment of the present invention, by updating the open list of the a-star algorithm, the newly generated forward search child node and the backward search child node may be added, and the discarded forward node and backward node may also be removed.
Further, the path planning method may further include: after a forward searching child node with the minimum total cost value based on the A-x algorithm is determined, backtracking from the target node to the starting node, and updating each path node and the path length from the path node to the target node in a cache; and/or, after determining a backward searching child node with the minimum total cost value based on the A-algorithm, backtracking from the starting node to the target node, and updating each path node and the path length from the path node to the starting node in a cache; wherein the minimum total cost value based on the A-algorithm is the minimum sum of the heuristic cost calculated based on the A-algorithm and the used cost.
In the embodiment of the invention, after the path of A is searched each time, the starting point is traced back by the node at the end, so that more path lengths from the node to the end point are recorded and multiplexed in the subsequent calculation process, and the efficiency of the subsequent calculation is further increased.
Referring to fig. 4, fig. 4 is a flowchart of another method for calculating a path length using the a-algorithm according to an embodiment of the present invention. The other method for calculating the path length by using the a-algorithm may include steps S41 to S417, which are described below.
It should be noted that steps S403 to S407 are execution steps relating to forward search, and steps S413 to S417 are execution steps relating to backward search.
In step S401, an open list is established.
In step S402, the open list is updated.
Specifically, more about the open list of algorithm a may be performed with reference to the steps shown in fig. 3 and described above, and the step of updating the open list of algorithm a may include storing each forward search child node and backward search child node, and may be removed from the open list after the child nodes are explored.
In step S403, it is determined that the current forward node algorithm is in the open list, if the determination result is yes, step S408 is continuously executed, otherwise step S404 is executed.
In step S404, a forward searching sub-node with the minimum total cost value is selected
In step S405, it is determined that the forward search child node algorithm is in the open list, if the determination result is yes, step S408 is continuously performed, otherwise step S406 is performed.
In step S406, the traversal generates a forward search child node and calculates a total cost value of the forward search child node.
In step S407, each way node and its path length to the target node are cached.
In step S408, the cached path length is used as a heuristic cost.
In the embodiment of the present invention, through steps S403 to S407, an a-star algorithm may be adopted to calculate the path length from the current forward node to the target node.
In step S413, it is determined that the current backward node algorithm is in the open list, if the determination result is yes, step S408 is continuously performed, otherwise step S414 is performed.
In step S414, a backward search child node with the minimum total cost value is selected.
In step S415, the algorithm for searching the child nodes after determining is in the open list, if the determination result is yes, step S408 is continuously executed, otherwise step S416 is executed.
In step S416, the traversal generates a backward search child node and calculates a total cost value of the backward search child node.
In step S417, each way node and its path length to the start node are cached.
In the embodiment of the present invention, through steps S413 to S417, the path length from the current backward node to the starting node may be calculated by using an a-x algorithm.
Specifically, more contents of step S401 to step S417 may be executed by referring to the contents of step S31 to step S36 and the above description is omitted here.
Referring to fig. 5, fig. 5 is a schematic structural diagram of a path planning apparatus based on a bidirectional hybrid a-x algorithm according to an embodiment of the present invention. The path planning device based on the bidirectional hybrid a-star algorithm may include:
a node determining module 51, configured to determine a starting node and a target node of a path search in the raster map;
a traversing module 52, configured to traverse and generate a forward search sub-node of the current forward node from the starting node, and traverse and generate a backward search sub-node of the current backward node from the target node;
the heuristic cost determining module 53 is configured to calculate, for each forward search sub-node, a first path length from a current forward node to the target node by using an obstacle avoidance algorithm, calculate a second path length from the current forward node to the target node by using a heuristic function curve algorithm different from the obstacle avoidance algorithm, and use a larger value of the first path length and the second path length as a heuristic cost of the current forward node; and/or for each backward search sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the heuristic cost of the current backward node.
In a specific implementation, the device may correspond to a chip having a data processing function in a terminal; or to a chip module including a chip having a data processing function in the terminal, or to the terminal.
For the principle, specific implementation and beneficial effects of the path planning apparatus based on the bidirectional hybrid a-algorithm, please refer to the related description about the path planning method based on the bidirectional hybrid a-algorithm described above, and details are not repeated herein.
Embodiments of the present invention further provide a computer-readable storage medium, on which a computer program is stored, where the computer program is executed by a processor to perform the steps of the above method. The computer readable storage medium may include, for example, non-volatile (non-volatile) or non-transitory (non-transitory) memory, and may also include optical disks, mechanical hard disks, solid state disks, and the like.
Specifically, in the embodiment of the present invention, the processor may be a Central Processing Unit (CPU), and the processor may also be other general-purpose processors, digital Signal Processors (DSPs), application Specific Integrated Circuits (ASICs), field Programmable Gate Arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, and the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
It will also be appreciated that the memory in the embodiments of the subject application can be either volatile memory or nonvolatile memory, or can include both volatile and nonvolatile memory. The nonvolatile memory may be a read-only memory (ROM), a Programmable ROM (PROM), an Erasable PROM (EPROM), an Electrically Erasable PROM (EEPROM), or a flash memory. Volatile memory can be Random Access Memory (RAM), which acts as external cache memory. By way of example and not limitation, many forms of Random Access Memory (RAM) are available, such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (enhanced SDRAM), synchronous DRAM (SLDRAM), synchronous Link DRAM (SLDRAM), and direct bus RAM (DR RAM).
The embodiment of the invention also provides a terminal, which comprises a memory and a processor, wherein the memory is stored with a computer program capable of running on the processor, and the processor executes the steps of the method when running the computer program. The terminal includes, but is not limited to, a mobile phone, a computer, a tablet computer, a server, a cloud platform, and other terminal devices.
The embodiment of the invention also provides a vehicle, and the vehicle can be integrated with or externally connected with the terminal.
It should be understood that the term "and/or" herein is merely one type of association relationship that describes an associated object, meaning that three relationships may exist, e.g., a and/or B may mean: a exists alone, A and B exist simultaneously, and B exists alone. In addition, the character "/" in this document indicates that the former and latter related objects are in an "or" relationship.
The "plurality" appearing in the embodiments of the present application means two or more.
The descriptions of the first, second, etc. appearing in the embodiments of the present application are only for illustrating and differentiating the objects, and do not represent the order or the particular limitation of the number of the devices in the embodiments of the present application, and do not constitute any limitation to the embodiments of the present application.
Although the present invention is disclosed above, the present invention is not limited thereto. Various changes and modifications may be effected by one skilled in the art without departing from the spirit and scope of the invention, as defined in the appended claims.

Claims (17)

1. A path planning method based on a bidirectional hybrid A-algorithm is characterized by comprising the following steps:
step A: determining an initial node and a target node of path search in the grid map;
and B: from the starting node, generating a forward search sub-node of the current forward node in a traversing manner, and from the target node, generating a backward search sub-node of the current backward node in a traversing manner;
and C: for each forward search sub-node, calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm, calculating to obtain a second path length from the current forward node to the target node by adopting an heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node;
and/or the presence of a gas in the gas,
and for each backward search sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the heuristic cost of the current backward node.
2. The path planning method according to claim 1, wherein one or more of the following are satisfied: the obstacle avoidance algorithm is selected from: a, an algorithm, a Voronoi diagram algorithm and an artificial potential field algorithm;
a heuristic function curve algorithm different from the obstacle avoidance algorithm is selected from: reeds-sheets curve algorithm and Dubins curve algorithm.
3. The path planning method according to claim 1, further comprising:
for each forward search sub-node and each backward search sub-node, determining the used cost by adopting a first penalty coefficient and/or a second penalty coefficient;
wherein the first penalty coefficient is used for representing that a node direction difference value between the forward searching child node and a parent node of the forward searching child node is larger than a first threshold value, or used for representing that a node direction difference value between the backward searching child node and a parent node of the backward searching child node is larger than the first threshold value;
the second penalty coefficient is used for representing that the gear direction of the forward searching child node is different from the gear direction between the father nodes of the forward searching child node, or representing that the gear direction of the backward searching child node is different from the gear direction between the father nodes of the backward searching child node.
4. A path planning method according to claim 3, wherein the used cost is determined using the following formula:
G＝G'+(w 1 +w 2 )×dis
wherein G is used for representing the used cost from the starting node to the forward searching child node or representing the used cost from the target node to the backward searching child node, G' is used for representing the used cost from the starting node to the parent node of the forward searching child node or representing the used cost from the target node to the parent node of the backward searching child node, w 1 A preset value, w, for representing said first penalty factor 2 A preset value used for representing the second penalty coefficient, dis is used for representing the forward searchThe path length from the parent node of the searching child node to the forward searching child node, or the path length from the parent node of the backward searching child node to the backward searching child node.
5. The path planning method according to claim 1, further comprising:
selecting a forward searching sub-node with the minimum total cost value and selecting a backward searching sub-node with the minimum total cost value;
the total cost value of the forward search child node is the sum of the heuristic cost and the used cost of the forward search child node, and the total cost value of the backward search child node is the sum of the heuristic cost and the used cost of the backward search child node.
6. The path planning method according to claim 5, further comprising:
if the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value meet any one of the following conditions, determining that the path planning is successful, otherwise, determining that the path planning is unsuccessful:
the distance between the forward searching sub-node with the minimum total cost value and the backward searching sub-node with the minimum total cost value is smaller than a first distance, and the forward searching sub-node and the backward searching sub-node are connected by adopting the heuristic function curve algorithm and do not collide with an obstacle;
the distance between the forward search subnode with the minimum total cost value and the target node is smaller than the first distance, and the forward search subnode is connected with the obstacle by adopting a heuristic function curve algorithm and does not collide with the obstacle;
the distance between the backward search subnode with the minimum total cost value and the starting node is smaller than the first distance, and the backward search subnode is connected with the barrier by adopting the heuristic function curve algorithm and does not collide with the barrier;
the forward searching child node with the minimum total cost value is the target node;
and the backward searching child node with the minimum total cost value is the starting node.
7. A path planning method according to claim 6, wherein said heuristic function curve algorithm is selected from the group consisting of Reeds-Sheeps curve algorithm and Dubins curve algorithm.
8. The path planning method according to claim 6, further comprising:
and D, when the path planning is not successful, discarding the current forward node and the current backward node, returning to the step B, traversing and generating the forward searching sub-node of the current forward node, merging the newly generated forward searching sub-node with the forward searching sub-node which is generated before and is not discarded, traversing and generating the backward searching sub-node of the current backward node, and merging the newly generated backward searching sub-node with the backward searching sub-node which is generated before and is not discarded.
9. The path planning method according to claim 8, further comprising:
and when the path planning is not determined to be successful within the preset time length or the path planning is not determined to be successful within the preset total number of turns of the path planning, modifying the resolution of the grid map, and returning to the step A.
10. The path planning method according to claim 1,
in the forward search, the attitude angle theta of the initial pose of the wheel in the path planning is adopted s An attitude angle theta representing the starting node and, when path planning is employed, the ending pose of the wheel e Representing the target node;
in the backward search, the attitude angle theta from the forward search is adopted e Inverted attitude angle theta s ' representing the target node, using an attitude angle theta from the time of forward search s Inverted attitude angle theta e ' denotes the start node.
11. The path planning method according to claim 1, wherein the obstacle avoidance algorithm is an a-x algorithm;
in the process of calculating the first path length from the current forward node to the target node by adopting the A-x algorithm, caching each path node and the path length from the path node to the target node from the current forward node when traversing the forward search child node and calculating the total cost value of the forward search child node;
and/or the presence of a gas in the atmosphere,
and in the process of calculating the third path length from the current backward node to the initial node by adopting the A-x algorithm, caching each path node and the path length from the path node to the initial node from the current backward node when traversing the backward search child nodes and calculating the total cost value of the backward search child nodes.
12. The path planning method according to claim 11, further comprising:
if the current forward node or the forward search child node generated by subsequent traversal is a cached node, adopting the cached path length as the heuristic cost based on the A-x algorithm;
and/or the presence of a gas in the gas,
and if the current backward node or the backward search child node generated by the subsequent traversal is a cached node, adopting the cached path length as the heuristic cost based on the A-algorithm.
13. The path planning method according to claim 11, wherein cached nodes are added to the open list of the a-algorithm, the method further comprising:
if the current forward node is not added to the open list of the A-algorithm, traversing the forward search child nodes of the current forward node, and updating the open list of the A-algorithm;
and/or the presence of a gas in the gas,
and if the current backward node is not added into the open list of the A-algorithm, traversing the backward search child nodes of the current backward node, and updating the open list of the A-algorithm.
14. The path planning method according to claim 11, further comprising:
after a forward searching child node with the minimum total cost value based on the A-x algorithm is determined, backtracking from the target node to the starting node, and updating each path node and the path length from the path node to the target node in a cache;
and/or the presence of a gas in the atmosphere,
after a backward searching child node with the minimum total cost value based on the A-star algorithm is determined, the starting node backtracks to the target node, and each path node and the path length from the path node to the starting node are updated in a cache;
wherein the minimum total cost value based on the A-algorithm is the minimum sum of the heuristic cost calculated based on the A-algorithm and the used cost.
15. A path planning device based on a bidirectional hybrid A-algorithm is characterized by comprising:
the node determining module is used for determining a starting node and a target node of path search in the raster map;
the traversal module is used for generating a forward search sub-node of the current forward node in a traversal mode from the starting node and generating a backward search sub-node of the current backward node in a traversal mode from the target node;
the heuristic cost determining module is used for calculating to obtain a first path length from a current forward node to the target node by adopting an obstacle avoidance algorithm for each forward search sub-node, calculating to obtain a second path length from the current forward node to the target node by adopting a heuristic function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the first path length and the second path length as the heuristic cost of the current forward node;
and/or the presence of a gas in the gas,
and for each backward searching sub-node, calculating to obtain a third path length from the current backward node to the initial node by adopting the obstacle avoidance algorithm, calculating to obtain a fourth path length from the current backward node to the initial node by adopting an enlightening function curve algorithm different from the obstacle avoidance algorithm, and taking the larger value of the third path length and the fourth path length as the enlightening cost of the current backward node.
16. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, performs the steps of the bi-directional hybrid a-algorithm based path planning method according to any one of claims 1 to 14.
17. A terminal comprising a memory and a processor, the memory having stored thereon a computer program operable on the processor, wherein the processor when executing the computer program performs the steps of the two-way hybrid a x algorithm based path planning method of any of claims 1 to 14.
CN202210603043.3A 2022-05-30 2022-05-30 Path planning method and device based on bidirectional hybrid A-algorithm and terminal Pending CN115291597A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210603043.3A CN115291597A (en) 2022-05-30 2022-05-30 Path planning method and device based on bidirectional hybrid A-algorithm and terminal

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210603043.3A CN115291597A (en) 2022-05-30 2022-05-30 Path planning method and device based on bidirectional hybrid A-algorithm and terminal

Publications (1)

Publication Number Publication Date
CN115291597A true CN115291597A (en) 2022-11-04

Family

ID=83820941

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210603043.3A Pending CN115291597A (en) 2022-05-30 2022-05-30 Path planning method and device based on bidirectional hybrid A-algorithm and terminal

Country Status (1)

Country Link
CN (1) CN115291597A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115892075A (en) * 2023-01-06 2023-04-04 阿里巴巴达摩院(杭州)科技有限公司 Trajectory planning method, automatic driving device and computer storage medium
CN115930969A (en) * 2023-01-09 2023-04-07 季华实验室 Path planning method and device for mobile robot, electronic equipment and storage medium
CN116976535A (en) * 2023-06-27 2023-10-31 上海师范大学 Path planning algorithm based on fusion of few obstacle sides and steering cost
CN116976535B (en) * 2023-06-27 2024-05-17 上海师范大学 Path planning method based on fusion of few obstacle sides and steering cost

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115892075A (en) * 2023-01-06 2023-04-04 阿里巴巴达摩院(杭州)科技有限公司 Trajectory planning method, automatic driving device and computer storage medium
CN115930969A (en) * 2023-01-09 2023-04-07 季华实验室 Path planning method and device for mobile robot, electronic equipment and storage medium
CN116976535A (en) * 2023-06-27 2023-10-31 上海师范大学 Path planning algorithm based on fusion of few obstacle sides and steering cost
CN116976535B (en) * 2023-06-27 2024-05-17 上海师范大学 Path planning method based on fusion of few obstacle sides and steering cost

Similar Documents

Publication Publication Date Title
CN111857160B (en) Unmanned vehicle path planning method and device
CN115291597A (en) Path planning method and device based on bidirectional hybrid A-algorithm and terminal
CN111426330B (en) Path generation method and device, unmanned transportation system and storage medium
US20180284785A1 (en) System and Method for Controlling Motion of Vehicle in Shared Environment
CN112099496B (en) Automatic driving training method, device, equipment and medium
CN108444490B (en) Robot path planning method based on depth fusion of visible view and A-x algorithm
CN108519094B (en) Local path planning method and cloud processing terminal
Ulbrich et al. Graph-based context representation, environment modeling and information aggregation for automated driving
Jesenski et al. Generation of scenes in intersections for the validation of highly automated driving functions
CN113359718B (en) Method and equipment for fusing global path planning and local path planning of mobile robot
CN114440916B (en) Navigation method, device, equipment and storage medium
CN109916421B (en) Path planning method and device
CN112444263B (en) Global path planning method and device
CN113188562B (en) Path planning method and device for travelable area, electronic equipment and storage medium
CN112700479B (en) Registration method based on CNN point cloud target detection
CN112731941B (en) Biped robot path planning method and device and biped robot
US20230071794A1 (en) Method and system for building lane-level map by using 3D point cloud map
CN112229419A (en) Dynamic path planning navigation method and system
EP4119412A1 (en) Vehicle-based data processing method and apparatus, computer, and storage medium
CN114281084B (en) Intelligent vehicle global path planning method based on improved A-algorithm
CN115077553A (en) Method, system, automobile, equipment and medium for planning track based on grid search
KR20170070488A (en) Method and apparatus for automatically generating drive route
CN112732844A (en) Method, apparatus, device and medium for automatically associating road object with road
WO2023135271A1 (en) Motion prediction and trajectory generation for mobile agents
CN116295478A (en) Navigation method and device for vehicle

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