Improving Urban Traffic Throughput with Vehicle Platooning: Theory and Experiments

In this paper we present a model-predictive control (MPC) based approach for vehicle platooning in an urban traffic setting. Our primary goal is to demonstrate that vehicle platooning has the potential to significantly increase throughput at intersections, which can create bottlenecks in the traffic flow. To do so, our approach relies on vehicle connectivity: vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communication. In particular, we introduce a customized V2V message set which features a velocity forecast, i.e. a prediction on the future velocity trajectory, which enables platooning vehicles to accurately maintain short following distances, thereby increasing throughput. Furthermore, V2I communication allows platoons to react immediately to changes in the state of nearby traffic lights, e.g. when the traffic phase becomes green, enabling additional gains in traffic efficiency. We present our design of the vehicle platooning system, and then evaluate performance by estimating the potential gains in terms of throughput using our results from simulation, as well as experiments conducted with real test vehicles on a closed track. Lastly, we briefly overview our demonstration of vehicle platooning on public roadways in Arcadia, CA.

reduced reliance on traffic lights [4]. V2V communication allows for nearby vehicles to coordinate their motion accurately and to form vehicle platoons: strings of vehicles driving at the same speed and at short distance. There are two primary benefits of vehicle platooning: an improvement in traffic efficiency due to increased roadway capacity, and an increase in fuel efficiency due to reduced aerodynamic drag forces acting on the platooning vehicles, especially for heavyduty vehicles such as semi-trucks. Regarding the first point, there is demonstrated potential for platooning to increase the capacity of both highways and urban roadways. For example, a microscopic simulation study in [5] predicts that increasing the penetration of vehicles capable of cooperative adaptive cruise control (CACC) will result in an increase in highway capacity, since it enables the driver to select smaller time headways. In [6] the authors predict that the throughput of urban roadways could potentially be doubled by forming platoons of vehicles, particularly by increasing the capacity of intersections, which they confirm with a subsequent simulation study. For the second point, experiments presented in [7] confirm that small spacings between two heavy-duty trucks results in reduced fuel consumption.
Previous demonstrations have showcased the technical feasibility of vehicle platooning. For example, vehicle platooning was demonstrated in 1994 and 1997 by the California PATH team on the I-18 highway in San Diego, CA [8]. Other experimental evaluations conducted on highways include [9], where the authors develop a platooning system architecture for heavy-duty vehicles. The system is evaluated in terms of controller tracking performance and fuel consumption over varying levels of road grade. In [10,11] the authors present the design of a CACC system and tested it on a fleet of test vehicles. A primary controller performance metric in these works is string stability [12], meaning that the preceding vehicles are able to attenuate disturbances in traffic downstream (for example, changes in velocity). In 2011 the first Grand Cooperative Driving Challenge was held in the Netherlands [13], with the goal of accelerating the deployment of cooperative driving technologies. The competition focused on CACC and included both an urban and highway driving challenge [14]. For the urban driving challenge one criterion used to judge the participating teams was throughput improvement at the traffic light. This scenario is similar to the one we considered in our previous work [15], where we focused on the trade-off between traffic throughput gains and safety.
In addition to maintaining a platoon formation, the related tasks of forming, merging, and splitting platoons require structured coordination between vehicles, i.e. interaction protocols, which can be achieved in principle with V2V communication. For example, in [16] state machines are provided which describe the sequence of events, coordinated via V2V communication, that must occur during merge, split, and change lane maneuvers. Furthermore, low level control laws for the leader vehicle to execute these maneuvers have been developed [17]. In [18] an extended message set is proposed for the purpose of enabling connected vehicles to coordinate more complex maneuvers in merging, intersection, and emergency vehicle scenarios for a follow-up Grand Cooperative Driving Challenge which was held in 2016 [19]. Other works studying communication include [20], where the authors present a strategy for maintaining string stability in a vehicle platoon while using significantly fewer communication resources.
Unlike the aforementioned studies, in this work we focus on advancing vehicle platooning to a public urban environment where increased intersection throughput can result in significant improvements in overall traffic efficiency. Enabling platooning in an urban environment involves addressing various challenges, such as forming and disbanding platoons in moving traffic, decision-making (e.g. whether or not to proceed through an upcoming intersection), and ensuring safety when a lead vehicle is present. These challenges are especially important on a public roadway, where the future behavior of vehicles ahead of the platoon and the phase of upcoming traffic lights are uncertain. We present a design for the urban platooning system, and then analyze performance by estimating throughput using data obtained from simulations and experiments conducted on a closed track. We also introduce a state machine for managing the participating platooning vehicles, and propose strategies for the platoon to ensure safety when it encounters an intersection and / or a leading vehicle, utilizing predictions of their future behavior.
The closest comparable effort that we are aware of is the MAVEN project, which has laid out the various technologies that are needed to develop and deploy urban platooning, and reported on test results with two automated vehicles [21], where technologies such as a green light optimal speed advisory system and a collective perception message were utilized. Unlike [21], our focus in this paper is on improving throughput by maintaining short (constant) distances between the vehicles as the platoon accelerates from rest to a nominal speed. In particular, we achieve such accurate tracking by transmitting velocity forecasts between platooning vehicles and using them as disturbance previews in our MPC problems.
The remainder of the paper is organized as follows. We outline our design for the urban vehicle platooning system in Sections II -IV, including a platoon model and management system, MPC formulation, and strategy for the leader to ensure safety. In Section V we present results from our simulation tool, and analyze the performance of the platooning system by estimating the potential gains in intersection throughput. Next, in Section VI we discuss the experimental setup and present results from conducting tests on a closed track and on public roadways in Arcadia, CA, including our estimates of throughput. We end with concluding remarks in Section VII and discuss some of the challenges we encountered during the tests, as well as potential solutions. We note that parts of Sections II -IV are adapted from our previous work [15], but the remaining content in the paper is completely new and advances platooning to an urban setting.

II. PLATOON MODEL AND MANAGEMENT
In this section we introduce the model of the platoon and various systems that enable management of its behavior (beyond the control algorithms themselves), including state estimation via on-board sensors, V2X communication, and a finite-state machine (FSM) system which ensures that the platoon acts in a coordinated manner, that is, vehicles start moving as a single platoon at the same time and break the platoon at the same time as needed. In particular, we discuss how vehicle-to-vehicle communication enables the follower vehicles to do accurate distance tracking of the leader, and how vehicle-to-infrastructure communication enables the leader to decide whether or not to proceed through an upcoming intersection.

A. Vehicle Models
The longitudinal dynamics of the leader vehicle [22] are modelled asṗ where the states are as follows: p L (t) is the position, h L (t) is the distance to the public vehicle ahead (specifically, the distance from the front bumper of the leader vehicle to the rear bumper of the front vehicle), d T L L (t) is the distance to the nearest upcoming intersection stop bar, v L (t) is the ego vehicle velocity, and T a L (t) ∈ R ≥0 is the accelerating wheel torque. The inputs T a,ref L (t) ∈ R ≥0 and T b L (t) ∈ R ≥0 are the accelerating wheel torque command and the braking wheel torque. Lastly, v F (t) is the velocity of the public vehicle ahead, henceforth referred to as the front vehicle. The parameters M , R w , and τ are the vehicle mass, wheel radius, and actuation time constant for acceleration, respectively. We note that (1e) models actuation delay while the vehicle is accelerating, which has been empirically estimated by collecting wheel torque measurements from the test vehicle. During these experiments we observed no delay while braking, and therefore the model does not include actuation delay while braking. Lastly, F f L (t) is a longitudinal force acting on the leader vehicle, given by where g is the gravitational constant, θ is road grade, A is the area of the vehicle, r is a rolling coefficient of the vehicle, ρ is air density, and c x is an air drag coefficient. We assume road grade is negligible, and thus θ = 0 for t ≥ 0. For simplicity, we represent (2) as where the parameters β, γ ∈ R ≥0 were identified by collecting driving data at a testing area near UC Berkeley, and then fitting predictions from (3) to the data (see Table I). We write the leader vehicle dynamics (1) concisely aṡ , and w L (t) := v F (t). Note that the velocity of the front vehicle v F (t) appears as a disturbance here. Since we cannot accurately predict the behavior of nonplatooning vehicles, we make the conservative assumption that the front vehicle will decelerate from its current speed until coming to a stop. This assumed trajectory of the front vehicle is used for planning, to be discussed further in Section III-A.
We model the longitudinal dynamics of each of the N − 1 follower vehicles in the platoon aṡ where s i (t), used for distance tracking relative to the leader vehicle, is defined as follows:

Platooning vehicles
Public lead vehicle We refer to s i (t) as the distance from follower i to the leader (note that (6) implies s 1 (t) = h 1 (t)). Furthermore, we let v 0 (t) = v L (t) so that (5c) is valid for follower i = 1. We write (5) compactly aṡ where . We note that the velocity of the leader and front vehicle v L (t) and v i−1 (t) appear as disturbances here -since these are both platooning vehicles in this case, we can receive a forecast of their future behavior via V2V communication. In Section II-C we discuss the information transmitted between platooning vehicles which includes a velocity forecast, to be used as a disturbance preview in our MPC formulation.
For planning, our goal is to obtain linear, discrete time models from (4) and (7). We use the procedure outlined in [15] for doing so: we first linearize the leader and follower vehicle dynamics about the nominal velocities v 0 L and v 0 i , respectively, and then discretize the resulting linear models each with time step ∆t = 0.1s, resulting in where the matrices A L ∈ R 5×5 and B L ∈ R 5×2 are functions of the velocity v 0 L , and A i ∈ R 6×6 and B i ∈ R 6×2 are functions of the velocity v 0 i . At each time step, the current ego vehicle velocity is substituted into these expressions to obtain the appropriate dynamics matrices to be used for MPC.

B. State Estimation
To localize the leader and follower vehicle positions p L (t) and p i (t) we use a differential GPS measurement which has lane-level accuracy. Furthermore, with GPS and information received from nearby traffic lights we can also estimate the distances d T L L (t) and d T L i (t) from each vehicle to the nearest upcoming traffic light. The forward-looking radar on each vehicle measures the headways h L (t) and h i (t), and standard on-board sensors provide the current velocity estimates v L (t) and v i (t), as well as estimates of the accelerating wheel torques T a L (t) and T a i (t).
An important sensing challenge for each follower i is to estimate the distance to the leader as defined in (6). We have tested two methods for doing so: 1) estimating s i (t) using GPS, and 2) estimating s i (t) directly using the radar measurements h i (t), which can be transmitted via V2V communication. For the first method, we use GPS to measure the distance d L i (t) from the center of vehicle i to the center of the leader vehicle and use the estimatê whered L i (t) is an estimate of d L i (t) from GPS. The main drawback to this approach is GPS measurement noise -we observed up to 3 meters of error when estimating s i (t) using GPS. Because of this, we also used a Kalman filter, where the idea is to use the current velocity of the leader (received via V2V communication) and the ego vehicle velocity to improve our estimate of s i (t). For the second method, we use the estimateŝ whereĥ k (t) is an estimate of h k (t) from radar. Since measurements from the forward-looking radar are generally very reliable, we observed smaller measurement errors using the second method. The main drawback to the second approach, however, is that it will require more vehicles in the platoon to communicate with one another (discussed further in the next section). For the experiments discussed in Section VI-B we used GPS to estimate s i (t), and for the experiments in Section VI-C we used radar measurements to estimate s i (t).

C. Vehicle-to-vehicle communication
We assume each platooning vehicle is capable of V2V communication. An important piece of information transmitted within the platoon is a forecast of the future velocity trajectory for each vehicle, given by (11) for the leader vehicle and follower vehicle i, respectively. Here, v L (k|t) is the planned velocity of the leader vehicle at time step k, obtained by solving an MPC problem at the current time step t (the notation is the same for the follower vehicles), and N p is the MPC horizon in time steps. Each follower vehicle receives a velocity forecast from the front vehicle and the leader vehicle, corresponding to the flow of information depicted in Figure 3a. The front vehicle forecast is used to ensure safety, and the leader vehicle forecast is used to do distance tracking of the leader.
In addition to the velocity forecast, each experimental vehicle transmits its radar measurement, current GPS coordinates, and plan status signal. A secondary reason for transmitting GPS coordinates, beyond estimating s i (t), is so that the leader vehicle can estimate the distance d N −1 L (t) from itself to the rear platooning vehicle. The transmission of GPS coordinates from follower N − 1 to the leader is shown in Figure 3b. This lets the leader check whether the entire platoon has enough where the blue node represents the leader vehicle and the grey node represents the rear vehicle. Figure 3a shows the transmission of velocity forecasts and 3b shows the transmission of GPS coordinates from the rear vehicle (used by the leader to determine if the platoon can make it through the intersection, see Section II-D). Figure 3c shows how we share radar measurements when we use the second method for estimating si(t) as in (10). time to pass through an upcoming intersection, as discussed in the next section. As mentioned in the previous section, for some of our experiments we used radar measurements, transmitted via V2V communication, to estimate s i (t). In Figure 3c we depict the flow of information in this case, for N = 4. We note that each vehicle, upon receiving an incoming message, checks the ID of the vehicle that transmitted it (indicating the vehicle's position in the platoon, e.g. leader vehicle, rear vehicle, etc.) to determine which information fields to extract, if any.

D. Vehicle-to-infrastructure communication
In addition to V2V messages, we assume the platooning vehicles also receive SPaT (signal, phase, and timing) messages from nearby traffic lights via V2I communication. In this way, each vehicle obtains the following prediction on the nearest upcoming traffic light state: where p up (t) ∈ {red, yellow, green} is the current phase of the nearest upcoming traffic light and c r (t) ∈ R ≥0 is a prediction on the time remaining in the current phase. We note that it is necessary to predict c r (t) here since in our experiments the traffic signals are actuated.
In the remainder of this section, we discuss how the leader decides whether or not the platoon should stop at an upcoming traffic light. This decision is handled by the leader onlythe follower vehicles simply track the leader, and therefore we do not allow platoon separation. Suppose the platoon is approaching a traffic light during its green phase, with c r (t) seconds remaining in the phase. In this scenario, the leader checks if the following condition holds to determine whether a stop is necessary (specifically, if (13) is false the platoon should stop), where L int is the intersection length. Condition (13) provides a quick and simple way to check whether the rear platooning vehicle, travelling at the current leader velocity v L (t), will pass through the intersection during the green phase. We use v L (t) in (13) since the leader effectively sets the speed for all platooning vehicles behind it, and also to avoid having to transmit v N −1 (t) to the leader. We note that when N is large, d N −1 L (t) is large and thus (13) is easily violated. This means the platoon may begin braking during a green light, which can be unexpected for nearby drivers. To avoid this, for large N allowing platoon separation may become necessary.
At low velocity (13) is not easily satisfied and will be overly restrictive, for example if the light just turned green and the platoon is stopped. For this reason, if v L (t) ≤ v low the leader simply checks if the following condition holds where the threshold t min is a tuning parameter. If so, it is considered safe to proceed. By checking (13) and (14) to determine whether to stop, we try to ensure the platoon will not be crossing the intersection when the phase becomes yellow. However, since the traffic signal is actuated and can change randomly due to uncertain traffic conditions, we cannot formally guarantee that this will never occur.
Suppose the leader determines it should stop while the phase is green, or that the phase is yellow, in which case the leader should stop if it can do so safely. Then, we also check if the leader is capable of stopping before the intersection stop line with a margin of d min , that is where −a min,brake ∈ R <0 is an upper bound on (1d) while the maximum braking force is applied. If (15) does not hold, then it is deemed safer for the leader to proceed through the intersection (in this scenario, for large N a platoon separation may also be necessary). For a red phase, however, we require the platoon to stop in any case.

E. Finite state machine (FSM)
We have designed a FSM (see Figure 4) which acts as a mechanism for safely forming and maintaining a platoon. There are four primary states in our FSM: 'Ready', 'Plan Proposed', 'Plan Active', and 'Plan Cancel'. Each platooning vehicle is initialized in the 'Ready' state and communicates its state at all times. The platoon formation process is initiated when the leader moves to the 'Plan Proposed' state by proposing to the follower vehicles the 'plan', including a plan ID, ordering of the vehicles in the platoon, desired gap / speed, etc. Note that the ordering of vehicles in the platoon refers to the list of vehicle IDs ordered from the leader to the last follower. As soon as the 'plan' is received by the followers, the states of the followers transition to the 'Plan Proposed' state. In the 'Plan Proposed' state, each vehicle acknowledges that the 'plan' is valid by checking the on-board sensor data and communicated GPS data. For example, each vehicle can confirm that the driver agrees to join the platoon and that the proposed 'Plan' is safe to follow. We also note that the leader can manually cancel the plan while in the 'Plan Proposed' state, forcing a transition to the 'Plan Cancel' state. When the leader receives an acknowledgement from every vehicle in the 'Plan', it moves to the 'Plan Active' state while also informing the followers so that all vehicles move to the 'Plan Active' state together. To ensure safety, while in the 'Plan Active' state every vehicle in the platoon continuously monitors the surrounding conditions to decide if the 'Plan' must stop. In our experiments, the conditions that cancel the plan include: 1) incorrect ordering of the vehicles, 2) message timeout, 3) any driver taps the gas / brake pedal, 4) front vehicle out of range (radar measurement too high), and 5) velocity upper / lower bound violated. Here, message timeout refers to when a particular message has not been received for a period of time longer than a specified threshold. When one of these conditions is detected by one vehicle, it informs the other vehicles in the platoon and they move together to the 'Plan Cancel' state. After some threshold time, each vehicle transitions from the 'Plan Cancel' state to the 'Ready' state and the platoon can be restarted as needed.
In Figure 5 we display some data collected while forming a platoon during testing in Arcadia, CA (see Section VI-C). The procedure for forming a platoon was to manually drive the test vehicles to get them close together and moving at similar speeds, at which point the leader vehicle would propose a 'plan' via the state machine and engage the platooning controllers simultaneously. This enabled platoon formation even while the vehicles are moving.

III. MPC FORMULATION
In this section we present our MPC problem formulation for the platoon. The leader vehicle has a separate MPC problem which allows it to react to changing traffic conditions and set the desired velocity for the following vehicles. For example, if a stop at an intersection is necessary, the leader computes a velocity trajectory in order to stop safely and comfortably at the intersection stop bar. Furthermore, the leader maintains a safe following distance when a vehicle is present ahead of it. The follower vehicles simply do distance tracking relative to the leader, as we do not allow platoon separation.

A. Leader vehicle MPC
The goal for the leader is to track a desired velocity when it is safe to do so. When necessary, it must yield to a slowermoving front vehicle or stop at the intersection stop bar. The MPC problem for the leader is min ui(·|t) where N p is the MPC horizon in time steps, and x L (k|t) and u L (k|t) are the planned state and input of the leader vehicle at time step k, computed at time step t, respectively (the notation for the other states is the same). Furthermore, d * L (k|t) is the distance from the leader vehicle to either the front vehicle or the upcoming intersection stop bar -whichever is a higher priority obstacle (the method for determining this is outlined in Section IV). Lastly,x L (t),v F (t) are estimates of the leader vehicle and front vehicle state, based on measurements from the on-board sensors, andv F (t + N p ) is an estimate of the front vehicle velocity at the end of the MPC planning horizon. Indeed, sinceŵ L (k) :=v F (k) appears as a disturbance in (16d), we must predict the future velocity trajectory of the front vehicle. To ensure safety, we assume worst-case behavior, i.e. the front vehicle will decelerate from its current speed at the rate a max,brake ∈ R >0 until coming to a complete stop as followŝ whereṽ 0 is an under-approximation of the front vehicle's current velocity v 0 , to be discussed further in Section IV. Here, −a max,brake ∈ R <0 is a lower bound for (1d) and (5d) while the maximum braking force is applied.
The leader vehicle cost function J L penalizes deviations from the desired velocity v des L (16a), nonzero control inputs (16b), and nonzero control input rates (16c), effectively penalizing vehicle jerk. The scalar α ∈ R >0 and matrix R ∈ R 2×2 are design parameters which allow one to tune controller performance. Increasing α, for example, smooths the acceleration and deceleration profiles of the vehicle, but reduces the controller's agility. Furthermore, we set where the diagonal entries R a , R b ∈ R can be increased to encourage the controller to use smaller actuation torques T L a,ref (t) and T L a (t), respectively, and the off-diagonal entries R 0 ∈ R are made sufficiently large in order to prevent the accelerating and braking control inputs from being active simultaneously.
The leader MPC problem is subject to the following constraints: vehicle dynamics (16d), lower and upper bounds on velocity (16e), distance constraint (16f), torque and reference torque constraints (16g) -(16i), and initial condition (16j). The terminal constraint (16k) ensures the leader maintains a safe distance to any obstacle ahead (namely, a front vehicle or intersection requiring a stop), and will be discussed further in Section IV. The parameters d min and t h are tuned to increase passenger comfort. For example, if t h is too small it may feel as if the vehicle is braking late when approaching slow-moving traffic or a stop bar, and if t h is too large the vehicle will brake harshly in response to cut-in vehicles. The values of all MPC parameters are given in Table II. At each time step, the leader vehicle solves its MPC problem and obtains an optimal control input sequence and velocity trajectory: The first control input u L (t|t) of the sequence (19) is then implemented on the vehicle, and the MPC problem is solved again at the next time step. Furthermore, the computed velocity trajectory in (20) is sent to the other platooning vehicles at each time step via V2V communication as a velocity forecast, as discussed in Section II-C.

B. Follower vehicle MPC
The goal of each follower vehicle is to maintain a desired distance s des i to the leader vehicle, while also maintaining a minimum safety distance d min to the front vehicle at all times. The MPC problem to be solved is defined as follows min ui(·|t) where the notation used is the same as in (16). The follower vehicle objective function J i penalizes deviations from the desired distance to the leader vehicle, given by where d des is a design parameter. Furthermore, we also include penalties on input (21b) and jerk (21c). Similar to the leader, these penalties have to be adjusted carefully to balance performance and passenger comfort. Furthermore, we note that constraints (21e) and (21k) are imposed with respect to the front (platooning) vehicle only, since safety tasks regarding an upcoming intersection are handled by the platoon leader (the terminal constraint (21k) will be discussed further in the next section). Similar to the leader, at each time step the follower vehicle solves its MPC problem and obtains an optimal control input sequence and velocity trajectory. We apply the first control input of the sequence, and the computed velocity trajectory is broadcast to the platoon via V2V communication. Hence, since velocity forecasts (11) are received by all follower vehicles via V2V communication, we use the following disturbance preview for MPC: where the planned velocity trajectories v L (k|t) and v i−1 (k|t) were computed by the leader and front vehicle when they solved their respective MPC problems.
Remark 1: Since we use the full velocity forecast as a disturbance preview in (23), a natural question that arises is whether or not these predictions are reliable. To address this question, in [15] we defined the trust horizon F , which allows us to adjust how much of the velocity forecasts are used. For a trust horizon of F , time steps t through t + F of all velocity forecasts are used. After time step t + F the front vehicle is assumed to decelerate at the maximum rate until coming to a stop, and therefore the terminal constraint (21k) is imposed at time step t + F . This is in contrast to the approach in this paper, where we assume the front (platooning) vehicle will fully realize the trajectory in its velocity forecast as in (23), corresponding to F = N p . Doing so introduces some risk to the follower vehicles; however, this is necessary to achieve a reasonable increase in traffic throughput with vehicle platooning, as shown in our previous work [15].

IV. SAFETY CONSTRAINTS AND MPC SOLUTION
We now discuss how we formally ensure safety in an urban traffic setting. First, in Section IV-A we describe the set of safe states for a vehicle in relation to the two primary obstacles it can encounter in an urban setting: another vehicle ahead of it, and an upcoming intersection. Furthermore, we show  that at each time instant the vehicle needs to consider only one of these obstacles, which we refer to as the priority obstacle, thereby simplifying the task of ensuring safety. Next, in Section IV-B we discuss how we use the safe sets from Section IV-A in our MPC problems, as well as how we efficiently solve the MPC problems at runtime.

A. Safe States and Priority Obstacle
Consider an ego vehicle (representing either a platoon leader or follower here), a front vehicle ahead of it, and an upcoming intersection. Throughout the section, we let a(t) and a F (t) be the accelerations of the ego and front vehicles, respectively, so that the vehicle dynamics becomė where h(t) is the headway of the ego vehicle, d T L (t) is the distance from the ego vehicle to the upcoming traffic light stop bar, and v F (t) and v(t) are the velocities of the front and ego vehicles, respectively. Since we observed no actuation delay while braking during experimentation, it is sufficient to use (24) in place of (1) for the analysis here.
We first assume that only a front vehicle is present, and define safety for the ego vehicle with respect to the front vehicle as To enforce (25), the ego vehicle must ensure it can maintain a minimum safety distance d min if the front vehicle applies the maximum braking force until coming to a stop. We formalize this requirement in the following Proposition: Proposition 1: Consider the vehicle dynamics given in (24). Let a min,brake , a max,brake ∈ R >0 , and a min,brake ≤ a max,brake . Suppose the accelerations a F (t) and a(t) satisfy where t s F := v F (0)/a max,brake and t s := v L (0)/a min,brake are the first time instants in seconds such that v F (t s F ) = 0 and v(t s ) = 0, respectively. Then, (25) : . For a proof we refer to [23], Lemma 1 (see also [17,24]). We note that in addition to v F (0), the set C F (v F (0)) also depends on a min,brake , a max,brake , d min ∈ R >0 . A plot of C F is given in Figure 6a. Next, we suppose that only an upcoming intersection requiring a stop is present. In this case, the ego vehicle must ensure it can make a complete stop and leave a distance of d min to the intersection stop bar. Formally, we require that if the ego vehicle decelerates until coming to a stop as in (27), then the following will hold We note that when the light cycles to green, this constraints is relaxed and the platoon is allowed to proceed. Similar to Proposition 1, we can show that (29) holds if the ego vehicle decelerates as in (27) and A plot of C T L is given in Figure 6b. Now, we suppose that both a front vehicle and an upcoming intersection requiring a stop are present simultaneously. In this scenario, we require that if the front and ego vehicle (representing the platoon leader here) decelerate until coming to a stop as in (26) and (27), then both (25) and (29) will hold.   Figure 7a there is a slow-moving truck attempting to turn right ahead of the leader vehicle. Since the truck takes priority over the intersection at this point, the platoon is forced to slow down. In Figure 7b the truck completes the right turn and priority shifts to the intersection.
To determine which obstacle is prioritized, the ego vehicle can check if the following condition holds: If (31) holds then the front vehicle is capable of stopping in front of the intersection stop line, and therefore must be prioritized. If (31) does not hold then the upcoming intersection is prioritized (see Figure 7 for an illustration). We summarize this idea in the following Proposition, which follows directly from the definitions of C F and C T L .
Based on Proposition 2, we conclude that for the leader vehicle MPC problem discussed in the previous section, it is sufficient to impose a terminal constraint with respect to only the priority obstacle. This is beneficial for efficiently solving the MPC problems at runtime, as discussed further in the next section.
Remark 2: In the above discussion we assumed d min is the same for both the front vehicle and the intersection, whereas in our experiments we used slightly different values of d min for each. Although this is beneficial for passenger comfort, there is one drawback to this adjustment: in corner cases where priority between the two obstacles can easily switch, we may only satisfy (25) and (29) for the minimum of these two values, i.e. for d min := min{d min,F , d min,T L }, where min,F and min,T L are the unique minimum distance values used for the front vehicle and intersection, respectively. We ensured, however, that this minimum safety margin is still sufficient for testing purposes. Furthermore, in normal traffic conditions the priority between obstacles is clear (usually, the front vehicle is clearly stopping at the intersection, or clearly passing through it).
Remark 3: If an upcoming intersection is not present (or does not require a stop), then the front vehicle is prioritized if one is present. This allows, for example, the platoon to pass through a green light if it is safe to do so. Similarly, if only a front vehicle is present then it is prioritized. If neither obstacle is present, then no obstacle-related constraints are imposed on the leader.

B. Terminal Constraints and MPC Solution
We now connect the discussion in the previous section to terminal constraints (16k) and (21k). For the follower vehicles, the primary safety task is to maintain a minimum distance to the front (platooning) vehicle. Therefore, the terminal constraint (21k) is imposed with respect to the front vehicle only. For the leader vehicle, the primary safety tasks are to stop at an upcoming intersection when necessary, and to maintain a minimum distance to the front (non-platooning) vehicle. Based on the discussion in Section IV-A, this is accomplished by imposing the terminal constraint (16k) with respect to the priority obstacle. To this end, we define h L (t + k|t), ifx L (t) andv F (t) satisfy (31), d T L L (t + k|t), otherwise, as the planned distance from the leader to the priority obstacle at time step k, computed at time step t, and as the terminal set with respect to the priority obstacle. We note that the priority obstacle will be the same throughout the MPC planning horizon, since (31) checks whether the front vehicle can stop before the intersection stop bar if it decelerates at the rate a max,brake , which is its assumed behavior in the leader MPC problem in (17).
To solve the leader and follower vehicle MPC problems at runtime we use the tool CVXGEN [25], which allows one to generate C code for solving a custom quadratic program (QP) reliably and efficiently. Since CVXGEN can only be used for moderately-sized QPs, it is beneficial to impose terminal constraint (16k) with respect to only the priority obstacle, as imposing a terminal constraint with respect to both obstacles would create additional (redundant) constraints. Furthermore, since our MPC problems must be represented as QPs with linear constraints, the sets C F and C T L discussed in the previous section cannot be directly encoded into our MPC problems. Instead, we use a procedure from [26] to compute polyhedral constraint sets to be used in place of C F and C T L . In particular, we compute a collection of sets C F (v F (0)) to be used for v F (0) ∈ [v min , v max ]. This collection of sets is computed offline, and the proper set is selected during runtime to be used for MPC (for more details, we refer the reader to [15]).
Since it is important to avoid infeasibility of the MPC problems during experimentation, all constraints in each problem (except for the vehicle dynamics constraints) are converted to soft constraints. This means that for a hard constraint such as Gx ≤ h, where x ∈ R n , G ∈ R m×n , and h ∈ R m , we instead add the term λ1 T (Gx − h) + to the objective function, where λ ∈ R >0 , 1 ∈ R m is the vector of all 1's, and y + for y ∈ R m indicates that we are thresholding each element of y so that y + ∈ R m ≥0 (see [27]).
V. SIMULATION RESULTS We now present results from our simulation tool developed in MATLAB, which enabled us to validate the platooning software prior to conducting real-world experiments. In particular, the tool is useful to confirm that the platoon preserves safety even when it encounters traffic lights and other nonplatooning vehicles, using the approach in Sections II-D and IV. Furthermore, we are able to estimate the potential gains in traffic throughput at intersections, using a metric from [15].

A. Urban Stop and Go Scenario
We use our tool to simulate the platoon travelling through an urban environment in stop and go traffic. The tool enables us to create signalized intersections with the following attributes: position (m), V2I communication range (m), cycle offset (s), red / yellow / green time (s), and cycle length (s). In particular, we placed intersections in the simulation environment so that the distance between traffic lights is similar to the Arcadia corridor (see Section VI-C).
The simulation results are shown in Figure 8. In the beginning of the simulation, the platoon encounters red lights at the first few intersections, stopping at each. Near the end of the simulation the platoon approaches a (non-platooning) public vehicle which is travelling much more slowly, and the platoon is forced to reduce its speed for the remainder of the simulation. We note that near the end of the simulation, the public vehicle comes to a complete stop at an intersection and as a result the platoon leader also stops, leaving a distance of 6m as desired. We also remark that for simulation we did not use the same controller parameters that we did for experimentation, where the parameters were mainly selected to improve passenger comfort.

B. Estimating Throughput
We now analyze the performance of the vehicle platooning system by estimating intersection throughput. To do so, we   [15]. At time t = 0 let the platoon be stopped at the (current) intersection stop bar with no vehicles ahead where L veh is the vehicle length (assumed to be uniformly 4.5 meters for all vehicles), and the intersection stop bar is assumed to be positioned at 0 meters. Suppose at time t = 0 the traffic light cycles from red to green, and the platoon immediately starts moving through the intersection. Let ∈ R >0 be the length of the intersection in meters, and define t L and t N −1 to be the smallest time instants in seconds such that p L (t L ) ≥ and p N −1 (t N −1 ) ≥ , respectively. We then estimate intersection throughput in vehicles per hour as Thus, performance is maximized when the platoon 1) accelerates to a high velocity while crossing the intersection, and 2) accurately maintains the desired inter-vehicle gaps while accelerating. We note that for the estimate (34) to be accurate, we must consider the length of each vehicle, as opposed to treating each as a point mass. Throughput analysis of simulation results (as well as the test-track experiments discussed in Section VI-B) is shown in Tables III and IV, where all estimates are obtained via (34). In particular, throughput is estimated at the 1st, 2nd, and 4th intersection, located at approximately 0.18 km, 0.43 km, and 1.33 km in the simulation, respectively. In Table III we show improved levels of throughput achieved using our vehicle platooning system, which are estimated from the simulation run shown in Figure 8. In Table IV we show baseline levels of throughput, which are estimated by running the same simulation with the trust horizon (discussed in Remark 1) set to F = 0. We note that throughput is much lower at the 4th intersection, due to the presence of a slower-moving public vehicle ahead of the platoon. Indeed, in situations like this, the benefit of vehicle platooning in terms of traffic throughput may not be fully realized.

VI. EXPERIMENTAL RESULTS
In this section we present the experimental results and evaluate the performance of our platooning controller via the throughput metric from Section V-B. We discuss the experimental setup in Section VI-A, and in Section VI-B we present results from preliminary tests on a closed track at the Hyundai-KIA Motors California Proving Grounds in California City, CA. Next, we give an overview of a final platooning demonstration on public roadways in Arcadia, CA in Section VI-C. Links to drone videos of each series of tests are also provided.

A. Test Vehicles
We use the three test vehicles shown in Figure 1, each of which is equipped with a production forward-looking radar and camera that estimate the front vehicle distance, velocity, and acceleration. To enable V2V and V2I communication, we use a Cohda Wireless MK5 V2X on-board unit (OBU), which also has an integrated GPS. The Cohda OBU allows the vehicles to exchange BSMs and custom V2V messages, which include a velocity forecast and other information. This transmitted information allows the third vehicle in the platoon, for instance, to estimate its current distance to the leader vehicle. The Cohda also allows each vehicle to communicate with any nearby traffic lights which are instrumented to broadcast SPaT and MAP messages. Lastly, the controller for each vehicle is implemented on a dSpace MicroAutoBox, and a Matrix embedded PC exchanges information between the Cohda, MicroAutoBox, and the ego vehicle controller area network (CAN bus). The Matrix also runs a state machine which manages the role of each vehicle in the platoon, and is discussed further in Section II-E. A diagram of the hardware setup is shown in Figure 9.
An important hardware consideration for platooning is that of communication latencies. In [15] we discussed how including a time stamp in transmitted messages enables each vehicle  Figure 1. Here, we had the platoon track a reference trajectory which was generated via our simulation tool. The position, inter-vehicle distance, velocity, and MPC torque command for each vehicle are shown in each subplot, respectively. The desired distance between vehicles was 6 meters.
to account for V2V communication delays. The idea is to use the time stamp to estimate the delay d in time-steps (with sampling time ∆t = 0.1s), and then to shift the velocity forecast used for MPC by d steps, where we assume the transmitting vehicle will maintain a constant velocity beyond its planned trajectory. For the experimental work presented in this paper, however, we assume there are no communication delays between vehicles, which is done for two reasons. The first reason is that we have observed that communication latencies are typically small enough to be ignored for our application. The second reason is that estimating d accurately is challenging in practice. Since the clocks on the test vehicle computers are not synchronized, one must estimate the clock skew between vehicles, which could potentially be timevarying, in order to accurately estimate delays.

B. Closed track experiments
Preliminary vehicle platooning experiments were conducted on a closed test track at the Hyundai-KIA Motors California Proving Grounds in California City, CA (see Figure 1). For all of the tests the leader vehicle does velocity tracking of a predetermined velocity trajectory (meaning v des L in (16a) becomes time-dependent), and the follower vehicles do distance tracking relative to the leader vehicle. The predetermined velocity trajectories used for tracking were either from real velocity data collected during previous experiments, or artificial velocity data generated by our simulation tool. In Figure  10 we show experimental results from a test using artificial velocity data which has a step function-like trajectory. For these experiments we used a larger admissible range of the wheel torque for the follower vehicles, as seen in the bottom plot of Figure 10. We note there is slightly larger tracking error (as well as larger variation of the wheel torque command) for the second follower in this experiment. We can mainly attribute this to state estimation error since GPS was used to estimate the distance s i (t) for all experiments at the California Proving Grounds, as discussed in Section II-B. However, throughout the experiment in Figure 10 the tracking error for all vehicles is less than about 1 meter.
A video of the testing is available online at https://youtu.be/ U-O9iUZElR8, which includes several test runs with varying levels of the trust horizon F (discussed in Remark 1). We note that in test runs with a small trust horizon, for example F = 10 (half of the velocity forecast is trusted) or F = 0 (none of the velocity forecast is trusted, meaning the vehicles effectively do not use V2V communication), large gaps appear between the platooning vehicles while they are accelerating. This behavior is expected, since using the full velocity forecast relaxes the constraints on following distance so that the follower vehicles can get closer to the vehicle ahead. In the test run shown in Figure 10 we used F = 15, demonstrating that we are able to get accurate tracking performance when using a large portion of the velocity forecast (elsewhere in the paper we use F = N p = 20). Similar to Section V-B, we estimate throughput for the test run shown in Figure 10 by treating the platoon as if it begins stopped at an intersection -our estimate is shown in Table III. Furthermore, in Table IV we show a baseline level of throughput computed using data from a test run with F = 0. As expected, significantly higher throughput is achieved by utilizing the velocity forecast.

C. Public Road Demonstration
To demonstrate vehicle platooning in an urban environment with a moderate level of traffic, we conducted further experiments in Arcadia, CA. Our testing area is a 2.45 km long stretch of roadway on Live Oak Ave between S Santa Anita Ave and Peck Rd, and has eight consecutive intersections which are instrumented to send out SPaT and MAP messages for our vehicle platoon to receive. All tests in Arcadia were completed with a 3-vehicle platoon using the same MPC parameters as shown in Table I, with the exception that v des L = 14 m/s was used here. Footage of our testing is available online: https://youtu.be/xPYR xP3FuY. It captures a few instances where the platoon stops at the stop bar for a red light with no vehicles queued ahead of it. When the light turns green the platoon reacts immediately and moves through the intersection more quickly and compactly than the humandriven vehicles near it, further demonstrating the potential for throughput improvement (see Figure 11).

VII. CONCLUSION AND FUTURE WORK
In this paper we presented the design and evaluation of a vehicle platooning system that can operate in an urban corridor with intersections and other traffic participants. The primary motivation for advancing platooning to an urban setting is to improve traffic efficiency by increasing throughput at intersections, which create bottlenecks for traffic flow. In particular, we evaluated the performance of our vehicle platooning architecture by estimating the level of throughput that would be achieved at the intersection, calculated by measuring the time instants at which each vehicle crosses the intersection. We note that the estimates obtained here are consistent with theoretical predictions, for example in [6] and [15].
An important challenge we encountered while testing on public roads is that of safely disengaging the platooning system and passing control back to the safety driver when necessary. To do so, we designed our system so that if any driver taps the brake pedal when the platoon is active, the controller for every platooning vehicle disengages immediately (via the finite state machine) and all drivers are notified immediately via a sound. We note, however, that this design can be problematic in certain scenarios. For example, suppose the platoon is approaching an intersection and begins braking when the driver in the leader vehicle, out of caution, disengages the platoon. This requires the drivers in the follower vehicles to react immediately, as their vehicles will suddenly stop braking when the controllers disengage. In the future we hope to address this issue by creating a safety system that ensures the vehicles start transitioning to a safe state immediately when the 'plan' is cancelled, providing the safety driver more time to react. One potential approach, for example, is to have the platooning system transition to an ACC state of operation immediately after disengagement. The ACC system would then remain active and maintain a safe distance to the front vehicle until the driver takes over.
Another future research direction relates to the procedure for setting cost weights in the MPC objective functions, which were manually tuned here. Indeed, in order to converge on acceptable values for the tuning parameters affecting vehicle drivability, such as the time headway constraint or penalty on vehicle jerk, multiple trial runs on a closed test track were necessary. To reduce development time, it would be interesting to see how a learning-based approach could potentially expedite this procedure. Furthermore, we note that the final tuning values we obtained are only valid for the class of test vehicles in this paper -other classes of vehicles, such as semi-trucks, have different performance characteristics and would therefore need separate tuning values. Learning-based performance tuning would also be beneficial for deploying a platoon with a large number of vehicles, since separate tuning values were used for each vehicle within the platoon in this paper. Learning could also accelerate the deployment of autonomous vehicles more broadly, by enabling companies to more easily meet customer's driving preferences.