CN113961002B - Active lane change planning method based on structured road sampling - Google Patents

Abstract

The invention discloses an active lane change planning method based on structured road sampling, which comprises the following steps: step S1: carrying out space sampling treatment on the road of the lane change scene; step S2: carrying out transverse and longitudinal polynomial lane change track generation and solution on the sampling points; step S3: and performing evaluation function calculation on the calculated tracks, and screening the optimal track. The invention builds the vehicle kinematic model, and generates the lane change track under the constraint of the vehicle kinematic model, thereby well solving the problem that the lane change track of the traditional method does not accord with the vehicle motion state; and (3) sampling the state space of the structured road to generate a plurality of groups of lane change tracks, and introducing an evaluation function to obtain the optimal lane change track.

Description

Active lane change planning method based on structured road sampling

Technical Field

The invention relates to a lane change planning method, in particular to an active lane change planning method based on structured road sampling.

Background

According to the invention, the structured road is subjected to state space sampling, polynomial curve generation is carried out on sampling points through vehicle kinematic constraint, an evaluation function is added, scoring calculation is carried out on a plurality of groups of generated lane change tracks, and a group of optimal lane change tracks are finally screened out through different weight settings, so that the safe collision-free lane change running of an auxiliary driving vehicle can be guided, and the riding comfort is improved.

The existing active lane change planning method is that a multi-section geometric curve is generated according to the current vehicle position point and the lane change arrival position point, collision detection is carried out according to the obstacle information identified by the radar and the generated geometric curve, and the vehicle can travel according to the curve lane change when no collision risk exists.

The prior art has the following disadvantages: the planned track of the active lane change in the prior art is calculated according to the traditional geometric method without considering the constraint of the vehicle kinematics, so that the planned track of the active lane change in the prior art is not in optimal line with the running of the vehicle.

The planned track of the active lane change in the prior art does not comprehensively consider factors such as collision risk, travelling comfort, track smoothness and the like, so that the planned track of the active lane change in the prior art is not optimal.

For example, an "automobile lane change collision avoidance control method" disclosed in chinese patent literature, its bulletin number CN105857294B, includes a problem that the optimal trajectory planning cannot be achieved without comprehensively considering factors such as collision risk, driving comfort, trajectory smoothness, and the like.

Disclosure of Invention

The invention provides an active lane change planning method based on structured road sampling, which aims to solve the problem that the optimal active lane change cannot be achieved due to the fact that factors such as collision risk, driving comfort and smooth track performance are not comprehensively considered in the prior art.

In order to achieve the above purpose, the present invention adopts the following technical scheme: an active lane change planning method based on structured road sampling is characterized by comprising the following steps:

step S1: carrying out space sampling treatment on the road of the lane change scene;

step S2: carrying out transverse and longitudinal polynomial lane change track generation and solution on the sampling points;

step S3: and performing evaluation function calculation on the calculated tracks, and screening the optimal track.

The invention builds the vehicle kinematic model, generates the lane change track under the constraint of the vehicle kinematic model, and well solves the problem that the lane change track of the traditional method does not accord with the vehicle motion state.

Preferably, step S1 comprises the steps of: step S11: summing the 5 th-order polynomial coefficients of the adjacent lane lines to obtain a mean value, and obtaining a polynomial equation of the middle lane line; step S12: according to the obtained reference lines, uniformly arranging sampling points at fixed intervals; and carrying out transverse and longitudinal polynomial track generation and solution on the sampling points, carrying out evaluation function calculation on a plurality of calculated tracks, screening out an optimal track, and solving the problem that the track generated by the traditional technical scheme is single and not the optimal solution.

Preferably, the contents of the polynomial equation for the intermediate lane:the method comprises the steps of obtaining track data of complex road conditions accurately through calculation of middle lane lines, performing evaluation function calculation on a plurality of calculated tracks, generating a large amount of scheme data, and comparing to obtain an optimal solution of active road change.

Preferably, step S12 includes the following: the longitudinal distance interval of the sampling points is max [4s ] Vc _urrentSpeed ,Vs _{toppingDistance} ]5, wherein Vc _urrentSpeed For the current speed of the vehicle, 4s.times.Vc _urrentSpeed The driving distance is the driving distance within 4s under the current vehicle speed; vs _{toppingDistance} The maximum braking distance is the current speed per hour; the sampling point transverse distance interval is (load_width-width)/2; the load_width is the road width of the adjacent lane, the vehicle_width is the vehicle width, and different movement track curves are obtained according to different actual vehicle condition road conditions, so that the planning track of the active lane change more accords with the real-time running condition of the vehicle.

Preferably, step S2 comprises the steps of: step S21: constructing a vehicle kinematic model according to the sampling points obtained in the steps; step S22: connecting the track points by adopting a polynomial spiral line according to the sampled track points to generate a track curve; the lane change track generation is mainly used for realizing the active lane change function of the auxiliary driving vehicle, and according to the sampling points obtained in the upper section, polynomial curve equations under the constraint of the vehicle motion model are respectively solved for different sampling points, so that the obtained curve is closer to the optimal route.

Preferably, the content of the vehicle kinematic model is: setting a vehicle attitude vector x= (X, y, θ, k, v), wherein X, y is a two-dimensional plane position, θ is a vehicle orientation, k represents a curvature, v is a vehicle linear velocity, and a scalar magnitude thereof satisfies the following relationship:and data support is provided for subsequent calculation, and the accuracy and the instantaneity of route planning data are ensured.

Preferably, the trajectory curve is defined as a cubic polynomial spiral, the curvature k of the trajectory is a cubic polynomial function of the arc length s, and the parameter vector p= [ p ] is substituted ₀ p ₁ p ₂ p ₃ s _f ]Wherein s is _f The arc length of the curve between boundary constraints is the arc length of the curve, parameters are substituted into actual data, and the accuracy of the data is ensured, so that the planning track of the active lane change is more in line with the real-time running condition of the vehicle.

Preferably, the evaluation function is a linear combination of multiple evaluation function items, the trajectory in step S2 is discretized into n+1 points, and the evaluation function includes the following:

departure lane cost function f _offset :Wherein Δs is the square of the deviation of the discrete point from the corresponding mapped point on the reference line;

smoothing cos function f _smooth :Wherein k is curvature;

comfort cost function f _jeck :Wherein J is jerk;

crash cost function f _collision ：f _collision ＝1/min(||P _i -P _obs I) and (i e 0-n), wherein P _obs The coordinate vector is the coordinate vector of the obstacle, and P is the coordinate vector of the discrete track;

centripetal acceleration cost function f _centripetal ：Wherein a is _i Is centripetal acceleration;

synthesizing cost: f=w _offset *f _offset +w _smooth *f _smooth +w _jeck *f _jeck +w _collision *f _collision +w _centripetal *f _centripetal Wherein w is _offset 、w _smooth 、w _jeck 、w _collision 、w _centripetal The weight coefficient is self-adjusting for each function, and according to the design of the adjustable weight coefficient, the cost function calculation processing of the track is finally completed; and comprehensively processing the calculated data to ensure that the finally obtained lane change track can comprehensively consider factors such as collision risk, travelling comfort, track smoothness and the like, so that the lane change of the system planning is close to the optimal solution.

Therefore, the invention has the following beneficial effects:

the method has the advantages that a vehicle kinematic model is built, a lane changing track is generated under the constraint of the vehicle kinematic model, and the problem that the lane changing track of the traditional method does not accord with the vehicle motion state is well solved;

and (3) sampling the state space of the structured road to generate a plurality of groups of lane change tracks, and introducing an evaluation function to obtain the optimal lane change track.

Drawings

FIG. 1 is a schematic view of a lane pick-up point;

FIG. 2 is a vehicle motion profile;

Detailed Description

The technical scheme of the invention is further specifically described below through examples and with reference to the accompanying drawings.

An active lane change planning method based on structured road sampling mainly comprises the following steps:

1. the method solves the problem that the track of the lane change planning in the traditional technology does not consider the kinematics of the vehicle, builds a vehicle kinematics model, and generates the planned track under the constraint of the model.

2. The method comprises the steps of carrying out space sampling processing on a road of a lane changing scene to generate a plurality of sampling points, carrying out transverse and longitudinal polynomial track generation solving on the sampling points, carrying out evaluation function calculation on the calculated plurality of tracks, screening out an optimal track, and solving the problem that the track generated by the traditional technical scheme is single and not optimal.

The main components are state space sampling, lane change track generation, speed track generation and evaluation function calculation:

state space sampling:

the state space sampling depends on a reference line, the reference line is a reference track of vehicle running, and under the condition of no global planning track of a navigation map, the middle lane line can be obtained by selecting the left or right adjacent lane line according to the state of the steering lamp and combining the selected lane lines, and finally the middle lane line is used as the reference line.

The specific implementation method is as follows:

1. summing the 5 th-order polynomial coefficients of the adjacent lane lines to obtain a mean value to obtain a polynomial equation of the middle lane line:

wherein, leftcoeffs is left lane line coefficient, right tcoeffs is right lane line coefficient;

2. according to the obtained reference lines, uniformly arranging sampling points at fixed intervals;

the specific method comprises the following steps:

1) The longitudinal distance interval of sampling points is max [4s ] Vc _urrentSpeed ,Vs _{toppingDistance} ]/5；

Wherein Vc _urrentSpeed For the current speed of the vehicle, 4s.times.Vc _urrentSpeed The driving distance is the driving distance within 4s under the current vehicle speed; vs _{toppingDistance} The maximum braking distance is the current speed per hour;

the sampling point transverse distance interval is (load_width-width)/2; the load_width is the road width of the adjacent lane, the vehicle_width is the vehicle width, as shown in fig. 1, wherein the round dots are sampling points, the black solid lines on two sides are lane lines, and the middle line is a reference line;

track changing track generation:

the lane change track generation is mainly used for realizing the active lane change function of the auxiliary driving vehicle, and polynomial curve equations under the constraint of the vehicle motion model are respectively solved for different sampling points according to the sampling points obtained in the upper section.

The specific implementation method is as follows:

1) Building a vehicle kinematic model, and setting a vehicle attitude vector X= (X, y, theta, k, v), wherein X, y is the position of a two-dimensional plane, theta is the vehicle orientation, k represents the curvature, v is the vehicle linear speed, and the scalar magnitude satisfies the following:

2) And (3) connecting the sampled track points by adopting a polynomial spiral line to generate a track curve. Defining the trajectory as a cubic polynomial spiral, i.e. the curvature k of the trajectory is a cubic polynomial function of the arc length s, bringing into the parameter vector p= [ p ] ₀ p ₁ p ₂ p ₃ s _f ]Wherein s is _f The arc length of the curve between boundary constraints is derived as follows to obtain a polynomial equation.

k(s)＝a+bs+cs ² +ds ³

k(s)＝a(p)+b(p)s+c(p)s ² +d(p)s ³

k(0)＝p ₀

k(s _f /3)＝p ₁

k(2s _f /3)＝p ₂

k(s _f )＝p ₃

a(p)＝p ₀

b(p)＝(-11p ₀ +18p ₁ -9p ₂ +2p ₃ )/2s _f

c(p)＝9(2p ₀ -5p ₁ +4p ₂ -p ₃ )/2s _f ²

d(p)＝9(-p ₀ +3p ₁ -3p ₂ +p ₃ )/2s _f ³

3) In the above, p ₀ It is known that only [ p ] needs to be solved ₁ p ₂ p ₃ s _f ]The polynomial equation is derived from the vehicle kinematic model through the following derivation.

dxP/ds＝cos[θ _p (s)]

dyP/ds＝sin[θ _p (s)]

dθP/ds＝k _p (s)

x _p (s)＝∫ ₀ ^s cos[θ _p (s)]ds

y _p (s)＝∫ ₀ ^s sin[θ _p (s)]ds

θ _p (s)＝a(p)s+b(p)s ² /2+c(p)s ³ /3+d(p)s ⁴ /4

k _p (s)＝a(p)+b(p)s+c(p)s ² +d(p)s ³

4) The end point of the set track is [ x _p (s _f ) y _p (s _f ) θ _p (s _f ) k _p (s _f )]It is necessary to find that it is equal to x _des Is a parameter of (a). By being relative to the parameter vector p= [ p ] ₁ p ₂ p ₃ s _f ]Evaluating endpoint state vectorsJacobian x _p (s _f )＝[x _p (s _f )y _p (s _f ) θ(s _f )k _p (s _f )]Generating a series of estimates { p } for p using Newton's method _i X is equal to x _des This iteration continues until deltax is considered small enough, or the maximum number of iterations is reached, ultimately producing all trajectories.

Δx＝(x _des -x _pi (s _f ))

Δp＝J _pi (x _pi (s _f )) ^-1 Δx

p _i+1 ＝p _i +Δp

The generated curve is shown in fig. 2, wherein the leftmost round point is the current position point of the vehicle, and the curve is a solved polynomial curve; and (3) generating a speed track: the speed track generation is mainly used for realizing the functions of constant-speed cruising, overtaking and following of driving-assisting vehicles for actively changing lanes.

The specific method comprises the following steps:

1) Under the constant-speed cruising condition, a longitudinal V/T (speed/time) sampling chart is constructed, 8 sampling moments are set on the T axis, each sampling moment is 0.5 seconds apart, 4 sampling speeds are set on the V axis, and the interval of each sampling speed is (V _TargetSpeed -V _CurrentSpeed )/4；

Wherein V is _TargetSpeed Is the cruising speed and the position s of the current vehicle position point is known _start Velocity v _start Acceleration a _start And velocity v of 4S position point _end Acceleration a _end Five variables, can solve the coefficient solutions coeffs of the following velocity curve polynomials;

2) In the presence of obstacles, overtaking or following is requiredThe speed sampling and processing method is consistent with the first point, wherein V _TargetSpeed Is the cruising speed/speed of the following vehicle and the position s of the current vehicle position point is known _start Velocity v _start Acceleration a _start And the position S of the 4S position point _end Velocity v _end Acceleration a _end Six variables, can solve coefficient solutions coeffs of the following velocity curve polynomials;

evaluation function, calculation:

in order to screen out the optimal track, the invention designs a method for evaluating a function, wherein the evaluating function is a linear combination of a plurality of evaluating function items;

the trajectory is discretized into n+1 points, and the evaluation function mainly comprises the following points:

1) Departure lane cost function f _offset ：

Wherein Δs is the square of the deviation of the discrete point from the corresponding mapped point on the reference line;

2) Smoothing cost function f _smooth :

Wherein k is curvature;

3) Comfort cost function f _jeck :

Wherein J is jerk;

4) Crash cost function f _collision ：

f _collision ＝1/min(||P _i -P _obs ||)，(i∈0～n)

Wherein P is _obs The coordinate vector is the coordinate vector of the obstacle, and P is the coordinate vector of the discrete track;

5) Centripetal acceleration cost function f _centripetal ：

Wherein a is centripetal acceleration;

6) Synthesizing cost:

f＝w _offset *f _offset +w _smooth *f _smooth +w _jeck *f _jeck +w _collision *f _collision +w _centripetal *f _centripetal

wherein w is _offset 、w _smooth 、w _jeck 、w _collision 、w _centripetal The self-adjusting weight coefficient of each function is designed according to the adjustable weight coefficient, and the cost function calculation processing of the track is finally completed.

Claims (7)

1. An active lane change planning method based on structured road sampling is characterized by comprising the following steps:

step S1: under the condition that a global planning track of a navigation map is not available, selecting left or right adjacent lane lines according to the state of a turn light, summing 5 times polynomial coefficients of the adjacent lane lines, taking a mean value, obtaining a polynomial equation of an intermediate lane line, and uniformly arranging sampling points at fixed intervals according to the obtained reference line;

step S2: constructing a vehicle kinematic model according to the sampling points obtained in the steps, carrying out transverse and longitudinal polynomial lane change track generation and solving on the sampling points under the constraint of the vehicle kinematic model, and adopting a polynomial spiral line to connect track points to generate a track curve;

step S3: and designing a track evaluation function based on linear combination of the lane departure function, the smoothness function, the comfort function, the collision detection function and the centripetal acceleration function, and screening an optimal track from the calculated tracks.

2. The method for active lane change planning based on structured road sampling according to claim 1, wherein the content of the polynomial equation of the intermediate lane:wherein, leftcoeffs is left-lane line coefficient, and right tcoeffs is right-lane line coefficient.

3. The method for planning an active lane change based on structured road sampling according to claim 1, wherein the longitudinal distance interval of the sampling points is max [4s x vc ] _urrentSpeed ,Vs _{toppingDistance} ]5, wherein Vc _urrentSpeed For the current speed of the vehicle, 4s.times.Vc _urrentSpeed The driving distance is the driving distance within 4s under the current vehicle speed; vs _{toppingDistance} The maximum braking distance is the current speed per hour; the sampling point transverse distance interval is (load_width-width)/2; where load_width is the road width of the adjacent lane and vehicle_width is the vehicle width.

4. The method for planning an active lane change based on structured road sampling according to claim 1, wherein the content of the vehicle kinematic model is: setting a vehicle attitude vector x= (X, y, θ, k, v), wherein X, y is a two-dimensional plane position, θ is a vehicle orientation, k represents a curvature, v is a vehicle linear velocity, and a scalar magnitude thereof satisfies the following relationship:

5. the method for active lane-change planning based on structured road sampling according to claim 1, wherein the trajectory curve is defined as three timesPolynomial spiral, curvature k of trajectory is a cubic polynomial function of arc length s, substituted into parameter vector p= [ p ] ₀ p ₁ p ₂ p ₃ s _f ]Wherein s is _f Is the arc length of the curve between the boundary constraints.

6. The method for active lane-changing planning based on structured road sampling according to claim 1, wherein the evaluation function in step S3 is calculated by:

7. the method for active lane-changing planning based on structured road sampling according to claim 6, wherein the trajectory in step S2 is discretized into n+1 points, and the evaluation function comprises the following:

departure lane cost function f _offset :Wherein DeltaS _i Square the deviation of the discrete point from the corresponding mapped point on the reference line;

smoothing cost function f _smooth :Wherein k is curvature;

comfort cost function f _jeck :Wherein J is _i Is jerk;

crash cost function f _collision ：f _collision ＝1/min(||P _i -P _obs I) i.e.0 to n, where P _obs Is the coordinate vector of the obstacle, P _i Is a coordinate vector of the discrete track;

centripetal acceleration cost function f _centripetal ：Wherein a is _i Is centripetal acceleration;

synthesizing cost: f=w _offset *f _offset +w _smooth *f _smooth +w _jeck *f _jeck +w _collision *f _collision +w _centripetal *f _centripetal ，

Wherein w is _offset 、w _smooth 、w _jeck 、w _collision 、w _centripetal The self-adjusting weight coefficient of each function is designed according to the adjustable weight coefficient, and the cost function calculation processing of the track is finally completed.