Lane changing trajectory planning method based on B spline

Abstract

The invention discloses a lane changing trajectory planning method based on a B spline. The method comprises the following steps: S1, calculating parameters of a lane changing process, and calculating a lane changing completion point based on a current vehicle state; calculating a trajectory curvature / curvature change rate constraint based on the vehicle turning radius and the acceleration; s2, boundary extraction of soft and hard boundary decoupling: obtaining a feasible solution space of trajectory planning based on road topology information and obstacle information, and taking the feasible solution space as a hard boundary; based on the lane changing completion point, a soft boundary is obtained in combination with the vehicle speed and the obstacle safety margin; and S3, trajectory planning modeling and solving are carried out, a trajectory optimization solving model is constructed, constraint and cost functions are set, and an optimal path is obtained. According to the method, the defect that traditional piecewisejerk can only restrain local anchor nodes is overcome; on the premise that the solving speed is higher, the solved trajectory curvature naturally has continuity, meanwhile, the lane changing key parameters of the lane changing type and the vehicle state are comprehensively considered, the lane changing process is represented more finely, and optimization of a high-quality trajectory is facilitated.

Description

Technical Field

[0001] The present invention belongs to the technical field of automobile trajectory planning, and in particular relates to a lane-changing trajectory planning method based on B-spline. Background Art

[0002] Changing lanes is a common driving behavior for autonomous vehicles. Improper lane changing operations can greatly affect driving comfort and operating efficiency, and even cause safety accidents. Therefore, how to plan a safe, comfortable, and efficient lane changing trajectory based on the vehicle's current state information (position, speed, acceleration, etc.) is an important issue facing autonomous driving trajectory planning.

[0003] At present, the commonly used lane-changing trajectory planning models include: constant-speed offset lane-changing trajectory model, arc lane-changing trajectory model, polynomial lane-changing trajectory model, B-spline curve lane-changing model, etc. Each model has its own advantages and disadvantages. Among them, the advantages of the trajectory planning method based on the constant-speed offset / arc lane-changing model are simple design and small calculation amount, but the disadvantages are more obvious: the planned trajectory is not smooth, and the speed, acceleration, and curvature jump at the lane-changing point, especially in high-speed scenarios, the comfort is poor. The trajectory planned based on the polynomial lane-changing model is relatively smooth, and there is no problem of sudden curvature changes; but the characteristics of the polynomial determine that this planning method can only constrain the starting point and the end point, and cannot ensure that the middle point of the trajectory meets the constraints. In comparison, the trajectory planned based on the B-spline method is not only smooth and faster in calculation speed, but also satisfies the convex hull constraint, so that the entire curve satisfies the constraint; in summary, this paper uses a planning method based on the B-spline model.

[0004] At the same time, the lane changing distance, wheel angle during lane changing, and wheel angle change rate also have a great impact on the lane changing experience. These factors should be given special consideration in trajectory planning. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a lane changing trajectory planning method based on B-spline, which overcomes the defect that the traditional piecewise jerk can only constrain local anchor nodes; under the premise of faster solution speed, the solved trajectory curvature naturally has continuity, and at the same time, it more finely characterizes the lane changing process, which is conducive to optimizing high-quality trajectories.

[0006] In order to solve the above technical problems, the present invention provides a lane change trajectory planning method based on B-spline, comprising the following steps:

[0007] Step S1. Calculate the parameters of the lane change process, calculate the lane change completion point based on the current vehicle state; calculate the trajectory curvature / curvature change rate constraint based on the vehicle turning radius and acceleration;

[0008] Step S2. Boundary extraction of soft and hard boundary decoupling, obtaining the feasible solution space of trajectory planning based on road topology information and obstacle information as the hard boundary; obtaining the soft boundary based on the lane change completion point, combined with the vehicle speed and obstacle safety margin;

[0009] Step S3: trajectory planning modeling and solving, constructing trajectory optimization solution model, setting constraints and cost functions, and obtaining the optimal path.

[0010] The technical solution further defined in the present invention is: in step S1, the lane change completion point is calculated by:

[0011] The position of the vehicle in the Frenet coordinate system is expressed as (curr_s, curr_l), the current vehicle speed is curr_v, and based on the current vehicle speed, the comfort lateral acceleration experience value l_dot is obtained, and the comfortable lane change completion point is:

[0012] comfortable_s=curr_s+curr_v*curr_l / l_dot;

[0013] If the distance force_s from the vehicle to the forced lane change point is greater than confortable_s, confortable_s is used as the lane change completion point s, that is, s = comfortable_s; otherwise, s = force_s;

[0014] The calculation method of trajectory curvature is:

[0015] The current vehicle speed is curr_v, the maximum deceleration is dec_max, the distance from the trajectory point to be calculated to the vehicle is delta_s, and the maximum centripetal acceleration is acc_max. Then the maximum curvature kappa of the trajectory point is

[0016] kappa_max=tan(theta_max) / L

[0017] kappa=acc_max / (curr_v*curr_v)

[0018] kappa=min(kappa_max,kappa);

[0019] Among them, kappa_max is the maximum curvature limit of the vehicle, which is calculated by the maximum wheel turning angle theta_max and the wheelbase L;

[0020] The curvature change rate is calculated as follows:

[0021] The curvature change rate calculation formula is:

[0022] dkappa=coff*kappa_max / (curr_v*curr_v);

[0023] Where coff is the curvature change rate coefficient.

[0024] Further, in step S2,

[0025] Hard boundary barrier_bounds = {barrier_bound0, barrier_bound1, barrier_bound2...}; where,

[0026] barrier_bound i =(curr_s,left_bound,right_bound); the calculation steps are as follows, taking left lane change as an example:

[0027] Step S21. Initialize the s value curr_s of the current point, the left and right boundaries of the current point left_bound, right_bound; i = 0;

[0028] Step S22. According to the current curr_s, query the corresponding road road_left_width on the target lane and the lateral offset curr_l of the current vehicle position in the target lane, and update the boundary of the current point:

[0029] left_bound=road_left_width-witdh / 2;

[0030] right_bound = curr_l;

[0031] Step S23. If there is an obstacle around the current position curr_s, update the corresponding left_bounds and right_bounds according to the detour direction of the obstacle and l;

[0032] Step S24. Update barrier_bound i =(curr_s,left_bound,right_bound);i=i+1;

[0033] Step S25. Loop step S22 to step S24 until all boundary extractions are completed.

[0034] Furthermore, in step S3, the B-spline is initialized, and the specific steps are as follows:

[0035] Step S31. Discretize the initial trajectory; along the s direction, sample the reference line with a fixed resolution to obtain the initial trajectory points; the initial trajectory points are discrete points P i, i=1,2...N format;

[0036] Step S32. Use the initial trajectory point PC as the control point to construct a B-spline curve. These control points are also the variables for solving the optimization problem, that is,

[0037]

[0038] where u is the node vector, PC i represents the i-th control point, N i,k (u) represents the k-order basis function coefficient corresponding to the i-th control point at time u;

[0039] Step S33. Perform uniform sampling on the B-spline according to the step size to obtain a sampling point set And the corresponding cumulative length Length i , i=1...Num; each State contains the x, y, theta of the current point.

[0040] Further, step S3. constraint setting, specifically,

[0041] Sampling is performed on the B-spline curve at a fixed resolution along the s direction; the sampling sequence is obtained

[0042] states={state0,state1,state2...};state i =(s i ,x i ,y i ,theta i );

[0043] According to the s value of each state in the sampling sequence and the linear difference in the hard and soft boundaries barrier_bounds and soft_bounds, the corresponding left and right hard and soft boundaries of the trajectory point are obtained;

[0044] Set the constraints of the starting point and the end point; the starting point P0 = [x0, y0, theta0] and the end point P end =[x end ,y end ,theta end ].

[0045] Furthermore, in step S3, the cost function is set to:

[0046] According to the first-order derivative BS′, second-order derivative BS”, and third-order derivative BS”’ of the B-spline, the corresponding weight coefficients are w1, w2, and w3.

[0047] cost=w1∫BS′ 2 +w2∫BS″2 +w3∫BS″′ 2 .

[0048] Furthermore, in step S3, the constraints and cost function are brought into the solver to obtain the optimized B-spline function Sampling is performed at equal intervals on the optimized B-spline function to obtain the pose points i=1...N, which is the final optimization solution result.

[0049] The beneficial effects of the present invention are:

[0050] 1. The trajectory planning algorithm is adopted. The convex hull property of B-spline ensures the effectiveness of global constraints and overcomes the defect that the traditional piecewise jerk can only constrain local anchor nodes. Under the premise of faster solution speed, the curvature of the solved trajectory is naturally continuous.

[0051] 2. The key lane-changing parameters (lane-changing completion point, curvature constraint, curvature change rate) are calculated by comprehensively considering the lane-changing type and vehicle status, which characterizes the lane-changing process more precisely and is conducive to optimizing high-quality trajectories.

[0052] 3. Based on the boundary analysis method of soft and hard boundary decoupling, the trajectory is made to be as close to the middle of the feasible domain as possible while ensuring no collision; and the calculation method of soft and hard boundaries can be subsequently expanded according to the obstacle type. BRIEF DESCRIPTION OF THE DRAWINGS DETAILED DESCRIPTION

[0053] Example 1

[0054] This embodiment provides a lane change trajectory planning method based on B-spline, comprising the following steps:

[0055] Step S1. Calculate the parameters of the lane change process, calculate the lane change completion point based on the current vehicle state; calculate the trajectory curvature / curvature change rate constraint based on the vehicle turning radius and acceleration;

[0056] Step S2. Boundary extraction of soft and hard boundary decoupling, obtaining the feasible solution space of trajectory planning based on road topology information and obstacle information as the hard boundary; obtaining the soft boundary based on the lane change completion point, combined with the vehicle speed and obstacle safety margin;

[0057] Step S3: trajectory planning modeling and solving, constructing trajectory optimization solution model, setting constraints and cost functions, and obtaining the optimal path.

[0058] In step S1, the lane change completion point is calculated as follows:

[0059] The position of the vehicle in the Frenet coordinate system is expressed as (curr_s, curr_l), the current vehicle speed is curr_v, and based on the current vehicle speed, the comfort lateral acceleration experience value l_dot is obtained, and the comfortable lane change completion point is:

[0060] comfortable_s=curr_s+curr_v*curr_l / l_dot;

[0061] If the distance force_s from the vehicle to the forced lane change point is greater than confortable_s, confortable_s is used as the lane change completion point s, that is, s = comfortable_s; otherwise, s = force_s;

[0062] The calculation method of trajectory curvature is:

[0063] The current vehicle speed is curr_v, the maximum deceleration is dec_max, the distance from the trajectory point to be calculated to the vehicle is delta_s, and the maximum centripetal acceleration is acc_max. Then the maximum curvature kappa of the trajectory point is

[0064] kappa_max=tan(theta_max) / L

[0065] kappa=acc_max / (curr_v*curr_v)

[0066] kappa=min(kappa_max,kappa);

[0067] Among them, kappa_max is the maximum curvature limit of the vehicle, which is calculated by the maximum wheel turning angle theta_max and the wheelbase L;

[0068] The curvature change rate is calculated as follows:

[0069] The curvature change rate calculation formula is:

[0070] dkappa=coff*kappa_max / (curr_v*curr_v);

[0071] Where coff is the curvature change rate coefficient.

[0072] In step S2,

[0073] Hard boundary barrier_bounds = {barrier_bound0, barrier_bound1, barrier_bound2...}; where,

[0074] barrier_boundi =(curr_s,left_bound,right_bound); the calculation steps are as follows, taking left lane change as an example:

[0075] Step S21. Initialize the s value curr_s of the current point, the left and right boundaries of the current point left_bound, right_bound; i = 0;

[0076] Step S22. According to the current curr_s, query the corresponding road road_left_width on the target lane and the lateral offset curr_l of the current vehicle position in the target lane, and update the boundary of the current point:

[0077] left_bound=road_left_width-witdh / 2;

[0078] right_bound = curr_l;

[0079] Step S23. If there is an obstacle around the current position curr_s, update the corresponding left_bounds and right_bounds according to the detour direction of the obstacle and l;

[0080] Step S24. Update barrier_bound i =(curr_s,left_bound,right_bound);i=i+1;

[0081] Step S25. Loop step S22 to step S24 until all boundary extractions are completed.

[0082] In step S3, the B-spline is initialized, and the specific steps are as follows:

[0083] Step S31. Discretize the initial trajectory; along the s direction, sample the reference line with a fixed resolution to obtain the initial trajectory points; the initial trajectory points are discrete points P i , i=1,2...N format;

[0084] Step S32. Use the initial trajectory point PC as the control point to construct a B-spline curve. These control points are also the variables for solving the optimization problem, that is,

[0085]

[0086] where u is the node vector, PC i represents the i-th control point, N i,k (u) represents the k-order basis function coefficient corresponding to the i-th control point at time u;

[0087] Step S33. Perform uniform sampling on the B-spline according to the step size to obtain a sampling point set And the corresponding cumulative length Length i , i=1...Num; each State contains the x, y, theta of the current point.

[0088] Further, step S3. constraint setting, specifically,

[0089] Sampling is performed on the B-spline curve at a fixed resolution along the s direction; the sampling sequence is obtained

[0090] states={state0,state1,state2...};state i =(s i ,x i ,y i ,theta i );

[0091] According to the s value of each state in the sampling sequence and the linear difference in the hard and soft boundaries barrier_bounds and soft_bounds, the corresponding left and right hard and soft boundaries of the trajectory point are obtained;

[0092] Set the constraints of the starting point and the end point; the starting point P0 = [x0, y0, theta0] and the end point P end =[x end ,y end ,theta end ].

[0093] In step S3, the cost function is set to:

[0094] According to the first-order derivative BS′, second-order derivative BS”, and third-order derivative BS”’ of the B-spline, the corresponding weight coefficients are w1, w2, and w3.

[0095] cost=w1∫BS′ 2 +w2∫BS″ 2 +w3∫BS″′ 2 .

[0096] In step S3, the constraints and cost function are brought into the solver to obtain the optimized B-spline function Sampling is performed at equal intervals on the optimized B-spline function to obtain the pose points i=1...N, which is the final optimization solution result.

[0097] This method uses a trajectory planning algorithm. The convex hull property of B-spline ensures the effectiveness of global constraints and overcomes the defect that the traditional piecewise jerk can only constrain local anchor nodes. Under the premise of faster solution speed, the curvature of the solved trajectory is naturally continuous. The key parameters of lane change (lane change completion point, curvature constraint, curvature change rate) are calculated by comprehensively considering the lane change type and vehicle status, which more accurately characterizes the lane change process and is conducive to optimizing high-quality trajectories. Based on the boundary analysis method of soft and hard boundary decoupling, the trajectory is made to be as close to the middle of the feasible domain as possible while ensuring no collision; and the calculation method of soft and hard boundaries can be subsequently expanded according to the type of obstacle.

[0098] In addition to the above embodiments, the present invention may also have other implementation modes. Any technical solution formed by equivalent replacement or equivalent transformation falls within the protection scope required by the present invention.

Claims

1. A lane-changing trajectory planning method based on B-spline, characterized in that: The following steps are included: Step S1. Calculate the parameters of the lane change process, calculate the lane change completion point based on the current vehicle state; calculate the trajectory curvature / curvature change rate constraint based on the vehicle turning radius and acceleration; Step S2. Boundary extraction of soft and hard boundary decoupling, obtaining a feasible solution space of trajectory planning based on road topology information and obstacle information as a hard boundary; Based on the lane change completion point, the soft boundary is obtained by combining the vehicle speed and obstacle safety margin; Step S3: trajectory planning modeling and solving, constructing trajectory optimization solution model, setting constraints and cost functions, and obtaining the optimal path.

2. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that: In step S1, the lane change completion point is calculated as follows: The position of the vehicle in the Frenet coordinate system is expressed as (curr_s, curr_l), the current vehicle speed is curr_v, and based on the current vehicle speed, the comfort lateral acceleration experience value l_dot is obtained, and the comfortable lane change completion point is: comfortable_s=curr_s+curr_v*curr_l / l_dot; If the distance force_s from the vehicle to the forced lane change point is greater than confortable_s, confortable_s is used as the lane change completion point s, that is, s = comfortable_s; otherwise, s = force_s; The calculation method of trajectory curvature is: The current vehicle speed is curr_v, the maximum deceleration is dec_max, the distance from the trajectory point to be calculated to the vehicle is delta_s, and the maximum centripetal acceleration is acc_max. Then the maximum curvature kappa of the trajectory point is kappa_max=tan(theta_max) / L kappa=acc_max / (curr_v*curr_v) kappa=min(kappa_max,kappa); Among them, kappa_max is the maximum curvature limit of the vehicle, which is calculated by the maximum wheel turning angle theta_max and the wheelbase L; The curvature change rate is calculated as follows: The curvature change rate calculation formula is: dkappa=coff*kappa_max / (curr_v*curr_v); Where coff is the curvature change rate coefficient.

3. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that: In step S2, Hard boundary barrier_bounds = {barrier_bound0, barrier_bound1, barrier_bound2...}; where barrier_bound i =(curr_s,left_bound,right_bound); the calculation steps are as follows, taking left lane change as an example: Step S21. Initialize the s value curr_s of the current point, the left and right boundaries of the current point left_bound, right_bound; i = 0; Step S22. According to the current curr_s, query the corresponding road road_left_width on the target lane and the lateral offset curr_l of the current vehicle position in the target lane, and update the boundary of the current point: left_bound=road_left_width-witdh / 2; right_bound = curr_l; Step S23. If there is an obstacle around the current position curr_s, update the corresponding left_bounds and right_bounds according to the detour direction of the obstacle and l; Step S24. Update barrier_bound i =(curr_s,left_bound,right_bound);i=i+1; Step S25. Loop step S22 to step S24 until all boundary extractions are completed.

4. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that: In step S3, the B-spline is initialized, and the specific steps are: Step S31. Discretize the initial trajectory; along the s direction, sample the reference line with a fixed resolution to obtain the initial trajectory points; the initial trajectory points are discrete points P i , i=1,2...N format; Step S32. Use the initial trajectory point PC as the control point to construct a B-spline curve. These control points are also the variables for solving the optimization problem, that is, where u is the node vector, PC i represents the i-th control point, N i,k (u) represents the k-order basis function coefficient corresponding to the i-th control point at time u; Step S33. Perform uniform sampling on the B-spline according to the step size to obtain a sampling point set And the corresponding cumulative length Length i , i=1...Num; each State contains the x, y, theta of the current point.

5. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that The step S3. constraint setting is specifically, Sampling on the B-spline curve with a fixed resolution along the s direction; Get the sampling sequence states = {state0, state1, state2...}; state i =(s i ,x i ,y i ,theta i ); According to the s value of each state in the sampling sequence and the linear difference in the hard and soft boundaries barrier_bounds and soft_bounds, the corresponding left and right hard and soft boundaries of the trajectory point are obtained; Set the constraints of the starting point and the end point; the starting point P0 = [x0, y0, theta0] and the end point P end =[x end ,y end ,theta end ].

6. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that: In step S3, the cost function is set to: According to the first-order derivative BS', second-order derivative BS", and third-order derivative BS' of B-spline, the corresponding weight coefficients w1, w2, and w3 are cost = w1∫BS' 2 +w2∫BS″ 2 +w3∫BS″′ 2 .

7. The lane-changing trajectory planning method based on B-spline according to claim 1, characterized in that: In step S3, the constraints and cost function are brought into the solver to obtain the optimized B-spline function. Sampling is performed at equal intervals on the optimized B-spline function to obtain the pose points i=1...N, which is the final optimization solution result.

Images