CN110836671B - Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus - Google Patents
Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus Download PDFInfo
- Publication number
- CN110836671B CN110836671B CN201911114431.XA CN201911114431A CN110836671B CN 110836671 B CN110836671 B CN 110836671B CN 201911114431 A CN201911114431 A CN 201911114431A CN 110836671 B CN110836671 B CN 110836671B
- Authority
- CN
- China
- Prior art keywords
- node
- child
- open list
- value
- nodes
- 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.)
- Active
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/20—Instruments for performing navigational calculations
Abstract
The disclosure provides a track planning method, a track planning device, a storage medium and electronic equipment, and relates to the technical field of intelligent navigation. The method comprises the following steps: acquiring a starting point and an end point in a map, and determining state parameters of the starting point, wherein the state parameters comprise time; establishing an open list to record the state parameters of each node, and adding a starting point into the open list; sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to the state variables in the preset interval time, adding the child nodes into the open list, and removing the father node from the open list; when the end point is added into the open list, the child nodes and the father nodes thereof are sequentially connected from the end point until the child nodes are connected to the starting point, and a target path is obtained; and generating a target track from the starting point to the end point according to the state parameter of each node in the target path. The method and the system can realize the track planning in a dynamic scene and conform to the kinematics characteristics of objects such as vehicles.
Description
Technical Field
The present disclosure relates to the field of intelligent navigation technologies, and in particular, to a trajectory planning method, a trajectory planning apparatus, a computer-readable storage medium, and an electronic device.
Background
Motion Planning (Motion Planning) is a method for determining a path meeting constraint conditions between a given starting point and a given end point, and is widely applied to the fields of vehicle navigation, automatic driving, unmanned aerial vehicles, robot control and the like.
Currently, motion planning mostly stays in a path planning stage, such as an a star (a) algorithm or a Hybrid a star (Hybrid a) algorithm commonly used in the industry, and a path of a geometric curve is planned between a starting point and an end point for a complex unstructured static scene. However, path planning is mostly for static scenes, and when applied to dynamic scenes, there are the following problems: dynamic collision avoidance cannot be effectively realized, if a moving obstacle (such as other vehicles) exists in a scene, collision may occur, or a poor path is planned in order to avoid the path of the moving obstacle; path planning may violate the actual kinematics of the vehicle, for example, a sharp turn path may be planned that is difficult to complete during normal vehicle travel.
Therefore, it is necessary to provide a trajectory planning method to solve the problem that the existing path planning method is difficult to be applied to a dynamic scene.
It is to be noted that the information disclosed in the above background section is only for enhancement of understanding of the background of the present disclosure, and thus may include information that does not constitute prior art known to those of ordinary skill in the art.
Disclosure of Invention
The present disclosure provides a trajectory planning method, a trajectory planning device, a computer-readable storage medium, and an electronic device, thereby overcoming, at least to some extent, the problem that the existing path planning method is difficult to apply to a dynamic scene.
Additional features and advantages of the disclosure will be set forth in the detailed description which follows, or in part will be obvious from the description, or may be learned by practice of the disclosure.
According to a first aspect of the present disclosure, there is provided a trajectory planning method, including: acquiring a starting point and an end point in a map, and determining state parameters of the starting point; establishing an open list to record the state parameters of each node, and adding the starting point into the open list, wherein the state parameters of each node comprise the time corresponding to the node; sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to state variables in preset interval time, adding the child nodes into the open list, and removing the father node from the open list; when the end point is added into the open list, sequentially connecting child nodes and father nodes thereof from the end point until the child nodes are connected to the starting point to obtain a target path; and generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
Optionally, the state parameters further include a posture and a speed.
Optionally, the determining the state parameter of the child node of the parent node according to the state variable in the preset interval time includes: acquiring each state variable combination; and respectively calculating the child nodes reached after the preset interval time is moved through each state variable combination based on the state parameters of the father node, and calculating the state parameters of the child nodes.
Optionally, the state variables include a speed variable and a steering variable.
Optionally, when the child node is added to the open list, the cost value of the child node is determined in the following manner: determining a first descendant value of the child node according to the first descendant value of the parent node of the child node and a difference between the state parameters of the child node and the parent node thereof; determining a cost value of the child node to the destination as a second descendant value of the child node; determining a cost value of the child node according to the first child value and the second child value of the child node; wherein the first progeny value of the starting point is 0.
Optionally, the cost value from any node to the end point is determined by: calculating the length of a Reeds-Shepp curve from any node to the terminal point as a first length; determining the length of a path from any node to the terminal point through an A star algorithm as a second length; and taking the larger length value of the first length and the second length as the cost value of any node to the terminal.
Optionally, when the child node is added to the open list, the method further includes: when the child node is already positioned in the open list, acquiring an original parent node of the child node; calculating the primary value of the child node based on the original father node, and calculating the current value of the child node based on the current father node; when the primary cost value is smaller than the current cost value, the relationship between the child node and the original father node is reserved; and when the primary cost value is larger than the current cost value, updating the father node of the child node as the current father node.
Optionally, after adding the child node to the open list, the method further includes: when a preset condition is met, generating a Reeds-Shepp curve from the child node to the terminal; calculating the state parameters of each node in the Reeds-Shepp curve when the uniform movement is adopted based on the state parameters of the child nodes; judging whether the Reeds-Shepp curve collides with an obstacle in the map or not according to the state parameters of all nodes in the Reeds-Shepp curve; when the fact that the Reeds-Shepp curve collides with the obstacles in the map is determined, the step of determining the state parameters of the child nodes of the father nodes by taking the node with the minimum cost value in the open list as the father node according to the state variables in the preset interval time is continuously executed; when the Reeds-Shepp curve is determined not to collide with the obstacles in the map, sequentially connecting child nodes and parent nodes thereof from the child nodes until the child nodes are connected to the starting point, and obtaining a target path by combining the Reeds-Shepp curve; and generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
Optionally, when the child node is added to the open list, the child node in the closed list is removed, and the remaining child nodes are added to the open list.
Optionally, after removing the parent node from the open list, the method further includes: and adding the father node into the closed list.
Optionally, after determining the state parameter of the child node of the parent node according to the state variable in the preset interval time, the method further includes: and when the child node is determined to collide with the obstacle in the map according to the state parameters of the child node, adding the child node into the closed list.
Optionally, after the parent node is removed from the open list, if the open list is empty, it is determined that the trajectory planning fails.
Optionally, before adding the starting point to the open list, the open list is empty.
According to a second aspect of the present disclosure, there is provided a trajectory planning apparatus comprising: the acquisition module is used for acquiring a starting point and an end point in a map and determining the state parameters of the starting point; the establishing module is used for establishing an open list so as to record the state parameter of each node, and adding the starting point into the open list, wherein the state parameter of each node comprises the time corresponding to the node; the expansion module is used for sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to the state variables in the preset interval time, adding the child nodes into the open list, and removing the father node from the open list; the connection module is used for sequentially connecting child nodes and father nodes thereof from the end point until the child nodes are connected to the starting point to obtain a target path when the end point is added into the open list; and the generating module is used for generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
Optionally, the state parameters further include a posture and a speed.
Optionally, the extension module is configured to calculate, based on the state parameter of the parent node, a child node that arrives after the preset interval time moves through each state variable combination, and calculate the state parameter of the child node.
Optionally, the state variables include a speed variable and a steering variable.
Optionally, the extension module includes a cost calculation unit, configured to determine the cost value of the child node by performing the following steps: determining a first descendant value of the child node according to the first descendant value of the parent node of the child node and a difference between the state parameters of the child node and the parent node thereof; determining a cost value of the child node to the destination as a second descendant value of the child node; determining a cost value of the child node according to the first child value and the second child value of the child node; wherein the first progeny value of the starting point is 0.
Optionally, the cost calculating unit is further configured to determine a cost value from any node to the end point by performing the following steps: calculating the length of a Reeds-Shepp curve from any node to the terminal point as a first length; determining the length of a path from any node to the terminal point through an A star algorithm as a second length; and taking the larger length value of the first length and the second length as the cost value of any node to the terminal.
Optionally, the extension module includes a node updating unit, configured to determine, when the child node is added to the open list, a parent node of the child node by performing the following steps: when the child node is already positioned in the open list, acquiring an original parent node of the child node; calculating the primary value of the child node based on the original father node, and calculating the current value of the child node based on the current father node; when the primary cost value is smaller than the current cost value, the relationship between the child node and the original father node is reserved; and when the primary cost value is larger than the current cost value, updating the father node of the child node as the current father node.
Optionally, the apparatus further comprises: a curve planning module, configured to, after the expansion module adds the child node to the open list, execute the following steps: when a preset condition is met, generating a Reeds-Shepp curve from the child node to the terminal; calculating the state parameters of each node in the Reeds-Shepp curve when the uniform movement is adopted based on the state parameters of the child nodes; judging whether the Reeds-Shepp curve collides with an obstacle in the map or not according to the state parameters of all nodes in the Reeds-Shepp curve; when the fact that the Reeds-Shepp curve collides with the obstacles in the map is determined, the expansion module is scheduled to continue to use the node with the minimum cost value in the open list as a father node, and the state parameters of the child nodes of the father node are determined according to the state variables in the preset interval time; when the Reeds-Shepp curve is determined not to collide with the obstacles in the map, sequentially connecting child nodes and parent nodes thereof from the child nodes until the child nodes are connected to the starting point, and obtaining a target path by combining the Reeds-Shepp curve; and generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
Optionally, the extension module is further configured to, when the child node is added to the open list, remove the child node located in the closed list, and add the remaining child nodes to the open list.
Optionally, the extension module is further configured to add the parent node to the closed list after the parent node is removed from the open list.
Optionally, the expansion module is further configured to add the child node to the closed list when it is determined that the child node collides with an obstacle in the map according to the state parameter of the child node.
Optionally, the extension module is further configured to determine that trajectory planning fails if the open list is empty after the parent node is removed from the open list.
Optionally, before the establishing module adds the starting point to the open list, the open list is empty.
According to a third aspect of the present disclosure, there is provided a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements any of the trajectory planning methods described above.
According to a fourth aspect of the present disclosure, there is provided an electronic device comprising: a processor; and a memory for storing executable instructions of the processor; wherein the processor is configured to perform any of the trajectory planning methods described above via execution of the executable instructions.
The technical scheme of the disclosure has the following beneficial effects:
according to the track planning method, the track planning device, the computer readable storage medium and the electronic equipment, an open list is established, the node with the minimum cost value in the open list is used as a father node for expansion in sequence, the child node obtained through expansion is added into the open list until an end point is added into the open list, the child node is reversely connected to a starting point from the end point according to the relation between the child node and the father node, and a target track from the starting point to the end point is generated according to the state parameters of each node. On one hand, each node records state parameters, not only including the position of the node, but also including the time corresponding to the node, and whether the node collides with a moving obstacle in the map can be judged based on the time information, so that the trajectory planning in a dynamic scene is realized. On the other hand, the state parameters of the child nodes are obtained through the state parameters of the parent nodes and the state variables of the preset interval time, and the process accords with the kinematics characteristics of the object, so that the planned path has better practicability.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the disclosure.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present disclosure and together with the description, serve to explain the principles of the disclosure. It is to be understood that the drawings in the following description are merely exemplary of the disclosure, and that other drawings may be derived from those drawings by one of ordinary skill in the art without the exercise of inventive faculty.
FIG. 1 shows a flow chart of a trajectory planning method in the present exemplary embodiment;
FIG. 2 illustrates a sub-flow diagram of a trajectory planning method in the present exemplary embodiment;
FIG. 3 illustrates a sub-flow diagram of another trajectory planning method in the present exemplary embodiment;
FIG. 4 shows a flow chart of another trajectory planning method in the present exemplary embodiment;
FIG. 5 is a block diagram showing the configuration of a trajectory planning apparatus in the present exemplary embodiment;
FIG. 6 illustrates a computer-readable storage medium for implementing the above-described method in the present exemplary embodiment;
fig. 7 shows an electronic device for implementing the above method in the present exemplary embodiment.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. Example embodiments may, however, be embodied in many different forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a thorough understanding of embodiments of the disclosure. One skilled in the relevant art will recognize, however, that the subject matter of the present disclosure can be practiced without one or more of the specific details, or with other methods, components, devices, steps, and the like. In other instances, well-known technical solutions have not been shown or described in detail to avoid obscuring aspects of the present disclosure.
Furthermore, the drawings are merely schematic illustrations of the present disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus their repetitive description will be omitted. Some of the block diagrams shown in the figures are functional entities and do not necessarily correspond to physically or logically separate entities. These functional entities may be implemented in the form of software, or in one or more hardware modules or integrated circuits, or in different networks and/or processor devices and/or microcontroller devices.
Trajectory planning refers to calculating a driving trajectory meeting internal/external constraint conditions between a starting state and an ending state of a moving object (such as a vehicle, a robot and the like), wherein the internal constraint mainly comprises kinematics characteristics, and the external constraint mainly comprises collision avoidance or other user-defined added limits.
An exemplary embodiment of the present disclosure first provides a trajectory planning method, which is specifically described below with reference to fig. 1. In the following description, a scenario of vehicle travel trajectory planning will be taken as an example, and the procedures and principles of the method are the same for other scenarios such as robot travel trajectory planning and unmanned vehicle travel trajectory planning.
As shown in fig. 1, the trajectory planning method may include the following steps S110 to S150:
step S110, acquiring a starting point and an end point in the map, and determining the state parameters of the starting point.
The map may be a local map comprising a start point and an end point and containing sufficient space to facilitate planning of a trajectory therein; other information such as roads, obstacles, etc. may also be included in the map. The positions of the start point and the end point may be represented by position coordinates in a map, or GPS (Global Positioning System) data, and the like, which is not limited in this disclosure.
The trajectory planning differs from the general path planning in that the trajectory planning requires recording of state information, for example, planning the vehicle to travel to point a, and in addition to recording the position of point a, the state of the vehicle traveling to point a should also be recorded. In the present exemplary embodiment, for a node in the map, the state when the vehicle reaches the node is represented by the state parameter. Among the state parameters, the necessary data include: the time, i.e. the time when the vehicle arrives at the node. In addition, the state parameters may further include: the attitude including the direction of the vehicle when it reaches the node, the steering angle (i.e., whether the vehicle itself is in a straight or steered state, and the angle of steering), and the like; speed, i.e. the speed at which the vehicle is travelling when it reaches the node. When the trajectory planning is started, the state parameters of the starting point may be determined, where the current time or zero time may be used as the time of the starting point, and the current vehicle driving state may be used as the posture and speed corresponding to the starting point, for example, when the vehicle is initially in a stationary state, the steering angle and speed corresponding to the starting point are both 0.
Step S120, an open list is established to record the state parameters of each node therein, and the starting point is added to the open list.
The Open List (Open List) may be regarded as a set of candidate nodes in trajectory planning, and position nodes to which the vehicle may travel are added to the Open List for subsequent screening processing. The open list may record state parameters of each node, including a time corresponding to each node, for example, when the starting point is added to the open list, the position and the time of the starting point are recorded in the open list, and the posture, the speed, and the like of the vehicle may also be recorded. In an alternative embodiment, the initial open list may be empty before adding the start point to the open list, and the start point is the first node to add to the open list.
And step S130, sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to the state variables in the preset interval time, adding the child nodes into the open list, and removing the father node from the open list.
The process of determining a child node from a parent node may be referred to herein as "extending". The exemplary embodiment is essentially a process that starts from a starting point and continues to expand to an end point, i.e., an expanded trajectory, i.e., a final planned trajectory. For each node, the cost value can be calculated, wherein the cost value is a measure of the cost of the driving track, and represents the cost of driving from the starting point to the node and then driving to the end point, and the cost comprises factors such as the path length, the required time, whether collision occurs or not, whether the speed direction is frequently switched or not and the like. How to calculate the cost value specifically will be explained below.
It should be noted that step S130 is a step executed in a loop, and each time the step is executed, expansion is performed, that is, after the vehicle starts from the current parent node and runs for a preset interval time, the reachable node is a child node; in short, each expansion represents a vehicle "step". The preset interval time is a periodic time of each expansion, and represents a smaller time unit, such as 5 seconds, 10 seconds, and the like, and the shorter the preset interval time is, the finer the planned trajectory is, so that the preset interval time can be determined according to actual requirements. The state variable refers to a change of a motion state, such as acceleration, deceleration, steering, etc., which can be achieved by the vehicle within a preset interval time, and the state variable within the preset interval time may be regarded as a constant value due to the short preset interval time.
The present exemplary embodiment calculates the state parameter of the child node through the state variable at the preset interval time based on the state parameter of the parent node. The calculation process can adopt a forward kinematics equation or other kinematics models, so that the kinematics characteristics between the parent node and the child node are satisfied, for example, the vehicle can complete a certain degree of turning in a preset interval time, but the turning cannot be realized, so that the child node cannot be located at the opposite road position of the parent node.
The state parameters of the child nodes may include other information such as the position, time, posture, and speed of the child nodes, as well as the state parameters of the starting point. After determining the state parameters of the child nodes, adding the state parameters into the open list. Since the parent node has completed the expansion, it may be removed from the open list to prevent duplicate expansions.
Step S140, when the end point is added to the open list, the child nodes and their parent nodes are connected in sequence from the end point until the end point is connected to the starting point, so as to obtain the target path.
After the end point is added to the open list, no further expansion is needed. Starting from the end point, connecting the end point and the parent node of the end point, taking the parent node as the child node, connecting the parent node … … of the child node, and so on, which is equivalent to connecting the expanded nodes one by one in the reverse direction until connecting to the start point, and obtaining the target path connecting the start point and the end point.
And step S150, generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
The state parameter of each node includes the time of each node, that is, the time when the vehicle travels to each node according to the plan. Thus, the state parameter can be added to the target path, for example, the time of each node is marked to the target path, and the target track from the starting point to the end point is obtained. The target track comprises the time information of each node, and when a moving obstacle exists in the map, whether the vehicle collides with the moving obstacle can be calculated, so that effective track planning is realized in a dynamic scene, and the actual requirement is met.
In order to make the extended process more consistent with the kinematics characteristics, in an alternative embodiment, in step S130, the state parameters of the child nodes of the parent node may be determined specifically by the following steps:
acquiring each state variable combination;
and respectively calculating the child nodes reached after the preset interval time of each state variable combination movement based on the state parameters of the father node, and calculating the state parameters of the child nodes.
The state variables may include parameters of multiple dimensions, such as speed variables, steering variables, and the like. The different state variables may form various combinations representing various movement patterns that the vehicle may take during the preset interval. For example, if the state variables include a speed variable and a steering variable, which are respectively expressed by Δ v and Δ Φ, Δ v has three cases within a preset interval Δ t: v. of00 and-v0Respectively, indicate acceleration (increase in velocity v)0) Keeping constant speed and reducing speed; Δ Φ also has three cases: phi00 and-phi0Respectively, indicating that the steered wheels are turned to the left by phi0Keeping straight and turning right phi0(Φ0Can also represent a turn to the right, -phi0Turning to the left). Then the total number of state variable groups is 3 × 3 — 9, which are:
Δv＝v0，ΔΦ＝Φ0accelerating left rotation;
Δv＝v0when the delta phi is 0, accelerating the straight line;
Δv＝v0，ΔΦ＝-Φ0accelerating right turn;
Δv＝0，ΔΦ＝Φ0turning left at constant speed;
… … (not to mention each other)
Under the state parameter of the current father node, after the vehicle moves for the preset interval time in the 9 modes, the positions and the states which can be reached are different, so that the father node has 9 child nodes. The child nodes and their state parameters obtained under the above-mentioned 9 state variable combinations can be calculated from the state parameters of the parent node by means of motion estimation.
The state parameter of each node may be represented as an array, such as the state parameter S (P) of the current parent nodecur)＝[x(Pcur)，y(Pcur)，θ(Pcur)，v(Pcur)，t(Pcur)]Respectively represent the current parent node PcurX-coordinate, y-coordinate, attitude angle, velocity, and time of day. State parameter S (P) for child nodechild)＝[x(Pchild)，y(Pchild)，θ(Pchild)，v(Pchild)，t(Pchild)]Wherein x (P)child)、y(Pchild) And theta (P)child) Can be calculated by the forward kinematics equation, and the rest can be calculated by the following formulas (1) and (2):
v(Pchild)＝v(Pcur)+Δv (1)
t(Pchild)＝t(Pcur)+Δt (2)
Δ t is a preset interval time.
At the acquisition of the child node PchildAfter the status parameter(s), the relationship between the status parameter(s) and the parent node can be recorded as follows:
Parent(Pchild)＝Pcur (3)
it should be added that the starting point is used as a special node, and can be regarded as an ancestor node, i.e. the parent node of the starting point is empty (N)ull)。
In an alternative embodiment, when a child node is added to the open list, its cost value may be determined and stored together with its state parameter, so as to select the node with the smallest replacement value from the open list in the subsequent expansion. Referring to fig. 2, the cost value of the child node may be determined through the following steps S210 to S230:
step S210, determining the first descendant value of the child node according to the first descendant value of the father node of the child node and the difference between the state parameters of the child node and the father node thereof;
step S220, determining the cost value from the child node to the terminal point as the two child values of the first child node;
and step S230, determining the cost value of the child node according to the first child value and the second child value of the child node.
Wherein, the first subThe cost value represents a cost from the starting point to the child node, and the second descendant value represents a cost from the child node to the ending point. The final cost value of the child node can be obtained by adding or weighting the two parts. Illustratively, the cost function is represented by f (-), the first descendant cost function is represented by g (-), the second descendant cost function is represented by h (-), and the child node PchildThe cost value of (a) is:
f(Pchild)＝g(Pchild)+h(Pchild) (4)
g(Pchild)＝g(Pcur)+diff(Pcur，Pchild) (5)
g(Pcur) Is a child node PchildParent node P ofcurA first descendant value of; diff (P)cur，Pchild) Representing a child node PchildParent node P ofcurThe specific calculation method of the difference between the state parameters depends on the specific information of the state parameters, such as: under the condition that the state parameters comprise positions and time, because the time difference between any one group of father nodes and child nodes is the preset interval time delta t, the time difference can be ignored, and the position difference, namely the distance (which can be Euclidean distance or Manhattan distance) is used as the difference of the state parameters; and under the condition that the state parameters comprise position, attitude, speed and time, ignoring the time difference, respectively calculating the position difference, the attitude angle difference and the speed difference, and weighting the three groups of difference values to obtain the difference of the state parameters, wherein the larger the movement distance is, the larger the change of the motion state is, and the larger the cost value is. In addition, a penalty term can be added into the g (-) function, so that a penalty value for calculating the penalty can be added for some special driving states, such as reversing, frequently switching the speed direction and the like.
As can be seen from equation (5), the calculation of the first descendant value is a recursive process, and the first descendant value of a child node depends on the first descendant value of its parent node, which in turn depends on the first descendant value of a grandparent node (parent node of the parent node). Therefore, a first child value of the first parent node needs to be determined. In the exemplary embodiment, the first parent node is a starting point, and the first child value of the starting point may be set to 0, which means that the cost from the starting point to the starting point is 0.
The g (-) function, the method for calculating the first descendant value, is introduced above. The calculation method of the h (-) function, i.e. the second descendant value, is described below by several specific schemes:
in the first scenario, referring to fig. 3, the cost value from any node to the destination is determined through the following steps S310 to S330:
step S310, calculating the length of a Reeds-Shepp curve from the node to the terminal point as a first length;
step S320, determining the length of a path from the node to a terminal point through an A star algorithm as a second length;
and step S330, taking the larger length value of the first length and the second length as the cost value from the node to the terminal.
The Reeds-Shepp curve (RS curve) is a path planning method, and can be obtained by solving a constraint column equation by adopting a curve path model. The Reeds-Shepp curve takes into account the kinematics of the vehicle, but does not take into account collision avoidance; the a-star algorithm takes into account static collision avoidance, but does not take into account the kinematics of the vehicle. The resulting first length and second length are both cost estimates for the node to the end point, and the exemplary embodiment may use the larger length value as the cost value for the node to the end point. The mathematical representation is as follows: setting a function hRS(. h) and hA*(. wherein h) isRS(. represents a function of a first length hA*(. cndot.) represents a function of the second length, from which can be defined:
h(·)max(hRS(·)，hA*(·)) (6)
and a second scheme is to simplify the first scheme, and the first length or the second length can be used as a cost value from the node to the terminal.
And thirdly, calculating the distance from the child node to the terminal point in a more simplified mode, wherein the distance can be Euclidean distance or Manhattan distance (according to the type of the map) and is used as the second child value of the child node.
In the course of trajectory planning, a situation may occur where the parent node is not unique, for example, a vehicle may travel from node a to node C, or from node B to node C, and then the parent node of node C may be node a or node B. Based on this, in an alternative embodiment, when adding the child node to the open list, the following steps may be performed:
when the child node is already positioned in the open list, acquiring an original father node of the child node;
calculating the primary value of the child node based on the original father node, and calculating the current value of the child node based on the current father node;
when the primary value is smaller than the current value, the relationship between the child node and the original father node is reserved;
and when the primary value is greater than the current cost value, updating the father node of the child node as the current father node.
The nodes in the open list are all child nodes of a certain node except the starting point. Therefore, if a child node (in P) that currently needs to join the open list is foundchildIndicates) has been in the open list, indicates PchildIn previous extensions, the open list was added with the child node identities of other nodes. I.e. PchildThere is already a parent node, called original parent node PoriThe father node in the current round of expansion is called as the current father node Pcur. Based on PoriCalculating PchildRepresents a cost value from the starting point to PoriTo PchildThe cost value of (a) is called primary value; based on PcurCalculating PchildRepresents a cost value from the starting point to PcurTo PchildThe cost value of (2) is called the current cost value; because the tracks are different, the driving cost is generally different, and the primary value and the current cost value are generally different, a better father node can be selected by comparing the cost values. As follows:
f(Pchild)＝g(Pcur)+diff(Pcur，Pchild)+h(Pchild) (7)
f′(Pchild)＝g(Pori)+diff(Pori，Pchild)+h(Pchild) (8)
f(Pchild) Is the current cost value, f' (P)child) Is of primary value. For a specific calculation method, the contents of the above equations (4) to (6) can be referred to.
If f (P)child)>f′(Pchild) Then determine Parent (P)child)＝Pori(ii) a If f (P)child)＜f′(Pchild) Then determine Parent (P)child)＝Pcur. In addition, f (P) may also be presentchild)＝f′(Pchild) In the case of (1), it is assumed that two parent nodes are equivalent, and either one of the parent nodes may be selected, or both of the parent nodes may be retained.
Fig. 4 shows another flow of the present exemplary embodiment, wherein after steps S110 to S130 are performed, it is determined whether an end point is added to the open list, if yes, step S140 is performed, and if not, it is continuously determined whether a preset condition is satisfied; if the preset condition is not satisfied, the step S130 is continuously executed, and if the preset condition is satisfied, the following steps S131 to S135 are executed:
and S131, generating a Reeds-Shepp curve from the child node to the terminal point when a preset condition is met. The child node is a child node added into the open list at present, and a Reeds-Shepp curve from the child node to the terminal point can be obtained by solving a constraint column equation by adopting a curve path model. The preset conditions may include: step S131 is performed every time N expansions are performed, for example every 3 expansions; the child node is positioned in a preset range around the terminal point, so that the Reeds-Shepp curve can be conveniently planned; and so on.
And S132, calculating the state parameters of each node in the Reeds-Shepp curve when the uniform-speed movement is adopted based on the state parameters of the child nodes. For example, the current speed or a preset speed (which may be an average speed in a normal driving process) may be used, and the state parameters of the reached node and each node at each preset driving interval time when the vehicle travels along the Reeds-Shepp curve at a constant speed are calculated, which is equivalent to equally dividing the Reeds-Shepp curve.
And S133, judging whether the Reeds-Shepp curve collides with an obstacle in the map according to the state parameters of each node in the Reeds-Shepp curve. Since avoidance of static obstacles has generally been considered when generating the Reeds-Shepp curve, the collision judgment here is mainly dynamic collision judgment: determining the track of the moving obstacle in the map, and if the track of the obstacle and the Reeds-Shepp curve are not intersected, collision cannot occur; if the obstacle track intersects with the Reeds-Shepp curve, judging whether the time of the intersection point in the obstacle track is closer to the time of the intersection point in the Reeds-Shepp curve; if the two times are close (within a threshold range), a collision will occur; if the two moments are far apart, no collision will occur.
Step S134, when the Reeds-Shepp curve is determined to collide with the obstacles in the map, the Reeds-Shepp curve cannot be used as the driving track, and the step S130 can be continuously executed to select the parent node from the open list for expansion.
And step S135, when the Reeds-Shepp curve is determined not to collide with the obstacles in the map, connecting the child node and the parent node thereof in sequence from the child node until the child node is connected to the starting point, and obtaining the target path by combining the Reeds-Shepp curve. The target path is composed of two parts, the former part is from the starting point to the child node, and the connection method of the path can refer to step S140 by expanding the obtained path; the latter part is the path of the Reeds-Shepp curve.
And step S150, generating a target track from the starting point to the end point according to the state parameter of each node in the target path. In the target path determined in step S135, the nodes of the previous path are nodes obtained by expansion, the state parameters of the nodes are calculated at the time of expansion, and the nodes of the subsequent path have the state parameters calculated in step S132. Therefore, the state parameters of each node in the target path are known, and the target track can be generated.
Based on the flow shown in fig. 4, the next part of the trajectory can be directly generated in an RS curve manner in the expanding process, so as to accelerate the processing process of trajectory planning.
In an alternative embodiment, to improve efficiency, a closed list (closedlst) may be built to hold nodes that have already been explored to prevent duplicate processing. In step S130, when adding the child node to the open list, the child node located in the closed list may be removed, and the remaining child nodes are added to the open list. For example, when the current expansion is performed, the parent node a is used to determine the child node B, C, D, wherein if the child node D is located in the closed list, the child node D is a processed node, and the processing is not repeated by adding an open list, so that the child node B, C may be added to the open list.
The initial closed list may be empty and during the expansion process, the closed list may be maintained by:
after the parent node is removed from the open list in step S130, it may be added to the closed list, for example, after the child node B, C of the parent node a is added to the open list, the parent node a may be removed from the open list and added to the closed list, and if other nodes are subsequently expanded, the node a is explored as a child node of the other nodes, and the open list does not need to be added.
Or after determining the state parameter of the child node of the parent node according to the state variable within the preset interval time in step S130, when it is determined that the child node collides with an obstacle in the map according to the state parameter of the child node, the child node may be added to the closed list. The collision judgment of the child node may include two aspects: one is static obstacle judgment, namely judging whether the child node is located at the position of the static obstacle; and secondly, judging a dynamic obstacle, namely judging whether the two moments are close to each other or not when the track passes through the child node according to the moving track of the dynamic obstacle, and if the two moments are close to each other, collision occurs. Nodes that may collide are often referred to as invalid or illegal nodes that need to be avoided when planning the trajectory and therefore will be added to the closed list.
Additionally, there may be situations where the trajectory planning fails, including but not limited to:
after the parent node is removed from the open list in step S130, if the open list is empty, it is determined that the trajectory planning has failed. This means that the extended child nodes are all located in the closed list, and no new child node is added to the open list, and the extension cannot be continued. This situation typically occurs when there are difficult obstacles in the map to surmount;
when the expansion reaches a specific number of times, the target path is still not obtained, which indicates that an exception may occur in the processing process, and the specific number of times can be related to the distance between the starting point and the end point;
the cost values of the nodes in the open list are all high, for example, all are higher than a specific value, which indicates that there may be an abnormality in the previous expansion process.
If the current planning fails, the planning may be resumed immediately, or the planning may be resumed after the information of the map, the start point, or the end point is updated, which is not limited by the present disclosure.
In summary, in the exemplary embodiment, an open list is established, a node with the smallest cost value in the open list is sequentially used as a parent node for expansion, a child node obtained through expansion is added into the open list, until an end point is added into the open list, the child node is reversely connected to a start point from the end point according to a relationship between the child node and the parent node, and a target trajectory from the start point to the end point is generated according to a state parameter of each node. On one hand, each node records state parameters, not only including the position of the node, but also including the time corresponding to the node, and whether the node collides with a moving obstacle in the map can be judged based on the time information, so that the trajectory planning in a dynamic scene is realized. On the other hand, the state parameters of the child nodes are obtained through the state parameters of the parent nodes and the state variables of the preset interval time, and the process accords with the kinematics characteristics of the object, so that the planned path has better practicability.
Exemplary embodiments of the present disclosure also provide a trajectory planning device. As shown in fig. 5, the trajectory planning apparatus 500 may include: an obtaining module 510, configured to obtain a starting point and an ending point in a map, and determine a state parameter of the starting point; an establishing module 520, configured to establish an open list to record a state parameter of each node therein, and add a starting point to the open list, where the state parameter of each node includes a time corresponding to the node; an extension module 530, configured to sequentially use a node with the smallest cost value in the open list as a parent node, determine, according to a state variable in a preset interval time, a state parameter of a child node of the parent node, add the child node to the open list, and remove the parent node from the open list; a connection module 540, configured to, when the end point is added to the open list, sequentially connect child nodes and parent nodes thereof from the end point until the end point is connected to the start point, so as to obtain a target path; a generating module 550, configured to generate a target track from a starting point to an end point according to the state parameter of each node in the target path.
In an alternative embodiment, the state parameters may also include attitude and velocity.
In an alternative embodiment, the extension module 530 may be configured to separately calculate child nodes reached after a preset interval time of movement through each state variable combination based on the state parameter of the parent node, and calculate the state parameters of the child nodes.
In an alternative embodiment, the state variables may include speed variables and steering variables.
In an alternative embodiment, the extension module 530 may comprise a cost calculation unit for determining the cost value of the child node by performing the following steps: determining a first child value of a child node according to a first child value of a parent node of the child node and a difference between state parameters of the child node and the parent node thereof; determining the cost value from the child node to the terminal point as a second child value of the child node; determining a cost value of the child node according to the first child value and the second child value of the child node; wherein the first descendant value of the starting point is 0.
In an optional implementation manner, the cost calculating unit may be further configured to determine a cost value from any node to the destination by performing the following steps: calculating the length of a Reeds-Shepp curve from any node to a terminal point as a first length; determining the length of a path from any node to a terminal point through an A star algorithm as a second length; and taking the larger length value of the first length and the second length as the cost value of any node to the terminal.
In an alternative embodiment, the extension module 530 may include a node update unit for determining a parent node of a child node when the child node is added to the open list by performing the following steps: when the child node is already positioned in the open list, acquiring an original father node of the child node; calculating the primary value of the child node based on the original father node, and calculating the current value of the child node based on the current father node; when the primary value is smaller than the current value, the relationship between the child node and the original father node is reserved; and when the primary value is greater than the current cost value, updating the father node of the child node to be the current father node.
In an alternative embodiment, the trajectory planning apparatus 500 may further include: the curve planning module is used for executing the following steps after the expansion module adds the child nodes into the open list: when a preset condition is met, generating a Reeds-Shepp curve from the child node to the terminal point; calculating the state parameters of each node in a Reeds-Shepp curve when the nodes move at a constant speed based on the state parameters of the child nodes; judging whether the Reeds-Shepp curve collides with an obstacle in the map or not according to the state parameters of each node in the Reeds-Shepp curve; when determining that the Reeds-Shepp curve collides with an obstacle in the map, the scheduling expansion module 530 continues to determine the state parameters of the child nodes of the parent nodes according to the state variables in the preset interval time by using the node with the minimum cost value in the open list as the parent node; when determining that the Reeds-Shepp curve does not collide with the obstacles in the map, sequentially connecting the child nodes and the father nodes thereof from the child nodes until the child nodes are connected to the starting point, and obtaining a target path by combining the Reeds-Shepp curve; and generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
In an optional implementation manner, the expanding module 530 may be further configured to, when adding the child node to the open list, remove the child node located in the closed list, and add the remaining child nodes to the open list.
In an alternative embodiment, the extension module 530 may be further configured to add the parent node to the closed list after the parent node is removed from the open list.
In an alternative embodiment, the expanding module 530 may be further configured to add the child node to the closed list when it is determined that the child node collides with an obstacle in the map according to the status parameter of the child node.
In an optional implementation manner, the extension module 530 may be further configured to determine that the trajectory planning fails if the open list is empty after the parent node is removed from the open list.
In an alternative embodiment, the open list is empty before the setup module 520 adds the start point to the open list.
The specific details of each module/unit in the above-mentioned apparatus have been described in detail in the method section, and the details that are not disclosed may refer to the contents of the method section, and thus are not described again.
As will be appreciated by one skilled in the art, aspects of the present disclosure may be embodied as a system, method or program product. Accordingly, various aspects of the present disclosure may be embodied in the form of: an entirely hardware embodiment, an entirely software embodiment (including firmware, microcode, etc.) or an embodiment combining hardware and software aspects that may all generally be referred to herein as a "circuit," module "or" system.
Exemplary embodiments of the present disclosure also provide a computer-readable storage medium having stored thereon a program product capable of implementing the above-described method of the present specification. In some possible embodiments, various aspects of the disclosure may also be implemented in the form of a program product comprising program code for causing an electronic device to perform the steps according to various exemplary embodiments of the disclosure described in the above-mentioned "exemplary methods" section of this specification, when the program product is run on the electronic device.
Referring to fig. 6, a program product 600 for implementing the above method according to an exemplary embodiment of the present disclosure is described, which may employ a portable compact disc read only memory (CD-ROM) and include program code, and may be run on an electronic device, such as a personal computer. However, the program product of the present disclosure is not limited thereto, and in this document, a readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The program product may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
A computer readable signal medium may include a propagated data signal with readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A readable signal medium may also be any readable medium that is not a readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Program code embodied on a readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Program code for carrying out operations for the present disclosure may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C + + or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computing device (e.g., through the internet using an internet service provider).
The exemplary embodiment of the present disclosure also provides an electronic device capable of implementing the above method. An electronic device 700 according to such an exemplary embodiment of the present disclosure is described below with reference to fig. 7. The electronic device 700 shown in fig. 7 is only an example and should not bring any limitation to the functions and the scope of use of the embodiments of the present disclosure.
As shown in fig. 7, electronic device 700 may take the form of a general purpose computing device. The components of the electronic device 700 may include, but are not limited to: at least one processing unit 710, at least one memory unit 720, a bus 730 that connects the various system components (including the memory unit 720 and the processing unit 710), and a display unit 740.
The memory unit 720 stores program code that may be executed by the processing unit 710 to cause the processing unit 710 to perform steps according to various exemplary embodiments of the present disclosure as described in the "exemplary methods" section above in this specification. For example, processing unit 710 may perform any one or more of the method steps of fig. 1-4.
The storage unit 720 may include readable media in the form of volatile memory units, such as a random access memory unit (RAM)721 and/or a cache memory unit 722, and may further include a read only memory unit (ROM) 723.
The memory unit 720 may also include programs/utilities 724 having a set (at least one) of program modules 725, such program modules 725 including, but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which, or some combination thereof, may comprise an implementation of a network environment.
The electronic device 700 may also communicate with one or more external devices 800 (e.g., keyboard, pointing device, bluetooth device, etc.), with one or more devices that enable a user to interact with the electronic device 700, and/or with any devices (e.g., router, modem, etc.) that enable the electronic device 700 to communicate with one or more other computing devices. Such communication may occur via an input/output (I/O) interface 750. Also, the electronic device 700 may communicate with one or more networks (e.g., a Local Area Network (LAN), a Wide Area Network (WAN), and/or a public network such as the internet) via the network adapter 760. As shown, the network adapter 760 communicates with the other modules of the electronic device 700 via the bus 730. It should be appreciated that although not shown in the figures, other hardware and/or software modules may be used in conjunction with the electronic device 700, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems, among others.
Through the above description of the embodiments, those skilled in the art will readily understand that the exemplary embodiments described herein may be implemented by software, or by software in combination with necessary hardware. Therefore, the technical solution according to the embodiments of the present disclosure may be embodied in the form of a software product, which may be stored in a non-volatile storage medium (which may be a CD-ROM, a usb disk, a removable hard disk, etc.) or on a network, and includes several instructions to enable a computing device (which may be a personal computer, a server, a terminal device, or a network device, etc.) to execute the method according to the exemplary embodiments of the present disclosure.
Furthermore, the above-described figures are merely schematic illustrations of processes included in methods according to exemplary embodiments of the present disclosure, and are not intended to be limiting. It will be readily understood that the processes shown in the above figures are not intended to indicate or limit the chronological order of the processes. In addition, it is also readily understood that these processes may be performed synchronously or asynchronously, e.g., in multiple modules.
It should be noted that although in the above detailed description several modules or units of the device for action execution are mentioned, such a division is not mandatory. Indeed, the features and functions of two or more modules or units described above may be embodied in one module or unit, according to exemplary embodiments of the present disclosure. Conversely, the features and functions of one module or unit described above may be further divided into embodiments by a plurality of modules or units.
Other embodiments of the disclosure will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure disclosed herein. This application is intended to cover any variations, uses, or adaptations of the disclosure following, in general, the principles of the disclosure and including such departures from the present disclosure as come within known or customary practice within the art to which the disclosure pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the disclosure being indicated by the following claims.
It will be understood that the present disclosure is not limited to the precise arrangements described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof. The scope of the present disclosure is to be limited only by the terms of the appended claims.
Claims (13)
1. A trajectory planning method, comprising:
acquiring a starting point and an end point in a map, and determining state parameters of the starting point;
establishing an open list to record the state parameters of each node, and adding the starting point into the open list, wherein the state parameters of each node comprise the time, the posture and the speed corresponding to the node;
sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to state variables in preset interval time, adding the child nodes into the open list, and removing the father node from the open list;
when the end point is added into the open list, sequentially connecting child nodes and father nodes thereof from the end point until the child nodes are connected to the starting point to obtain a target path;
generating a target track from the starting point to the end point according to the state parameter of each node in the target path;
when the child node is added into the open list, determining a cost value of the child node by the following method:
determining a first descendant value of the child node according to the first descendant value of the parent node of the child node and a difference between the state parameters of the child node and the parent node thereof; the first descendant value of the origin is 0;
determining a cost value of the child node to the destination as a second descendant value of the child node;
determining a cost value of the child node according to the first child value and the second child value of the child node;
wherein the cost value from any node to the end point is determined by:
calculating the length of a Reeds-Shepp curve from any node to the terminal point as a first length;
determining the length of a path from any node to the terminal point through an A star algorithm as a second length;
and taking the larger length value of the first length and the second length as the cost value of any node to the terminal.
2. The method according to claim 1, wherein the determining the state parameter of the child node of the parent node according to the state variable in the preset interval time comprises:
acquiring each state variable combination;
and respectively calculating the child nodes reached after the preset interval time is moved through each state variable combination based on the state parameters of the father node, and calculating the state parameters of the child nodes.
3. A method according to claim 1 or 2, characterized in that the state variables comprise a speed variable and a steering variable.
4. The method of claim 1, wherein when adding the child node to the open list, the method further comprises:
when the child node is already positioned in the open list, acquiring an original parent node of the child node;
calculating the primary value of the child node based on the original father node, and calculating the current value of the child node based on the current father node;
when the primary cost value is smaller than the current cost value, the relationship between the child node and the original father node is reserved;
and when the primary cost value is larger than the current cost value, updating the father node of the child node as the current father node.
5. The method of claim 1, wherein after adding the child node to the open list, the method further comprises:
when a preset condition is met, generating a Reeds-Shepp curve from the child node to the terminal;
calculating the state parameters of each node in the Reeds-Shepp curve when the uniform movement is adopted based on the state parameters of the child nodes;
judging whether the Reeds-Shepp curve collides with an obstacle in the map or not according to the state parameters of all nodes in the Reeds-Shepp curve;
when the fact that the Reeds-Shepp curve collides with the obstacles in the map is determined, the step of determining the state parameters of the child nodes of the father nodes by taking the node with the minimum cost value in the open list as the father node according to the state variables in the preset interval time is continuously executed;
when the Reeds-Shepp curve is determined not to collide with the obstacles in the map, sequentially connecting child nodes and parent nodes thereof from the child nodes until the child nodes are connected to the starting point, and obtaining a target path by combining the Reeds-Shepp curve;
and generating a target track from the starting point to the end point according to the state parameter of each node in the target path.
6. The method according to claim 1, wherein when adding the child node to the open list, the child node located in the closed list is removed, and the remaining child nodes are added to the open list.
7. The method of claim 6, wherein after removing the parent node from the open list, the method further comprises:
and adding the father node into the closed list.
8. The method of claim 6, wherein after determining the state parameters of the child nodes of the parent node according to the state variables within the preset time interval, the method further comprises:
and when the child node is determined to collide with the obstacle in the map according to the state parameters of the child node, adding the child node into the closed list.
9. The method of claim 6, wherein after removing the parent node from the open list, if the open list is empty, then determining that trajectory planning has failed.
10. The method of claim 1, wherein the open list is empty before the starting point is added to the open list.
11. A trajectory planning apparatus, comprising:
the acquisition module is used for acquiring a starting point and an end point in a map and determining the state parameters of the starting point;
the establishing module is used for establishing an open list so as to record the state parameters of each node, and adding the starting point into the open list, wherein the state parameters of each node comprise the time, the posture and the speed corresponding to the node;
the expansion module is used for sequentially taking the node with the minimum cost value in the open list as a father node, determining the state parameters of the child nodes of the father node according to the state variables in the preset interval time, adding the child nodes into the open list, and removing the father node from the open list;
the connection module is used for sequentially connecting child nodes and father nodes thereof from the end point until the child nodes are connected to the starting point to obtain a target path when the end point is added into the open list;
the generating module is used for generating a target track from the starting point to the end point according to the state parameter of each node in the target path;
wherein the extension module comprises a cost calculation unit for determining the cost value of the child node by performing the steps of:
determining a first descendant value of the child node according to the first descendant value of the parent node of the child node and a difference between the state parameters of the child node and the parent node thereof; the first descendant value of the origin is 0;
determining a cost value of the child node to the destination as a second descendant value of the child node;
determining a cost value of the child node according to the first child value and the second child value of the child node;
wherein the cost calculation unit is further configured to determine a cost value from any node to the end point by performing the following steps:
calculating the length of a Reeds-Shepp curve from any node to the terminal point as a first length;
determining the length of a path from any node to the terminal point through an A star algorithm as a second length;
and taking the larger length value of the first length and the second length as the cost value of any node to the terminal.
12. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the method of any one of claims 1 to 10.
13. An electronic device, comprising:
a processor; and
a memory for storing executable instructions of the processor;
wherein the processor is configured to perform the method of any of claims 1 to 10 via execution of the executable instructions.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911114431.XA CN110836671B (en) | 2019-11-14 | 2019-11-14 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911114431.XA CN110836671B (en) | 2019-11-14 | 2019-11-14 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110836671A CN110836671A (en) | 2020-02-25 |
| CN110836671B true CN110836671B (en) | 2021-09-14 |
Family
ID=69575115
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911114431.XA Active CN110836671B (en) | 2019-11-14 | 2019-11-14 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110836671B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111369066B (en) * | 2020-03-09 | 2022-06-24 | 广东南方数码科技股份有限公司 | Path planning method and device, electronic equipment and readable storage medium |
| CN111397624A (en) * | 2020-03-27 | 2020-07-10 | 湖南大学 | Global path planning method based on JPS and Hybrid A |
| CN112181999B (en) * | 2020-09-21 | 2022-01-07 | 北京京东乾石科技有限公司 | Track updating method and device, equipment and storage medium |
| CN112783166A (en) * | 2020-12-30 | 2021-05-11 | 深兰人工智能(深圳)有限公司 | Local trajectory planning method and device, electronic equipment and storage medium |
| CN115083139B (en) * | 2021-03-12 | 2023-11-24 | 上海交通大学 | Multi-vehicle scheduling method |
| CN114379584B (en) * | 2021-12-17 | 2023-12-22 | 阿波罗智联(北京)科技有限公司 | Vehicle control method, device, vehicle, equipment and storage medium |
| CN114475658B (en) * | 2022-02-23 | 2023-08-25 | 广州小鹏自动驾驶科技有限公司 | Automatic driving speed planning method and device, vehicle and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106441303A (en) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | Path programming method based on A* algorithm capable of searching continuous neighborhoods |
| CN108073176A (en) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | A kind of modified D*Lite vehicle dynamic path planing methods |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN109540146A (en) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | Paths planning method and device |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
| CN110274596A (en) * | 2019-04-14 | 2019-09-24 | 西安烛龙智能科技有限公司 | A kind of intelligent barrier avoiding method based on timing node |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106092111B (en) * | 2016-06-03 | 2019-09-24 | 山东师范大学 | A kind of vehicle route dynamic programming method, server and navigation system |
| CN108510095B (en) * | 2017-02-23 | 2020-12-22 | 北京京东乾石科技有限公司 | Method and device for determining goods picking path |
| US10663966B2 (en) * | 2017-03-29 | 2020-05-26 | Mitsubishi Electric Research Laboratories, Inc. | Vehicle motion control system and method |
-
2019
- 2019-11-14 CN CN201911114431.XA patent/CN110836671B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106441303A (en) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | Path programming method based on A* algorithm capable of searching continuous neighborhoods |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN108073176A (en) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | A kind of modified D*Lite vehicle dynamic path planing methods |
| CN109540146A (en) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | Paths planning method and device |
| CN110274596A (en) * | 2019-04-14 | 2019-09-24 | 西安烛龙智能科技有限公司 | A kind of intelligent barrier avoiding method based on timing node |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110836671A (en) | 2020-02-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110836671B (en) | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus | |
| Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
| US10459441B2 (en) | Method and system for operating autonomous driving vehicles based on motion plans | |
| CN114084155B (en) | Predictive intelligent automobile decision control method and device, automobile and storage medium | |
| CN112595337B (en) | Obstacle avoidance path planning method and device, electronic device, vehicle and storage medium | |
| Van Parys et al. | Distributed model predictive formation control with inter-vehicle collision avoidance | |
| JP2023547745A (en) | Vehicle obstacle avoidance methods, devices, electronic devices, storage media | |
| CN111795699B (en) | Unmanned vehicle path planning method and device and computer readable storage medium | |
| Hu et al. | Plug and play distributed model predictive control for heavy duty vehicle platooning and interaction with passenger vehicles | |
| CN109434831A (en) | Robot operation method and device, robot, electronic device and readable medium | |
| CN110782092A (en) | Trajectory planning method and device of unmanned distribution vehicle in unstructured scene | |
| CN112325898B (en) | Path planning method, device, equipment and storage medium | |
| CN110647151A (en) | Coordinate conversion method and device, computer readable storage medium and electronic equipment | |
| Altché et al. | Partitioning of the free space-time for on-road navigation of autonomous ground vehicles | |
| Nakamura et al. | Online update of safety assurances using confidence-based predictions | |
| Malayjerdi et al. | Practical path planning techniques in overtaking for autonomous shuttles | |
| CN113671941A (en) | Trajectory planning method, device, equipment and storage medium | |
| CN113442908A (en) | Automatic parking path planning method and system and parking control equipment | |
| WO2023201952A1 (en) | Method and apparatus for determining optimal traveling trajectory of vehicle | |
| JP2017124659A (en) | Parking assisting system, parking assisting method and program | |
| JPWO2020157990A1 (en) | Route planning device, route planning method, and program | |
| CN115097826A (en) | Vehicle turning track planning method and device | |
| Li et al. | Decision making for autonomous vehicles | |
| CN111832142A (en) | Method and apparatus for outputting information | |
| CN112747763B (en) | Local path planning method, device, electronic equipment and storage medium |
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 |