# Planning a Continuous Vehicle Trajectory for an Automated Lane Change Maneuver by Nonlinear Programming considering Car-Following Rule and Curved Roads

Authors: Chong Wei; Shurong Li
Journal: Journal of Advanced Transportation
 (2020)

---

## Abstract

This study proposes a trajectory planning method for an automated lane change maneuver. We consider that the spatiotemporal trajectory of a controlled vehicle can be represented by a polynomial function and estimate the parameters of the polynomial function through nonlinear programming that does not rely on the assumed time horizon of lane change and the assumed final state of the controlled vehicle. When the controlled vehicle achieves its target lane, the state of this vehicle should be constrained by both the position and the speed of the forward vehicle. We integrate a car-following model into the nonlinear programming to constrain the state of the controlled vehicle at the end of the lane change. Notably, the interaction factors are taken into consideration: the motion of the vehicle that follows the controlled vehicle in the target lane can be influenced by the trajectory planning results of the controlled vehicle. The proposed trajectory planning method can simultaneously estimate the motion of the following vehicle and plan the trajectory for the controlled vehicle. Our proposed model can also work for curved road sections. We represent the curve information in the nonlinear programming by a regression model. We solve the nonlinear programming through the sequential quadratic program (SQP) algorithm and design a method to specify an initial guess for the algorithm. This paper finally demonstrates the effect of the proposed trajectory planning method under different scenarios.


---

## Body

## 1. Introduction

A lane change maneuver is one of the complex driving maneuvers. This maneuver is constrained by the states of the surrounding vehicles and involves both longitudinal and lateral motions. To automatically drive a controlled vehicle from its current lane to a target lane, advanced driver assistance systems (ADAS) need to employ an algorithm to plan a trajectory for the lane change maneuver that will allow the vehicle to track its trajectory and complete the maneuver [1]. A vehicle trajectory should contain both spatial and temporal information about the future motion of the controlled vehicle. This paper aims to propose a nonlinear program that can generate continuous trajectories for the lane change maneuver. We consider that a vehicle has already decided to change the lane and can obtain some of the precise states [2, 3], such as the positions, speeds, and accelerations, of the surrounding vehicles through V2V (vehicle to vehicle) communications.Several methods have been proposed to solve the vehicle trajectory planning problem. Below, we make a brief review of existing works in the area of lane change trajectory planning. Interested readers can refer to [4–7] for a comprehensive literature review.Recent scientific literature has shown that combinatorial methods, such as the state lattice, are widely applied to solve vehicle trajectory planning problems [4]. Such a method must first create a discrete graph offline and then search for a trajectory from the graph [8–11]. For example, Ferguson et al. [12] used Newton’s method to generate candidate spatial paths in a spatial space and stored the paths in a lookup table. The trajectory planner generates and evaluates spatiotemporal trajectories based on the lookup table. Ziegler [13] established a geometric graph and used the shortest path algorithms to search for paths in the graph. The trajectory was formulated as quintic splines and was generated according to the selected path. Werling et al. [14] generated a candidate set that consisted of s-shaped swerve trajectories and selected the best trajectory from this finite set. McNaughton et al. [15] developed a spatiotemporal lattice method that considered vehicle dynamics based on the bicycle model. Since a vehicle trajectory is a routing solution in a spatiotemporal space, combinatorial methods must discretize a large spatiotemporal space and lose the flexibility for the trajectory planning to avoid the curse of dimensionality.In recent years, several studies have discussed trajectory planning based on mathematical programming [16–18]. Such a method can avoid ergodic searching in a spatiotemporal space with large size and is an ideal tool for planning a vehicle’s trajectory in a continuous spatiotemporal space. More importantly, mathematical programming can consider constraint conditions such as safety and comfort constraints in a straightforward manner.Shamir [19] proposed a nonlinear program to plan the trajectory, where the lane change trajectory is based on underlying polynomial equations. The programming attempts to minimize the total kinetic energy with a constraint for speed and acceleration. However, the study assumed that the initial and final points of the lane-changing maneuver were both known conditions and that the initial and final velocities were equal. This assumption may not be found in real-world cases. Milam et al. [20] and Schwartz and Milam [21] formulated the trajectory planning as an optimal control problem (OCP) and solved the programming using a sequential quadratic programming (SQP) solver. The proposed OCP can constrain the trajectory by vehicle dynamics, whereas the assumption of the initial state of the vehicle and the static obstacle cannot account for the practical lane change driving scenario with moving obstacles (i.e., the moving surrounding vehicles). Carvalho et al. [22] adopted a model predictive control (MPC) method to combine the trajectory planning and control. Their proposed MPC method was a nonlinear program that can be solved by a customized SQP algorithm. However, their study did not discuss a moving obstacle, such as vehicles around the controlled vehicle. Ziegler et al. [23] proposed a continuous optimization scheme for trajectory planning. The study constructed a constrained nonlinear program that can plan the future positions of the controlled vehicle for each time interval along a prespecified time horizon. The programming can guarantee a collision-free planning result. Maeda et al. [24] employed the final state control (FSC) method for trajectory planning. The proposed FSC aimed to minimize the jerk of the controlled vehicle and mainly constrained the movement of the vehicle by the initial/final state of the vehicle. The proposed FSC method can avoid the collision between the controlled vehicle and pedestrians by combining the future position information about pedestrians in the jerk minimization problem. However, the proposed FSC method was strongly dependent on the assumed final state of the vehicle. Nilsson [25] proposed a low-complexity quadratic program to plan a vehicle trajectory for lane change maneuvers. The method can satisfy the collision-free requirement for the vehicles near the controlled vehicle. However, the method was only collision-free when considering safety spaces, but it ignored speed issues. The method was designed for lane changes on a straight-line lane and may suffer from adaption problems, especially when handling lane changes for road sections with curves. Luo et al. [26] proposed a nonlinear program for trajectory planning that can maintain safe space between the controlled vehicle and other vehicles. This study treated the trajectory as a polynomial function and optimized the parameters of the polynomial function. A continuous and derivable vehicle trajectory can be generated by solving the programming.Nonlinear programming has long been recognized as a powerful tool for trajectory planning. This study further develops this methodology to improve its feasibility. Previous studies usually assumed that the length of a control time horizon is known and fixed, so the shape of the trajectory can be constrained well, but we find that it is difficult to set a precise value for the time horizon of the lane change maneuver before undertaking trajectory planning. We also find that previous studies often assumed that the longitudinal position in Cartesian coordinates of a vehicle at the end of the lane change maneuver can be used as a given condition. However, the final longitudinal position is available only if the vehicle drives along a straight road. Furthermore, Papadimitriou and Tomizuka [27] showed that the shape of the trajectory for a lane change maneuver is highly dependent on the final state of the vehicle. Although a few studies (e.g., Jula et al. [28] and Nilsson et al. [25]) have noted that the final position of the vehicle should maintain a safety space relative to the forward vehicle, we still need to further consider that the state of a vehicle at the end of the lane change maneuver should be constrained by the position, speed, and acceleration of the forward vehicle in the target lane, according to the car-following theory (see Kesting et al. [29]; Toledo et al. [30]; Kesting et al. [31]; and Song et al. [32]). This study proposes a new methodology that no longer needs to use the exact time horizon length and exact final position information to constrain the trajectory. We consider that although we do not know the exact length of the time horizon and the final position of a controlled vehicle at the end of the lane change maneuver, the final state of the controlled vehicle should satisfy several equality conditions. For example, the vehicle must reach the center of the target lane, and the driving direction must be equal to the direction of the center of the target lane. Instead of using the exact time horizon length and final position information, we attempt to use equality conditions to constrain the shape parameters of the trajectory. In this study, we integrate a car-following rule into the trajectory planner and use the car-following model to constrain the state of the controlled vehicle at the end of the lane change. This approach can also allow the controlled vehicle to switch from the lane change maneuver to the car-following maneuver smoothly. In addition, the lane change maneuver of the controlled vehicle may interact with the motion of the surrounding vehicles. We attempt to simultaneously estimate the motion of the surrounding vehicles and plan the trajectory for the controlled vehicle. This strategy helps us determine the shape of the trajectory with respect to the motion of the surrounding vehicles. We also note that the trajectory planning results should match road curves well, which includes not only straight lines but also circular curves and transition curves. Our study provides a simple method for combining the road curve information into trajectory planning. The proposed method can be easily adapted in practice: it does not require knowing the start and end position of a curve, and it is not limited to the type of curve.The trajectory planner needs to replan the trajectory at a specified frequency during the lane change maneuver. However, at each replanning step, the trajectory planner always uses the same procedure to generate the trajectory. This study focuses on the trajectory planning method itself and leaves the discussion of technical details, such as the replanning issue, for other studies.The rest of the paper is organized as follows. Section2 explains the background of the lane change maneuver. Section 3 describes the nonlinear programming that is proposed to plan a lane change trajectory. Section 4 discusses the solution techniques for the proposed programming. Section 5 presents a series of numerical examples. Section 6 concludes the study.


## 2. Background of the Lane Change Maneuver

This study focuses on the lane change maneuver scenario, as shown in Figure1. In Figure 1(a), vehicle 0 begins to change the position from its current lane to a target lane. The lane change maneuver involves four vehicles around vehicle 0. We index these vehicles as vehicles 1, 2, 3, and 4. Vehicle 0 must avoid colliding with these surrounding vehicles during the lane change. As illustrated in Figure 1(a), at the beginning of the lane change, vehicle 1 in the target lane is behind vehicle 0, while vehicle 3 in the target lane should be at the front of vehicle 0.Figure 1
Vehicles involved in the lane change maneuver. (a) The initial state of the vehicles when vehicle 0 begins a lane change maneuver. (b) The final state of the vehicles when vehicle 0 finishes the change lane maneuver.
(a)(b)When vehicle 0 finishes the lane change maneuver at the time instantT, it must switch from the lane change maneuver to a car-following maneuver. Accordingly, we consider that the state of vehicle 0 at time instant T should satisfy three conditions. First, vehicle 0 should achieve the center of the target lane. Second, vehicle 0 should drive along the tangential direction of the center of the target lane. Third, to maintain the safety and comfort of vehicle 0, the planned acceleration of vehicle 0 at time instant T should be equal to the acceleration determined by a car-following model. Specifically, the final acceleration of vehicle 0 is derived by the car-following model, whose inputs include the planned speed of vehicle 0 at time instant T, the speed of vehicle 3 at time instant T, and the distance between vehicles 0 and 3 at time instant T (i.e., Δs in Figure 1(a)).We also estimate the motion of vehicle 1 based on the jerk of vehicle 1. We assume that the driver of vehicle 1 is not willing to reduce the speed too much and wishes to keep the jerk as close to zero as possible. It is also reasonable to assume that vehicle 1 can accept the lane change maneuver of vehicle 0 if two requirements are satisfied. First, the corresponding jerk and other kinematic variables are within the acceptable bounds. Second, the final acceleration of vehicle 1 at time instantT is smaller than the safe acceleration, which is derived using the car-following model. Since vehicle 1 follows vehicle 0 at time instant T, this car-following acceleration is dependent on the speed of vehicle 1 at time instant T, the speed of vehicle 0 at time instant T, and the distance between vehicle 1 and 0 at time instant T (i.e., Δd in Figure 1(b)).


## 3. Trajectory Planning by a Nonlinear Program (TRNP)

### 3.1. Basic Symbol Definitions

We define the total time horizon for the lane change maneuver asT, which serves as a decision variable of the proposed trajectory planning method. Then, we sample a series of time points with equal time distance between 0 and T and use i=0,1,…,I to index these time instants and denote the actual time of the i-th time instant as ti. Therefore, for vehicle 0, it starts the lane change at time instant t0=0 and finishes the lane change at time instant tI=T. This discretized maneuver yields the value of ti as(1)ti=iTI,where I is given as a constant value. However, it should be noted that ti is not a constant and is dependent on the value of T.At time instantti, we introduce vehicle 0’s kinematic variables as the longitudinal position x0,i, the lateral position y0,i, the longitudinal velocity v0,ix, the lateral velocity v0,iy, the longitudinal acceleration a0,ix, the lateral acceleration a0,iy, the longitudinal jerk j0,ix, and the lateral jerk j0,iy. To construct a feasible trajectory, polynomials have been widely recognized as an effective tool in satisfying the traveler’s comfort [7]. For example, Papadimitriou and Tomizuka [27] and Yoneda et al. [33] constrained the initial and final states of the controlled vehicle and used two fifth-order polynomial functions to represent a trajectory. Therefore, polynomial functions are adopted in our study to represent the longitudinal and lateral kinematic models. Moreover, compared to the usual method to build the kinematic vehicle model using the position, speed, and acceleration, our study adopts the position, speed, acceleration, and jerk as decision variables in constructing the kinematic model. Therefore, to adapt to the kinematic model, we choose to increase the polynomial functions to a higher order (e.g., Shui [34] and You et al. [35]) and establish the kinematic model as(2)x0,i=β0+β1ti+β2ti2+β3ti3+β4ti4+β5ti5+β6ti6,(3)y0,i=α0+α1ti+α2ti2+α3ti3+α4ti4+α5ti5+α6ti6,where α0,…,α6 and β0,…,β6 in equations (2) and (3) are the parameters of the polynomial functions. Considering the kinematic continuity, v0,ix and v0,iy can be derived as ∂x0,i/∂ti and ∂y0,i/∂ti, respectively. a0,ix and a0,iy can be obtained as ∂v0,ix/∂ti and ∂v0,iy/∂ti, respectively. j0,ix and j0,iy can be further obtained as ∂a0,ix/∂ti and ∂a0,iy/∂ti, respectively. At the beginning of the lane change maneuver, the initial states (positions, speeds, and accelerations) for vehicles can be obtained through techniques such as autonomous perception or V2X communications. Since we know the initial state of vehicle 0 at time instant t0, the values for α0,α1,α2 and β0,β1,β2 can be derived as(4)α0=y0,0,α1=v0,0y,α2=a0,0y2,β0=x0,0,β1=v0,0x,β2=a0,0x2.In equation (4), we use y0,0, v0,0y, a0,0y, x0,0, v0,0x, and a0,0x to denote y0,i, v0,iy, a0,iy, x0,i, v0,ix, and a0,ix for vehicle 0 at the initial time instant i=0.We further define the speed of vehicle 0 at time instantT (i.e., for i=I) as v0,I:(5)v0,I=v0,Ix2+v0,Iy20.5,and define the acceleration of vehicle 0 at time instant T as a0,I:(6)a0,I=∂v0,I∂T=0.5v0,Ix2+v0,Iy2−0.52v0,Ixa0,Ix+2v0,Iya0,Iy.Let us denote the jerk of vehicle 1 asj1. The speed of vehicle 1 at time instant ti is(7)v1,i=v1,0+a1,0ti+12j1ti2,where v1,0 and a1,0 are the initial speed and acceleration of vehicle 1. Then, the curve distance between the position of vehicle 1 at time instant ti and the origin of the target lane can be obtained as(8)s1,i=s1,0+v1,0ti+12a1,0ti2+16j1ti3,where s1,0 is the initial curve distance of vehicle 1.


### 3.2. The Nonlinear Program

We construct a nonlinear program to plan the trajectory for vehicle 0 while estimating the motion of vehicle 1. The proposed program treatsα3, α4, α5, α6, β3, β4, β5, β6, T, and j1 as decision variables. As shown in Figure 2, the proposed nonlinear programming aims to minimize the safety risk and discomfort for vehicle 0 and vehicle 1, while improving the traveling efficiency of vehicle 0. The detailed formulation of the programming is given as(9a)min∑i=1Iρ0a0,ix2I+ρ1a0,iy2I+ρ2j0,ix2I+ρ3j0,iy2I+ρ4T2+ρ5j12,s.t.(9b)Y∗x0,I=α0+α1T+α2T2+α3T3+α4T4+α5T5+α6T6,(9c)v0,Iyv0,Ix=∂Y∗x∂x|x=x0,I,(9d)a0,I=ℂv0,I,v3,I,Δs,(9e)amin≤a1,I≤ℂv1,I,v0,I,Δd,(9f)r2≤x0,i−x1,i2+y0,i−y1,i2,fori=1,…,I,(9g)r2≤x0,i−x3,i2+y0,i−y3,i2,fori=1,…,I,(9h)m2≤x0,ik−x4,i12+y0,ik−y4,i12,fork=0,1,…,4,fori=1,…,I,(9i)jmin≤j1≤0,(9j)0≤v0,ix≤vmaxx,fori=1,…,I,(9k)0≤v0,iy≤vmaxy,fori=1,…,I,(9l)aminx≤a0,ix≤amaxx,fori=1,…,I,(9m)aminy≤a0,iy≤amaxy,fori=1,…,I,(9n)jminx≤j0,ix≤jmaxx,fori=1,…,I,(9o)jminy≤j0,iy≤jmaxy,fori=1,…,I,(9p)0≤T≤Tmax,(9q)x0,0≤x0,I≤x0,0+Δxmax,(9r)Δs≥0,(9s)Δd≥0.Figure 2
The outline of the proposed nonlinear programming.Equation (9a) is the objective function of the nonlinear program, where ρ0, ρ1, ρ2, ρ3, ρ4, and ρ5 are weight parameters. The objective function takes into account several factors, including driving comfort, safety, efficiency, and negative influence of the surrounding vehicles, where the combination method in the objective function has been validated in many works [36–38]. Since we aim to minimize the safety risk and discomfort for vehicle 0, the objective function contains the average quadratic acceleration and jerk of vehicle 0 (see the first four items). Then, we also seek to make the time horizon T of the lane change maneuver as short as possible to improve efficiency. Finally, we add j12 to the objective function because we consider that the driver of vehicle 1 always wishes to keep the jerk as close to zero as possible and aims to reduce the negative influence exerted by the lane change maneuver of vehicle 0.In constraint (9b),x0,I denotes the longitudinal position of vehicle 0 at the end of the lane change T. As shown in Figure 3, Y∗x is a function that returns the lateral position of the point on the center of the target lane, where the input is longitudinal position x. Constraint (9b) ensures that vehicle 0 at time instant T is located at the center of the target lane. We describe how to construct the function Y∗x in Section 3.3.Figure 3
Road curve functions and curve distance.Constraint (9c) is for the driving direction of vehicle 0 at time instant T. This constraint ensures that the ratio of the lateral speed to the longitudinal speed at time instant T is equal to the derivative of Y∗x at x0,I (the direction of the target lane).Constraint (9d) ensures that the value of a0,I defined by equation (6) must be equal to the acceleration determined by a car-following model. As shown by equation (6), a0,I can be considered a function of the decision variables α3, α4, α5, α6, β3, β4, β5, β6, and T. Constraint (9d) uses a car-following model to constrain the values of these variables.We choose the full velocity difference model (FVDM) proposed by Jiang et al. [39] as the underlying car-following model. According to the FVDM model, the car-following acceleration of vehicle 0 at time instant T is dependent on the speed of vehicle 0 at time instant T, the speed of vehicle 3 at time instant T, and the distance Δs between vehicle 0 and 3 at time instant T. Here, we use ℂv0,I,v3,I,Δs to denote the FVDM function that returns the acceleration of vehicle 0 at time instant T. This function is defined as(10)ℂv0,I,v3,I,Δs=κV1+V2tanhC1Δs−sc−C2−v0,I+λv3,I−v0,I,where κ, λ, sc, C1, C2, V1, and V2 are the prespecified parameters of the car-following model, where the values of them are inherited from the work of Jiang et al. [39]. v0,I is the speed of vehicle 0 at time instant T, whose value can be calculated by equation (5), and v3,I means the speed of vehicle 3 at time instant T, which can be computed as(11)v3,I=v3,0+a3,0T+12j3,0T2,where v3,0 is the speed of vehicle 3 at time instant t0, a3,0 is the acceleration of vehicle 3 at time instant t0, and j3,0 is the jerk of vehicle 3 at time instant t0. The curve distance between the position of vehicle 3 and the origin of the target lane at time instant T (see Figure 4) can be further obtained as(12)s3,I=s3,0+v3,0T+12a3,0T2+16j3,0T3,where s3,0 is the initial curve distance between the position of vehicle 3 and the origin of the target lane at time instant t0. The curve distance between vehicles 0 and 3 at time instant T can be obtained as(13)Δs=s3,I−S∗x0,I−l,where l is the length of a vehicle and S∗x0,I returns the curve distance between the position of vehicle 0 and the origin of the target lane at time instant T (see Figure 4).Figure 4
Curve distance-related function illustration.Constraint (9e) ensures that at the end of the lane change maneuver, the acceleration of vehicle 1, a1,I, is not smaller than the low boundary amin and is not larger than the safe acceleration of vehicle 1 provided by the FVDM. The car-following acceleration of vehicle 1 is obtained as ℂv1,I,v0,I,Δd, where we can calculate a1,I and v1,I as a1,0+j1T and v1,0+a1,0T+0.5j1T2, respectively. And Δd is denoted as the distance between vehicles 1 and 0 in the target lane at time instant T. Since the curve distance s1,I between the position of vehicle 1 and the origin of the target lane at time instant T is given as s1,0+v1,0T+1/2a1,0T2+1/6j1T3, the value of Δd can be derived as(14)Δd=S∗x0,I−s1,I−l.Constraint (9f) ensures that the Euclidean distance between vehicles 0 and 1 is greater than the diagonal length r of a vehicle at time instant ti. In constraint (9f), x1,i and y1,i denote the longitudinal and lateral positions of vehicle 1 at time instant ti, respectively. We obtain x1,i as x1,i=X∗s1,i, where s1,i is given by equation (8) and X∗s is a function that returns the longitudinal position x for a point located at the center of the target lane whose curve distance is s (see Figure 4). y1,i can be further obtained as y1,i=Y∗x1,i.Constraint (9g) ensures that the distance between vehicles 0 and 3 must be larger than the diagonal length r of a vehicle. We use x3,i and y3,i to denote the longitudinal and lateral positions of vehicle 3 at time instant ti, respectively. These values can be obtained as x3,i=X∗s3,i and y3,i=Y∗x3,i, respectively.Constraint (9h) is for the distance between vehicles 0 and 4. Similar to the work in Ziegler et al. [23], we decompose a vehicle shape into five circles with diameter m (see Figure 5). Then, these circles are indexed as k=0,1,…,4, where the center of the 0-th circles is also the centroid of the vehicle. We use x0,ik and y0,ik to denote the longitudinal and lateral positions of the center of the k-th circle of vehicle 0 at time instant ti, respectively. The position x0,ik,y0,ik for k=0,1,…,4 can be derived from x0,i,y0,i. We denote x4,i,k and y4,i,k as the longitudinal and lateral positions of the k-th circle’s center for vehicle 4 at time instant ti, respectively. x4,i and y4,i are obtained as Xs4,i and Yx4,i, respectively. s4,i is the curve distance between the position of vehicle 4 and the origin of the current lane at time instant ti. We employ the following equation to predict s4,i:(15)s4,i=s4,0+v4,0ti+12a4,0ti2+16j4,0ti3,where s4,0 is the curve distance between the position of vehicle 4 and the origin of the current lane at time instant t0, v4,0 is the speed of vehicle 4 at time instant t0, a4,0 is the acceleration of vehicle 4 at time instant t0, and j4,0 is the jerk of vehicle 4 at time instant t0.Figure 5
Decomposing a vehicle shape into five circles.Constraint (9i) requires the jerk of vehicle 1 to be a nonpositive value. Constraints (9j) to (9o) are for the bounds of v0,ix, v0,iy, a0,ix, a0,iy, j0,ix, and j0,iy with i=1,…,I for vehicle 0.Constraint (9p) enforces the range of the time horizon T from 0 to Tmax, where Tmax denotes the maximum allowed length of the time horizon for the lane change maneuver. Constraint (9q) enforces the range of longitudinal position of vehicle 0 from x0,0 to x0,0+Δxmax, where Δxmax is the maximum allowed movement distance of vehicle 0 along the x-axis for the lane change maneuver.


### 3.3. Combining Road Curve Information by Polynomial Regression Model

The proposed model can execute trajectory planning on curved roads. We use functionsYxY∗x, S(x), S∗(x), and Xs to combine curve information into the nonlinear program. Previous studies (Lundquist et al. [40]; Fatemi et al. [41]; and Hammarstrand et al. [42]) confirmed that polynomial regression models can approximate road curves well in Cartesian coordinates. Following these studies, we also employ the polynomial regression model to approximate Yx, Y∗x, S(x), S∗(x), and Xs. First, we can obtain sampling points among the road curves from a high-definition map, such as the grey points in Figure 6. Then, we adopt the polynomial regression model to estimate the road curves (i.e., the grey lines in Figure 6) and obtain the internal parameters through the least squares method. This method is not required to know the geometric design of a road and is not restricted by the type of curve.Figure 6
Road curve fitting using the polynomial regression model.


## 3.1. Basic Symbol Definitions

We define the total time horizon for the lane change maneuver asT, which serves as a decision variable of the proposed trajectory planning method. Then, we sample a series of time points with equal time distance between 0 and T and use i=0,1,…,I to index these time instants and denote the actual time of the i-th time instant as ti. Therefore, for vehicle 0, it starts the lane change at time instant t0=0 and finishes the lane change at time instant tI=T. This discretized maneuver yields the value of ti as(1)ti=iTI,where I is given as a constant value. However, it should be noted that ti is not a constant and is dependent on the value of T.At time instantti, we introduce vehicle 0’s kinematic variables as the longitudinal position x0,i, the lateral position y0,i, the longitudinal velocity v0,ix, the lateral velocity v0,iy, the longitudinal acceleration a0,ix, the lateral acceleration a0,iy, the longitudinal jerk j0,ix, and the lateral jerk j0,iy. To construct a feasible trajectory, polynomials have been widely recognized as an effective tool in satisfying the traveler’s comfort [7]. For example, Papadimitriou and Tomizuka [27] and Yoneda et al. [33] constrained the initial and final states of the controlled vehicle and used two fifth-order polynomial functions to represent a trajectory. Therefore, polynomial functions are adopted in our study to represent the longitudinal and lateral kinematic models. Moreover, compared to the usual method to build the kinematic vehicle model using the position, speed, and acceleration, our study adopts the position, speed, acceleration, and jerk as decision variables in constructing the kinematic model. Therefore, to adapt to the kinematic model, we choose to increase the polynomial functions to a higher order (e.g., Shui [34] and You et al. [35]) and establish the kinematic model as(2)x0,i=β0+β1ti+β2ti2+β3ti3+β4ti4+β5ti5+β6ti6,(3)y0,i=α0+α1ti+α2ti2+α3ti3+α4ti4+α5ti5+α6ti6,where α0,…,α6 and β0,…,β6 in equations (2) and (3) are the parameters of the polynomial functions. Considering the kinematic continuity, v0,ix and v0,iy can be derived as ∂x0,i/∂ti and ∂y0,i/∂ti, respectively. a0,ix and a0,iy can be obtained as ∂v0,ix/∂ti and ∂v0,iy/∂ti, respectively. j0,ix and j0,iy can be further obtained as ∂a0,ix/∂ti and ∂a0,iy/∂ti, respectively. At the beginning of the lane change maneuver, the initial states (positions, speeds, and accelerations) for vehicles can be obtained through techniques such as autonomous perception or V2X communications. Since we know the initial state of vehicle 0 at time instant t0, the values for α0,α1,α2 and β0,β1,β2 can be derived as(4)α0=y0,0,α1=v0,0y,α2=a0,0y2,β0=x0,0,β1=v0,0x,β2=a0,0x2.In equation (4), we use y0,0, v0,0y, a0,0y, x0,0, v0,0x, and a0,0x to denote y0,i, v0,iy, a0,iy, x0,i, v0,ix, and a0,ix for vehicle 0 at the initial time instant i=0.We further define the speed of vehicle 0 at time instantT (i.e., for i=I) as v0,I:(5)v0,I=v0,Ix2+v0,Iy20.5,and define the acceleration of vehicle 0 at time instant T as a0,I:(6)a0,I=∂v0,I∂T=0.5v0,Ix2+v0,Iy2−0.52v0,Ixa0,Ix+2v0,Iya0,Iy.Let us denote the jerk of vehicle 1 asj1. The speed of vehicle 1 at time instant ti is(7)v1,i=v1,0+a1,0ti+12j1ti2,where v1,0 and a1,0 are the initial speed and acceleration of vehicle 1. Then, the curve distance between the position of vehicle 1 at time instant ti and the origin of the target lane can be obtained as(8)s1,i=s1,0+v1,0ti+12a1,0ti2+16j1ti3,where s1,0 is the initial curve distance of vehicle 1.


## 3.2. The Nonlinear Program

We construct a nonlinear program to plan the trajectory for vehicle 0 while estimating the motion of vehicle 1. The proposed program treatsα3, α4, α5, α6, β3, β4, β5, β6, T, and j1 as decision variables. As shown in Figure 2, the proposed nonlinear programming aims to minimize the safety risk and discomfort for vehicle 0 and vehicle 1, while improving the traveling efficiency of vehicle 0. The detailed formulation of the programming is given as(9a)min∑i=1Iρ0a0,ix2I+ρ1a0,iy2I+ρ2j0,ix2I+ρ3j0,iy2I+ρ4T2+ρ5j12,s.t.(9b)Y∗x0,I=α0+α1T+α2T2+α3T3+α4T4+α5T5+α6T6,(9c)v0,Iyv0,Ix=∂Y∗x∂x|x=x0,I,(9d)a0,I=ℂv0,I,v3,I,Δs,(9e)amin≤a1,I≤ℂv1,I,v0,I,Δd,(9f)r2≤x0,i−x1,i2+y0,i−y1,i2,fori=1,…,I,(9g)r2≤x0,i−x3,i2+y0,i−y3,i2,fori=1,…,I,(9h)m2≤x0,ik−x4,i12+y0,ik−y4,i12,fork=0,1,…,4,fori=1,…,I,(9i)jmin≤j1≤0,(9j)0≤v0,ix≤vmaxx,fori=1,…,I,(9k)0≤v0,iy≤vmaxy,fori=1,…,I,(9l)aminx≤a0,ix≤amaxx,fori=1,…,I,(9m)aminy≤a0,iy≤amaxy,fori=1,…,I,(9n)jminx≤j0,ix≤jmaxx,fori=1,…,I,(9o)jminy≤j0,iy≤jmaxy,fori=1,…,I,(9p)0≤T≤Tmax,(9q)x0,0≤x0,I≤x0,0+Δxmax,(9r)Δs≥0,(9s)Δd≥0.Figure 2
The outline of the proposed nonlinear programming.Equation (9a) is the objective function of the nonlinear program, where ρ0, ρ1, ρ2, ρ3, ρ4, and ρ5 are weight parameters. The objective function takes into account several factors, including driving comfort, safety, efficiency, and negative influence of the surrounding vehicles, where the combination method in the objective function has been validated in many works [36–38]. Since we aim to minimize the safety risk and discomfort for vehicle 0, the objective function contains the average quadratic acceleration and jerk of vehicle 0 (see the first four items). Then, we also seek to make the time horizon T of the lane change maneuver as short as possible to improve efficiency. Finally, we add j12 to the objective function because we consider that the driver of vehicle 1 always wishes to keep the jerk as close to zero as possible and aims to reduce the negative influence exerted by the lane change maneuver of vehicle 0.In constraint (9b),x0,I denotes the longitudinal position of vehicle 0 at the end of the lane change T. As shown in Figure 3, Y∗x is a function that returns the lateral position of the point on the center of the target lane, where the input is longitudinal position x. Constraint (9b) ensures that vehicle 0 at time instant T is located at the center of the target lane. We describe how to construct the function Y∗x in Section 3.3.Figure 3
Road curve functions and curve distance.Constraint (9c) is for the driving direction of vehicle 0 at time instant T. This constraint ensures that the ratio of the lateral speed to the longitudinal speed at time instant T is equal to the derivative of Y∗x at x0,I (the direction of the target lane).Constraint (9d) ensures that the value of a0,I defined by equation (6) must be equal to the acceleration determined by a car-following model. As shown by equation (6), a0,I can be considered a function of the decision variables α3, α4, α5, α6, β3, β4, β5, β6, and T. Constraint (9d) uses a car-following model to constrain the values of these variables.We choose the full velocity difference model (FVDM) proposed by Jiang et al. [39] as the underlying car-following model. According to the FVDM model, the car-following acceleration of vehicle 0 at time instant T is dependent on the speed of vehicle 0 at time instant T, the speed of vehicle 3 at time instant T, and the distance Δs between vehicle 0 and 3 at time instant T. Here, we use ℂv0,I,v3,I,Δs to denote the FVDM function that returns the acceleration of vehicle 0 at time instant T. This function is defined as(10)ℂv0,I,v3,I,Δs=κV1+V2tanhC1Δs−sc−C2−v0,I+λv3,I−v0,I,where κ, λ, sc, C1, C2, V1, and V2 are the prespecified parameters of the car-following model, where the values of them are inherited from the work of Jiang et al. [39]. v0,I is the speed of vehicle 0 at time instant T, whose value can be calculated by equation (5), and v3,I means the speed of vehicle 3 at time instant T, which can be computed as(11)v3,I=v3,0+a3,0T+12j3,0T2,where v3,0 is the speed of vehicle 3 at time instant t0, a3,0 is the acceleration of vehicle 3 at time instant t0, and j3,0 is the jerk of vehicle 3 at time instant t0. The curve distance between the position of vehicle 3 and the origin of the target lane at time instant T (see Figure 4) can be further obtained as(12)s3,I=s3,0+v3,0T+12a3,0T2+16j3,0T3,where s3,0 is the initial curve distance between the position of vehicle 3 and the origin of the target lane at time instant t0. The curve distance between vehicles 0 and 3 at time instant T can be obtained as(13)Δs=s3,I−S∗x0,I−l,where l is the length of a vehicle and S∗x0,I returns the curve distance between the position of vehicle 0 and the origin of the target lane at time instant T (see Figure 4).Figure 4
Curve distance-related function illustration.Constraint (9e) ensures that at the end of the lane change maneuver, the acceleration of vehicle 1, a1,I, is not smaller than the low boundary amin and is not larger than the safe acceleration of vehicle 1 provided by the FVDM. The car-following acceleration of vehicle 1 is obtained as ℂv1,I,v0,I,Δd, where we can calculate a1,I and v1,I as a1,0+j1T and v1,0+a1,0T+0.5j1T2, respectively. And Δd is denoted as the distance between vehicles 1 and 0 in the target lane at time instant T. Since the curve distance s1,I between the position of vehicle 1 and the origin of the target lane at time instant T is given as s1,0+v1,0T+1/2a1,0T2+1/6j1T3, the value of Δd can be derived as(14)Δd=S∗x0,I−s1,I−l.Constraint (9f) ensures that the Euclidean distance between vehicles 0 and 1 is greater than the diagonal length r of a vehicle at time instant ti. In constraint (9f), x1,i and y1,i denote the longitudinal and lateral positions of vehicle 1 at time instant ti, respectively. We obtain x1,i as x1,i=X∗s1,i, where s1,i is given by equation (8) and X∗s is a function that returns the longitudinal position x for a point located at the center of the target lane whose curve distance is s (see Figure 4). y1,i can be further obtained as y1,i=Y∗x1,i.Constraint (9g) ensures that the distance between vehicles 0 and 3 must be larger than the diagonal length r of a vehicle. We use x3,i and y3,i to denote the longitudinal and lateral positions of vehicle 3 at time instant ti, respectively. These values can be obtained as x3,i=X∗s3,i and y3,i=Y∗x3,i, respectively.Constraint (9h) is for the distance between vehicles 0 and 4. Similar to the work in Ziegler et al. [23], we decompose a vehicle shape into five circles with diameter m (see Figure 5). Then, these circles are indexed as k=0,1,…,4, where the center of the 0-th circles is also the centroid of the vehicle. We use x0,ik and y0,ik to denote the longitudinal and lateral positions of the center of the k-th circle of vehicle 0 at time instant ti, respectively. The position x0,ik,y0,ik for k=0,1,…,4 can be derived from x0,i,y0,i. We denote x4,i,k and y4,i,k as the longitudinal and lateral positions of the k-th circle’s center for vehicle 4 at time instant ti, respectively. x4,i and y4,i are obtained as Xs4,i and Yx4,i, respectively. s4,i is the curve distance between the position of vehicle 4 and the origin of the current lane at time instant ti. We employ the following equation to predict s4,i:(15)s4,i=s4,0+v4,0ti+12a4,0ti2+16j4,0ti3,where s4,0 is the curve distance between the position of vehicle 4 and the origin of the current lane at time instant t0, v4,0 is the speed of vehicle 4 at time instant t0, a4,0 is the acceleration of vehicle 4 at time instant t0, and j4,0 is the jerk of vehicle 4 at time instant t0.Figure 5
Decomposing a vehicle shape into five circles.Constraint (9i) requires the jerk of vehicle 1 to be a nonpositive value. Constraints (9j) to (9o) are for the bounds of v0,ix, v0,iy, a0,ix, a0,iy, j0,ix, and j0,iy with i=1,…,I for vehicle 0.Constraint (9p) enforces the range of the time horizon T from 0 to Tmax, where Tmax denotes the maximum allowed length of the time horizon for the lane change maneuver. Constraint (9q) enforces the range of longitudinal position of vehicle 0 from x0,0 to x0,0+Δxmax, where Δxmax is the maximum allowed movement distance of vehicle 0 along the x-axis for the lane change maneuver.


## 3.3. Combining Road Curve Information by Polynomial Regression Model

The proposed model can execute trajectory planning on curved roads. We use functionsYxY∗x, S(x), S∗(x), and Xs to combine curve information into the nonlinear program. Previous studies (Lundquist et al. [40]; Fatemi et al. [41]; and Hammarstrand et al. [42]) confirmed that polynomial regression models can approximate road curves well in Cartesian coordinates. Following these studies, we also employ the polynomial regression model to approximate Yx, Y∗x, S(x), S∗(x), and Xs. First, we can obtain sampling points among the road curves from a high-definition map, such as the grey points in Figure 6. Then, we adopt the polynomial regression model to estimate the road curves (i.e., the grey lines in Figure 6) and obtain the internal parameters through the least squares method. This method is not required to know the geometric design of a road and is not restricted by the type of curve.Figure 6
Road curve fitting using the polynomial regression model.


## 4. Solution Techniques

We use the SQP algorithm to solve the nonlinear programming defined by equations (9). Although the SQP algorithm may not obtain a global solution for the nonlinear problem, previous studies indicated that it is not necessary to find a global solution for the trajectory planning problem [23, 43, 44]. This is because a feasible solution can satisfy the requirements for safety and comfort during the lane change maneuver.A random initial guess for the solution may lead to a failure for the SQP algorithm to find a feasible solution. This study develops a simple and effective way to find the initial guess. To obtain an initial solution for SQP, we setα6=0, β6=0,j1=0, and T=T^ (T^ is a prespecified parameter). Therefore, the remaining unknown variables are α3, α4, α5, β3, β4, and β5, which can be solved through equations (16)–(21).(16)β0+β1T^+β2T^2+β3T^3+β4T^4+β5T^5−x^0,I=0,(17)α0+α1T^+α2T^2+α3T^3+α4T^4+α5T^5−Y∗x^0,I=0,(18)β1+2β2T^+3β3T^2+4β4T^3+5β5T^4−v^0,Ix=0,(19)α1+2α2T^+3α3T^2+4α4T^3+5α5T^4−v^0,Iy=0,(20)2β2+6β3T^+12β4T^2+20β5T^3−a^0,Ix=0,(21)2α2+6α3T^+12α4T^2+20α5T^3−a^0,Iy=0.In equations (16)–(21), the values for T^, x^0,I, Y∗x^0,I, v^0,Ix, v^0,Iy, a^0,Ix, and a^0,Iy are prespecified values, which can be computed using analytical methods according to the kinematic and geological relationship as follows.In equation (22), we use x^0,I to denote a prespecified value for x0,I. Here, we suggest prespecifying the value of Δs as Δs^ first and then determining x^0,I according to the specified Δs^ and T^ by the following equations:(22)Δs^=s^3,I−S∗x^0,I−l,(23)s^3,I=v3,0T^+12a3,0T^2+16j3,0T^3,where x^0,I is the only unknown variable in equations (22) and (23), whose value can be computed by solving the equations. For equations (18) and (19), we can derive v^0,Ix and v^0,Iy based on v^0,I, using the orientation of the road curves. We assume that the speed of vehicle 0 is equal to the speed of vehicle 3 at time instant T^ and thus get v^0,I=v^3,I=v3,0+a3,0T^+1/2j3,0T^2. Similarly, for equations (20) and (21), we determine a^0,Ix and a^0,Iy according to a^0,I which can be computed using ℂv^0,I,v^3,I,Δs^.After determiningx^0,I, v^0,Ix, v^0,Iy, a^0,Ix, and a^0,Iy, the remaining unknown variables of equations (16)–(21) are α3, α4, α5, β3, β4, and β5. We can find a solution for these variables using the Newton method. Then, the initial guess α^3, α^4, α^5, β^3, β^4, and β^5 can be obtained.Finally, we set the initial guess for the proposed nonlinear programming asα3=α^3, α4=α^4, α5=α^5, α6=0β3=β^3, β4=β^4, β5=β^5, β6=0,j1=0, and T=T^.


## 5. Numerical Examples

In this section, we generate a series of numerical examples and demonstrate the effectiveness of the proposed trajectory planning model. This section is organized as follows: we first present the method for generating random test scenarios and hyperparameter setting. Then, we provide descriptive statistics to summarize the test results. Subsequently, we show the details of the proposed trajectory planning model using four scenarios. Finally, sensitivity analysis is conducted to illustrate the impact on the feasibility and computation burden of the algorithm under different factors.


### 5.1. Traffic Scenario Generation

In this section, we introduce the method for generating the test scenarios. As stated before, the initial state information includes position, speed, acceleration, and jerk.


#### 5.1.1. Vehicle Speed Generation

(i)
Generate the speed for vehicle 0 asv0,0∼U15,20, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 15 m/s and 20 m/s, respectively.(ii)
Generate the speed for vehicle 1 asv1,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iii)
Generate the speed for vehicle 3 asv3,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iv)
Generate the speed for vehicle 4 asv4,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.


#### 5.1.2. Vehicle Longitudinal Position Generation

(i)
Generate the distance between vehicle 1 and vehicle 3 as(24)d1,3=maxv1,0,v3,0theadway,wheretheadway follows a Weibull distribution where the shape parameter and scale parameter are defined as 5.955 and 1.3829, respectively [45].(ii)
Generate the distance between vehicle 0 and vehicle 4 as(25)d0,4=maxv0,0,v4,0theadway,wheretheadway follows Weibull distribution, where the shape parameter and scale parameter are 5.955 and 1.3829, respectively.(ii)
We assume that the initial value of vehicle 1 isx1,0=300. Then, we can get x3,0 = x1,0+d1,3.(iii)
Generatex0,0 from the uniform distribution x0,0∼Ux1,0+v0,0tsafety,x3,0−v0,0tsafety, where we assume that the time gap of the vehicle 0 and 1 is larger than the minimum value (tsafety, and the relationship for the time gap of vehicles 0 and 3 is similar to the time gap of vehicles 0 and 1. The value of tsafety (1.6 s) is specified based on the model proposed by Kesting et al. [29].(iv)
Getx4,0=x0,0+d0,4.Because we assume the vehicles are in the steady traffic flow, the acceleration and jerk of vehicles 0, 1, 3, and 4 at the beginning time of a lane change maneuver area0,0=0, a1,0=0, a3,0=0, a4,0=0, j0=0, j1=0, j3=0, and j4=0.Finally, we generate 1000 scenarios to test the effectiveness of the proposed lane change trajectory algorithm. The statistics of generated scenarios are outlined in Table1, where the mean, 5-th percentile, and 95-th percentile statistical indicators are adopted.Moreover, we test the proposed model on a section of a road with a transition curve. We set the width of a lane as 3.5 m. The values of hyperparameters are specified based on Nilsson et al. [46] and are listed in Table 2.Table 1
Statistics of generated scenarios.
SymbolsExplanationsMean5%95%x0,0Initial longitudinal position of vehicle 0 (m)353.80327.50435.47v0,0Initial speed of vehicle 0 (m/s)17.4615.2919.69x1,0Initial longitudinal position of vehicle 1 (m)300.00300.00300.00v1,0Initial speed of vehicle 1 (m/s)17.4414.7220.38x3,0Initial longitudinal position of vehicle 3 (m)408.95357.31529.70v3,0Initial speed of vehicle 3 (m/s)17.4514.3520.87x4,0Initial longitudinal position of vehicle 4 (m)462.92390.41606.45v4,0Initial speed of vehicle 4 (m/s)17.38614.6320.32Table 2
Hyperparameter setting.
HyperparametersValuesHyperparameters of car-following modelPrespecified parameter (s−1), κ0.4Prespecified parameter,λ0.5Prespecified parameter (m),Sc4.8Prespecified parameter (m−1), C10.13Prespecified parameter,C21.57Prespecified parameter (m/s),V16.75Prespecified parameter (m/s),V27.91Hyperparameters of vehicle 1Bounds of the acceleration (m/s2), amin−4Bounds of the jerk (m/s3), jmin−3Hyperparameters of vehicle 0Bounds of the speed (m/s),vmaxx,vmaxy30, 30Bounds for the acceleration (m/s2), aminx,amaxx,aminy,amaxy−3, 3, −3, 3Bounds for the jerk (m/s3), jminx, jmaxx, jminy, jmaxy−3, 2, −3, 2Hyperparameters of the nonlinear programVehicle length, vehicle width (m)4.8, 1.8Weights of the objective function,ρ0, ρ1, ρ2, ρ3, ρ4,ρ51, 1, 1, 1, 20, 20Maximum length of the time horizon (s),Tmax10Maximum movement distance of vehicle 0 along thex-axis (m), Δxmax200Hyperparameters of the random initial algorithmPrespecified time horizon (s),T^3Prespecified ending distance of vehicle 0 and vehicle 3 (m),Δs^100


### 5.2. Statistics of Results

The proposed lane change trajectory algorithm can solve most of the scenarios (92.9%). Table3 lists the statistics of the results. The proposed algorithm can determine the value of T flexibly, where the mean is 4.45 s. Also, the proposed model can determine the jerk j1 of vehicle 1 automatically, which ranges from −0.47 to −0.23. This suggests that our model can avoid a drastic speed change for the following vehicle in the target lane.Table 3
Statistics of trajectory planning results.
SymbolsMean5-th percentile95-th percentileT (s)4.453.3310.17x0,I−x0,0 (m)65.1251.31102.11v0,I (m/s)15.9311.4318.38a0,I (m/s2)2.541.564.05j1 (m/s3)-0.23-0.47-0.019The numerical tests are conducted on a laptop with a 1.70 GHz i5-3317U CPU having 8.00 GB of RAM running on Windows 10. The whole procedure of optimization is provided by the SQP solver in the MATLAB Optimization Toolbox. Figure7 presents the statistics of CPU time for computation. The 15%, 50%, and 95% percentiles of CPU time for computation are 0.44, 5.74, 11.83 seconds. Furthermore, the histogram in Figure 7 shows that a considerable proportion can obtain a feasible solution in less than 1.66 seconds. We believe that improvement in the language and optimization box (i.e., using C language or AMPL) can further reduce the computation time significantly.Figure 7
CPU time for solving the SQP algorithm.Figure8 shows the factors that affect CPU time. As shown in the color bar on the right side, the darker red color means the longer CPU time, while the lighter red color means the solution can be obtained in a relatively short CPU time. The x-, y-, z-axes in Figure 8 represent the time headway between vehicle 0 and its surrounding vehicles (vehicle 1, vehicle 3, and vehicle 4). Figure 8 indicates that with the time headway becoming longer, the required CPU time can decrease significantly (the color becomes lighter).Figure 8
Relationship between CPU time and headways.


### 5.3. Case Analysis

In this section, we attempt to use numerical examples to demonstrate that the proposed trajectory planning method can generate an appropriate trajectory for the controlled vehicle to meet several situations.As shown in Table4, we prepare four scenarios to test the trajectory planning method. In scenarios 1 and 2, vehicle 0 changes from a lane with low speed to a lane with fast speed. Compared to scenario 2, the initial position of vehicle 1 in scenario 1 is closer to that of vehicle 0, and it drives with a higher initial speed. However, in scenarios 3 and 4, vehicle 0 changes from a lane with a fast speed to a lane at a low speed. Compared to scenario 3, the average speed of the vehicles in the target lane of scenario 4 is slower than that of the vehicles in the target lane. For vehicles 1, 3, and 4, we set initial accelerations and jerks as a1,0=0, a3,0=0, a4,0=0, j3=0, and j4=0.Table 4
Test scenarios for trajectory planning.
SymbolsScenarios1234x0,0 (m)360360360365s0,0 (m)360.51360.51360.51365.53v0,0 (m/s)15152020x1,0 (m)330260350350s1,0 (m)330.61260.52350.70350.70v1,0 (m/s)25201510x3,0 (m)460460460460s3,0 (m)461.81461.81461.81461.81v3,0 (m/s)20201510x4,0 (m)420420420420s4,0 (m)420.94420.94420.94420.94v4,0 (m/s)15152020We observe that the computation times for scenarios 1 to 4 are 2.59 s, 0.60 s, 0.49 s, and 0.65 s, respectively.Figure9(a) exhibits the time-dependent longitudinal and lateral positions’ dynamics of vehicle 0’s planned trajectory under scenarios 1 and 2. Figure 9(b) shows the planned trajectories of vehicle 0 under scenarios 3 and 4. As reported in Table 5, the value of x0,I in scenario 1 is significantly larger than that in scenario 2. And the total time horizon T of the lane change maneuver under scenario 1 is also greater than that under scenario 2, whereas vehicle 0 is closer to vehicle 3 at the end of the lane change maneuver under scenario 1. These features suggest that when planning the lane change trajectory for the controlled vehicle, the small distance between the controlled vehicle and its surrounding vehicle can lead to a complex and dangerous scenario, which may require more time for finishing the lane change maneuver. Moreover, the time-related positions of vehicle 0 and its surrounding vehicles are depicted in detail in Figure 10. Through different time indexes, we can observe that vehicle 0 for the 4 scenarios can keep a safe distance from its surrounding vehicles during the planning horizon, which can validate the safety of the proposed algorithm.Figure 9
Planned trajectory of vehicle 0 for scenarios 1, 2, 3, and 4.
(a)(b)(c)(d)(e)(f)(g)(h)(i)(j)Table 5
Trajectory planning results for test scenarios.
SymbolsScenarios1234T (s)4.623.693.763.79x0,I−x0,0 (m)83.9656.6269.0866.83v0,I (m/s)19.6516.7216.6614.82a0,I (m/s2)−2.090.81−1.63−2.47Δd (m)13.2286.3224.1644.28s1,I−s1,0 (m)101.3270.5455.2437.90v1,I (m/s)15.7517.3814.0310a1,I (m/s2)−4−1.42−0.520j1 (m/s3)−0.87−0.38−0.140Δs (m)109.09118.1288.266.86Figure 10
Time-related dynamics between vehicles 0, 1, 3, and 4 under scenarios 1, 2, 3, and 4. (a) Time-related trajectory planning results for scenario 1. (b) Time-related trajectory planning results for scenario 2. (c) Time-related trajectory planning results for scenario 3. (d) Time-related trajectory planning results for scenario 4.
(a)(b)(c)(d)Since the proposed nonlinear programming can simultaneously consider the motions of vehicles 0 and 1, the planned trajectory of vehicle 0 can vary with different initial states of vehicle 1. As presented by Figures9(c) and 9(e), the speed on the x-/y-axis of vehicle 0 under scenario 1 tends to be faster than that under scenario 2. This is because compared to scenario 2, the initial speed of vehicle 1 is faster, and the initial position of vehicle 1 is closer to vehicle 0 under scenario 1. Table 5 further shows that the speed of vehicle 0 at the end of the lane change, v0,I, under scenario 1 is 19.65 m/s, which approaches the speed of vehicle 3.From Figures9(g) and 9(i), we can find that vehicle 0 under scenario 1 tends to drive with a higher acceleration compared to scenario 2. At the beginning of the lane change, the acceleration on the x-axis of vehicle 0 under scenario 1 grows rapidly and then diminishes gradually. However, the acceleration along the x-axis of vehicle 0 under scenario 2 increases slowly from the initial instant to the end instant during the lane change maneuver.Figures9(d) and 9(f) show the speed information for vehicle 0 under scenarios 3 and 4. Figures 9(h) and 9(j) provide the acceleration information for vehicle 0 under scenarios 3 and 4. Since the average speed of vehicles in the target lane in scenario 4 is slower than that in scenario 3, vehicle 0 tends to decelerate more dramatically in scenario 4 than in scenario 3.Table5 reports the ending state of vehicle 1 at time instant T. As mentioned in Section 2, we consider that the driver of vehicle 1 is not willing to reduce the speed too much and wishes to keep the jerk as close to zero as possible. Our proposed nonlinear program can optimize the possible jerk for vehicle 1 under this assumption. Table 5 shows that the jerk of vehicle 1 under scenario 1 is smaller than that under scenario 2 because vehicle 1 needs to decelerate more dramatically to avoid colliding with vehicle 0 in scenario 1. In scenario 4, since the initial speed of vehicle 1 is slow enough, vehicle 1 can remain safe, even if the jerk of vehicle 1 is 0.


### 5.4. Sensitivity Analysis

In this section, we conduct a series of sensitivity analyses to explore the feasibility of the proposed algorithm as well as the computational load of the algorithm in detail. First, we use scenario 1 (Table4) as the base scenario. Then, we change the distance between vehicle 0 and vehicle 1 (Figure 11), the distance between vehicle 0 and vehicle 3 (Figure 12), and the distance between vehicle 0 and vehicle 4 (Figure 13) alternately. We set the distance range from 2 m to 150 m and then determine the longitudinal position of the corresponding surrounding vehicle. At the same time, other parameters are set as the same as the base case.Figure 11
Effect of the distance between vehicle 0 and vehicle 1. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)Figure 12
Effect of the distance between vehicle 0 and vehicle 3. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)Figure 13
Effect of the distance between vehicle 0 and vehicle 4. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)In order to take the sensitivity analysis between vehicle 0 and vehicle 1 (Figure11), we generate 75 scenarios where the values of x0,0, v0,0, v1,0, x3,0, v3,0, x4,0, and v4,0 are the same as those in the base scenario (scenario 1) while the value of x1,0 ranges from 358 m to 210 m (x0,0=360m in scenario 1). Here, we generate a scenario analysis every 2 m, so x1,0=358,356,…,212,210m for 75 generated scenarios. Therefore, we can derive that the distance between vehicle 0 and vehicle 1 is 2,4,…,148,150m for the 75 generated scenarios. Then, the proposed lane change planning algorithm is executed to check the feasibility of the algorithm and CPU computation load under the 75 generated scenarios.It can be seen that the feasibility of the proposed lane-changing planning model can be guaranteed when the distance between the vehicles is larger than a certain value (28 m in Figure11(a), 100 m in Figure 12(a), and 22 m in Figure 13(a)). The larger distance between the controlled vehicle and its surrounding vehicles can decrease the required computational CPU time (Figures 11(b), 12(b), and 13(b)). This indicates that a scenario with a little distance between the controlled vehicle and its surrounding vehicles means a dangerous or even infeasible lane-changing driving situation and thus involves complex maneuvers to complete the lane-changing task (therefore, the optimization toolbox needs to take longer time to find a feasible solution). We also find that the leading vehicle in the target lane is the most critical factor for the lane change maneuver. The ego vehicle is able to execute the lane change maneuver if and only if a large enough distance (larger than 100m) between the ego vehicle and the leading vehicle in the target lane is available.


## 5.1. Traffic Scenario Generation

In this section, we introduce the method for generating the test scenarios. As stated before, the initial state information includes position, speed, acceleration, and jerk.


### 5.1.1. Vehicle Speed Generation

(i)
Generate the speed for vehicle 0 asv0,0∼U15,20, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 15 m/s and 20 m/s, respectively.(ii)
Generate the speed for vehicle 1 asv1,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iii)
Generate the speed for vehicle 3 asv3,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iv)
Generate the speed for vehicle 4 asv4,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.


### 5.1.2. Vehicle Longitudinal Position Generation

(i)
Generate the distance between vehicle 1 and vehicle 3 as(24)d1,3=maxv1,0,v3,0theadway,wheretheadway follows a Weibull distribution where the shape parameter and scale parameter are defined as 5.955 and 1.3829, respectively [45].(ii)
Generate the distance between vehicle 0 and vehicle 4 as(25)d0,4=maxv0,0,v4,0theadway,wheretheadway follows Weibull distribution, where the shape parameter and scale parameter are 5.955 and 1.3829, respectively.(ii)
We assume that the initial value of vehicle 1 isx1,0=300. Then, we can get x3,0 = x1,0+d1,3.(iii)
Generatex0,0 from the uniform distribution x0,0∼Ux1,0+v0,0tsafety,x3,0−v0,0tsafety, where we assume that the time gap of the vehicle 0 and 1 is larger than the minimum value (tsafety, and the relationship for the time gap of vehicles 0 and 3 is similar to the time gap of vehicles 0 and 1. The value of tsafety (1.6 s) is specified based on the model proposed by Kesting et al. [29].(iv)
Getx4,0=x0,0+d0,4.Because we assume the vehicles are in the steady traffic flow, the acceleration and jerk of vehicles 0, 1, 3, and 4 at the beginning time of a lane change maneuver area0,0=0, a1,0=0, a3,0=0, a4,0=0, j0=0, j1=0, j3=0, and j4=0.Finally, we generate 1000 scenarios to test the effectiveness of the proposed lane change trajectory algorithm. The statistics of generated scenarios are outlined in Table1, where the mean, 5-th percentile, and 95-th percentile statistical indicators are adopted.Moreover, we test the proposed model on a section of a road with a transition curve. We set the width of a lane as 3.5 m. The values of hyperparameters are specified based on Nilsson et al. [46] and are listed in Table 2.Table 1
Statistics of generated scenarios.
SymbolsExplanationsMean5%95%x0,0Initial longitudinal position of vehicle 0 (m)353.80327.50435.47v0,0Initial speed of vehicle 0 (m/s)17.4615.2919.69x1,0Initial longitudinal position of vehicle 1 (m)300.00300.00300.00v1,0Initial speed of vehicle 1 (m/s)17.4414.7220.38x3,0Initial longitudinal position of vehicle 3 (m)408.95357.31529.70v3,0Initial speed of vehicle 3 (m/s)17.4514.3520.87x4,0Initial longitudinal position of vehicle 4 (m)462.92390.41606.45v4,0Initial speed of vehicle 4 (m/s)17.38614.6320.32Table 2
Hyperparameter setting.
HyperparametersValuesHyperparameters of car-following modelPrespecified parameter (s−1), κ0.4Prespecified parameter,λ0.5Prespecified parameter (m),Sc4.8Prespecified parameter (m−1), C10.13Prespecified parameter,C21.57Prespecified parameter (m/s),V16.75Prespecified parameter (m/s),V27.91Hyperparameters of vehicle 1Bounds of the acceleration (m/s2), amin−4Bounds of the jerk (m/s3), jmin−3Hyperparameters of vehicle 0Bounds of the speed (m/s),vmaxx,vmaxy30, 30Bounds for the acceleration (m/s2), aminx,amaxx,aminy,amaxy−3, 3, −3, 3Bounds for the jerk (m/s3), jminx, jmaxx, jminy, jmaxy−3, 2, −3, 2Hyperparameters of the nonlinear programVehicle length, vehicle width (m)4.8, 1.8Weights of the objective function,ρ0, ρ1, ρ2, ρ3, ρ4,ρ51, 1, 1, 1, 20, 20Maximum length of the time horizon (s),Tmax10Maximum movement distance of vehicle 0 along thex-axis (m), Δxmax200Hyperparameters of the random initial algorithmPrespecified time horizon (s),T^3Prespecified ending distance of vehicle 0 and vehicle 3 (m),Δs^100


## 5.1.1. Vehicle Speed Generation

(i)
Generate the speed for vehicle 0 asv0,0∼U15,20, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 15 m/s and 20 m/s, respectively.(ii)
Generate the speed for vehicle 1 asv1,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iii)
Generate the speed for vehicle 3 asv3,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.(iv)
Generate the speed for vehicle 4 asv4,0∼U0.9v0,0,1.1v0,0, where we assume the speed is following the uniform distribution, and the minimum and maximum values are 0.9v0,0 and 1.1v0,0 respectively.


## 5.1.2. Vehicle Longitudinal Position Generation

(i)
Generate the distance between vehicle 1 and vehicle 3 as(24)d1,3=maxv1,0,v3,0theadway,wheretheadway follows a Weibull distribution where the shape parameter and scale parameter are defined as 5.955 and 1.3829, respectively [45].(ii)
Generate the distance between vehicle 0 and vehicle 4 as(25)d0,4=maxv0,0,v4,0theadway,wheretheadway follows Weibull distribution, where the shape parameter and scale parameter are 5.955 and 1.3829, respectively.(ii)
We assume that the initial value of vehicle 1 isx1,0=300. Then, we can get x3,0 = x1,0+d1,3.(iii)
Generatex0,0 from the uniform distribution x0,0∼Ux1,0+v0,0tsafety,x3,0−v0,0tsafety, where we assume that the time gap of the vehicle 0 and 1 is larger than the minimum value (tsafety, and the relationship for the time gap of vehicles 0 and 3 is similar to the time gap of vehicles 0 and 1. The value of tsafety (1.6 s) is specified based on the model proposed by Kesting et al. [29].(iv)
Getx4,0=x0,0+d0,4.Because we assume the vehicles are in the steady traffic flow, the acceleration and jerk of vehicles 0, 1, 3, and 4 at the beginning time of a lane change maneuver area0,0=0, a1,0=0, a3,0=0, a4,0=0, j0=0, j1=0, j3=0, and j4=0.Finally, we generate 1000 scenarios to test the effectiveness of the proposed lane change trajectory algorithm. The statistics of generated scenarios are outlined in Table1, where the mean, 5-th percentile, and 95-th percentile statistical indicators are adopted.Moreover, we test the proposed model on a section of a road with a transition curve. We set the width of a lane as 3.5 m. The values of hyperparameters are specified based on Nilsson et al. [46] and are listed in Table 2.Table 1
Statistics of generated scenarios.
SymbolsExplanationsMean5%95%x0,0Initial longitudinal position of vehicle 0 (m)353.80327.50435.47v0,0Initial speed of vehicle 0 (m/s)17.4615.2919.69x1,0Initial longitudinal position of vehicle 1 (m)300.00300.00300.00v1,0Initial speed of vehicle 1 (m/s)17.4414.7220.38x3,0Initial longitudinal position of vehicle 3 (m)408.95357.31529.70v3,0Initial speed of vehicle 3 (m/s)17.4514.3520.87x4,0Initial longitudinal position of vehicle 4 (m)462.92390.41606.45v4,0Initial speed of vehicle 4 (m/s)17.38614.6320.32Table 2
Hyperparameter setting.
HyperparametersValuesHyperparameters of car-following modelPrespecified parameter (s−1), κ0.4Prespecified parameter,λ0.5Prespecified parameter (m),Sc4.8Prespecified parameter (m−1), C10.13Prespecified parameter,C21.57Prespecified parameter (m/s),V16.75Prespecified parameter (m/s),V27.91Hyperparameters of vehicle 1Bounds of the acceleration (m/s2), amin−4Bounds of the jerk (m/s3), jmin−3Hyperparameters of vehicle 0Bounds of the speed (m/s),vmaxx,vmaxy30, 30Bounds for the acceleration (m/s2), aminx,amaxx,aminy,amaxy−3, 3, −3, 3Bounds for the jerk (m/s3), jminx, jmaxx, jminy, jmaxy−3, 2, −3, 2Hyperparameters of the nonlinear programVehicle length, vehicle width (m)4.8, 1.8Weights of the objective function,ρ0, ρ1, ρ2, ρ3, ρ4,ρ51, 1, 1, 1, 20, 20Maximum length of the time horizon (s),Tmax10Maximum movement distance of vehicle 0 along thex-axis (m), Δxmax200Hyperparameters of the random initial algorithmPrespecified time horizon (s),T^3Prespecified ending distance of vehicle 0 and vehicle 3 (m),Δs^100


## 5.2. Statistics of Results

The proposed lane change trajectory algorithm can solve most of the scenarios (92.9%). Table3 lists the statistics of the results. The proposed algorithm can determine the value of T flexibly, where the mean is 4.45 s. Also, the proposed model can determine the jerk j1 of vehicle 1 automatically, which ranges from −0.47 to −0.23. This suggests that our model can avoid a drastic speed change for the following vehicle in the target lane.Table 3
Statistics of trajectory planning results.
SymbolsMean5-th percentile95-th percentileT (s)4.453.3310.17x0,I−x0,0 (m)65.1251.31102.11v0,I (m/s)15.9311.4318.38a0,I (m/s2)2.541.564.05j1 (m/s3)-0.23-0.47-0.019The numerical tests are conducted on a laptop with a 1.70 GHz i5-3317U CPU having 8.00 GB of RAM running on Windows 10. The whole procedure of optimization is provided by the SQP solver in the MATLAB Optimization Toolbox. Figure7 presents the statistics of CPU time for computation. The 15%, 50%, and 95% percentiles of CPU time for computation are 0.44, 5.74, 11.83 seconds. Furthermore, the histogram in Figure 7 shows that a considerable proportion can obtain a feasible solution in less than 1.66 seconds. We believe that improvement in the language and optimization box (i.e., using C language or AMPL) can further reduce the computation time significantly.Figure 7
CPU time for solving the SQP algorithm.Figure8 shows the factors that affect CPU time. As shown in the color bar on the right side, the darker red color means the longer CPU time, while the lighter red color means the solution can be obtained in a relatively short CPU time. The x-, y-, z-axes in Figure 8 represent the time headway between vehicle 0 and its surrounding vehicles (vehicle 1, vehicle 3, and vehicle 4). Figure 8 indicates that with the time headway becoming longer, the required CPU time can decrease significantly (the color becomes lighter).Figure 8
Relationship between CPU time and headways.


## 5.3. Case Analysis

In this section, we attempt to use numerical examples to demonstrate that the proposed trajectory planning method can generate an appropriate trajectory for the controlled vehicle to meet several situations.As shown in Table4, we prepare four scenarios to test the trajectory planning method. In scenarios 1 and 2, vehicle 0 changes from a lane with low speed to a lane with fast speed. Compared to scenario 2, the initial position of vehicle 1 in scenario 1 is closer to that of vehicle 0, and it drives with a higher initial speed. However, in scenarios 3 and 4, vehicle 0 changes from a lane with a fast speed to a lane at a low speed. Compared to scenario 3, the average speed of the vehicles in the target lane of scenario 4 is slower than that of the vehicles in the target lane. For vehicles 1, 3, and 4, we set initial accelerations and jerks as a1,0=0, a3,0=0, a4,0=0, j3=0, and j4=0.Table 4
Test scenarios for trajectory planning.
SymbolsScenarios1234x0,0 (m)360360360365s0,0 (m)360.51360.51360.51365.53v0,0 (m/s)15152020x1,0 (m)330260350350s1,0 (m)330.61260.52350.70350.70v1,0 (m/s)25201510x3,0 (m)460460460460s3,0 (m)461.81461.81461.81461.81v3,0 (m/s)20201510x4,0 (m)420420420420s4,0 (m)420.94420.94420.94420.94v4,0 (m/s)15152020We observe that the computation times for scenarios 1 to 4 are 2.59 s, 0.60 s, 0.49 s, and 0.65 s, respectively.Figure9(a) exhibits the time-dependent longitudinal and lateral positions’ dynamics of vehicle 0’s planned trajectory under scenarios 1 and 2. Figure 9(b) shows the planned trajectories of vehicle 0 under scenarios 3 and 4. As reported in Table 5, the value of x0,I in scenario 1 is significantly larger than that in scenario 2. And the total time horizon T of the lane change maneuver under scenario 1 is also greater than that under scenario 2, whereas vehicle 0 is closer to vehicle 3 at the end of the lane change maneuver under scenario 1. These features suggest that when planning the lane change trajectory for the controlled vehicle, the small distance between the controlled vehicle and its surrounding vehicle can lead to a complex and dangerous scenario, which may require more time for finishing the lane change maneuver. Moreover, the time-related positions of vehicle 0 and its surrounding vehicles are depicted in detail in Figure 10. Through different time indexes, we can observe that vehicle 0 for the 4 scenarios can keep a safe distance from its surrounding vehicles during the planning horizon, which can validate the safety of the proposed algorithm.Figure 9
Planned trajectory of vehicle 0 for scenarios 1, 2, 3, and 4.
(a)(b)(c)(d)(e)(f)(g)(h)(i)(j)Table 5
Trajectory planning results for test scenarios.
SymbolsScenarios1234T (s)4.623.693.763.79x0,I−x0,0 (m)83.9656.6269.0866.83v0,I (m/s)19.6516.7216.6614.82a0,I (m/s2)−2.090.81−1.63−2.47Δd (m)13.2286.3224.1644.28s1,I−s1,0 (m)101.3270.5455.2437.90v1,I (m/s)15.7517.3814.0310a1,I (m/s2)−4−1.42−0.520j1 (m/s3)−0.87−0.38−0.140Δs (m)109.09118.1288.266.86Figure 10
Time-related dynamics between vehicles 0, 1, 3, and 4 under scenarios 1, 2, 3, and 4. (a) Time-related trajectory planning results for scenario 1. (b) Time-related trajectory planning results for scenario 2. (c) Time-related trajectory planning results for scenario 3. (d) Time-related trajectory planning results for scenario 4.
(a)(b)(c)(d)Since the proposed nonlinear programming can simultaneously consider the motions of vehicles 0 and 1, the planned trajectory of vehicle 0 can vary with different initial states of vehicle 1. As presented by Figures9(c) and 9(e), the speed on the x-/y-axis of vehicle 0 under scenario 1 tends to be faster than that under scenario 2. This is because compared to scenario 2, the initial speed of vehicle 1 is faster, and the initial position of vehicle 1 is closer to vehicle 0 under scenario 1. Table 5 further shows that the speed of vehicle 0 at the end of the lane change, v0,I, under scenario 1 is 19.65 m/s, which approaches the speed of vehicle 3.From Figures9(g) and 9(i), we can find that vehicle 0 under scenario 1 tends to drive with a higher acceleration compared to scenario 2. At the beginning of the lane change, the acceleration on the x-axis of vehicle 0 under scenario 1 grows rapidly and then diminishes gradually. However, the acceleration along the x-axis of vehicle 0 under scenario 2 increases slowly from the initial instant to the end instant during the lane change maneuver.Figures9(d) and 9(f) show the speed information for vehicle 0 under scenarios 3 and 4. Figures 9(h) and 9(j) provide the acceleration information for vehicle 0 under scenarios 3 and 4. Since the average speed of vehicles in the target lane in scenario 4 is slower than that in scenario 3, vehicle 0 tends to decelerate more dramatically in scenario 4 than in scenario 3.Table5 reports the ending state of vehicle 1 at time instant T. As mentioned in Section 2, we consider that the driver of vehicle 1 is not willing to reduce the speed too much and wishes to keep the jerk as close to zero as possible. Our proposed nonlinear program can optimize the possible jerk for vehicle 1 under this assumption. Table 5 shows that the jerk of vehicle 1 under scenario 1 is smaller than that under scenario 2 because vehicle 1 needs to decelerate more dramatically to avoid colliding with vehicle 0 in scenario 1. In scenario 4, since the initial speed of vehicle 1 is slow enough, vehicle 1 can remain safe, even if the jerk of vehicle 1 is 0.


## 5.4. Sensitivity Analysis

In this section, we conduct a series of sensitivity analyses to explore the feasibility of the proposed algorithm as well as the computational load of the algorithm in detail. First, we use scenario 1 (Table4) as the base scenario. Then, we change the distance between vehicle 0 and vehicle 1 (Figure 11), the distance between vehicle 0 and vehicle 3 (Figure 12), and the distance between vehicle 0 and vehicle 4 (Figure 13) alternately. We set the distance range from 2 m to 150 m and then determine the longitudinal position of the corresponding surrounding vehicle. At the same time, other parameters are set as the same as the base case.Figure 11
Effect of the distance between vehicle 0 and vehicle 1. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)Figure 12
Effect of the distance between vehicle 0 and vehicle 3. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)Figure 13
Effect of the distance between vehicle 0 and vehicle 4. (a) Effect of the distance on the feasibility of the algorithm. (b) Effect of the distance on the computational load of the algorithm.
(a)(b)In order to take the sensitivity analysis between vehicle 0 and vehicle 1 (Figure11), we generate 75 scenarios where the values of x0,0, v0,0, v1,0, x3,0, v3,0, x4,0, and v4,0 are the same as those in the base scenario (scenario 1) while the value of x1,0 ranges from 358 m to 210 m (x0,0=360m in scenario 1). Here, we generate a scenario analysis every 2 m, so x1,0=358,356,…,212,210m for 75 generated scenarios. Therefore, we can derive that the distance between vehicle 0 and vehicle 1 is 2,4,…,148,150m for the 75 generated scenarios. Then, the proposed lane change planning algorithm is executed to check the feasibility of the algorithm and CPU computation load under the 75 generated scenarios.It can be seen that the feasibility of the proposed lane-changing planning model can be guaranteed when the distance between the vehicles is larger than a certain value (28 m in Figure11(a), 100 m in Figure 12(a), and 22 m in Figure 13(a)). The larger distance between the controlled vehicle and its surrounding vehicles can decrease the required computational CPU time (Figures 11(b), 12(b), and 13(b)). This indicates that a scenario with a little distance between the controlled vehicle and its surrounding vehicles means a dangerous or even infeasible lane-changing driving situation and thus involves complex maneuvers to complete the lane-changing task (therefore, the optimization toolbox needs to take longer time to find a feasible solution). We also find that the leading vehicle in the target lane is the most critical factor for the lane change maneuver. The ego vehicle is able to execute the lane change maneuver if and only if a large enough distance (larger than 100m) between the ego vehicle and the leading vehicle in the target lane is available.


## 6. Conclusions

This study proposed a nonlinear trajectory planning program for automated vehicles to conduct a lane change maneuver. When constructing the spatiotemporal lane change trajectory, the polynomial function is adopted and its parameters are determined through a nonlinear program. Instead of assigning assumed values for the planning horizon and final state, this study incorporates them into the nonlinear program’s decision variables, which can be obtained after optimization. Specifically, for the controlled vehicle’s final state, it is further constrained through positions and speeds of the forward vehicle in the target lane, which is achieved through a car-following model. Moreover, we consider the interactions between the following vehicle in the target lane and the controlled vehicle: the motion of the new following vehicle is influenced by the trajectory planning results of the controlled vehicle, and the influence of the new following vehicle is built through the car-following model. Therefore, the proposed trajectory planning methods can simultaneously plan trajectories for the new following vehicle and the controlled lane change vehicle. The proposed nonlinear program is not restricted by the type of road, so the model can be easily applied in practice. Finally, we solve the nonlinear programming through the SQP algorithm and assign an initial solution using the Newton method to improve the solution quality. The numerical results demonstrate that the performance of the proposed trajectory planning method can satisfy the requirements of safety and comfort under most scenarios.The main contributions are as follows. First, we provide a new methodology that no longer needs to use the preassumed exact time horizon length and the preassumed exact final position information to constrain the trajectory. Therefore, the proposed method can be more flexible for the lane change maneuver and can be adaptive to more generalized scenarios. Second, we integrate a car-following rule into the trajectory planner and use the car-following model to constrain the state of the controlled vehicle at the end of the lane change. This approach can allow the controlled vehicle to switch from the lane change maneuver to the car-following maneuver smoothly. Third, we attempt to provide a lane change plan allowing the controlled vehicle to interact with the surrounding vehicles. This strategy helps us determine the shape of the trajectory with respect to the motion of the surrounding vehicles. Finally, the proposed method is not limited to a certain type of road curve (i.e., a straight-line road). The proposed method can perform well for different types of road curves. Therefore, it presents great value in practice.Future research may focus on building a realistic trajectory prediction model and incorporate it into the nonlinear program. Moreover, we will explore more computational issues to improve the feasibility and optimality of the trajectory planning program in future studies. For example, we will establish a feasibility check model, which starts before the trajectory planning process and can examine whether a lane change decision is appropriate under the current driving scenario. Through the anticipation feasibility check model, we can address the problems in terms of the significant computational resources’ waste (time and memory) caused by the strict or infeasible lane change scenario and provide a possible solution for the computationally efficient solution in the automated vehicle.



---

*Source: 8867447-2020-12-18.xml*