CN115185263A - Trajectory planning method and computer readable storage medium - Google Patents
Trajectory planning method and computer readable storage medium Download PDFInfo
- Publication number
- CN115185263A CN115185263A CN202210546061.2A CN202210546061A CN115185263A CN 115185263 A CN115185263 A CN 115185263A CN 202210546061 A CN202210546061 A CN 202210546061A CN 115185263 A CN115185263 A CN 115185263A
- Authority
- CN
- China
- Prior art keywords
- node
- target
- nodes
- acquiring
- preset
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 230000000977 initiatory effect Effects 0.000 claims abstract description 6
- 238000004590 computer program Methods 0.000 claims description 25
- 238000012216 screening Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 description 29
- 238000004422 calculation algorithm Methods 0.000 description 21
- 230000006870 function Effects 0.000 description 10
- 230000008569 process Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
The application is applicable to the technical field of computers, and provides a trajectory planning method, which comprises the following steps: acquiring a starting node and a terminating node of a target vehicle, and acquiring state information corresponding to the starting node; establishing an open list, and adding an initial node into the open list; acquiring a candidate node with the minimum cost value in the open list as a target node; when the target node is not the termination node, acquiring the magnitude and the direction of the preset speed of the target vehicle, acquiring the next target node and the corresponding target state information thereof according to the magnitude and the direction of the preset speed, and judging whether the next target node is the termination node or not; and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes. The method carries out the track planning in real time, meets the requirements of the dynamic constraint and the dynamic solution of the vehicle, plans the track on the full-time airspace and improves the passing efficiency of the intersection.
Description
Technical Field
The present application relates to the field of computer technologies, and in particular, to a trajectory planning method and a computer-readable storage medium.
Background
The development of computer technology and communication technology makes intersection management and control means enter a new stage. The safety and the efficiency of the intersection can be effectively improved by planning and controlling the tracks of the networked automatic driving Vehicles (CAVs). The path search algorithm is a widely used method for finding an effective path, and common shortest path search algorithms include Dijkstra algorithm and a-x algorithm, however, the tracks searched by the algorithms do not conform to the kinematic characteristics of the vehicle, and cannot guide the action of the vehicle. Dolgov et al adds heuristic conditions based on the A-algorithm, and proposes a hybrid A-algorithm, which makes use of the path search algorithm in vehicle trajectory planning.
However, the hybrid a-star algorithm greatly increases the consumption of computing resources, and when trajectory planning is performed, the amount of computation is large, so that it is difficult to meet the real-time requirement, and the traffic efficiency of the autonomous vehicle cannot be further improved.
Disclosure of Invention
The embodiment of the application provides a trajectory planning method and a computer-readable storage medium, which can solve the problems.
In a first aspect, an embodiment of the present application provides a trajectory planning method, including:
acquiring a starting node and a terminating node of a target vehicle, and acquiring state information corresponding to the starting node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time;
establishing an open list, and adding the starting node into the open list; the open list is used for storing each candidate node and corresponding state information thereof;
acquiring the candidate node with the minimum cost value in the open list as a target node;
when the target node is not the termination node, acquiring the magnitude and direction of a preset speed of the target vehicle, acquiring a next target node and corresponding target state information thereof according to the magnitude and direction of the preset speed, and judging whether the next target node is the termination node or not;
and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes.
Further, the obtaining of the next target node and the corresponding target state information thereof according to the magnitude and the direction of the preset speed includes:
acquiring an initial node according to the preset speed and direction of the target vehicle;
and taking the initial node meeting the preset constraint condition as the next target node, and acquiring target state information corresponding to the next target node.
Further, the obtaining an initial node according to the magnitude and the direction of the preset speed of the target vehicle includes:
and acquiring iteration nodes according to the preset speed and the direction of the target vehicle, and taking the unprocessed iteration nodes as initial nodes.
Further, the obtaining of the iterative node according to the magnitude and the direction of the preset speed of the target vehicle includes:
acquiring a plurality of first nodes according to the preset speed and direction of the target vehicle;
determining a plurality of iteration nodes from the first nodes according to a preset grid interval; wherein the iteration nodes are in different preset grid intervals.
Further, the determining a plurality of iteration nodes from the first nodes according to a preset grid interval includes:
and when the plurality of first nodes are in the same preset grid interval, taking any one of the first nodes as an iteration node.
Further, the taking the initial node meeting the preset constraint condition as the next target node includes:
acquiring a second node meeting a preset collision constraint condition from the initial node;
and acquiring a third node meeting a preset range constraint condition from the second nodes, and taking the third node as the next target node.
Further, the obtaining a second node satisfying a preset collision constraint condition from the initial nodes includes:
acquiring a first boundary area of the initial node;
and determining a second node meeting a preset collision constraint condition according to the first boundary area.
Further, the acquiring a third node satisfying a preset range constraint condition from the second node includes:
when the second node is in a preset limit area, marking the second node as a fourth node;
and acquiring the course angle of the fourth node, and screening a second node from the fourth node according to the course angle of the fourth node, the course angle of the initial node and the course angle of the final node.
Further, the obtaining of the candidate node with the minimum cost value in the open list is a target node, and includes:
planning a target track from each candidate node to the termination node according to a Dubins curve, and calculating the length of the target track; wherein the target trajectory is composed of an arc and a straight line.
In a second aspect, an embodiment of the present application provides a trajectory planning apparatus, including:
the system comprises a first acquisition unit, a second acquisition unit and a third acquisition unit, wherein the first acquisition unit is used for acquiring a starting node and a terminating node of a target vehicle and acquiring state information corresponding to the starting node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time;
the first processing unit is used for establishing an open list and adding the starting node into the open list; the open list is used for storing each candidate node and corresponding state information thereof;
a second obtaining unit, configured to obtain the candidate node with the smallest cost value in the open list as a target node;
the second processing unit is used for acquiring the size and the direction of the preset speed of the target vehicle when the target node is not the termination node, acquiring the next target node and the corresponding target state information thereof according to the size and the direction of the preset speed, and judging whether the next target node is the termination node or not;
and the third processing unit is used for determining the driving track of the target vehicle according to the state information corresponding to the starting node, all the target nodes and the state information corresponding to the target nodes when the target node is the ending node.
Further, the second processing unit is specifically configured to:
acquiring an initial node according to the preset speed and the direction of the target vehicle;
and taking the initial node meeting the preset constraint condition as the next target node, and acquiring the target state information corresponding to the next target node.
Further, the second processing unit is specifically configured to:
and acquiring iterative nodes according to the preset speed and direction of the target vehicle, and taking the unprocessed iterative nodes as initial nodes.
Further, the second processing unit is specifically configured to:
acquiring a plurality of first nodes according to the preset speed and the preset direction of the target vehicle;
determining a plurality of iteration nodes from the first nodes according to a preset grid interval; wherein the iteration nodes are in different preset grid intervals.
Further, the second processing unit is specifically configured to:
and when the plurality of first nodes are in the same preset grid interval, taking any one of the first nodes as an iteration node.
Further, the second processing unit is specifically configured to:
acquiring a second node meeting a preset collision constraint condition from the initial node;
and acquiring a third node meeting a preset range constraint condition from the second nodes, and taking the third node as the next target node.
Further, the second processing unit is specifically configured to:
acquiring a first boundary area of the initial node;
and determining a second node meeting a preset collision constraint condition according to the first boundary area.
Further, the second processing unit is specifically configured to:
when the second node is in a preset limit area, marking the second node as a fourth node;
and acquiring the course angle of the fourth node, and screening a second node from the fourth node according to the course angle of the fourth node, the course angle of the initial node and the course angle of the final node.
Further, the second obtaining unit is specifically configured to:
planning a target track from each candidate node to the termination node according to a Dubins curve, and calculating the length of the target track; wherein the target trajectory is composed of an arc and a straight line.
In a third aspect, an embodiment of the present application provides a trajectory planning device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the method according to the first aspect is implemented.
In a fourth aspect, embodiments of the present application provide a computer-readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the computer program implements the method according to the first aspect.
In the embodiment of the application, a starting node and a terminating node of a target vehicle are obtained, and state information corresponding to the starting node is obtained; establishing an open list, and adding the starting node into the open list; acquiring a candidate node with the minimum cost value in the open list as a target node; when the target node is not the termination node, acquiring the size and the direction of a preset speed of the target vehicle, acquiring a next target node and target state information corresponding to the next target node according to the size and the direction of the preset speed, and judging whether the next target node is the termination node; and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes. According to the method, the state information comprises the target time corresponding to the starting node, the steering angle of the target vehicle at the target time and the course angle of the target vehicle at the target time, the hybrid A-x algorithm is expanded and is expanded to the time dimension, the requirements of dynamic constraint and dynamic solution of the vehicle are met when the track planning is carried out in real time, the track of the automatic driving vehicle is planned on the full-time airspace, and the passing efficiency of the intersection is improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive exercise.
Fig. 1 is a schematic flow chart of a trajectory planning method according to a first embodiment of the present application;
fig. 2 is a schematic diagram of an intersection in a trajectory planning method according to a first embodiment of the present application;
FIG. 3 is a schematic diagram of a dynamic model of an autonomous vehicle in a trajectory planning method according to a first embodiment of the present application;
FIG. 4 is a diagram illustrating a boundary area of a vehicle in a trajectory planning method according to a first embodiment of the present application;
FIG. 5 is a schematic diagram of a trajectory planning device according to a second embodiment of the present application;
fig. 6 is a schematic diagram of a trajectory planning apparatus according to a third embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should also be understood that the term "and/or" as used in this specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
As used in this specification and the appended claims, the term "if" may be interpreted contextually as "when", "upon" or "in response to a determination" or "in response to a detection". Similarly, the phrase "if it is determined" or "if a [ described condition or event ] is detected" may be interpreted contextually to mean "upon determining" or "in response to determining" or "upon detecting [ described condition or event ]" or "in response to detecting [ described condition or event ]".
Furthermore, in the description of the present application and the appended claims, the terms "first," "second," "third," and the like are used for distinguishing between descriptions and not necessarily for describing or implying relative importance.
Reference throughout this specification to "one embodiment" or "some embodiments," or the like, means that a particular feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of the present application. Thus, appearances of the phrases "in one embodiment," "in some embodiments," "in other embodiments," or the like, in various places throughout this specification are not necessarily all referring to the same embodiment, but rather "one or more but not all embodiments," unless otherwise expressly specified. The terms "comprising," "including," "having," and variations thereof mean "including, but not limited to," unless expressly specified otherwise.
The trajectory planning means that a driving trajectory which meets internal/external constraint conditions is calculated between the initial state and the final state of the moving object, wherein the internal constraint mainly comprises kinematics characteristics, and the external constraint mainly comprises collision avoidance or other user-defined added limits.
Referring to fig. 1, fig. 1 is a schematic flow chart of a trajectory planning method according to a first embodiment of the present application. The main execution body of the trajectory planning method in this embodiment is a device with a trajectory planning function, such as a desktop computer, a server, and the like. The trajectory planning method as shown in fig. 1 may include:
s101: acquiring a starting node and a terminating node of a target vehicle, and acquiring state information corresponding to the starting node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time.
In this embodiment, the trajectory planning refers to calculating a driving trajectory meeting internal or external constraint conditions between the initial state and the final state of the moving object. The moving object may be a moving vehicle. Therefore, in this embodiment, when the unmanned trajectory planning is performed, the device needs to first acquire the start node and the end node of the target vehicle.
The device acquires the start node and the end node of the target vehicle from a preset map, and the device can acquire the position information of the start node and the end node, wherein the position information can comprise the coordinate information of the start node and the end node. The preset map may be a local map of a road, as shown in fig. 2, and may also include a complete intersection, where fig. 2 is an intersection of four-entrance roads and 6 bidirectional lanes, an arrow indicates canalization of the intersection, the innermost lane is used for left-turning of a vehicle, the middle lane is used for straight-going of the vehicle, and the outermost lane is used for right-turning of the vehicle. RW represents the width of the lane. The signal lamp is used for controlling a manually driven vehicle. And the automatic driving vehicle runs according to the planned track and is not restricted by the signal lamp. In addition, the preset map may include road, obstacle, road condition information, and the like, which is not limited herein.
When planning the track, the state information of the nodes is further obtained, wherein it can be understood that the state information of the start node includes the position information of the start node, and in addition, the state information of the start node includes the target time corresponding to the start node, the steering angle of the target vehicle at the target time, and the heading angle of the target vehicle at the target time.
In this embodiment, the motion of the vehicle is limited by the speed, the acceleration, and the heading angle, and the driving process needs to satisfy a certain constraint, in this embodiment, an automatic driving vehicle dynamics model is constructed, and some parameters in the state information of the vehicle node can be determined by the model, as shown in fig. 3 and the following formula:
wherein (x) i (t),y i (t)) represents the center point of the rear axle of the target vehicle i at time t; theta i (t) represents a heading angle of the target vehicle at time t; phi is a unit of i (t) is the steering angle; a is i (t) acceleration of the vehicle; omega i (t) is the angular velocity of the vehicle. To minimize the delay in the passage of the subject vehicle through the intersection, we assume that the vehicle is at a maximum speed v max Passing through the intersection, simultaneously steering the vehicle at maximum and maximum anglesThe large angular velocity is limited:
v i (t)=v max
in order to be capable of dynamic solution, a time dimension is added to the state information of each node. In this way, a trajectory that has already been planned can be considered a dynamic obstacle. The path with the highest efficiency can be searched out on the premise of ensuring the safety of the subsequently planned track.
S102: establishing an open list, and adding the starting node into the open list; the open list is used for storing each candidate node and corresponding state information thereof.
The device establishes an open list for storing each candidate node and its corresponding state information, that is, the open list is a set of candidate nodes, which are position nodes where the target vehicle may travel. The state information corresponding to the candidate node comprises the position information of the candidate node, the time corresponding to the candidate node, the steering angle of the vehicle at the time and the heading angle of the vehicle at the time, and the attitude, the speed and the like of the vehicle.
The device adds the start node to the open list, which may be empty before the start node is added, i.e. the start node is the first node to add to the open list. The open list may not be empty until the start node joins.
S103: and acquiring the candidate node with the minimum cost value in the open list as a target node.
The device obtains the cost value of each candidate node in the open list, wherein the cost value is used for representing the cost consumed by the target vehicle from the starting node to the candidate node and then from the candidate node to the ending node. Factors that may be considered when calculating the cost value include, but are not limited to, the length of the trajectory, the time required for movement, the presence of obstacles, and the like.
Generally, when the cost value is calculated, a heuristic function of a hybrid a-star algorithm can be used for calculation, wherein the heuristic function of the hybrid a-star algorithm consists of two parts, namely a cost value (cost) of the a-star algorithm and a cost value of a Reeds-Shepp curve, and the larger value of the cost value and the cost value is used as the final cost value of the node.
In another embodiment, the device may use the Dubins curve instead of the Reeds-Shepp curve to calculate the cost value. The equipment plans a target track from each candidate node to the termination node according to the Dubins curve and calculates the length of the target track, wherein the target track is composed of an arc and a straight line. In particular, the cost value may refer to the distance from the current node to the target node, which is not a straight-line distance, but the length of the trajectory planned according to the Dubins curve. Particularly, the track of the vehicle is influenced by factors such as the maximum heading angle and the like during running, so the track is a track formed by combining an arc and a straight line.
Let S denote the Motion Primitive of straight going of the vehicle (Motion Primitive), L denote the Motion Primitive of left turn of the vehicle (Motion Primitive), and R denote the Motion Primitive of right turn of the vehicle (Motion Primitive). It can be demonstrated that the shortest path from any starting point to an end point can be made up of no more than three Motion primitives. The sequence of three Motion primitives (Motion primatives) is called a Word. Since two consecutive identical Motion primitives (Motion primatives) can be merged into one, there are 10 combinations of all possible words, and Dubins proves that the optimal combination can only be one of the following 6 combinations:
L α R β L γ ,R α L β R γ ,L α S d L γ ,L α S d R γ ,R α S d L γ ,R α S d R γ
wherein the subscripts denote radians or distances, such as L α Indicating that the target vehicle is turned rightward by a maximum steering angle (the steering operation should be performed at the maximum steering angle because the shortest distance is obtained, and the turning radius is the minimum turning radius of the vehicle), S d Represents the distance d of the straight running of the target vehicle; α, γ ∈ [0,2 π), β ∈ [ π,2 π ], when β ∈ [ β ]<There must be other sequences that are better than the current sequence at π. And thenAnd calculating the cost values of the nodes, namely calculating the 6 sequences respectively, and then taking the node with the minimum cost value as the current cost value.
For radian and distance calculations, the above 6 words can be actually classified into two categories, namely CSC and CCC. Where C denotes Circle and S denotes Stright. The CSC type is calculated by determining two circle centers according to a starting point and an end point and then obtaining a common tangent of the two circles. For the CCC type Word, the three tangent circles are used for calculating the radian of three turns.
In addition, it can be understood that when the open list is empty, an error occurs, and the trajectory planning does not have a valid path, and the trajectory planning needs to be performed again.
S104: and when the target node is not the termination node, acquiring the size and the direction of the preset speed of the target vehicle, acquiring the next target node and the corresponding target state information thereof according to the size and the direction of the preset speed, and judging whether the next target node is the termination node.
The equipment judges whether the target node is a termination node, when the target node is not the termination node, the angular velocity of the target vehicle is obtained, the next target node and the corresponding target state information of the next target node are obtained according to the angular velocity of the target vehicle, and whether the next target node is the termination node or not is judged.
In fact, the process of continuous expansion is continuously expanded from the initial node to the final node, so when the target node is not the final node, the process of continuous expansion is needed to find the next target node. Therefore, it can be understood that this step is a loop step, and the loop is restarted to acquire the next target node each time it is determined that the target node is not the end node.
The device obtains the size and the direction of the preset speed of the target vehicle, obtains the position of the next target node according to the size and the direction of the preset speed of the target vehicle, and obtains the corresponding target state information of the next target node. Specifically, the device may acquire an initial node according to the angular velocity of the target vehicle, and thenAnd taking the initial node meeting the preset constraint condition as the next target node, and acquiring the target state information corresponding to the next target node. The magnitude and direction of the preset speed of the target vehicle may be the maximum angular velocity of the target vehicle, set ω max For the maximum angular velocity of the target vehicle, the current target nodes are respectively rotated by-omega max ,0,ω max And obtaining a plurality of initial nodes from a plurality of angles. The device stores preset constraint conditions in advance, and the preset constraint conditions are used for screening out the next target node from the initial nodes. The preset constraint condition may include a preset range constraint condition, a preset collision constraint condition, and the like, without limitation.
In this embodiment, in order to avoid that the path planned by the algorithm is invalid and to further increase the speed of the algorithm, the specific implementation method of taking the initial node that meets the preset constraint condition as the next target node may include: the equipment acquires a second node meeting a preset collision constraint condition from the initial node; and acquiring a third node meeting the preset range constraint condition from the second node, and taking the third node as a next target node. Namely, the device screens the initial nodes according to the preset collision constraint conditions to screen out the second nodes, the initial nodes which are possibly collided are discarded and not used as the second nodes, then the third nodes are screened out from the second nodes according to the preset range constraint conditions, and the second nodes which are not in the preset range are also discarded and not used as the third nodes.
In order to further ensure the driving safety of the vehicle passing through the intersection, the specific implementation mode of obtaining the second node satisfying the preset collision constraint condition from the initial node may be as follows: and acquiring a first boundary area of the initial node, and determining a second node meeting a preset collision constraint condition according to the first boundary area. In order to ensure the calculation accuracy, a boundary area is added to the vehicle in the preset collision condition set by the device in the embodiment, as shown in fig. 4, the boundary area of the automatic driving vehicle is a square which is completely slightly larger than the vehicle body, and the driving safety of the vehicle can be ensured. The manually driven vehicle will take into account the uncertainty of its travel in the subsequent modeling, with a 10% fluctuation in speed. For simplicity of calculation, the manned vehicle is wrapped using the circumscribed circle as a boundary. When the boundary areas of the two vehicles have an overlapping part, namely, the two vehicles are judged to generate conflict, the conflict can be generated.
Further, to facilitate mixed-flow traffic management, the device may act on the human-driven vehicle along a fixed trajectory generated by a Bezier curve. The Bezier curve may generate a smooth curve through a given control point. Unlike an autonomous vehicle that can travel completely along a planned trajectory, a manually driven vehicle cannot travel completely along a given trajectory, allowing the speed of the manually driven vehicle to fluctuate by 10%. And all possible positions of the human-driven vehicle are considered as dynamic obstacles in the trajectory planning of the autonomous vehicle.
In order to avoid the algorithm planning a path that is invalid and to further limit the search area and increase the speed of the algorithm, the device defines a limit area for each steered vehicle. When the third node meeting the preset range constraint condition is obtained from the second node, the equipment marks the second node as a fourth node when the second node is in the preset limit area, obtains the heading angle of the fourth node, and screens out the second node from the fourth node according to the heading angle of the fourth node, the heading angle of the initial node and the heading angle of the final node. Specifically, when searching for a node, if the node corresponding to the action exceeds the restricted area, the node is directly discarded. Furthermore, there is also a limit to the steering of the action, such as a straight-going vehicle, during which the angle should not be reversed. Thus, in this algorithm, the angle of each node in the trajectory, and the heading angle θ of the origin 0 (and heading angle θ of endpoint) f ) Cannot exceed 90 °, i.e.:and
in one embodiment, when the initial node is obtained according to the magnitude and direction of the preset speed of the target vehicle, the iterative node may be obtained according to the magnitude and direction of the preset speed of the target vehicle, and the unprocessed iterative node may be used as the initial node. That is, the devices are respectively facing- ω with the current target node max ,0,ω max If the direction advances by a step length distance, a plurality of iteration nodes can be obtained, the device judges whether the iteration nodes are processed or not, if the iteration nodes are processed, the iteration nodes are found to be searched and possibly expanded, further processing is not needed, the processed iteration nodes can be discarded, the processed node devices can be marked, and the processing state of the nodes can be conveniently obtained. The device judges whether the iteration node is processed or not, takes the unprocessed iteration node as an initial node, and further judges whether the node can be used as a next target node or not.
On the other hand, in order to reduce the search time of the algorithm, when the device acquires the iteration nodes according to the magnitude and direction of the preset speed of the target vehicle, the following method can be adopted: acquiring a plurality of first nodes according to the preset speed and direction of the target vehicle; determining a plurality of iteration nodes from the first nodes according to a preset grid interval; and the iteration nodes are in different preset grid intervals.
Further, the specific manner of determining the plurality of iteration nodes from the first nodes according to the preset grid interval may be: and when the plurality of first nodes are in the same preset grid interval, taking any first node as an iteration node.
In this embodiment, 3D rasterization is used to limit the search area of the algorithm. Since the heuristic function of the modified hybrid a-star algorithm is the Dubins curve, many similar nodes are searched when searching for a node. To avoid searching for redundant nodes and to increase algorithm search speed, the device rasterizes the intersection control area in the dimension of (x, y, θ). In order to enable the model to be dynamically solved, the time dimension is added to each node, and the node information is changed from (x, y, theta) to (x, y, theta, time).
Specifically, rasterization is a discretization process on continuous variables. The variable is divided into discrete intervals, and when two variables fall into the same interval, the two variables are judged to be the same variable without searching the variable. For example, there are (x) 1 ,y 1 ,theta 1 ) And (x) 2 ,y 2 ,theta 2 ) Two nodes, if x 1 And x 2 ,y 1 And y 2 ,theta 1 And theta 2 And both nodes belong to the same interval, the two nodes are considered as the same node, and searching is not needed. Of course, if one of the variables (e.g., x) is distributed between two intervals, it is considered as two nodes, and a search needs to be performed on the second node. On one hand, the rasterization (discretization) can reduce the operation amount and can also avoid errors caused by floating point numbers of a computer.
In this way, the already planned trajectory can be considered as a dynamic obstacle. The path with the highest efficiency can be searched out on the premise of ensuring the safety of the subsequently planned track.
Finally, the device obtains the target state information corresponding to the next target node, which may refer to the above-mentioned method for automatically driving the vehicle dynamics model, and details are not repeated here.
S105: and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes.
When the device judges that the target node is the termination node, it indicates that all target nodes between the start node and the termination node have been found, and the start node, all target nodes and the termination node are connected in sequence without continuing search expansion, that is, the driving track of the target vehicle is obtained. Then, the device may acquire the relevant information of the driving trajectory from the state information of all the nodes in the driving trajectory.
In the embodiment of the application, a starting node and a terminating node of a target vehicle are obtained, and state information corresponding to the starting node is obtained; establishing an open list, and adding the starting node into the open list; acquiring a candidate node with the minimum cost value in the open list as a target node; when the target node is not the termination node, acquiring the size and the direction of a preset speed of the target vehicle, acquiring a next target node and target state information corresponding to the next target node according to the size and the direction of the preset speed, and judging whether the next target node is the termination node; and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes. According to the method, the state information comprises the target time corresponding to the starting node, the steering angle of the target vehicle at the target time and the course angle of the target vehicle at the target time, the hybrid A-x algorithm is expanded and is expanded to the time dimension, the requirements of dynamic constraint and dynamic solution of the vehicle are met when the track planning is carried out in real time, the track of the automatic driving vehicle is planned on the full-time airspace, and the passing efficiency of the intersection is improved.
It should be understood that, the sequence numbers of the steps in the foregoing embodiments do not imply an execution sequence, and the execution sequence of each process should be determined by its function and inherent logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.
Referring to fig. 5, fig. 5 is a schematic view of a trajectory planning device according to a second embodiment of the present application. The units are included for performing the steps in the corresponding embodiment of fig. 1. Please refer to fig. 1 for the related description of the corresponding embodiment. For convenience of explanation, only the portions related to the present embodiment are shown. Referring to fig. 5, the trajectory planning device 5 includes:
a first obtaining unit 510, configured to obtain a start node and an end node of a target vehicle, and obtain state information corresponding to the start node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time;
a first processing unit 520, configured to establish an open list, and add the start node to the open list; the open list is used for storing each candidate node and corresponding state information thereof;
a second obtaining unit 530, configured to obtain the candidate node with the smallest cost value in the open list as a target node;
a second processing unit 540, configured to, when the target node is not the end node, obtain a magnitude and a direction of a preset speed of the target vehicle, obtain a next target node and target state information corresponding to the next target node according to the magnitude and the direction of the preset speed, and determine whether the next target node is the end node;
a third processing unit 550, configured to determine, when the target node is the end node, a driving trajectory of the target vehicle according to the state information corresponding to the start node, all the target nodes and the state information corresponding to the target nodes.
Further, the second processing unit 540 is specifically configured to:
acquiring an initial node according to the preset speed and the direction of the target vehicle;
and taking the initial node meeting the preset constraint condition as the next target node, and acquiring the target state information corresponding to the next target node.
Further, the second processing unit 540 is specifically configured to:
and acquiring iteration nodes according to the preset speed and the direction of the target vehicle, and taking the unprocessed iteration nodes as initial nodes.
Further, the second processing unit 540 is specifically configured to:
acquiring a plurality of first nodes according to the preset speed and the preset direction of the target vehicle;
determining a plurality of iteration nodes from the first nodes according to a preset grid interval; wherein the iteration nodes are in different preset grid intervals.
Further, the second processing unit 540 is specifically configured to:
and when the plurality of first nodes are in the same preset grid interval, taking any one of the first nodes as an iteration node.
Further, the second processing unit 540 is specifically configured to:
acquiring a second node meeting a preset collision constraint condition from the initial node;
and acquiring a third node meeting a preset range constraint condition from the second nodes, and taking the third node as the next target node.
Further, the second processing unit 540 is specifically configured to:
acquiring a first boundary area of the initial node;
and determining a second node meeting a preset collision constraint condition according to the first boundary area.
Further, the second processing unit 540 is specifically configured to:
when the second node is in a preset limit area, marking the second node as a fourth node;
and acquiring a course angle of the fourth node, and screening a second node from the fourth node according to the course angle of the fourth node, the course angle of the initial node and the course angle of the terminal node.
Further, the second obtaining unit 530 is specifically configured to:
planning a target track from each candidate node to the termination node according to a Dubins curve, and calculating the length of the target track; wherein the target trajectory is composed of an arc and a straight line.
Fig. 6 is a schematic diagram of a trajectory planning apparatus according to a third embodiment of the present application. As shown in fig. 6, the trajectory planning device 6 of this embodiment includes: a processor 60, a memory 61 and a computer program 62, such as a trajectory planning program, stored in said memory 61 and executable on said processor 60. The processor 60, when executing the computer program 62, implements the steps in the various trajectory planning method embodiments described above, such as the steps 101 to 105 shown in fig. 1. Alternatively, the processor 60, when executing the computer program 62, implements the functions of the modules/units in the above device embodiments, such as the functions of the modules 510 to 550 shown in fig. 5.
Illustratively, the computer program 62 may be partitioned into one or more modules/units that are stored in the memory 61 and executed by the processor 60 to accomplish the present application. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions for describing the execution of the computer program 62 in the trajectory planning device 6. For example, the computer program 62 may be divided into a first acquiring unit, a first processing unit, a second acquiring unit, a second processing unit, and a third processing unit, and the specific functions of each unit are as follows:
the system comprises a first acquisition unit, a second acquisition unit and a third acquisition unit, wherein the first acquisition unit is used for acquiring a starting node and an ending node of a target vehicle and acquiring state information corresponding to the starting node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time;
the first processing unit is used for establishing an open list and adding the starting node into the open list; the open list is used for storing each candidate node and corresponding state information thereof;
a second obtaining unit, configured to obtain the candidate node with the smallest cost value in the open list as a target node;
the second processing unit is used for acquiring the size and the direction of the preset speed of the target vehicle when the target node is not the termination node, acquiring the next target node and the corresponding target state information thereof according to the size and the direction of the preset speed, and judging whether the next target node is the termination node or not;
and the third processing unit is used for determining the driving track of the target vehicle according to the state information corresponding to the starting node, all the target nodes and the state information corresponding to the target nodes when the target node is the ending node.
The trajectory planning device may include, but is not limited to, a processor 60, a memory 61. It will be understood by those skilled in the art that fig. 6 is merely an example of the trajectory planning device 6, and does not constitute a limitation of the trajectory planning device 6, and may include more or fewer components than those shown, or some components in combination, or different components, for example, the trajectory planning device may also include input and output devices, network access devices, buses, etc.
The Processor 60 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 61 may be an internal storage unit of the trajectory planning device 6, such as a hard disk or a memory of the trajectory planning device 6. The memory 61 may also be an external storage device of the trajectory planning device 6, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like, provided on the trajectory planning device 6. Further, the trajectory planning device 6 may also comprise both an internal storage unit and an external storage device of the trajectory planning device 6. The memory 61 is used for storing the computer program and other programs and data required by the trajectory planning device. The memory 61 may also be used to temporarily store data that has been or will be output.
It should be noted that, for the information interaction, execution process, and other contents between the above-mentioned devices/units, the specific functions and technical effects thereof are based on the same concept as those of the embodiment of the method of the present application, and specific reference may be made to the part of the embodiment of the method, which is not described herein again.
An embodiment of the present application further provides a network device, where the network device includes: at least one processor, a memory, and a computer program stored in the memory and executable on the at least one processor, the processor implementing the steps of any of the various method embodiments described above when executing the computer program.
The embodiments of the present application further provide a computer-readable storage medium, where a computer program is stored, and when the computer program is executed by a processor, the computer program can implement the steps in the above-mentioned method embodiments.
The embodiments of the present application provide a computer program product, which when running on a mobile terminal, enables the mobile terminal to implement the steps in the above method embodiments when executed.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, all or part of the processes in the methods of the embodiments described above can be implemented by a computer program, which can be stored in a computer-readable storage medium and can implement the steps of the embodiments of the methods described above when the computer program is executed by a processor. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer readable medium may include at least: any entity or device capable of carrying computer program code to a photographing apparatus/terminal device, a recording medium, a computer Memory, a Read-Only Memory (ROM), a Random Access Memory (RAM), an electrical carrier signal, a telecommunications signal, and a software distribution medium. Such as a usb-disk, a removable hard disk, a magnetic or optical disk, etc. In certain jurisdictions, computer-readable media may not be an electrical carrier signal or a telecommunications signal in accordance with legislative and patent practice.
In the foregoing embodiments, the descriptions of the respective embodiments have their respective emphasis, and for parts that are not described or recited in detail in a certain embodiment, reference may be made to the description of other embodiments.
Those of ordinary skill in the art would appreciate that the elements and algorithm steps of the various embodiments described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the technical solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus/network device and method may be implemented in other ways. For example, the above-described apparatus/network device embodiments are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, multiple units or components may be combined or may be integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, and not to limit the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the present disclosure, and are intended to be included within the scope thereof.
Claims (10)
1. A trajectory planning method, comprising:
acquiring a starting node and a terminating node of a target vehicle, and acquiring state information corresponding to the starting node; the state information comprises a target time corresponding to the starting node, a steering angle of the target vehicle at the target time and a course angle of the target vehicle at the target time;
establishing an open list, and adding the starting node into the open list; the open list is used for storing each candidate node and corresponding state information thereof;
acquiring the candidate node with the minimum cost value in the open list as a target node;
when the target node is not the termination node, acquiring the size and the direction of a preset speed of the target vehicle, acquiring a next target node and target state information corresponding to the next target node according to the size and the direction of the preset speed, and judging whether the next target node is the termination node or not;
and when the target node is the termination node, determining the driving track of the target vehicle according to the state information corresponding to the initiation node, all the target nodes and the state information corresponding to the target nodes.
2. The trajectory planning method according to claim 1, wherein the obtaining of the next target node and the corresponding target state information according to the magnitude and the direction of the preset speed comprises:
acquiring an initial node according to the preset speed and the direction of the target vehicle;
and taking the initial node meeting the preset constraint condition as the next target node, and acquiring target state information corresponding to the next target node.
3. The trajectory planning method according to claim 2, wherein the obtaining of the initial node according to the magnitude and direction of the preset speed of the target vehicle comprises:
and acquiring iteration nodes according to the preset speed and the direction of the target vehicle, and taking the unprocessed iteration nodes as initial nodes.
4. The trajectory planning method according to claim 3, wherein the obtaining iterative nodes according to the magnitude and direction of the preset speed of the target vehicle comprises:
acquiring a plurality of first nodes according to the preset speed and the preset direction of the target vehicle;
determining a plurality of iteration nodes from the first nodes according to a preset grid interval; wherein the iteration nodes are in different preset grid intervals.
5. The trajectory planning method of claim 4, wherein determining a plurality of iterative nodes from the first nodes according to a predetermined grid interval comprises:
and when the plurality of first nodes are in the same preset grid interval, taking any one of the first nodes as an iteration node.
6. The trajectory planning method according to claim 2, wherein the step of using the initial node satisfying the preset constraint condition as the next target node comprises:
acquiring a second node meeting a preset collision constraint condition from the initial node;
and acquiring a third node meeting a preset range constraint condition from the second node, and taking the third node as the next target node.
7. The trajectory planning method according to claim 6, wherein the obtaining a second node satisfying a predetermined collision constraint condition from the initial nodes comprises:
acquiring a first boundary area of the initial node;
and determining a second node meeting a preset collision constraint condition according to the first boundary area.
8. The trajectory planning method according to claim 6, wherein the obtaining of the third node satisfying the preset range constraint from the second node comprises:
when the second node is in a preset limit area, marking the second node as a fourth node;
and acquiring the course angle of the fourth node, and screening out a second node from the fourth node according to the course angle of the fourth node, the course angle of the initial node and the course angle of the terminal node.
9. The trajectory planning method according to claim 1, wherein the obtaining of the candidate node with the smallest cost value in the open list is a target node, and includes:
planning a target track from each candidate node to the termination node according to a Dubins curve, and calculating the length of the target track; wherein the target trajectory is composed of an arc and a straight line.
10. A computer-readable storage medium, in which a computer program is stored which, when being executed by a processor, carries out the method according to any one of claims 1 to 9.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210546061.2A CN115185263A (en) | 2022-05-19 | 2022-05-19 | Trajectory planning method and computer readable storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210546061.2A CN115185263A (en) | 2022-05-19 | 2022-05-19 | Trajectory planning method and computer readable storage medium |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115185263A true CN115185263A (en) | 2022-10-14 |
Family
ID=83513434
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210546061.2A Pending CN115185263A (en) | 2022-05-19 | 2022-05-19 | Trajectory planning method and computer readable storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115185263A (en) |
-
2022
- 2022-05-19 CN CN202210546061.2A patent/CN115185263A/en active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3699048A1 (en) | Travelling track prediction method and device for vehicle | |
WO2022052406A1 (en) | Automatic driving training method, apparatus and device, and medium | |
CN112001986B (en) | Virtual lane generation method and device, electronic equipment and storage medium | |
CN111653113B (en) | Method, device, terminal and storage medium for determining local path of vehicle | |
CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
CN109916422B (en) | Global path planning method and device | |
CN115195790B (en) | Method and device for predicting vehicle track, electronic equipment and storage medium | |
CN112325898B (en) | Path planning method, device, equipment and storage medium | |
CN113442908B (en) | Automatic parking path planning method and system and parking control equipment | |
CN115900738A (en) | Path planning method, device, vehicle and storage medium | |
CN114323051A (en) | Method and device for planning crossing driving track and electronic equipment | |
CN113619604A (en) | Integrated decision and control method and device for automatic driving automobile and storage medium | |
US20230358547A1 (en) | Map matching method and apparatus, electronic device and storage medium | |
CN117261938A (en) | Path planning method, path planning device, vehicle and storage medium | |
CN115185263A (en) | Trajectory planning method and computer readable storage medium | |
CN116424336A (en) | Track prediction method and device and Internet of vehicles equipment | |
WO2023135271A1 (en) | Motion prediction and trajectory generation for mobile agents | |
CN116215517A (en) | Collision detection method, device, apparatus, storage medium, and autonomous vehicle | |
CN114750782A (en) | Path planning method, device and equipment and vehicle control method, device and equipment | |
CN113432618A (en) | Trajectory generation method and apparatus, computing device and computer-readable storage medium | |
CN117885764B (en) | Vehicle track planning method and device, vehicle and storage medium | |
CN114637284A (en) | U-turn lane generation method and device, computer readable storage medium and terminal | |
CN117508232A (en) | Track prediction method, device, equipment and medium for vehicle surrounding obstacle | |
CN115061479A (en) | Lane relation determination method and device, electronic equipment and storage medium | |
CN115760909A (en) | Path generation method, path generation device, computer equipment, storage medium and program product |
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 |