CN117093004A - Automatic driving vehicle track optimization method based on segmented Bezier curve - Google Patents
Automatic driving vehicle track optimization method based on segmented Bezier curve Download PDFInfo
- Publication number
- CN117093004A CN117093004A CN202311199421.7A CN202311199421A CN117093004A CN 117093004 A CN117093004 A CN 117093004A CN 202311199421 A CN202311199421 A CN 202311199421A CN 117093004 A CN117093004 A CN 117093004A
- Authority
- CN
- China
- Prior art keywords
- track
- vehicle
- point
- coordinate system
- bezier 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 50
- 238000005457 optimization Methods 0.000 title claims abstract description 24
- 230000003068 static effect Effects 0.000 claims abstract description 21
- 230000004888 barrier function Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims abstract description 7
- 238000006243 chemical reaction Methods 0.000 claims description 21
- 230000006870 function Effects 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 9
- 230000036461 convulsion Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000008447 perception Effects 0.000 claims description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 1
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 description 1
- 108010050014 systemin Proteins 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an automatic driving vehicle track optimization method based on a segmented Bezier curve, which comprises the following steps: acquiring the position and state of a vehicle of a self-vehicle, decision coarse track, dynamic and static barrier information and lane central line information; converting the position and state of the vehicle, the rough decision track and the dynamic and static obstacle information from a Cartesian coordinate system to a Frenet coordinate system, and filling the information into a grid map; taking the converted current vehicle position as a planning starting point, and constructing a segmented space-time three-dimensional convex space based on the grid map filled with the dynamic and static barrier information and the decision coarse track; and (3) performing track optimization by adopting a segmented Bezier curve, converting a cost function and constraint conditions into the form of each matrix in the mathematical expression of the quasi-quadratic programming problem, and performing optimal track solving. The invention constrains the high-order quantity of the track by means of the convex hull characteristic and the convex graph characteristic of the Bezier curve, and ensures the safety of the track by constraining the control points in the convex space.
Description
Technical Field
The invention belongs to the technical field of automatic driving vehicle track planning, and particularly relates to an automatic driving vehicle track optimization method based on a segmented Bezier curve.
Background
In recent years, automatic driving is always a research hotspot of university researchers and enterprise research engineers, and the technical architecture of the automatic driving is mainly divided into four parts of perception, decision, planning and control, wherein the task of the planning algorithm part is to find an optimal track which follows the decision layer result, has no collision and accords with the kinematic constraint of the vehicle for the vehicle, and whether the planned track is safe, comfortable and smooth enough or not directly influences the driving safety and the passenger comfort of the vehicle.
The existing optimization-based track planning algorithm is mostly based on a penta-order polynomial to fit the transverse and longitudinal tracks of the vehicle, however, the expression capacity of the penta-order polynomial is limited, and the optimization-based track planning algorithm is not suitable for the problems of complex-configuration space-time obstacle and dynamic constraint under space-time joint planning. Furthermore, in current research on polynomial trajectories, the constraint is only checked on a limited number of sampling points, and this approach cannot detect collisions between sample points, and thus cannot guarantee safety and feasibility.
Disclosure of Invention
In view of the above-mentioned shortcomings of the prior art, an object of the present invention is to provide an automatic driving vehicle track optimization method based on a segmented bezier curve, which can conveniently constrain the higher-order quantity of the track and ensure the safety of the track by constraining control points in a convex space by means of the convex hull characteristic and the convex graph characteristic of the bezier curve.
In order to achieve the above purpose, the invention adopts the following technical scheme:
the invention discloses an automatic driving vehicle track optimization method based on a segmented Bezier curve, which comprises the following steps:
1) Acquiring the position and state of a vehicle of a self-vehicle, decision coarse track, dynamic and static barrier information and lane central line information;
2) Converting the position and state of the vehicle, the rough decision track and the dynamic and static obstacle information obtained in the step 1) from a Cartesian coordinate system to a Frenet coordinate system, and filling the information into a grid map;
3) Taking the converted current vehicle position as a planning starting point, and constructing a segmented space-time three-dimensional convex space based on the grid map filled with the dynamic and static barrier information and the decision coarse track;
4) Based on the space-time three-dimensional convex space information constructed in the step 3) and the decision coarse track information obtained in the step 1), track optimization is performed by adopting a segmented Bezier curve, a cost function and constraint conditions are converted into the form of each matrix in the mathematical expression of the quasi-quadratic programming problem, and a solver library function is called to perform optimal track solving.
Further, the vehicle position and the state in the step 1) include: vehicle position, lateral longitudinal speed, lateral longitudinal acceleration, vehicle yaw angle, vehicle steering angle, and vehicle parameters in a Cartesian coordinate system; the decision rough track is a decision track of a vehicle for a period of time in the future, which is output by an automatic driving vehicle decision algorithm, and consists of a series of vehicle discrete state points, wherein each vehicle discrete state point comprises a vehicle position and a state of a corresponding time stamp; the dynamic and static obstacle information is surrounding obstacle information perceived by an automatic driving vehicle, wherein the static obstacle is an obstacle (such as a roadside stationary vehicle) with unchanged state in a planning period, and the dynamic obstacle is an obstacle with changed state along with time in the planning period (such as a vehicle and a pedestrian which continuously move around the vehicle); the lane center line information is center line information of a current driving lane and a nearby lane of the vehicle obtained by the automatic driving vehicle through a high-precision map or perception, and the storage mode includes but is not limited to: a series of lane centerline discrete points, a lane centerline functional expression fitted with spline curves.
Further, the resolution of the grid map in the step 2) is set to 0.2m.
Further, the conversion from the cartesian coordinate system to the Frenet coordinate system in the step 2) is divided into coordinate point position conversion and motion state conversion; when coordinate point position conversion is carried out, the point to be converted P is projected to a reference line (namely a lane central line), and when the longitudinal coordinate s of the point to be converted P under the Frenet coordinate system is determined, the projection point M= (x(s) matched with the point to be converted P is searched on the reference line * ),y(s * ))，s * For the corresponding longitudinal coordinates of the projection points, the length of the line segment PM is shortest, namely:
wherein D(s) is the length of the segment PM:
the eligible projection point M can be perpendicular to PM in its tangential direction, with PM length of |L * I, the projection point M is located at the reference trajectory s=s * At this point, the coordinate value of the point to be converted P in the Frenet coordinate system can be determined as (s * ,l * ) The method comprises the steps of carrying out a first treatment on the surface of the In calculating s * When the initial solution s with shorter distance is obtained by using a dichotomy init In the initial solution s init Is iteratively solved using newton's method such that an exact solution of D'(s) =0 is s=s * The iterative formula is as follows:
in the method, in the process of the invention,longitudinal coordinates for each iteration;
usingFor s init Assignment and repeat countingCalculating until +.>When the value is converged, the convergence value is recorded as s * The method comprises the steps of carrying out a first treatment on the surface of the In calculating l * When the solution formula is as follows:
where l is the transverse coordinate in Frenet coordinate system, (x) x ,y x ) For the coordinates of the point to be converted in a Cartesian coordinate system, (x) r ,y r ) Is the coordinate of the projection point in a Cartesian coordinate system, theta r The included angle between the projection point vector and the X axis of the Cartesian coordinate system;
the above process completes the conversion of the coordinate points from the cartesian coordinate system to the Frenet coordinate system, and for vehicles and dynamic obstacles, the conversion of the motion states (speed, acceleration, etc.) is required, and the conversion from the cartesian coordinate system to the Frenet coordinate system is performed according to the following formula:
wherein s is,0,1,2 order derivatives of the transformed longitudinal coordinates with respect to time, l' are 0,1,2 order derivatives of the transformed transverse coordinates with respect to the longitudinal coordinates, v x For the speed of the vehicle in Cartesian coordinates, θ x K is the included angle between the vector of the point to be converted and the X axis of the Cartesian coordinate system r 、k r ' 0,1 order derivatives of curvature of the reference line at the projection point with respect to time, a respectively x For acceleration of the vehicle in Cartesian coordinates, k x Is the steering curvature of the current vehicle.
Further, the method for constructing the segmented space-time three-dimensional convex space in the step 3) comprises the following steps:
31 Acquiring all state points contained in the decision rough track, taking values of a planning starting point and corresponding s, l and t of a first state point in the decision rough track as upper and lower boundaries of a first convex space to construct the first convex space, wherein s is a longitudinal coordinate under a Frenet coordinate system, l is a transverse coordinate under the Frenet coordinate system, and t is a corresponding time stamp;
32 Performing grid coordinate check and collision check on the first convex space, if the grid coordinates exceed the set grid map size and the collision check passes, performing step 33); if the grid coordinates exceed the set grid map size or the collision check is not passed, the first convex space is constructed to fail, and the next planning cycle is waited;
33 Expanding the constructed first convex space, which comprises the following specific steps: sequentially expanding the upper and lower limits of the S, L, T axis of the first convex space in a fixed step length, overlapping with the boundary of the dynamic and static obstacle to stop, and taking the value of the upper and lower limits of the S, L, T axis of the first convex space as the final upper and lower limits of the S, L, T axis of the first convex space when stopping expansion;
34 Acquiring the state point of the last decision coarse track contained in the first convex space after expansion, updating the upper limit of the first convex space into a S, L, T value corresponding to the state point of the last decision coarse track contained in the first convex space, and constructing a next convex space by the state point of the last decision coarse track contained in the first convex space and the next state point;
35 Repeating steps 32) -34) sequentially for the next convex volume constructed in step 34) until all decision coarse track state points are contained in the constructed decision coarse track.
Further, the optimized trajectory constructed by constructing n segments of bezier curves in the step 4) is expressed as:
where σ ε { s, l } represents the dimension of the Bezier curve,an ith control point representing a jth segment of the bezier curve; t is t 0 ,t 1 ,...,t n For the time dimension coordinates of the start point and the end point of each segment of the bezier curve,is Bernstein polynomial; alpha j Asynchronous time-domain scaling factor for the jth segment Bezier curve, in order to define the Bezier curve for each segment trajectory at a fixed interval [0,1 ]]And (3) inner part.
Further, in the step 4), the integral of the square of the jerk value of the track with respect to time is adopted as the optimized cost function, and the mathematical expression is as follows:
wherein J is j As a cost function of the jth segment of the bezier curve,jerk values, w, longitudinal and transverse to the track, respectively s And w l The control weights are longitudinal and transverse respectively.
Further, the constraints in step 4) are categorized into three categories: terminal constraints, continuity constraints, and collision constraints, and dynamic constraints; terminal constraint means that the generated track should meet the starting point state and the end point state of the track and the initial point state of the decision coarse trackStatus->Same, wherein σ ε { s, l }, -A }, B }>K-th derivative of position coordinates at time t, mathematical expression thereofThe formula is as follows:
the continuity constraint means that the k-order derivative of the resulting optimized trajectory is continuous at the junction of each adjacent convex space; the continuity constraint between the j-th convex space and the j+1th convex space is expressed as:
the collision constraint and the dynamic constraint refer to the constraint related to the collision boundary and dynamics of the vehicle in the algorithm, the constraint degrees of the constraints of different algorithms are different, the position, the speed and the acceleration of the track need to be constrained, namely the k=0, 1 and 2-order derivatives of the Bezier curve, and the constraint is expressed as the following linear inequality constraint form:
in the method, in the process of the invention,for the lower and upper bounds of the inequality constraint, k represents the order of the derivative of the required constraint quantity.
Further, in the step 4), the track optimization problem is converted into a general quadratic programming form of an OOQP solver for solving:
Ax＝b,d≤Cx≤f,l≤x≤u,
wherein Q is a symmetrical positive half-definite n×An n matrix; x epsilon R n Is a variable vector; a and C respectively represent a dimension m a X n and m c X n, b, d, f, l, u are vectors of appropriate dimensions.
The invention has the beneficial effects that:
1. the scheme of track optimization by using the multi-section curves has better adaptability to complex traffic conditions, reduces the solving difficulty of the track optimization problem to a certain extent, and reduces the calculation complexity of a planning algorithm.
2. The trajectory optimization is performed based on the Bezier curve, so that the convex graph characteristics of the Bezier curve are benefited, the high-order quantity of the trajectory can be conveniently represented and restrained, and compared with the traditional trajectory optimization method based on the quintic polynomial, the trajectory optimization method based on the five-degree polynomial has stronger expressive power on space-time barriers and dynamic constraints with complex configurations, has better robustness under complex road working conditions, and is more practical.
3. The method is based on the Bezier curve to optimize the track, and is beneficial to the convex hull characteristic of the Bezier curve, and the method can ensure that the generated track is fixed in the convex space only by restricting the control points in the given convex space when the track is optimized, so that the safety of the generated track is ensured.
4. The method for constructing the segmented space-time three-dimensional convex space has clear logic and simple calculation, greatly improves the real-time performance of the algorithm, and can better exert the advantages of the track optimization method based on the segmented Bezier curve compared with the traditional convex space construction method.
Drawings
FIG. 1 is a flow chart of a track optimization method based on a segmented Bezier curve in the invention;
FIG. 2 is a flow chart of a method for constructing a segmented spatio-temporal three-dimensional convex space in the present invention.
Detailed Description
The invention will be further described with reference to examples and drawings, to which reference is made, but which are not intended to limit the scope of the invention.
Referring to fig. 1-2, the method for optimizing the track of the automatic driving vehicle based on the segmented bezier curve comprises the following steps:
1) Acquiring the position and state of a vehicle of a self-vehicle, decision coarse track, dynamic and static barrier information and lane central line information;
the position and state of the own vehicle include: vehicle position, lateral longitudinal speed, lateral longitudinal acceleration, vehicle yaw angle, vehicle steering angle, and vehicle parameters in a Cartesian coordinate system; the decision rough track is a decision track of a vehicle for a period of time in the future, which is output by an automatic driving vehicle decision algorithm, and consists of a series of vehicle discrete state points, wherein each vehicle discrete state point comprises a vehicle position and a state of a corresponding time stamp; the dynamic and static obstacle information is surrounding obstacle information perceived by an automatic driving vehicle, wherein the static obstacle is an obstacle (such as a roadside stationary vehicle) with unchanged state in a planning period, and the dynamic obstacle is an obstacle with changed state along with time in the planning period (such as a vehicle and a pedestrian which continuously move around the vehicle); the lane center line information is center line information of a current driving lane and a nearby lane of the vehicle obtained by the automatic driving vehicle through a high-precision map or perception, and the storage mode includes but is not limited to: a series of lane centerline discrete points, a lane centerline functional expression fitted with spline curves.
2) Converting the position and state of the vehicle, the rough decision track and the dynamic and static obstacle information obtained in the step 1) from a Cartesian coordinate system to a Frenet coordinate system, and filling the information into a grid map;
wherein the resolution of the grid map is set to 0.2m.
The conversion from the Cartesian coordinate system to the Frenet coordinate system is divided into coordinate point position conversion and motion state conversion; when coordinate point position conversion is carried out, the point P to be converted is projected to a reference line (namely a lane central line), and the point P to be converted is determined under the Frenet coordinate systemIn the case of the longitudinal coordinate s, a projection point m= (x(s) matching the point P to be converted is found on the reference line * ),y(s * ))，s * For the corresponding longitudinal coordinates of the projection points, the length of the line segment PM is shortest, namely:
wherein D(s) is the length of the segment PM:
the eligible projection point M can be perpendicular to PM in its tangential direction, with PM length of |L * I, the projection point M is located at the reference trajectory s=s * At this point, the coordinate value of the point to be converted P in the Frenet coordinate system can be determined as (s * ,l * ) The method comprises the steps of carrying out a first treatment on the surface of the In calculating s * When the initial solution s with shorter distance is obtained by using a dichotomy init In the initial solution s init Is iteratively solved using newton's method such that an exact solution of D'(s) =0 is s=s * The iterative formula is as follows:
in the method, in the process of the invention,longitudinal coordinates for each iteration;
usingFor s init Assigning and repeating the calculation until +.>When the value is converged, the convergence value is recorded as s * The method comprises the steps of carrying out a first treatment on the surface of the In calculating l * When the solution formula is as follows:
where l is the transverse coordinate in Frenet coordinate system, (x) x ,y x ) For the coordinates of the point to be converted in a Cartesian coordinate system, (x) r ,y r ) Is the coordinate of the projection point in a Cartesian coordinate system, theta r The included angle between the projection point vector and the X axis of the Cartesian coordinate system;
the above process completes the conversion of the coordinate points from the cartesian coordinate system to the Frenet coordinate system, and for vehicles and dynamic obstacles, the conversion of the motion states (speed, acceleration, etc.) is required, and the conversion from the cartesian coordinate system to the Frenet coordinate system is performed according to the following formula:
wherein s is,0,1,2 order derivatives of the transformed longitudinal coordinates with respect to time, l' are 0,1,2 order derivatives of the transformed transverse coordinates with respect to the longitudinal coordinates, v x For the speed of the vehicle in Cartesian coordinates, θ x K is the included angle between the vector of the point to be converted and the X axis of the Cartesian coordinate system r 、k r ' 0,1 order derivatives of curvature of the reference line at the projection point with respect to time, a respectively x For acceleration of the vehicle in Cartesian coordinates, k x Is the steering curvature of the current vehicle.
3) Taking the converted current vehicle position as a planning starting point, and constructing a segmented space-time three-dimensional convex space based on the grid map filled with the dynamic and static barrier information and the decision coarse track;
the construction method of the segmented space-time three-dimensional convex space comprises the following steps:
31 Acquiring all state points contained in the decision rough track, taking values of a planning starting point and corresponding s, l and t of a first state point in the decision rough track as upper and lower boundaries of a first convex space to construct the first convex space, wherein s is a longitudinal coordinate under a Frenet coordinate system, l is a transverse coordinate under the Frenet coordinate system, and t is a corresponding time stamp;
32 Performing grid coordinate check and collision check on the first convex space, if the grid coordinates exceed the set grid map size and the collision check passes, performing step 33); if the grid coordinates exceed the set grid map size or the collision check is not passed, the first convex space is constructed to fail, and the next planning cycle is waited;
33 Expanding the constructed first convex space, which comprises the following specific steps: sequentially expanding the upper and lower limits of the S, L, T axis of the first convex space in a fixed step length, overlapping with the boundary of the dynamic and static obstacle to stop, and taking the value of the upper and lower limits of the S, L, T axis of the first convex space as the final upper and lower limits of the S, L, T axis of the first convex space when stopping expansion;
34 Acquiring the state point of the last decision coarse track contained in the first convex space after expansion, updating the upper limit of the first convex space into a S, L, T value corresponding to the state point of the last decision coarse track contained in the first convex space, and constructing a next convex space by the state point of the last decision coarse track contained in the first convex space and the next state point;
35 Repeating steps 32) -34) sequentially for the next convex volume constructed in step 34) until all decision coarse track state points are contained in the constructed decision coarse track.
4) Based on the space-time three-dimensional convex space information constructed in the step 3) and the decision coarse track information obtained in the step 1), track optimization is carried out by adopting a segmented Bezier curve, a cost function and constraint conditions are converted into the form of each matrix in a quasi-quadratic programming problem mathematical expression, and a solver library function is called to carry out optimal track solving;
wherein, the optimized track formed by constructing n segments of Bezier curves in the step 4) is expressed as follows:
where σ ε { s, l } represents the dimension of the Bezier curve,an ith control point representing a jth segment of the bezier curve; t is t 0 ,t 1 ,...,t n For the time dimension coordinates of the start point and the end point of each segment of the bezier curve,is Bernstein polynomial; alpha j Asynchronous time-domain scaling factor for the jth segment Bezier curve, in order to define the Bezier curve for each segment trajectory at a fixed interval [0,1 ]]And (3) inner part.
The square of the jerk value of the trace is used as the cost function of optimization, and the mathematical expression is:
wherein J is j As a cost function of the jth segment of the bezier curve,jerk values, w, longitudinal and transverse to the track, respectively s And w l The control weights are longitudinal and transverse respectively.
Constraints fall into three categories: terminal constraints, continuity constraints, and collision constraints, and dynamic constraints; terminal constraint means that the generated track should meet the starting point state and the end point state of the track and the initial point state of the decision coarse trackStatus->Same, wherein σ ε { s, l }, -A }, B }>The k-th derivative of the position coordinate at the time t is expressed in the following mathematical expression:
the continuity constraint means that the k-order derivative of the resulting optimized trajectory is continuous at the junction of each adjacent convex space; the continuity constraint between the j-th convex space and the j+1th convex space is expressed as:
the collision constraint and the dynamic constraint refer to the constraint related to the collision boundary and dynamics of the vehicle in the algorithm, the constraint degrees of the constraints of different algorithms are different, the position, the speed and the acceleration of the track need to be constrained, namely the k=0, 1 and 2-order derivatives of the Bezier curve, and the constraint is expressed as the following linear inequality constraint form:
in the method, in the process of the invention,for the lower and upper bounds of the inequality constraint, k represents the order of the derivative of the required constraint quantity.
Converting the track optimization problem into a general quadratic programming form of an OOQP solver for solving:
Ax＝b,d≤Cx≤f,l≤x≤u,
wherein Q is a symmetrical positive semi-definite n multiplied by n matrix; x epsilon R n Is a variable vector; a and C respectively represent a dimension m a X n and m c X n, b, d, f, l, u are vectors of appropriate dimensions.
The present invention has been described in terms of the preferred embodiments thereof, and it should be understood by those skilled in the art that various modifications can be made without departing from the principles of the invention, and such modifications should also be considered as being within the scope of the invention.
Claims (9)
1. An automatic driving vehicle track optimization method based on a segmented Bezier curve is characterized by comprising the following steps:
1) Acquiring the position and state of a vehicle of a self-vehicle, decision coarse track, dynamic and static barrier information and lane central line information;
2) Converting the position and state of the vehicle, the rough decision track and the dynamic and static obstacle information obtained in the step 1) from a Cartesian coordinate system to a Frenet coordinate system, and filling the information into a grid map;
3) Taking the converted current vehicle position as a planning starting point, and constructing a segmented space-time three-dimensional convex space based on the grid map filled with the dynamic and static barrier information and the decision coarse track;
4) Based on the space-time three-dimensional convex space information constructed in the step 3) and the decision coarse track information obtained in the step 1), track optimization is performed by adopting a segmented Bezier curve, a cost function and constraint conditions are converted into the form of each matrix in the mathematical expression of the quasi-quadratic programming problem, and a solver library function is called to perform optimal track solving.
2. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 1, wherein the positions and states of the autonomous vehicle in step 1) include: vehicle position, lateral longitudinal speed, lateral longitudinal acceleration, vehicle yaw angle, vehicle steering angle, and vehicle parameters in a Cartesian coordinate system; the decision rough track is a decision track of a vehicle for a period of time in the future, which is output by an automatic driving vehicle decision algorithm, and consists of a series of vehicle discrete state points, wherein each vehicle discrete state point comprises a vehicle position and a state of a corresponding time stamp; the dynamic and static obstacle information is surrounding obstacle information perceived by an automatic driving vehicle, wherein the static obstacle is an obstacle with the state unchanged in a planning period, and the dynamic obstacle is an obstacle with the state changed with time in the planning period; the lane center line information is center line information of a current driving lane and a nearby lane of the vehicle obtained by the automatic driving vehicle through a high-precision map or perception.
3. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 1, wherein the resolution of the grid map in step 2) is set to 0.2m.
4. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 1, wherein the conversion from the cartesian coordinate system to the Frenet coordinate system in step 2) is divided into coordinate point position conversion and motion state conversion; when coordinate point position conversion is performed, a point to be converted P is projected to a reference line, and when the longitudinal coordinate s of the point to be converted P in the Frenet coordinate system is determined, a projection point M= (x(s) matching the point to be converted P is found on the reference line * ),y(s * ))，s * For the corresponding longitudinal coordinates of the projection points, the length of the line segment PM is shortest, namely:
wherein D(s) is the length of the segment PM:
the eligible projection point M can be perpendicular to PM in its tangential direction, with PM length of |L * I, the projection point M is located at the reference trajectory s=s * At this point, the coordinate value of the point to be converted P in the Frenet coordinate system can be determined as (s * ,l * ) The method comprises the steps of carrying out a first treatment on the surface of the In calculating s * When the initial solution s with shorter distance is obtained by using a dichotomy init In the initial solution s init Is iteratively solved using newton's method such that an exact solution of D'(s) =0 is s=s * The iterative formula is as follows:
in the method, in the process of the invention,longitudinal coordinates for each iteration;
usingFor s init Assigning and repeating the calculation until +.>When the value is converged, the convergence value is recorded as s * The method comprises the steps of carrying out a first treatment on the surface of the In calculating l * When the solution formula is as follows:
where l is the transverse coordinate in Frenet coordinate system, (x) x ,y x ) For the coordinates of the point to be converted in a Cartesian coordinate system, (x) r ,y r ) Is the coordinate of the projection point in a Cartesian coordinate system, theta r The included angle between the projection point vector and the X axis of the Cartesian coordinate system;
the above process completes the conversion of the coordinate points from the Cartesian coordinate system to the Frenet coordinate system, and for vehicles and dynamic obstacles, the conversion of the motion state is required to be carried out, and the conversion from the Cartesian coordinate system to the Frenet coordinate system is carried out according to the following formula:
wherein s is,0,1,2 order derivatives of the transformed longitudinal coordinates with respect to time, l' are 0,1,2 order derivatives of the transformed transverse coordinates with respect to the longitudinal coordinates, v x For the speed of the vehicle in Cartesian coordinates, θ x K is the included angle between the vector of the point to be converted and the X axis of the Cartesian coordinate system r 、k r ' 0,1 order derivatives of curvature of the reference line at the projection point with respect to time, a respectively x For acceleration of the vehicle in Cartesian coordinates, k x Is the steering curvature of the current vehicle.
5. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 4, wherein the method for constructing the segmented spatio-temporal three-dimensional convex space in step 3) comprises the steps of:
31 Acquiring all state points contained in the decision rough track, taking values of a planning starting point and corresponding s, l and t of a first state point in the decision rough track as upper and lower boundaries of a first convex space to construct the first convex space, wherein s is a longitudinal coordinate under a Frenet coordinate system, l is a transverse coordinate under the Frenet coordinate system, and t is a corresponding time stamp;
32 Performing grid coordinate check and collision check on the first convex space, if the grid coordinates exceed the set grid map size and the collision check passes, performing step 33); if the grid coordinates exceed the set grid map size or the collision check is not passed, the first convex space is constructed to fail, and the next planning cycle is waited;
33 Expanding the constructed first convex space, which comprises the following specific steps: sequentially expanding the upper and lower limits of the S, L, T axis of the first convex space in a fixed step length, overlapping with the boundary of the dynamic and static obstacle to stop, and taking the value of the upper and lower limits of the S, L, T axis of the first convex space as the final upper and lower limits of the S, L, T axis of the first convex space when stopping expansion;
34 Acquiring the state point of the last decision coarse track contained in the first convex space after expansion, updating the upper limit of the first convex space into a S, L, T value corresponding to the state point of the last decision coarse track contained in the first convex space, and constructing a next convex space by the state point of the last decision coarse track contained in the first convex space and the next state point;
35 Repeating steps 32) -34) sequentially for the next convex volume constructed in step 34) until all decision coarse track state points are contained in the constructed decision coarse track.
6. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 5, wherein the optimized trajectory constructed by constructing n segments of bezier curves in step 4) is expressed as:
where σ ε { s, l } represents the dimension of the Bezier curve,an ith control point representing a jth segment of the bezier curve; t is t 0 ,t 1 ,...,t n For the time dimension coordinates of the starting point and the ending point of each segment of Bezier curve, +.>Is Bernstein polynomial; alpha j Asynchronous time domain scaling factor for the jth segment of the bezier curve.
7. The method for optimizing the trajectory of an autonomous vehicle based on a piecewise bezier curve according to claim 5, wherein the step 4) uses the integral of the square of the jerk value of the trajectory over time as the optimized cost function, and the mathematical expression is:
wherein J is j As a cost function of the jth segment of the bezier curve,jerk values, w, longitudinal and transverse to the track, respectively s And w l The control weights are longitudinal and transverse respectively.
8. The method for optimizing the trajectory of an autonomous vehicle based on a segmented bezier curve according to claim 5, wherein the constraints in said step 4) are summarized in three categories: terminal constraints, continuity constraints, and collision constraints, and dynamic constraints; terminal constraint means that the generated track should meet the starting point state and the end point state of the track and the initial point state of the decision coarse trackStatus->Same, wherein σ ε { s, l }, -A }, B }>The k-th derivative of the position coordinate at the time t is expressed in the following mathematical expression:
the continuity constraint means that the k-order derivative of the resulting optimized trajectory is continuous at the junction of each adjacent convex space; the continuity constraint between the j-th convex space and the j+1th convex space is expressed as:
the collision constraint and the dynamic constraint refer to the constraint related to the collision boundary and dynamics of the vehicle in the algorithm, the constraint degrees of the constraints of different algorithms are different, the position, the speed and the acceleration of the track need to be constrained, namely the k=0, 1 and 2-order derivatives of the Bezier curve, and the constraint is expressed as the following linear inequality constraint form:
in the method, in the process of the invention,for the lower and upper bounds of the inequality constraint, k represents the order of the derivative of the required constraint quantity.
9. The method for optimizing the trajectory of the automatic driving vehicle based on the segmented bezier curve according to claim 5, wherein the step 4) converts the trajectory optimization problem into a general quadratic programming form of an OOQP solver for solving:
Ax＝b,d≤Cx≤f,l≤x≤u,
wherein Q is a symmetrical positive half-definite n×nA matrix; x epsilon R n Is a variable vector; a and C respectively represent a dimension m a X n and m c X n, b, d, f, l, u are vectors of appropriate dimensions.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311199421.7A CN117093004A (en) | 2023-09-18 | 2023-09-18 | Automatic driving vehicle track optimization method based on segmented Bezier curve |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311199421.7A CN117093004A (en) | 2023-09-18 | 2023-09-18 | Automatic driving vehicle track optimization method based on segmented Bezier curve |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117093004A true CN117093004A (en) | 2023-11-21 |
Family
ID=88776996
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311199421.7A Pending CN117093004A (en) | 2023-09-18 | 2023-09-18 | Automatic driving vehicle track optimization method based on segmented Bezier curve |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117093004A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117885765A (en) * | 2024-03-15 | 2024-04-16 | 中国第一汽车股份有限公司 | Vehicle control method and device and vehicle |
-
2023
- 2023-09-18 CN CN202311199421.7A patent/CN117093004A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117885765A (en) * | 2024-03-15 | 2024-04-16 | 中国第一汽车股份有限公司 | Vehicle control method and device and vehicle |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107063280B (en) | Intelligent vehicle path planning system and method based on control sampling | |
| CN110609547B (en) | Mobile robot planning method based on visual map guidance | |
| CN117093004A (en) | Automatic driving vehicle track optimization method based on segmented Bezier curve | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| Li et al. | Semantic-level maneuver sampling and trajectory planning for on-road autonomous driving in dynamic scenarios | |
| CN114281084A (en) | Intelligent vehicle global path planning method based on improved A-x algorithm | |
| CN109556623B (en) | Iterative path planning algorithm fusing whisker algorithm and tangent graph method | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| CN113124875A (en) | Path navigation method | |
| CN116465427B (en) | Intelligent vehicle lane changing obstacle avoidance path planning method based on space-time risk quantification | |
| Na et al. | Interaction-aware trajectory prediction of surrounding vehicles based on hierarchical framework in highway scenarios | |
| Li | Motion planning for dynamic scenario vehicles in autonomous-driving simulations | |
| Hsu et al. | Design of smooth path based on the conversion between η 3 spline and Bezier curve | |
| Wang et al. | Research on path planning of mobile robot based on improved jump point search algorithm | |
| CN115891982A (en) | Automatic parking path planning method based on quadratic programming and parallel iteration | |
| Chen et al. | A robust trajectory planning method based on historical information for autonomous vehicles | |
| CN114815853A (en) | Path planning method and system considering road surface obstacle characteristics | |
| Wang et al. | A Risk-field Based Motion Planning Method for Multi-vehicle Conflict Scenario | |
| Zhang et al. | A convolution-based grid map reconfiguration method for autonomous driving in highly constrained environments | |
| Gratzer et al. | Flatness-Based Mixed-Integer Obstacle Avoidance MPC for Collision-Safe Automated Urban Driving | |
| Li et al. | An efficient sampling-based hybrid a* algorithm for intelligent vehicles | |
| Zhao et al. | On-road trajectory planning of connected and automated vehicles in complex traffic settings: a hierarchical framework of trajectory refinement | |
| Hua | Research on Automatic Lane Change Path Planning Based on Improved Rapidly-Exploring Random Trees | |
| Zhu et al. | Obstacle-Avoidance Path Planning of Unmanned Vehicles based on Polynomial Optimization | |
| Liu et al. | A spline-lstm for autonomous truck trajectory prediction based on curve feature extraction |
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 |