CN113804207A - Vehicle path planning method, system, equipment and storage medium - Google Patents
Vehicle path planning method, system, equipment and storage medium Download PDFInfo
- Publication number
- CN113804207A CN113804207A CN202010962442.XA CN202010962442A CN113804207A CN 113804207 A CN113804207 A CN 113804207A CN 202010962442 A CN202010962442 A CN 202010962442A CN 113804207 A CN113804207 A CN 113804207A
- Authority
- CN
- China
- Prior art keywords
- path
- node
- coordinate system
- candidate
- vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000011156 evaluation Methods 0.000 claims description 56
- 238000005070 sampling Methods 0.000 claims description 13
- 230000006870 function Effects 0.000 description 16
- 230000003068 static effect Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000009499 grossing Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention provides a vehicle path planning method, a system, equipment and a storage medium, wherein the method comprises the following steps: constructing a Cartesian coordinate system and a Frenet coordinate system in a road; determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system; in each path search, determining a current candidate path node based on a Frenet coordinate system; calculating the cost of each candidate path node according to a Cartesian coordinate system, and selecting the current path node according to the cost; and determining a planned path according to the searched path nodes. The method effectively reduces the search space by describing the road trend by using the Frenet coordinate system, and simultaneously reduces the real vehicle running environment by calculating the cost function through the Cartesian coordinate system, thereby improving the path planning efficiency on the basis of ensuring the path planning accuracy.
Description
Technical Field
The invention relates to the technical field of data processing, in particular to a vehicle path planning method, a vehicle path planning system, vehicle path planning equipment and a storage medium.
Background
The technology of the current mobile robot is developed rapidly, and with the continuous expansion of application scenes and modes of the robot in recent years, various mobile robots are layered endlessly, and an unmanned vehicle is one of the mobile robots. The existing unmanned vehicle local path planning method is various, and most path planning algorithms are based on a Cartesian coordinate system. However, the unmanned vehicle routing algorithm using the cartesian coordinate system usually takes a lot of time and cannot meet the requirement of time efficiency.
Disclosure of Invention
Aiming at the problems in the prior art, the invention aims to provide a vehicle path planning method, a vehicle path planning system, vehicle path planning equipment and a storage medium, wherein the Frenent coordinate system and a Cartesian coordinate system are combined to improve the path planning efficiency.
The embodiment of the invention provides a vehicle path planning method, which comprises the following steps:
constructing a Cartesian coordinate system and a Frenet coordinate system in a road;
determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system;
in each path search, determining a current candidate path node based on a Frenet coordinate system;
calculating the cost of each candidate path node according to a Cartesian coordinate system, and selecting the current path node according to the cost;
and determining a planned path according to the searched path nodes.
Optionally, the determining a current candidate path node based on the Frenet coordinate system includes the following steps:
and sampling at least one adjacent node in the increasing direction of the S-axis coordinate of the previous path node in the Frenet coordinate system, and adding a candidate path node list.
Optionally, after constructing the cartesian coordinate system and the Frenet coordinate system in the road, the method further includes the following steps:
and rasterizing the constructed Frenet coordinate system, and taking each grid point as a node.
Optionally, the determining a current candidate path node based on the Frenet coordinate system includes the following steps:
and sampling three adjacent grid points in the S-axis coordinate increasing direction of the path node at the previous moment in the Frenet coordinate system, and adding a candidate path node list.
Optionally, the calculating the cost of each candidate path node includes the following steps:
calculating each evaluation value of each candidate path node, wherein the evaluation values comprise evaluation values of path smoothness and/or evaluation values of the influence degree of obstacles on the path;
and weighting and summing the evaluation values according to the set weights of the evaluation values to obtain the cost of the candidate path node.
Optionally, the following steps are adopted to calculate each evaluation value of each candidate path node:
calculating the evaluation value of the path smoothness according to the orientation angles of the candidate path node and the previous path node in a Cartesian coordinate system;
and determining a line segment according to the position information of the candidate path node and the previous path node in a Cartesian coordinate system, and calculating the evaluation value of the influence degree of the obstacle on the path according to the distance between the obstacle and the line segment.
Optionally, the calculating each evaluation value of each candidate path node further includes calculating an evaluation value of a path right tendency and/or an evaluation value of a road tooth to a path repulsive force by using the following steps:
calculating the right tendency evaluation value of the path according to the distance between the candidate path node and the right edge of the road;
and calculating the evaluation value of the road tooth to the path repulsion according to the distance between the candidate path node and the left road tooth and the distance between the candidate path node and the right road tooth.
Optionally, the selecting a current path node according to the cost includes the following steps:
selecting a candidate path node with the minimum cost as a current path node;
the current path node is removed from the candidate path node list.
Optionally, the determining a planned path according to the searched path node includes the following steps:
judging whether the s coordinate value of the current path node is larger than the sum of the s coordinate value of the starting point and the path set planning length along the s direction;
if yes, the search is finished, the current path node is taken as the end point, and the path from the starting point to the end point is determined according to the searched path nodes.
The embodiment of the invention also provides a vehicle path planning system, which is used for realizing the vehicle path planning method and comprises the following steps:
the coordinate system building module is used for building a Cartesian coordinate system and a Frenet coordinate system in a road;
the node determining module is used for determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system;
the path searching module is used for determining current candidate path nodes based on a Frenet coordinate system in each path searching; calculating the cost of each candidate path node according to a Frenet coordinate system and a Cartesian coordinate system, and selecting the current path node according to the cost;
and the path planning module is used for determining a planned path according to the searched path nodes.
An embodiment of the present invention further provides a vehicle path planning device, including:
a processor;
a memory having stored therein executable instructions of the processor;
wherein the processor is configured to perform the steps of the vehicle path planning method via execution of the executable instructions.
Embodiments of the present invention further provide a computer-readable storage medium for storing a program, where the program is executed to implement the steps of the vehicle path planning method.
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.
The vehicle path planning method, the vehicle path planning system, the vehicle path planning equipment and the storage medium have the following beneficial effects:
the method combines a Freent coordinate system and a Cartesian coordinate system to carry out path planning, efficiently reduces the search space by using the Frenet coordinate system to describe the road trend, and reduces the real vehicle running environment by calculating a cost function through the Cartesian coordinate system, thereby improving the path planning efficiency on the basis of ensuring the path planning accuracy; the invention can be applied to path planning of unmanned vehicles and other vehicles, such as path planning in auxiliary driving.
Drawings
Other features, objects and advantages of the present invention will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, with reference to the accompanying drawings.
FIG. 1 is a flow chart of a vehicle path planning method according to an embodiment of the invention;
FIG. 2 is a schematic diagram of a Frenet coordinate system and a Cartesian coordinate system in accordance with one embodiment of the present invention;
FIG. 3 is a schematic diagram of a vehicle path planning system according to an embodiment of the present invention;
FIG. 4 is a schematic structural diagram of a vehicle path planning apparatus according to an embodiment of the present invention;
fig. 5 is a schematic structural diagram of a computer-readable storage medium according to an embodiment of the present invention.
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.
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.
As shown in fig. 1, an embodiment of the present invention provides a vehicle path planning method, including the following steps:
s100: constructing a Cartesian coordinate system and a Frenet coordinate system in a road, wherein the Frenet coordinate system takes a road middle line as an S axis and takes a left direction perpendicular to the S axis as an L axis, the road trend can be better described by introducing the Frenet coordinate system, and the road middle line is composed of a series of discrete points;
as shown in fig. 2, a schematic diagram of constructing a cartesian coordinate system and a Frenet coordinate system in a road is shown, where M is an origin of coordinates, X is a horizontal axis, Y is a vertical axis, and in the Frenet coordinate system, O is the origin of coordinates, S is the horizontal axis, and Y is the vertical axis. Suppose there is a point p (x) in the Cartesian coordinate systemp,yp) Finding two discrete points s (x) closest to p in the road middle lines,ys) And e (x)e,ye) Let s be(s) in the Frenet coordinate systems0), e has the coordinate(s) in the Frenet coordinate systeme0), then an arbitrary point p (x) in a cartesian coordinate systemp,yp) With its coordinates(s) in the Frenet coordinate systemp,lp) The relationship between (A) and (B) can be obtained by the following formula, which is defined as formula (1):
sp＝ss+λ(se-ss)
s200: determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system;
s300: in each path search, determining a current candidate path node based on a Frenet coordinate system;
s400: calculating the cost of each candidate path node according to a Cartesian coordinate system, and selecting the current path node according to the cost;
s500: and determining a planned path according to the searched path nodes.
The vehicle path planning method combines a Freent coordinate system and a Cartesian coordinate system to carry out path planning, when the path searching is carried out in the steps S300 and S400, the search space is efficiently reduced by describing the road trend by using the Frenet coordinate system, meanwhile, the real vehicle running environment is restored by calculating a cost function through the Cartesian coordinate system, and finally, the planned path is determined according to each path node obtained by searching, so that the path planning efficiency is improved on the basis of ensuring the path planning accuracy.
In this embodiment, in the step S300, the determining a current candidate path node based on the Frenet coordinate system includes the following steps:
and sampling at least one adjacent node in the increasing direction of the S-axis coordinate of the previous path node in the Frenet coordinate system, and adding a candidate path node list. Here, the previous path node is the path node obtained by the previous search. Therefore, the present invention efficiently reduces the search space by using the Frenet coordinate system to describe the road tendency.
Further, in this embodiment, the step S100: after constructing the Cartesian coordinate system and the Frenet coordinate system in the road, the method further comprises the following steps of:
and rasterizing the constructed Frenet coordinate system, and taking each grid point as a node.
The Frenet coordinate system after rasterization is shown in FIG. 2. Wherein the sampling interval Δ s in the s direction and the sampling interval Δ l in the l direction are determined. Δ s is used as a reference base. Assuming that the total length of the single-frame path plan of the vehicle is stotal。
The arbitrary waypoint states of the vehicle road are represented by nodes, and each node data structure is as follows: node { s, l, x, y, θ, d, str }, where (s, l) are coordinate values of the Node in a Frenet coordinate system, respectively, (x, y, θ) are position coordinate values and orientation angles of the Node in a cartesian coordinate system, d is a mileage of the Node to a path starting point, and str is a unique string identifier of an arbitrary coordinate (s, l) in a grid obtained by taking (sc, lc) as a reference and Δ s and Δ l as grid resolutions in the Frenet coordinate system, and is defined as formula (2) below:
str＝std：：tσ_string(g_s)+″_"+std：：to_string(g_l)
the step S200: and determining the position information of each node in the road under a Frenet coordinate system and the position information of each node in the road under a Cartesian coordinate system, namely determining the position information (s, l, str) of each node in the road under the Frenet coordinate system and the position information (x, y, theta) of each node in the road under the Cartesian coordinate system.
The attitude of the vehicle body under a Cartesian coordinate system at the initial moment is assumed to be (x)0,y0,θ0) As can be seen from the formula (1), the coordinates of the starting point of the vehicle at the initial time point in the Frenet coordinate system are(s)0,l0)，d00.0 str0 can be calculated from equation (2), and the data structure Node of the initial time starting point is obtained(s0,l0,x0,y0,θ0,d0,str0) The cost cos t of the vehicle moving to this point is 0.0.
In this embodiment, the sampling at least one node that is close to a previous path node in the Frenet coordinate system in the increasing direction of the S-axis coordinate includes the following steps:
adjacent three grid points (cur _ node 'S + deltas, cur _ node' l + deltal), (cur _ node 'S + deltas, cur _ node' l) and (cur _ node 'S + deltas, cur _ node' l-deltal) are sampled in the direction of increasing the S-axis coordinate to the path node at the previous moment in the Frenet coordinate system, and a candidate path node list is added. Wherein, cur _ node ' represents the path node obtained by the previous search, i.e. the previous path node, cur _ node's represents the s coordinate of the previous path node, and cur _ node ' l represents the l coordinate of the previous path node.
In this embodiment, in the step S300, selecting the current path node according to the cost includes the following steps:
selecting a candidate path node with the minimum cost as a current path node;
the current path node is removed from the candidate path node list.
Further, the step S500: determining a planned path according to the searched path nodes, comprising the following steps:
judging whether the s coordinate value of the current path node is larger than the s coordinate value of the starting point or not and setting a planning length s of the path along the direction stotalThe sum of (1);
if so, ending the search, taking the current path node as an end point, and determining a path from the starting point to the end point according to each searched path node;
if not, the process returns to step S300, and continues to search for the next path node.
Specifically, in this embodiment, the following steps may be adopted to search for the path node:
(1) obtaining a data structure Node(s) of an initial time starting point0,l0,x0,y0,θ0,d0,str0) After the cost cos t, adding the node of the starting point into an OPEN _ SET and a priority queue ordered according to the cost, wherein the queue corresponds to a candidate node list;
(2) corresponding to the step S300, three adjacent grid points (cur _ Node 'S + Δ S, cur _ Node' l + Δ l), (cur _ Node 'S + Δ S, cur _ Node' l) and (cur _ Node 'S + Δ S, cur _ Node' l- Δ l) are sampled from the path Node obtained by the previous search in the direction of increasing the S-axis coordinate, that is, three adjacent nodes are respectively nodesright_up、NoderightAnd Noderight_downAnd parent _ node of three adjacent nodes is set as cur _ node. If the search is the first search, adjacent three grid points are sampled from the starting point to the increasing direction of the S-axis coordinate, and three adjacent nodes are obtained.
(3) Corresponding to the above step S300, three adjacent nodes are traversed. For each adjacent node, firstly judging whether the adjacent node is positioned in a road, if not, deleting the sampling point, otherwise, detecting whether the adjacent node is positioned in a CLOSE _ SET; if the node is in CLOSE _ SET, deleting the sampling point, otherwise detecting whether the adjacent node is in OPEN _ SET; if the sampling point is in the OPEN _ SET, deleting the sampling point, otherwise, calculating the cost of the sampling point through a cost function system according to the adjacent nodes and the parent _ node, and adding the cost into the OPEN _ SET and the queue;
(4) corresponding to the step S400, the node with the smallest cost is taken out from the queue as the current path node cur _ node, and the current path node cur _ node is deleted from the queue and placed in CLOSE _ SET;
(5) if cur _ node>s0+stotalIf yes, the single frame planning length reaches the path planning length s set along the direction stotalMaking final _ node be cur _ node, that is, the current node is the end point node, and the search is finished;
otherwise, returning to the step (2), and continuing to search the next path node, namely circularly executing the steps (2), (3) and (4) until the search is finished;
(6) if the search is finished, the whole planned path can be recurred according to final _ node and its parent _ node.
In this embodiment, in the step S400, calculating the cost of each candidate path node includes the following steps:
calculating respective evaluation values of respective candidate path nodes including an evaluation value g of path smoothingsmoothAnd/or evaluation value of the influence degree of the obstacle on the path;
and weighting and summing the evaluation values according to the set weights of the evaluation values to obtain the cost of the candidate path node.
In this embodiment, the evaluation value of the degree of influence of the obstacle on the path may be further divided into an evaluation value of the degree of influence of the dynamic obstacle on the path and a static obstacle gdynamicEvaluation value g for degree of influence on pathstaticAnd different weight coefficients may be employed, respectively.
In this embodiment, the following steps are taken to calculate each evaluation value of each candidate path node:
calculating the evaluation value g of the path smoothness according to the orientation angles of the candidate path node and the previous path node in a Cartesian coordinate systemsmooth；
Determining a line segment according to the position information of the candidate path node and the previous path node in a Cartesian coordinate system, and calculating the evaluation value g of the influence degree of the dynamic obstacle on the path according to the distance between the dynamic obstacle and the line segmentdynamicCalculating the evaluation value g of the influence degree of the static obstacle on the path according to the distance between the static obstacle and the line segmentstatic。
In this embodiment, the calculating the evaluation values of the candidate path nodes further includes calculating a path rightward inclination evaluation value g by using the following stepsrefAnd/or calculating an evaluation value g of road tooth-to-path repulsionboundary：
Calculating the right tendency evaluation value g of the path according to the distance between the candidate path node and the right edge of the roadref；
According to the candidate path node and the left horseCalculating evaluation value g of road tooth to path repulsion force by the distance between the road tooth and the right road toothboundary。
Specifically, in this embodiment, the following cost function system formula may be adopted to calculate the cost:
cost＝smooth_w·gsmooth+ref_w·gref+boundary_w·gboundary+static_w·gstatic+dynamic_w·gdynamic
where smooth _ w is the path smoothing weight, gsmoothFor the evaluation value of path smoothing, ref _ w is the right tendency weight of the path, grefBoundary _ w is road tooth repulsion weight, g, for the evaluation value of the path right tendencyboundaryStatic _ w is the weight of the degree of influence of static obstacles on the path, g is the evaluation value of road tooth on the repulsive force of the pathstaticThe evaluation value of the influence degree of the static obstacle on the path, dynamic _ w is the sensitive weight of the path on the dynamic obstacle, gdynamicThe evaluation value is the influence degree of the dynamic obstacle on the path.
Each evaluation value can be calculated by using each evaluation function as follows:
(1) the merit function for path smoothing is as follows:
gsmooth＝exp(|h(cur_node″.θ-parent_node.θ)|)
the function h is used for converting the angle cur _ node'. theta-parent _ node theta to be between [ -pi, pi), and exp () is an exponential function with a natural constant e as a base. cur _ node "is a candidate path node, cur _ node". theta. is an orientation angle of the candidate path node in a cartesian coordinate system, parent _ node is a previous path node, and parent _ node. theta. is an orientation angle of the previous path node in the cartesian coordinate system.
(2) The path-to-right tendency evaluation function is as follows:
gref＝|cur_node″.l-ref_l|
wherein ref _ l is the reference distance to the right of the middle line of the traversal road, and cur _ node ". l is the coordinate of the candidate path node.
(3) Evaluation function of road tooth to path repulsion:
gboundary＝exp(-0.5·(left_d-cur_node″.l)·(left_d-cur_node″.l)/(σ·σ))+exp(-0.5·(right_d-cur_node″.l)·(right_d-cur_node″.l)/(σ·σ))
where left _ d is the distance from the left road tooth at cur _ node.s to the road middle line, right _ d is the distance from the right road tooth at cur _ node.s to the road middle line, and σ is the gaussian standard deviation.
(4) Evaluation function of influence degree of static obstacles on path:
gstatic＝∑exp(-0.5·d1·d1/(σ·σ))
wherein d is1The distance between the static obstacle and the line segment with parent node xy and cur node xy as end points, namely the line segment with the xy coordinates of the previous path node in a Cartesian coordinate system and the xy coordinates of the candidate path node in the Cartesian coordinate system as end points.
(5) Evaluation function of influence degree of dynamic obstacle on path:
gdynamic＝∑exp(-0.5·d2·d2/(σ·σ))
wherein d is2The distance between the dynamic barrier and the line segment with parent node xy and cur node xy as end points, namely the line segment with xy coordinates of the previous path node in a Cartesian coordinate system and xy coordinates of the candidate path node in the Cartesian coordinate system as end points.
Because the obstacle is moving, the time t dimension is required to be introduced in the vehicle path planning stage, the corresponding relation between s and t in the frame track on the vehicle is approximately the corresponding relation between the current frame s and t, namely the time t required for the vehicle to move from the planning starting point to the cur _ node2。
As shown in fig. 3, an embodiment of the present invention further provides a vehicle path planning system, which is used to implement the vehicle path planning method, and the system includes:
a coordinate system constructing module M100, configured to construct a Cartesian coordinate system and a Frenet coordinate system in a road;
the node determining module M200 is configured to determine location information of each node in the road in a Frenet coordinate system and location information in a cartesian coordinate system;
a path searching module M300, configured to determine, in each path search, a current candidate path node based on a Frenet coordinate system; calculating the cost of each candidate path node according to a Frenet coordinate system and a Cartesian coordinate system, and selecting the current path node according to the cost;
and a path planning module M400, configured to determine a planned path according to the searched path node.
The vehicle path planning system of the invention combines a Freent coordinate system and a Cartesian coordinate system to carry out path planning, when a path searching module M300 is adopted to carry out path searching, the Frenet coordinate system is utilized to describe road trends, the searching space is efficiently reduced, meanwhile, the Cartesian coordinate system is used for calculating a cost function to restore the real vehicle running environment, and finally, the path planning module M400 determines the planned path according to each path node obtained by searching, thereby improving the path planning efficiency on the basis of ensuring the accuracy of path planning.
In the vehicle path planning system of the present invention, the functions of each module may be implemented by using the specific implementation of the vehicle path planning method described above, which is not described herein again.
The embodiment of the invention also provides vehicle path planning equipment, which comprises a processor; a memory having stored therein executable instructions of the processor; wherein the processor is configured to perform the steps of the vehicle path planning method via execution of the executable instructions.
As will be appreciated by one skilled in the art, aspects of the present invention may be embodied as a system, method or program product. Thus, various aspects of the invention 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" platform.
An electronic device 600 according to this embodiment of the invention is described below with reference to fig. 4. The electronic device 600 shown in fig. 4 is only an example, and should not bring any limitation to the functions and the scope of use of the embodiments of the present invention.
As shown in fig. 4, the electronic device 600 is embodied in the form of a general purpose computing device. The components of the electronic device 600 may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 that connects the various system components (including the storage unit 620 and the processing unit 610), a display unit 640, and the like.
Wherein the memory unit stores program code executable by the processing unit 610 to cause the processing unit 610 to perform steps according to various exemplary embodiments of the present invention described in the vehicle path planning method section above in this specification. For example, the processing unit 610 may perform the steps as shown in fig. 1.
The storage unit 620 may include readable media in the form of volatile memory units, such as a random access memory unit (RAM)6201 and/or a cache memory unit 6202, and may further include a read-only memory unit (ROM) 6203.
The memory unit 620 may also include a program/utility 6204 having a set (at least one) of program modules 6205, such program modules 6205 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 link.
The electronic device 600 may also communicate with one or more external devices 700 (e.g., keyboard, pointing device, bluetooth device, etc.), with one or more devices that enable a user to interact with the electronic device 600, and/or with any devices (e.g., router, modem, etc.) that enable the electronic device 600 to communicate with one or more other computing devices. Such communication may occur via an input/output (I/O) interface 650. Also, the electronic device 600 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 660. The network adapter 660 may communicate with other modules of the electronic device 600 via the bus 630. 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 600, 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.
Embodiments of the present invention further provide a computer-readable storage medium for storing a program, where the program is executed to implement the steps of the vehicle path planning method. In some possible embodiments, aspects of the invention may also be implemented in the form of a program product comprising program code for causing a terminal device to perform the steps according to various exemplary embodiments of the invention described in the vehicle path planning method section above of this specification when the program product is executed on the terminal device.
Referring to fig. 5, a program product 800 for implementing the above method according to an embodiment of the present invention is described, which may employ a portable compact disc read only memory (CD-ROM) and include program code, and may be executed on a terminal device, such as a personal computer. However, the program product of the present invention is not limited in this regard and, in the present 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.
The computer readable storage 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 storage 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 storage 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 aspects of the present invention 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).
In conclusion, by adopting the vehicle path planning method, the vehicle path planning system, the vehicle path planning equipment and the storage medium, the path planning is carried out by combining the Frenet coordinate system and the Cartesian coordinate system, the search space is efficiently reduced by describing the road trend by using the Frenet coordinate system, and meanwhile, the real vehicle running environment is restored by calculating the cost function through the Cartesian coordinate system, so that the path planning efficiency is improved on the basis of ensuring the path planning accuracy; the invention can be applied to path planning of unmanned vehicles and other vehicles, such as path planning in auxiliary driving.
The foregoing is a more detailed description of the invention in connection with specific preferred embodiments and it is not intended that the invention be limited to these specific details. For those skilled in the art to which the invention pertains, several simple deductions or substitutions can be made without departing from the spirit of the invention, and all shall be considered as belonging to the protection scope of the invention.
Claims (12)
1. A vehicle path planning method is characterized by comprising the following steps:
constructing a Cartesian coordinate system and a Frenet coordinate system in a road;
determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system;
in each path search, determining a current candidate path node based on a Frenet coordinate system;
calculating the cost of each candidate path node according to a Cartesian coordinate system, and selecting the current path node according to the cost;
and determining a planned path according to the searched path nodes.
2. The vehicle path planning method according to claim 1, wherein determining the current candidate path node based on the Frenet coordinate system comprises the steps of:
and sampling at least one adjacent node in the increasing direction of the S-axis coordinate of the previous path node in the Frenet coordinate system, and adding a candidate path node list.
3. The vehicle path planning method according to claim 2, further comprising the following steps after constructing the cartesian coordinate system and the Frenet coordinate system in the road:
and rasterizing the constructed Frenet coordinate system, and taking each grid point as a node.
4. The vehicle path planning method according to claim 1, wherein determining the current candidate path node based on the Frenet coordinate system comprises the steps of:
and sampling three adjacent grid points in the S-axis coordinate increasing direction of the path node at the previous moment in the Frenet coordinate system, and adding a candidate path node list.
5. The vehicle path planning method according to claim 1, wherein the calculating the cost of each candidate path node comprises the steps of:
calculating each evaluation value of each candidate path node, wherein the evaluation values comprise evaluation values of path smoothness and/or evaluation values of the influence degree of obstacles on the path;
and weighting and summing the evaluation values according to the set weights of the evaluation values to obtain the cost of the candidate path node.
6. The vehicle path planning method according to claim 5, wherein each evaluation value of each candidate path node is calculated by:
calculating the evaluation value of the path smoothness according to the orientation angles of the candidate path node and the previous path node in a Cartesian coordinate system;
and determining a line segment according to the position information of the candidate path node and the previous path node in a Cartesian coordinate system, and calculating the evaluation value of the influence degree of the obstacle on the path according to the distance between the obstacle and the line segment.
7. The vehicle path planning method according to claim 5, wherein the calculating each evaluation value of each candidate path node further comprises calculating a path right tendency evaluation value and/or calculating an evaluation value of road tooth-to-path repulsion force by adopting the following steps:
calculating the right tendency evaluation value of the path according to the distance between the candidate path node and the right edge of the road;
and calculating the evaluation value of the road tooth to the path repulsion according to the distance between the candidate path node and the left road tooth and the distance between the candidate path node and the right road tooth.
8. The vehicle path planning method according to claim 1, wherein the selecting a current path node according to the cost comprises the steps of:
selecting a candidate path node with the minimum cost as a current path node;
the current path node is removed from the candidate path node list.
9. The vehicle path planning method according to claim 1, wherein the determining of the planned path based on the searched path nodes comprises the steps of:
judging whether the s coordinate value of the current path node is larger than the sum of the s coordinate value of the starting point and the path set planning length along the s direction;
if yes, the search is finished, the current path node is taken as the end point, and the path from the starting point to the end point is determined according to the searched path nodes.
10. A vehicle path planning system for implementing the vehicle path planning method according to any one of claims 1 to 9, characterized by comprising:
the coordinate system building module is used for building a Cartesian coordinate system and a Frenet coordinate system in a road;
the node determining module is used for determining the position information of each node in the road under a Frenet coordinate system and the position information under a Cartesian coordinate system;
the path searching module is used for determining current candidate path nodes based on a Frenet coordinate system in each path searching; calculating the cost of each candidate path node according to a Frenet coordinate system and a Cartesian coordinate system, and selecting the current path node according to the cost;
and the path planning module is used for determining a planned path according to the searched path nodes.
11. A vehicle path planning apparatus, comprising:
a processor;
a memory having stored therein executable instructions of the processor;
wherein the processor is configured to perform the steps of the vehicle path planning method of any of claims 1-9 via execution of the executable instructions.
12. A computer readable storage medium storing a program, characterized in that the program when executed implements the steps of the vehicle path planning method of any one of claims 1 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010962442.XA CN113804207B (en) | 2020-09-14 | 2020-09-14 | Vehicle path planning method, system, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010962442.XA CN113804207B (en) | 2020-09-14 | 2020-09-14 | Vehicle path planning method, system, equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113804207A true CN113804207A (en) | 2021-12-17 |
| CN113804207B CN113804207B (en) | 2024-06-18 |
Family
ID=78943470
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010962442.XA Active CN113804207B (en) | 2020-09-14 | 2020-09-14 | Vehicle path planning method, system, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113804207B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007257274A (en) * | 2006-03-23 | 2007-10-04 | Toyota Motor Corp | Path retrieval system and path retrieval method for traveling object |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN110647151A (en) * | 2019-10-16 | 2020-01-03 | 北京京东乾石科技有限公司 | Coordinate conversion method and device, computer readable storage medium and electronic equipment |
| CN110749333A (en) * | 2019-11-07 | 2020-02-04 | 中南大学 | Unmanned vehicle motion planning method based on multi-objective optimization |
-
2020
- 2020-09-14 CN CN202010962442.XA patent/CN113804207B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007257274A (en) * | 2006-03-23 | 2007-10-04 | Toyota Motor Corp | Path retrieval system and path retrieval method for traveling object |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN110647151A (en) * | 2019-10-16 | 2020-01-03 | 北京京东乾石科技有限公司 | Coordinate conversion method and device, computer readable storage medium and electronic equipment |
| CN110749333A (en) * | 2019-11-07 | 2020-02-04 | 中南大学 | Unmanned vehicle motion planning method based on multi-objective optimization |
Non-Patent Citations (2)
| Title |
|---|
| 宋晓茹;高泽鹏;陈超波;钱富才;: "基于ADRC的机器鱼的路径跟踪研究", 计算机仿真, no. 05, 15 May 2020 (2020-05-15), pages 301 - 305 * |
| 肖宏宇;付志强;陈慧勇;: "面向低速自动驾驶车辆的避障规划研究", 同济大学学报(自然科学版), vol. 47, no. 1, pages 164 - 170 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113804207B (en) | 2024-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110260870B (en) | Map matching method, device, equipment and medium based on hidden Markov model | |
| US10593110B2 (en) | Method and device for computing a path in a game scene | |
| CN109432777B (en) | Path generation method and device, electronic equipment and storage medium | |
| CN111928867B (en) | Path planning method, system, equipment and storage medium based on time expansion | |
| CN110647151B (en) | Coordinate conversion method and device, computer readable storage medium and electronic equipment | |
| US10982966B2 (en) | Generation of route network data for movement | |
| KR20140055134A (en) | Apparatus and method for planning path of robot, and the recording media storing the program for performing the said method | |
| CN111401779B (en) | Robot positioning deployment method, device, equipment and storage medium | |
| CN109434831A (en) | Robot operation method and device, robot, electronic device and readable medium | |
| CN110926491A (en) | Planning method and system for shortest path | |
| CN111862604B (en) | Unmanned vehicle control method and device, computer storage medium and electronic equipment | |
| CN111678527A (en) | Path network graph generation method and device, electronic equipment and storage medium | |
| JP2021076605A (en) | Method and system for tracking trajectory based on visual localization and odometry | |
| CN114626169A (en) | Traffic network optimization method, device, equipment, readable storage medium and product | |
| JPWO2020105189A1 (en) | Route planning device, route planning method, and program | |
| CN113804207B (en) | Vehicle path planning method, system, equipment and storage medium | |
| CN116878532A (en) | Path planning method and device for mobile object and related equipment | |
| CN108776668B (en) | Path estimation method, system, equipment and storage medium based on road network nodes | |
| CN115061499B (en) | Unmanned aerial vehicle control method and unmanned aerial vehicle control device | |
| JPWO2020157990A1 (en) | Route planning device, route planning method, and program | |
| CN112379692B (en) | Method, device and equipment for determining unmanned aerial vehicle air route and storage medium | |
| JP2021143830A (en) | Information processing device and information processing method | |
| Bigaj et al. | A memetic algorithm based procedure for a global path planning of a movement constrained mobile robot | |
| US20220300002A1 (en) | Methods and systems for path planning in a known environment | |
| JP2022023794A (en) | Indoor autonomous navigation based on natural language |
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 |