Facilitating Cooperative and Distributed Multi-Vehicle Lane Change Maneuvers

The $i$-th CAV state variables are defined as $\mathbf{z}^{i}(t):=[s^{i}(t),e_{y}^{i}(t),e_{\psi}^{i}(t),v^{i}(t)]^{\top}$ for $i\in\{1,2,...,n\}$, where $s^{i}(t)$ and $e_{y}^{i}(t)$ represent the longitudinal and lateral displacement of the vehicle with respect to the desired lane’s center line, respectively, as seen in Fig. 2. $n$ is the number of CAVs in the platoon. $e_{\psi}^{i}(t)$ is the heading angle difference between that of the vehicle and the center line and $v^{i}$ is the longitudinal velocity of the vehicle at the center of gravity (C.G.). The control input vector is $\mathbf{u}^{i}(t)=[a^{i}(t),\delta_{f}^{i}(t)]^{\top}$, where $a^{i}(t)$ is the longitudinal acceleration of the vehicle at the C.G. and $\delta_{f}^{i}(t)$ is the steering angle. The vehicle dynamics are

$l_{f}^{i}$ and $l_{r}^{i}$ are distance from the front axle to the C.G. and distance from the rear axle to the C.G., respectively. $\kappa(t)$ is the curvature of the reference centerline.

The continuous-time dynamics model is discretized using forward Euler method with sampling time $\Delta t$ as follows

where $\mathbf{z}^{i}(k)$ denotes the state vector $\mathbf{z}^{i}$ at time $k\Delta t+t$ for $k\in\mathbb{Z}^{+}_{0}$.

During the prediction horizon $N$, we use a longitudinal uncertain prediction model to predict NPC vehicle’s motion. The model assumes uncertainty on acceleration and that the NPC vehicle does not change lanes within the prediction horizon. Given the OL centerline and road curvature information: $(\kappa_{t},\dots,\kappa_{t+N})$, the resulting discretized dynamics of the NPC vehicle is as follows

for $k=t,...,t+N$; where $e_{y}^{NPC}(t)$ is $e_{y}$ of the NPC vehicle at time $t$. ${w_{k}}\sim N(0,\sigma^{2})$ is a random variable representing acceleration at time $k$. The variance $\sigma^{2}$ is the design parameter of the model and depends on the acceleration limitation of the NPC vehicle. In our simulation, we set $\sigma^{2}=0.2$ $m/s^{2}$. The proposed approach can accommodate different prediction model for improved prediction accuracy and robustness.

The CAVs in the platoon are indexed by $i$ starting from the front-most CAV. Note that the facilitator is always assigned $i=1$. The FSM makes use of parameters and binary inputs associated with every CAVs in the platoon and denoted by the superscript $i\in\{1,2,...,n\}$ for the $i$-th CAV. The parameters and binary inputs used in our proposed FSM are listed in Table 1.

The desired minimum longitudinal inter-vehicular distances are defined as the following:

where $d_{safe}$ is the user-defined safe vehicular distance and impacts the conservativeness of the controlled vehicle’s driving behavior. In the context of multi-CAV lane change, $d^{1}_{des}$ represents the desired free space that the facilitator must create. $d^{i}_{des}$ corresponds to the desired distance (relative to the facilitator) for the $i$-th CAV to execute a lane change into the free space created by the facilitator. These inter-vehicular distances fully describe the relative positions of the multi-CAV platoon and the facilitator.

Typically, the simplest descriptions of highway driving of an autonomous vehicle is categorized into two modes: Lane Keeping (LK) and Lane Change (LC). In lane keeping mode, a CAV stays in its current lane and attempts to follow the reference velocity while maintaining a safe distance from the preceding vehicle. In lane change mode, the CAV attempts to change lanes into TL if such maneuver is safe and feasible. An FSM that shows the transition map between the two modes for a multi-CAV platoon is shown in Fig. 3(a). Note that this two-mode FSM is sufficient for a passive and opportunistic multi-CAV platoon lane change strategy, in which switch to Lane Change mode occurs if and only if a platoon-wise lane change is simultaneously feasible ($\prod_{i=1}^{n}LC_{feasible}^{i}$ is True). After completing a multi-CAV platoon lane change, the planner switches back to Lane Keeping mode.

Our proposed FSM has an additional mode: Gap Regulation (GR) as shown in Fig. 3(b). In Gap Regulation mode, the CAV regulates the inter-vehicular distance of interest (gap) to the desired longitudinal distance. By assigning different desired inter-vehicular distance per CAV, the multi-CAV platoon is regulated into a desired formation in a distributed manner. Furthermore, the mode transition logic is different for the facilitator and the multi-CAV platoon but not independent. The mode transition depends on not just the controlled CAV’s states but also on the states of other CAVs as a mechanism for coordination. For instance, the multi-CAV platoon (excluding the facilitator) switches to Gap Regulation mode only after the facilitator has completed a lane change into the TL. In this mode, the facilitator modifies the environment and creates free space in the TL for the rest of the multi-CAV platoon to change lanes while the rest of the multi-CAV platoon gets in desired formation to execute a simultaneous lane change as shown in Fig. 1(b)-(c).

The mode transition logic for the facilitator and the non-facilitator CAV are listed in Fig. 3(c) in separate columns. We denote the FSM mode of the $i$-th CAV at time t as $z^{i}_{FSM}(t)\in\{LK,LC,GR\}$ and the vector containing FSM binary inputs listed in Table 1 for all CAVs (i.e. $i=\{1,2,\cdots,n\}$) as $\mathbf{x}_{FSM}$. The $i$-th CAV’s FSM mode transition at time t is denoted as

Note that the proposed FSM in Fig. 3 and Table 1 also allows simultaneous platoon-wise lane change when feasible by design. Further, in cases of infeasibilities while in LC mode due to environmental factors, the mode transitions back to Lane Keeping mode and returns to OL by design for safety.

Path planning is formulated as a constrained finite time optimization problem (CFTOP) to plan dynamically feasible, smooth, and collision-free trajectories for each vehicle. The MPC path-planner solves the CFTOP repeatedly in receding horizon at a constant frequency. The path-planner is designed using the MPC framework with an augmented state vector $\tilde{\mathbf{z}}^{i}_{k|t}:={[\mathbf{z}_{k|t}^{i}{}^{\top},d^{i}_{k|t}]}^{\top}$ where $\mathbf{z}^{i}_{k|t}$ is defined as $\mathbf{z}$ of the $i$-th vehicle at time $k$ predicted at time $t$. $d^{i}_{k|t}$ is $d^{i}$ at time $k$ predicted at time $t$ with the following dynamics

Therefore, the augmented state dynamics $\tilde{\mathbf{z}}^{i}_{k+1|t}:=g(\tilde{\mathbf{z}}^{i}_{k|t},\mathbf{u}^{i}_{k|t},w_{k})$ is represented by (2) and $d^{i}_{k+1|t}$ which depends on the NPC prediction with uncertainty in (3) when $i=1$. For $i\neq 1$, $d^{i}_{k+1|t}$ is deterministic because it is the inter-vehicular distance between two CAVs. In a distributed multi-CAV system, the exact motion forecasts of other CAVs are communicated via V2V. Using the augmented state dynamics, the path-planner is defined as follows

where $\mathbf{u}^{i}(\cdot|t)=\{\mathbf{u}^{i}_{t|t},...,\mathbf{u}^{i}_{t+N-1|t}\}$ is the control input sequence over the horizon for the $i$-th vehicle, $N$. Furthermore, $\Delta\mathbf{u}^{i}_{k|t}:=\mathbf{u}^{i}_{k|t}-\mathbf{u}^{i}_{k-1|t}$ and $\tilde{\mathbf{z}}^{i,des}_{k|t}$ is the desired state of the $i$-th vehicle which is determined by the lane center line and the speed limit of the road. $Q:=diag(0,c_{y}$, $c_{\psi}$, $c_{v}$, $c_{d})$ where each scalar elements represent the tracking costs for $e_{y}^{i}$, $e_{\psi}^{i}$, $v^{i}$, and $d^{i}$, respectively. $\mathcal{R}_{u}$ $\mathcal{R}_{\Delta u}$ are the input cost matrix and input rate cost matrix, respectively. Note that these cost matrices are positive definite matrices and tuning parameters. (9e) sets the initial condition at time t/. $\mathcal{Z}$ is a feasible state set representing the road boundaries and vehicle dynamics limitations, $\mathcal{U}$ is a feasible input set, and $\Delta\mathbf{u}^{min}_{\cdot|\cdot}$ and $\Delta\mathbf{u}^{max}_{\cdot|\cdot}$ are input rate limits that arise from vehicle dynamic limitations. $z^{i}_{FSM}(t)$ is the FSM mode of the $i$-th CAV at time t at which the CFTOP (9) is solved. The FSM mode is fixed during the horizon and its transition is described by (6).

As previously stated, the planner has three modes with different constraints and tracking costs. Firstly, in LC mode, a state terminal constraint (9g) is applied. The terminal state set, $\mathcal{Z}_{f}$, is determined by the TL’s lateral displacement from the OL and curvature at $s^{i}_{N|t}$ and requires the controlled vehicle to reach the target lane at the end of the horizon. (9h) is applied to constrain the vehicle to the free space in the target lane and avoid collisions. The $s^{i,min}_{k|t}$ and $s^{i,max}_{k|t}$ are the longitudinal displacement of the rear and front (with respect to that of the controlled vehicle) vehicles in the TL at time $k$ predicted at time $t$, respectively, if any. The expectation-type constraint is applied because $s^{i,min}_{k|t}$ and $s^{i,max}_{k|t}$ may be stochastic NPC predictions. This constraint ensures that the controlled vehicle remains in free space in the TL while changing lanes within the horizon. Secondly, in LK and GR mode, (9i) and (9j) are applied for safe adaptive cruise control if there exists a preceding vehicle (its longitudinal position denoted as $s_{front,k|t}^{i}$) in the same lane as the controlled vehicle. Otherwise, (9i) and (9j) are not applied. To prevent infeasibility occurring from surrounding vehicles’ sudden deceleration, the safety constraint (9i) is relaxed by the time-varying slack variable $\epsilon^{i}_{k|t}$ with the cost $c_{s}$. In LC mode, the desired $e_{y}$ is the lateral-displacement of the TL whereas it is zero (OL) in other modes. Lastly, the tracking cost for $d^{i}_{k|t}$ is only included in GR mode. Accordingly, $c_{d}=q_{d}$ if in GR mode and $c_{d}=0$ otherwise, where $q_{d}\in\mathbb{R}^{+}$ is a tracking cost for gap regulation and a design parameter.

In distributed path planning of a multi-vehicle system, resolving trajectory conflicts between CAVs in the multi-CAV platoon is central to designing a safe and robust system. Trajectory conflicts in our lane change scenario may arise when lane change intention and motion predictions of other CAVs are not communicated and are wrong, respectively. For instance, during a simultaneous lane change of multiple CAVs, the controlled CAV’s independently-computed optimal lane change trajectory may conflict with other CAVs’ trajectories if their lane change intentions are not considered. Therefore, during a simultaneous lane change, a CAV must plan its trajectory while considering other lane changes of other CAVs. In our distributed coordination method, conflicts are prevented by using the following implementation steps.

Lane change intentions of other CAVs are considered when planning a simultaneous lane change of multiple CAVs by using the virtual vehicle-based method (Debada et al., 2017). In this method, virtual vehicles are forecasts of CAVs onto their target lane and are considered when planning a lane change trajectory. The controlled CAV plans a lane change trajectory while considering the virtual CAVs in the target lane. This imposes stricter constraint (9h) and reserve space for the virtual CAV’s corresponding real CAV to change lanes into the free space, if feasible. Since all CAVs mutually consider other CAVs’ virtual vehicles during simultaneous lane change, conflicts are prevented in their optimal lane change trajectories.

Furthermore, in our planner formulation, the exact motion predictions of other CAVs at time t, which are the state sequence $\mathbf{z}^{i}(\cdot|t)$ for $i\in\{1,2,...,n\}$ of CFTOP (9), are obtained via V2V technology. Thus, the exact planned motion of other CAVs at time t are known which allows the controlled CAV to plan a LC trajectory that avoids collision with other CAVs as well as their virtual vehicles. During a simultaneous lane change of multi-CAV platoon, either the rear, front, or both vehicles are other CAVs’ virtual vehicle. Therefore, at least one of the $s^{i,min}_{k|t}$ and $s^{i,max}_{k|t}$ terms in (9h), which is the longitudinal motion predictions of rear and front vehicles in the target lane, is no longer an uncertain prediction but instead known exactly at time $t$ during simultaneous lane changes.

To close the loop in our simulation, the NPC vehicles are controlled using the Enhanced Intelligent Driver Model (EIDM), which is based on the intelligent driver model (IDM). The EIDM relaxes the sudden deceleration in not safety-critical situations while inheriting IDM’s crash-free characteristics (Kesting et al., 2010). This model combines the IDM and constant-acceleration heuristics, which assumes that the leading vehicle will not change its acceleration for the next few seconds, to relax the sudden deceleration in not safety-critical situations (Kesting et al., 2010). Furthermore, a scaling constant called coolness factor, $c$, is a model parameter that determines the sensitivity with respect to changes of the gap with the vehicle in front. Kesting et al. (2010) recommend $c\in[0.95,1.00]$ to simulate realistic driving behavior. This car-following model outputs vehicle acceleration to emulate an adaptive cruise control behavior. The EIDM parameters used in the simulation are reported in Table 2. $s_{0}$ represents the minimum desired spacing between the controlled vehicle and the preceding vehicle. $v_{0}$ is the desired velocity, $T$ is the desired time headway, $a$ is the maximum vehicle acceleration, and $b$ is the comfortable braking deceleration where $b>0$.

Our proposed planner was tested in a dense traffic scenario based on reconstructed NGSIM dataset (Montanino and Punzo, 2013). The NGSIM dataset contains the recorded trajectories (sampling frequency of 10 Hz) of all vehicles passing a segment of freeway I-80 for 15 minutes (Montanino and Punzo, 2013). The initial scene is generated by randomly selecting the initial frame and two adjacent lanes (i.e. lane 1 and 2) out of 5 total lanes. Then, three vehicles with the shortest inter-vehicle distances in the initial scene are selected as CAVs. The inter-vehicle distances must be smaller than the user-defined threshold which is set to 9 meters in our simulation. If the threshold condition is not satisfied, the scene is discarded and generated again randomly. After selecting the CAVs, the desired velocity of all CAVs and NPCs are set to the maximum velocity of all vehicles in the initial scene: $v^{max}_{scene}$. The NPC vehicles are simulated using the NPC controller as described previously. CAVs and NPCs are simulated in closed loop with kinematic bicycle model by applying the planner and NPC controller at 20 Hz update rate using the parameters in Table 2 for 25 seconds. Note that the NPC vehicles in the scene have different initial speed and time-varying acceleration, which renders it a more challenging scenario for multi-CAV platoon to change lanes.

In 200 randomly generated dense traffic scenarios, the baseline and our proposed lane change strategies were applied. The performance metrics are the number of successful lane change completion of all three CAVs and the average lane change competition time in successful lane change simulation runs. Table 3 reports the obtained results. Over two-fold increase in number of lane change completions using our proposed strategy is mainly attributed to the single vehicle lane change characteristics. It only requires a single vehicle (facilitator) to change lanes to initiate the coordinated maneuvers to enable multi-CAV lane change whereas the baseline controller requires three-vehicles to change lanes simultaneously. After changing lanes, the facilitator regulates the free space by proactively modifying the environment to render the lane change feasible for the other CAVs. Moreover, multi-CAV platoon completed lane change using the proposed strategy in every traffic scenario that the baseline strategy successfully completed lane change. On average, the baseline strategy takes 12.20 seconds to complete a multi-CAV platoon lane change while the proposed strategy takes 12.64 seconds. Fig. 4 shows snapshots of platoon-wise lane change using the proposed strategy in a dense traffic scenario generated from NGSIM dataset as well as time series of states and inputs of the CAVs during the simulation. In this scenario, platoon-wise lane change using the baseline strategy is not feasible during the duration of the simulation.