# A Universal Trajectory Planning Method for Automated Lane-Changing and Overtaking Maneuvers

Authors: Ying Wang; Chong Wei
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

Lane-changing and overtaking are conventional maneuvers on roads, and the reference trajectory is one of the prerequisites to execute these maneuvers. This study proposes a universal trajectory planning method for automated lane-changing and overtaking maneuvers, in which the trajectory is regarded as the combination of a path and its traffic state profiles. The two-dimensional path is represented by a suitable curve to connect the initial position with final position of the ego vehicle. Based on the planned path, its traffic state profiles are generated by solving a nonlinear mathematical optimization model. Moreover, the study discretizes the time horizon into several time intervals and determines the parameters to obtain the continuous and smooth profiles, which guarantees the safety and comfort of the ego vehicle. Finally, a series of simulation experiments are performed in the MATLAB platform and the results show the feasibility and effectiveness of the proposed universal trajectory planning method.


---

## Body

## 1. Introduction

Automated driving system can significantly alleviate traffic congestion, ensure traffic safety, and improve road capacity [1–4]. In order to realize the intelligent vehicle as soon as possible, some automated driving technologies are being tested on roads. Among the test items, the lane-changing and overtaking maneuvers are included [5–8]. To execute these maneuvers automatically, a feasible reference trajectory is needed. The ego vehicle will drive along the planned trajectory to finish the relevant maneuvers. This paper proposes a novel universal trajectory planning method for the automated lane-changing and overtaking maneuvers. With the development of advanced sensors and vehicle-to-vehicle (V2V) communications, real-time traffic information of vehicles, such as position, speed, and acceleration, can be obtained conveniently and accurately [9–12], and the trajectory is planned based on these traffic state information.Different types of curves have different performance characteristics in terms of continuity and smoothness. Therefore, selecting a suitable curve function to represent the trajectory according to the traffic condition is important. In the relevant trajectory planning studies, the commonly used geometric curves include the spline curve [13–17], trapezoidal acceleration curve [18–22], Bezier curve [23–26], and polynomial curve [27–33].Ziegler et al. [13] employed a quantic spline curve to generate the trajectory. They divided the space into multiple geometric graphs and adopted the shortest path algorithm to search for the feasible trajectory for each graph. Rousseau et al. [17] utilized the trajectory planning method based on the B-spline curve. The parameters of curve function were determined by minimizing the driving time. Yin et al. [18] described a trapezoidal acceleration motion planning model. The planned trajectory could avoid the potential collisions with obstacles by analyzing the variation law of the actuator. Chen et al. [22] presented a trajectory planning method using the 3D Bezier curve, which enhanced the flexibility of trajectory and guaranteed the conformity to the realistic driving maneuver.Among the commonly used geometric curves, the polynomial curve is the most widely used in trajectory planning. This type of curve can plan the smooth trajectory with a low computational cost, and the order of polynomial can be tuned for obtaining the desired trajectory performance. Nelson et al. [27] were the first to propose the polynomial method. They used the polynomial curve to replace the arc curve for planning the continuous-curvature trajectory, which guaranteed the traceability of the trajectory. Nilsson et al. [29] presented a trajectory planning model using the discrete quadratic polynomial curve. They divided the planning area into three regions (i.e., preregion, periregion, and postregion) and solved the optimal positions of the ego vehicle in each region to determine the trajectory function. Zhang et al. [30] adopted two time-dependent cubic polynomial functions to describe the longitudinal and lateral motions, respectively. They concluded that the continuity of the polynomial curve could ensure the robustness of trajectory. Yang et al. [32] proposed a trajectory planning model based on the cubic polynomial curve. They focused on the trajectory replanning problem in dynamic driving environments. Wei et al. [33] increased the polynomial function to the fifth order and solved the optimal trajectory by treating the driving time and movement distance as free variables.However, there are still several common disadvantages existing in the previous studies. First, these trajectory planning models usually only focus on the automated lane-changing or overtaking maneuver, while neglecting the universality of the model for lane-changing and overtaking maneuvers. Second, the traffic states of the ego vehicle solved by the previous model usually were discretized, which might result in hard tracking or even crashes. Third, most of the studies designed the lane-changing or overtaking scenarios simply, in which they assumed that the surrounding environment was static, the speeds of other vehicles were constants, or only one or two vehicles were around the ego vehicle. These assumptions were inconsistent with the real-world traffic characteristics.Aiming at these problems, a trajectory planning method based on the mathematical optimization framework is proposed in this paper. The contributions can be summarized as follows:(1)
A universal framework of trajectory planning for the automated lane-changing and overtaking maneuvers is proposed. The study considers that the combination of a path and its traffic state profiles determines a complete spatiotemporal trajectory. The suitable curve is employed to represent the two-dimensional path. And then a nonlinear optimization model is established to generate the traffic state profiles, whose computational cost is low since the path has been planned.(2)
The traffic state profiles are continuous and smooth. In previous studies, the traffic states of the ego vehicle solved by the nonlinear model usually were discretized. Our proposed model discretizes the time horizon into several time intervals and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles.The rest of the paper is organized as follows. Section2 introduces the framework of the universal trajectory planning method. Section 3 describes the technical details of the proposed method, including the generation process of the path and traffic state profiles. Section 4 presents the simulation results and several typical numerical examples. Section 5 concludes the study and discusses the future work.


## 2. Framework of the Universal Trajectory Planning Method

Figure1 presents the framework of the proposed universal trajectory planning method, which plans the trajectory through four stages. In stage 1, the current traffic states of the vehicles are collected via GPS, digital maps, and sensors. In stage 2, based on the known boundary conditions, a suitable curve is employed to generate the two-dimensional path connecting the initial position and the final position. In stage 3, the objective function and relevant constraints are established to solve the traffic state profiles along the path, such as speed, acceleration, and jerk profiles. The combination of the path and its traffic state profiles determines a complete spatiotemporal trajectory. In stage 4, the generated trajectory parameters are used as inputs for the actuators to execute the desired control.Figure 1
Framework of the trajectory planning method.


## 3. The Universal Trajectory Planning Method

Planning a reference trajectory before changing lane or overtaking is necessary and important. The quality of the trajectory has a direct impact on the performance of the automated driving behaviors. This study considers that the combination of a path and its traffic state profiles determines a complete spatiotemporal trajectory. Therefore, in the following sections, we will describe the universal generation method of path and traffic state profiles in detail.


### 3.1. Path Generation

A suitable curve should be selected to plan the lane-changing or overtaking path. Here, we choose the quintic polynomial, which has the advantages of a continuous third derivative and smooth curvature, as shown in the following equation:(1)yx=a0+a1x+a2x2+a3x3+a4x4+a5x5,where x and y are the longitudinal and lateral positions of the ego vehicle, respectively. a0, …, a5 are unknown parameters that need to be calculated.As for the lane-changing maneuver, the boundary traffic information can be collected with the help of sensors, GPS, and digital maps. Based on such information, the following equations can be obtained:(2)y0=a0+a1x0+a2x02+a3x03+a4x04+a5x05,(3)yf=a0+a1xf+a2xf2+a3xf3+a4xf4+a5xf5,(4)c0=a1+2a2x0+3a3x02+4a4x03+5a5x04,(5)cf=a1+2a2xf+3a3xf2+4a4xf3+5a5xf4,(6)k0=2a2+6a3x0+12a4x02+20a5x03,(7)kf=2a2+6a3xf+12a4xf2+20a5xf3,where x0, y0, c0, and k0 represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at current moment, respectively. xf, yf, cf, and kf represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at final moment, respectively. By solving equations (2)–(7), the values of a0, …, a5 are obtained, and then the lane-changing path can be generated.As for overtaking maneuver, the same method as lane-changing is adopted to generate the path. A complete overtaking path is composed of two lane-changing paths, as shown in Figure2. In the figure, vehicle 0 is the ego vehicle and vehicles 1, 2, and 3 are the surrounding vehicles.Figure 2
Overtaking path of the ego vehicle.After collecting the boundary traffic information of the ego vehicle in lane-changing or overtaking, the unknown parameters of the path function can be solved and then the suitable path is generated. The study chooses the polynomial curve to represent the path, which has the advantages of a continuous derivative and smooth curvature. Other types of curve can also be employed according to the traffic scenario, while the framework of the path generation is unchanged.


### 3.2. Traffic State Profile Generation

In this section, a nonlinear programming model is constructed to generate the traffic state profiles (speed, acceleration, and jerk profiles) for the automated lane-changing or overtaking maneuver. Since the discrete traffic states of the ego vehicle may result in crashes, our model discretizes the time horizon and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. The driving distance profile is shown in the following equation:(8)st=b0+b1t+b2t2+b3t3+b4t4+b5t5.The corresponding speed, acceleration, and jerk profiles are shown in the following equations:(9)vt=s˙t=b1+2b2t+3b3t2+4b4t3+5b5t4,(10)at=s¨t=2b2+6b3t+12b4t2+20b5t3,(11)jt=s…t=6b3+24b4t+60b5t2.Six unknown parametersb0, …, b5 need to be determined. Based on the known initial traffic states of the ego vehicle, three equations are derived as follows:(12)s0=s0,0,v0=v0,0,a0=a0,0,where s0,0, v0,0, and a0,0 are the current driving distance, speed, and acceleration of the ego vehicle, respectively. The values of b0, b1, and b2 can be obtained by solving equation (12). However, the unknown parameters b3, b4, and b5 and total driving time tf are not easy to determine, and the arbitrary values may affect the solution efficiency of the model [29, 34]. Therefore, we treat b3, b4, b5, and tf as free variables and establish a nonlinear model to search for the optimal traffic state profiles.The study dividestf into I time intervals and uses tii=1,…,I to denote the end moment of each time interval. Let t0=0 and set constraint ti>ti−1. As shown in Figure 3, si is the driving distance of the ego vehicle at ti. x0,i and y0,i are the longitudinal and lateral positions of the ego vehicle at ti, respectively. s0=0 and si is calculated as ∫0x0,i1+dy/dy2dx. Furthermore, ti for i=1,…,I are treated as free variables. The unknown parameters b3, b4, and b5 and driving time tf can be represented as functions of ti. Therefore, the planning traffic state profile problem can be converted into the optimization problem.Figure 3
Generation of the traffic state profiles: (a) lane-changing maneuver; (b) overtaking maneuver.
(a)
(b)


#### 3.2.1. Optimal Traffic State Profile Planning

The objective function of the constrained nonlinear optimization model is shown in equation (13). a0,i and j0,i denote the acceleration and jerk of the ego vehicle at ti, respectively, and tf denotes the total driving time:(13)minJ=1I∑i=1Ia0,i2+1I∑i=1Ij0,i2+tf2.During the lane-changing or overtaking process, the passengers will feel comfortable when the accelerations and jerks of the ego vehicle are small, soa0,i and j0,i are used to reflect the comfort in equation (13). The total driving time tf can represent the efficiency. Short time indicates high efficiency.


#### 3.2.2. Collision-Avoidance Constraint

To avoid the potential collisions, the ego vehicle should maintain sufficient distance from the surrounding vehicles during lane-changing or overtaking maneuver. For illustrative purposes, the study assumes that the vehicles drive along the straight roads. This limitation can be easily removed by introducing a curved road function. As shown in Figure4, there are three vehicles around the ego vehicle (vehicle 0). The leading vehicle on the current lane is indexed as vehicle 1, the following vehicle on the target lane is indexed as vehicle 2, and the leading vehicle on the target lane is indexed as vehicle 3.Figure 4
Relative positions of the vehicles.The safe distance between the ego vehicle and vehicle 1 is discussed first. As shown in Figure5, the ego vehicle is simplified a geometry of several circles with diameter m. Following Ziegler et al. [35], the Euclidean distance between the center of each circle of the ego vehicle and the center of the first circle of vehicle 1 should be greater than diameter m of the circle. Therefore, the distance constraint between the ego vehicle (vehicle 0) and vehicle 1 can be shown in the following equation:(14)m2≤x0,ik−x1,i12+y0,ik−y1,i12,fork=1,…,5,fori=1,…,I,where k is used to denote the index of circles; x0,ik and y0,ik represent the positions of the ego vehicle of the k-th circle at ti; and x1,i1 and y1,i1 represent the positions of the first circle of vehicle 1 at ti, whose values can be solved from (x1,i,y1,i). x1,i is calculated as x1,0+v1,0ti+1/2∗a1,0ti2, and y1,i is 0m on the straight lane.Figure 5
Mechanism of the collision-avoidance constraint.Then, the constraints of the collision avoidance between the ego vehicle and the other two vehicles are discussed. As shown in equations (15)-(16), the Euclidean distance between the ego vehicle and vehicles 2 and 3 should be greater than the diagonal length r of the vehicle:(15)r2≤x0,i−x2,i2+y0,i−y2,i2,fori=1,…,I,(16)r2≤x0,i−x3,i2+y0,i−y3,i2,fori=1,…,I,where x0,i, x2,i, and x3,i are the longitudinal central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively. y0,i, y2,i, and y3,i are the lateral central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively.x
0
,
i and y0,i are the variables that need to be solved in the model. x2,i and x3,i can be calculated as x2,0+v2,0ti+1/2∗a2,0ti2 and x3,0+v3,0ti+1/2∗a3,0ti2, respectively; y2,i and y3,i are the lane width 3.75m. Here, the initial traffic states of relevant vehicles are directly derived from the sensors.


#### 3.2.3. Acceleration Constraint Based on Car-following Maneuver

In real-world traffic environment, the ego vehicle usually shifts to a car-following maneuver at the end of the lane-changing or overtaking process. To guarantee a comfort driving behavior, the planned final accelerationa0,I of the ego vehicle should be smaller than the car-following acceleration, as shown in the following equation:(17)a0,I≤Av0,I,v3,I,d0,3,where Av0,I,v3,I,d0,3 denotes the car-following acceleration of the ego vehicle at tI; v0,I and v3,I denote the speeds of the ego vehicle and vehicle 3 at tI, respectively; and d0,3 denotes the distance between the ego vehicle and vehicle 3 at tI.Here,v0,I is solved by the proposed nonlinear model; v3,I and d0,3 are calculated as follows, where l is the length of the vehicle:(18)v3,I=v3,0+a3,0ti,d0,3=x3,I−x0,I−l.Whena0,I is smaller than the car-following acceleration, the ego vehicle will shift from a lane-changing or overtaking maneuver to a car-following maneuver comfortably, i.e., no accelerate or decrease immediately.We choose a linear dynamic model [36] as the underlying car-following model. The car-following acceleration of the ego vehicle Av0,I,v3,I,d0,3 can be given as follows:(19)Av0,I,v3,I,d0,3=w1d0,3−Δs∗+w2v3,I−v0,I,where w1 and w2 denote the variation rate of the distance gap and the variation rate of the speed gap between the ego vehicle and vehicle 3, respectively; Δs∗ denotes the ideal distance gap between the ego vehicle and vehicle 3. The function of the three parameters is to help the ego vehicle reach the equilibrium condition as soon as possible by adjusting its acceleration. Following Pariota et al. [36], the parameters in the model are set as w1=0.0343,w2=0.948, and Δs∗=30.


#### 3.2.4. Traffic State Constraint

The speedv0,i, acceleration a0,i, and jerk j0,i of the ego vehicle at ti can be obtained according to equations (9)–(11). To ensure the driving safety and comfort, the total lane-changing or overtaking time and traffic states of the ego vehicle should be simultaneously constrained as follows:(20)0<tf<tf,max,vmin<v0,i<vmax,fori=1,…,I,amin<a0,i<amax,fori=1,…,I,jmin<j0,i<jmax,fori=1,…,I,where tf,max, vmax, amax, and jmax denote the allowable maximum total time, speed, acceleration, and jerk, respectively. vmin, amin, and jmin denote the allowable minimum speed, acceleration, and jerk, respectively.In the above constraints, the speed of the ego vehicle should always be positive and not exceed the maximum allowable speed, i.e., no stop or backward motion. Moreover, the smaller the jerk, the smaller the acceleration variation, and the higher the driving comfort degree [37].


### 3.3. Trajectory Planning Based on the Nonlinear Model

After receiving the lane-changing or overtaking decision, our proposed trajectory planning model will work. The steps are as follows:(1)
Traffic information sensingCollecting the current traffic information of the relevant vehicles through the advanced sensors, GPS, and digital maps.(2)
Path generationBased on the collected traffic information, using the quintic polynomial curve to represent the lane-changing or overtaking path, the unknown parameters of the path function can be solved according to the boundary conditions.(3)
Traffic state profile generationConsidering the safety and comfort constraints, establishing a nonlinear optimization model to solve the optimal traffic state profiles of the ego vehicle along the path.(4)
Lane-changing or overtaking trajectory planningIf a feasible solution is found for the proposed nonlinear optimization model, the lane-changing or overtaking trajectory of the ego vehicle can be planned. However, if there is no feasible solution, the current lane will continue to be used, and the planner will iterate the above three steps until a feasible trajectory is planned.The sequence quadratic programming (SQP) algorithm is selected to solve the nonlinear optimization model. Previous studies [19, 33, 34] prove that this algorithm can find the feasible solution for the nonlinear problem well. Through multiple iterations, the optimal traffic state profiles of the ego vehicle are acquired based on the SQP algorithm. Therefore, the optimal lane-changing or overtaking trajectory can be planned.


## 3.1. Path Generation

A suitable curve should be selected to plan the lane-changing or overtaking path. Here, we choose the quintic polynomial, which has the advantages of a continuous third derivative and smooth curvature, as shown in the following equation:(1)yx=a0+a1x+a2x2+a3x3+a4x4+a5x5,where x and y are the longitudinal and lateral positions of the ego vehicle, respectively. a0, …, a5 are unknown parameters that need to be calculated.As for the lane-changing maneuver, the boundary traffic information can be collected with the help of sensors, GPS, and digital maps. Based on such information, the following equations can be obtained:(2)y0=a0+a1x0+a2x02+a3x03+a4x04+a5x05,(3)yf=a0+a1xf+a2xf2+a3xf3+a4xf4+a5xf5,(4)c0=a1+2a2x0+3a3x02+4a4x03+5a5x04,(5)cf=a1+2a2xf+3a3xf2+4a4xf3+5a5xf4,(6)k0=2a2+6a3x0+12a4x02+20a5x03,(7)kf=2a2+6a3xf+12a4xf2+20a5xf3,where x0, y0, c0, and k0 represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at current moment, respectively. xf, yf, cf, and kf represent the longitudinal position, the lateral position, the derivative, and the curvature of the lane curve at final moment, respectively. By solving equations (2)–(7), the values of a0, …, a5 are obtained, and then the lane-changing path can be generated.As for overtaking maneuver, the same method as lane-changing is adopted to generate the path. A complete overtaking path is composed of two lane-changing paths, as shown in Figure2. In the figure, vehicle 0 is the ego vehicle and vehicles 1, 2, and 3 are the surrounding vehicles.Figure 2
Overtaking path of the ego vehicle.After collecting the boundary traffic information of the ego vehicle in lane-changing or overtaking, the unknown parameters of the path function can be solved and then the suitable path is generated. The study chooses the polynomial curve to represent the path, which has the advantages of a continuous derivative and smooth curvature. Other types of curve can also be employed according to the traffic scenario, while the framework of the path generation is unchanged.


## 3.2. Traffic State Profile Generation

In this section, a nonlinear programming model is constructed to generate the traffic state profiles (speed, acceleration, and jerk profiles) for the automated lane-changing or overtaking maneuver. Since the discrete traffic states of the ego vehicle may result in crashes, our model discretizes the time horizon and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. The driving distance profile is shown in the following equation:(8)st=b0+b1t+b2t2+b3t3+b4t4+b5t5.The corresponding speed, acceleration, and jerk profiles are shown in the following equations:(9)vt=s˙t=b1+2b2t+3b3t2+4b4t3+5b5t4,(10)at=s¨t=2b2+6b3t+12b4t2+20b5t3,(11)jt=s…t=6b3+24b4t+60b5t2.Six unknown parametersb0, …, b5 need to be determined. Based on the known initial traffic states of the ego vehicle, three equations are derived as follows:(12)s0=s0,0,v0=v0,0,a0=a0,0,where s0,0, v0,0, and a0,0 are the current driving distance, speed, and acceleration of the ego vehicle, respectively. The values of b0, b1, and b2 can be obtained by solving equation (12). However, the unknown parameters b3, b4, and b5 and total driving time tf are not easy to determine, and the arbitrary values may affect the solution efficiency of the model [29, 34]. Therefore, we treat b3, b4, b5, and tf as free variables and establish a nonlinear model to search for the optimal traffic state profiles.The study dividestf into I time intervals and uses tii=1,…,I to denote the end moment of each time interval. Let t0=0 and set constraint ti>ti−1. As shown in Figure 3, si is the driving distance of the ego vehicle at ti. x0,i and y0,i are the longitudinal and lateral positions of the ego vehicle at ti, respectively. s0=0 and si is calculated as ∫0x0,i1+dy/dy2dx. Furthermore, ti for i=1,…,I are treated as free variables. The unknown parameters b3, b4, and b5 and driving time tf can be represented as functions of ti. Therefore, the planning traffic state profile problem can be converted into the optimization problem.Figure 3
Generation of the traffic state profiles: (a) lane-changing maneuver; (b) overtaking maneuver.
(a)
(b)


### 3.2.1. Optimal Traffic State Profile Planning

The objective function of the constrained nonlinear optimization model is shown in equation (13). a0,i and j0,i denote the acceleration and jerk of the ego vehicle at ti, respectively, and tf denotes the total driving time:(13)minJ=1I∑i=1Ia0,i2+1I∑i=1Ij0,i2+tf2.During the lane-changing or overtaking process, the passengers will feel comfortable when the accelerations and jerks of the ego vehicle are small, soa0,i and j0,i are used to reflect the comfort in equation (13). The total driving time tf can represent the efficiency. Short time indicates high efficiency.


### 3.2.2. Collision-Avoidance Constraint

To avoid the potential collisions, the ego vehicle should maintain sufficient distance from the surrounding vehicles during lane-changing or overtaking maneuver. For illustrative purposes, the study assumes that the vehicles drive along the straight roads. This limitation can be easily removed by introducing a curved road function. As shown in Figure4, there are three vehicles around the ego vehicle (vehicle 0). The leading vehicle on the current lane is indexed as vehicle 1, the following vehicle on the target lane is indexed as vehicle 2, and the leading vehicle on the target lane is indexed as vehicle 3.Figure 4
Relative positions of the vehicles.The safe distance between the ego vehicle and vehicle 1 is discussed first. As shown in Figure5, the ego vehicle is simplified a geometry of several circles with diameter m. Following Ziegler et al. [35], the Euclidean distance between the center of each circle of the ego vehicle and the center of the first circle of vehicle 1 should be greater than diameter m of the circle. Therefore, the distance constraint between the ego vehicle (vehicle 0) and vehicle 1 can be shown in the following equation:(14)m2≤x0,ik−x1,i12+y0,ik−y1,i12,fork=1,…,5,fori=1,…,I,where k is used to denote the index of circles; x0,ik and y0,ik represent the positions of the ego vehicle of the k-th circle at ti; and x1,i1 and y1,i1 represent the positions of the first circle of vehicle 1 at ti, whose values can be solved from (x1,i,y1,i). x1,i is calculated as x1,0+v1,0ti+1/2∗a1,0ti2, and y1,i is 0m on the straight lane.Figure 5
Mechanism of the collision-avoidance constraint.Then, the constraints of the collision avoidance between the ego vehicle and the other two vehicles are discussed. As shown in equations (15)-(16), the Euclidean distance between the ego vehicle and vehicles 2 and 3 should be greater than the diagonal length r of the vehicle:(15)r2≤x0,i−x2,i2+y0,i−y2,i2,fori=1,…,I,(16)r2≤x0,i−x3,i2+y0,i−y3,i2,fori=1,…,I,where x0,i, x2,i, and x3,i are the longitudinal central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively. y0,i, y2,i, and y3,i are the lateral central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively.x
0
,
i and y0,i are the variables that need to be solved in the model. x2,i and x3,i can be calculated as x2,0+v2,0ti+1/2∗a2,0ti2 and x3,0+v3,0ti+1/2∗a3,0ti2, respectively; y2,i and y3,i are the lane width 3.75m. Here, the initial traffic states of relevant vehicles are directly derived from the sensors.


### 3.2.3. Acceleration Constraint Based on Car-following Maneuver

In real-world traffic environment, the ego vehicle usually shifts to a car-following maneuver at the end of the lane-changing or overtaking process. To guarantee a comfort driving behavior, the planned final accelerationa0,I of the ego vehicle should be smaller than the car-following acceleration, as shown in the following equation:(17)a0,I≤Av0,I,v3,I,d0,3,where Av0,I,v3,I,d0,3 denotes the car-following acceleration of the ego vehicle at tI; v0,I and v3,I denote the speeds of the ego vehicle and vehicle 3 at tI, respectively; and d0,3 denotes the distance between the ego vehicle and vehicle 3 at tI.Here,v0,I is solved by the proposed nonlinear model; v3,I and d0,3 are calculated as follows, where l is the length of the vehicle:(18)v3,I=v3,0+a3,0ti,d0,3=x3,I−x0,I−l.Whena0,I is smaller than the car-following acceleration, the ego vehicle will shift from a lane-changing or overtaking maneuver to a car-following maneuver comfortably, i.e., no accelerate or decrease immediately.We choose a linear dynamic model [36] as the underlying car-following model. The car-following acceleration of the ego vehicle Av0,I,v3,I,d0,3 can be given as follows:(19)Av0,I,v3,I,d0,3=w1d0,3−Δs∗+w2v3,I−v0,I,where w1 and w2 denote the variation rate of the distance gap and the variation rate of the speed gap between the ego vehicle and vehicle 3, respectively; Δs∗ denotes the ideal distance gap between the ego vehicle and vehicle 3. The function of the three parameters is to help the ego vehicle reach the equilibrium condition as soon as possible by adjusting its acceleration. Following Pariota et al. [36], the parameters in the model are set as w1=0.0343,w2=0.948, and Δs∗=30.


### 3.2.4. Traffic State Constraint

The speedv0,i, acceleration a0,i, and jerk j0,i of the ego vehicle at ti can be obtained according to equations (9)–(11). To ensure the driving safety and comfort, the total lane-changing or overtaking time and traffic states of the ego vehicle should be simultaneously constrained as follows:(20)0<tf<tf,max,vmin<v0,i<vmax,fori=1,…,I,amin<a0,i<amax,fori=1,…,I,jmin<j0,i<jmax,fori=1,…,I,where tf,max, vmax, amax, and jmax denote the allowable maximum total time, speed, acceleration, and jerk, respectively. vmin, amin, and jmin denote the allowable minimum speed, acceleration, and jerk, respectively.In the above constraints, the speed of the ego vehicle should always be positive and not exceed the maximum allowable speed, i.e., no stop or backward motion. Moreover, the smaller the jerk, the smaller the acceleration variation, and the higher the driving comfort degree [37].


## 3.2.1. Optimal Traffic State Profile Planning

The objective function of the constrained nonlinear optimization model is shown in equation (13). a0,i and j0,i denote the acceleration and jerk of the ego vehicle at ti, respectively, and tf denotes the total driving time:(13)minJ=1I∑i=1Ia0,i2+1I∑i=1Ij0,i2+tf2.During the lane-changing or overtaking process, the passengers will feel comfortable when the accelerations and jerks of the ego vehicle are small, soa0,i and j0,i are used to reflect the comfort in equation (13). The total driving time tf can represent the efficiency. Short time indicates high efficiency.


## 3.2.2. Collision-Avoidance Constraint

To avoid the potential collisions, the ego vehicle should maintain sufficient distance from the surrounding vehicles during lane-changing or overtaking maneuver. For illustrative purposes, the study assumes that the vehicles drive along the straight roads. This limitation can be easily removed by introducing a curved road function. As shown in Figure4, there are three vehicles around the ego vehicle (vehicle 0). The leading vehicle on the current lane is indexed as vehicle 1, the following vehicle on the target lane is indexed as vehicle 2, and the leading vehicle on the target lane is indexed as vehicle 3.Figure 4
Relative positions of the vehicles.The safe distance between the ego vehicle and vehicle 1 is discussed first. As shown in Figure5, the ego vehicle is simplified a geometry of several circles with diameter m. Following Ziegler et al. [35], the Euclidean distance between the center of each circle of the ego vehicle and the center of the first circle of vehicle 1 should be greater than diameter m of the circle. Therefore, the distance constraint between the ego vehicle (vehicle 0) and vehicle 1 can be shown in the following equation:(14)m2≤x0,ik−x1,i12+y0,ik−y1,i12,fork=1,…,5,fori=1,…,I,where k is used to denote the index of circles; x0,ik and y0,ik represent the positions of the ego vehicle of the k-th circle at ti; and x1,i1 and y1,i1 represent the positions of the first circle of vehicle 1 at ti, whose values can be solved from (x1,i,y1,i). x1,i is calculated as x1,0+v1,0ti+1/2∗a1,0ti2, and y1,i is 0m on the straight lane.Figure 5
Mechanism of the collision-avoidance constraint.Then, the constraints of the collision avoidance between the ego vehicle and the other two vehicles are discussed. As shown in equations (15)-(16), the Euclidean distance between the ego vehicle and vehicles 2 and 3 should be greater than the diagonal length r of the vehicle:(15)r2≤x0,i−x2,i2+y0,i−y2,i2,fori=1,…,I,(16)r2≤x0,i−x3,i2+y0,i−y3,i2,fori=1,…,I,where x0,i, x2,i, and x3,i are the longitudinal central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively. y0,i, y2,i, and y3,i are the lateral central positions of the ego vehicle, vehicle 2, and vehicle 3 at ti, respectively.x
0
,
i and y0,i are the variables that need to be solved in the model. x2,i and x3,i can be calculated as x2,0+v2,0ti+1/2∗a2,0ti2 and x3,0+v3,0ti+1/2∗a3,0ti2, respectively; y2,i and y3,i are the lane width 3.75m. Here, the initial traffic states of relevant vehicles are directly derived from the sensors.


## 3.2.3. Acceleration Constraint Based on Car-following Maneuver

In real-world traffic environment, the ego vehicle usually shifts to a car-following maneuver at the end of the lane-changing or overtaking process. To guarantee a comfort driving behavior, the planned final accelerationa0,I of the ego vehicle should be smaller than the car-following acceleration, as shown in the following equation:(17)a0,I≤Av0,I,v3,I,d0,3,where Av0,I,v3,I,d0,3 denotes the car-following acceleration of the ego vehicle at tI; v0,I and v3,I denote the speeds of the ego vehicle and vehicle 3 at tI, respectively; and d0,3 denotes the distance between the ego vehicle and vehicle 3 at tI.Here,v0,I is solved by the proposed nonlinear model; v3,I and d0,3 are calculated as follows, where l is the length of the vehicle:(18)v3,I=v3,0+a3,0ti,d0,3=x3,I−x0,I−l.Whena0,I is smaller than the car-following acceleration, the ego vehicle will shift from a lane-changing or overtaking maneuver to a car-following maneuver comfortably, i.e., no accelerate or decrease immediately.We choose a linear dynamic model [36] as the underlying car-following model. The car-following acceleration of the ego vehicle Av0,I,v3,I,d0,3 can be given as follows:(19)Av0,I,v3,I,d0,3=w1d0,3−Δs∗+w2v3,I−v0,I,where w1 and w2 denote the variation rate of the distance gap and the variation rate of the speed gap between the ego vehicle and vehicle 3, respectively; Δs∗ denotes the ideal distance gap between the ego vehicle and vehicle 3. The function of the three parameters is to help the ego vehicle reach the equilibrium condition as soon as possible by adjusting its acceleration. Following Pariota et al. [36], the parameters in the model are set as w1=0.0343,w2=0.948, and Δs∗=30.


## 3.2.4. Traffic State Constraint

The speedv0,i, acceleration a0,i, and jerk j0,i of the ego vehicle at ti can be obtained according to equations (9)–(11). To ensure the driving safety and comfort, the total lane-changing or overtaking time and traffic states of the ego vehicle should be simultaneously constrained as follows:(20)0<tf<tf,max,vmin<v0,i<vmax,fori=1,…,I,amin<a0,i<amax,fori=1,…,I,jmin<j0,i<jmax,fori=1,…,I,where tf,max, vmax, amax, and jmax denote the allowable maximum total time, speed, acceleration, and jerk, respectively. vmin, amin, and jmin denote the allowable minimum speed, acceleration, and jerk, respectively.In the above constraints, the speed of the ego vehicle should always be positive and not exceed the maximum allowable speed, i.e., no stop or backward motion. Moreover, the smaller the jerk, the smaller the acceleration variation, and the higher the driving comfort degree [37].


## 3.3. Trajectory Planning Based on the Nonlinear Model

After receiving the lane-changing or overtaking decision, our proposed trajectory planning model will work. The steps are as follows:(1)
Traffic information sensingCollecting the current traffic information of the relevant vehicles through the advanced sensors, GPS, and digital maps.(2)
Path generationBased on the collected traffic information, using the quintic polynomial curve to represent the lane-changing or overtaking path, the unknown parameters of the path function can be solved according to the boundary conditions.(3)
Traffic state profile generationConsidering the safety and comfort constraints, establishing a nonlinear optimization model to solve the optimal traffic state profiles of the ego vehicle along the path.(4)
Lane-changing or overtaking trajectory planningIf a feasible solution is found for the proposed nonlinear optimization model, the lane-changing or overtaking trajectory of the ego vehicle can be planned. However, if there is no feasible solution, the current lane will continue to be used, and the planner will iterate the above three steps until a feasible trajectory is planned.The sequence quadratic programming (SQP) algorithm is selected to solve the nonlinear optimization model. Previous studies [19, 33, 34] prove that this algorithm can find the feasible solution for the nonlinear problem well. Through multiple iterations, the optimal traffic state profiles of the ego vehicle are acquired based on the SQP algorithm. Therefore, the optimal lane-changing or overtaking trajectory can be planned.


## 4. Simulation and Discussion

In this section, the types of test scenarios are described first; then, the performance characteristics of the simulation results are shown; finally, three detailed trajectory planning cases are demonstrated.


### 4.1. Scenario Description and Simulation Result Analysis

Various test scenarios should be considered to evaluate the proposed model. Yang et al. [32] have proposed that lane-changing maneuver in real-world traffic can be roughly categorized into two types: the ego vehicle is located in front of vehicle 2 and the ego vehicle is located behind vehicle 2 at the initial moment. Similar to Yang et al. [32], we also consider that the overtaking maneuver includes two types according to the initial relative positions between the ego vehicle and vehicle 2 (see Figures 6(a) and 6(b)). In the figure, the solid vehicles indicate their current positions and the dashed vehicle indicates the final position of the ego vehicle.Figure 6
Overtaking positions of vehicles: (a) first type of scenario; (b) second type of scenario.
(a)
(b)As shown in Figure6(a), under the first type of scenario, the ego vehicle is located in front of vehicle 2 at the initial moment. The ego vehicle can change lane to reach the target gap between vehicle 2 and vehicle 3 directly. Under the second type of scenario (see Figure 6(b)), the ego vehicle is located behind vehicle 2 at the initial moment. The ego vehicle needs to overtake vehicle 2 to reach the target gap between vehicle 2 and vehicle 3.Furthermore, 100 random overtaking test cases for each type are generated to verify the feasibility and effectiveness of the proposed trajectory planning method. MATLAB is employed as the simulation platform. Table1 presents the relevant parameters of the model, including the lower and upper boundaries of the traffic states, and the constant quantities. Inputting the current traffic information, the planner can automatically plan a comfort and safe trajectory for the ego vehicle.Table 1
Design parameters of the model.
Symbol
Explanation
Value
I
Number of time intervals
20
t
f
,
max
Maximum total driving time
13 s
v
min
Minimum speed
0 m/s
v
max
Maximum speed
30 m/s
a
min
Minimum acceleration
−3 m/s2
a
max
Maximum acceleration
3 m/s2
j
min
Minimum jerk
−3 m/s3
j
max
Maximum jerk
3 m/s3
m
Diameter of the circle
2.50 m
r
Diagonal length of the vehicle
5.12 m
L
Length of the vehicle
4.80 mThrough our proposed model, 82 of the 100 test cases can find the feasible overtaking trajectory under the first type of scenario; 76 of the 100 test cases can find the feasible overtaking trajectory under the second type of scenario, which proves that this method is applied to most traffic conditions. The distribution of the overtaking time is shown in Figure7. From the figure, it can be indicated that the driving time of most cases is between 6 and 7 s for the first type of overtaking scenario. The shortest time is 4.72 s, the longest time is 10.24 s, and the average time is approximately 6.36 s. For the second type of overtaking scenario, most driving time is between 7 and 8 s, the shortest time is 4.68 s, the longest time is 11.56 s, and the average time is approximately 7.71 s.Figure 7
Distribution of the driving time.A comprehensive presentation of the simulation results is presented in Table2. Using the current traffic states of vehicles as the input variables, the overtaking time tf can be determined automatically by the proposed model. Since the ego vehicle needs to overtake vehicle 2 first to reach the target gap under the second type of scenario, the mean value of tf (7.71 s) is larger than the mean value of tf (6.36 s) under the first type of scenario. Moreover, the variations in the trajectory performance parameters (speed, acceleration, and jerk) in Table 2 are small, which guarantees the ride comfort.Table 2
Simulation results under the test scenarios.
Variable
The first type of scenario
The second type of scenario
t
f
,
mean
s
6.36
7.71
t
f
,
max
s
10.24
11.56
v
m
a
x
−
v
0
|
m
/
s
3.63
4.24
a
max
m
/
s
2
1.52
2.89
j
max
m
/
s
3
1.96
2.73
Δ
x
f
mean
m
106.85
135.46
Δ
s
f
mean
m
106.98
136.18


### 4.2. Numerical Examples

In this section, two special cases are demonstrated in detail. Table3 shows the initial traffic states of vehicles under two detailed scenarios. In scenario 1, the ego vehicle (111.59 m) is located in front of vehicle 2 (100.00 m), which belongs to the first type of scenario. In scenario 2, the ego vehicle (89.17 m) is located behind vehicle 2 (100.00 m), which belongs to the second type of scenario. The speed gap (4.87 m/s) between the ego vehicle and vehicle 2 under scenario 1 is larger than that (0.95 m/s) under scenario 2.Table 3
Initial traffic states of vehicles under two detailed scenarios.
Scenario 1
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
111.59 (m)
14.52 (m/s)
−0.61 (m/s2)
119.95 (m)
12.08 (m/s)
0.45 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
100.00 (m)
19.39 (m/s)
−1.33 (m/s2)
123.47 (m)
18.62 (m/s)
−0.29 (m/s2)
Scenario 2
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
89.17 (m)
14.56 (m/s)
−2.11 (m/s2)
103.80 (m)
12.08 (m/s)
0.19 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
100.00 (m)
13.61 (m/s)
−0.23 (m/s2)
123.60 (m)
14.31 (m/s)
−0.8 (m/s2)Figures8 and 9 present the overtaking trajectories and the variations in the speeds of vehicles under scenario 1, respectively. From Figure 8, it is shown that the overtaking longitudinal distance xf is 115 m and the total driving time tf is 7.14 s. Figure 9 indicates that the ego vehicle first trends to decelerate slightly to avoid the collisions with surrounding vehicles and then to accelerate to a maximum value and finally to decrease the speed slowly to a stable state. It can be concluded that the ego vehicle can dynamically adjust its speed for adapting the speed change in surrounding vehicles.Figure 8
Trajectories under scenario 1: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)Figure 9
Speeds of vehicles under scenario 1.Figures10 and 11 display the overtaking trajectories and the variations in speeds of vehicles under scenario 2, respectively. The overtaking longitudinal distance xf is 147 m, and the total driving time tf is 9.02 s. From Figure 11, it can be seen that the ego vehicle trends to decrease the speed from 0 s to 1.69 s and then to change its speed lightly from 1.69 s to 7.35 s. After 7.35 s, the ego vehicle accelerates to a value that is larger than the final speed of vehicle 1.Figure 10
Trajectories under scenario 2: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)Figure 11
Speeds of vehicles under scenario 2.Figure12 shows the speed comparison of the ego vehicle under scenarios 1 and 2. Under scenario 1, the ego vehicle decreases its speed lightly at beginning because the ego vehicle has enough initial speed gap (4.87 m/s) to adjust its position. However, since the initial speed gap (0.95 m/s) between the ego vehicle and vehicle 2 is small under scenario 2, the ego vehicle needs to decelerate quickly at the beginning for avoiding a collision with vehicle 2.Figure 12
Speed comparison of the ego vehicle.Figures13 and 14 show the planned accelerations and jerks of the ego vehicle under two scenarios, respectively. From the figures, it can be indicated that different initial traffic states of vehicles will have a significant impact on acceleration and jerk profiles. These trajectory performance variables of the ego vehicle under scenarios 1 and 2 are all small, which ensures the comfort of passengers during the overtaking process.Figure 13
Acceleration comparison of the ego vehicle.Figure 14
Jerk comparison of the ego vehicle.


### 4.3. Trajectory Correction Scenario

If the traffic states of surrounding vehicles change suddenly, the planned trajectory of the ego vehicle needs to be corrected to prevent the potential collision. The corrected trajectory can still be generated using our proposed model. Next, we will describe a trajectory correction scenario in detail. Table4 shows the initial traffic states of the vehicles.Table 4
Initial traffic states of vehicles under correction scenario.
Trajectory correction scenario
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
100.00 (m)
15.26 (m/s)
0.62 (m/s2)
108.47 (m)
13.01 (m/s)
0.53 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
97.59 (m)
13.27 (m/s)
−0.16 (m/s2)
110.22 (m)
14.15 (m/s)
0.24 (m/s2)In this scenario, the ego vehicle originally intends to change lanes. However, vehicle 3 suddenly decelerates at 2.62 s. The ego vehicle perceives that the distance between itself and vehicle 3 will become very short, and there may be a collision. Therefore, the lane-changing trajectory is corrected, and the ego vehicle returns to the original lane. Figure15 presents the trajectories of the relevant vehicles in this scenario. The simulation result indicates that the total longitudinal distance of the ego vehicle is 91.73 m, and the driving time is 6.64 s. During the trajectory correction process, we assume that vehicle 2 follows the leading vehicle politely, so the motion of vehicle 2 after 2.62 s is not discussed.Figure 15
Trajectories under correction scenario: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)In Figure15, the ego vehicle successfully returns to the original lane tracking the corrected trajectory and does not collide with the surrounding vehicles. It can be concluded that the proposed model is effective for emergent traffic condition.Furthermore, the real-time distance of the ego vehicle and surrounding vehicles is shown in Figure16. Here, the distance denotes the driving distance from the origin point of the lane to the current position of vehicle. In Figure 16, vehicle 3 decelerates at 2.62 s, and then its speed decreases rapidly. If the ego vehicle does not correct the trajectory, it is likely to collide with vehicle 3.Figure 16
Driving distance of the vehicles under correction scenario.


## 4.1. Scenario Description and Simulation Result Analysis

Various test scenarios should be considered to evaluate the proposed model. Yang et al. [32] have proposed that lane-changing maneuver in real-world traffic can be roughly categorized into two types: the ego vehicle is located in front of vehicle 2 and the ego vehicle is located behind vehicle 2 at the initial moment. Similar to Yang et al. [32], we also consider that the overtaking maneuver includes two types according to the initial relative positions between the ego vehicle and vehicle 2 (see Figures 6(a) and 6(b)). In the figure, the solid vehicles indicate their current positions and the dashed vehicle indicates the final position of the ego vehicle.Figure 6
Overtaking positions of vehicles: (a) first type of scenario; (b) second type of scenario.
(a)
(b)As shown in Figure6(a), under the first type of scenario, the ego vehicle is located in front of vehicle 2 at the initial moment. The ego vehicle can change lane to reach the target gap between vehicle 2 and vehicle 3 directly. Under the second type of scenario (see Figure 6(b)), the ego vehicle is located behind vehicle 2 at the initial moment. The ego vehicle needs to overtake vehicle 2 to reach the target gap between vehicle 2 and vehicle 3.Furthermore, 100 random overtaking test cases for each type are generated to verify the feasibility and effectiveness of the proposed trajectory planning method. MATLAB is employed as the simulation platform. Table1 presents the relevant parameters of the model, including the lower and upper boundaries of the traffic states, and the constant quantities. Inputting the current traffic information, the planner can automatically plan a comfort and safe trajectory for the ego vehicle.Table 1
Design parameters of the model.
Symbol
Explanation
Value
I
Number of time intervals
20
t
f
,
max
Maximum total driving time
13 s
v
min
Minimum speed
0 m/s
v
max
Maximum speed
30 m/s
a
min
Minimum acceleration
−3 m/s2
a
max
Maximum acceleration
3 m/s2
j
min
Minimum jerk
−3 m/s3
j
max
Maximum jerk
3 m/s3
m
Diameter of the circle
2.50 m
r
Diagonal length of the vehicle
5.12 m
L
Length of the vehicle
4.80 mThrough our proposed model, 82 of the 100 test cases can find the feasible overtaking trajectory under the first type of scenario; 76 of the 100 test cases can find the feasible overtaking trajectory under the second type of scenario, which proves that this method is applied to most traffic conditions. The distribution of the overtaking time is shown in Figure7. From the figure, it can be indicated that the driving time of most cases is between 6 and 7 s for the first type of overtaking scenario. The shortest time is 4.72 s, the longest time is 10.24 s, and the average time is approximately 6.36 s. For the second type of overtaking scenario, most driving time is between 7 and 8 s, the shortest time is 4.68 s, the longest time is 11.56 s, and the average time is approximately 7.71 s.Figure 7
Distribution of the driving time.A comprehensive presentation of the simulation results is presented in Table2. Using the current traffic states of vehicles as the input variables, the overtaking time tf can be determined automatically by the proposed model. Since the ego vehicle needs to overtake vehicle 2 first to reach the target gap under the second type of scenario, the mean value of tf (7.71 s) is larger than the mean value of tf (6.36 s) under the first type of scenario. Moreover, the variations in the trajectory performance parameters (speed, acceleration, and jerk) in Table 2 are small, which guarantees the ride comfort.Table 2
Simulation results under the test scenarios.
Variable
The first type of scenario
The second type of scenario
t
f
,
mean
s
6.36
7.71
t
f
,
max
s
10.24
11.56
v
m
a
x
−
v
0
|
m
/
s
3.63
4.24
a
max
m
/
s
2
1.52
2.89
j
max
m
/
s
3
1.96
2.73
Δ
x
f
mean
m
106.85
135.46
Δ
s
f
mean
m
106.98
136.18


## 4.2. Numerical Examples

In this section, two special cases are demonstrated in detail. Table3 shows the initial traffic states of vehicles under two detailed scenarios. In scenario 1, the ego vehicle (111.59 m) is located in front of vehicle 2 (100.00 m), which belongs to the first type of scenario. In scenario 2, the ego vehicle (89.17 m) is located behind vehicle 2 (100.00 m), which belongs to the second type of scenario. The speed gap (4.87 m/s) between the ego vehicle and vehicle 2 under scenario 1 is larger than that (0.95 m/s) under scenario 2.Table 3
Initial traffic states of vehicles under two detailed scenarios.
Scenario 1
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
111.59 (m)
14.52 (m/s)
−0.61 (m/s2)
119.95 (m)
12.08 (m/s)
0.45 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
100.00 (m)
19.39 (m/s)
−1.33 (m/s2)
123.47 (m)
18.62 (m/s)
−0.29 (m/s2)
Scenario 2
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
89.17 (m)
14.56 (m/s)
−2.11 (m/s2)
103.80 (m)
12.08 (m/s)
0.19 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
100.00 (m)
13.61 (m/s)
−0.23 (m/s2)
123.60 (m)
14.31 (m/s)
−0.8 (m/s2)Figures8 and 9 present the overtaking trajectories and the variations in the speeds of vehicles under scenario 1, respectively. From Figure 8, it is shown that the overtaking longitudinal distance xf is 115 m and the total driving time tf is 7.14 s. Figure 9 indicates that the ego vehicle first trends to decelerate slightly to avoid the collisions with surrounding vehicles and then to accelerate to a maximum value and finally to decrease the speed slowly to a stable state. It can be concluded that the ego vehicle can dynamically adjust its speed for adapting the speed change in surrounding vehicles.Figure 8
Trajectories under scenario 1: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)Figure 9
Speeds of vehicles under scenario 1.Figures10 and 11 display the overtaking trajectories and the variations in speeds of vehicles under scenario 2, respectively. The overtaking longitudinal distance xf is 147 m, and the total driving time tf is 9.02 s. From Figure 11, it can be seen that the ego vehicle trends to decrease the speed from 0 s to 1.69 s and then to change its speed lightly from 1.69 s to 7.35 s. After 7.35 s, the ego vehicle accelerates to a value that is larger than the final speed of vehicle 1.Figure 10
Trajectories under scenario 2: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)Figure 11
Speeds of vehicles under scenario 2.Figure12 shows the speed comparison of the ego vehicle under scenarios 1 and 2. Under scenario 1, the ego vehicle decreases its speed lightly at beginning because the ego vehicle has enough initial speed gap (4.87 m/s) to adjust its position. However, since the initial speed gap (0.95 m/s) between the ego vehicle and vehicle 2 is small under scenario 2, the ego vehicle needs to decelerate quickly at the beginning for avoiding a collision with vehicle 2.Figure 12
Speed comparison of the ego vehicle.Figures13 and 14 show the planned accelerations and jerks of the ego vehicle under two scenarios, respectively. From the figures, it can be indicated that different initial traffic states of vehicles will have a significant impact on acceleration and jerk profiles. These trajectory performance variables of the ego vehicle under scenarios 1 and 2 are all small, which ensures the comfort of passengers during the overtaking process.Figure 13
Acceleration comparison of the ego vehicle.Figure 14
Jerk comparison of the ego vehicle.


## 4.3. Trajectory Correction Scenario

If the traffic states of surrounding vehicles change suddenly, the planned trajectory of the ego vehicle needs to be corrected to prevent the potential collision. The corrected trajectory can still be generated using our proposed model. Next, we will describe a trajectory correction scenario in detail. Table4 shows the initial traffic states of the vehicles.Table 4
Initial traffic states of vehicles under correction scenario.
Trajectory correction scenario
x
0,0
v
0,0
a
0,0
x
1,0
v
1,0
a
1,0
100.00 (m)
15.26 (m/s)
0.62 (m/s2)
108.47 (m)
13.01 (m/s)
0.53 (m/s2)
x
2,0
v
2,0
a
2,0
x
3,0
v
3,0
a
3,0
97.59 (m)
13.27 (m/s)
−0.16 (m/s2)
110.22 (m)
14.15 (m/s)
0.24 (m/s2)In this scenario, the ego vehicle originally intends to change lanes. However, vehicle 3 suddenly decelerates at 2.62 s. The ego vehicle perceives that the distance between itself and vehicle 3 will become very short, and there may be a collision. Therefore, the lane-changing trajectory is corrected, and the ego vehicle returns to the original lane. Figure15 presents the trajectories of the relevant vehicles in this scenario. The simulation result indicates that the total longitudinal distance of the ego vehicle is 91.73 m, and the driving time is 6.64 s. During the trajectory correction process, we assume that vehicle 2 follows the leading vehicle politely, so the motion of vehicle 2 after 2.62 s is not discussed.Figure 15
Trajectories under correction scenario: (a) trajectories of the ego vehicle, vehicle 1, and vehicle 2; (b) trajectories of the ego vehicle, vehicle 1, and vehicle 3.
(a)
(b)In Figure15, the ego vehicle successfully returns to the original lane tracking the corrected trajectory and does not collide with the surrounding vehicles. It can be concluded that the proposed model is effective for emergent traffic condition.Furthermore, the real-time distance of the ego vehicle and surrounding vehicles is shown in Figure16. Here, the distance denotes the driving distance from the origin point of the lane to the current position of vehicle. In Figure 16, vehicle 3 decelerates at 2.62 s, and then its speed decreases rapidly. If the ego vehicle does not correct the trajectory, it is likely to collide with vehicle 3.Figure 16
Driving distance of the vehicles under correction scenario.


## 5. Conclusion

This paper proposes a universal trajectory planning method that is suitable for the automated lane-changing and overtaking maneuvers. The proposed method first generates a two-dimensional path in the Cartesian coordinate system to connect the initial position with the final position, and then a nonlinear mathematical optimization model is established to obtain the traffic state profiles along the path. The combination of a path and its traffic state profiles determines a spatiotemporal trajectory. Moreover, considering the safety and comfort of the ego vehicle during the driving process, the study discretizes the total time horizon into several time intervals and solves the unknown parameters of the function to generate the continuous and smooth traffic state profiles. Furthermore, a series of numerical examples are conducted under the typical scenarios. The simulation results demonstrate that the intelligent vehicle can avoid the potential collisions effectively when tracking the planned trajectory. Future works will concentrate on the trajectory replanning and tracking issues for proposing a more realistic lane-changing and overtaking model.



---

*Source: 1023975-2020-04-25.xml*