Polling-systems-based Autonomous Vehicle Coordination
in Traffic Intersections with No Traffic Signals

arXiv:1607.07896v1 [cs.SY] 26 Jul 2016

David Miculescu

Abstract—The rapid development of autonomous vehicles
spurred a careful investigation of the potential benefits of allautonomous transportation networks. Most studies conclude that
autonomous systems can enable drastic improvements in performance. A widely studied concept is all-autonomous, collision-free
intersections, where vehicles arriving in a traffic intersection with
no traffic light adjust their speeds to cross safely through the
intersection as quickly as possible. In this paper, we propose
a coordination control algorithm for this problem, assuming
stochastic models for the arrival times of the vehicles. The
proposed algorithm provides provable guarantees on safety and
performance. More precisely, it is shown that no collisions occur
surely, and moreover a rigorous upper bound is provided for
the expected wait time. The algorithm is also demonstrated in
simulations. The proposed algorithms are inspired by polling
systems. In fact, the problem studied in this paper leads to a
new polling system where customers are subject to differential
constraints, which may be interesting in its own right.

I. I NTRODUCTION
Autonomous vehicles hold the potential to revolutionize transportation and logistics. Self-driving cars may reduce congestion and emissions, while substantially enhancing
safety [1], [2]. Drones may be used for delivering goods
in urban centers or for providing emergency supplies in
disaster-struck areas [3], [4]. Almost all studies on autonomous
vehicles, including those mentioned above, envision large
fleets of autonomous vehicles that work in coordination to
enable efficient transportation and logistics services. In fact,
in many cases, well-coordinated fleets are key for economic
and societal impact. For instance, self-driving cars create the
most value when they are deployed as a fleet of autonomous
taxis for mobility on demand [2], [5]; delivery drones are most
useful when they can relay packages from one to another [4].
In almost all such examples, efficient coordination algorithms can enable substantial savings in energy, increase in
throughput and capacity, and reduction in delays and emissions. In fact, the overall performance of the system is often
highly sensitive to the operator’s choice of the underlying coordination algorithms [5]. Thus, designing effective coordination
algorithms is vital towards understanding the real potential for
impact that autonomous vehicles provide.
The application domain of such coordination algorithms is
not limited to autonomous cars and delivery drones. There
are already a number of existing applications where effective
coordination of a fleet of autonomous vehicles is key. For
instance, robotic vehicles that shuttle materials to service packaging requests in Amazon warehouses [6], [7] and autonomous
This work was partially funded by the National Science Foundation through
Grant #1350685.
The authors are with the Department of Aeronautics and Astronautics and
the Laboratory for Information and Decision Systems, Massachusetts Institute
of Technology, Cambridge, MA 02139. {dmicul, sertac}@mit.edu

Sertac Karaman

Fig. 1. An illustration of a fully autonomous traffic intersection. Autonomous
vehicles arriving near the intersection are fully controlled by a central control
system. The control system ensures that the vehicles safely pass through
the intersection; furthermore, the central control system provides provable
guarantees on performance, e.g., an upper bound on the average time it takes
a typical vehicle to go through this intersection region.

trucks that carry cargo containers in seaports [8]–[10] can all
benefit from better coordination algorithms to reduce delays
and energy consumption, while increasing throughput.
High-performance coordination algorithms for autonomous
vehicles that have to work in tight spaces would have tremendous immediate impact on these application domains. They
wouldalso pave the way for the design of future urban centers
with shared autonomous vehicles [5], next generation air
transportation systems [11], and industrial environments that
house autonomous vehicles to shuttle goods [12].
In systems involving high-performance autonomous vehicles, “traffic intersections,” where flows of vehicles intersect,
are among the most critical. Efficient coordination algorithms
are most beneficial in these places where interaction between
vehicles is frequent. In this paper, we study “signalless traffic
intersections” in which vehicles coordinate among each other
to carefully adjust their speed in such a way that maximal
throughput can be achieved with minimal delay. To be precise,
we assume that vehicles arrive stochastically at the ends of
two roads that intersect at their other end. (See Figure 1.) We
assume that a central controller commands each vehicle in the
system. The vehicles are bound by second-order differential
constraints with bounded velocity and bounded acceleration.
In this setting, we propose a novel coordination algorithms that
provide guarantees on safety and performance. In doing so,
we establish novel connections with existing results in polling
systems literature, which may be of interest in its own right.
The importance of coordination algorithms for signalless
traffic intersections has not gone unnoticed. In particular, there
is a large and growing body of literature based on multi-agent
simulations [5], [13]–[17], genetic algorithms [18], tokenbased approaches [19], [20], auction-based approaches [21],

and discrete-time occupancy theory [22]. A comprehensive
recent summary and comparison of some of these approaches
are provided by Dai et al. [23]. These approaches propose
practical algorithms that seem to perform well in simulation
studies. However, most provide no performance guarantees.
Control-theoretic formulations and analyses have also been
considered. The existing literature includes a hierarchicaldistributed coordination scheme that balances computational
complexity and optimality [24], an approximation algorithm
based on an optimal control formulation [25], an MPCbased approach [26], a distributed control generated from
its formal specifications [27], a game-theoretic cruise control
approach [28], an analysis of intersecting vehicle flows [29],
[30] as well as approximation approaches based on hybrid
systems theory [31], [32]. Some of these approaches provide
provable guarantees. However, many of them are conservative,
and often fail to provide any guarantees on performance.
Finally, problems similar to the one considered in the
present paper were discussed in the context of air traffic
control and conflict resolution [33]–[43]. The work we report
in the present paper is most related to the analysis of aircraft
flows by Mao et al. [40]–[43]. Their analysis considers two
intersecting flows of aircraft, and computes the maximum
throughput that can be achieved by various maneuvers, such as
heading change. The analysis is geometric and is for aircraft
maneuvers; it considers a series of vehicles arriving one after
another, aimed at finding the maximum throughput in the
worst case. In contrast, our analysis is tailored for autonomous
ground vehicles that move in a lane, and thus, have to slow
down and speed up, but can not maneuver sideways. More
importantly, our analysis takes into account stochastic arrivals,
allowing performance results for the average case. The analysis
in this paper may apply to aircraft models as well, extending
existing literature in air traffic control for intersecting aircraft
flows, with stochastic models.
The main contribution of this paper is a motion coordination
algorithm for autonomous vehicles operating in traffic intersections with no traffic signals. Our algorithm is based on polling
systems [44], and it provides provable guarantees on safety
and performance. Our results extend many of the widely-used
polling policies to traffic intersections of autonomous vehicles.
This extension is non-trivial due to the differential constraints
of the vehicles, which do not appear in the traditional application areas of polling systems, such as data networks.
Our main theoretical result is the following. If the road
length is larger than a certain threshold (which depends on the
maximum speed and maximum acceleration of the vehicles),
then no collisions occur surely, and, moreover, the delay that
each vehicle experiences is bounded by the delay experienced by a customer arriving in a traditional polling system
without the differential constraints. This result is remarkable.
Essentially, as far as the delay is concerned, the differential
constraints of the vehicles are irrelevant, as long as the road
length is larger than a certain threshold; most of the existing
results on delay in the polling systems literature directly apply.
The novel connections with polling systems and the new
motion coordination algorithm provides us with an insight
for signalless traffic intersection control: smart platooning,

i.e., effective clustering of vehicles, is key for achieving good
delay-throughput tradeoff. To achieve the best throughput, the
vehicles in the same lane must slow down to form a cluster,
and pass through the intersection as a platoon, in order to
minimize the time it takes to switch over to the other lane. This
behavior naturally comes out from the motion coordination
algorithms. We emphasize this insight throughout the paper
with examples, simulations, and analyses, when applicable.
Let us note at this point that the polling systems literature is
fairly rich [45]–[49]. Motivated by applications in communication systems, transportation systems, and manufacturing, the
literature has flourished during the last few decades. Optimal
polling policies were characterized for a large class of input
processes [50] and analytical expressions were derived for a
range of polling policies [45]–[47]. These foundational results
have been utilized in several application domains [48], [49].
The existing applications of polling systems also include
urban traffic flows [48]. However, to the best of our knowledge,
the applications of polling systems in the context of allautonomous traffic intersections is novel. Furthermore, the
problem formulation presented in this paper can be generalized
leading to a new class of polling systems where the customers
are subject to differential constraints, which may be interesting
in its own right. In this case, the customers must be “steered”
to a suitable state before they can be serviced. Our results
imply that, in a certain class of such polling systems, the
differential constraints can be managed, i.e., the differentiallyconstrained polling system can achieve the same performance
that its counterpart with no differential constraints achieves.
Although this paper focuses on applications in urban transportation, let us emphasize that potential applications also
include air transportation as well as warehouse automation
and manufacturing. In particular, trajectory-based operations
considered for the NextGen air transportation system (see,
e.g., [11]) by the Federal Aviation Administration in the U.S.
will enable trajectory planning and precision execution for aircraft. An effective use of the shared airspace may be possible
by setting up virtual roads and intersections, where the aircraft coordinate their motion for increased performance. Furthermore, autonomous robotic vehicles servicing warehouses,
factories, and transportation hubs (e.g., airports, seaports, train
stations, etc.) may enable efficient transportation of goods and
people, with effective motion coordination algorithms.
A preliminary version of our results were presented in the
Conference on Decision and Control (CDC) in 2014 [51]. In
addition to the results presented in the preliminary version of
this paper, the present paper provides a full proof of safety
and performance results for a slightly more general setting,
provides new results on stability, and provides conjectures and
open problems based on new simulation results.
This paper is organized as follows. We formalize the problem of signalless intersection control with stochastic arrivals in
Section II. We describe our control policy in Section III, where
we also introduce preliminaries, such as key results from the
polling systems literature. We state our main theoretical results
in Section IV, and provide the results of supporting simulation
studies in Section V. We conclude the paper with remarks in
Section VI. We provide the technical proofs in the appendix.

II. PROBLEM DEFINITION
We are interested in the problem of motion coordination
through a traffic intersection, as depicted in Figure 1. Consider
a traffic intersection where two orthogonal lanes intersect.
Suppose each vehicle is subject to second order dynamics of
the following form:
ẍ(t) = u(t),

(1)

where x(t) denotes the position of the front bumper of the
vehicle, 0 ≤ ẋ(t) ≤ vm is the maximum velocity constraint,
and |u(t)| ≤ am is the maximum acceleration constraint. The
region where the two lanes intersect is called the intersection
region. The portion of the road within a distance L of the
intersection region is called the control region. We assume
that the input signal u(t) is directly controlled by a central
control system for all vehicles that are in the control region.
This central control system does not know a priori the
precise times that each vehicle will arrive at the control region.
However, we assume that certain statistics of their interarrival
times are available. More precisely, we model the arrival times
{ti,k : i ∈ N} as a suitable stochastic process, where ti,k is the
time that the ith vehicle from lane k ∈ {1, 2} enters the control
region (front bumper is exactly a distance of L away from the
intersection region). In other words, the ith vehicle entering
the control region from lane k is at position −L at time ti,k ,
i.e., xi,k (ti,k ) = −L, where xi,k (t) denotes the position of
this vehicle at time t. From this point on, the position xi,k (t)
of the same vehicle is governed according to the dynamics
given in Equation (1). If the lane number of the ith vehicle is
clear from context, we simplify the notation and drop the lane
subscript, i.e., xi denotes the trajectory of the ith vehicle, ti
denotes the time that the ith vehicle enters the control region,
et cetera. We assume that the vehicles enter the control region
with maximum speed, i.e., ẋ(ti,k ) = vm for all i ∈ N and all
k ∈ {1, 2}. To simplify notation, the ith vehicle that enters
lane k is at times referred to as vehicle (i, k).
We represent each vehicle as a two-dimensional, rectangular
rigid body with length l and width w. The position of this
rigid body is encoded with respect to one of the corners of

(0,0)

vehicles in lane 1

vehicles in lane 2

Fig. 2.

An illustration of the control region.

the intersection region. See Figure 2. The position of the rigid
body represents the center of the front bumper of the vehicle.
The orientation of this rigid body depends on which lane the
vehicle is traveling in. To formalize, let us define the rigid
body at position y ∈ R in lane k with R(y, k) ⊆ R2 , i.e.,
R(y, 1) := {(y1 , y2 ) ∈ R : y − l < y1 < y, 0 < y2 < w},

R(y, 2) := {(y1 , y2 ) ∈ R : 0 < y1 < w, y − l < y2 < y}.

With this notation, the ith vehicle in lane k at time t is
represented by R(xi,k (t), k). Let Ik (t) denote the indices for
all vehicles that are inside the control region at time t and in
lane k. For instance, if the 3rd, 4th, and 5th vehicles are in
lane 1 at time t, then we have I1 (t) = {3, 4, 5}. Clearly, Ik (t)
is a set of consecutive natural numbers for all t ≥ 0 and all
k ∈ {1, 2}. Finally, we define safety as follows.
Definition II.1 (Safety) The control region is said to be safe
at time t ∈ R≥0 , if there are no pairwise collisions among the
vehicles, i.e., R(xi,k (t), k)∩R(xj,l (t), l) = ∅ for all i ∈ Ik (t),
all j ∈ Il (t) and all k, l ∈ {1, 2} satisfying (i, k) 6= (j, l).
For each vehicle, we define the delay incurred in traveling
through the system, or delay for short, as follows. Recall that
ti,k is the time that the ith vehicle enters lane k. Let Ti,k
denote the time that the same vehicle completely exits the
intersection region, that is, the rear bumper of the vehicle is
outside the intersection region. More precisely, Ti,k is the time
instance for which xi,k (Ti,k ) = l + w. Then, the time it takes
for the same vehicle to travel through the system is Ti,k − ti,k .
We define the delay of a vehicle as the difference between this
time and (L + l + w)vm , which is the time it would have taken
the vehicle to travel through the system had the control region
been completely empty, i.e., at full velocity vm .
Definition II.2 (Delay) The delay for vehicle (i, k) is defined
as
L+l+w
.
Di,k := (Ti,k − ti,k ) −
vm
In this paper, we are interested in developing coordination
algorithms that govern this vehicle-to-infrastructure (V2I) controller such that both performance (e.g., in terms of delay)
and safety (avoiding collisions between vehicles) are simultaneously guaranteed. We will discuss such algorithms in the
next section. At this point, we tacitly leave out an important
part of the problem formulation: we do not specify anything
about the arrival process, i.e., the distribution of the stochastic
process {ti,k : i ∈ N}. We will discuss the arrival process as
a part of our model and assumptions in Section IV.
III. C ONTROL P OLICY
In this section, we propose a policy for the intersection
coordination problem we introduced in Section II. Our policy
is based on the polling policies that appear in the polling
systems literature [47]. Before describing our policy, we briefly
introduce queueing and polling systems and their analyses, in
order to provide a framework for scheduling vehicles in an
efficient manner. This framework is rich enough that we can
apply it to our intersection management problem as defined in
Section II, providing us our performance bounds.

A. Background on Queueing and Polling Systems
A widely-studied queueing model is the following. Suppose
customers arrive into a single queue in which their requests
are processed by a server, one by one, in the order that they
arrive. Let ti denote the arrival time of the ith customer. Let
si denote the time it takes for this customer to be serviced
by the server, once it gets the server’s attention. Often, both
{ti : i ∈ N} and {si : i ∈ N} are stochastic processes. Given
the statistics of arrival times and service times, the queueing
theory literature asks the following types of questions: What
is the average wait time for a typical customer? What is the
time average queue length? Queueing theory aims to answer
these kinds of questions for a variety of arrival and servicing
models. This literature has found profound applications in a
number of domains [44], including urban traffic [52].
Polling systems are extensions of queueing systems. In a
polling system, the server services multiple sets of customers
arriving in different queues. The server may choose to serve
the first customer of any queue. However, the server must pay
a set-up cost, also called the server switchover time, each time
it serves customers coming from a queue that is different than
the queue of the previous customer. This situation may arise,
for example, in manufacturing machines that need to change
their tooling each time they switch to processing a different
kind of manufacturing good, where changing the tooling may
require time. Another important application domain is data
networks. For instance, consider a network switch that is
serving packets arriving from different channels, and switching
channels requires running a piece of setup software that takes
a non-negligible amount of time to execute [48].
Let us describe a widely-studied polling system example
that extends the queueing system example we discussed earlier.
Consider a polling system with two queues and one server. Let
ti,k denote the time that customer i arrives in queue k, and let
si,k denote the amount of time it takes the server to service
this customer, where k ∈ {1, 2}. Both {ti,k : i ∈ N} and
{si,k : i ∈ N} are stochastic processes, for k = 1, 2. The
customers are serviced by the server one at a time. Switching
from one queue to the other requires a switchover time, say
r, which is also a random variable. That is, if the server last
serviced a customer from queue 1 and it decides to service
a customer from queue 2 next, then a switchover time of r
time units is incurred in addition to the service time of the
customers; no switchover time is required, if the server decides
to continue servicing customers from queue 1.1
Central to the operation of a polling system is a controller
that decides which queue to serve next. This decision is a
determining factor for the performance of the system, for
instance, in terms of the average delay and queue length. In
both cases, the control policy must trade off the following.
On the one hand, it should switch between different queues
often enough, in order to ensure customers in one queue
1 Strictly speaking, we must specify how the switchover time is incurred
when all queues are empty. For example, in the wait-and-see system (see [47]),
if all queues are empty, then the server waits in the queue it last serviced.
If a customer from this queue arrives, then no switchover time is incurred.
However, if a customer from a different queue arrives, then the server incurs
the switchover time and switches to the other queue.

do not wait too long for the server to process customers in
a different queue; on the other hand, the server should not
switch too often in order not to incur lengthy switchover
times. Ultimately, customers are clustered into chunks to be
processed one right after another. Best polling policies are
those that achieve the best cluster sizes in an online manner.
The polling systems literature analyzes the performance of
various polling policies [47]. Popular examples include the
exhaustive policy, gated policy, and the k-limited policy. In
the exhaustive policy, the server continues to service customers
from the same queue until that queue is empty. In the gated
policy, right after the server switches over to a new queue, it
takes a snapshot of this queue; and the server services only
those customers in the snapshot, but not the customers that
arrive after the snapshot is taken. In the k-limited policy, the
server services the customers in the same queue until either
k customers are serviced or all customers in the queue are
serviced, whichever comes first. Once these customers are
serviced, the server switches to the next queue. These policies
can be formalized easily. For details, we refer the reader, for
example, to the seminal paper by Takagi [47].
Most of the existing literature focuses on the case when the
interarrival times of the customers have independent identical
memoryless distribution, i.e., the arrival times process {ti,k :
i ∈ N} is a Poisson process for all queues k ∈ {1, 2}. For
example, the necessary and sufficient conditions for stability,
the expected delay, and the steady state queue length are
known for all of the three policies considered above [47].
In fact, these values can be computed when the number of
queues is more than two and the intensity of arrival times
is different across queues [47]. For more general arrival
processes, performance bounds and stability results have been
derived for the exhaustive and gated policies [53]–[55]. Mean
waiting times for exhaustive policy is always better than gated
policy in both a symmetric polling system and in a heavily
unbalanced polling system [56].
Unfortunately, it is analytically challenging to find optimal
policies [47], [57]. However, approximation results for limiting
cases are available. For example, on the one hand, the exhaustive policy is known to induce lower delay when compared to
the gated polling system in a light-load regime, i.e., when
the intensities of the arrival times are close to zero for both
queues [47]; Furthermore, for the G/G/1−type polling system
(general arrival process, general service time distributions,
single server, no stochastic independence assumptions), the
exhaustive policy was shown to be the optimal polling policy
in the sense of minimizing the total amount of unfinished work
in the system, which is the weighted sum (based on load) of
the mean waiting times [46], [58], [59]. In a fully symmetric
system, i.e., mean service times and arrival rates are the same
across all queues, this total workload is exactly the mean customer delay. In a system in which mean service times are the
same, but the arrival rates are allowed to differ across queues,
the total workload is exactly the mean waited customer delay.
Moreover, optimal polling policies have been investigated for
the GI/GI/1−type polling system (general arrival process,
general service time distribution, single server, all stochastic
processes are mutually independent). The exhaustive policy is

optimal in the sense of minimizing the number of customers in
the system [50]. For more general optimization criteria, such as
fairness, discounting, including switchtime cost, similar results
are available for the analytically-more-tractable M/GI/1-type
polling system [57], [60]. An extensive survey of optimal policies for various polling systems is given by Vishnevskii [46].
B. Simulating Polling Systems Behavior
The coordination algorithms we present below heavily rely
on the polling systems policies. In particular, in a number of
places we simulate the behavior of a polling policy forward
in time. We devote this section to formalizing this procedure.
Consider a polling system with two queues and deterministic service and set-up times. That is, the customers arrive
at times {ti,k : i ∈ N}, where k ∈ {1, 2}; their service
requires s time units, and the server requires r time units
to switch queues. The arrival times of the customers are
stochastic processes. For this section, consider the case when
the variables s and r are fixed (i.e., deterministic and same)
for all the customers. The coordination algorithm we present
in Section III-C relies on simulating the behavior of a polling
system that has fixed service time s and fixed set-up time r.
For notational convenience, we represent a polling system
with the symbol P. The algorithms we describe below interact with a polling system through two procedures. The
procedure P.AddToQueue(k) adds one customer to queue
k. The P.Simulate() procedure simulates the behavior of
the polling system, assuming no additional customers arrive.
More precisely, the P.Simulate() returns two sequences
of time instances, namely T1 = (τi1 ,1 , τi2 ,1 , . . . , τin1 ,1 ) and
T2 = (τj1 ,2 , τj2 ,2 , . . . , τjn2 ,2 ), where τi,k is the time that the
server is scheduled to begin servicing the ith customer in
queue k, assuming no additional customers arrive. We call τi,k
the schedule time of vehicle i in lane k. Note that the behavior
of the polling system P depends on its polling policy (e.g.,
exhaustive, gated, k-limited, et cetera), the service time s, the
set-up time r, the number of customers in the two queues, and
the time the server began servicing the current customer (if the
server is currently serving any customers) or the time that the
set-up operation started (if the server is currently switching to
the next queue). Given the values of all these variables, the
P.Simulate() procedure is well-defined, in the sense that the
sets T1 and T2 are uniquely determined, if the service time s
and the set-up time r are fixed. Given T1 and T2 , we define
the corresponding service order of vehicles as


(i1 , k1 ), . . . , (in , kn ) ,

described in Section II. More precisely, the control algorithm
computes trajectory xi,k for each vehicle (i, k) in the control
region, such that trajectory xi,k is dynamically feasible and
no two vehicles collide. The coordination algorithm is eventtriggered in the sense that each trajectory xi,k is updated
whenever a new vehicle arrives at the control region.
The core procedure embedded in this coordination algorithm is presented in Algorithm 1. Each time a new customer arrives in queue k, this procedure is triggered. The
procedure computes and returns the trajectories xi,1 for all
i ∈ {i1 , i2 , . . . , in1 } and xj,2 and all j ∈ {j1 , j2 , . . . , jn2 },
each time a new vehicle arrives at the control region.
Before presenting the coordination algorithm, let us introduce the following motion planning procedure. The
MotionSynthesize procedure generates a trajectory for each
vehicle given the time this vehicle must reach the intersection
region and the trajectory of the vehicle directly in front of it.
Furthermore, the trajectory is designed such that the vehicle
stays as closely as possible to the intersection region at all
times. Denote t00 as the arrival time of this vehicle, and t0f as
the time to reach the intersection region. Suppose the trajectory
of the vehicle in front is denoted by y : [t0 , tf ] → R,
where t0 is its arrival time in the control region and tf is
the time it is scheduled to reach the intersection region. Then,
the MotionSynthesize procedure computes a new trajectory
such that the two vehicles do not collide and the new trajectory
reaches the intersection at time t0f with full speed vm , i.e.,
MotionSynthesize(zi,k (t00 ), t00 , t0f , y) :=
Z tf
arg min
|x(t)|dt

x : [t00 ,t0f ]→R

subject to ẍ(t) = u(t), for all t ∈ [t00 , t0f ];

0 ≤ ẋ(t) ≤ vm , for all t ∈ [t00 , t0f ];
|u(t)| ≤ am , for all t ∈ [t00 , t0f ];

|x(t) − y(t)| ≥ l, for all t ∈ [t00 , tf ];
x(t00 ) = xi,k (t00 );
x(t0f ) = 0;

k=1

τim−1 ,km−1 < τim ,km .

k=2

C. The Intersection Coordination Algorithm
We propose an event-triggered coordination algorithm that
plans the motions of all vehicles that enter the control region as

ẋ(t00 ) = ẋi,k (t00 );

ẋ(t0f ) = vm ,

where zi,k (t00 ) is the state of vehicle (i, k) at time t00 , i.e.,
zi,k (t00 ) := xi,k (t00 ), ẋi,k (t00 ) , vm and am are the maximum
velocity and the maximum acceleration of the vehicles, l is
the length of the vehicle, and L is the control region length.
Each time a new customer arrives in the control region from
lane k, Algorithm 1 is triggered. First, the algorithm executes

such that the schedule time is strictly increasing in this
sequence, i.e., for all m > 1,

See Figure 3 for an example of computing schedule times and
service order under the exhaustive policy.

t0

c
a b

d e f

j
g

h i

Fig. 3.
Arrival times for vehicles in lanes 1 and 2 are depicted with
the vertical arrows. Newly arrived vehicle j is in lane 1. Shaded blocks
show the result of simulating the polling system under the exhaustive policy.
Service time s and switch time r are assumed to be s = r = 1.
T1= (4, 5, 6, 7, 13). T2 = (1, 2, 9, 10, 11). Schedule order for this instance

is (a, 2), (b, 2), (c, 1), (d, 1), (e, 1), (f, 1), (g, 2), (h, 2), (i, 2), (j, 1) .

P.AddToQueue(k);
(T1 , T2 ) ← P.Simulate();
3 for k = 1, 2 do
4
for i = 1, 2, . . . , nk do
5
τi,k ← Tk (i);
6
xi,k ← MotionSynthesize(xi−1,k , τi,k + vLm );
7
end
8 end
Algorithm 1: NewArrival(k) procedure is triggered when a new
vehicle arrives in the control region from lane k.
1

2

Control region

Intersection region

occur in the control region. (See Definition II.1.) Second, the
coordination algorithm provides good performance. Recall that
the coordination algorithm simulates a polling system. We
show that the additional delay incurred in traversing the control
region is no more than the delay that would incur in servicing
a customer in the corresponding polling system.
These guarantees hold under certain assumptions. In what
follows, we present a class of stochastic process models for
vehicle arrival times and two important assumptions. Subsequently, we state and prove our theoretical results.
A. Vehicle arrival time model

(b)

(a)

(c)

Fig. 4. We show the three time instances that are described in the text. In (a),
the front bumper of the vehicle is entering the intersection region. In (b), the
rear bumper is leaving the control region and entering the intersection region.
In (c), the rear bumper is leaving the intersection region. In the figure above,
the control region (shaded light grey) and the intersection region (shaded in
dark grey) are shown.

the AddToQueue(k) procedure, which adding into queue k
of polling system P one customer that represents the newly
arrived customer (Line 1). Then, the algorithm simulates the
polling system forward in time assuming no new customers
will arrive, with fixed service time s = l/vm and fixed setup time r = w/vm (Line 2). Note that the service time s
is precisely the amount of time from when a vehicle’s front
bumper enters the intersection region to when its rear bumper
leaves the control region assuming the vehicle is traveling
at vm . The set-up time on the other hand is the amount of
time from when the vehicle’s rear bumper leaves the control
region to when its rear bumper exits the intersection region.
The result of the simulation is a sequence of times, namely T1
and T2 . Let us denote the ith element of Tk by τi,k . Finally, the
algorithm generates a trajectory using the MotionSynthesize
procedure for each vehicle such that the ith vehicle in lane k is
scheduled to reach the intersection at time τi,k + L/vm while
avoiding collision with the vehicle in front (Lines 3-8). To be
precise, define x0,k (t) = l for all t ∈ R≥0 and k ∈ {1, 2};
then, MotionSynthesize computes xi,k for all i 6= 0.2
We emphasize that the coordination algorithm depends on
the polling policy that governs the polling system P. In the
next section, we show that safety is guaranteed for a wide
range of polling policies and that performance can be bounded
with respect to the performance of the polling system P, which
in turn depends on the said polling policy.

First, we model the arrival times {ti,k : i ∈ N} as a
hard-core stochastic point process on the non-negative real
line [61] such that the ti+1,k − ti,k ≥ l/vm for all i ∈ N and
k ∈ {1, 2}.3 The inequality guarantees that no two vehicles
are in collision at the time of arrival. A widely studied hardcore stochastic point process is the Matérn process [62]–[64].
First, a marked Poisson point process is generated on the
non-negative real line, where each point ti is marked with
an independent uniformly random real number between zero
and one denoted by m(ti ). Subsequently, any point ti that
satisfies the following is deleted: there exists another tj such
that the distance between ti and tj is no more than d and
m(tj ) > m(ti ). Clearly, no two points have distance less than
d. Hence, the Matérn process is a hard-core point process.
In addition, we model the effect of overcrowding in the
control region to rule out cases that trivially contradict safety.
More precisely, we tacitly assume that, if vehicle i arrives
in lane k at time ti,k and at this time there is no input that
saves it from hitting the car in front, then the same vehicle
chooses not to enter the intersection.4 Let us emphasize that
this assumption does not guarantee safety immediately. That
there exists some path that does not lead to a collision does
not immediately imply that there is a path that is both safe
and provides performance guarantees. In other words, this
assumption does not render the problem trivial. Without such
an assumption, however, the system is (trivially) unsafe. We
emphasize that it is impossible to guarantee safety without
this assumption. That is, without it, one is faced with trivial
cases where any coordination algorithm is unsafe with a nonzero probability. However, we show through computational
experiments that the chance that a vehicle must choose not to
enter the intersection is small, when the road length L is very
large or the intensity of the arrival times is very small (i.e.,
the light load case). We conjecture that our analysis applies in
these limiting cases, even without this assumption.

IV. ANALYSIS
In this section, we show that the coordination algorithm
presented in Section III-C has two important properties.
Firstly, the algorithm is safe in the sense that no collisions
2 Since the vehicle closest to the intersection region in each lane
does not have a vehicle directly in front, the safety constraint in the
MotionSynthesize procedure, i.e., |x(t) − y(t)| ≥ l, can be ignored.

3 Roughly speaking, hard-core point processes are those that ensure the
distance between its points is lower bounded by a certain number. See [61].
4 For practical purposes, we envision a system with a fork right at the
entrance of the control region, and when it is clearly unsafe to enter the control
region (i.e., it is impossible to avoid a collision), then the same vehicle takes
the exit at the fork and does not enter the control region.

B. Assumptions on polling policies and road length

C. Main theoretical results: Safety, performance and stability

Our first assumption is rather technical. Recall that the
behavior of the coordination algorithm depends on the policy
of the polling system it simulates. We introduce a regularity
assumption on this policy. Roughly speaking, we assume that
the policy does not respond to the arrival of a customer in
queue 1 by favoring the servicing of customers in queue 2,
and vice versa. The assumption is formalized as follows.

Our first two main theoretical results are the safety and
performance guarantees, presented below in Theorems IV.5
and IV.6. These results are enabled by the following lemma.

Assumption IV.1 (Regular polling policies) Suppose
customer
A arrives at time t00 in queue 1. Let

(i1 , k1 ), . . . , (in , kn ) be the service order of customers
computed at the previous call to Algorithm 1 before the
arrival of customer A. At time t00 , we add customer A to
queue 1 and afterwards simulate the polling system according
to P , i.e., P.AddToQueue(1); (T10 , T20 ) ← P.Simulate()
to obtain (T10 , T20 ). Then, we say that the polling policy
P is regular if customer A is simply inserted into the
service order of customers, without otherwise disturbing the
service
 order of customers, i.e., the updated service order

is (il , kl ), . . . , (im−1 , km−1 ), (A, 1), (im , km ), . . . (in , kn ) ,
where 1 ≤ l ≤ m ≤ n. The same result also holds for queue
1, whenever a new customer is added to queue 2.

Before providing the proof of this lemma, let us point out
two important corollaries, which are our main results.
First, the algorithm guarantees safety surely. In other words,
it is guaranteed that no collisions occur as new customers
arrive at the control region.

Let us note that this assumption is satisfied by many widelystudied polling policies, as stated in the proposition below.
Proposition IV.2 The exhaustive, gated, and the k-limited
polling policies (see Subsection III-A) satisfy Assumption IV.1.
Proof: (Sketch) This result can be verified easily for
each of the three policies. We sketch the proof only for
the exhaustive policy. Without loss of generality, suppose a
new customer arrives in queue 1. First, suppose the server is
currently servicing a customer from queue 1. This would cause
the queue 2 customers to be serviced after the newly arrived
customer, while not affecting the service order of customers
in queue 1. Second, suppose the server is servicing a customer
from queue 2 at the arrival time of the new customer. Then,
the the newly arrived customer is simply inserted at the end of
the previous service order, not affecting the schedule order of
the customers in the polling system. Hence, Assumption IV.1
holds for the exhaustive policy. Similarly, it can be shown that
Assumption IV.1 is satisfied by gated and k-limited policies,
merely by utilizing the same proof technique.
Our second assumption is that the length of the control
region is bounded from below by a certain distance L∗ . Note
that this minimum road length L∗ is a function of the dynamics
of the vehicles, but not of the geometry of the vehicles.
Assumption IV.3 (The length of the control region) The
length L of each of the control regions is bounded from below
2
as follows: L ≥ 2vm
/am := L∗ .
As we will show later, this assumption guarantees that the
control region is long enough to allow the safe coordination
of vehicles while guaranteeing good performance.

Lemma IV.4 Suppose Assumptions IV.1 and IV.3 hold. Then,
each time a new vehicle arrives and Algorithm 1 is called,
every call to the MotionSynthesize procedure (Line 6 of
Algorithm 1) yields a feasible optimization problem.

Theorem IV.5 (Safety) Suppose Assumptions IV.1 and IV.3
hold. Then, the control region is safe at all times t ≥ 0 in
the sense of Definition II.1.
Second, the delay experienced by the vehicles is bounded
by the delay of the corresponding polling system.
Theorem IV.6 (Performance) Suppose Assumptions IV.1 and
IV.3 hold. Recall that the delay incurred in transitioning
through the control region for vehicle i in lane k is denoted
by Di,k . (See Section II.) Let Wi,k denote the wait time of
the corresponding customer added to the polling system (see
Algorithm 1, Line 1) when vehicle i arrives in the control
region from lane k. Then, we have the following:
Di,k ≤ Wi,k ,

almost surely.

In essence, Theorem IV.6 states that the differential constraints that bind the customers as they traverse the control
region are irrelevant. The delay is no more than the delay that
is incurred in the corresponding polling system. Hence, one
can employ any polling policy in the coordination algorithm
and guarantee the same performance provided by this policy.
We emphasize that the expected wait time is known for many
polling policies, in particular the exhaustive and gated policies,
when the arrival process is Poisson [47].
Our third main result is a stability criterion, presented in
Theorem IV.9. Let us define Ak (t) as the total number of
vehicles that have arrived in lane k during the time interval
[0, t], according to the arrival process {ti,k : i ∈ N}. Next, let
us define θkL,P (t) as the number of lane k vehicles that are
thinned due to the overcrowding assumption during the time
interval [0, t]. Note that θkL,P (t) changes with the road length
L and the polling policy P chosen for the algorithm; however,
when the polling policy is clear from context we simply write
θkL (t). We are ready to present the notion of stability.
Definition IV.7 (Stability) We say that the coordination algorithm with polling policy P and arrival process {ti,k } is
stable if
θ1L,P (t) + θ2L,P (t)
] = 0.
L→∞ t→∞
t
If the coordination algorithm is not stable, in the above sense,
then we say that the algorithm is unstable.
lim lim E[

0:16

Expected Wait Time (s)

Note that the stability of the coordination algorithm is dependent only on the arrival process and the polling policy and
does not depend on the length of the control region. Intuitively,
this definition of stability means that as we make the control
region longer and longer, the intensity of the thinned vehicles
tends to 0. Next, we define SkL,P (t) = Ak (t) − θkL,P (t). This
is the number of lane k vehicles that enter the control region
and are assigned a schedule time by the coordination algorithm
during the time interval [0, t]. Again, when clear from context,
the superscript P denoting the polling policy will be dropped.
We present the following lemma.

Corresponding Polling System with Poisson Arrivals
Theoretical assuming Poisson Arrivals
Corresponding Polling System with Mat4ern Arrivals
Proposed Algorithm with Mat4ern Arrivals

0:14
0:12
0:1
0:08
0:06

Lemma IV.8 For any control region length L > 0, and any
polling policy P, we have the following:

1
S L,P (t) + S2L,P (t)
]≤ ,
lim E[ 1
t→∞
t
s
where s is the service time of a vehicle.

Now, we present a sufficient condition for the instability
of the coordination algorithm. By ergodicity of the arrival
processes, we have that for any t, E[Ak (t)/t] = λk , which
is the intensity of the arrival process in lane k.

0:04

0

0:1

0:2

0:3

0:4

0:5

0:6

0:7

0:8

0:9

1

6
Fig. 7.
Comparison of the expected wait time of Algorithm 1 and the
corresponding polling system according to a Matérn arrival process with
parameter λ. The expected wait time of a polling system with Poisson arrival
process with intensity λ is computed via simulations and also using polling
systems theory. All polling policies in this figure use exhaustive polling policy.

V. C OMPUTATIONAL E XPERIMENTS
Theorem IV.9 (Sufficient condition for instability) Let
Ak (t) count the arriving vehicles in lane k, and λk be the
intensity of the arrival process in lane k. The coordination
algorithm is unstable, if
1
.
s
Proof: For any L > 0 and t > 0, we have
λ1 + λ2 >


θ1L (t) + θ2L (t) = A1 (t) + A2 (t) − S1L (t) + S2L (t) .
Dividing by t, taking expectation, and then taking the limit,
we obtain
θ1L (t) + θ2L (t)
S L (t) + S2L (t)
] = λ1 + λ2 − lim E[ 1
]
t→∞
t→∞
t
t
1
≥ λ1 + λ2 − > 0.
s
Since this holds for any L > 0, the system is unstable.
Furthermore, we conjecture the following.
lim E[

Conjecture IV.10 (Sufficient condition for stability) Let P
be any regular polling policy. If
1
,
(2)
s
then the coordination algorithm with policy P is stable.

In this section, we evaluate the proposed coordination
algorithm in simulation studies. First, we describe the simulation setup and the computational methods used in this
study in Section V-A. Subsequently, we describe the results
of computational experiments for light-load, medium-load
and heavy-load cases in Section V-B, where we observe the
platooning phenomenon. Next, we study the effects of the
overcrowding assumption in Section V-C. Lastly, we compare
the performance of our algorithm with an intersection managed
by a traditional traffic light with human drivers in Section V-D.
A. The simulation environment and computational methods
We used Matlab to generate the simulation results presented
in this section, solving the linear programs with Gurobi [65].
Incoming vehicle arrival times were generated by a Matérn
process [64], independent in each lane, but symmetric, i.e.,
equal arrival intensities. The vehicle length, width, maximum
velocity, and maximum acceleration were taken to be 2 meters,
1 meter, 10 meters/sec, and 4 meters/sec2 , respectively.
Vehicle trajectories were obtained by discretizing the
MotionSynthesize procedure, leading to a linear program:

λ1 + λ2 <

This sufficient condition for stability is inspired by the similar
results in the polling system literature [47]. The restriction to
regular polling policies in the above conjecture is crucial. For
instance, consider the polling policy that only allows one lane
access and completely stops the other lane; this policy is not
a regular polling policy. Even when the arrival rates λ1 and
λ2 are chosen to satisfy Inequality (2), the intensity of thinned
vehicles will always be greater than some  > 0 for any L.
Thus, this (non-regular) policy is unstable.

max

N
X

xi

i=0

xi+1 = xi + (vi + vi+1 ) · ∆t/2, for all i;
vi+1 = vi + ui · ∆t, for all i;
0 ≤ vi ≤ vm , for all i;

−am ≤ ui ≤ am , for all i;

xi ≤ y(t0 + i · ∆t) − l, for all i;

x0 = −L; xN = 0;

v0 = vm ; vn = vm ,

Fig. 5. Trajectories of vehicles controlled by Algorithm 1 with the exhaustive polling policy. The top, middle, and bottom figures are representatives for
the light, medium, and heavy load cases, respectively. The top half of each figure shows the trajectories of vehicles in Lane 1, and the bottom half shows the
trajectories of vehicles from Lane 2. Each figure shows a small window of time after the system reaches steady state.

where ∆t is the constant step size of the time history, t0 is
the arrival time of some vehicle that triggers the algorithm,
[x0 , . . . xN , v0 , . . . , vN , u0 , . . . , uN −1 ]T is the state vector,
representing the discretized position, velocity and acceleration
of the vehicle at time t0 + i · ∆t, and y is the trajectory of the
vehicle directly in front. Note that the trajectory y is evaluated
at time t0 +i·∆t by a quadratic interpolation scheme, assuming
a linear velocity history ẏ over the relevant time interval.
Note that the running time of our algorithm scales polynomially with increasing number of vehicles, since we solve
this linear program at most once for each vehicle that is in the
system, when a new vehicle arrives in the system.
We initialize the polling system with deterministic service
and switching times, as mentioned in section III-C. We use the
“wait-and-see” rule [47] to govern the switching dynamics.
Expected delays were estimated by running a simulation
for a long time and then averaging the delay incurred by each
vehicle in the system. We find that N = 800 yields sufficiently
smooth trajectories. However, in general, the required degree
of discretization increases with longer control regions.

B. The effect of load conditions and platooning
In Figure 5, we show trajectories of vehicles under an
exhaustive policy with increasing load. In Figure 6, we show
trajectories of vehicles under a k-limited polling policy. In
none of these simulations, the MotionSynthesize procedure
reported infeasible, which is in line with Lemma IV.4, hence
with Theorems IV.5 and IV.6.
First, notice that the “platooning behavior” emerges naturally, particularly when the load becomes substantial. Specifically, vehicles slow down to form a cluster with other vehicles.
This cluster of vehicles speeds up and passes through the
intersection at maximum speed. In this way, the intersection,
which is the shared resource, is utilized as efficiently as
possible. The vehicles that slow down do so at the latest
time possible, so that more vehicles can be packed into the
control region. This behavior is observed easily in Figure 5
as a result of utilizing the exhaustive policy. It is visible in
the utilization of the k-limited policy as well, as shown in
Figure 6. This platooning behavior emerges naturally, as it also
arises in polling systems, which we discussed in Section III-A.

Fig. 6. Trajectories of vehicles controlled by Algorithm 1 with the k-limited polling policy, where k = 4 in the upper plot and k = 8 in the lower plot. The
top half of each figure shows the trajectories of vehicles in Lane 1, and the bottom half shows the trajectories of vehicles from Lane 2. Each figure shows a
small window of time after the system reaches steady state.

Next, we compute performance bounds on Algorithm 1
under the exhaustive policy, assuming vehicles arrive according to a Matérn process. Define the Poisson process with
parameter λ as the Poisson point process in the line that has
intensity λ. Define the Matérn process with parameter λ as the
Matérn process obtained by thinning a Poisson process with
parameter λ. Note that the intensity of the Matérn process with
parameter λ is
1 − exp(−2λb)
,
2b
where b is the service time [64]. In Figure 7, we compare the
performance of our proposed algorithm with the performance
of the corresponding polling systems, which is in line with our
result in Theorem IV.6. Note that the average delay incurred
in Algorithm 1 for Matérn arrivals of intensity λI (λ) seems
to be bounded from above by the average wait time of an
exhaustive polling system with Poisson arrivals of intensity λ.
We observe this behavior consistently in simulation studies,
and we conjecture it holds for all regular polling policies.
λI (λ) =

C. Unsafe arrivals in terms of road length and arrival rate
Recall the overcrowding model from Section IV-A. We
remove any incoming vehicle before it ever enters the control
region, if the same vehicle has no safe trajectory. We say that
this vehicle is diverted away from the control region. We call
this phenomenon diversion. The result is that the the stochastic
process is “thinned” by the removal of unsafe vehicles.
In this section, we study the effect of road length and arrival
rate on unsafe arrivals. Our key observation is the following.
In the computational experiments we present here, we find that

the fraction/intensity of the vehicles that are diverted converges
to zero exponentially fast with increasing road length as well
as with decreasing vehicle arrival rate. In fact, we observe
that this overcrowding phenomenon is extremely rare, when
the road length is reasonably large (e.g., twice the limit given
by Assumption IV.3) and the arrival process slightly below the
limit of instability (e.g., 90% of instability limit).
Before presenting our simulation results, note that the choice
of polling policy affects overcrowding. For the remainder of
the simulations carried out in this section, we employed the
exhaustive policy, although any regular polling policy can be
used and should show similar results. First, let us consider how
often a vehicle may be removed. In Figure 8, the percentage
of diverted vehicles is plotted against arrival process intensity.
These quantities are each per lane, i.e., arrival process intensity
in each lane, and percentage of vehicles per lane. We see
that until approximately 2.15 vehicles per second, virtually no
thinning occurs. These results were obtained by running our
proposed algorithm for 50,000 seconds of simulation time, and
comparing the number of thinned vehicles per lane to the total
number of vehicles that approached that lane. Note that this
simulation uses the shortest possible control region length L∗ ,
as determined by Assumption IV.3. For longer control regions,
the onset is pushed even closer to the point of instability.
Next, we consider how often diversion occurs as a function
of control region length. In Figure 9, we plot the log of the
intensity of thinned vehicles as a function of control region
length. We fix an arrival rate of 2.45 vehicles per second
in each lane (while instability limit is at 2.5 vehicles per
second). Each data point represents 50,000 to 100,000 seconds
of simulation time, depending on how quickly the intensity

log10 (Fraction of Thinned Vehicles)

0

!2:5

log( Intensity of Thinned Veh)

!0:5
!1

!1:5
!2

!2:5
!3

!3:5
!4

!4:5
!5

0

0:05

0:1

0:15

0:2

0:25

0:3

0:35

0:4

Arrival Process Intensity below Instability (veh/sec)

!3
!3:5
!4
!4:5
!5
!5:5
!6
50

60

70

80

90

100

110

120

130

L (m)

Fig. 8. Logarithm of the fraction of thinned vehicles is plotted as a function
of arrival process intensity. Algorithm 1 is simulated with exhaustive polling
policy. The length of the control region is set to the minimum control region
length L∗ (See Assumption IV.3) to guarantee safety of Algorithm 1.

Fig. 9. Logarithm of the intensity of thinned vehicles is plotted as a function
of control region length L. Algorithm 1 was run with exhaustive polling policy
and arrival intensity of 2.45 vehicles per second (close to 2.5 vehicles per
second instability limit) in each lane according to Matérn process.

converged. The log of the diversion intensity and road length
seem to have a linear correspondence. Thus, the number of
diversions decreases exponentially with increasing road length.

see the the phase of its lane, no matter how far away the
vehicle may be from the intersection; however, the vehicle
does not know how much longer the current phase will last.
Also, we assume that the a vehicle has perfect knowledge
of the instantaneous velocity of the vehicle directly in front
and also of the distance to the vehicle directly in front.
Although the vehicle does not know the future trajectory of the
vehicle in front, the vehicle does know what the instantaneous
acceleration of the vehicle in front is. Although knowledge of
the instantaneous acceleration of the vehicle in front is not
realistic, this assumption actually improves the performance
of the traffic light, making a more competitive scenario with
which we benchmark our algorithm.
With these assumptions in mind, we design the most aggressive control action for each vehicle such that safety with
the vehicle in front is ensured at the next time step. A vehicle
decides its acceleration as to position itself as close as possible
to the vehicle directly in front, while also preserving safety.
The manner in which we do this is by finding, at each time
step, the maximum acceleration over the next time interval that
will guarantee the following condition is satisfied at the next
2
time step: vi2 /(2am ) ≤ d − l + vi−1
/(2am ), where d is the
distance between the bumpers of the two vehicles, and vi and
vi−1 are the velocities of the vehicle and the vehicle directly
in front at the next time step, respectively. The vehicle has
knowledge of the right-hand side of the inequality, i.e., the
distance that the vehicle in front will travel if it slams on the
breaks beginning at the next time step. The left-hand side is the
distance that the current vehicle will travel if it slams on the
breaks beginning at the next time step. We choose maximum a
such that vi := vcurrent +a∆t will satisfy the above inequality.
We define delay for a vehicle approaching the traffic light
similarly to our definition of delay earlier. (See Definition II.2.)
Delay is the additional time required for a vehicle to traverse
the intersection due to the presence of other vehicles on the
road and the traffic light, i.e., the difference in time between

D. Comparison with a traditional traffic light
In this section, we compare the performance of the proposed
algorithm with a similar traffic light scenario.
Consider two lanes of vehicles approaching an intersection
of the same dimensions as the system described in Section II.
Geometries and dynamics of the vehicles are the same as in
the earlier problem definition. The positions of the vehicles are
populated in the real line according to a Matérn process, and
all vehicles start with the maximum velocity vm . The traffic
light cycles through three phases for each lane in the following
order: green, yellow, red, yellow, and so on.
In the green phase, vehicles quickly accelerate to maximum
speed, attempting to pass through the intersection. During the
red phase, vehicles decelerate quickly to a full stop, approaching as close to the intersection as safely possible. During the
yellow phase, vehicles do either one of two things. If the
vehicle can come to a full stop before entering the intersection,
the vehicle decelerates to a stop as close to the intersection
as safely possible; otherwise, the vehicle accelerates quickly
and passes through the intersection. This implies that a yellow
phase which follows a red phase is simply an extension of the
previous red phase. Also, the traffic cycle is staggered so that
while one lane is in its green phase, the other is in its red
phase, and vice versa. Although both lanes are in the yellow
phase simultaneously, due to the staggering of the green and
red phases, only one lane permits its vehicles to traverse the
intersection, provided that they cannot safely come to a stop
before entering the intersection region.
At each time step, an aggressive control action is determined
locally for each vehicle, inspired by the following assumptions.
To keep in line with the traffic light scenario, we assume
that in determining a vehicle’s control action, the vehicle can

8
7
6

Delay (sec)

VI. CONCLUSIONS

Proposed Algorithm
Tra/c Light with g=r=5 sec
Tra/c Light with g=r=10 sec
Tra/c Light with g=r=15 sec
Exhaustive Policy with Poisson Arrivals

5
4
3
2
1
0
0

0:5

1

1:5

2

2:5

Arrival Process Intensity (veh/sec)
Fig. 10. Delay of traffic intersection under Algorithm 1 with exhaustive
policy compared with delay of a traditional traffic intersection with equal
green and red light phases of 5 seconds, 10 seconds, and 15 seconds. Expected
wait time of polling system with exhaustive policy and Poisson arrivals is also
plotted.

a vehicle fully traversing through the system and a vehicle
continuing at maximum velocity through the system.
The traffic light scenario described provides a lower bound
on the average delay of a traditional traffic light intersection.
The results of the comparison study are provided in Figure 10. Algorithm 1 with the exhaustive policy outperforms
this traffic light scenario by at least two orders of magnitude
in terms of delay. Beyond an arrival rate of 0.5 vehicles per
second in each lane, the delay of the traffic light scenario
increases drastically, even though the point of instability of an
intersection of these two flows is 2.5 vehicles per second.
The durations of the red and green light phases are equal
to each other, while the duration of the yellow light phase is
always a constant, depending only on the geometry and the
dynamics of the vehicles. The duration of the yellow light
phase is the minimum amount of time to guarantee safety
between the two lanes of intersecting traffic, i.e., suppose a
vehicle sees a red light and cannot come to a stop before the
intersection region, then it must be allowed to safely traverse
the intersection region. For example, suppose a vehicle is
traveling at full speed vm at the instant the traffic light switches
from the green phase to yellow phase. The distance required
2
for the vehicle to come to a full stop is vm
/(2am ). However,
suppose the remaining road length in front of the vehicle is
slightly less than this quantity. Then, the vehicle will continue
with full speed in order to traverse the intersection region. The
time it takes to exit the intersection region is slightly less than
vm /(2am )+(l+w)/vm . Thus a necessary condition for safety
is that the duration of the yellow phase must be at least as
long. Furthermore, one can also show that for any other initial
configuration of position and velocity at the end of the green
phase, the time it takes for such a problematic vehicle to clear
the intersection is no greater than vm /(2am ) + (l + w)/vm .
We set the duration of the yellow phase to this quantity.

In this paper, we considered the problem of coordinating
the motion of vehicles through an intersection with no traffic
lights. We proposed a coordination algorithm that provides
provable guarantees on both safety and performance in allautonomous traffic intersections. The proposed algorithm, at
its core, schedules vehicles to use the intersection region according to a polling policy, which can be selected from a wide
variety of policies for traditional polling systems. Provable
performance bounds were established for the average delay
of the proposed algorithm by considering its corresponding
polling policy. It was also shown that the proposed algorithm is
computationally efficient. In simulation studies, the proposed
system was compared to a traditional intersection system
with a red-yellow-green traffic light; it was shown that the
proposed system achieves delays that are at least two orders
of magnitude smaller when compared to the traditional system.
R EFERENCES
[1] J. B. Greenblatt and S. Saxena, “Autonomous taxis could greatly reduce
greenhouse-gas emissions of US light-duty vehicles,” Nature Climate
Change, vol. 5, no. 9, pp. 860–863, Jul. 2015.
[2] D. J. Fagnant and K. Kockelman, “Preparing a nation for autonomous
vehicles: opportunities, barriers and policy recommendations,” Transportation Research Part A, 2015.
[3] K. B. Sandvik and K. Lohne, “The Rise of the Humanitarian Drone:
Giving Content to an Emerging Concept,” Millennium - Journal of
International Studies, vol. 43, no. 1, pp. 145–164, Sep. 2014.
[4] R. D’Andrea, “Guest Editorial Can Drones Deliver?” IEEE Transactions
on Automation Science and Engineering, vol. 11, no. 3, pp. 647–648,
Jun. 2014.
[5] D. J. Fagnant and K. M. Kockelman, “The travel and environmental
implications of shared autonomous vehicles, using agent-based model
scenarios,” Transportation Research Part C, 2014.
[6] E. Guizzo, “Three Engineers, Hundreds of Robots, One Warehouse,”
IEEE Spectrum, pp. 26–34, Jun. 2008.
[7] P. R. Wurman, R. DAndrea, and M. Mountz, “Coordinating Hundreds
of Cooperative, Autonomous Vehicles in Warehouses,” AI Magazine,
vol. 29, no. 1, pp. 9–19, Mar. 2008.
[8] S. Hoshino and J. Ota, “Design of an automated transportation system in
a seaport container terminal for the reliability of operating robots,” in Intelligent Robots and Systems, 2007. IROS 2007. IEEE/RSJ International
Conference on. IEEE, 2007, pp. 4259–4264.
[9] S. Hoshino, J. Ota, A. Shinozaki, and H. Hashimoto, “Design of an agv
transportation system by considering management model in an act.” in
IAS, 2006, pp. 505–514.
[10] M. Grunow, H.-O. Günther, and M. Lehmann, Dispatching multi-load
AGVs in highly automated seaport container terminals. Springer, 2005.
[11] R. Lyons, “Complexity analysis of the Next Gen Air Traffic Management
System: trajectory based operations,” Work A Journal of Prevention,
Assessment and Rehabilitation, vol. 41, no. 1, pp. 4514–4522, Feb. 2012.
[12] R. D. M. M. Peter R Wurman, “Coordinating Hundreds of Cooperative,
Autonomous Vehicles in Warehouses,” pp. 1–12, Mar. 2008.
[13] P. S. K Dresner, “A Multiagent Approach to Autonomous Intersection
Management,” Journal of Artifical Intelligence Research, vol. 31, pp.
591–656, Feb. 2009.
[14] P. S. Tsz-Chiu Au, “Motion Planning Algorithms for Autonomous
Intersection Management,” in Bridging the Gap Between Task and
Motion Planning Workshop, Jun. 2010.
[15] R. Azimi, G. Bhatia, R. Rajkumar, and P. Mudalige, “Intersection
Management using Vehicular Networks,” in Society for Automotive
Engineers (SAE) World Congress, Jan. 2012, pp. 1–13.
[16] S. Azimi, G. Bhatia, R. Rajkumar, and P. Mudalige, “Reliable intersection protocols using vehicular networks,” in Cyber-Physical Systems
(ICCPS), 2013 ACM/IEEE International Conference on. IEEE, 2013,
pp. 1–10.
[17] R. Azimi, G. Bhatia, R. R. Rajkumar, and P. Mudalige, “Stip: Spatiotemporal intersection protocols for autonomous vehicles,” in ICCPS’14:
ACM/IEEE 5th International Conference on Cyber-Physical Systems
(with CPS Week 2014). IEEE Computer Society, 2014, pp. 1–12.

[18] E. Onieva, V. Milanés, J. Villagrá, J. Pérez, and J. Godoy, “A fuzzy
aid rear-end collision warning/avoidance system,” Expert Systems With
Applications, vol. 39, no. 18, pp. 13 148–13 157, Dec. 2012.
[19] K. Liu, E. Chan, V. Lee, K. Kapitanova, and S. H. Son, “Design
and Evaluation of Token-based Reservation for a Roadway System,”
Transportation Research Part C, vol. 26, pp. 184–202, Jan. 2013.
[20] F. Perronnet, A. Abbas-Turki, and A. El Moudni, “A sequenced-based
protocol to manage autonomous vehicles at isolated intersections,” in
Intelligent Transportation Systems-(ITSC), 2013 16th International IEEE
Conference on. IEEE, 2013, pp. 1811–1816.
[21] D. Carlino, S. D. Boyles, and P. Stone, “Auction-based autonomous
intersection management,” in IEEE Intelligent Transportation Systems
Conference, Sep. 2013.
[22] Q. Lu and K.-D. Kim, “Intelligent intersection management of autonomous traffic using discrete-time occupancies trajectory,” Journal of
Traffic and Logistics Engineering Vol, vol. 4, no. 1, 2016.
[23] P. Dai, K. Liu, Q. Zhuge, E. H.-M. Sha, V. C. S. Lee, and S. H.
Son, “Quality-of-experience-oriented autonomous intersection control in
vehicular networks.”
[24] P. Tallapragada and J. Cortés, “Hierarchical-distributed optimized coordination of intersection traffic,” arXiv preprint arXiv:1601.00246, 2016.
[25] R. Hult, G. R. Campos, P. Falcone, and H. Wymeersch, “An approximate
solution to the optimal coordination problem for autonomous vehicles
at intersections,” in American Control Conference (ACC), 2015. IEEE,
2015, pp. 763–768.
[26] K.-D. Kim and P. R. Kumar, “An mpc-based approach to provable
system-wide safety and liveness of autonomous ground traffic,” Automatic Control, IEEE Transactions on, vol. 59, no. 12, pp. 3341–3356,
2014.
[27] H. Duo, Y. Mo, and R. M. Murray, “Synthesis of Distributed Longitudinal Control,” in IEEE Conference on Decision and Control, 2015.
[28] I. Zohdy, R. K. Kamalanathsharma, and H. Rakha, “Intersection management for autonomous vehicles using cooperative adaptive cruise control
systems,” in Transportation Research Board 92nd Annual Meeting, no.
13-0772, 2013.
[29] M. Lupu, E. Feron, and Z.-H. Mao, “Traffic complexity of intersecting
flows of aircraft under variations of pilot preferences in maneuver
choice,” in Decision and Control (CDC), 2010 49th IEEE Conference
on. IEEE, 2010, pp. 1189–1194.
[30] Z.-H. Mao, E. Feron, and K. Bilimoria, “Stability and performance of
intersecting aircraft flows under decentralized conflict avoidance rules,”
Intelligent Transportation Systems, IEEE Transactions on, vol. 2, no. 2,
pp. 101–109, 2001.
[31] A. Colombo and D. Del Vecchio, “Efficient algorithms for collision
avoidance at intersections,” in Proceedings of the 15th ACM international conference on Hybrid Systems: Computation and Control. ACM,
2012, pp. 145–154.
[32] M. R. Hafner, D. Cunningham, L. Caminiti, and D. Del Vecchio,
“Cooperative Collision Avoidance at Intersections:Algorithms and Experiments,” IEEE Transactions on Intelligent Transportation Systems (to
appear), Mar. 2013.
[33] A. Alonso-Ayuso, L. F. Escudero, and F. J. Martin-Campo, “Collision
Avoidance in Air Traffic Management: A Mixed-Integer Linear Optimization Approach,” IEEE Transactions on Intelligent Transportation
Systems (to appear), vol. 12, no. 1, pp. 47–57, Feb. 2011.
[34] R. A. Paielli and H. Erzberger, “Conflict Probability for Free Flight,”
Journal of Guidance, Control, and Dynamics, vol. 20, no. 3, pp. 588–
596, May 1997.
[35] L. Pallottino, E. Feron, and A. Bicchi, “Conflict Resolution Problems
for Air Traffic Management Systems Solved With Mixed Integer Programming,” IEEE Transactions on Intelligent Transportation Systems (to
appear), vol. 3, no. 1, pp. 3–11, Mar. 2002.
[36] E. Lalish, K. A. Morgansen, and T. Tsukamaki, “Decentralized Reactive
Collision Avoidance for Multiple Unicycle-Type Vehicles,” in American
Control Conference, 2008.
[37] M. Ishutkina, E. Feron, and K. Bilimoria, “Describing Air Traffic
Complexity Using Mathematical Programming,” in AIAA 5th ATIO
and16th Lighter-Than-Air Sys Tech. and Balloon Systems Conferences.
Reston, Virigina: American Institute of Aeronautics and Astronautics,
Jun. 2012, pp. 1–9.
[38] K. Lee, E. Feron, and A. Pritchett, “Describing Airspace Complexity:
Airspace Response to Disturbances,” Journal of Guidance, Control, and
Dynamics, vol. 32, no. 1, pp. 210–222, Jan. 2009.
[39] E. Frazzoli, Z. H. Mao, J. H. Oh, and E. Feron, “Resolution of Conflicts
Involving Many Aircraft via Semidefinite Programming,” Journal of
Guidance, Control, and Dynamics, vol. 24, no. 1, pp. 79–86, Jan. 2001.

[40] Z.-H. Mao, D. Dugail, and E. Feron, “Space Partition for Conflict
Resolution of Intersecting Flows of Mobile Agents,” IEEE Transactions
on Intelligent Transportation Systems (to appear), vol. 8, no. 3, pp.
512–527, Aug. 2007.
[41] Z. H. Mao, E. Feron, and K. Bilimoria, “Stability and Performance
of Intersecting Aircraft Flows Under Decentralized Conflict Avoidance
Rules,” IEEE Transactions on Intelligent Transportation Systems (to
appear), vol. 2, no. 2, pp. 101–109, Jun. 2001.
[42] Z.-H. Mao and E. Feron, “Stability and performance of intersecting
aircraft flows under sequential conflict resolution,” in Proceedings of
American Control Conference. IEEE, 2001, pp. 722–729.
[43] Z. H. Mao, D. Dugail, E. Feron, and K. Bilimoria, “Stability of Intersecting Aircraft Flows Using Heading-Change Maneuvers for Conflict
Avoidance,” IEEE Transactions on Intelligent Transportation Systems
(to appear), vol. 6, no. 4, pp. 357–369, Dec. 2005.
[44] D. Gross and C. Harris, Fundamentals of Queueing Theory. Wiley,
1998.
[45] O. J. Boxma, O. Kella, and K. Kosiński, “Queue lengths and workloads
in polling systems,” Operations Research Letters, vol. 39, no. 6, pp.
401–405, 2011.
[46] V. M. Vishnevskii and O. V. Semenova, “Mathematical methods to study
the polling systems,” Automation and Remote Control, vol. 67, no. 2,
pp. 173–220, Feb. 2006.
[47] H. Takagi, “Queueing Analysis of Polling Models,” ACM Computing
Surveys, vol. 20, no. 1, pp. 5–28, Mar. 1998.
[48] M. A. Boon, R. Van der Mei, and E. M. Winands, “Applications of
polling systems,” Surveys in Operations Research and Management
Science, vol. 16, no. 2, pp. 67–82, 2011.
[49] H. Levy and M. Sidi, “Polling systems: applications, modeling, and
optimization,” IEEE Transactions on Communications, vol. 38, no. 10,
pp. 1750–1760, 1990.
[50] Z. Liu, P. Nain, and D. Towsley, “On optimal polling policies,” Queueing
Systems, vol. 11, no. 1-2, pp. 59–83, 1992.
[51] D. Miculescu and S. Karaman, “Polling-systems-based control of highperformance provably-safe autonomous intersections,” in IEEE Conference on Decision and Control, 2014.
[52] R. C. Larson and A. R. Odoni, Urban operations research. PrenticeHall, 1981, no. Monograph.
[53] E. Altman and D. Kofman, “Bounds for performance measures of token
rings,” IEEE/ACM Transactions on Networking (TON), vol. 4, no. 2, pp.
292–299, 1996.
[54] E. Altman, S. Foss, E. Riehl, and S. Stidham Jr, “Performance bounds
and pathwise stability for generalized vacation and polling systems,”
Technical Report UNC/OR TR93-8, Department of Operations Research,
University of North Carolina at Chapel Hill, Tech. Rep., 1993.
[55] R. L. Cruz, “A calculus for network delay. i. network elements in
isolation,” Information Theory, IEEE Transactions on, vol. 37, no. 1,
pp. 114–131, 1991.
[56] I. Rubin and L. F. De Moraes, “Message delay analysis for polling
and token multiple-access schemes for local communication networks,”
Selected Areas in Communications, IEEE Journal on, vol. 1, no. 5, pp.
935–947, 1983.
[57] I. Duenyas and M. P. Van Oyen, “Heuristic scheduling of parallel
heterogeneous queues with set-ups,” Management Science, vol. 42, no. 6,
pp. 814–829, 1996.
[58] H. Levy, M. Sidi, and O. J. Boxma, “Dominance relations in polling
systems,” Queueing systems, vol. 6, no. 1, pp. 155–171, 1990.
[59] O. J. Boxma, H. Levy, and J. A. Weststrate, “Efficient visit orders for
polling systems,” Performance Evaluation, vol. 18, no. 2, pp. 103–123,
1993.
[60] M. Hofri and K. W. Ross, “On the optimal control of two queues with
server setup times and its analysis,” SIAM journal on computing, vol. 16,
no. 2, pp. 399–420, 1987.
[61] D. Stoyan, W. S. Kendall, and J. Mecke, Stochastic Geometry and Its
Applications, 2nd ed. Wiley, 1995.
[62] J. Teichmann, F. Ballani, and K. G. van den Boogaart, “Generalizations
of Matern’s hard-core point processes,” Spatial Statistics, vol. 3, pp.
33–53, Sep. 2012.
[63] F. Baccelli and P. Bermolen, “Extremal versus additive Matérn point
processes,” Queueing Systems, vol. 71, no. 1-2, pp. 179–197, Mar. 2012.
[64] D. Stoyan and H. Stoyan, “On One of Matern’s Hard-core Point Process
Models,” Math. Nachr., pp. 205–214, Jan. 1985.
[65] I. Gurobi Optimization, “Gurobi optimizer reference manual,” 2015.
[Online]. Available: http://www.gurobi.com
[66] A. A. Agrachev, “Introduction to optimal control theory,” Summer
School on Mathematical Control Theory, Trieste, 2001.

Lemma IV.4 Suppose Assumptions IV.1 and IV.3 hold. Then,
each time a new vehicle arrives and Algorithm 1 is called,
every call to the MotionSynthesize procedure (Line 6 of
Algorithm 1) yields a feasible optimization problem.
First, we roughly outline the result that we are after. Suppose
a new vehicle, which we denote vehicle A, enters the control
region at time t00 . Without loss of generality, let us assume
vehicle A enters lane 2. Since Algorithm 1 is called (i.e., the
vehicle is not removed before entering the control region),
there exists a safe trajectory for vehicle A. Every call to
the MotionSynthesize procedure before time t00 yielded
a feasible optimization problem in the same way. Now to
prove the lemma, we show that at time t00 each call to the
MotionSynthesize procedure yields a feasible solution.
To make this precise, we introduce the following notation. We denote t0 as the latest time that Algorithm 1 was
called before the arrival of vehicle A, i.e., t0 = max{t <
t00 : Algorithm 1 is called at time t}. Recall that the ith vehicle in lane k is denoted as vehicle (i, k). We denote τi,k as
the schedule time of vehicle (i, k) assigned at time t0 . Also,
we denote xi,k as the trajectory of vehicle (i, k) assigned
by the MotionSynthesize procedure at time t0 . Similarly,
0
we denote τi,k
and x0i,k as the schedule time and trajectory
assigned at time t00 . Lastly, we denote τA as the schedule time
of newly arrived vehicle A assigned at time t00 . See Table I
for a summary of this notation.
call to Algorithm 1

t0

t00

schedule time

τi,k

0
τi,k

schedule time of vehicle A

-

τA

assigned trajectory

xi,k

x0i,k

scheduled to arrive
at intersection region

τi,k + L/vm

0 + L/v
τi,k
m

TABLE I

N OTATION PERTAINING TO VEHICLES IN THE SYSTEM AT TIMES t0 AND t00 .

We use the boldface notation as follows. We denote a state
z := (p, v). We denote a state history z := (x, ẋ). Next, let
C (z0 , t̃0 , t̃f , x0 ) denote the set of trajectories with initial state
z0 := (p0 , v0 ) at time t̃0 that reach the intersection region with
full velocity at time t̃f and are safe with x0 . See Figure 11.
More precisely, we define the set of feasible trajectories as

C (z0 , t̃0 , t̃f ,x0 ) := x : [t̃0 , t̃f ] → R ∃u ∈ U,
ẍ(t) = u(t), |u(t)| ≤ am , ∀t ∈ [t̃0 , t̃f ];
ẋ(t) ∈ [0, vm ], ∀t ∈ [t̃0 , t̃f ];

z(t̃0 ) = z0 ; z(t̃f ) = (0, vm );
|x(t)| ≥ l + |x0 (t)|, ∀t ∈ [t̃0 , t̃f ] ∩ D(x0 ) ,

t0

position

A PPENDIX
We devote this appendix to the proof of Lemma IV.4. In
what follows, we present a number of intermediate results
leading to the proof of Lemma IV.4. Then, we provide proofs
of these intermediate results separately. We restate the lemma
here for convenience.

time

tf

x0

−L
Fig. 11. Sample trajectories from set C (z0 , t0 , tf , x0 ) are shown in solid
black. Trajectory x0 is shown as the dashed red line. Note that all sample
trajectories have the same initial and terminal states, same terminal time, and
are safe with trajectory x0 .

where u ∈ U is a measurable function, u : [t̃0 , t̃f ] → R, and
0
D(x0 ) is the domain of trajectory x0 . Also, we write Ci,k
for
0
0
0
0
C (zi,k (t0 ), t0 , τi,k + L/vm , xi−1,k ). The MotionSynthesize
procedure at time t00 for vehicle (i, k) is simply
0
Z τi,k
+L/vm
x0i,k ← min0
|x(t)|dt.
(3)
x∈Ci,k

t00

Hence, Lemma IV.4 basically states that the optimization
problem in Equation (3) has a feasible solution each time
the MotionSynthesize procedure is called. The following
lemma restates Lemma IV.4 using the new notation.
Lemma VI.1 Suppose Assumptions IV.1 and IV.3 hold. Suppose a new vehicle arrives at time t00 and Algorithm 1 is called.
Then, for every vehicle (i, k) in the system at time t00 , the
optimization problem in Equation (3) has a feasible solution.
To prove Lemma VI.1 (or equivalently Lemma IV.4), we
consider two cases separately: (i) vehicles that are scheduled
to arrive at the intersection before vehicle A, and (ii) vehicles
that are scheduled to arrive at the intersection after vehicle A.
We show that the optimization problem in Equation (3) yields
a feasible solution for all these vehicles.
In Section A of this appendix, we focus on the first case,
and we prove the following lemma.
Lemma VI.2 Suppose that at time t00 the schedule of vehicle
0
(i, k) is earlier than that of vehicle A, i.e., τi,k
< τA . Then,
0
Ci,k is non-empty and the minimum in Equation (3) is attained,
for vehicle (i, k). Moreover, the trajectory x0i,k is simply a
truncation of xi,k , i.e., x0i,k = xi,k |{t : t≥t00 } .
This lemma states that vehicles scheduled to arrive at the
intersection before vehicle A simply continue on their previous trajectories. That is, the call to the MotionSynthesize
procedure at time t00 (triggered by the arrival of vehicle A)
does not change the trajectories of this set of vehicles; for
these vehicles, the MotionSynthesize procedure returns the
same set of trajectories at time t00 it returned in the previous
call to the procedure at time t0 . In Section A of this appendix,
we prove this result by induction. We use Bellman’s optimality
principle to show that the previous trajectory is still optimal.

1

Next, we verify Equation (3) for vehicles scheduled later
than vehicle A. We prove the following lemma.

Ii

vm

∂F

To formalize our argument for this lemma, we introduce the
following notation. Firstly, since all regular polling policies
(See Assumption IV.1 for a definition) are first-come first-serve
(FCFS), all vehicles scheduled after vehicle A must be in lane
1, i.e., not in the lane vehicle A entered. Thus, for notational
simplicity, for the remainder of the proof, we safely drop the
0
k subscript, i.e., denote τi,k
by τi0 , and xi,k by xi , et cetera.
Secondly, for any vehicle i scheduled after vehicle A, we
0
define νi := i − min{ i | τi,1
> τA } + 1. Note that this vehicle
th
i is the νi vehicle scheduled after vehicle A. See Figure 12.
For each vehicle i scheduled after vehicle A, we define:
Fi := { (p0 , v0 ) ∈ [−L, 0] × [0, vm ] |

2
p0 + v02 /(2am ) ≤ −vm
/(2am ) − (νi − 1)l }.

See Figure 13. Furthermore, we denote the right-hand side
boundary of the set Fi by ∂Fi , i.e.,
∂Fi := { (p0 , v0 ) ∈ [−L, 0] × [0, vm ] |

2
p0 + v02 /(2am ) = −vm
/(2am ) − (νi − 1)l }.

Let us provide some insight into sets ∂Fi and Fi . Notice
that following the boundary ∂Fi , vehicle i can decelerate as
rapidly as possible (with deceleration am ) and come to a
2
full stop exactly vm
/(2am ) + (νi − 1)l further away from
the intersection region. Suppose νi = 1, i.e., vehicle i is
the first vehicle to go through the intersection after vehicle
A. Then, following ∂Fi , vehicle i can come to a full stop
2
/(2am ) away from the intersection region. Notice
exactly vm
2
that vm /(2am ) is exactly the distance it takes vehicle i
to accelerate with maximum acceleration (am ) and reach
maximum speed (vm ) right when it arrives at the intersection
region. When νi > 1, vehicle i can follow ∂Fi in the same
manner and stop at a point that has an additional distance
of νi − 1 vehicle lengths (i.e., (νi − 1)l) to the intersection
region. Hence, k vehicles with νi ∈ {1, 2, . . . , k}, by following
their respective ∂Fi , come to a full stop, one behind another,
2
such that the foremost vehicle has distance vm
/(2am ) from
the intersection region. These vehicles can move together in
this formation with maximum acceleration (am ) to reach the
intersection at maximum speed (vm ).
The set Fi is critical in proving Lemma VI.3, and it provides
us with insights into how the algorithm guarantees safety.

i = 16

i = 15

i = 14

νi = 3

νi = 2

νi = 1

i = 13

i = 12

Fig. 12. Lane 1 is shown in full. Vehicles scheduled before vehicle A are
shaded grey. Vehicles scheduled after vehicle A have white interiors. For each
vehicle scheduled after vehicle A, its value of νi is displayed directly below.

(νi − 1)l

Fi
2
vm
/(2am )

−L

velocity

i

Lemma VI.3 Suppose that at time t00 the schedule of vehicle
0
0
(i, k) is later than that of vehicle A, i.e., τi,k
> τA . Then, Ci,k
is non-empty and the minimum is attained in Equation (3).

2
vm
/(2am )

position

Fig. 13. Sets Fi and ∂Fi are shown. Set Fi is shaded in grey. Set ∂Fi ,
the right-hand side boundary of Fi , is traced in red. State Ii has coordinates
(−(νi − l)l, vm ). Set Fi can be described as the set of all states from which
state Ii can be reached at a time indefinitely far into the future.

We prove Lemma VI.3 in three steps. First, we show that
the state zi (t00 ) of the νith vehicle after vehicle A at time t00 is
inside the set Fi . See Figure 13. Second, we show that each
such vehicle has a feasible trajectory satisfying a particular
property: the νith vehicle continues as long as possible along its
last assigned trajectory xi . See Figure 14. Finally, we show that
the MotionSynthesize procedure actually returns such a trajectory. This last step is more technical, requiring results from
optimal control theory showing that the optimization problem
in Equation (3) is well-posed. These steps are formalized in
Lemmas VI.4, VI.5, and VI.6, respectively. We state these
lemmas below, and provide insights into their proofs.
First, we show that at time t00 the state of the νith vehicle
after vehicle A is in Fi . These vehicles are able to come to a
full stop far enough away from the intersection region, such
that they can attain full speed before crossing it.
Lemma VI.4 For any vehicle i scheduled after vehicle A at
time t00 , i.e., τi0 > τA , we have zi (t00 ) ∈ Fi .
Let us provide some intuition into the proof of this Lemma.
First, we define state Ii := ( −(νi −1)l, vm ). See Figure 13. By
the construction of the MotionSynthesize procedure, the νith
vehicle after vehicle A arrives at state Ii and then continues at
maximum speed to the intersection region. (One can observe
this phenomena in the heavy load case of Figure 5. The reason
is that the MotionSynthesize procedure, while constraining
the terminal state and time, also requires safety.) Due to the
regular polling policy condition (Assumption IV.1) and the
minimum road length condition (Assumption IV.3), it turns
out that the time between the arrival of vehicle A and the
arrival of vehicle i at state Ii (according to previous trajectory
xi ) is greater than or equal to 2vm /am , i.e.,
τi + L/vm − (νi − 1)s − t00 ≥ 2vm /am .
We then conclude by showing that the state zi of the νith vehicle at time t00 must be in Fi , i.e., zi (t00 ) ∈ Fi , via a straightforward application of Pontryagin’s Minimum Principle. Note
that if zi (t00 ) 6∈ Fi , then there can be only two outcomes:
either vehicle i is not able to reach state Ii , or vehicle i can
reach state Ii but only in an amount of time strictly less than

1

vm

∂F

velocity

i

xi
t00
−L

tc [xi ]
position

Fig. 14. Trajectory xi is depicted in position-velocity space as the dashed
red line; it begins at time t00 , exits set Fi at time tc [xi ], and continues to the
state (0, vm ). Sample trajectories from set Ei0 follow trajectory also begin at
time t00 , replicate trajectory xi until time tc [xi ], and then choose any (feasible
and safe) path to state (0, vm ). Sample trajectories of Ei0 are shown in black.

2vm /am , clearly violating the above inequality. Detailed proof
is given in Section B of this appendix.
Second, we show that not only does there exist a feasible
trajectory for the νith vehicle after vehicle A, i.e., Ci0 6= ∅, but
we also show that vehicle i continues along its last assigned
trajectory xi as long as possible. See Figure 14. We will write
this result rigorously as Ei0 6= ∅, and we now proceed to
make this notion precise. Let E (y, t̃0 , t̃f , x0 ) be the set of all
extensions of trajectory y starting at time t̃0 that reach the
intersection region at time t̃f and are safe with trajectory x0 .
More precisely, we define



E (y, t̃0 , t̃f , x0 ) := x ∈ C y(t̃0 ), ẏ(t̃0 ) , t̃0 , t̃f , x0
x(t) = y(t), ∀t ∈ [t̃0 , t̃f ] ∩ D(y) .

We denote tc [xi ] as the time that trajectory xi leaves the set
∂Fi , i.e., tc [xi ] := sup{t | zi (t) ∈ ∂Fi }. For shorthand, we
write Ei0 for E (xi |[t0 ,tc [xi ]] , t00 , τi0 + L/vm , x0i−1 ). Note that
Ei0 ⊆ Ci0 . We now formalize our second intermediate result.
Lemma VI.5 Consider vehicle i scheduled after vehicle A at
0
time t00 , i.e., τi0 > τA . Suppose zi (t00 ) ∈ Fi and x0i−1 ∈ Ei−1
.
0
Then, Ei 6= ∅.
First, note that Lemma VI.5 states that there exists a feasible
trajectory for the νith vehicle, i.e., Ci 6= ∅. Moreover, there
exists a particular trajectory for the νith vehicle that follows
its last assigned trajectory xi at least until it leaves set Fi , i.e.,
at time tc [xi ], as depicted in Figure 14.
The proof of Lemma VI.5 is simply the construction of
a trajectory in the set Ei0 , and it is given in Section C of
this appendix. This construction is quite elaborate in the
general case. However, we first present a simple case that
demonstrates the basic building blocks of the construction. In
this simple case, we concern ourselves with the case νi = 1.
We construct a feasible trajectory in Ei0 for this vehicle as
follows: vehicle i follows its last assigned trajectory xi until
it reaches the boundary ∂Fi , decelerates along ∂Fi , and then
finally accelerates to full speed before crossing the intersection
region. The transition point from deceleration to acceleration
is dependent solely on the terminal time τi0 + L/vm at which
vehicle i arrive at the intersection region. We then argue

1

that this trajectory for vehicle i must be safe with the newly
updated trajectory x0i−1 of the vehicle directly in front. One
can see this from noting that the trajectory constructed above
for vehicle i is clearly safe with the last assigned trajectory
xi−1 for vehicle i − 1, and then noting that xi−1 and x0i−1 are
essentially identical, by the earlier stated Lemma VI.2. The
general case (νi > 1) is more complicated in its construction
due to the fact that the updated trajectory x0i−1 of the vehicle
directly in front changes; thus, additional care must be taken
in constructing a feasible trajectory in Ei0 for vehicle i that is
safe with the updated trajectory x0i−1 .
Now, we formalize our final intermediate result.
Lemma VI.6 Suppose vehicle i is scheduled after vehicle
A at time t00 , and Ei0 6= ∅. Then, the MotionSynthesize
procedure described in Equation (3) returns a solution from
the set Ei0 ⊆ Ci0 .
This lemma states that if Ei0 is non-empty, then the
MotionSynthesize procedure admits a trajectory in Ei0 . This
rather straightforward lemma is quite technical in nature,
while adding no further insight. The details of the proof of
Lemma VI.6 is given in Section D of this appendix.
Now, let us give a brief overview. From Lemmas VI.2
and VI.3, the proof of Lemma IV.4 follows. Recall that
Lemma VI.2 verifies the feasibility of the optimization problem in Equation (3) for all vehicles with updated schedules
0
earlier than vehicle A, i.e., τi,k
< τA . Similarly, Lemma VI.3
shows the feasibility of Equation (3) for all vehicles with
0
> τA . We
updated schedules later than vehicle A, i.e., τi,k
prove this lemma by induction, using our three intermediate
results: Lemmas VI.4, VI.5, and VI.6.
Consider the first vehicle with updated schedule after the
arrival of vehicle A, i.e., νi = 1. By Lemmas VI.4 and VI.5, Ei0
is non-empty. Next, by Lemma VI.6, trajectory x0i , as defined
by Equation (3), exists and is contained in Ei0 . Now, consider
the case νi > 1, and assume (by induction) that trajectory
x0i−1 , as defined by Equation (3), exists and is contained in
0
Ei−1
. Then, by Lemmas VI.4 and VI.5, the set Ei0 is nonempty. By Lemma VI.6, the MotionSynthesize procedure
admits a solution x0i contained in Ei0 . Thus, we have shown,
for any vehicle i scheduled after vehicle A, i.e., τi0 > τA , the
MotionSynthesize procedure always admits a trajectory in
Ei0 . Lemma VI.3 follows directly from noting Ci0 ⊆ Ei0 .
To be completely rigorous, one has to verify that the
optimization problem in Equation (3) has a feasible solution
for newly arrived vehicle A. However, this is not difficult,
since vehicle A in lane 2 follows a vehicle whose updated
trajectory is simply its last assigned trajectory. Therefore, by
using almost identical arguments as for vehicle i in lane 1
satisfying νi = 1, one can show that vehicle A also has a
feasible trajectory with terminal time τA + L/vm .

xi,k = MS(zi,k (t0 ), t0 , tf , xi−1,k ).
Also note that by definition, we have
x0i,k := MS(zi,k (t00 ), t00 , tf , ∅).
See Figure 15. By Bellman’s Principle of Optimality,
xi,k |[t00 ,tf ] = MS(zi,k (t00 ), t00 , tf , xi−1,k )

= MS(zi,k (t00 ), t00 , tf , xi−1,k |[t00 ,tf ] )
= MS(zi,k (t00 ), t00 , tf , ∅).

Therefore, we have x0i,k = xi,k |[t00 ,tf ] . Now, we consider any
0
vehicle (i, k) that is scheduled before vehicle A, i.e., τi,k
<
τA . By induction, we assume vehicle (i−1, k) continues on its
previous trajectory, i.e., x0i−1,k = xi−1,k |{t : t≥t00 } . We claim
that vehicle (i, k) also continues on its previous trajectory, i.e.,
x0i,k = xi,k |[t00 ,tf ] . Recall
x0i,k := MS(zi,k (t00 ), t00 , tf , x0i−1,k ).
Note that
xi,k |[t00 ,tf ] = MS(zi,k (t00 ), t00 , tf , xi−1,k )

= MS(zi,k (t00 ), t00 , tf , xi−1,k |[t00 ,tf ] )

= MS(zi,k (t00 ), t00 , tf , x0i−1,k ).

Hence, x0i,k = xi,k |[t00 ,tf ] . The proof of this lemma is complete.

position

Recall that τi,k is the schedule time for vehicle i in lane
k; hence, the same vehicle was scheduled to arrive at the
intersection region at time τi,k + L/vm , when the algorithm
was run at time t0 . Recall that the schedule time for the same
0
vehicle is τi,k
, when the algorithm is run again at time t00 .
Consider vehicles scheduled before vehicle A, according to
the updated schedule times. First, note that by the regular
polling policy condition (See Assumption IV.1), any vehicle
(i, k) scheduled before the newly arrived vehicle A does not
0
0
change its schedule time, i.e., τi,k
< τA implies τi,k
= τi,k .
For shorthand, we denote τi,k + L/vm by tf . Note that for
0
all such vehicles, we have τi,k + L/vm = τi,k
+ L/vm = tf .
Now, we prove (by induction) that x0i,k = xi,k |[t00 ,tf ] .
Out of all the vehicles scheduled before vehicle A,
let us consider the first vehicle in lane k at time t00 ,
i.e., i = min Ik (t00 ). We denote MS as short-hand for
MotionSynthesize in this section. Note that trajectory xi,k
satisfies

t00

t0

A. Proof of Lemma VI.2

time

tf

xi−1,k
x0i,k
xi,k

−L
Fig. 15. Trajectories xi,k and xi−1,k that had been previously generated at
time t0 . Newly generated trajectory x0i,k is also shown, assuming that vehicle
(i, k) is the first vehicle in lane k at time t00 . Note that in this scenario, updated
trajectory x0i,k is simply a restriction of previous trajectory xi,k .

B. Proof of Lemma VI.4
In this section, we show that if at time t00 vehicle i is
scheduled to arrive at the intersection region after vehicle A,
then the state of vehicle i is currently contained in Fi , i.e.,
zi (t00 ) ∈ Fi . In summary, we argue as follows. Recall that for
any vehicle i scheduled to arrive at the intersection region after
vehicle A, we defined νi such that vehicle i is the νi th vehicle
after vehicle A. Let us define state Ii as Ii := ( −(νi −1)l, vm ).
See Figure 13. By the construction of the MotionSynthesize
procedure, any such vehicle i arrives at state Ii and then
continues at maximum speed to the intersection region. Due
to the regular polling policy condition (Assumption IV.1) and
the minimum road length condition (Assumption IV.3), it turns
out that the time between the arrival of vehicle A and the
arrival of vehicle i at state Ii (according to previous trajectory
xi ) is greater than or equal to 2vm /am . We then conclude
by showing that zi (t00 ) ∈ Fi holds via an optimal control
argument.
Now, we make the previous arguments precise. First, we
show that vehicle i arrives at state Ii at time τi + L/vm −
(νi − 1)s:
Proposition VI.7 Suppose vehicle i is scheduled after vehicle
A at time t00 , i.e., τi0 < τA . Let xi be the previous trajectory of
vehicle i, let zi be the state history, i.e., zi = (xi , ẋi ), and let τi
be its previous schedule time. Then, zi (τi +L/vm −(νi −1)s) =
( −(νi − 1)l, vm ) := Ii .
Proof: See Figure 16. Essentially, this proposition states
that all vehicles scheduled after vehicle A move as a platoon
when the first vehicle (νi = 1) begins to cross the intersection
region. This phenomenon can be observed clearly when the
algorithm is run in the heavy load regime. See Figure 5. We
prove this by induction on νi . First, consider the case νi = 1.
Then, zi (τi + L/vm ) = (0, vm ) immediately holds from the
construction of the MotionSynthesize procedure. Next, we
consider the more general case νi > 1. We assume zi−1 (τi−1 +
L/vm − (νi−1 − 1)s) = ( −(νi−1 − 1)l, vm ). Denote
t̄ := τi−1 + L/vm − (νi−1 − 1)s = τi + L/vm − (νi − 1)s.

1

time

t̄

kB = kC . Then, the previous schedule time of vehicle C can
be expressed as

position

νi = 1
νi = 2
νi = 3
νi = 4
νi = 5

τC = τB + s + rθBC ,

−4l

(7)

where r we defined as the switch time between queues.
Combining Equations (6) and (7), we obtain

4s

τi = τB + s + rθBC + (νi − 1)s.

−L

(8)

Next, we establish τB + s ≥ t00 in the following proposition.

Fig. 16. Sample trajectories for the first five vehicles scheduled after newly
arrived vehicle A. Time t̄ := τi + L/vm − (νi − 1)s is exactly the terminal
time of the first vehicle scheduled after vehicle A, i.e., νi = 1. Note how these
vehicles move as a platoon, as the first vehicle begins to cross the intersection
region.

Proposition VI.8 Recall that t00 is the arrival time of newly
arrived vehicle A in lane 2. Let (4) and (5) the schedule orders
at times t00 and t0 , respectively, be generated according to a
regular polling policy (Assumption IV.1). Then, τB + s ≥ t00
holds.

By safety of previous trajectories xi and xi−1 , we have

Proof: Suppose vehicles B and A are in the same lane.
Then, τA = τB + s. Since τA ≥ t00 always holds, we
have established this proposition. Now suppose, vehicles B
and A are in opposite lanes. Then, applying the previous
argument only assures us that vehicle A arrives before the
server finishes servicing vehicle B and and switching lanes,
i.e., t00 ≥ τA = τB + s + r. Thus, we argue differently. First,
note that vehicles B and C must be in the same lane. Suppose
that vehicle A arrives strictly later than the service completion
of vehicle B, i.e., t00 > τB + s. Then, we claim that vehicle
C is serviced earlier than vehicle A, contradicting the updated
schedule order (4). Consider what happens at the service
completion of vehicle B, i.e., at time τB + s. Since vehicle
A has not arrived yet, queue 2 is empty. Thus, according
to the regular polling policy condition (Assumption IV.1),
vehicle C will be serviced immediately after vehicle B, i.e.,
τC = τB + s. A nontrivial amount of time τB + s − t00 > 0 has
elapsed. Note that any polling policy (not just regular polling
policies) cannot interrupt the service of a customer in order
to service another customer. Thus, vehicle A will be serviced
after vehicle C, contradicting the updated schedule order (4).
Therefore, τB + s ≥ t00 .
From Equation (8) and Proposition VI.8, we establish

xi (t̄) ≤ xi−1 (t̄) − l = −(νi−1 − 1)l − l = −(νi − 1)l.
We claim equality also holds. To see this, suppose xi (t̄) <
−(νi − 1)l. Then, the time that vehicle i arrives at the
intersection region (according to trajectory xi ) is strictly later
than τi + L/vm , a contradiction. Thus, xi (t̄) = −(νi − 1)l.
Similarly, we show that ẋi (t̄) = vm . By construction of
the MotionSynthesize procedure, ẋi (t̄) ≤ vm . We claim
equality. To see this, suppose ẋi (t̄) < vm . Then, vehicle i
arrives at the intersection region (according to xi ) strictly later
than time τi + L/vm , a contradiction. Thus, ẋi (t̄) = vm .
Although not necessary for the development of the proof of
this lemma, it is an immediate corollary of Proposition VI.7
that vehicle i travels with maximum speed from state Ii to the
intersection region.
Next, we show that the time between the arrival of vehicle
A and the arrival of vehicle i at state Ii is at least 2vm /am ,
i.e., Inequality (11). Let

(il , kl ), . . . , (im−1 , km−1 ), (B, kB ),
(A, 2),

(C, kC ), (im , km ), . . . , (in , kn )
be the service order of vehicles computed at time

(i1 , k1 ), . . . , (im−1 , km−1 ), (B, kB ),

(C, kC ), (im , km ), . . . , (in , kn )

(4)

t00 ,

and let
(5)

be the service order of vehicles computed at time t0 . (See
Section III-B and also Assumption IV.1.) First, note that all
vehicles scheduled after vehicle A are in lane 1, i.e., kC =
km = · · · = kn = 1, since a regular polling policy must
be first-come first-serve (FCFS). From the previous schedule
order (5), we can express the previous schedule time τi of
vehicle i as
τi = τC + (νi − 1)s.

(6)

Define θBC to be equal to 1 if vehicles B and C are in opposite
lanes, i.e., kB 6= kC , and 0, if they are in the same lanes, i.e.,

1

τi ≥ t00 + (νi − 1)s.

(9)

Hence, the time that vehicle i reaches the intersection region
(according to previous trajectory xi ) is at time
τi + L/vm ≥ t00 + (νi − 1)s + L/vm

≥ t00 + (νi − 1)s + 2vm /am ,

(10)

applying Inequality (9), first, and then the minimum control
region length condition (Assumption IV.3). Rearranging Inequality (10), we obtain
τi + L/vm − (νi − 1)s − t00 ≥ 2vm /am .

(11)

Recall that Proposition VI.7 states that vehicle i arrives at state
Ii (according to trajectory xi ) at time τi + L/vm − (νi − 1)s.
Thus, Inequality (11) simply says that the time from the arrival
of vehicle A until the arrival time of vehicle i at state Ii is
greater than or equal to 2vm /am .

Ii

vm

∂F

(νi − 1)l

Ω(Ii )

velocity

i

unboundedly. Due to the difficulty of handling state constraints
directly, we relax this problem to the following optimal control
problem, for which a solution is readily available:
Z t̃f
max
dt
(13a)
x:[0,t̃f ]→R

x(0) = 0;

position

We conclude the proof of this lemma by showing that the
state of vehicle i at time t00 must be in Fi , i.e., zi (t00 ) ∈ Fi .
Consider the set of all states that can reach state Ii , i.e.,
Ω(Ii ) := {z ∈ [−L, 0] × [0, vm ] : ∃ tf s.t. C (z, t00 , tf , ∅) 6= ∅}.
See Figure 17. First, note that from any state z ∈ Fi , we can
accelerate to maximum speed and then continue at maximum
speed until arriving at state Ii ; hence, Fi ⊆ Ω(Ii ). Now,
suppose the state of vehicle i is not in Fi at time t00 , i.e.,
zi (t00 ) ∈ Ω(Ii ) \ Fi . Note that by construction, any state
2
)/(2am ).
(p0 , v0 ) ∈ Ω(Ii ) \ Fi satisfies |p0 | < (v02 + vm
Essentially, this means that a feasible trajectory initialized in
Ω(Ii ) \ Fi at time t00 cannot possibly come to a full stop
after time t00 , since this trajectory cannot satisfy the terminal
constraints. We make this precise in the following proposition.

Proposition VI.9 Let a vehicle be initialized with velocity v0 ,
i.e., ẋ(0) = v0 . Let d be the distance it must travel, i.e.,
x(0) = 0 and x(t̃f ) = d. Suppose the following constraints
2
)/(2am ) and ẋ(t̃f ) = vm . Then, the
are satisfied: d < (v02 +vm
time max t̃f of the longest duration feasible trajectory satisfies
max t̃f < 2vm /am .
Proof: We solve the following free terminal time optimal
control problem:
Z t̃f
max
dt
(12a)
0

subject to ẍ(t) = u(t), for all t ∈ [0, t̃f ];

(12b)

|u(t)| ≤ am , for all t ∈ [0, t̃f ];

(12d)

x(t̃f ) = d;

(12f)

0 ≤ ẋ(t) ≤ vm , for all t ∈ [0, t̃f ];

x(0) = 0;

ẋ(0) = v0 ;

ẋ(t̃f ) = vm .

(12c)
(12e)

Denote vl as the lowest velocity that the vehicle achieves
during its trajectory, i.e., vl = inf{ẋ(t) : ∀t ∈ [0, t̃f ], ∀x ∈
C ((0, vm ), 0, t̃f , ∅)}. Suppose vl ≤ 0. Note that the distance
required to decelerate from the current velocity v0 to a full stop
and then accelerate from a full stop to full speed is precisely
2
2
(v02 + vm
)/(2am ). But since d < (v02 + vm
)/(2am ), we have
a contradiction. Thus, vl > 0, and, hence, (12) does not grow

(13c)

ẋ(0) = v0 ;

x(t̃f ) = ξ0 ;

Fig. 17. State Ii := (−(νi − 1)l, vm ) is shown. Set Ω(Ii ), shaded in grey,
depicts the set of all states from which state Ii can be reached. Note that Fi
lies entirely within Ω(Ii ).

x:[0,t̃f ]→R

(13b)

|u(t)| ≤ am , for all t ∈ [0, t̃f ];

2
vm
/(2am )

−L

0

subject to ẍ(t) = u(t), for all t ∈ [0, t̃f ];

(13d)

ẋ(t̃f ) = vm .

(13e)

By Pontryagin’s Minimum Principle, the control law for (13) is
simply bang-bang control. Furthermore, the optimal trajectory
x̃ which maximizes the terminal time t̃f can be divided into
two distinct phases. In the first phase, the vehicle decelerates to
the lowest speed vl with maximum deceleration, while in the
second phase, vehicle accelerates to full speed, with maximum
acceleration. This trajectory is summarized in Table II. Note
∆t

∆x

1

v0 −vl
am

2

vm −vl
am

2
v0
−vl2
2am
2
−vl2
vm
2am

vm +v0 −2vl
am

2
2
−2vl2
+v0
vm
2am

Phase

Total

∆v

ẍ

vl − v0

−am

vm − vl

am

vm − v0

TABLE II
L ATEST S CHEDULE T IME T RAJECTORY

that the velocity of the vehicle throughout the trajectory
satisfies the velocity constraint (12c); therefore, we have also
solved (12). Finally, note that
max t̃f =

vm + v0 − 2vl
v m + v0
vm
<
≤2
,
am
am
am

where the first inequality holds from vl > 0 and the second
inequality from v0 ≤ vm .
Supposing zi (t00 ) ∈ Ω(Ii ) \ Fi and applying Proposition VI.9, the time between the arrival of vehicle A and the
arrival of vehicle i at state Ii is strictly less than 2vm /am , contradicting Inequality (11). Hence, zi (t00 ) ∈ Ω(Ii ) ∩ Fi = Fi .
This completes the proof of Lemma VI.4.

C. Proof of Lemma VI.5
In this section, we show that the set Ei0 is non-empty. Recall
that a trajectory x̃i ∈ Ei0 has the following properties:

x̃i (t) = xi (t), for all t ∈ [t00 , tc [xi ]];

(16)

x̃i (t) ≤ x0i−1 (t) − l, for all t ≥ t00 .

(15)
z0

(17)

Any feasible trajectory x can be uniquely described by the
triple (V [x], δ[x], t0 ), where V [x] maps the position of trajectory x to the velocity at that position, δ[x] maps the position
of trajectory x to the amount of time spent at that position,
and t0 is the initial time of the trajectory. Note that δ[x](p) is
0 for any position p with nonzero velocity. Because a vehicle
might fully stop, the mapping of its trajectory from the position
space to the time domain is not a function in general. Thus, we
will make frequent use of the arrival time of a trajectory x at
position p, i.e., tA [x](p), and the departure time of a trajectory
x from position p, i.e., tD [x](p). Formerly, we define:

−L

V [x̄]

V [x̃]

p0

position

Fig. 18. Notation for Proposition VI.10. “High-performance” velocity path
V [x̂] and “safe” velocity path V [x̄] are depicted in solid black. Bounded
velocity path V [x̃] is depicted in dashed black. Note that all trajectories have
the same initial state z0 . Note that terminal time of x̃ is also bounded by the
terminal times of the “high-performance” and “safe” trajectories.

∂F

V [xi ]

i

V [x̄i ]

First, consider the case νi = 1. We introduce the following
proposition. Note Figure 18.
Proposition VI.10 Let z0 := (p0 , v0 ). Suppose there exist a
“high-performance” trajectory x̂ ∈ C (z0 , 0, t̂, ∅) and a “safe”
trajectory x̄ ∈ C (z0 , 0, t̄, ∅), satisfying
t̄ ≥ t̂.

(18)

Suppose the following inequalities hold for all p ∈ [p0 , 0]:
V [x̄](p) ≤ V [x̂](p)

δ[x̄](p) ≥ δ[x̂](p).

δ[x̄](p) ≥ δ[x̃](p) ≥ δ[x̂](p).

−L

t00
position

ts ts + σ
2
−vm /(2am )

Fig. 19. Using terminology from Proposition VI.10, “safe” velocity path
V [x̄i ] as generated by control ūi defined in Equation (23) is depicted in
solid black. Recall that time tc [xi ] is defined as the time that trajectory xi
exits set Fi . “High-performance” velocity path V [xi ] replicates V [x̄i ] from
t00 to tc [xi ] and then afterwards, follows the dashed black line. Note that
during “safe” trajectory x̄i , the vehicle halts for an amount of time σ at a
2 /(2a ) from the intersection region.
distance vm
m

(19)
(20)

Then, for any terminal time t̃ satisfying t̄ ≥ t̃ ≥ t̂, there exists
a trajectory x̃ ∈ C (z0 , 0, t̃, ∅) satisfying the following, for all
p ∈ [p0 , 0]:
V [x̄](p) ≤ V [x̃](p) ≤ V [x̂](p)

zi (t00 )

vm
vic

tc [xi ]

tA [x](p) := inf{t : x(t) = p}
tD [x](p) := sup{t : x(t) = p}.

velocity

(14)

V [x̂]

velocity

x̃i (τi0 + L/vm ) = 0;
x̃˙ i (τi0 + L/vm ) = vm ;

vm

(21)
(22)

We define x := T (z0 , u) as the unique solution of the linear
ODE ẍ = u with initial state z0 . Let ui be a control action that
generates trajectory xi , i.e., ui := ẍi . Denote tf and t0f as the
times that vehicle i reaches the intersection region according
to trajectories xi and x0i , respectively, i.e., tf := τi + L/vm
and t0f := τi0 +L/vm . Let vic := ẋi (tc [xi ]). Consider trajectory
x̄i := T (zi (t00 ), ūi ) (depicted in Figure 19), where ūi :

ui (t) : t ∈ [t00 , tc [xi ])



−a
: t ∈ [tc [xi ], ts )
m
ūi (t) =
(23)

0
:
t
∈
[t
,
t
+
σ)
s
s



am
: t ∈ [ts + σ, ts + σ + vm /am ],
where
ts := tc [xi ] + vic /am ,

(24)

σ := max{0, t0f − ts − vm /am }.

(25)

Borrowing the language from Proposition VI.10, the “highperformance” trajectory is xi , and the “safe” trajectory is
x̄i . We show the existence of a feasible trajectory with
terminal time t0f . First, note by construction of σ, the time
that x̄i reaches the intersection region is no earlier than
t0f . Hence, ts + σ + vm /am ≥ t0f ≥ tf . Second, note
2
2
δ[xi ](−vm
/(2am )) ≤ σ = δ[x̄i ](−vm
/(2am )); otherwise,
xi would arrive at the intersection region strictly later than
t0f , i.e., tf > t0f , which is a contradiction. Hence, Inequality (20) holds. Lastly, directly from the construction of ūi ,
Inequality (19) holds. Thus, by Proposition VI.10, there exists
trajectory x̃i ∈ C (zi (t00 ), t00 , t0f , ∅) satisfying Inequalities (21)
and (22).
Now, we state a proposition, depicted in Figure 20:
Proposition VI.11 Suppose there exist a “fast” trajectory
x̂ ∈ C (ẑ0 , t0 , t̂, ∅) and a “slow” trajectory x̄ ∈ C (z̄0 , t0 , t̄, ∅),
such that for all p ∈ (p0 , pf ):
V [x̄](p) ≤ V [x̂](p)

(26)

tD [x̄](p0 ) ≥ tD [x̂](p0 ).

(28)

δ[x̄](p) ≥ δ[x̂](p)

(27)

1

z̄0 t0

V [x̄i ]
t00

tD [x̄](p0 )
p0

−L

tD [x̄](pf )

pf

Then, for all t ∈ [tA [x̂](p0 ), tD [x̄](pf )], we have
x̄(t) ≤ x̂(t).

(29)

First, note that Inequalities (26) and (27) are satisfied for
trajectories x̄i , x̃i , and xi , i.e.,
V [x̄i ] ≤ V [x̃i ] ≤ V [xi ]
δ[x̄i ] ≥ δ[x̃i ] ≥ δ[xi ].

Fig. 21. Notation for Proposition VI.12. Previous trajectory xi is identified by
following the black arrows, “high-performance” trajectory x̂i by red arrows,
“safe” trajectory by green arrows, and updated trajectory x0i−1 of vehicle
in front by blue arrows. Both “high-performance” trajectory x̂i and safe
trajectory x̄ replicate xi until time t× . After time t× , “high-performance”
velocity path V [x̂i ] always has a higher velocity than the velocity path V [x̄i ]
of the “safe” trajectory. See Inequality (31). Time tci is the time that trajectory
xi leaves set Fi . See notation in (35). Trajectory x̄i is safe with updated
trajectory x0i−1 of the vehicle directly in front. Note that we shift velocity path
of vehicle i − 1 to the left by l, the vehicle length. We adopt this visualization
since it is more natural to discuss safety of vehicle i with respect to the rear
bumper of vehicle i − 1.

and a “safe” trajectory x̄i ∈ E (x̂i |[t00 ,t× ] , t00 , t̄, x0i−1 ). Define
p× := x̂i (t× ). Suppose the following are satisfied for all p ∈
[p× , 0]:
V [x̄i ](p) ≤ V [x̂i ](p)

Define p0i := xi (t00 ). Note that Inequality (28) also holds for
trajectories x̄i , x̃i , and xi , i.e.,

(32)

V [x̄i ](p) ≥ V [x0i−1 ](p + l)

Then, by Proposition VI.11, for all t ≥ t00 :

δ[x̄i ](p) ≤ δ[x0i−1 ](p + l).

x̄i (t) ≤ x̃i (t) ≤ xi (t).

(33)
(34)

Then, Ei0 is non-empty.

Hence, x̃i (t) = xi (t) for all t ∈ [t00 , tc [xi ]]. It is left to show
that trajectories x̃i and x0i−1 are safe for all time. Since νi = 1,
we have x0i−1 = xi−1 |{t : t≥t00 } , by Proposition VI.2. Thus, for
any t ≥ t00 :
− l.

We use the following notation throughout the three cases:
tci := tc [xi ];

tci−1 := tc [xi−1 ];

pci := xi (tci );

pci−1 := xi−1 (tci−1 ) − l;

vic

:=

ẋi (tci );

c
vi−1

:=

(35)

ẋi−1 (tci−1 ).

Case 1. See Figure 22. Suppose the following:

Hence, x̃i ∈ Ei0 .
Now, we consider vehicle i satisfying νi > 1. Recall that at
the start of the induction step, the updated trajectory x0i−1 of
the vehicle i − 1 and the previous trajectory xi of vehicle i are
known; and we would like to construct the updated trajectory
x0i of vehicle i. We consider three cases. In each of those
cases, we construct a “high-performance” trajectory x̂i and a
“safe” trajectory x̄i , and then apply the following proposition
to establish that there exists a trajectory x̃i for vehicle i that
has both performance and safety guarantees, i.e., Ei0 is nonempty. Consider Figure 21.
×

Proposition VI.12 Let time t be such that t ≥ tc [xi ].
Suppose there exists a “high-performance” trajectory x̂i ∈
E (xi |[t00 ,t× ] , t00 , t̂, ∅) for some t̂ satisfying
t0f ≥ t̂,

(31)

δ[x̄i ](p) ≥ δ[x̂i ](p)

Suppose the following are satisfied for all p ∈ (p× , −l):

tD [x̄i ](p0i ) ≥ tD [x̃i ](p0i ) ≥ tD [xi ](p0i ).

x0i−1 (t)

position

p×

−L

Fig. 20. Notation for Proposition VI.11. “Fast” velocity path V [x̂] and “slow”
velocity path V [x̄] are shown. Note that at each position p ∈ (p0 , pf ), the
velocity of the “fast” trajectory is greater than that of the “slow” trajectory.
See Inequality (26). Also, Inequality (28) states that x̂ departs position p0
earlier than x̄. The claim of this proposition, expressed in Inequality (29),
states that x̂ is always ahead, or closer to the intersection, than x̄.

×

V [x0i−1 ](p + l)

t00

position

x̃(t) ≤ xi (t) ≤ xi−1 (t) − l =

V [x̂i ]
V [xi ]

vm
velocity

V [x̄]

t×

i
tci

velocity

tA [x̂](p0 )
V [x̂]
tD [x̂](p0 )

Ii

∂F

vm

ẑ0 t0

(30)

1

tci ≥ tci−1 ;

(36)

pci ≤ pci−1 .

(37)

First, we construct the “high-performance” trajectory x̂i :=
T (zi (t00 ), ûi ), where control action ûi is defined as follows:

0 c

ui (t) : t ∈ [t0 , ti )
ûi (t) = am
(38)
: t ∈ [tci , t1 )


0
: t ∈ [t1 , t2 ],
where
t1 := tci + (vm − vic )/am ,
t2 := t1 +

pci −

2
vm
−(vic )2
2am

vm

.

Using xi as the “slow” trajectory and x̂i as the “fast” trajectory, Inequalities (26), (27), and (28) are satisfied; thus, by

1

Ii

i

V [x̂i ]

tci

t00

V [x̄i ]
V [x0i−1 ](p + l)

pci

−L

vic

tci−1

pci−1

position

where
c
t3 := tci + (vic − vi−1
)/am

t4 := t0i−1 − tci−1 + t3

0
t0i−1 := τi−1
+ L/vm .

t00

tci

V [xi ]

pci−1 pci

vm
v×
c
vi−1

vic
p×

position

Fig. 23. Depiction of Case 2 in Section C. Only the first segment of previous
trajectory xi is depicted, by a black arrow. “High-performance” trajectory x̂i
is depicted by the sequence of red arrows, “safe” trajectory by green arrows,
and updated trajectory x0i−1 of vehicle in front by blue arrows. Inequality (44)
is depicted clearly. Also, Inequality (43) states that previous trajectory xi
leaves set Fi , denoted time tci , after previous trajectory xi−1 of vehicle i − 1
leaves set Fi−1 , denoted time tci−1 . Both “high-performance” trajectory x̂i
and “safe” trajectory x̄ replicate xi until time tci . Position p× is the first
position that “high-performance” trajectory x̂i and the rear bumper of the
updated trajectory x0i−1 intersect. See Definition (45). Also, note that x̂i
departs position p× at a later time than when the rear bumper of the updated
×
trajectory x0i−1 arrives at this position, i.e., t×
i ≥ ti−1 . Furthermore, “highperformance” trajectory x̂i and “safe” trajectory x̄i are equivalent up until
time t×
i . See definition of control ûi in Equation (47).

since t3 ≥ tci−1 . By Proposition VI.12, Ei0 is non-empty.
Case 2. See Figure 23. Suppose the following:
tci ≥ tci−1

pci > pci−1 .

(43)
(44)

In this case, we use the same “high-performance” trajectory x̂i
as defined in Equation (38); and by the same argument made
earlier, Inequality (30) holds. Next, we construct the “safe”
trajectory x̄i . By Inequality (44), V [x̂i ](p) and V [x0i−1 ](p + l)
must intersect after position pci . Define
p× := inf{p ≥ pci : V [x̂i ](p) = V [x0i−1 ](p + l)}
×

×

v := V [x̂i ](p ) = V

[x0i−1 ](p× ).

(45)
(46)

Define
(40)

×
t×
i := tD [x̂i ](p ),

(41)

0
×
t×
i−1 := tA [xi−1 ](p + l).

(42)

To borrow the language of Proposition VI.12, tci is t× , x̂i is the
“high-performance” trajectory, and x̄i is the “safe” trajectory.
First, note x̂(t) = xi (t) = x̄i (t) for all t ∈ [t00 , tci ]. Second,
by construction, Inequalities (31), (32), (33) and (34) hold.
Lastly, note that x̄i and x0i−1 are safe for all time. For any
t ∈ [t00 , tci−1 ]:
x̄i (t) = xi (t) ≤ xi−1 (t) − l = x0i−1 (t) − l.

×
By Inequalities (43) and (44), we have t×
i ≥ ti−1 . Define
0
“safe” trajectory x̄i := T (zi (t0 ), ūi ), where control action ūi
is defined as follows:


: t ∈ [t00 , t×
ûi (t)
i )
×
×
ūi (t) = u0i−1 (t − t×
(47)
+
t
)
:
t
∈
[t
,
t
5)
i
i−1
i


0
: t ∈ [t5 , t5 + s],

where

[tci−1 , t3 ]:

×
t5 := t0i−1 + t×
i − ti−1 .

x̄i (t) ≤ pci−1 ≤ x0i−1 (tci−1 ) − l.

×
To borrow the language of Proposition VI.12, t×
i is t , x̂i
is the “high-performance” trajectory, and x̄i is the “safe”
trajectory. First, note x̂i (t) = xi (t) = x̄i (t) for all t ∈ [t00 , t×
i ].
Second, by construction, Inequalities (31), (32), (33) and (34)

For any t ≥ t3 :
x̄i (t) = x0i−1 (t − t3 + tci−1 ) − l ≤ x0i−1 (t) − l,

V [x̄i ]

t00

−L

Proposition VI.11, we have xi (t) ≤ x̂i (t) for all time t. Since
trajectory xi reaches the intersection region at time tf , which
satisfies t0f > tf , then trajectory x̂i must reach the intersection
region before time t0f , i.e., t0f ≥ t2 ; thus, Inequality (30) is met.
By Inequalities (36) and (37), we have t3 ≥ tci−1 . Now,
we construct the “safe” trajectory x̄i . Define trajectory x̄i :=
T (zi (t00 ), ūi ), where control action ūi is defined as follows:

: t ∈ [t00 , tci )

ui (t)

−a
: t ∈ [tci , t3 )
m
(39)
ūi (t) =
u0i−1 (t − t3 + tci−1 ) : t ∈ [t3 , t4 )



0
: t ∈ [t4 , t4 + s],

Ii
t×
i−1
t×
i

V [x0i−1 ](p + l)

c
vi−1

Fig. 22.
Depiction of Case 1 in Section C. Previous trajectory xi is
identified by following the black arrows, “high-performance” trajectory x̂i by
red arrows, “safe” trajectory by green arrows, and updated trajectory x0i−1
of vehicle in front by blue arrows. Both “high-performance” trajectory x̂i
and safe trajectory x̄ replicate xi until time tci , which denotes the time that
xi leaves the set Fi . Inequality (37) is depicted clearly. Also, Inequality (36)
states that previous trajectory xi leaves set Fi , denoted time tci , after previous
trajectory xi−1 of vehicle i−1 leaves set Fi−1 , denoted time tci−1 . In “highperformance” trajectory x̂, time t1 denotes the time that the vehicle reaches
maximum speed, i.e., t1 is the earliest time t such that x̂˙ i (t) = vm . See
definition of control ûi in Equation (38). Time t2 is the time that “highperformance” trajectory x̂i reaches the intersection region, i.e., state (0, vm ).
c ).
Time t3 is the time that “safe” trajectory x̄i arrives at state (pci−1 , vi−1
See definition of control ūi in Equation (39). Note that “safe” trajectory x̄i
arrives at this state later than when previous trajectory xi−1 departs from this
state, i.e., t3 ≥ tci−1 .

For any t ∈

i

velocity

tci−1

V [x̂i ]

∂F

vm

V [xi ]

t3
t00

t2

velocity

t1

∂F

1

1

Ii

t6

∂F

tci
V [xi ]

t0ci−1
t×
i

V [x̂i ]

V [x0i−1 ](p + l)
t00
tci t0ci−1 t8
−L

pci p×
i

p0ci−1

V [x̄i ]

vm
vic
vi×
velocity

t00

t7
t9

i

l

0c
vi−1

position

x̄i (t) = xi (t) ≤ xi−1 (t) − l = x0i−1 (t) − l,

2
vm
+(vic )2 −2(vi× )2
2am

.
vm
“High-performance” trajectory x̂i reaches the intersection region no later than t0f . To show this, we use Equation (51) to
re-express t7 as a function of vi× and some constants:
pci − m i2am i
vm − vi×
v c − vi×
+
+
.
t7 = + i
am
am
vm
We upper bound t7 as follows. Note that t7 is quadratic in
0c
vi× with positive curvature. Also, vi× must range in [vi−1
, vic ].
Therefore,
0c
t7 = t7 (vi× ) ≤ max{t7 (vi−1
), t7 (vic )}

0c
) is the time that the “high-performance”
Note that t7 (vi−1
trajectory x̂i reaches the intersection region, with the constraint
0
that vehicle i arrives at p0c
i−1 before updated trajectory xi−1
0c
0
0
of vehicle i − 1 departs pi−1 + l. Using ti−1 := τi−1 + L/vm :
0c
t7 (vi−1
) ≤ t0i−1 + s = t0f .

Note that t7 (vic ) is the time that “high-performance” trajectory
x̂i reaches the intersection region if it copies exactly the
previous trajectory xi of vehicle i until position pci and then
fully accelerates until the intersection region:
t7 (vi× ) ≤ tf ≤ t0f .

since tci ≥ tci−1 . For any t ∈ [tci−1 , t×
i ]:

x̄i (t) ≤ x0i−1 (t) − l,

by Proposition VI.11. For any t ≥ t×
i :

×
0
x̄i (t) = x0i−1 (t − t×
i + ti−1 ) − l ≤ xi−1 (t) − l,

×
0
since t×
i ≥ ti−1 . By Proposition VI.12, Ei is non-empty.

Case 3. See Figure 24. Suppose tci−1 ≥ tci . Since tc [x0i−1 ] ≥
0
(because x0i−1 ∈ Ei−1
by Lemma VI.5), we have

tci−1

(48)

Next, we construct “safe” trajectory x̄i . Define trajectory
x̄i := T (zi (t00 ), ūi ), where control action ūi is defined as
follows:

ûi (t)
: t ∈ [t00 , t×

i )


−a
×
: t ∈ [ti , t8 )
m
ūi (t) =
0
0
0c

u
(t
−
t
+
t
[x
](p
+
l))
:
t ∈ [t8 , t9 )
8
A i−1

i−1
i−1


0
: t ∈ [t9 , t9 + s],
(52)
where
×
0c
t8 := t×
i + (vi − vi−1 )/am

t9 := t8 + t0i−1 − tA [x0i−1 ](p0c
i−1 + l).

use the following notation:
:= tc [x0i−1 ]
:= x0i−1 (t0c
i−1 ) − l
0
0c
:= ẋi−1 (ti−1 ).

t7 := t6 +

pci −

v 2 +(v c )2 −2(v × )2

hold. Lastly, note that x̄i and x0i−1 are safe for all time. For
any t ∈ [t00 , tci−1 ]:

Throughout this case, we

0c

ti−1
p0c
i−1

 0c
vi−1

×
t6 := t×
i + (vm − vi )/am

tci

Fig. 24.
Depiction of Case 3 in Section C. Previous trajectory xi is
depicted by a black arrow, “high-performance” trajectory x̂i is depicted by
the sequence of red arrows, “safe” trajectory by green arrows, and updated
trajectory x0i−1 of vehicle in front by blue arrows. Inequality (48) states
that previous trajectory xi leaves set Fi , denoted time tci , earlier than when
updated trajectory x0i−1 of vehicle i − 1 leaves set Fi−1 , denoted time
×
t0c
i−1 . See definition in (49). We define time ti in Equation (50). “Highperformance” trajectory x̂i and “safe” trajectory x̄ are equivalent up until
time t×
i . After this time, x̂i accelerates until reaching maximum speed at
time t6 ; while x̄i continues to decelerate until its arrival at position p0c
i−1 at
time t8 . Note that x̄i arrives at position p0c
i−1 later than when the rear bumper
of updated trajectory x0i−1 of vehicle i − 1 arrives at this same position, i.e.,
t8 ≥ tA [(x0i−1 ](p0c
i−1 + l). Also, time t7 is the time that “high-performance”
trajectory x̂i reaches the intersection region, i.e., state (0, vm ).

tci ≤ tc [x0i−1 ].

where

(49)

First, note that x̄i (t) = x̂i (t) for all t ∈ [t00 , t×
i ]. Second, by
construction, Inequalities (31), (32), (33) and (34) hold. Lastly,
x̄i and x0i−1 are safe for all time. For any t ∈ [t00 , tci−1 ]:
x̄i (t) ≤ xi (t) ≤ xi−1 (t) − l = x0i−1 (t) − l.

Define:
0c
c
c
t×
i := min{ti−1 , ti + vi /am }

c
vi× := vic − am (t×
i − ti ).

(50)

For any t ∈ [tci−1 , t0c
i−1 ]:

x̄i (t) ≤ x0i−1 (t) − l

(51)

Now, we construct the “high-performance” trajectory x̂i :=
T (zi (t00 ), ûi ), where control action ûi is defined as follows:

ui (t) : t ∈ [t00 , tci )



−a
: t ∈ [tci , t×
m
i )
ûi (t) =
×

a
:
t
∈
[t
,
t
m
6)

i


0
: t ∈ [t6 , t7 ],

1

by Proposition VI.11. For any t ∈ [t0c
i−1 , t8 ]:

0
x̄i (t) ≤ p0c
i−1 ≤ xi−1 (t) − l.

For any t ≥ t8 :

0
x̄i (t) = x0i−1 (t − t8 + tA [x0i−1 ](p0c
i−1 + l)) − l ≤ xi−1 (t) − l,

since t8 ≥ tA [(x0i−1 ](p0c
i−1 + l) by Inequality (48). By Proposition VI.12, Ei0 is non-empty.

vm
V [x̂]

V [x̄]

−L

p0

velocity

ν

Vν

position

Fig. 25. Velocity path Vν , shaded blue, as defined in Equation (53), used
in the proof of Proposition VI.10. We wish to find some velocity path Vν
(bounded between velocity paths V [x̂] and V [x̄]) that has terminal time t̃ in
between the terminal times of the “fast” trajectory x̂ and the “slow” trajectory
x̄. When t̃ is strictly less than tm , we can always find some ν > 0 such that
the trajectory xν constructed from velocity path Vν has terminal time t̃. For
terminal times t̃ greater than or equal to tm , any bounded velocity path is fully
stopped at some point along its trajectory, and we can choose the duration at
this stopped location appropriately to verify the proposition.

xν̃ has terminal time t̃. Define the map T : (0, vm ] → R as
follows:
Z 0
X
1
dp +
δ[x̂](p).
T (ν) =
p0 Vν (p)
{p : δ[x̂](p)>0}

First, note that Tν is exactly the terminal time of trajectory
xν . Second, T (vm ) = t̂. Next, by the monotone convergence theorem, limν→0+ T (ν) = tm . Lastly, note that T
is continuous; therefore, for any t̃ ∈ [t̂, tm ), there exists
ν̃ ∈ (0, vm ] such that T (ν̃) = t̃; and by definition of T ,
trajectory xν̃ := G (Vν̃ , δ[x̂], 0) has terminal time t̃.
It is left to show the continuity of T . Let  > 0 be given.
Choose any ν ∈ (0, vm ]. We show there exists δ > 0 such that
for any ω ∈ (0, vm ] satisfying |ν − ω| ≤ δ:
Z 0
Z 0
1
1
dp −
dp ≤ .
V
(p)
V
p0 ν
p0 ω (p)
First, note that for any p ∈ [p0 , 0]:
|Vν (p) − Vω (p)| ≤ |ν − ω|.

Proofs used in Section C
Proof of Proposition VI.10: Define:
Z 0
X
1
dp +
tm =
p0 V [x̄](p)

Define set Zω = {p : Vν (p) < min{ν, ω}}. Next, note that for
any p ∈ Zω :

δ[x̂](p).

Vν (p) = Vω (p).

{p : δ[x̂](p)>0}

This is the terminal time of the trajectory uniquely generated by the triple (V [x̄], δ[x̂], 0), i.e., G (V [x̄], δ[x̂], 0). This
trajectory copies the “slow” trajectory x̄ except at points of
zero velocity; at which, the trajectory only stays as long
as the “fast” trajectory x̂. Note that this trajectory satisfies
Inequalities (21) and (22). Furthermore, for any terminal time
t̃ ∈ [tm , t̄], we can just as easily create a trajectory satisfying
those inequalities. Define:
t̃ − tm
t̄ − tm
δλ := λ(δ[x̄] − δ[x̂]) + δ[x̂].
λ :=

Note that for any λ ∈ [0, 1], for all p ∈ [p0 , 0]:
δ[x̄](p) ≥ δλ (p) ≥ δ[x̂](p).
Therefore, the trajectory x̃ uniquely generated by the triple
(V [x̄], δλ , 0), i.e., x̃ := (V [x̄], δλ , 0), satisfies Inequalities (21)
and (22).
Now, suppose t̃ < tm . In this case, the velocity path V [x̄]
is too slow, and we must construct a different velocity path
in order to meet the terminal time t̃. Consider the following
velocity path.
For each ν ∈ [0, vm ], we define path Vν : [p0 , 0] → [0, vm ]
as follows:


V [x̄](p) : V [x̄](p) > ν
Vν (p) = ν
(53)
: V [x̄](p) ≤ ν ≤ V [x̂](p)


V [x̂](p) : V [x̂](p) < ν.
See Figure 25. Define xν := G (Vν , δ[x̂], 0). For any ν ∈
[0, vm ], trajectory xν satisfies Inequalities (21) and (22). We
claim that for any t̃ ∈ [t̂, tm ), there always exists ν̃ such that

1

Furthermore, for any p ∈ Zω{ :
Vν (p) ≥ min{ν, ω}

Vω (p) ≥ min{ν, ω}.
Then,
0

Z 0
1
1
dp −
dp
V
(p)
V
ν
p0 ω (p)
Z p0
Z
1
1
1
1
≤
−
dp +
−
dp
V
(p)
V
(p)
V
(p)
V
{
ν
ω
ν
ω
Zω
Zω
Z
1
1
≤
−
dp
V
(p)
V
(p)
{
ν
ω
Zω
Z
Z
|Vν − Vω |
|ν − ω|
≤
dp ≤
dp
min{ν 2 , ω 2 } Zω{
{ Vν (p)Vω (p)
Zω
4|p0 |δ
|p0 |δ
≤
≤
≤ .
min{ν 2 , ω 2 }
ν2
Z

The penultimate inequality holds, since ω ≥ ν/2 for
sufficiently small δ, i.e., δ ≤ ν/2. The last inequality holds
by choosing sufficiently small δ, i.e., δ ≤ ν 2 /(4|p0 |).

Proof of Proposition VI.12 Suppose t0f ∈ [t̂, t̄]. From the
construction of the “high-performance” trajectory x̂ and the
“safe” trajectory x̄, there exists (by Proposition VI.10) a trajectory x̃i ∈ E (x̂i |[t00 ,t× ] , t00 , t0f , ∅) satisfying for all p ∈ [p× , 0]:
V [x̄i ](p) ≤ V [x̃i ](p) ≤ V [x̂i ](p)

δ[x̄i ](p) ≥ δ[x̃i ](p) ≥ δ[x̂i ](p).

(54)
(55)

Now, we show x̃i is safe with x0i−1 . For all t ∈ [t00 , tD [x̄i ](t× )]:
x̃i (t) = x̄i (t) ≤ x0i−1 (t) − l,

x̃i (t) ≤

x0i−1 (t)

− l,

i

(56)

δ[x̃i ](p) ≤ δ[x0i−1 ](p + l).

tA [x̄i ](p+ )
2
vm
/(2am ) + (νi − 1)l

t00

(57)
(58)

Also, note:
(59)

From Inequalities (57), (58), (59), Inequality (56) holds (by
Proposition VI.13) for all t > tD [x̄i ](t× ). Hence, we have
shown that x̃i ∈ Ei0 .
Now, suppose t0f > t̄. We proceed by constructing a
¯i than trajectory x̄i in order to bound the
“slower” trajectory x̄
terminal time t0f , as in Equation (61), and afterwards apply
Proposition VI.10 to show existence of a trajectory x̃i with
terminal time t0f . We then show x̃i is safe with x0i−1 .
Borrowing the language of Proposition VI.10, x̄i acts as the
¯i acts as the “slow” trajectory. In order
“fast” trajectory, and x̄
¯i , we first find the earliest intersection point of
to construct x̄
velocity path V [x̄i ] and the blue curve depicted in Figure 26.
Formerly, we define:
s
)
(
2
vm
+
+ (νi − 1)l
p = inf p : V [x̄i ](p) = p +
2am

ts ts + σ
p+

p0i

vm
vic
v+

t+

V [x̄i ]

−L

0
tA [x0i−1 ](0) = τi−1
+ L/vm = t0f − s = tA [x̃i ](l).

t

tci

we argue as follows. From Inequalities (33), (34), (54),
and (55), for all p ∈ (p× , −l):
V [x̃i ](p) ≥ V [x0i−1 ](p + l)

t̄

Ii

∂F

velocity

by construction of x̃i . To show that for all t > tD [x̄i ](t× ),

position

¯i ] as generated by control ū
¯i defined
Fig. 26. “Slow” velocity path V [x̄
in Equation (60) is depicted with green arrows; “fast” velocity path V [x̄i ]
¯i vehicle
is depicted with red arrows. Note that during “slow” trajectory x̄
2 /(2a ) + (ν − 1)l from
halts for an amount of time σ at a distance vm
m
i
¯i arrives at this position at time ts and
the intersection region. Trajectory x̄
¯i arrives at state
departs this position at time ts +σ. Time t+ is the time that x̄
¯i reaches the intersection region, i.e., state (0, vm ),
(p+ , v + ). Trajectory x̄
at time t̄¯. “Fast” trajectory x̄i arrives at state (p+ , v + ) at time tA [x̄i ](p+ ),
and arrives at the intersection region, i.e., state (0, vm ), at time t̄.

tA [x̂](p0 )

t

tD [x̂](p)

tA [x̂](pf )

tD [x̄](pf )

pf
p
x̄(t)
p0
−L

x̂

tD [x̂](p0 )

x̄

tD [x̄](p)

tD [x̄](p0 )

v + = V [x̄i ](p+ ).
We use notation developed in Equations (35). Now, consider
¯i := T (zi (t00 ), ū
¯i ), where control action ū
¯i is
trajectory x̄
defined as follows (depicted in Figure 26):


ūi (t)
: t ∈ [t00 , tci )





: t ∈ [tci , ts )
−am
¯i (t) = 0
ū
: t ∈ [ts , ts + σ) (60)


am
: t ∈ [ts + σ, t+ ]



ū (t − t+ + t [x̄ ](p+ )) : t ∈ [t+ , t̄¯],
i
A i
where
ts := tci + vic /am
t+ := ts + σ + v + /am
t̄¯ := t+ + t̄ − tA [x̄i ](p+ )


σ := max 0, t0f − ts + v + /am + t̄ − tA [x̄i ](p+ ) .
By construction of σ:
t̄¯ ≥ t0f ≥ t̄.

(61)

¯i , for all p:
By construction of ū
¯i ](p) ≤ V [x̄i ](p)
V [x̄
¯i ](p) ≥ δ[x̄i ](p).
δ[x̄
¯ and
Since Inequalities (19) and (20) hold for trajectories x̄
x̄, there exists (by Proposition VI.10) a trajectory x̃i ∈

Fig. 27. Notation for proof of Proposition VI.11 on a position-time plot.
Note that trajectories x̂ and x̄ are both monotonically increasing functions.
Note that Inequality (28) is illustrated clearly: the departure time of x̄ from
position p0 is later than the departure time of x̂ from the same position, i.e.,
tD [x̄](p0 ) ≥ tD [x̂](p0 ). We show that x̄(t) ≤ x̂(t) for all time t from the
arrival of x̂ at position p0 until the departure of x̄ from position pf , i.e., for
all t ∈ [tA [x̂](p0 ), tD [x̄](pf )].

C (zi (t00 ), t00 , t0f , ∅) satisfying Inequalities (26) and (27) for
¯i , x̃i , and x̄i , i.e., for all p:
trajectories x̄
¯i ](p) ≤ V [x̃i ](p) ≤ V [x̄i ](p)
V [x̄
¯i ](p) ≥ δ[x̃i ](p) ≥ δ[x̄i ](p).
δ[x̄
¯i , x̃i , and
Note that Inequality (28) also holds for trajectories x̄
x̄i , i.e.,
tD [x̄i ](p0i ) ≥ tD [x̃i ](p0i ) ≥ tD [xi ](p0i ),
where p0i := xi (t00 ). Since Inequalities (26), (27), and (28)
hold, then (by Proposition VI.11) for all t ≥ t00 :
¯i (t) ≤ x̃i (t) ≤ x̄i (t).
x̄
Safety of x̃i and x0i−1 immediately follows. Also,
x̃i (t) = xi (t) for all t ∈ [t00 , tc [xi ]]. Thus, x̃i ∈ Ei0 .

Proof of Proposition VI.11 See Figure 27. Let
t ∈ [tA [x̂](p0 ), tA [x̂](pf )).

1

tA [x̄](p0 )

tA [x̄](p)

tA [x̄](pf )

t

tD [x̂](pf )

Since t ≥ tA [x̄](p) and x̄ is monotonically increasing:
x̄(t) ≥ x̄(tA [x̄](p)) = p = x̂(t).

pf
x̄

It remains to show the above inequality holds for

x̄(t)

p0
−L

t ∈ [tA [x̄](p0 ), tD [x̂](p0 )].

x̂

p

tA [x̂](p)

tD [x̄](p0 )

Note
x̄(t) ≥ p0 = x̂(tD [x̂](p0 )) ≥ x̂(t),

tD [x̂](p0 )

Fig. 28. Notation for proof of Proposition VI.13 on a position-time plot.
Note that trajectories x̂ and x̄ are both monotonically increasing functions.
Note that Inequality (64) is illustrated clearly: the arrival time of x̄ at position
pf is earlier than the arrival time of x̂ at the same position. We show that
x̄(t) ≥ x̂(t) for all time t from the arrival of x̄ at position p0 until the
departure of x̂ from position pf , i.e., for all t ∈ [tA [x̂](p0 ), tD [x̄](pf )]. This
proposition complements Proposition VI.11, in that it applies its argument in
reverse.

We can find p ∈ [p0 , pf ) such that x̂(t) = p. By Inequalities (26), (27), and (28):
t ≤ tD [x̂](p)

Z

p

1
dq +
V [x̂](q)

= tD [x̂](p0 ) +
p0

≤ tD [x̄](p0 ) +

Z

p

1
dq +
V [x̄](q)

p0

X

δ[x̂](q)

{q∈(p0 ,p] : δ[x̂](q)>0}

X

δ[x̄](q)

{q∈(p0 ,p] : δ[x̄](q)>0}

= tD [x̄](p).
Since t ≤ tD [x̄](p) and x̄ is monotonically increasing:
x̄(t) ≤ x̄(tD [x̄](p)) = p = x̂(t).

It remains to show the above inequality holds for
t ∈ [tA [x̂](pf ), tD [x̄](pf )].

Note x̄(t) ≤ pf = x̂(tA [x̂](pf )) ≤ x̂(t).



Proposition VI.13 Suppose there exist a “fast” trajectory
x̂ ∈ C (ẑ0 , t0 , t̂, ∅) and a “slow” trajectory x̄ ∈ C (z̄0 , t0 , t̄, ∅),
such that for all p ∈ (p0 , pf ):
V [x̄](p) ≤ V [x̂](p)

(62)

δ[x̄](p) ≥ δ[x̂](p)

(63)

tA [x̄](pf ) ≤ tA [x̂](pf ).

(64)

x̄(t) ≥ x̂(t).

(65)

Then, for all t ∈ [tA [x̄](p0 ), tD [x̂](pf )]:

Proof:
Consider
Figure
28.
Let
t
∈
(tD [x̂](p0 ), tD [x̂](pf )]. Find p ∈ (p0 , pf ] such that x̂(t) = p.
By Inequalities (62), (63), and (64):
t ≥ tA [x̂](p)
= tA [x̂](pf ) −
≥ tA [x̄](pf ) −

Z

pf

p0

Z

pf

p0

1
dq −
V [x̂](q)
1
dq −
V [x̄](q)

X

δ[x̂](q)

{q∈[p,pf ) : δ[x̂](q)>0}

X

δ[x̄](q)

{q∈[p,pf ) : δ[x̄](q)>0}

= tA [x̄](p).

1

where the last inequality holds since x̂ is monotonically
increasing.

D. Proof of Lemma VI.6
In this section, we first show that if the set Ei0 is non-empty,
then the optimization problem in the MotionSynthesize
procedure (see Equation (3)) attains its minimum. We use
Filippov’s theorem in this step. See Theorem VI.14.
Next, we show that the minimizing trajectory, i.e., the
updated trajectory x0i , is contained in Ei0 . More specifically,
we show that the following inequalities hold for any x̃ ∈ Ei0
and all time t:
x̃(t) ≤ x0i (t) ≤ xi (t),

(66)

where xi is the previous trajectory of vehicle i. From the above
inequalites, x0i ∈ Ei0 directly follows.
The first inequality holds by the following argument. For
all x ∈ Ci0 and for all time t:
x(t) ≤ x0i (t),
Ei0

(67)

Ci0 .

and then noting x̃ ∈
⊆
The second inequality holds
from the following. For all x ∈ Ci0 and for all time t:
x(t) ≤ xi (t),

(68)

and then noting x0i ∈ Ci0 . We show Inequality (67) holds with
a proof by contradiction. Then, we show Inequality (68) holds
by induction on νi .
To show the existence of a minimum in the
MotionSynthesize procedure, we employ the following
theorem from optimal control theory.

where K := [−L(t0f −t00 ), 0]×[−L, 0]×[0, vm ] and p : U → U
is defined as


max{u, 0} : q3 = 0
p(u) = min{u, 0} : q3 = vm


u
: q3 ∈ (0, vm ).
First, note that U = [−am , am ] is compact. Next, note
that Equation (69) is satisfied for all q 6∈ K and for all
u ∈ U . Lastly, note that f (q, U ) is convex for all q ∈ K.
Consider initial state (column vector) q0 := [0, zi (t00 )]. By
Theorem VI.14, A is compact.
Define the following set of state histories:
Q[x0i−1 ] := {q ∈ Q | ∀t, q2 (t) ≤ x0i−1 (t) − l;
q2 (t0f ) = 0; q3 (t0f ) = vm }.

Next, define the following attainable set:
A[x0i−1 ] := {q(t0f ) : q ∈ Q[x0i−1 ]} ⊆ R3 .
Now, A[x0i−1 ] is also compact. First, note that since A[x0i−1 ] ⊆
A, then A[x0i−1 ] is bounded. We, now show A[x0i−1 ] is also
closed.
Let {an } by a convergent sequence with elements an ∈
A[x0i−1 ] ⊆ R3 . We show that the limit of sequence {an } is
also contained in A[x0i−1 ]. By definition of A[x0i−1 ], we can
embed convergent sequence {an } into some sequence {q n } of
state histories with elements q n ∈ Q[x0i−1 ]. Next, we invoke
Theorem VI.14 to find a convergent subsequence q nk , which
converges uniformly to some q̃ ∈ Q. Next, note for all t and
for all n:

Theorem VI.14 Filippov [66] Let U be the set of all measurable functions from set [t0 , tf ] to U , where U ⊆ Rm is a
compact set. Let there exist a compact K ⊆ Rn such that for
all q 6∈ K and for all u ∈ U :
f (q, u) = 0.

(69)

For each q ∈ K, let
f (q, U ) := {f (q, u) : u ∈ U } ⊆ Rn

q2nk (t) ≤ x0i−1 (t) − l

lim q2nk (t) ≤ lim x0i−1 (t) − l

k→∞

Thus, q̃ ∈ Q[x0i−1 ]. Moreover, by our embedding of {an }
into {q n }, we have limn→∞ an = q̃(t0f ) ∈ A[x0i−1 ]. Hence,
A[x0i−1 ] is also closed. Hence, we have shown that A[x0i−1 ]
is compact.
Now, consider optimization problem

be convex. Then, for each initial state q0 ∈ K and any
terminal time tf > 0, the attainable set
A := {qu (tf ) : u ∈ U, qu (t0 ) = q0 } ⊆ Rn
is compact, where qu : [t0 , tf ] → Rn satisfies q̇u (τ ) =
f (qu (τ ), u(τ )) for all τ ∈ [t0 , tf ] and qu (t0 ) = q0 .
Define Q := {qu : u ∈ U, qu (0) = q0 } as the set of all
state histories qu from initial state q0 generated by a control
u ∈ U. Then, for any sequence {q n }∞
n=1 of state histories
where q n ∈ Q, there exists a subsequence {q nk }∞
k=1 of state
histories that converges uniformly to some q̃ ∈ Q.
Consider the following system with state vector q :=
[q1 , q2 , q3 ]T :
(
[q2 (t), q3 (t), p(u(t))]T : q ∈ K
q̇(t) =
0
: q 6∈ K,

k→∞

q̃2 (t) ≤ x0i−1 (t) − l.

[−1, 0, 0]T a.

min

a∈A[x0i−1 ]

(70)

Optimization problem (70) is nontrivial, since Ci0 6= ∅ implies
A[x0i−1 ] 6= ∅. Also, note that the cost function is linear and
A[x0i−1 ] is compact; hence, a global minimum exists. Consider
optimization problem
min

q∈Q[x0i−1 ]

−q1 (t0f )

(71)

|x(t)|dt.

(72)

and optimization problem
Z

t0f

min

x∈Ci0

t00

By construction, optimization problems (70) and (71) are
equivalent. Next, notice optimization problems (71) and (72)
are equivalent. For any x ∈ Ci0 , define q1 : [t00 , t0f ] → R as
Rt
q1 (t) := t0 x(τ )dτ . Then, [q1 , x, ẋ]T ∈ Q and −q1 (t0f ) =
0

R t0f

|x(t)|dt. Conversely, note that for any q ∈ Q, we have
position history q2 ∈ Ci0 . Hence, the solution to optimization
problem (72) exists.
Thus far, we have shown x0i , as defined by the
MotionSynthesize procedure, exists. It remains to show
Inequalities (67) and (68) hold. First we show Inequality (67)
with a proof by contradiction. We use the following lemma:
t00

Lemma VI.15 Let x
C (z0 , t0 , t0f , xs ) satisfy

C (z0 , t0 , tf , xs ) and x0

∈

∈

t0f ≥ tf .
Suppose there exists time t̄ satisfying
x(t̄) < x0 (t̄).
Then, there exists trajectory y ∈ C (z0 , t0 , tf , xs ) satisfying
for all t ∈ [0, tf ]:
x(t) ≤ y(t)

(73)

x(t̄) < y(t̄).

Ci0 . Suppose there
x0i (ta ). By Lemma

(74)
[t00 , t0f ]

Let x ∈
exists ta ∈
such that
x(ta ) >
VI.15, there exists trajectory
y ∈ Ci0 satisfying for all time t:
y(t) ≥ x0i (t)

t00

|y(t)|dt <

Z

t0f

t00

|y(ta )| < |xi (ta )|.

t00

Z

tf

t00

x(t) ≤ xi−1 (t).
Recall trajectory x0i−1 is the optimal trajectory for vehicle i−1
given by the MotionSynthesize procedure at time t0 . Thus,
0
since x0i−1 ∈ Ci−1
, we have
≤ xi−1 (t).

≤ xi−1 (t) − l,

where the second inequality holds from the induction step
in Inequality (75). Now, suppose there exists ta such that
x(ta ) > xi (ta ). Recall that terminal time of x ∈ Ci0 is
equal to t0f ; and the terminal time of xi is equal to tf . Since
t0f ≥ tf , we can apply Lemma VI.15 to create trajectory
y ∈ C (zi (t00 ), t00 , tf , xi−1 ) satisfying for all t:
|y(t)| ≤ |xi (t)|

|y(ta )| < |xi (ta )|.
By continuity of y:
Z tf
t00

|y(t)|dt <

tf

Z

t00

|xi (t)|dt,

contradicting the optimality of xi . Hence, Inequality (68) is
established.

From these definitions, we have
x0 (ť) = x(ť)

(76)

0

x(t̂) = x (t̂).

(77)

Case 1. First, suppose
ẋ0 (ť) = ẋ(ť)

(78)

ẋ(t̂) = ẋ0 (t̂).

(79)

Consider Figure 29. Define trajectory y1 :


x(t) : t ∈ [t0 , ť)
y1 (t) = x0 (t) : t ∈ [ť, t̂)


x(t) : t ∈ [t̂, tf ].

(80)

Note that trajectory y1 satisfies Inequalities (73) and (74).
Case 2. Now, suppose

|xi (t)|dt,

contradicting construction of xi . Thus, Inequality (68) holds
for νi = 1.
Now, assume νi > 1. By induction, we assume for any
0
x ∈ Ci−1
and for all t:

x0i−1 (t)

x(t) ≤ x0i−1 (t) − l

t̂ := inf{t ≥ t̄ : x0 (t) = x(t)}.

|x0i (t)|dt,

|y(t)| ≤ |xi (t)|

|y(t)|dt <

Since trajectory x is contained in Ci0 , trajectories x and x0i−1
are safe. Furthermore, x must also be safe with xi−1 . Note
that for all time t:

ť := sup{t ≤ t̄ : x0 (t) = x(t)}

contradicting construction of x0i . Thus, Inequality (67) holds.
We conclude by showing Inequality (68) holds, by induction on νi . Let x ∈ Ci0 . Suppose νi = 1. Then, by
Lemma VI.2, x0i−1 = xi−1 |t≥t00 , from which xi is safe
with the same trajectory as x. Thus, we are almost ready to
apply Proposition VI.15. Suppose there exists ta satisfying
xi (ta ) < x(ta ). Then, by Lemma VI.15, there exists trajectory
y ∈ C (zi (t00 ), t00 , tf , xi−1 ) satisfying for all t:

By continuity of y:
Z tf

x(t) ≤ xi (t).

Proofs used in Section D
Proof of Lemma VI.15: Define

y(ta ) > x0i (ta ).
By continuity of y:
Z t0f

Let x ∈ Ci0 . We show for all t:

(75)

ẋ0 (ť) = ẋ(ť)

(81)

0

ẋ(t̂) 6= ẋ (t̂).
0

(82)
0

0

Recall for any t we define z (t) := (x (t), ẋ (t)) and
z(t) := (x(t), ẋ(t)). From Inequalities (77) and (82), by
Lemma VI.16, there exists trajectory x̃ ∈ C (z 0 (t1 ), t2 , ∅, xs )
such that for all t ∈ [t1 , t2 ]:
x̃(t) ≥ max{x0 (t), x(t)}

x̃(t2 ) = x(t2 )
˙ 1 ) = ẋ(t2 )
x̃(t

t0

ť

t̄

tf time t0f

t̂

t0

t3

ť

t4 t̄

tf time t0f

t6

y1
position

position

x

y3

0

x

x̃1

x

x0
−L

−L

Fig. 29. Notation for Case 1 in the proof of Lemma VI.15 used in Section
D. Trajectory y1 as defined in Equation (80) is depicted in shaded blue.
Trajectory x is shown in red, and trajectory x0 in black. The assumptions
in this scenario, given in Equations (78) and (79), are depicted clearly. The
result of Lemma VI.15 is shown, i.e., y(t) ≥ x(t) for all time t and also
y(t̄) > x(t̄).

t0

ť

t̄

t1

t̂

t2

tf time t0f

From Inequalities (76) and (84), by Lemma VI.16, there exists
trajectory x̃1 ∈ C (z(t3 ), t3 , ∅, xs ) satisfying for all t ∈ [t3 , t4 ]:

y2
position

x̃
x

Fig. 31. Notation for Case 3 in the proof of Lemma VI.15 used in Section
D. Trajectory y3 as defined in Equation (86) is depicted in shaded blue.
Trajectory x is shown in red, and trajectory x0 in black. Trajectory x̃1 is the
dashed black line, and exists from a direct application of Lemma VI.16. The
assumptions used in Case 3, given in Equations (84) and (85), are depicted.
The result of Lemma VI.15 is shown, i.e., y(t) ≥ x(t) for all time t and also
y(t̄) > x(t̄).

x̃1 (t) ≥ max{x(t), x0 (t)}

x

0

x̃1 (t4 ) = x0 (t4 )
x̃˙ 1 (t4 ) = ẋ0 (t4 ),
where

−L
Fig. 30. Notation for Case 2 in the proof of Lemma VI.15 used in Section
D. Trajectory y2 as defined in Equation (83) is depicted in shaded blue.
Trajectory x is shown in red, and trajectory x0 in black. Trajectory x̃ is the
dashed black line, and exists from a direct application of Lemma VI.16. The
assumptions of Case 2, given in Equations (81) and (82), are depicted. The
difference between Cases 1 and 2, is that we now assume ẋ(t̂) 6= ẋ0 (t̂). The
result of Lemma VI.15 is shown, i.e., y(t) ≥ x(t) for all time t and also
y(t̄) > x(t̄).

t3 := sup{t ≤ ť : ∃ τ, z(t) = z 0 (τ )}
t4 := inf{t ≥ ť : ∃ τ, z(τ ) = z 0 (t)}.

Inequality (84) and the definition of t4 imply that x0 (t4 ) ≥
x(t4 ), from which we define
t6 := inf{t ≥ t4 : x(t) = x0 (t)}.
Clearly, t6 ≥ t4 .
Case 3. Suppose
ẋ(t6 ) = ẋ0 (t6 ).

where
t1 := sup{t ≤ t̂ : ∃ τ, z 0 (t) = z(τ )}

Consider Figure 31. We construct

x(t)



x̃ (t)
1
y3 (t) =
x0 (t)



x(t)

0

t2 := inf{t ≥ t̂ : ∃ τ, z (τ ) = z(t)}.

Note that t1 ≥ ť. Now, we construct trajectory y2 :

x(t) : t ∈ [t0 , ť)



x0 (t) : t ∈ [ť, t )
1
y2 (t) =

x̃(t)
:
t
∈
[t
,
t2 )
1



x(t) : t ∈ [t2 , tf ].

(83)

Note that trajectory y2 satisfies Inequalities (73) and (74).
Case 1 and 2 together exhaust the scenario ẋ0 (ť) = ẋ(ť). See
Equations (76) and (81). In the remaining cases, we consider
the following scenario:
0

ẋ (ť) 6= ẋ(ť).

ẋ(ť) < ẋ (ť).

trajectory y3 :
:
:
:
:

[t0 , t3 )
[t3 , t4 )
[t4 , t6 )
[t6 , tf ].

(86)

Note that trajectory y3 satisfies Inequalities (73) and (74).
Case 4. Now, suppose
ẋ(t6 ) 6= ẋ0 (t6 ).
By Inequality (84) and the definition of t6 :
ẋ0 (t6 ) < ẋ(t6 ).
By Lemma VI.16, there exists trajectory
C (z 0 (t5 ), t5 , ∅, xs ) satisfying for all t ∈ [t5 , t7 ]:

(87)
x̃2

∈

x̃2 (t) ≥ max{x(t), x0 (t)}

Then, from the definition of t̄ and ť:
0

(85)

(84)

1

x̃2 (t7 ) = x(t7 )
x̃˙ 2 (t7 ) = ẋ(t7 )

1

t0

t3

ť

t4 t̄

t5

t6

t7

tf time t0f
y4

V [x]

position

x̃2

t0a
ta

x̃1

t0b
τb0 t
b

τ0
Aτ

V [y]
τ

ẏ(τ )
velocity

x

vm
vτ0

x0
−L

−L

where
t5 := sup{t < t6 : ∃ τ, z(τ ) = z 0 (t)}
t7 := inf{t > t6 : ∃ τ, z(t) = z 0 (τ )}.

Note t4 ≥ t3 . Next, note that x(t) ≤ x0 (t) for all t ∈ [t3 , t4 ].
Consider Figure 32. We define trajectory y4 :


x(t)
: [t0 , t3 )





x̃
(t)
:
[t3 , t4 )
 1
0
(88)
y4 (t) = x (t) : [t4 , t5 )



x̃2 (t) : [t5 , t7 )



x(t)
: [t7 , tf ].
Note that trajectory y4 satisfies Inequalities (73) and (74). 
Lemma VI.16 Denote z0 := (p0 , v0 ). Denote x(t) and y(t)
denote (x(t), ẋ(t)) and (y(t), ẏ(t)), respectively. Let x ∈
C (z0 , t0 , tf , xs ) and y ∈ C (z0 , t0 , t0f , xs ). Suppose there
exists time t̂ satisfying
x(t̂) = y(t̂)
ẋ(t̂) > ẏ(t̂).
Then, there exists trajectory x̃ ∈ C (y(ta ), ta , ∅, xs ) satisfying
for all t ∈ [ta , t0b ]:
x̃(t) ≥ max{x(t), y(t)}

x̃(t0b )
˙ 0b )
x̃(t

=
=

x(t0b )
ẋ(t0b )

(89)
(90)

p̂

y(τ )

p0τ position pb

Fig. 33. Notation used in proof of Lemma VI.16. Velocity path x is the
higher velocity path, while velocity path y is the lower one. Trajectories x
and y are at position p̂ at the same time t̂, and velocity of trajectory x is
higher than that of y, i.e., ẋ(t̂) > ẏ(t̂). Velocity paths x and y intersect at
a position before and after position p̂; the closest to p̂, we denote pa and
pb , respectively. However, trajectories x and y depart from position pa and
arrive at position pb at different times. We denote t0a and ta as the times
that trajectories x and y depart from position pa , respectively. Similarly, we
denote t0b and tb as the times that trajectories x and y arrive at position pb ,
respectively. As we move along the lower velocity path V [y], we can choose
at any time τ ∈ [ta , tb ] to break away from velocity path V [y] in order to
follow a maximum acceleration curve Aτ , depicted in red, until intersection
with the higher velocity path V [x]. State (p0τ , vτ0 ) is the earliest state at which
acceleration curve Aτ and velocity path V [x] intersect; earliest in the sense
of earliest position. (See definition (92)). Time τ 0 is the earliest time that the
trajectory following black arrows, which we denote x[τ ], arrives at this state.
Trajectory x[τ ] arrives at position pb at time τb0 .

ta

t0a

τ̃ t̂

pb

t0b
T (τ )

time

tb

y

p̂
position

Fig. 32. Notation for Case 4 in the proof of Lemma VI.15 used in Section
D. Trajectory y4 as defined in Equation (88) is depicted in shaded blue.
Trajectory x is shown in red, and trajectory x0 in black. Trajectory x̃1 is
the dashed black line, and exists from a direct application of Lemma VI.16
at time ť. Trajectory x̃2 , depicted in the dashed black line, also comes from
applying Lemma VI.16 at time t6 . The assumptions used in Case 4, given in
Equations (84) and (87), are depicted. The result of Lemma VI.15 is shown,
i.e., y(t) ≥ x(t) for all time t and also y(t̄) > x(t̄).

pa

τ

x

pa
−L
Fig. 34. Notation used in proof of Lemma VI.16. Trajectories x and y are
at position p̂ at the same time t̂, and velocity of trajectory x is higher than
that of y, i.e., ẋ(t̂) > ẏ(t̂). Times t0a and ta are the times when trajectories
x and y depart from position pa , respectively. Note that t0a ≥ ta . Also,
times t0b and tb are the times when trajectories x and y arrive at position pb ,
respectively. Note that t0b ≤ tb . For any time τ ∈ [ta , tb ], we can construct
trajectory x[τ ]. We denote the time that trajectory x[τ ] arrives at position
pb as T (τ ). Sample trajectories x[τ ] with their associated terminal times
T (τ ) are depicted in light blue. Since the mapping τ 7→ T (τ ) as defined in
Equation (94) is continuous, we can find a time τ̃ at which to break away
from trajectory y as shown in Figure 33, so that the resulting trajectory x[τ̃ ]
has terminal time T (τ̃ ) equal to t0b .

(91)

where

Note that for any p ∈ (pa , pb ):

ta := sup{t ≤ t̂ : ∃ τ, y(t) = x(τ )}
t0b := inf{t ≥ t̂ : ∃ τ, x(t) = y(τ )}.

V [x](p) > V [y](p).

Proof: See Figures 33 and 34. Denote
Note that ta is the departure time of trajectory y from position
pa . Similarly, let t0a := tD [x](pa ) be the departure time
of trajectory x from position pa . We claim ta ≤ t0a . See
Figure 34. Essentially, this happens since trajectory x travels

p̂ := x(t̂) = y(t̂)
pa := y(ta )
pb := x(t0b ).

1

1

By construction of x[τ̃ ]:

faster than y before time t̂. More precisely, note:
Z p̂
1
t0a = tD [x](pa ) = t̂ −
dp
pa V [x](p)
Z p̂
1
≥ t̂ −
dp ≥ ta .
V
[y](p)
pa

x̃(tb ) = x[τ̃ ](tb ) = x(tb )
˙ b ) = ẋ[τ̃ ](tb ) = ẋ(tb ),
x̃(t

Also, note that t0b is the arrival time of trajectory x at position
pb . Similarly, let tb := tA [y](pb ) be the arrival time of
trajectory y at position pb . Since trajectory x travels faster
than y after time t̂, we have t0b ≤ tb . See Figure 34. More
precisely, note:
Z pb
1
dp
t0b = tA [x](pb ) = t̂ +
V [x](p)
p̂
Z pb
1
dp ≤ tb .
≤ t̂ +
V [y](p)
p̂

satisfying Inequalities (90) and (91).
For all p ∈ (pa , pb ):
V [x̃](p) ≥ V [y](p)
δ[x̃](p) ≤ δ[y](p)
and
tD [x̃](pa ) = tD [y](pa ).
Thus, by Proposition VI.11, for all t ∈ [ta , t0b ]:
x̃(t) ≥ y(t).

For any τ ∈ [ta , tb ], we define path Aτ as follows. See
Figure 34. For any p ∈ [y(τ ), 0]:
p
Aτ (p) := ẏ(τ )2 + 2am (y(τ ) − p).

Similarly, for all p ∈ (pa , pb ):

Define

and
p0τ
vτ0

:= inf{p : ∃ p, V [x](p) = Aτ (p)}
:= V [x](pτ ) = Aτ (pτ ).

V [x̃](p) ≤ V [x](p)

δ[x̃](p) ≥ δ[x](p),

(92)
(93)

Define trajectory x[τ ] := T (y(ta ), u[τ ]), where control action
u[τ ] is defined as follows:


: t ∈ [ta , τ )
ÿ(t)
u[τ ](t) = am
: t ∈ [τ, τ 0 )


ẍ(t − ζ[τ ]) : t ∈ [τ 0 , τb0 ],

tA [x̃](pb ) = tA [x](pb ).
Thus, by Proposition VI.13, for all t ∈ [ta , t0b ]:
x̃(t) ≥ x(t).

ζ[τ ] := τb0 − t0b .
See Figure 33. For any τ ∈ [ta , tb ], let T (τ ) be the arrival
time of x[τ ] at position pb , which is simply the terminal time
of x[τ ] defined above. More precisely, we define:
Z pb
1
dp.
(94)
T (τ ) := τ +
V
[x[τ
]](p)
x[τ ](τ )
We claim that T is continuous. To see this, consider Figure 34.
A small change in τ creates a small change in T . We provide
the proof of this claim at the end.
Assuming T is continuous, we continue to establish the
lemma. Since
T (ta ) = ta + t0b − t0a ≤ t0b
T (tb ) = tb ≥

t0b ,

there exists τ̃ ∈ [ta , tb ] such that T (τ̃ ) = t0b , from which
ζ[τ̃ ] = 0. We define trajectory x̃ as follows:
(
x[τ̃ ](t) : t ∈ [ta , t0b )
x̃(t) =
x(t)
: t ∈ [t0b , tf ].

(96)

Inequalities (95) and (96) together establish Inequality (89).
To complete the proof, it remains to show that x̃ is safe with
xs . However, this follows directly from either Inequality (95)
or (96). For all t:

where
τ 0 := τ + (vτ0 − ẏ(τ ))/am
Z pb
1
τb0 := τ 0 +
dp
V
[x](p)
0
pτ

(95)

x̃(t) ≥ y(t) ≥ xs (t) − l.
We conclude by showing T is continuous. Choose τ1 ∈
[ta , tb ]. Let  > 0. We show that there exists γ > 0 such that
for any τ2 satisfying
|τ2 − τ1 | ≤ γ,
we have
|T (τ2 ) − T (τ1 )| ≤ .
For shorthand, we denote
Vi := V [x[τi ]]
pi := y(τi )
vi := Vi (pi ),
for i = 1, 2. See Figure 35. We also denote
p3 := inf{p : Aτ1 (p) = V [x](p)}
p4 := inf{p : Aτ2 (p) = V [x](p)}.
See Figures 35 and 36.
Case 1. First, suppose
v1 > 0
τ2 < τ1 .

vm

vm
V [x]

V1
V2

−L

pa position

p2

p3

pb

pa

|T (τ2 ) − T (τ1 )|

q
v12 + 4am vm γ − v1 .

(97)

Note that V1 and V2 only differ on the interval (p2 , p3 ).
Bounding the difference between V1 and V2 amounts to
bounding the difference between two square root functions.
Note that the maximum difference between V1 and V2 occurs
at position p1 . We denote c(γ) as the maximum difference
at this position. To derive the expression for c(γ), consider
Figure 35. The maximum possible distance between positions
p2 and p1 is equal to vm γ. The maximump
possible velocity (of
either V1 or V2 ) at position p2 is equal to v12 + 2am vm γ. By
using full acceleration from position p2 to p1 , the maximum
possible
velocity (of either V1 or V2 ) at position p2 is equal to
p
v12 + 4am vm γ. The lowest possible velocity (of either V1 or
V2 ) at position p1 is simply v1 . Note that for all p ∈ [p1 , p3 ]:

p4 position pb

V2 (p) ≥ v1 .

p1

p2

1
dp
V [y](p)

≤ |τ2 − τ1 | ≤ γ.

pb

Since d(γ) is continuous and limγ→0 d(γ) = 0, we can always
find γ > 0 so that d(γ) ≤ .
Case 2. Now, suppose
v1 > 0
τ2 > τ1 .
Note that for any p ∈ [pa , pb ]:
|V2 (p) − V1 (p)| ≤ c̃(γ),
where
q

v12 + 2am vm γ −

q

v12 − 2am vm γ.

Note that V1 and V2 only differ on the interval (p1 , p4 ).
Consider Figure 36. The maximum difference between V1
and V2 now occurs at position p2 . We use two maximum
acceleration arcs pivoted at (p1 , v1 ). The lowest
achievable
p
velocity (by either V1 or V2 ) at position p2pis v12 − 2am vm γ,
while the highest achievable velocity is v12 + 2am vm γ.
Note that for all p ∈ [p2 , p4 ]:

Also, note that
Z

Z

≤ 2γ + 2c(γ)L/v12 := d(γ).

c̃(γ) :=

p2

p2

Z pb
1
1
dp −
dp
≤ |τ2 − τ1 | +
V
(p)
V
2
1 (p)
p2
p1
Z pb
Z p1
1
1
1
dp +
−
dp
≤γ+
V
(p)
V
(p)
V
2
2
1 (p)
p
p1
Z2 p3
|V2 (p) − V1 (p)|
≤ 2γ +
dp
V1 (p)V2 (p)
p1
Z p3
1
≤ 2γ + c(γ)
dp
V
(p)V
1
2 (p)
p1

where

1
dp ≤
V2 (p)

p1

Fig. 36. Notation for Case 2 in Proof of Lemma VI.16. We show that the
mapping τ 7→ T (τ ), as defined in Equation (94), is continuous. Velocity paths
V [x] and V [y] are the upper and lower black paths, respectively. Velocity
paths V1 and V2 are depicted along orange and blue arrows, respectively. The
red arcs are maximum acceleration curves. Velocity v2 is the velocity of V2 at
time τ2 . Time τ1 is the time that V1 breaks away from V [y]. The conditions
of Case 2 are depicted: v1 > 0, and τ2 > τ1 . By construction, V1 and V2
disagree only on the interval (p1 , p4 ). Moreover, we bound the absolute value
of the difference between V1 and V2 by c̃(γ), by using maximum acceleration
curves over the interval (p1 , p2 ), depicted in grey.

|V2 (p) − V1 (p)| ≤ c(γ),

p1

v2

Now,

Note that for any p ∈ [pa , pb ]:

Z

v1

c̃(γ)

−L

Fig. 35. Notation for Case 1 in Proof of Lemma VI.16. We show that the
mapping τ 7→ T (τ ), as defined in Equation (94), is continuous. Velocity paths
V [x] and V [y] are the upper and lower black paths, respectively. Velocity
paths V1 and V2 are depicted along orange and blue arrows, respectively.
The red arcs are maximum acceleration curves. Velocity v1 is the velocity
of V1 at time τ1 . Time τ2 is the time that V2 breaks away from V [y]. The
conditions of Case 1 are depicted: v1 > 0, and τ2 < τ1 . By construction,
V1 and V2 disagree only on the interval (p2 , p3 ). Moreover, we bound the
absolute value of the difference between V1 and V2 , by using maximum
acceleration curves over the interval (p2 , p1 ), depicted in grey.

c(γ) :=

V2

V [y]

v2
v1

c(γ)
p1

V1

velocity

V [y]

velocity

V [x]

(98)

1

P2 (ξ) ≥ v1 /2,

1

for sufficiently small γ, i.e., any γ satisfying γ ≤ 3v1 /(2am )
is sufficiently small. Also, note that
Z p2
Z p2
1
1
dp ≤
dp
p1 V1 (p)
p1 V [y]
≤ |τ2 − τ1 | ≤ γ.

can always find γ > 0 such that q(γ) ≤ .
Case 4. Now, suppose

v1 = 0

(99)

τ2 > τ1 .

Then,
|T (τ2 ) − T (τ1 )|

Z pb
Z pb
1
1
dp −
dp
≤ |τ2 − τ1 | +
p1 V1 (p)
p2 V2 (p)
Z p2
Z pb
1
1
1
≤γ+
dp
dp +
−
V2 (p) V1 (p)
p1 V1 (p)
p2
Z p4
|V2 − V1 |
≤ 2γ +
dp
p2 V1 (p)V2 (p)
Z p4
1
≤ 2γ + c̃(γ)
dp
V
(p)V
1
2 (p)
p2
˜
≤ 2γ + 2c̃(γ)L/v 2 := d(γ),

Note that V1 and V2 differ only on interval (p1 , p4 ).We
bound the difference in velocities as follows. The maximum
difference in velocities occurss at position p2 . The maximum
distance between p1 and p2 is equal to vm γ. The maximum
possible velocity (of either V1 or V2 ) at position p2 is, thus,
√
2am vm γ, while the lowest possible velocity is 0. Hence, for
all p ∈ [p1 , p4 ]:
√
|V2 (p) − V1 (p)| ≤ C̃ γ,

1

where the last inequality holds for sufficiently small γ. Since
˜
˜ = 0, we can always find
d(γ)
is continuous and limγ→0 d(γ)
˜ ≤ .
γ > 0 sufficiently small so that d(γ)
Case 3. Now, suppose
v1 = 0
τ2 < τ1 .
Note that V1 and V2 differ only on the interval (p2 , p3 ). On
this interval we bound the difference in velocities by using
Equation (97) and setting v1 equal to 0. Thus, for all p ∈
[p2 , p3 ]:
√
|V2 (p) − V1 (p)| ≤ C γ
√

V1 (p) ≤ V2 (p),

where C := 4am vm . Also, note that Inequality (98) still
holds when v1 is equal to 0. For any γ > 0 the following
holds:
|T (τ2 ) − T (τ1 )|

Z pb
Z pb
1
1
≤ |τ2 − τ1 | +
dp −
dp
p2 V2 (p)
p1 V1 (p)
Z p1
Z pb
1
1
1
≤γ+
dp +
−
dp
V2 (p) V1 (p)
p2 V2 (p)
p1
Z p3
1
1
≤ 2γ +
−
dp
V2 (p) V1 (p)
p1
Z p1 +γ
Z p3
1
1
|V2 − V1 |
≤ 2γ +
−
dp +
dp
V2 (p) V1 (p)
V1 V2
p1
p1 +γ
Z p3
Z p1 +γ 
1
1
1
√
≤ 2γ +
+
dp + C γ
dp
V
V
V
V2
2
1
1
p1
p1 +γ
Z p1 +γ
Z p3
1
1
√
≤ 2γ + 2
dp + C γ
dp
2
V
(p)
V
(p)
1
p1
p1 +γ 1
Z
p3
p
1
√
= 2γ + 2 2γ/am + C γ
dp
2a
(p
− p1 )
m
p1 +γ
√
p − p 
p
C γ
3
1
= 2γ + 2 2γ/am +
log
:= q(γ).
2am
γ
Note that q(γ) is continuous and limγ→0 q(γ) = 0. Thus, we

where C̃ :=

√

2am vm . Note also that for all p ∈ [p2 , p4 ]:

1
.
V1 (p) ≥ V2 (p) ≥ p
2am (p − p2 )
Also, note that Inequality (99) still holds when v1 is equal to
0. Then,

|T (τ2 ) − T (τ1 )|

Z pb
Z pb
1
1
≤ |τ2 − τ1 | +
dp −
dp
p2 V2 (p)
p1 V1 (p)
Z pb
Z p2
1
1
1
dp +
−
dp
≤γ+
V1 (p)
V2 (p) V1 (p)
p2
Zp1 p4
1
1
≤ 2γ +
−
dp
V
(p)
V
2
1 (p)
p2
Z p2 +γ
Z p4
1
1
|V2 − V1 |
≤ 2γ +
−
dp +
dp
V2 (p) V1 (p)
V1 V2
p2
p2 +γ
Z p2 +γ 
Z p4
1
1
1
√
≤ 2γ +
+
dp + C̃ γ
dp
V2
V1
p2
p2 +γ V1 V2
Z p2 +γ
Z p4
1
1
√
≤ 2γ + 2
dp + C̃ γ
dp
2
V
(p)
V
(p)
2
2
p2
p2 +γ
Z p4
p
1
√
≤ 2γ + 2 2γ/am + C̃ γ
dp
2a
(p
− p2 )
m
p2 +γ
√
p − p 
p
C̃ γ
4
2
≤ 2γ + 2 2γ/am +
log
:= q̃(γ).
2am
γ

Note that q̃(γ) is continuous and limγ→0 q̃(γ) = 0. Thus, we
can always find γ > 0 such that q̃(γ) ≤ .
Hence, have shown that T is continuous.

