CN110849385A - Trajectory planning method and system for searching conjugate gradient descent based on double-layer heuristic search - Google Patents
Trajectory planning method and system for searching conjugate gradient descent based on double-layer heuristic search Download PDFInfo
- Publication number
- CN110849385A CN110849385A CN201911192327.2A CN201911192327A CN110849385A CN 110849385 A CN110849385 A CN 110849385A CN 201911192327 A CN201911192327 A CN 201911192327A CN 110849385 A CN110849385 A CN 110849385A
- Authority
- CN
- China
- Prior art keywords
- node
- gradient descent
- conjugate gradient
- searching
- double
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a trajectory planning method and a trajectory planning system for searching conjugate gradient descent based on double-layer heuristic search, which comprises the following steps of establishing a queue for storing starting point information in an initialization stage; opening a list to obtain a starting point, a target point and a vehicle motion model; selecting the motion mode with the minimum cost value from the opening list as a father node; calculating and judging surrounding nodes based on the father node, recording the nodes as expansion nodes and generating tracks after preset conditions are met; the generated trajectory is optimized using a conjugate gradient descent method. The invention has the beneficial effects that: based on the incomplete barrier-free heuristic search, aiming at solving the search cost consumption of wrong directions, guiding the track to move towards the target point; search for a barrier-based heuristic to address directing trajectories away from obstacles or cul-de-sac areas based on the integrity; and optimizing the generated track based on a conjugate gradient descent method, and solving the problem of track oscillation.
Description
Technical Field
The invention relates to the technical field of automatic driving motion planning, in particular to a trajectory planning method for searching conjugate gradient descent based on double-layer heuristic and a trajectory planning system for searching conjugate gradient descent based on double-layer heuristic.
Background
In recent years, in graph theory, the most widely applied is a search algorithm, such as depth-first search, breadth-first search, and the like. In addition to the brute force search algorithm of depth-first and breadth-first, some shortest path algorithms can also be used to find the shortest path, and the efficiency is higher than that of depth-first and breadth-first search. In some large-scale network games, a scene often exists, and you in the games need to walk from the current position to the destination, and you only need to find the target position in the map and click to automatically find the way. In an actual scene, a game map is usually large, and paths, obstacles and the like in the game map are not very regular and cannot be easily abstracted into nodes and line segments. All obstacles need to be bypassed in the process of route finding, and a path which cannot be bypassed is found. It seems that the shortest path is the most satisfactory, but if the map is large and the nodes are many, the algorithm for the shortest path is still too inefficient to perform. The route planning of map software in daily life does not need to find the shortest route, but finds a suboptimal solution under the condition of balancing efficiency and route quality, and the A-star algorithm is a search algorithm capable of balancing the efficiency and the route quality and finding a suboptimal route.
However, the existing A-star algorithm searches in a discrete space, the generated track is not continuous, the kinematic constraint of a vehicle is not considered, the track is directed at the motion of a mass point model, a path has no feasibility in the actual vehicle running process, the track is not optimized, and the track point has a vibration problem.
Disclosure of Invention
This section is for the purpose of summarizing some aspects of embodiments of the invention and to briefly introduce some preferred embodiments. In this section, as well as in the abstract and the title of the invention of this application, simplifications or omissions may be made to avoid obscuring the purpose of the section, the abstract and the title, and such simplifications or omissions are not intended to limit the scope of the invention.
The present invention has been made in view of the above-mentioned conventional problems.
Therefore, one technical problem solved by the present invention is: the trajectory planning method based on double-layer heuristic search conjugate gradient descent is provided, vehicle kinematic constraints are considered, the trajectory is optimized, and oscillation of trajectory points is reduced.
In order to solve the technical problems, the invention provides the following technical scheme: a trajectory planning method for searching conjugate gradient descent based on double-layer heuristic search comprises the following steps of establishing a queue for storing starting point information in an initialization stage; opening a list to obtain a starting point, a target point and a vehicle motion model; selecting the motion mode with the minimum cost value from the opening list as a father node; calculating and judging surrounding nodes based on the father node, recording the nodes as expansion nodes and generating tracks after preset conditions are met; the generated trajectory is optimized using a conjugate gradient descent method.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: and selecting the minimum cost value comprises constructing a total cost function, wherein the total cost function is equal to the sum of the historical cost function and the heuristic cost function, and the minimum total cost is obtained to be used as the current required minimum cost value.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the calculation and judgment comprise that the value from the current expansion node to the father node is solved according to the Euclidean distance or the Manhattan distance; selecting the smallest value; and recording the node at the moment as an expansion node.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the vehicle motion model satisfies the kinematic constraint of the vehicle, and comprises the following building steps,
defining an initial model:
the formula above can be combined to obtain:
assuming that the rate of change of direction of the vehicle is equal to the angular velocity of the vehicle, the angular velocity of the vehicle is
Substituting the angular velocity results in:
obtaining the vehicle kinematic model under an inertial coordinate system XY:
as a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the vehicle kinematics model includes an output δf、δrAnd V, and the output slip angle β, including the model, is,
as a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the starting point push enters an OPEN priority queue; taking a head node of an OPEN priority queue; setting an index of the node; if the node index is located in a closed table, performing next traversal, and if the node index is located in a closed table, performing expansion; the node enters a closed table and pops from an Open table; detecting whether the current node is an end point or exceeds the maximum time of calculation; if the node is currently going forward, the dubins curve from the current point to the end point is calculated and can be reached, and then the current node is returned.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the expansion node comprises the following setting steps of establishing a next expansion node; setting an extended node index identifier; judging whether the expansion node meets the constraint or not and traversing or not; determining that the newly expanded node is not in a closed table or has not been traversed; updating the G value; if the extension node is not in the Open table or a path with a shorter G value is found; updating the H value; if the expansion node and the current node are in the same grid and the cost value is larger, skipping, and if not, reserving the expansion node and updating the front node.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the track optimization comprises the steps of establishing an optimization model; determining an optimization direction; selecting an optimized step length; and finally simplifying, including optimizing direction simplification, step size simplification and gradient calculation simplification.
As a preferred scheme of the trajectory planning method for searching conjugate gradient descent based on the double-layer heuristic method, the method comprises the following steps: the optimization model comprises initialization definitions x (k) a solution vector representing a k-th iteration, d (k) a direction vector representing the k-th iteration, and r (k) a residual vector representing the k-th iteration; then x (0) is 0, d (k) is 0, r (0) is 0; and performing the kth iteration, calculating a residual vector, calculating a step size and updating a solution vector.
The invention solves the technical problems that: the trajectory planning system for searching conjugate gradient descent based on the double-layer heuristic search is provided, the vehicle kinematic constraint is considered, the trajectory is optimized, and the oscillation of the trajectory point is reduced.
In order to solve the technical problems, the invention provides the following technical scheme: a trajectory planning system for searching conjugate gradient descent based on double-layer heuristic search comprises an initialization module, an operation module, a trajectory generation module and an optimization module; the initialization module is used for creating a queue for storing starting point information; the operation module is connected with the initialization module and is used for obtaining a starting point, a target point and a vehicle motion model from the initialized queue and selecting a motion mode with the minimum cost value from the starting list as a father node; the track generation module is connected with the operation module and used for calculating and judging surrounding nodes based on the father node according to the calculation result of the operation module to generate tracks; the optimization module is used for optimizing the generated track.
The invention has the beneficial effects that: based on the incomplete barrier-free heuristic search, aiming at solving the search cost consumption of wrong directions, guiding the track to move towards the target point; search for a barrier-based heuristic to address directing trajectories away from obstacles or cul-de-sac areas based on the integrity; and optimizing the generated track based on a conjugate gradient descent method, and solving the problem of track oscillation.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive exercise. Wherein:
fig. 1 is a schematic algorithm flow diagram of a trajectory planning method for searching conjugate gradient descent based on double-layer heuristic search according to a first embodiment of the present invention;
fig. 2 is a schematic diagram illustrating a track generation effect according to a first embodiment of the present invention;
FIG. 3 is a schematic diagram illustrating the effect of the first embodiment of the present invention in an ultra-narrow obstacle environment;
fig. 4 is a schematic structural diagram of an overall principle of a trajectory planning system for searching for conjugate gradient descent based on a double-layer heuristic method according to a second embodiment of the present invention.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, specific embodiments accompanied with figures are described in detail below, and it is apparent that the described embodiments are a part of the embodiments of the present invention, not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without making creative efforts based on the embodiments of the present invention, shall fall within the protection scope of the present invention.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but the present invention may be practiced in other ways than those specifically described and will be readily apparent to those of ordinary skill in the art without departing from the spirit of the present invention, and therefore the present invention is not limited to the specific embodiments disclosed below.
Furthermore, reference herein to "one embodiment" or "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one implementation of the invention. The appearances of the phrase "in one embodiment" in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments.
The present invention will be described in detail with reference to the drawings, wherein the cross-sectional views illustrating the structure of the device are not enlarged partially in general scale for convenience of illustration, and the drawings are only exemplary and should not be construed as limiting the scope of the present invention. In addition, the three-dimensional dimensions of length, width and depth should be included in the actual fabrication.
Meanwhile, in the description of the present invention, it should be noted that the terms "upper, lower, inner and outer" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, and are only for convenience of describing the present invention and simplifying the description, but do not indicate or imply that the referred device or element must have a specific orientation, be constructed in a specific orientation and operate, and thus, cannot be construed as limiting the present invention. Furthermore, the terms first, second, or third are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
The terms "mounted, connected and connected" in the present invention are to be understood broadly, unless otherwise explicitly specified or limited, for example: can be fixedly connected, detachably connected or integrally connected; they may be mechanically, electrically, or directly connected, or indirectly connected through intervening media, or may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in specific cases to those skilled in the art.
Example 1
Referring to the schematic diagrams of fig. 1 to 3, an algorithm flow schematic diagram of a trajectory planning method for searching conjugate gradient descent based on double-layer heuristic search is provided for the present embodiment, and in order to solve the problem that the trajectory guidance is not accurate enough based on euclidean distance or manhattan distance in the existing single-layer heuristic search, the present embodiment is based on "incomplete obstacle-free" and "complete obstacle-having" double-layer heuristic search, and can solve the problems of excessive guidance consumption and effective obstacle avoidance.
Because the traditional algorithm only uses a single layer (such as the euclidean distance or the manhattan distance) as a basis, the state update is not optimal, and in the embodiment, when the update H value is calculated, the euclidean distance is used as a reference in the first layer, the dubins curve is used as an H value basis in the second layer, and the two layers are subjected to weighted fusion to obtain a final H value.
The a-star algorithm belongs to the classical algorithm in the field of motion planning, the algorithm idea is simple, and the conventional a-star algorithm is known to those skilled in the art, so that the description in the embodiment is omitted. Based on the improvement on the basis of the A-star algorithm, the core idea of the algorithm is to continue the heuristic search of the A-star algorithm.
It should be noted that the integrity constraint may simply be considered as being completely autonomously controlled. That is, if we consider a person as a point on the x-y plane, we can always go from one coordinate to another. A person may turn around a corner coin, walk straight ahead, sideways, backwards, etc. They can control the coordinates at all. However, the car is not complete because it cannot be fully controlled at any time. Thus, the vehicle is referred to as a non-integrity constraint. Cars can only turn around a circle of minimum radius, so they cannot move directly to a point just to their right and left. The Euclidean cost calculation and the motion model establishment adopted by the method just embody the two points.
Specifically, the trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic comprises the following steps,
s1: in the initialization stage, a queue for storing starting point information is established;
s2: the open list obtains a starting point, a target point, and a vehicle motion model.
Referring again to the illustration of fig. 1, the open list includes,
the starting point push enters an OPEN priority queue;
taking a head node of an OPEN priority queue;
setting index of the node;
if the node index is located in the closed table, performing next traversal, and if the node index is located in the closed table, performing expansion;
the node enters a closed table and pops from an Open table;
detecting whether the current node is an end point or exceeds the maximum time of calculation;
if the node is currently going forward, the dubins curve from the current point to the end point is calculated and can be reached, and then the current node is returned.
Further, in this embodiment:
the vehicle motion model satisfies the kinematic constraint of the vehicle, and comprises the following building steps,
defining an initial model:
the formula above can be combined to obtain:
assuming that the rate of change of direction of the vehicle is equal to the angular velocity of the vehicle, the angular velocity of the vehicle is
Substituting the angular velocity can yield:
obtaining a vehicle kinematic model under an inertial coordinate system XY:
the kinematic model of the vehicle includes an output deltaf、δrAnd V, and the output slip angle β, including the model, is,
s3: selecting the motion mode with the minimum cost value from the opening list as a father node; selecting the minimum cost value in the step comprises constructing a total cost function, wherein the total cost function is equal to the sum of the historical cost function and the heuristic cost function, and the minimum total cost is obtained and used as the current required minimum cost value.
S4: and calculating and judging surrounding nodes based on the father node, recording the nodes as expansion nodes and generating tracks after preset conditions are met.
The step includes the steps of calculating and judging,
solving the value from the current expansion node to the father node according to the Euclidean distance or the Manhattan distance;
selecting the minimum value;
and recording the node at the moment as an expansion node.
While the extension node comprises the following setup steps,
creating a next expansion node;
setting an extended node index identifier;
judging whether the expansion nodes meet the constraint or not, and traversing or not;
determining that the newly expanded node is not in a closed table or has not been traversed;
updating the G value;
if the extension node is not in the Open table or a path with a shorter G value is found;
updating the H value;
if the expansion node and the current node are in the same grid and the cost value is larger, skipping, and if not, reserving the expansion node and updating the front node.
S5: the generated trajectory is optimized using a conjugate gradient descent method.
The optimization of the trajectory includes that,
establishing an optimization model;
determining an optimization direction;
selecting an optimized step length;
and finally simplifying, including optimizing direction simplification, step size simplification and gradient calculation simplification.
And the optimization model includes, in addition,
initializing and defining xk to represent a solution vector of the kth iteration, dk to represent a direction vector of the kth iteration and rk to represent a residual vector of the kth iteration;
then x0 ═ 0, dk ═ 0, r0 ═ 0;
and performing the kth iteration, calculating a residual vector, calculating a step size and updating a solution vector.
In the embodiment, in order to verify the feasibility of the improved algorithm, different scenes are respectively selected and set as a starting point and an end point, the operation effect of the algorithm is as shown in the following fig. 2-3, as shown in fig. 1, a parking lot environment is from a position a to a position B, line1 is a double-layer heuristic search, and line2 is a track obtained by optimizing the double-layer heuristic search and conjugate gradient descent. Both tracks can be driven, and the purple line path is smoother. As shown in fig. 2, in an ultra-narrow obstacle environment, a route is planned from a position a to a position B, and line2 is a double-layer heuristic search + conjugate gradient descent method, so that the algorithm can be well used for obstacle avoidance driving in an ultra-narrow passage. It should be noted that, the evaluation planning route optimization includes shortest time, shortest path, slow curvature change rate of the curve, multi-step guidance of the curve, and best smoothness, so that the route optimization is not a single evaluation standard in the comprehensive view.
It should be recognized that embodiments of the present invention can be realized and implemented by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer readable memory. The methods may be implemented in a computer program using standard programming techniques, including a non-transitory computer-readable storage medium configured with the computer program, where the storage medium so configured causes a computer to operate in a specific and predefined manner, according to the methods and figures described in the detailed description. Each program may be implemented in a high level procedural or object oriented programming language to communicate with a computer system. However, the program(s) can be implemented in assembly or machine language, if desired. In any case, the language may be a compiled or interpreted language. Furthermore, the program can be run on a programmed application specific integrated circuit for this purpose.
Further, the operations of processes described herein can be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by context. The processes described herein (or variations and/or combinations thereof) may be performed under the control of one or more computer systems configured with executable instructions, and may be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) collectively executed on one or more processors, by hardware, or combinations thereof. The computer program includes a plurality of instructions executable by one or more processors.
Further, the method may be implemented in any type of computing platform operatively connected to a suitable interface, including but not limited to a personal computer, mini computer, mainframe, workstation, networked or distributed computing environment, separate or integrated computer platform, or in communication with a charged particle tool or other imaging device, and the like. Aspects of the invention may be embodied in machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, optically read and/or write storage medium, RAM, ROM, or the like, such that it may be read by a programmable computer, which when read by the storage medium or device, is operative to configure and operate the computer to perform the procedures described herein. Further, the machine-readable code, or portions thereof, may be transmitted over a wired or wireless network. The invention described herein includes these and other different types of non-transitory computer-readable storage media when such media include instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor. The invention also includes the computer itself when programmed according to the methods and techniques described herein. A computer program can be applied to input data to perform the functions described herein to transform the input data to generate output data that is stored to non-volatile memory. The output information may also be applied to one or more output devices, such as a display. In a preferred embodiment of the invention, the transformed data represents physical and tangible objects, including particular visual depictions of physical and tangible objects produced on a display.
Example 2
Referring to the illustration of fig. 4, the present embodiment provides a trajectory planning system for searching conjugate gradient descent based on a double-layer heuristic, which includes an initialization module 100, an operation module 200, a trajectory generation module 300, and an optimization module 400; specifically, the initialization module 100 is configured to create a queue for storing start point information; the operation module 200 is connected with the initialization module 100, and is used for obtaining a starting point, a target point and a vehicle motion model from the initialized queue and selecting a motion mode with the minimum cost value from an opening list as a father node; the track generation module 300 is connected to the operation module 200, and is configured to calculate and determine surrounding nodes based on the parent node according to a calculation result of the operation module 200 to generate a track; the optimization module 400 is configured to optimize the generated trajectory.
As used in this application, the terms "component," "module," "system," and the like are intended to refer to a computer-related entity, either hardware, firmware, a combination of hardware and software, or software in execution. For example, a component may be, but is not limited to being: a process running on a processor, an object, an executable, a thread of execution, a program, and/or a computer. By way of example, both an application running on a computing device and the computing device can be a component. One or more components can reside within a process and/or thread of execution and a component can be localized on one computer and/or distributed between two or more computers. In addition, these components can execute from various computer readable media having various data structures thereon. The components may communicate by way of local and/or remote processes such as in accordance with a signal having one or more data packets (e.g., data from one component interacting with another component in a local system, distributed system, and/or across a network such as the internet with other systems by way of the signal).
It should be noted that the above-mentioned embodiments are only for illustrating the technical solutions of the present invention and not for limiting, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that modifications or equivalent substitutions may 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, which should be covered by the claims of the present invention.
Claims (10)
1. A trajectory planning method for searching conjugate gradient descent based on double-layer heuristic search is characterized by comprising the following steps: comprises the following steps of (a) carrying out,
in the initialization stage, a queue for storing starting point information is established;
opening a list to obtain a starting point, a target point and a vehicle motion model;
selecting the motion mode with the minimum cost value from the opening list as a father node;
calculating and judging surrounding nodes based on the father node, recording the nodes as expansion nodes and generating tracks after preset conditions are met;
the generated trajectory is optimized using a conjugate gradient descent method.
2. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 1, wherein: and selecting the minimum cost value comprises constructing a total cost function, wherein the total cost function is equal to the sum of the historical cost function and the heuristic cost function, and the minimum total cost is obtained to be used as the current required minimum cost value.
3. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 1 or 2, characterized in that: the computational determination includes a determination that the computing device is,
solving the value from the current expansion node to the father node according to the Euclidean distance or the Manhattan distance;
selecting the smallest value;
and recording the node at the moment as an expansion node.
4. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 3, wherein: the vehicle motion model satisfies the kinematic constraint of the vehicle, and comprises the following building steps,
defining an initial model:
the formula above can be combined to obtain:
assuming that the rate of change of direction of the vehicle is equal to the angular velocity of the vehicle, the angular velocity of the vehicle is
Substituting the angular velocity results in:
obtaining the vehicle kinematic model under an inertial coordinate system XY:
5. the trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 4, wherein: the vehicle kinematics model includes an output δf、δrAnd a group V of the group V,and the output slip angle β comprising the model is,
6. the trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 5, wherein: the open list includes, for each of the plurality of active lists,
the starting point push enters an OPEN priority queue;
taking a head node of an OPEN priority queue;
setting an index of the node;
if the node index is located in a closed table, performing next traversal, and if the node index is located in a closed table, performing expansion;
the node enters a closed table and pops from an Open table;
detecting whether the current node is an end point or exceeds the maximum time of calculation;
if the node is currently going forward, the dubins curve from the current point to the end point is calculated and can be reached, and then the current node is returned.
7. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 5, wherein: the extension node comprises the following setup steps,
creating a next expansion node;
setting an extended node index identifier;
judging whether the expansion node meets the constraint or not and traversing or not;
determining that the newly expanded node is not in a closed table or has not been traversed;
updating the G value;
if the extension node is not in the Open table or a path with a shorter G value is found;
updating the H value;
if the expansion node and the current node are in the same grid and the cost value is larger, skipping, and if not, reserving the expansion node and updating the front node.
8. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 6 or 7, wherein: the optimizing of the trajectory may include optimizing the trajectory,
establishing an optimization model;
determining an optimization direction;
selecting an optimized step length;
and finally simplifying, including optimizing direction simplification, step size simplification and gradient calculation simplification.
9. The trajectory planning method for searching for conjugate gradient descent based on the double-layer heuristic as claimed in claim 5, wherein: the optimization model includes a set of models including,
initializing definition x (k) a solution vector representing a k-th iteration, d (k) a direction vector representing the k-th iteration, and r (k) a residual vector representing the k-th iteration;
then x (0) is 0, d (k) is 0, r (0) is 0;
and performing the kth iteration, calculating a residual vector, calculating a step size and updating a solution vector.
10. A trajectory planning system for searching conjugate gradient descent based on double-layer heuristic search is characterized in that: the system comprises an initialization module (100), an operation module (200), a track generation module (300) and an optimization module (400);
the initialization module (100) is used for creating a queue for storing starting point information;
the operation module (200) is connected with the initialization module (100) and is used for obtaining a starting point, a target point and a vehicle motion model from the initialized queue and selecting a motion mode with the minimum cost value from the opening list as a father node;
the track generation module (300) is connected with the operation module (200) and is used for calculating and judging surrounding nodes based on the father node according to the calculation result of the operation module (200) to generate a track;
the optimization module (400) is configured to optimize the generated trajectory.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911192327.2A CN110849385B (en) | 2019-11-28 | 2019-11-28 | Track planning method and system based on double-layer heuristic search conjugate gradient descent |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911192327.2A CN110849385B (en) | 2019-11-28 | 2019-11-28 | Track planning method and system based on double-layer heuristic search conjugate gradient descent |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110849385A true CN110849385A (en) | 2020-02-28 |
| CN110849385B CN110849385B (en) | 2023-09-22 |
Family
ID=69606600
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911192327.2A Active CN110849385B (en) | 2019-11-28 | 2019-11-28 | Track planning method and system based on double-layer heuristic search conjugate gradient descent |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110849385B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112683290A (en) * | 2020-12-29 | 2021-04-20 | 的卢技术有限公司 | Vehicle track planning method, electronic equipment and computer readable storage medium |
| CN113701780A (en) * | 2021-09-14 | 2021-11-26 | 成都信息工程大学 | Real-time obstacle avoidance planning method based on A-star algorithm |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444482A (en) * | 2018-06-15 | 2018-08-24 | 东北大学 | A kind of autonomous pathfinding barrier-avoiding method of unmanned plane and system |
| CN108958292A (en) * | 2018-08-23 | 2018-12-07 | 北京理工大学 | A kind of aircraft based on RRT* algorithm is dashed forward anti-method for planning track |
| CN110361013A (en) * | 2019-07-22 | 2019-10-22 | 上海应用技术大学 | A kind of path planning system and method for auto model |
-
2019
- 2019-11-28 CN CN201911192327.2A patent/CN110849385B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444482A (en) * | 2018-06-15 | 2018-08-24 | 东北大学 | A kind of autonomous pathfinding barrier-avoiding method of unmanned plane and system |
| CN108958292A (en) * | 2018-08-23 | 2018-12-07 | 北京理工大学 | A kind of aircraft based on RRT* algorithm is dashed forward anti-method for planning track |
| CN110361013A (en) * | 2019-07-22 | 2019-10-22 | 上海应用技术大学 | A kind of path planning system and method for auto model |
Non-Patent Citations (1)
| Title |
|---|
| 杜星星: ""复杂环境下无人车局部路径规划问题研究"", 《中国优秀硕士学位论文全文数据库 工程科技‖辑》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112683290A (en) * | 2020-12-29 | 2021-04-20 | 的卢技术有限公司 | Vehicle track planning method, electronic equipment and computer readable storage medium |
| CN113701780A (en) * | 2021-09-14 | 2021-11-26 | 成都信息工程大学 | Real-time obstacle avoidance planning method based on A-star algorithm |
| CN113701780B (en) * | 2021-09-14 | 2023-08-29 | 成都信息工程大学 | Real-time obstacle avoidance planning method based on A star algorithm |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110849385B (en) | 2023-09-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112325884B (en) | DWA-based ROS robot local path planning method | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| CN111174798A (en) | Foot type robot path planning method | |
| US11262764B2 (en) | Computer-implemented method and a system for defining a path for a vehicle within an environment with obstacles | |
| US20210103286A1 (en) | Systems and methods for adaptive path planning | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| Wen et al. | UAV online path planning algorithm in a low altitude dangerous environment | |
| CN111780777A (en) | Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning | |
| CN112888612A (en) | Autonomous vehicle planning | |
| CN108444490B (en) | Robot path planning method based on depth fusion of visible view and A-x algorithm | |
| CN114510057A (en) | ROS-based mobile robot autonomous navigation method in indoor environment | |
| CN111880561B (en) | Unmanned aerial vehicle three-dimensional path planning method based on improved whale algorithm in urban environment | |
| CN112306067B (en) | Global path planning method and system | |
| CN113359718B (en) | Method and equipment for fusing global path planning and local path planning of mobile robot | |
| CN112965471B (en) | Artificial potential field path planning method considering angular velocity constraint and improving repulsive field | |
| CN110849385A (en) | Trajectory planning method and system for searching conjugate gradient descent based on double-layer heuristic search | |
| CN114281084B (en) | Intelligent vehicle global path planning method based on improved A-algorithm | |
| CN114779772B (en) | Path planning method and device integrating global algorithm and local algorithm | |
| CN112947486A (en) | Path planning method and chip of mobile robot and mobile robot | |
| CN110954124A (en) | Adaptive path planning method and system based on A-PSO algorithm | |
| CN116009527A (en) | Path planning algorithm based on dynamic scene structure expansion perception | |
| CN114545921A (en) | Unmanned vehicle path planning algorithm based on improved RRT algorithm | |
| CN115903816A (en) | Low-energy-consumption mobile robot path planning method | |
| CN116610109A (en) | Gradient-based forward ant colony algorithm unmanned vehicle path planning method | |
| CN115326070A (en) | Unmanned aerial vehicle flight path planning method based on three-dimensional tangent line graph method in urban low-altitude environment |
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 |