A Cooperation-Aware Lane Change Method for Autonomous Vehicles

The decision set $\mathcal{D}$ consists of various candidate decisions. Each decision $\tau_{b}^{d}$ is a reference trajectory, which is an initialization for the subsequent interactive prediction and trajectory planning. For the computational convenience, we set the duration of each reference trajectory $T_{d}$ to be equal to $T_{p}$. In this paper, we limit our discussion on combining three lateral actions (left lane change, right lane change and staying in the current lane) and three longitudinal options (accelerating, decelerating, and keeping in the current speed range) in Fig. 2 to construct $\mathcal{D}$. Note that $\mathcal{D}$ can be extended to contain more decisions according to a practical demand.

In order to generate each $\tau_{b}^{d}$, we first calculate the lateral and the longitudinal accelerations. Inspired by the work in [37], we use the PNTLA method to generate a continuous-time lateral reference trajectory. This method can calculate the smooth trajectory with curvature continuity based on the maximum lateral acceleration $a_{y\max}$ and the maximum lateral jerk $\Delta a_{y\max}$. As shown in Fig. 3, the PNTLA method assumes that when vehicles change lanes, the lateral acceleration varies linearly, and the acceleration curve is composed of two isosceles trapezoids with the same size but opposite directions. Hence, the lateral jerk $\Delta a_{y}$ is expressed as,

where $t_{1}$ to $t_{5}$ are five key timestamps. Based on the properties of isosceles trapezoids, they are calculated as [37],

where $d_{w}$ denotes the lateral displacement during a lane change. With the timestamps $t_{1}$ to $t_{5}$, the lateral acceleration curve can be determined.

The longitudinal reference trajectory is generated in a similar manner. The longitudinal acceleration curve is described as an isosceles trapezoid which guarantees the smoothness and curvature continuity of a generated trajectory. The longitudinal jerk $\Delta a_{x}$ is expressed as,

where $\Delta a_{x\max}$ denotes a maximum longitudinal jerk, $t_{6}$ to $t_{8}$ are three key timestamps. Based on the properties of isosceles trapezoids, $t_{6}$ to $t_{8}$ are calculated as,

where $a_{x\max}$ denotes a maximum longitudinal acceleration, $v_{x0}$ is the current speed, $v_{x1}$ is the desired speed. Similarly, with the timestamps $t_{6}$ to $t_{8}$, the longitudinal acceleration curve is established.

After obtaining the lateral and longitudinal acceleration curves, we can calculate a continuous-time velocity profile by integrating the acceleration curves, and then calculate a continuous-time position profile by integrating the acceleration curves twice. Since the reference trajectory does not consider the vehicle dynamics, the heading angle is set to be equal to the angle between the direction of velocity and the longitudinal axis. Therefore, we obtain a continuous-time trajectory, which should be discretized with $\Delta t$ to obtain each $\tau_{b}^{d}$.

To select a $\tau_{b}^{d}\in\mathcal{D}$ as an optimal decision $\tau_{opt}^{d}$ for the ego vehicle, it is crucial to predict how surrounding vehicles will react to each $\tau_{b}^{d}$. However, due to the strong spatial-temporal dependencies among vehicles, predicting their trajectories is challenging. In this paper, we propose an interactive trajectory prediction method based on the graph-based spatial-temporal convolutional network (GSTCN) [38] to tackle this task, since this network can effectively capture the spatial-temporal dependencies among vehicles and then predict their future trajectories simultaneously. As shown in Fig. 4, GSTCN is composed of three modules: a spatial graph convolutional module, a temporal dependency extractor module, and a trajectory prediction module. The first module uses a graph convolution network to extract the spatial dependencies between vehicles. The following temporal dependency extractor module learns the temporal dependencies. The trajectory prediction module consists of an encoder and a decoder, where both of them are composed of the gated recurrent unit (GRU) networks to deal with the trajectory sequence generation. More details of the GSTCN can be found in [38]. Here, the proposed interactive trajectory prediction method will be introduced in detail from two stages: training step and prediction step.

After obtaining the predicted interactive trajectories $\{\tau_{1,b}^{f},\cdots,\tau_{n,b}^{f},\cdots,\tau_{N,b}^{f}\}$ under every $\tau_{b}^{d}\in\mathcal{D}$, we should determine which $\tau_{b}^{d}$ is the most recommended in terms of safety, efficiency and comfort [39, 35]. We evaluate each $\tau_{b}^{d}$ by calculating their costs. The cost function consists of three components and is calculated as

where $J_{s}$, $J_{e}$, and $J_{c}$ are the costs of safety, efficiency and comfort, respectively. $\lambda_{s}$, $\lambda_{e}$, and $\lambda_{c}$ represent the corresponding weights, which can be customized by the user.

The safety cost $J_{s}$ is related to the collision risks between the ego vehicle and nearby vehicles, which consists of risks in both lateral and longitudinal directions. Therefore, the safety cost $J_{s}$ is represented as,

where $J_{s-lon}$ and $J_{s-lat}$ represent the safety costs in longitudinal and lateral directions, respectively. $\eta=1$ if the ego vehicle changes to other lanes, otherwise $\eta=0$, which guarantees that if the $\tau_{b}^{d}$ does not relate to lane change, the lateral risks can be ignored.

The longitudinal safety cost $J_{s-lon}$ relates to the collision risks between the ego vehicle and the preceding and following vehicles in the same lane. In this paper, we use the time-to-collision (TTC) [40] to characterize the collision risks, which is calculated based on the relative distances and velocities against the ego vehicle and the preceding and following vehicles in the same lane. The smaller TTC is, the larger the collision risk is. Thus, we define $J_{s-lon}$ as,

with

where $v_{ego}[k]$ obtained from $\tau_{b}^{d}$ is the velocity of the ego vehicle at the time step $k$. The subscripts $pv-lon$ and $fv-lon$ represent the indices of the nearest preceding and following vehicles in the same lane, respectively. $v_{pv-lon}$ is the velocity of the nearest preceding vehicle in the same lane at the time step $k$, which is obtained from $\{\tau_{1,b}^{f},\cdots,\tau_{n,b}^{f},\cdots,\tau_{N,b}^{f}\}$. $s=[x,y]^{\top}$ denotes the position of a vehicle, and $\left\|\cdot\right\|_{2}$ is the $l_{2}$ norm. Hence, $\Delta v_{pv-lon}[k]$ and $\Delta s_{pv-lon}[k]$ denote the relative velocity and distance between the ego vehicle and the nearest preceding vehicle in the same lane at the time step $k$. $\Delta v_{fv-lon}$ and $\Delta s_{fv-lon}$ denote the relative velocity and distance between the ego vehicle and the nearest following vehicle in the same lane. $\kappa_{s-lat}$ is the regularization coefficient of the longitudinal safety cost, which is determined by practical demands. $\sigma(\cdot)=\min(\cdot,0)$, which guarantees that two vehicles moving away from each other will not contribute a safety cost.

Similarly, the lateral safety cost $J_{s-lat}$ is calculated based on the relative distances and velocities against the ego vehicle and the preceding and following vehicles in the target lane, which is defined as,

where $\kappa_{s-lat}$ is the regularization coefficient of the lateral safety cost. The subscripts $pv-lat$ and $fv-lat$ represent the indices of the nearest preceding and following vehicles in the target lane, respectively.

Moreover, the efficiency cost $J_{e}$ is related to the velocities of the ego vehicle and surrounding vehicles. The closer their speeds are to the desired speed, the smaller the efficiency cost is. Hence, $J_{e}$ is defined as,

where $\kappa_{e}$ is the regularization coefficient of the efficiency cost, $v_{des}$ is the desirable velocity, $v_{n}$ and $v_{ego}$ are the velocities of the $n$th nearby vehicle and the ego vehicle, respectively. Since we want to make decisions that can help the ego vehicle to cooperate with others, the efficiency cost indicates that both the ego vehicle and surrounding vehicles want to reach the desirable speeds.

In addition, the comfort cost $J_{c}$ aims to make the ego vehicle travel with a relatively smooth trajectory, so it relates to the jerk in both lateral and longitudinal directions. $J_{c}$ is defined as,

where $\kappa_{c-lon}$ and $\kappa_{c-lat}$ are regularization coefficients of the comfort cost. $\Delta a_{ego-lon}$ and $\Delta a_{ego-lat}$ denote the longitudinal and lateral jerks of the ego vehicle, which can be obtained by calculating the derivative of acceleration with respect to time from $\tau_{b}^{d}$.

After the cost function is defined, we select a $\tau_{b}^{d}\in\mathcal{D}$ with the minimal cost $J_{d}$ as the optimal decision $\tau_{opt}^{d}$, which makes a balance among safety, efficiency, and comfort.

Note that in the process of generating the reference trajectory candidates, we utilize the lateral displacement, the maximum lateral acceleration, the maximum lateral jerk, the maximum longitudinal acceleration, and the maximum longitudinal jerk. Therefore, the feasibility of a motion planning has been considered during the phase of the lane change decision.

To realize the trajectory planning for the ego vehicle, we first establish a mathematical model of vehicle kinematics. The more accurately the model describes the motions of a vehicle, the more accurately the planned trajectory will be tracked by a control system. However, due to the limited computing resources and high requirements for computing time in driving environments, the vehicle kinematics model should also have the advantages of easy implementation and low computation complexity. To this end, we use the bicycle model in [41] to describe the nonlinear vehicle kinematics. With the Euler discretization, this vehicle kinematic model can be defined as,

where $\beta$ denotes the angle between the speed and the heading, $l_{f}$ and $l_{r}$ denote the length from the gravity center of a vehicle to the front axle and the rear axle, respectively, $a$ is the acceleration, and $\delta$ denotes the steering angle. Thus, we obtain a predictive model for MPC. As mentioned in Section II, the acceleration and the steering angle are taken as control actions, and thus we define them as $\mathbf{u}[k]=[a[k],~{}\delta[k]]^{\top}$.

Due to the mechanical and physical limitations in velocity, heading angle, and position, we need to take the boundaries of these variables as constraints in the process of motion planning to achieve a feasible solution. Specifically, they are represented as,

where the first constraint considers the road boundaries and the available range of the heading angle and the velocity, i.e., $\tau_{\min}$ and $\tau_{\max}$; the second constraint ensures the acceleration and the steering angle do not exceed the vehicle’s capacity from $\mathbf{u}_{\min}$ to $\mathbf{u}_{\max}$; the last constraint guarantees the smoothness and comfort of generated trajectories by restraining the control input rate to a range from $\Delta\mathbf{u}_{\min}$ to $\Delta\mathbf{u}_{\max}$.

In addition to the above mechanical and physical constraints, we also consider the safety constraints. Since the vehicle-occupied road area imposes restrictions on the movements of other vehicles, it is essential to model a precise vehicle shape. Unlike previous works that model the shape of vehicles as a mass point or particles with radial gaps, we assume vehicles are rectangular which is closer to an actual circumstance. As shown in Fig. 5, the vehicle-occupied road region is represented by a rectangle $\mathcal{R}$, which is expressed as,

As in [42], the matrix $\mathbf{A}$ and the vector $\mathbf{b}$ are expressed as,

with the length $l$ and width $w$ of a vehicle, and the current coordinate of the gravity center $[x_{0},y_{0}]$.

With this vehicle shape model, the distance between two vehicles can be represented as,

where $\mathcal{R}_{1}$ and $\mathcal{R}_{2}$ are the road regions occupied by two vehicles, respectively, and $\mathbf{p}_{1}$ and $\mathbf{p}_{2}$ denote any coordinates in the occupied road regions. Therefore, the safety constraints is represented as,

where $\mathcal{R}_{ego}[k]$ and $\mathcal{R}_{n}[k]$ denote the road regions occupied the ego vehicle and the $n$th nearby vehicle at the time step $k$, respectively, $d_{\min}$ is the minimum safety distance between vehicles. With the interactive trajectory prediction of nearby vehicles $\{\tau_{1,opt}^{f},\cdots,\tau_{n,opt}^{f},\cdots,\tau_{N,opt}^{f}\}$, we can calculate their occupied regions in the planning time horizon. In this way, vehicular interactions are considered during trajectory planning.

Note that the collision avoidance constraint (22) is nondifferentiable, so we adopt the approach in [43] to convert this constraint to a smooth and differentiable constraint by using strong duality theory. Based on this method, the safety constraints (22) can be reformulated as,

where $\mathbf{\lambda}_{n}$, $\mathbf{\mu}_{n}$ and $\rho_{n}$ are the dual variables. Interested readers are referred to [43] for more details. With this exact and smooth reformulation, standard nonlinear solvers can deal with the safety constraints.

The objective is to minimize the deviation between the trajectory updated by the vehicle kinematic model and the reference trajectory associated with the optimal decision; meanwhile, smooth control actions are preferred for comfort. This objective function is defined as follows,

where $\tau[k+i|k]$ represents the state at the time step $k+i$ predicted by the state at the time step $k$, $M$ is the length of optimization horizon under the sampling period $\Delta t$, $\mathbf{Q}_{\tau}$, $\mathbf{Q}_{\mathbf{u}}$ and $\mathbf{Q}_{\Delta\mathbf{u}}$ denote the corresponding weight matrices. The first term of the objective function penalizes the divergence of the solved trajectory from the reference trajectory. The second and third term suppress the excessive control actions and control input rates, respectively.

With the objective function and constraints introduced in the above subsections, the trajectory planning is formulated as an optimization problem, i.e.,

where the first constraint guarantees the initial solved state $\tau[k|k]$ to be equal to the reference state $\tau_{opt}^{d}[k]$.

To solve the optimization problem (25), we propose an algorithm to plan the trajectory for a lane change based on interior point optimizer (Ipopt) [44]. Ipopt is a widely used numerical solver for large-scale nonlinear optimization problems.

The algorithm for trajectory planning based on Ipopt is described in Algorithm 1. The optimization problem at the time step $k$ is established as (25). By solving this problem, a sequence of optimal control actions $\mathbf{U}^{*}[k]=\{\mathbf{u}^{*}[k],\cdots,\mathbf{u}^{*}[k+M-1]\}$ can be obtained. As the basic MPC algorithm in [45], we only apply the first control action $\mathbf{u}^{*}[k]$ to the ego vehicle to update its state. At the next time step ${k+1}$, the horizon moves forward and a new optimization problem is formulated and solved by our algorithm. By repeating this procedure, the ego vehicle can obtain an optimal trajectory that is smooth, collision-free, and satisfies the vehicle kinematic model. Till now we have formulated an optimization and an Ipopt-based algorithm to complete the task of trajectory planning.

An open-source automotive driving simulator [46] is used to implement the simulation scenarios. We consider two-lane or three-lane highway. All vehicles have the same shapes; i.e., 4 meters in length and 1.8 meters in width. Only the ego vehicle uses our proposed method for lane change. For other vehicles, the longitudinal motions and lateral lane change behaviors are governed by the Intelligent Driver Model (IDM) [47] and the Minimizing Overall Braking Induced by Lane changes (MOBIL) [48] model, respectively. In order to increase the complexity of the simulated scenarios, the parameters in the IDM and the MOBIL are randomly sampled from uniform distributions. The sampling period $\Delta t$ is set to be equal to 0.1s.

In order to further demonstrate the superiority of our proposed method, we compare it with the following baselines.

• •

  IDM+MOBIL: This baseline controls the lateral and longitudinal motions with MOBIL and IDM, respectively, which is the same as other vehicles.

• •

  Our proposed method without interactive trajectory prediction (denoted as Proposed W/O IP): This baseline is the same as our proposed method except that it uses constant velocity, instead of the interactive trajectory prediction method in Section IV-B, to predict other vehicles’ trajectories.

In this subsection, we make quantitative analysis for the proposed method. We totally conduct 100 simulations under different traffic densities. In each simulation, vehicles run on a three-lane highway, and the parameters and initial positions for each vehicle are random. The mean velocities of the ego vehicle and surrounding vehicles are listed in Table I.

We can see that ego vehicles equipped with our proposed method achieve the fastest speeds, demonstrating the effectiveness of interactive trajectory prediction. Meanwhile the mean speed of other vehicles are faster than that of other baselines. This is because the ego vehicle also considers the driving efficiencies of surrounding vehicles when making decisions on lane changes. Therefore, their driving efficiencies will be improved when the ego vehicle cooperates with others. The ego vehicle using IDM+MOBIL only considers the observation at the current time but ignores the prediction of the future, so the mean speeds are the slowest.

As shown in Fig. 6, we also study two practical cases for lane change, including obstacle avoidance in dense traffic, and complex traffic with three lanes having different speeds and space gaps.