CN116839614A - Global path planning method considering dynamics and obstacle avoidance safety constraint - Google Patents
Global path planning method considering dynamics and obstacle avoidance safety constraint Download PDFInfo
- Publication number
- CN116839614A CN116839614A CN202310808867.9A CN202310808867A CN116839614A CN 116839614 A CN116839614 A CN 116839614A CN 202310808867 A CN202310808867 A CN 202310808867A CN 116839614 A CN116839614 A CN 116839614A
- Authority
- CN
- China
- Prior art keywords
- global path
- point
- path
- straight
- curve
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000005457 optimization Methods 0.000 claims abstract description 59
- 230000003068 static effect Effects 0.000 claims abstract description 10
- 238000011478 gradient descent method Methods 0.000 claims abstract description 5
- 238000001514 detection method Methods 0.000 claims description 7
- 238000009499 grossing Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000010845 search algorithm Methods 0.000 description 4
- 238000012805 post-processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention relates to a global path planning method considering dynamics and obstacle avoidance safety constraint, firstly, an initial global path from a starting point to an end point is planned according to a global path planning rule; then extracting static obstacle position information in a high-precision map, constructing an optimization problem by taking curvature and distance between the static obstacle and the obstacle as cost functions, and carrying out first optimization solution on a planned initial global path based on a gradient descent method to obtain a preprocessed global path; and finally, constructing a convex optimization problem by taking smoothness and length as cost functions, performing secondary optimization on the preprocessed global path through a QP algorithm, and finally outputting the global path meeting the requirements of vehicle dynamics and obstacle avoidance safety so as to improve the passing rate of the automatic driving vehicle in a narrow-channel scene and improve the smoothness and comfort of the automatic driving vehicle.
Description
Technical Field
The invention relates to the technical field of path planning of intelligent driving vehicles, in particular to a global path planning method considering dynamics and obstacle avoidance safety constraints.
Background
The path planning is one of core technologies of intelligent driving automobiles, and the main function of the path planning is to provide a vehicle driving path and guide the intelligent driving automobiles to safely drive. The path planning of the intelligent driving automobile is divided into two layers of work of global path planning and local path planning. Global path planning plans a shortest path from a start point to an end point based on known map information. The local path planning is to plan a safe and comfortable path to guide the automobile to run by taking the global path as a reference path according to the real-time perceived obstacle information. The current industry does not consider vehicle dynamics and obstacle avoidance safety constraints when performing global path planning, but rather takes into account mainly in the local path planning module. The difficulty of planning the local path in the narrow channel scene is increased, the planned local path is difficult to enable the host vehicle to pass smoothly, and the host vehicle can only pass through auxiliary modes such as getting rid of poverty, turning around or reversing, and the automatic driving fluency and comfortableness are seriously affected.
The prior art discloses a path planning method, a device, a vehicle and a medium for an automatic driving vehicle, wherein the method comprises the following steps: identifying road section information of all road sections in the target area; matching the actual passing difficulty level of each road section according to the road section information; and constructing a topological map according to the actual traffic difficulty level of each road section and the connection relation between the road sections, and searching the global path of the topological map to obtain the optimal global planning path of the automatic driving vehicle, and controlling the automatic driving of the vehicle to drive along the optimal global planning. The patent improves the search algorithm of global path planning, and considers collision detection security cost when calculating search algorithm cost (path weight, which means how much cost is spent in passing through the path), thereby globally considering the risk brought by narrow environment. However, this approach tends to trap the search path into a locally optimal solution and does not take into account vehicle dynamics constraints.
Disclosure of Invention
The invention aims to provide a global path planning method for improving driving fluency and comfort and considering dynamics and obstacle avoidance safety constraints, so as to solve the technical problem that a narrow channel scene in the prior art is difficult to pass.
In order to achieve the above object, the present invention provides a global path planning method considering dynamics and obstacle avoidance security constraints, comprising the steps of:
s1: generating an initial global path according to the map and the input starting point and end point information;
s2: extracting curve information and straight-path information between curves in the initial global path according to the curvature and curvature difference of the initial global path;
s3: extracting static obstacle position information in a map, performing collision detection and curvature calculation on an initial global path according to curve information in the initial global path and straight-path information among curves, and extracting curves and straight-paths which do not meet obstacle avoidance and vehicle dynamics requirements;
s4: performing numerical optimization on curves and straight channels which do not meet the obstacle avoidance and vehicle dynamics requirements, and obtaining the path points of the curves and the straight channels after pretreatment; obtaining left and right boundaries of each path point according to the obstacle information in the fixed distance range at two sides of the path point;
s5: constructing a convex optimization problem by taking smoothness, length and offset distance from an original lane line as cost functions, wherein an optimization variable of the convex optimization problem is coordinates of path points of a curve and a straight road after pretreatment, meanwhile, applying a hard constraint form to left and right boundaries of each path point obtained in the step S4, and solving the convex optimization problem through a QP algorithm to obtain the curve and the straight road after secondary optimization;
s6: matching and splicing the curve and the straight channel after the secondary optimization with the initial global path to form an optimized global path.
Further, step S1 includes:
s101: analyzing the map, extracting a weighted directed graph by taking the road section as a node, and calculating costs of all nodes according to the road attribute;
s102: searching an initial global path connecting a starting point node and an end point node by adopting a searching algorithm;
s103: analyzing the data of the map to obtain a lane reference line in the road section, wherein the lane reference line is given in the form of a discrete point array, and each discrete point comprises two-dimensional and above coordinate information to obtain the id and the coordinate of each global path point in the initial global path;
s104: and calculating the curvature and the orientation angle of the global path point, and assigning the curvature and the orientation angle to the global path point array as information.
Further, in step S2, the curve information in the initial global path and the straight-path information between the curves are assigned to the global path point;
the curve information comprises curve id, curve type, curve in-and-out angle, curve start point id; the straight road information between the curves comprises a straight road id, a straight road orientation angle and a straight road starting and stopping point id;
the global path point information includes the path point id, coordinates, curvature, orientation angle, a straight/curved road identifier, and the id of the straight/curved road where the straight/curved road identifier is used to indicate whether the reference point of the path point is in a straight road or a curved road.
Further, in step S3, each global path point in the initial global path is sequentially determined, and if a certain global path point does not meet the obstacle avoidance and vehicle dynamics requirements, the curve or straight path where the global path point is located does not meet the obstacle avoidance and vehicle dynamics requirements.
Further, in step S3, collision detection is performed by using the global path point coordinates as the coordinates of the center point of the rear axle of the vehicle, using the direction angle of the global path point as the direction angle of the direction of the vehicle, calculating four corner coordinates of the vehicle, calculating the distances between the four corner coordinates and the obstacle point, if the distance between the four corner coordinates and the obstacle point is smaller than the given safety distance, the global path point does not meet the obstacle avoidance safety requirement, and if the distances between the four corner points and the obstacle are larger than the safety distance, the global path point meets the obstacle avoidance safety requirement.
Further, in step S3, whether the global path point meets the vehicle dynamics requirement is determined by the curvature of the global path point, and if the curvature of the global path point is greater than the critical curvature, the global path point meets the vehicle dynamics requirement; if the curvature of the global path point is less than or equal to the critical curvature, the global path point does not meet the vehicle dynamics requirements.
Further, in step S4, different non-convex optimization problems are respectively constructed for curves and straight roads which do not meet the obstacle avoidance and vehicle dynamics requirements, and the solution is performed based on a gradient descent method; wherein,
for a curve, the optimization variables are the radius and the circle center position of the curve arc, and the Cost function comprises the reciprocal cost_curve of the curve arc radius, the distance cost_obs_c between the arc and an obstacle and the offset distance cost_displacement_c between the arc and the lane line;
for a straight road, the optimization variable is a straight line segment equation parameter, and the Cost function comprises a distance cost_obs_s between a straight line segment and an obstacle and an offset distance cos_displacement_s between a straight line segment path point and a lane line.
Further, assuming that the Cost function of the curve is Cost1, then:
Cost1＝Cost_curve+Cost_obs_c+Cost_deviation_c；
wherein ,w curve r is the radius of an arc, and is the dynamic weight coefficient;
w obs_c for the obstacle avoidance weight coefficient, dci is the distance between the arc and each obstacle point, and n is the number of all the obstacle points;
(x r ，y r ) Is the center coordinates of (x) i ，y i ) The coordinates of the ith obstacle point are the half vehicle width w;
Cost_deviation_c＝w deviation_c l c ，w deviation_c for the weight coefficient close to the original path, l c Is the sum of the distances of the arc from the original path point;
let the Cost function of the straight path be Cost2, then:
Cost2＝Cost_obs_s+Cos_deviation_s；
wherein ,w obs_s dsi is the distance from each obstacle point to a straight line for avoiding the obstacle weight coefficient;
Cost_deviation_s＝w deviation_s l s ，w deviation_s for the weight coefficient close to the original path, l s Is the sum of the distances from the original path point to the straight line segment.
Further, in step S5, a cost function is taken as the smoothness, the length and the offset distance from the original lane line, and the specific form of the cost function is as follows:
Cost3＝Cost_smooth+Cost_length+Cost_deviation；
wherein ,w smooth representing a smoothing term weight coefficient; x is x i 、y i Representing the optimized ith path point coordinate;
w length a weight coefficient representing a path length term;
x i-ref 、y i-ref representing the ith waypoint coordinates to be optimized.
Further, in step S6, distances between the line segment start points and end points of the curve and the straight road after the second optimization and the global path points of the initial global path are calculated, global path points of the initial global path closest to the optimized line segment start points and end points are found, and the optimized line segment is used for replacing the corresponding path points in the initial global path, so as to obtain the optimized global path.
Compared with the prior art, the invention has the beneficial effects that:
the method comprises the steps of firstly planning an initial global path from a starting point to an end point; then extracting static obstacle position information in the map, and adopting a numerical optimization-based method to carry out post-processing on the searched initial global path to generate a smooth pre-processed global path which accords with vehicle dynamics and collision safety; and finally, constructing a convex optimization problem by taking smoothness, length and offset distance from the original lane line as cost functions, performing secondary optimization on the preprocessed global path through a QP algorithm, and finally outputting the global path meeting the requirements of vehicle dynamics and obstacle avoidance safety. The invention carries out numerical optimization and secondary optimization on the generated initial global path, in the optimization process, the initial global path is used as a reference line and is used for subsequent optimization, so that the initial global path is directly used for subsequent local path planning, the reference line is not required to be smoothed again in real time, and online computing resources are saved. In addition, the vehicle dynamics constraint and the collision safety constraint are considered, the planning difficulty of the local path of the narrow channel is reduced, the passing rate of the planned path is improved, and the fluency and the comfortableness of automatic driving are improved.
Drawings
FIG. 1 is a flow chart of a global path planning method of the present invention;
FIG. 2 is an exemplary diagram of a global path planning method of the present invention for curve processing.
Detailed Description
Further advantages and effects of the present invention will become readily apparent to those skilled in the art from the disclosure herein, by referring to the accompanying drawings and the preferred embodiments. The invention may be practiced or carried out in other embodiments that depart from the specific details, and the details of the present description may be modified or varied from the spirit and scope of the present invention. It should be understood that the preferred embodiments are presented by way of illustration only and not by way of limitation.
It should be noted that the illustrations provided in the following embodiments merely illustrate the basic concept of the present invention by way of illustration, and only the components related to the present invention are shown in the drawings and are not drawn according to the number, shape and size of the components in actual implementation, and the form, number and proportion of the components in actual implementation may be arbitrarily changed, and the layout of the components may be more complicated.
As shown in fig. 1, a global path planning method taking dynamics and obstacle avoidance security constraints into consideration according to a preferred embodiment of the present invention includes the following steps:
s1: generating an initial global path according to the map and the input starting point and end point information;
s2: extracting curve information and straight-path information between curves in the initial global path according to the curvature and curvature difference of the initial global path;
s3: extracting static obstacle position information in a map, performing collision detection and curvature calculation on an initial global path according to curve information in the initial global path and straight-path information among curves, and extracting curves and straight-paths which do not meet obstacle avoidance and vehicle dynamics requirements;
s4: performing numerical optimization on curves and straight channels which do not meet the obstacle avoidance and vehicle dynamics requirements, and obtaining the path points of the curves and the straight channels after pretreatment; obtaining left and right boundaries of each path point according to the obstacle information in the fixed distance range at two sides of the path point; the fixed distance in the fixed distance range on both sides of the path point refers to the distance from the center of the right lane to the lane line plus a safety buff. Because the global path is typically the right lane centerline. Safety buff represents the safety margin and is a calibration value;
s5: constructing a convex optimization problem by taking smoothness, length and offset distance from an original lane line as cost functions, wherein an optimization variable of the convex optimization problem is coordinates of path points of a curve and a straight road after pretreatment, a specific form of the cost function adopts a form of FemPosssmother algorithm, meanwhile, the left and right boundaries of each path point obtained in the step S4 are applied in a form of hard constraint, and the convex optimization problem is solved by a QP algorithm to obtain the curve and the straight road after secondary optimization;
s6: matching and splicing the curve and the straight channel after the secondary optimization with the initial global path to form an optimized global path.
The embodiment firstly plans an initial global path from a starting point to an end point; then extracting static obstacle position information in the map, and adopting a numerical optimization-based method to carry out post-processing on the searched initial global path to generate a smooth pre-processed global path which accords with vehicle dynamics and collision safety; and finally, constructing a convex optimization problem by taking smoothness, length and offset distance from an original lane line as cost functions, performing secondary optimization on the preprocessed global path by a QP (quadratic programming) algorithm, and finally outputting the global path meeting the requirements of vehicle dynamics and obstacle avoidance safety. In the embodiment, the numerical optimization and the secondary optimization are performed on the generated initial global path, and in the optimization process, the initial global path is used as a reference line and used for subsequent optimization, so that the initial global path is directly used for subsequent local path planning, the reference line is not required to be smoothed again in real time, and online computing resources are saved. In addition, the vehicle dynamics constraint and the collision safety constraint are considered, the planning difficulty of the local path of the narrow channel is reduced, the passing rate of the planned path is improved, and the fluency and the comfortableness of automatic driving are improved.
Specifically, step S1 of the present embodiment includes:
s101: analyzing the map, extracting a weighted directed graph by taking the road section as a node, and calculating costs of all nodes according to the road attribute; the map is a high-precision map, and the road attribute comprises length, curves and the like.
S102: searching an initial global path connecting a starting point node and an end point node by adopting a searching algorithm; in this embodiment, an a-search algorithm is used, and it should be noted that other search algorithms may also be used. In addition, in this embodiment, a global topology path connecting a start node and an end node is first searched, and then the global topology path is subjected to global path search to obtain an initial global path.
S103: and analyzing the data of the map to obtain a lane reference line in the road section, wherein the lane reference line is given in the form of a discrete point array, each discrete point comprises two-dimensional and above coordinate information, and the id and the coordinate of each global path point in the initial global path are obtained. Wherein, the discrete point array is represented as [ P1, P2, … … Pn ], and the P comprises two-dimensional or more coordinate information. It should be noted that the id and coordinates of each global path point are determined with reference to the lane reference line.
S104: and calculating the curvature and the orientation angle of the global path point, and assigning the curvature and the orientation angle to the global path point array as information.
Further, in step S2, the embodiment assigns the curve information in the initial global path and the straight-path information between the curves to the global path point;
the curve information comprises curve id, curve type, curve in-and-out angle, curve start point id; the straight road information between the curves comprises a straight road id, a straight road orientation angle and a straight road starting and stopping point id;
the global path point information includes the path point id, coordinates, curvature, orientation angle, a straight/curved road identifier, and the id of the straight/curved road where the straight/curved road identifier is used to indicate whether the reference point of the path point is in a straight road or a curved road.
In addition, the present embodiment extracts static obstacle position information in the map including position information of walls, posts, railings, and the like at step S3. And step S3, each global path point in the initial global path is sequentially judged, and if one global path point does not meet the obstacle avoidance and vehicle dynamics requirements, the curve or straight path where the global path point is located does not meet the obstacle avoidance and vehicle dynamics requirements.
In step S3, collision detection is performed by using the global path point coordinates as the coordinates of the center point of the rear axle of the vehicle, using the direction angle of the global path point as the direction angle of the direction of the vehicle, calculating four corner coordinates of the vehicle, calculating the distance between the four corner coordinates and the obstacle point, if the distance between the four corner coordinates and the obstacle point is smaller than a given safety distance, the global path point does not meet the obstacle avoidance safety requirement, and if the distances between the four corner points and the obstacle are larger than the safety distance, the global path point meets the obstacle avoidance safety requirement.
In step S3, whether the global path point meets the vehicle dynamics requirement is determined by the curvature of the global path point, and if the curvature of the global path point is greater than the critical curvature, the global path point meets the vehicle dynamics requirement; if the curvature of the global path point is less than or equal to the critical curvature, the global path point does not meet the vehicle dynamics requirements. The critical curvature is determined based on vehicle dynamics and the minimum turning radius of the vehicle. For example, according to the dynamics of the vehicle, the minimum turning radius is 5m, the curvature threshold is 0.2.
In step S4, different non-convex optimization problems are respectively constructed for curves and straight roads which do not meet the obstacle avoidance and vehicle dynamics requirements, and are solved based on a gradient descent method; wherein,
for a curve, the optimization variables are the radius and the circle center position of the curve arc, and the Cost function comprises the reciprocal cost_curve of the curve arc radius, the distance cost_obs_c between the arc and an obstacle and the offset distance cost_displacement_c between the arc and the lane line;
for straight channels, the optimization variables are straight-line segment equation parameters; the straight line segment equation is ax+by+c=0; wherein a, b and c are constants, and belong to linear equation coefficients. The Cost function comprises a distance cost_obs_s between the straight line segment and the obstacle and an offset distance cos_displacement_s between the path point of the straight line segment and the lane line.
Specifically, assuming that the Cost function of the curve is Cost1, then:
Cost1＝Cost_curve+Cost_obs_c+Cost_deviation_c；
wherein ,w curve r is the radius of an arc, and is the dynamic weight coefficient;
w obs_c for the obstacle avoidance weight coefficient, dci is the distance between the arc and each obstacle point, and n is the number of all the obstacle points;
(x r ，y r ) Is the center coordinates of (x) i ，y i ) Is the coordinates of the ith obstacle point, w is halfVehicle width;
Cost_deviation_c＝w deviation_c l c ，w deviation_c for the weight coefficient close to the original path, l c Is the sum of the distances of the arc from the original path point;
let the Cost function of the straight path be Cost2, then:
Cost2＝Cost_obs_s+Cos_deviation_s；
wherein ,w obs_s dsi is the distance from each obstacle point to a straight line for avoiding the obstacle weight coefficient;
Cost_deviation_s＝w deviation_s l s ，w deviation_s for the weight coefficient close to the original path, l s Is the sum of the distances from the original path point to the straight line segment. Cos_Deviation_s ensures that the offset of the optimized path from the original path cannot be too great.
And the solving result in the step S4 is the optimal variable with the minimum cost for meeting the constraint. The curve solving result is curve arc radius R and circle center position coordinate (x) with minimum cost function meeting safety constraint and curvature constraint r ，y r ). The straight-path result is the least costly straight-line segment equation parameter (a, b, c) that satisfies the collision constraint.
In addition, in step S5, the present embodiment takes the smoothness, the length and the offset distance from the original lane line as a cost function, and the specific form of the cost function is as follows:
Cost3＝Cost_smooth+Cost_length+Cost_deviation；
wherein ,w smooth representing a smoothing term weight coefficient; x is x i 、y i Representing the optimized ith path point coordinate;
w length representing path length termsWeight coefficient of (2);
x i-ref 、y i-ref representing the ith waypoint coordinates to be optimized.
Finally, in step S6, the distances between the start point and the end point of the line segment of the curve and the straight channel after the second optimization and the global path point of the initial global path are calculated, the global path point of the initial global path closest to the start point and the end point of the line segment after the optimization is found, and the corresponding path point in the initial global path is replaced by the optimized line segment, so as to obtain the optimized global path. The curve matching and splicing process is as shown in fig. 2, a curve of an initial global path is obtained, an optimized curve is obtained through the method of the embodiment, the optimized curve approximates an arc, global path points closest to the starting point and the end point of the optimized curve are found, the starting point and the end point of the optimized curve are connected with the global path points closest to the starting point and the end point of the optimized curve through interpolation pretreatment, and finally a smooth path is formed through secondary optimization smoothing process.
In summary, the embodiment of the invention firstly draws an initial global path from a starting point to an end point according to the global path planning rule; then extracting static obstacle position information in a high-precision map, constructing an optimization problem by taking curvature and distance between the static obstacle and the obstacle as cost functions, and carrying out first optimization solution on a planned initial global path based on a gradient descent method to obtain a preprocessed global path; and finally, constructing a convex optimization problem by taking smoothness and length as cost functions, performing secondary optimization on the preprocessed global path through a QP algorithm, and finally outputting the global path meeting the requirements of vehicle dynamics and obstacle avoidance safety so as to improve the passing rate of the automatic driving vehicle in a narrow-channel scene and improve the smoothness and comfort of the automatic driving vehicle.
The above embodiments are merely preferred embodiments for fully explaining the present invention, and the scope of the present invention is not limited thereto. Equivalent substitutions and modifications will occur to those skilled in the art based on the present invention, and are intended to be within the scope of the present invention.
Claims (10)
1. The global path planning method considering dynamics and obstacle avoidance safety constraints is characterized by comprising the following steps:
s1: generating an initial global path according to the map and the input starting point and end point information;
s2: extracting curve information and straight-path information between curves in the initial global path according to the curvature and curvature difference of the initial global path;
s3: extracting static obstacle position information in a map, performing collision detection and curvature calculation on an initial global path according to curve information in the initial global path and straight-path information among curves, and extracting curves and straight-paths which do not meet obstacle avoidance and vehicle dynamics requirements;
s4: performing numerical optimization on curves and straight channels which do not meet the obstacle avoidance and vehicle dynamics requirements, and obtaining the path points of the curves and the straight channels after pretreatment; obtaining left and right boundaries of each path point according to the obstacle information in the fixed distance range at two sides of the path point;
s5: constructing a convex optimization problem by taking smoothness, length and offset distance from an original lane line as cost functions, wherein an optimization variable of the convex optimization problem is coordinates of path points of a curve and a straight road after pretreatment, meanwhile, applying a hard constraint form to left and right boundaries of each path point obtained in the step S4, and solving the convex optimization problem through a QP algorithm to obtain the curve and the straight road after secondary optimization;
s6: matching and splicing the curve and the straight channel after the secondary optimization with the initial global path to form an optimized global path.
2. The global path planning method taking into account dynamics and obstacle avoidance security constraints according to claim 1, wherein step S1 comprises:
s101: analyzing the map, extracting a weighted directed graph by taking the road section as a node, and calculating costs of all nodes according to the road attribute;
s102: searching an initial global path connecting a starting point node and an end point node by adopting a searching algorithm;
s103: analyzing the data of the map to obtain a lane reference line in the road section, wherein the lane reference line is given in the form of a discrete point array, and each discrete point comprises two-dimensional and above coordinate information to obtain the id and the coordinate of each global path point in the initial global path;
s104: and calculating the curvature and the orientation angle of the global path point, and assigning the curvature and the orientation angle to the global path point array as information.
3. The global path planning method considering dynamics and obstacle avoidance security constraints according to claim 2, characterized in that in step S2, curve information in the initial global path and straight-path information between curves are assigned to global path points;
the curve information comprises curve id, curve type, curve in-and-out angle, curve start point id; the straight road information between the curves comprises a straight road id, a straight road orientation angle and a straight road starting and stopping point id;
the global path point information includes the path point id, coordinates, curvature, orientation angle, a straight/curved road identifier, and the id of the straight/curved road where the straight/curved road identifier is used to indicate whether the reference point of the path point is in a straight road or a curved road.
4. The global path planning method considering dynamics and obstacle avoidance security constraints according to claim 3, wherein in step S3, each global path point in the initial global path is sequentially determined, and if a certain global path point does not meet the obstacle avoidance and vehicle dynamics requirements, the curve or straight path where the global path point is located does not meet the obstacle avoidance and vehicle dynamics requirements.
5. The global path planning method considering dynamics and obstacle avoidance security constraints according to claim 4, wherein in step S3, collision detection is performed by using global path point coordinates as rear axle center point coordinates of the own vehicle, using direction angles of the global path points as direction angles of the own vehicle, calculating four corner coordinates of the own vehicle, calculating distances between the four corner coordinates and obstacle points, if the distance between the four corner coordinates and the obstacle points is smaller than a given safety distance, the global path point does not meet the obstacle avoidance security requirement, and if the distance between the four corner points and the obstacle is greater than the safety distance, the global path point meets the obstacle avoidance security requirement.
6. The global path planning method considering dynamics and obstacle avoidance security constraints according to claim 5, wherein in step S3, whether the global path point meets the vehicle dynamics requirement is determined by the curvature of the global path point, and if the curvature of the global path point is greater than a critical curvature, the global path point meets the vehicle dynamics requirement; if the curvature of the global path point is less than or equal to the critical curvature, the global path point does not meet the vehicle dynamics requirements.
7. The global path planning method considering dynamics and obstacle avoidance safety constraints according to claim 6, wherein in step S4, different non-convex optimization problems are respectively constructed for curves and straight roads which do not meet the obstacle avoidance and vehicle dynamics requirements, and are solved based on a gradient descent method; wherein,
for a curve, the optimization variables are the radius and the circle center position of the curve arc, and the Cost function comprises the reciprocal cost_curve of the curve arc radius, the distance cost_obs_c between the arc and an obstacle and the offset distance cost_displacement_c between the arc and the lane line;
for a straight road, the optimization variable is a straight line segment equation parameter, and the Cost function comprises a distance cost_obs_s between a straight line segment and an obstacle and an offset distance cos_displacement_s between a straight line segment path point and a lane line.
8. The global path planning method considering dynamics and obstacle avoidance safety constraints according to claim 7, wherein assuming that the Cost function of the curve is Cost1, then:
Cost1＝Cost_curve+Cost_obs_c+Cost_deviation_c；
wherein ,w curve r is the radius of an arc, and is the dynamic weight coefficient;
w obs_c for the obstacle avoidance weight coefficient, dci is the distance between the arc and each obstacle point, and n is the number of all the obstacle points;
(x r ，y r ) Is the center coordinates of (x) i ，y i ) The coordinates of the ith obstacle point are the half vehicle width w;
Cost_deviation_c＝w deviation_c l c ，w deviation_c for the weight coefficient close to the original path, l c Is the sum of the distances of the arc from the original path point;
let the Cost function of the straight path be Cost2, then:
Cost2＝Cost_obs_s+Cos_deviation_s；
wherein ,w obs_s dsi is the distance from each obstacle point to a straight line for avoiding the obstacle weight coefficient;
Cost_deviation_s＝w deviation_s l s ，w deviation_ s is a weight coefficient close to the original path, l s Is the sum of the distances from the original path point to the straight line segment.
9. The global path planning method taking into account dynamics and obstacle avoidance security constraints according to claim 1, characterized in that in step S5, the cost function is in the form of smoothness, length and offset distance from the original lane line, and is specifically as follows:
Cost3＝Cost_smooth+Cost_length+Cost_deviation；
wherein ,w smooth representing a smoothing term weight coefficient; x is x i 、y i Representing the optimized ith path point coordinate;
w length a weight coefficient representing a path length term;
x i-ref 、y i-ref representing the ith waypoint coordinates to be optimized.
10. The global path planning method considering dynamics and obstacle avoidance security constraints according to claim 1, wherein in step S6, distances between the start point and the end point of the line segment of the curve and the straight line after the second optimization and the global path point of the initial global path are calculated, global path points of the initial global path closest to the start point and the end point of the line segment after the optimization are found, and corresponding path points in the initial global path are replaced by the line segment after the optimization, so as to obtain the optimized global path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310808867.9A CN116839614A (en) | 2023-07-03 | 2023-07-03 | Global path planning method considering dynamics and obstacle avoidance safety constraint |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310808867.9A CN116839614A (en) | 2023-07-03 | 2023-07-03 | Global path planning method considering dynamics and obstacle avoidance safety constraint |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116839614A true CN116839614A (en) | 2023-10-03 |
Family
ID=88166509
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310808867.9A Pending CN116839614A (en) | 2023-07-03 | 2023-07-03 | Global path planning method considering dynamics and obstacle avoidance safety constraint |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116839614A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117631670A (en) * | 2023-12-01 | 2024-03-01 | 陕西明泰电子科技发展有限公司 | Robot obstacle avoidance path optimization method and system in complex environment |
-
2023
- 2023-07-03 CN CN202310808867.9A patent/CN116839614A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117631670A (en) * | 2023-12-01 | 2024-03-01 | 陕西明泰电子科技发展有限公司 | Robot obstacle avoidance path optimization method and system in complex environment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Hang et al. | Cooperative decision making of connected automated vehicles at multi-lane merging zone: A coalitional game approach | |
| CN111426330B (en) | Path generation method and device, unmanned transportation system and storage medium | |
| Hang et al. | Decision making of connected automated vehicles at an unsignalized roundabout considering personalized driving behaviours | |
| CN111830979B (en) | Track optimization method and device | |
| Morsali et al. | Spatio-temporal planning in multi-vehicle scenarios for autonomous vehicle using support vector machines | |
| CN114610016B (en) | Intelligent vehicle collision avoidance path planning method based on obstacle dynamic virtual expansion | |
| Berntorp et al. | Motion planning of autonomous road vehicles by particle filtering | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| CN113682312B (en) | Autonomous channel switching method and system integrating deep reinforcement learning | |
| Chen et al. | Foad: Fast optimization-based autonomous driving motion planner | |
| Kim et al. | Obstacle avoidance path planning based on output constrained model predictive control | |
| CN112193244A (en) | Automatic driving vehicle motion planning method based on linear constraint | |
| Artunedo et al. | Motion planning approach considering localization uncertainty | |
| CN116839614A (en) | Global path planning method considering dynamics and obstacle avoidance safety constraint | |
| CN115752501B (en) | Navigation path recommending method, device, equipment and storage medium | |
| Ortega et al. | Overtaking maneuver scenario building for autonomous vehicles with PreScan software | |
| CN113361152A (en) | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus | |
| CN113419534B (en) | Bezier curve-based steering road section path planning method | |
| Zhang et al. | Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles | |
| Fu et al. | Model predictive trajectory optimization and tracking in highly constrained environments | |
| Kim et al. | A hierarchical motion planning framework for autonomous driving in structured highway environments | |
| Vailland et al. | Cubic bézier local path planner for non-holonomic feasible and comfortable path generation | |
| CN116674529A (en) | Parking path planning and parking method for unstructured scene automatic driving vehicle | |
| Dai et al. | Human-like lane-changing trajectory planning algorithm for human-machine conflict mitigation | |
| Liu et al. | Human-like trajectory planning for autonomous vehicles based on spatiotemporal geometric transformation |
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 |