Segmented real-time lane changing trajectory planning method for vehicle

Abstract

The invention discloses a vehicle sectional type real-time lane change trajectory planning method, which is used for planning a trajectory of a vehicle A changing a lane from a lane I to a lane II through a lane line L. The method comprises the following steps: calculating a state range of a transit position of the vehicle A on the lane line L according to states of the vehicle A and a vehicle B1 in front of the vehicle A on the lane I; modeling a plurality of indexes of the first section of the lane changing process into an optimal control problem based on the range of the lane changing starting point state and the transfer position state of the vehicle A, and optimally calculating the track of the first section of the lane changing process; when the vehicle arrives at the transfer position, the terminal point state range of the vehicle A in the lane II is calculated according to the states of the front vehicle B2 and the rear vehicle B3 at the target position of the lane II; and generating a plurality of tracks by utilizing a quintic polynomial, and selecting an optimal track as a track of a second section of lane changing process by considering a plurality of indexes. According to the method, the lane changing track is planned in a segmented mode, and energy-saving, comfortable and safe lane changing of the electric vehicle under the dynamic traffic scene is achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory planning, and in particular relates to a vehicle segmented real-time lane-changing trajectory planning method combining optimal control and quintic polynomials. Background Art

[0002] The iterative cost reduction of power batteries and the large-scale application of intelligent technologies such as generative AI will accelerate the global adoption of new energy vehicles. Incorporating energy efficiency optimization into this intelligent process has important practical significance. However, current lane change planning models for smart cars do not consider the impact of energy consumption, lacking a lane change trajectory planning model that balances energy conservation, safety, comfort, and real-time performance in dynamic traffic scenarios. Summary of the Invention

[0003] The present invention provides a segmented real-time lane-changing trajectory planning method for vehicles. With energy saving as the optimization goal and comfort as the constraint condition, the lane-changing trajectory is planned segment by segment, so that electric vehicles can complete energy-saving, comfortable and safe lane changes in dynamic traffic scenarios.

[0004] In order to achieve the above technical objectives, the present invention adopts the following technical solutions:

[0005] A segmented real-time lane-changing trajectory planning method for a vehicle is used to plan the trajectory of a vehicle A changing lanes from lane I to lane II via lane line L, comprising:

[0006] Plan the first lane-changing trajectory of vehicle A from lane I to lane line L: Based on the states of vehicle A and its obstacle B1 in lane I, calculate the state range of vehicle A's transfer position on lane line L. Then, based on the state of vehicle A at the lane-changing starting point and the state range of the transfer position, model multiple indicators of the first lane-changing process as an optimal control problem, and optimize the trajectory of the first lane-changing process. The vehicle's state includes its lateral and longitudinal position and velocity.

[0007] When the vehicle reaches the transfer location, the second lane-changing trajectory of vehicle A from lane line L to lane II is planned. The final state range of vehicle A in lane II is calculated based on the states of the obstacles B2 in front and B3 behind the target position in lane II. A quintic polynomial is then used to generate multiple trajectories, and the optimal trajectory is selected as the trajectory for the second lane-changing process, taking into account multiple indicators.

[0008] Furthermore, when modeling the optimal control problem to optimize the calculation of the first trajectory, and / or when using a quintic polynomial to select the second lane-changing trajectory, the vehicle's energy consumption index is considered; the energy consumption index is calculated using the VT-CPEM energy consumption model.

[0009] Furthermore, the state range of the transfer position of vehicle A on lane line L is calculated as follows:

[0010] First, determine the conditions under which a lateral collision does not occur between vehicles A and B1:

[0011] d1+x B1 >x A1 (1)

[0012] Where d1 is the distance between vehicle A and vehicle B1 at the start of lane change, x B1 is the minimum lateral displacement of vehicle B1 during the first lane-changing process of vehicle A, x A1 is the maximum lateral displacement of vehicle A during the first lane-changing process, and:

[0013]

[0014] Where, t c is the actual time of the first lane-changing process of vehicle A, a A 、v x0 are the lateral acceleration and lateral initial velocity of vehicle A during the entire lane changing process, a B1 、v B1 It is divided into the acceleration and speed of vehicle B1 during the first lane change of vehicle A; vehicle B1 is traveling in the transverse lane I without changing lanes;

[0015] Then, solve inequality (1) to obtain the maximum lane-changing time t1 of vehicle A in the first lane-changing process;

[0016] Substituting the maximum lane-changing time t1 into equation (2), we can obtain the state range of the vehicle's transfer position on lane line L, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval:

[0017] s yc =D 车 , v xc ∈[v x0 ,v x0 +a A t1],v yc ∈[0,v ymax ]

[0018] Among them, D 车 is the width of all vehicles, v ymax is the maximum longitudinal speed of the vehicle when changing lanes.

[0019] Furthermore, the calculation of the distance d1 takes into account the algorithm solution time of the first lane change trajectory planning, and the calculation formula is:

[0020] d1=d0-(v x0 -v B1 )×tcom (4)

[0021] Where d0 is the distance between vehicle A and vehicle B1 at the beginning of lane change trajectory planning, t com The solution time of the algorithm for planning the first lane-changing trajectory.

[0022] Furthermore, the multiple indicators of the first lane-changing process are modeled as an optimal control problem, and the cost function is:

[0023]

[0024] Where J represents the cost function, is the longitudinal acceleration of vehicle A at time t, a ymax is the maximum longitudinal acceleration of vehicle A, v(t), a(t) are the speed and acceleration of vehicle A at time t, E(v(t), a(t)) is the energy consumption cost of vehicle A, E max represents the maximum energy consumption, β1, β2, and β3 represent the weighted coefficients of comfort, efficiency, and energy economy, respectively, and β1+β2+β3=1;

[0025]

[0026] Where, v x (t),a x (t) are the lateral velocity and lateral acceleration of vehicle A at time t, v y (t),a y (t) are the longitudinal velocity and longitudinal acceleration of vehicle A at time t, E Recoverable (v(t), a(t)) is the energy consumed by vehicle A at time t, E Consume (v(t), a(t)) is the braking energy recovery of vehicle A at time t;

[0027] By solving the above cost function, we can get the actual time t of the first lane-changing process: c And the acceleration a(t) at each moment, and then the velocity v(t) and displacement s(t) at each moment are obtained by integration.

[0028] Furthermore, the constraints of the optimal control problem include:

[0029]

[0030] Furthermore, the same cost function and constraints as those in the first lane-changing trajectory optimal control problem are used to select the optimal trajectory from the multiple generated trajectories.

[0031] Furthermore, the terminal state range of vehicle A in lane II is calculated as follows:

[0032] First, determine the conditions under which a lateral collision does not occur between vehicle A and vehicle B2:

[0033] d2+x B2 >x A2 (8)

[0034] Where d2 is the distance between vehicle A and vehicle B2 at the start of the second lane change, x B2 is the minimum lateral displacement of vehicle B2 during the second lane-changing process of vehicle A, x A2 is the maximum lateral displacement of vehicle A during the second lane-changing process, and:

[0035]

[0036] Where, t e is the actual time of the second lane-changing process of vehicle A, a A 、v xc are the lateral acceleration of vehicle A during the entire lane-changing process and the lateral velocity at the end of the first lane-changing process, respectively. B2 、v B2 It is divided into the acceleration and speed of vehicle B2 during the second lane change process of vehicle A; vehicle B2 is traveling in the transverse lane II without changing lanes;

[0037] Then, the conditions for no lateral collision between vehicle A and vehicle B3 are also determined:

[0038] d3+x A3 >x B3 (11)

[0039] Where d3 is the distance between vehicle A and vehicle B3 at the start of the second lane change, x B3 is the maximum lateral displacement of vehicle B3 during the second lane-changing process of vehicle A, x A3 is the minimum lateral displacement of vehicle A during the second lane-changing process, and:

[0040]

[0041] Where a B3 、v B3 It is divided into the acceleration and speed of vehicle B3 during the second lane change of vehicle A; vehicle B3 is traveling in the transverse lane II without changing lanes;

[0042] Then solve inequality (8) to get the maximum lane-changing time t2 of vehicle A in the second lane-changing process, and solve inequality (11) to get the minimum lane-changing time t3 of vehicle A in the second lane-changing process, and get the lane-changing time range t of vehicle A in the second lane-changing process.e ∈[t3,t2];

[0043] Finally, the maximum lane-changing time t2 of the second lane-changing process is substituted into equation (9), and the minimum lane-changing time t3 of the second lane-changing process is substituted into equation (12), and the state range of the vehicle at the end of lane-changing in lane II is obtained, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval:

[0044]

[0045] v xe ∈[v xc +a A t3,v xc +a A t2],v ye =0

[0046] Among them, D 车道 is the width of the lane, a A is the lateral acceleration of vehicle A during the entire lane changing process.

[0047] Furthermore, the method of generating multiple trajectories using a quintic polynomial is:

[0048] The lane-changing time range t of vehicle A in the second lane-changing process e ∈[t3,t2], uniformly discretized into N time t e1 ,t e2 ,…,t eN ; Set the horizontal position interval Uniformly discretized into M lateral positions s xe1 ,s xe2 ,…,s xeM ;

[0049] Expressing the position of vehicle A as a function of time t in the lateral and longitudinal directions, we obtain the following expressions:

[0050]

[0051] Where m and n represent the numbers of discrete time points and discrete horizontal positions, respectively, and m = 1, 2, ..., M; n = 1, 2, ..., N; s x (t), s y (t) represent the lateral position and longitudinal position of vehicle A at time t, respectively; a 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n The coefficients of the fifth-order polynomial representing the lateral position with respect to time t, b 0m×n 、b1m×n 、b 2m×n 、b 3m×n 、b 4m×n 、b 5m×n The coefficients of the fifth-order polynomial representing the longitudinal position with respect to time t;

[0052] Based on the kinematic constraints between displacement, velocity, and acceleration, the function expressions of the velocity and acceleration of vehicle A in the lateral and longitudinal directions with respect to time t are obtained as follows:

[0053]

[0054] Where, v x (t), v y (t) represent the lateral velocity and longitudinal velocity of vehicle A at time t, respectively; a x (t), a y (t) denote the lateral acceleration and longitudinal acceleration of vehicle A at time t, respectively;

[0055] The state X of vehicle A at the starting point of the second lane-changing process is c =[s xc ,D 车 ,v xc ,v yc ,a xc ,a yc ] and the terminal state X e =[s xe ,D 车道 ,v xe ,v ye ,a xe ,a ye ], and v ye =0,a xe =0,a ye =0, and substituting into formula (14) we get:

[0056]

[0057] Wherein, the time of the starting point of the second lane-changing process is t=0;

[0058] Solve equations (17) and (18) to obtain the coefficient a in equation (14): 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n and coefficient b 0m×n 、b 1m×n 、b 2m×n 、b 3m×n 、b 4m×n 、b 5m×n, and then obtain the functional expressions of the position, velocity and acceleration of vehicle A in the second lane-changing process;

[0059] Finally, multiple trajectories are obtained based on the different values ​​of the numbers m and n of the discrete time points and discrete lateral positions.

[0060] Beneficial effects

[0061] The segmented lane-changing trajectory planning method proposed in this invention combines optimal control and quintic polynomials. Taking into account the time-varying characteristics of surrounding obstacles, the entire lane-changing trajectory is divided into two segments. By detecting changes in obstructing vehicles during the lane-changing process at the starting point and midpoint, the lane-changing trajectory is adjusted in real time to ensure safety.

[0062] At the start of a lane change, a safe transfer position is calculated using information such as the speed and position of the ego vehicle and the obstacle ahead in the lane. The energy optimization problem from the lane change starting point to the transfer point is modeled as an optimal control problem, resulting in the first trajectory from the starting point to the transfer point. Once the vehicle reaches the transfer point, the final position range is calculated using information such as the speed and position of obstacles ahead and behind the target lane. Multiple trajectories are generated using a quintic polynomial, and the optimal second lane change trajectory is determined based on comfort and economy.

[0063] The present invention establishes a comprehensive evaluation system that takes into account comfort, safety, energy consumption, and efficiency. Considering that the solution performance under optimal control planning is better, but the real-time performance is slightly worse, it is placed in the first stage of planning. Considering that the quintic polynomial has fast calculation speed and good real-time performance, but the planning result performance is average, because the vehicle is already at the midpoint position in the second stage of planning, for safety reasons, higher real-time performance is required at this time, so it is placed in the second stage of planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a schematic diagram of the first lane change section of the embodiment of the present application;

[0065] Figure 2 It is the longitudinal acceleration and maximum lateral velocity distribution map of the highD dataset;

[0066] Figure 3 This is a schematic diagram of the second lane change in an embodiment of the present application;

[0067] Figure 4 is a cluster of quintic polynomial trajectories obtained in the embodiment of the present application;

[0068] Figure 5 This is a comparison diagram of different scenario trajectories in the embodiment of the present application, where (a) (b) (c) correspond to scenario 1, scenario 2, and scenario 3 respectively;

[0069] Figure 6This is a comparison chart of energy consumption in different scenarios of the embodiment of the present application, where (a) (b) (c) correspond to scenario one, scenario two, and scenario three respectively. DETAILED DESCRIPTION

[0070] The following is a detailed description of an embodiment of the present invention. This embodiment is based on the technical solution of the present invention, provides a detailed implementation method and a specific operation process, and further explains the technical solution of the present invention.

[0071] This embodiment provides a segmented, real-time lane-changing trajectory planning method for a vehicle, which is used to plan the trajectory of vehicle A as it changes lanes from lane I to lane II via lane line L. The method includes: planning the first segment of the lane-changing trajectory of vehicle A from lane I to lane line L; and when the vehicle reaches the transfer location, planning the second segment of the lane-changing trajectory of vehicle A from lane line L to lane II.

[0072] 1. The first lane change trajectory planning.

[0073] The first lane change process of vehicle A from lane I to lane line L is as follows: Figure 1 As shown. Assuming that all vehicles are driving along the center line of the lane, at the initial moment of lane change, the lateral speed of vehicle A is v x0 , the longitudinal speed is 0, and the speed of the obstacle vehicle B1 in front of lane I before changing lanes is v B1 The initial longitudinal distance between the two vehicles is d0, and the center of mass state of the vehicle body at the lane change starting point is (0,0,v x0 ,0), the center of mass state of vehicle A at the midpoint of lane change (on the lane line between lane I and lane II) is (s xc ,s yc ,v xc ,v yc ). Since the lateral angle between the car body and the road is very small during the lane change process, the yaw angle of the car at the midpoint is ignored, that is, the car body is parallel to the lane line at the midpoint. Considering that there is no lateral interference with the preceding car at the midpoint of the lane change, s yc The width of the obstacle vehicle is set to 1.8m, which is slightly larger than the width of the obstacle vehicle (all vehicle parameters are set to 1.8m wide and 4m long in this paper), while also ensuring that there is no longitudinal interference with the target lane vehicle. In order to generate a feasible region that is safe in the lateral direction, it is assumed that the ego vehicle A always accelerates at the maximum allowed acceleration (2m / s 2 ) uniform acceleration, for the obstacle car B1 in front, assume that it has been uniformly decelerating, such as Figure 2 As shown in the 4880 sets of driving data extracted from the highD dataset, it was found that the maximum lateral acceleration of the driver is generally -2m / s 2 to 2m / s 2 Therefore, the deceleration of the obstacle vehicle B1 can be set to -2m / s2 In the present invention, the state vector of the vehicle includes the lateral displacement, the longitudinal displacement, the lateral speed, the longitudinal speed, or also includes the lateral acceleration and the longitudinal acceleration.

[0074] Step 1.1: Calculate the state range of the transfer position of vehicle A on lane line L based on the state of vehicle A and its obstacle vehicle B1 in front of it on lane I. In step 1, the state of the vehicle includes the lateral and longitudinal position and speed.

[0075] First, determine the conditions under which a lateral collision does not occur between vehicles A and B1:

[0076] d1+x B1 >x A1 (1)

[0077] Where d1 is the distance between vehicle A and vehicle B1 at the start of lane change, x B1 is the minimum lateral displacement of vehicle B1 during the first lane-changing process of vehicle A, x A1 is the maximum lateral displacement of vehicle A during the first lane-changing process, and:

[0078]

[0079] d1=d0-(v x0 -v B1 )×t com (4)

[0080] Where, t c is the actual time of the first lane-changing process of vehicle A, a A 、v x0 are the lateral acceleration and lateral initial velocity of vehicle A during the entire lane changing process, a B1 、v B1 It is divided into the acceleration and speed of vehicle B1 during the first lane change process of vehicle A. Among them, vehicle B1 is driving in the lateral lane I without changing lanes, so there is no longitudinal displacement, speed and acceleration. d0 is the distance between vehicle A and vehicle B1 when the lane change trajectory planning starts, t com The solution time of the algorithm for planning the first lane-changing trajectory.

[0081] Then, the maximum lane-changing time t1 allowed for vehicle A in the first lane-changing process is obtained by solving inequality (1).

[0082] Substituting the maximum lane-changing time t1 into equation (2) yields the range of the vehicle's transfer position state (sxc, syc, vxc, vyc) on lane line L, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval:

[0083] s yc =D 车 , v xc ∈[v x0 ,v x0 +a A t1],v yc ∈[0,v ymax ]

[0084] Among them, D 车 is the width of all vehicles, which is set to D in this embodiment. 车 =1.8;v ymax is the maximum longitudinal speed of the vehicle when changing lanes. From the highD dataset, it is found that the lateral speed of the driver when changing lanes generally does not exceed 2m / s. Therefore, in this embodiment, v is set ymax =2m / s.

[0085] Step 1.2: Based on the state of vehicle A at the lane change starting point and the state range of the transfer position, multiple indicators of the first lane change process are modeled as an optimal control problem.

[0086] In the present invention, the lateral displacement s at any time t during the lane change process is x (t), longitudinal displacement s y (t), lateral velocity v x (t), longitudinal velocity v y (t), together construct the vehicle state vector represented as: X(t) = [s x (t),s y (t),v x (t),v y (t)] T .

[0087] The initial lane-changing state of vehicle A is X0 = [s x0 ,s y0 ,v x0 ,v y0 ] T =[0,0,v x0 ,0] T , the lane change status is X c =[s xc ,s yc ,v xc ,v yc ] T =[[0,x A1 ],D 车 ,[v x0 , v x0 +a A t c ],[0,v ymax ]]T (sx0,sy0) is the position coordinate when lane change starts, (v x0 ,v y0 ) are the lateral speed and longitudinal speed at the beginning of lane change. (s xc ,s yc ) is the coordinate of the lane-changing position, (v xc ,v yc ) are the lateral and longitudinal velocities at the transition position.

[0088] The vehicle kinematic constraints are: The control vectors are lateral acceleration and longitudinal acceleration, expressed as:

[0089] In the first lane-changing process, the comfort, efficiency, and energy economy indicators are modeled as an optimal control problem, and the cost function is:

[0090]

[0091] Where J represents the cost function, is the longitudinal acceleration of vehicle A at time t, a ymax is the maximum longitudinal acceleration of vehicle A, v(t), a(t) are the speed and acceleration of vehicle A at time t, E(v(t), a(t)) is the energy consumption cost of vehicle A, E max represents the maximum energy consumption, β1, β2, and β3 represent the weighted coefficients of comfort, efficiency, and energy economy, respectively, and β1+β2+β3=1. In this embodiment, a ymax =2, t max Take 3s, E max It can be obtained from the maximum lane change time and the lane change end speed.

[0092] When modeling the optimal control problem to optimize the calculation of the first trajectory segment, this embodiment of the present invention considers the vehicle's energy consumption. Compared to traditional fuel vehicles, electric vehicles regenerate energy during braking, and their energy consumption model is more complex. This embodiment calculates the energy consumption using the VT-CPEM energy consumption model, which takes road slope, vehicle acceleration, speed, and other data as input and outputs the energy consumption of the electric vehicle during a specific driving cycle.

[0093] In the VT-CPEM energy consumption model, considering the air resistance, driving resistance, road slope, and inertia force of the electric vehicle, the power at the wheel can be expressed as:

[0094]

[0095] Where v(t) is the speed of the car, a(t) is the acceleration of the car (negative value when decelerating), g is the acceleration of gravity, α is the road slope, Cr =1.75, c1=0.0328, c2=4.575 are rolling resistance parameters, which vary with different road surfaces, m is the mass of the car, t is the driving time, P Air =1.25536 is the air mass density, A f is the frontal area of ​​the vehicle, C D =0.28 is the air resistance coefficient.

[0096] When acceleration a(t)>0, the car consumes energy while driving, and the energy consumed is E Consume When the acceleration a(t) is less than 0, the car is recovering energy, and the recovered energy is E Recoverable , the braking energy recovery efficiency is η rb , respectively calculated as:

[0097]

[0098] From this, the energy consumption cost function can be established as:

[0099]

[0100] Where, v x (t),a x (t) are the lateral velocity and lateral acceleration of vehicle A at time t, v y (t),a y (t) are the longitudinal velocity and longitudinal acceleration of vehicle A at time t, E Recoverable (v(t), a(t)) is the energy consumed by vehicle A at time t, E Consume (v(t), a(t)) is the braking energy recovery of vehicle A at time t;

[0101] In addition, considering the impact of lane changing on the comfort of vehicle passengers, the comfort during lane changing is mainly related to the lateral acceleration of lane changing. The maximum lateral acceleration comfort threshold that can be accepted by passengers prone to dizziness is 0.203g. The maximum steering angle of the vehicle and other factors are added. The constraint conditions are shown in formula (7):

[0102]

[0103] Step 1.3: Optimize and calculate the trajectory of the first lane-changing process. The model thus obtains the optimal lane-changing trajectory.

[0104] The commercial software GPOPS2 can be used to solve the above cost function and obtain the actual time t of the first lane change process. c And the acceleration a(t) at each moment. Then, by integration, we can get the velocity v(t) and displacement s(t) at each moment.

[0105] 2. Planning of the first lane change trajectory.

[0106] When the vehicle reaches the transfer location, the second stage of planning begins. Since the calculation time of the quintic polynomial planning is only about 2ms, the influence of time can be ignored. Assume that there are two obstacle vehicles in the target lane II, namely B2 and B3. The lane change process is as follows: Figure 3 shown.

[0107] Step 2.1. Calculate the terminal state range of vehicle A in lane II based on the states of the front obstacle vehicle B2 and the rear obstacle vehicle B3 at the target position in lane II.

[0108] First, determine the conditions under which a lateral collision does not occur between vehicle A and vehicle B2:

[0109] d2+x B2 >x A2 (8)

[0110] Where d2 is the distance between vehicle A and vehicle B2 at the start of the second lane change, x B2 is the minimum lateral displacement of vehicle B2 during the second lane-changing process of vehicle A, x A2 is the maximum lateral displacement of vehicle A during the second lane-changing process.

[0111] Referring to the modeling method of the first planning stage, in order to avoid collision with the vehicle ahead, B2, we can assume that the ego vehicle is accelerating during the second lane change process, accelerating uniformly at maximum acceleration, while the vehicle ahead, B2, in the target lane, is decelerating at maximum deceleration of -2 m / s2 in the lateral lane II without changing lanes. The calculated x A2 and x B2 :

[0112]

[0113] Where, t e is the actual time of the second lane-changing process of vehicle A, a A 、v xc are the lateral acceleration of vehicle A during the entire lane-changing process and the lateral velocity at the end of the first lane-changing process, respectively. B2 、v B2 It is divided into the acceleration and speed of vehicle B2 during the second lane change process of vehicle A.

[0114] Then, the conditions for no lateral collision between vehicle A and vehicle B3 are also determined:

[0115] d3+x A3 >x B3 (11)

[0116] Where d3 is the distance between vehicle A and vehicle B3 at the start of the second lane change, x B3 is the maximum lateral displacement of vehicle B3 during the second lane-changing process of vehicle A, x A3 is the minimum lateral displacement of vehicle A during the second lane-changing process.

[0117] In order to avoid rear-end collision with the following vehicle, we can assume that the following vehicle B3 is accelerating during the lane change process, while the ego vehicle is decelerating uniformly during the lane change process. Then:

[0118]

[0119] Where a B3 、v B3 It is divided into the acceleration and speed of vehicle B3 during the second lane change process of vehicle A; among them, vehicle B3 is driving in the transverse lane II without changing lanes.

[0120] Then solve inequality (8) to get the maximum lane-changing time t2 of vehicle A in the second lane-changing process, and solve inequality (11) to get the minimum lane-changing time t3 of vehicle A in the second lane-changing process, and get the lane-changing time range t of vehicle A in the second lane-changing process. e ∈[t3,t2].

[0121] Finally, the maximum lane-changing time t2 of the second lane-changing process is substituted into equation (9), and the minimum lane-changing time t3 of the second lane-changing process is substituted into equation (12), and the state range of the vehicle at the end of lane-changing in lane II is obtained, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval:

[0122]

[0123] v xe ∈[v xc +a A t3,v xc +a A t2],v ye =0

[0124] Among them, D 车道 is the width of the lane, a A is the lateral acceleration of vehicle A during the entire lane changing process.

[0125] Step 2.2: Generate multiple trajectories using a quintic polynomial.

[0126] First, the lane-changing time range t of vehicle A in the second lane-changing process is e ∈[t3,t2], uniformly discretized into N time t e1 ,t e2 ,…,teN ; Set the horizontal position interval Uniformly discretized into M lateral positions s xe1 ,s xe2 ,…,s xeM .

[0127] Then, the position of vehicle A is expressed as a function of the lateral and longitudinal directions with respect to time t, and the following expressions are obtained:

[0128]

[0129] Where m and n represent the numbers of discrete time points and discrete horizontal positions, respectively, and m = 1, 2, ..., M; n = 1, 2, ..., N; s x (t), s y (t) represent the lateral position and longitudinal position of vehicle A at time t, respectively; a 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n The coefficients of the fifth-order polynomial representing the lateral position with respect to time t, b 0m×n 、b 1m×n 、b 2m×n 、b 3m×n 、b 4m×n 、b 5m×n The coefficients of the fifth-order polynomial representing the longitudinal position with respect to time t;

[0130] In addition, based on the kinematic constraint relationship between displacement, velocity, and acceleration, the function expressions of the velocity and acceleration of vehicle A in the lateral and longitudinal directions with respect to time t are obtained as follows:

[0131]

[0132] Where, v x (t), v y (t) represent the lateral velocity and longitudinal velocity of vehicle A at time t, respectively; a x (t), a y (t) denote the lateral acceleration and longitudinal acceleration of vehicle A at time t, respectively;

[0133] Then the state X of vehicle A at the starting point of the second lane-changing process is c =[s xc ,D 车 ,v xc ,v yc ,a xc ,a yc ] and the terminal state X e =[s xe ,D车道 ,v xe ,v ye ,a xe ,a ye ], and v ye =0,a xe =0,a ye =0, and substituting into formula (14) we get:

[0134]

[0135]

[0136] Where, let the time of the starting point of the second lane-changing process be t = 0; solve equations (17) and (18) to obtain the coefficient a in equation (14): 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n and coefficient b 0m×n 、b 1m×n 、b 2m×n 、b 3m×n 、b 4m×n 、b 5m×n , and then obtain the functional expressions of the position, velocity and acceleration of vehicle A in the second lane-changing process.

[0137] Finally, multiple trajectories are obtained based on the different values ​​of the numbers m and n of the discrete time points and discrete lateral positions.

[0138] Step 2.3: Consider multiple indicators and select the optimal trajectory as the trajectory for the second lane-changing process.

[0139] In this embodiment, the same cost function and constraints as those used in the first lane-changing trajectory optimal control problem are used. The weighting coefficients β1, β2, and β3 representing comfort, efficiency, and energy economy can be adjusted as needed. The optimal trajectory is selected from the multiple generated trajectories, i.e., the trajectory with the smallest cost function J.

[0140] During segmented trajectory planning, the requirements for the two trajectories are also different. For the first trajectory segment from the starting point to the transfer location, even though there is an impact of calculation time, since the vehicle continues to travel along the centerline of the lane at the beginning, considering the error in calculation time, the vehicle only continues to travel a small distance along the lane. This error can be eliminated through prediction, so the real-time requirements are not very stringent, and the main considerations are economy and comfort. For the second trajectory segment, since the vehicle is already on the lane lines of two adjacent lanes, if the planning algorithm calculation time is too long, it may cause interference with the target lane vehicle, poor safety, and make it difficult for the vehicle to travel according to the planned path. Therefore, the second trajectory planning needs to first consider real-time performance, and the algorithm used must have the advantage of fast calculation speed. In order to determine the trajectory planning method used for the first and second trajectories, the trajectory of the optimal control planning is compared with the quintic polynomial trajectory. Three sets of complete lane change data for different drivers at different lane change speeds were extracted from the highD dataset (Table 1). These data represent high, medium, and low speeds, respectively. Using these data as templates, the lane change start and end states were set. In quintic polynomial trajectory planning, the lane change time was discretized in steps of 0.1 seconds to generate multiple trajectory clusters. Trajectories with excessive acceleration that resulted in unsatisfactory comfort were removed from these clusters. The trajectory with the lowest energy consumption was then selected from the remaining clusters. This example uses a Nissan Leaf as an example, with a vehicle weight of 1521 kg, Af = 2.3316, a road slope of 5°, and a simulation device CPU of i5-11260H with 16GB of RAM.

[0141] Table 1. Lane-changing data

[0142]

[0143]

[0144] The simulation results are as follows Figure 5 、 Figure 6 As shown, from Figure 5 From the trajectory comparison chart, we can see that the optimal control trajectory is smoother than the trajectory of the real driver in the highD dataset and the trajectory of the fifth-order polynomial programming. At the same time, the lateral and longitudinal accelerations are also smaller, and the comfort is better. Figure 6 The energy consumption comparison chart shows that the optimal control trajectory consumes less energy than the actual driver trajectory and the quintic polynomial trajectory. The solution results for different scenarios are shown in Table 2.

[0145] Table 2. Obstacle-free scene solution results

[0146]

[0147] Through simulation verification under three working conditions, it can be found that the trajectory planned by optimal control takes less time to change lanes, requires shorter longitudinal displacement, consumes less energy, and is more comfortable than the trajectory planned by quintic polynomial. However, the calculation time of quintic polynomial planning is shorter, only about 7ms, and has very good real-time performance. Although the calculation time of optimal control planning is about 50ms, during high-speed lane changes, the displacement caused by the error in calculation time is only about 1m, and this distance error can be eliminated through calculation. Therefore, the present invention uses optimal control to plan the first segment of the trajectory and quintic polynomial planning for the second segment of the trajectory, which is feasible.

[0148] The above embodiments are preferred embodiments of the present application. Ordinary technicians in this field can also make various changes or improvements on this basis. Without departing from the overall concept of the present application, these changes or improvements should fall within the scope of protection required by the present application.

Claims

1. A vehicle segmented real-time lane-changing trajectory planning method, characterized in that: It is used to plan the trajectory of vehicle A changing lanes from lane I to lane II via lane line L, including: Plan the first lane-changing trajectory of vehicle A from lane I to lane line L: Calculate the state range of the transfer position of vehicle A on lane line L based on the state of vehicle A and its obstacle vehicle B1 in front of it on lane I; Then, based on the state of vehicle A at the starting point of lane change and the state range of the transfer position, model multiple indicators of the first lane-changing process as an optimal control problem, and optimize the calculation of the trajectory of the first lane-changing process; the state of the vehicle includes the lateral and longitudinal position and speed; When the vehicle arrives at the transfer position, the second lane-changing trajectory of vehicle A from lane line L to lane II is planned: the terminal state range of vehicle A in lane II is calculated according to the states of the front obstacle vehicle B2 and the rear obstacle vehicle B3 at the target position of lane II; then multiple trajectories are generated using a quintic polynomial, and the optimal trajectory is selected as the trajectory of the second lane-changing process by considering multiple indicators.

2. The vehicle segmented real-time lane change trajectory planning method according to claim 1, characterized in that: When modeling the optimal control problem to optimize the calculation of the first trajectory, and / or when selecting the second lane-changing trajectory using a quintic polynomial, the vehicle's energy consumption index is considered; the energy consumption index is calculated using the VT-CPEM energy consumption model.

3. The vehicle segmented real-time lane change trajectory planning method according to claim 1, characterized in that: Calculate the state range of the transfer position of vehicle A on lane line L, specifically: First, determine the conditions under which a lateral collision does not occur between vehicle A and vehicle B1: d1+x B1 >x A1 (1) Where d1 is the distance between vehicle A and vehicle B1 at the start of lane change, x B1 is the minimum lateral displacement of vehicle B1 during the first lane change process of vehicle A, x A1 is the maximum lateral displacement of vehicle A during the first lane change process, and: Where, t c is the actual time of the first lane-changing process of vehicle A, a A 、v x0 are the lateral acceleration and lateral initial velocity of vehicle A during the entire lane changing process, a B1 、v B1 It is divided into the acceleration and speed of vehicle B1 during the first lane change process of vehicle A; wherein vehicle B1 is traveling in the transverse lane I without changing lanes; Then, solving inequality (1) obtains the maximum lane-changing time t1 of vehicle A in the first lane-changing process; Substituting the maximum lane change time t1 into equation (2), we can obtain the state range of the vehicle's transfer position on the lane line L, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval: Among them, D 车 is the width of all vehicles, v ymax The maximum longitudinal speed of the vehicle when changing lanes.

4. The vehicle segmented real-time lane change trajectory planning method according to claim 3, characterized in that: The calculation of distance d1 takes into account the algorithm solution time of the first lane change trajectory planning, and the calculation formula is: d1=d0-(in x0 -v B1 )×t com (4) Where d0 is the distance between vehicle A and vehicle B1 at the beginning of lane change trajectory planning, t com The solution time of the algorithm for planning the first lane-changing trajectory.

5. The vehicle segmented real-time lane change trajectory planning method according to claim 3, characterized in that: The multiple indicators of the first lane-changing process are modeled as an optimal control problem, and the cost function is: In the formula, J represents the cost function, is the longitudinal acceleration of vehicle A at time t, a ymax is the maximum longitudinal acceleration of vehicle A, v(t), a(t) are the speed and acceleration of vehicle A at time t, E(v(t), a(t)) is the energy consumption cost of vehicle A, E max represents the maximum energy consumption, β1, β2, β3 represent the weighted coefficients of comfort, efficiency, and energy economy, and β1+β2+β3=1; where, In the formula, v x (t),a x (t) are the lateral velocity and lateral acceleration of vehicle A at time t, v y (t),a y (t) are the longitudinal velocity and longitudinal acceleration of vehicle A at time t, E Recoverable (v(t), a(t)) is the energy consumed by vehicle A at time t, E Consume (v(t), a(t)) is the braking recovery energy of vehicle A at time t; By solving the above cost function, we can get the actual time t of the first lane-changing process: c And the acceleration a(t) at each moment, and then the velocity v(t) and displacement s(t) at each moment are obtained by integration.

6. The vehicle segmented real-time lane change trajectory planning method according to claim 5, characterized in that: The constraints of the optimal control problem include:

7. The vehicle segmented real-time lane change trajectory planning method according to claim 6, characterized in that: The same cost function and constraints as those in the first lane-changing trajectory optimal control problem are used to select the optimal trajectory from the multiple generated trajectories.

8. The vehicle segmented real-time lane change trajectory planning method according to claim 1, characterized in that: Calculate the terminal state range of vehicle A in lane II, specifically: First, determine the conditions under which a lateral collision does not occur between vehicle A and vehicle B2: d2+x B2 >x A2 (8) Where d2 is the distance between vehicle A and vehicle B2 at the start of the second lane change, x B2 is the minimum lateral displacement of vehicle B2 during the second lane-changing process of vehicle A, x A2 is the maximum lateral displacement of vehicle A during the second lane-changing process, and: In the formula, t e is the actual time of the second lane-changing process of vehicle A, a A 、v xc are the lateral acceleration of vehicle A during the entire lane-changing process and the lateral velocity at the end of the first lane-changing process, respectively. B2 、v B2 It is divided into the acceleration and speed of vehicle B2 during the second lane change process of vehicle A; wherein vehicle B2 is traveling in the transverse lane II without changing lanes; Then, the conditions for no lateral collision between vehicle A and vehicle B3 are also determined: d3+x A3 >x B3 (11) Where d3 is the distance between vehicle A and vehicle B3 at the start of the second lane change, x B3 is the maximum lateral displacement of vehicle B3 during the second lane-changing process of vehicle A, x A3 is the minimum lateral displacement of vehicle A during the second lane change process, and: In the formula, a B3 、v B3 It is divided into the acceleration and speed of vehicle B3 during the second lane change process of vehicle A; wherein vehicle B3 is traveling in the transverse lane II without changing lanes; Then, solve inequality (8) to obtain the maximum lane-changing time t2 of vehicle A in the second lane-changing process, and solve inequality (11) to obtain the minimum lane-changing time t3 of vehicle A in the second lane-changing process, and obtain the lane-changing time range t e ∈[t3,t2]; Finally, the maximum lane-changing time t2 of the second lane-changing process is substituted into equation (9), and the minimum lane-changing time t3 of the second lane-changing process is substituted into equation (12), and the state range of the vehicle at the end position of lane II is obtained, including the lateral position interval, longitudinal position interval, lateral speed interval, and longitudinal speed interval: in xe ∈[in xc +a A t3,v xc +a A t2], in ye =0 Among them, D 车道 is the width of the lane, a A is the lateral acceleration of vehicle A during the entire lane changing process.

9. The vehicle segmented real-time lane change trajectory planning method according to claim 8, characterized in that: The method of generating multiple trajectories using a quintic polynomial is: The lane-changing time range t of vehicle A in the second lane-changing process e ∈[t3,t2], uniformly discretized into N time t e1 ,t e2 ,…,t eN ; Set the horizontal position interval Uniformly discretized into M lateral positions s xe1 ,s xe2 ,…,s xeM ; Expressing the position of vehicle A as a function of the lateral and longitudinal directions with respect to time t, the following expressions are obtained: Where m and n represent the numbers of discrete time points and discrete horizontal positions, respectively, and m = 1, 2, ..., M; n = 1, 2, ..., N; s x (t), s y (t) represent the lateral position and longitudinal position of vehicle A at time t, respectively; a 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n The coefficients of the fifth-order polynomial representing the lateral position with respect to time t, b 0m×n , b 1m×n , b 2m×n , b 3m×n , b 4m×n , b 5m×n The coefficients of the fifth-order polynomial representing the longitudinal position with respect to time t; Based on the kinematic constraint relationship between displacement, velocity, and acceleration, the function expressions of the velocity and acceleration of vehicle A in the lateral and longitudinal directions with respect to time t are obtained as follows: In the formula, v x (t), v y (t) represent the lateral speed and longitudinal speed of vehicle A at time t, respectively; a x (t), a y (t) represent the lateral acceleration and longitudinal acceleration of vehicle A at time t, respectively; The state X of vehicle A at the starting point of the second lane change process is c =[s xc ,D 车 ,v xc ,v yc ,a xc ,a yc ] and the state X of the end point e =[s xe ,D 车道 ,v xe ,v ye ,a xe ,a ye ], and v ye =0,a xe =0,a ye =0, substituting into formula (14) we get: Wherein, the time of the starting point of the second lane-changing process is assumed to be t=0; Solving equations (17) and (18) yields the coefficient a in equation (14): 0m×n 、a 1m×n 、a 2m×n 、a 3m×n 、a 4m×n 、a 5m×n and coefficient b 0m×n , b 1m×n , b 2m×n , b 3m×n , b 4m×n , b 5m×n , and then obtain the functional expressions of the position, velocity and acceleration of vehicle A in the second lane-changing process; Finally, multiple trajectories are obtained based on different values ​​of the numbers m and n of the discrete time points and discrete lateral positions.

Images