# Energy-efficient predictive control for connected, automated driving under
localization uncertainty

Eunhyek Joa Eric Yongkeun Choi Francesco Borrelli The authors are with the
Model Predictive Control Lab, Department of Mechanical Engineering, University
of California at Berkeley (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS>fborrelli@berkeley.edu).

###### Abstract

This paper presents a data-driven Model Predictive Control (MPC) for energy-
efficient urban road driving for connected, automated vehicles. The proposed
MPC aims to minimize total energy consumption by controlling the vehicle’s
longitudinal motion on roads with traffic lights and preceding vehicles. Its
terminal cost function and terminal constraints are learned from data, which
consists of the closed-loop state and input trajectories. The terminal cost
function represents the remaining energy-to-spend starting from a given
terminal state. The terminal constraints are designed to ensure that the
controlled vehicle timely crosses the upcoming traffic light, adheres to
traffic laws, and accounts for the preceding vehicles. We validate the
effectiveness of our method through both simulations and real-world vehicle
experiments, demonstrating 19% improvement in average energy consumption
compared to conventional approaches that involve solving a long-horizon
optimal control problem for speed planning and employing a separate controller
for speed tracking.

††publicationid: pubid: 0000–0000/00$00.00 © 2021 IEEE

##  I Introduction

Extensive research has been conducted to study how Connected and Automated
Vehicles (CAVs) can improve our daily life driving experiences, covering areas
from fleet dispatching [1] and routing [2, 3] to urban driving [4] and parking
[5] scenarios. The benefits of CAV technology include improved traffic safety,
better road utilization, and reduced energy consumption and have been well
demonstrated across various driving scenarios [6, 7, 8].

In this paper, we focus on the potential of CAV technology to enhance energy
efficiency for urban road driving. Existing literature primarily explores
optimization-based algorithms that consider vehicle dynamics and powertrain
energy usage to minimize energy consumption while also prioritizing safety,
compliance with traffic laws, and driving comfort. However, the complexity of
incorporating all these factors often renders optimization algorithms too
computationally demanding for real-time applications. Therefore, recent
studies have suggested a hierarchical control architecture to manage this
complexity, dividing responsibilities between an upper layer that plans the
vehicle’s future trajectory and a lower layer that executes the planned
trajectory through direct vehicle control.

The upper layer has been designed using various optimization control methods.
In [9, 10], a Dynamic Programming (DP) method is used for the upper layer
planning algorithms, with Model Predictive Control (MPC) being employed at the
lower layer. Pontryagin’s Minimum Principle (PMP) method has been used in [11,
12] for planning, while convex optimization is used in [13, 14] for their
planning algorithms. The selection of these optimization methods hinges on
finding a balance between the complexity of the model and its computational
feasibility. Notably, these studies often assume ideal conditions, such as
empty roads at the planning stage, forcing the lower layer to deviate the
vehicle’s trajectory in real time based on immediate conditions, such as the
behavior of the vehicle ahead. However, deviations from the planned trajectory
can lead to unforeseen consequences. For instance, the ego vehicle may fail to
pass a traffic light as planned due to unforeseen traffic ahead that was not
accounted for at the planning stage. Moreover, the inherent drawbacks of
having two separate control layers include potential delays and latency, which
can lead to spatial and temporal misalignments and, consequently,
discrepancies in the control layers’ decisions. In response to these
challenges, there have been efforts to unify a control layer employing deep
reinforcement learning (DRL), as indicated by various studies [15, 16, 17].
Despite its potential, the application of DRL faces substantial obstacles in
real-world scenarios, primarily due to its sensitivity to tuning and the
complexities of ensuring safety. Therefore, in this paper, we propose a
unified control architecture that employs a single data-driven MPC that
directly computes longitudinal acceleration from the traffic environment and
route information while ensuring safety.

This paper’s goal is also to address key practical implementation issues. In
particular, achieving high-precision localization typically requires expensive
technologies like differential GPS or LiDAR. Recognizing the cost constraints
associated with the imminent deployment of CAVs, we consider localization
sensor uncertainty in the proposed controller. While existing research often
assumes exact vehicle positioning—an assumption that may not be viable with
the forthcoming wave of CAV technologies that utilize less accurate
localization methods—our approach could be crucial, especially in scenarios
requiring vehicles to stop at signalized intersections without a lead vehicle.
Here, localization inaccuracies could introduce substantial uncertainties. To
address these issues, we robustify the data-driven MPC strategy ensuring it
remains effective despite localization uncertainties.

In our conference paper [18], we proposed a data-driven MPC that solves the
given problem under free-flow conditions and experimentally validates the
algorithm under one scenario. In this paper, we extend the algorithm to handle
realistic urban road scenarios with preceding vehicles and investigate the
proposed algorithm with multiple scenarios in simulations and vehicle
experiments.

Our contributions are summarized as follows:

  * •

We propose a novel data-driven MPC to solve an energy-efficient urban road
driving problem for connected, automated vehicles considering localization
uncertainty.

  * •

We propose a unified MPC controller to replace the conventional hierarchical
approach, effectively mitigating compound errors and latency issues.

  * •

We experimentally demonstrate the energy saving of the proposed algorithm
through vehicle tests where the actual test vehicle is controlled to complete
a given route within a user-defined duration under localization uncertainty
while interacting with a virtual front vehicle and deterministic traffic
lights behavior.

Notation: Throughout the paper, we use the following notation.
$\mathbf{0}^{n\times m}$ represents an $n$-by-$m$ zero matrix. The positive
semi-definite matrix $P$ is denoted as $P\succeq 0$. The Minkowski sum of two
sets is denoted as
$\mathcal{X}\oplus\mathcal{Y}=\\{x+y:x\in\mathcal{X},y\in\mathcal{Y}\\}$. The
Pontryagin difference between two sets is defined as
$\mathcal{X}\ominus\mathcal{Y}=\\{x\in\mathcal{X}:x+y\in\mathcal{X},\forall
y\in\mathcal{Y}\\}$. The m-th column vector of a matrix $H$ is denoted as
$[H]_{m}$. The m-th component of a vector $h$ is $[h]_{m}$. The notation
$x_{l:m}$ means the sequence of the variable $x$ from time step $l$ to time
step $m$.

##  II Problem Setup

In this section, we introduce the problem setup of energy-efficient urban
driving for connected, automated vehicles under localization uncertainty. We
make the following assumptions.

  * •

Route Availability: We assume the existence of a predefined route from a
starting point to a goal point, which is typical for automated vehicles.

  * •

Deterministic Traffic Lights: We assume that all traffic light cycles are
deterministic. This assumption is valid when adaptive traffic control systems
are either not equipped or deactivated. If traffic light cycles vary, their
stochastic nature can be considered, as discussed in [19].

  * •

V2I Communication Infrastructure: The infrastructure provides essential data
such as distance and traffic flow speed between consecutive traffic lights
through V2I communication. This information enables the calculation of the
time required to traverse a traffic light from its preceding one. Such data
aligns with the V2X communication standard in the US, as detailed in [20].

  * •

Negligible Road Grade Impact: We assume that the road grade along the route is
sufficiently small to disregard gravitational potential energy. If the road
grade is known, the associated gravitational potential energy could be
factored into the terminal cost as discussed in Sec. IV-B.

###  II-A Vehicle Model, Measurement Model, and Observer

We model vehicle longitudinal dynamics as a double integrator:

$\displaystyle\mathbf{x}(t)=\begin{bmatrix}s(t)&v_{x}(t)\end{bmatrix}^{\top},~{}u(t)=a_{x}(t),$
(1) $\displaystyle\dot{\mathbf{x}}(t)=\begin{bmatrix}0&1\\\
0&0\end{bmatrix}\mathbf{x}(t)+\begin{bmatrix}0\\\ 1\end{bmatrix}u(t),$

where $s$ represents a longitudinal position along the centerline of the given
route, and $v_{x}$ and $a_{x}$ are the ego vehicle’s speed and acceleration,
respectively. Throughout the paper, we will refer to $s$ as the position for
brevity. Forces due to road grade, air drag, and rolling resistance are not
included in (1) because $a_{x}$ is net longitudinal acceleration. We regard
controlling the vehicle under those forces as the task of the actuator-level
controller.

We discretize the dynamics (1) as

$\displaystyle\mathbf{x}_{k}=\begin{bmatrix}s_{k}&v_{x,k}\end{bmatrix}^{\top},~{}u_{k}=a_{x,k},$
(2) $\displaystyle\mathbf{x}_{k+1}=\mathbf{A}\mathbf{x}_{k}+\mathbf{B}u_{k},$

where $\mathbf{x}_{k}$ denotes the state, and $u_{k}$ denotes the input at
time step $k$. $s_{k}$ represents the position, while $v_{x}$ and $a_{x}$
correspond to the vehicle’s longitudinal speed and acceleration at time step
$k$. The discretization sampling time $T_{s}$ is 1 sec.

At time step $k$, we measure the states from sensors as:

$\displaystyle\mathbf{y}_{k}=\mathbf{C}\mathbf{x}_{k}+\mathbf{D}w_{k}=\begin{bmatrix}1&0\\\
0&1\end{bmatrix}\mathbf{x}_{k}+\begin{bmatrix}1\\\ 0\end{bmatrix}w_{k},$ (3)

where $\mathbf{y}_{k}$ denotes a measurement at time step $k$, which consists
of the position $s_{k}$ and the speed $v_{x,k}$. The position $s_{k}$ is
measured by a localization module, and $w_{k}$ represents its localization
uncertainty. The speed $v_{x,k}$ is measured by vehicle wheel encoders. We
assume the measurement noises of the vehicle speed are small enough to be
neglected.

We assume that the longitudinal localization uncertainty $w$ is a random
variable with distribution $p_{w}(w)$ and contained in a segment $\mathcal{W}$
as follows:

$\displaystyle w_{k}\sim p_{w}(w),~{}w_{k}\in\mathcal{W}.$ (4)

Let $w_{\text{min}}$ and $w_{\text{max}}$ denote the extremes of the segment
$\mathcal{W}$. As the $\mathcal{W}\subset\mathbb{R}$, $\mathcal{W}$ can be
written as $\\{w~{}|~{}w_{\text{min}}\leq w\leq w_{\text{max}}\\}$. In
practice, the localization uncertainty may be designed as a Gaussian
distribution, which is not bounded. In this case, a high confidence interval
can be used to approximate $\mathcal{W}$.

We design a discrete state observer [21] with a predictor to estimate the
state $\mathbf{x}_{k}$ as follows:

$\displaystyle\hat{\mathbf{x}}_{0}=\mathbf{y}_{0},$ (5)
$\displaystyle\hat{\mathbf{x}}_{k+1}=\mathbf{A}\hat{\mathbf{x}}_{k}+\mathbf{B}u_{k}+\begin{bmatrix}L&0\\\
0&1\end{bmatrix}\biggr{(}\mathbf{y}_{k+1}-\mathbf{C}(\mathbf{A}\hat{\mathbf{x}}_{k}+\mathbf{B}u_{k})\biggr{)},$
$\displaystyle~{}~{}~{}~{}~{}~{}=\mathbf{A}\hat{\mathbf{x}}_{k}+\mathbf{B}u_{k}+\mathbf{D}n_{k},$

where
$\hat{\mathbf{x}}_{k}=\begin{bmatrix}\hat{s}_{k}&\hat{v}_{x,k}\end{bmatrix}^{\top}$
is an estimated state, $L$ is an observer gain, and
$n_{k}=L(s_{k}-\hat{s}_{k})+Lw_{k+1}$, which is a lumped noise. Note that as
the speed measurement is accurate, the observer (5) is designed to set the
current speed estimate to the current speed measurement, i.e.,
$\hat{v}_{x,k}=\begin{bmatrix}0&1\end{bmatrix}y_{k}$. On the other hand, as
the position measurement is not accurate, the observer (5) is designed to
suppress the localization uncertainty.

######  Proposition 1.

[18, Prop. 1] Let $\Delta s_{k}=s_{k}-\hat{s}_{k}$. Then, $\Delta
s_{k}\in\mathcal{W}$ and $n_{k}\in 2L\mathcal{W}$ for all realizations of
noise that satisfies (4), i.e., $\forall w_{k}\sim
p_{w}(w),~{}w_{k}\in\mathcal{W}$.

###### Proof.

From (2) and (5), we have that:

$\displaystyle\hat{s}_{0}=\begin{bmatrix}1&0\end{bmatrix}y_{0},~{}\Delta
s_{0}=-w_{0},$ (6) $\displaystyle\Delta s_{k+1}=(1-L)\Delta s_{k}-Lw_{k+1}.$

Proof by induction. First, $\Delta s_{0}=-w_{0}\in\mathcal{W}$. Now, let
$\Delta s_{k}\in\mathcal{W}$. Since $\Delta s_{k+1}=(1-L)\Delta
s_{k}-Lw_{k+1}\in(1-L)\mathcal{W}\oplus L\mathcal{W}=\mathcal{W}$, $\Delta
s_{k+1}\in\mathcal{W}$. Thus, $\Delta s_{k}\in\mathcal{W}$ for all $k\geq 0$
by induction. Furthermore, $n_{k}=L\Delta s_{k}+Lw_{k+1}\in L\mathcal{W}\oplus
L\mathcal{W}=2L\mathcal{W}$. ∎

From Proposition 1, the lumped noise $n_{k}$ in (5) is a random variable with
bounded support $2L\mathcal{W}$. Let $p_{n}(n)$ denote the corresponding
probability density function. Then, the lumped noise $n_{k}$ is a random
variable with the probability density function $p_{n}(n)$ and bounded support
$2L\mathcal{W}$:

$\displaystyle n_{k}\sim p_{n}(n),~{}n_{k}\in 2L\mathcal{W}.$ (7)

###  II-B Energy Model

We define the energy $E$ as the sum of the energy stored in a battery/fuel
tank and the kinetic energy $E_{\text{kin}}$ as:

$\displaystyle E(t)=E_{\text{stor}}(t)+E_{\text{kin}}(t).$ (8)

The energy consumption at time step $k$ is defined as the change in energy
between time step $k$ and $k+1$ as follows:

$\displaystyle\Delta E_{k}=E(kT_{s})-E((k+1)T_{s}).$ (9)

We model $\Delta E_{k}$ (9) as a parameterized function
$\ell(\mathbf{x}_{k},u_{k})$ which is defined as follows:

$\displaystyle\ell(\mathbf{x}_{k},u_{k})=\begin{bmatrix}v_{x,k}&u_{k}&1\end{bmatrix}\mathbf{P}\begin{bmatrix}v_{x,k}\\\
u_{k}\\\ 1\end{bmatrix},$ (10)

where $\mathbf{P}$ is a symmetric matrix in $\mathbb{R}^{3\times 3}$, which
includes parameters. Additionally, $\mathbf{P}$ is positive semidefinite,
ensuring that the parametrized function remains nonnegative, in accordance
with the nonnegative nature of energy consumption in (9). To obtain the
parameters in $\mathbf{P}$, we collect the energy consumption data from our
test vehicle during urban road driving. After collecting data, we solve the
following regression problem to obtain the parameters in $\mathbf{P}$.:

$\displaystyle\min_{\mathbf{P}\in\mathbb{R}^{3\times
3}}\sum_{k=0}^{T_{\text{data}}}\lVert\Delta
E_{k}-\ell(\mathbf{x}_{k},u_{k})\rVert_{2}^{2}$ (11)
$\displaystyle~{}~{}~{}\text{s.t.,}~{}~{}\mathbf{P}\succeq 0,$

where $T_{\text{data}}$ is the end time of the data. This is a semidefinite
programming that can be solved with convex optimization solvers. It is
noteworthy that our parameterized cost does not depend on the position whose
actual value cannot be obtained. Thus, the following holds:

$\displaystyle\ell(\mathbf{x}_{k},u_{k})=\ell(\hat{\mathbf{x}}_{k},u_{k}).$
(12)

###  II-C Prediction Module of the Front Vehicle

Let $s^{pv}_{k}$ and $v^{pv}_{k}$ denote the longitudinal position and speed
of the preceding vehicle at time step $k$, respectively. $d_{k}$ denotes the
distance between the preceding vehicle and the ego vehicle, which can be
derived as $s^{pv}_{k}-s_{k}$. At time step $k$, $d_{k}$ and $v^{pv}_{k}$ are
measured by proximity sensors such as radars and LiDARs. We assume that the
measurement noises associated with these sensors are negligible.

In this paper, we assume that there is a given prediction module that predicts
the future behavior of $v^{pv}_{k}$. Let $\hat{v}^{pv}_{i|k}$ denote the
preceding vehicle’s speed at time step $k+i$ predicted at time step $k$. The
prediction module computes a $T_{p}$-step predicted state sequence for
$v^{pv}$ denoted by $\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{T_{p}}$, with initial
conditions $\hat{v}^{pv}_{0|k}=v^{pv}_{k}$.

Once the predicted speed sequences of the preceding vehicle,
$\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{T_{p}}$, the distance between the preceding
vehicle and the ego vehicle at time step $k$, $d_{k}$, and the ego vehicle’s
position estimate at time step $k$, $\hat{s}_{k}$, are available, the
predicted position sequences of the preceding vehicle,
$\\{\hat{s}^{pv}_{i|k}\\}_{i=0}^{T_{p}}$ can be calculated using the following
dynamics:

$\displaystyle\hat{s}^{pv}_{i+1|k}=\hat{s}^{pv}_{i|k}+\hat{v}^{pv}_{i|k}T_{s},$
(13) $\displaystyle\hat{s}^{pv}_{0|k}=\hat{s}_{k}+d_{k}.$

In Sec. IV-C, we need the predicted behavior of the preceding vehicle beyond
the maximum prediction horizon $T_{p}$. In this case, we extrapolate the
prediction using a constant speed assumption as follows:

$\displaystyle\hat{s}^{pv}_{i+1|k}=\hat{s}^{pv}_{i|k}+\hat{v}^{pv}_{T_{p}|k}T_{s},$
(14) $\displaystyle\hat{v}^{pv}_{i|k}=\hat{v}^{pv}_{T_{p}|k},$
$\displaystyle\forall i\geq T_{p}.$

In the literature, various methodologies have been employed to analyze the
behavior prediction of the leading vehicle over a limited forecast horizon.
These approaches include scenario-based [22], physics-based [23], pattern-
based [24], and planning-based [25]. Interested readers can refer to [26].

###  II-D Collision Avoidance Constraints

To avoid a collision with the preceding vehicle, the system (2) is subject to
the following constraints:

$\displaystyle\mathbf{x}_{k}\in\mathcal{C}(s^{pv}_{k},v^{pv}_{k})=\\{\mathbf{x}|s^{pv}_{k}+v^{pv}_{k}\cdot\mathrm{TTC}\geq\begin{bmatrix}1&\mathrm{TTC}\end{bmatrix}\mathbf{x}+d_{0}\\},$
(15)

where $d_{0}$ denotes the minimum distance considering the size of vehicles,
and $\mathrm{TTC}$ denotes time-to-collision (TTC) [27]. This constraint (15)
imposes that the distance between the ego vehicle and the preceding vehicle
maintains at least a $1$ second TTC gap. Throughout the paper, $d_{0}=5\ m$
and $\mathrm{TTC}=1\ sec$.

###  II-E State and Input Constraints

The system (2) is subject to the following constraints:

$\displaystyle\mathbf{x}_{k}\in\mathcal{X}=\\{\mathbf{x}~{}|~{}0\leq\begin{bmatrix}0&1\end{bmatrix}\mathbf{x}\leq
v_{x,\max}\\},$ (16) $\displaystyle
u_{k}\in\mathcal{U}=\\{u~{}|~{}a_{x,\min}\leq u\leq a_{x,\max}\\},$
$\displaystyle\forall k\geq 0,~{}\forall w_{k}\in\mathcal{W},$

where $v_{x,\max}$ is the maximum allowable speed, and $a_{x,\min/\max}$ is
the minimum/maximum acceleration. $\mathcal{X}$ is a convex polyhedron, and
$\mathcal{U}$ is a polytope. Note that as the state constraints are imposed on
the vehicle speed, $\mathbf{x}_{k}\in\mathcal{X}$ is identical to
$\hat{\mathbf{x}}_{k}\in\mathcal{X}$.

###  II-F Traffic Light Crossing Time Constraints

We impose traffic light crossing time constraints on the system (2) to ensure
that the vehicle crosses each traffic light within a predetermined time step.
This time step is chosen to avoid disrupting traffic flow by the controlled
vehicle. To find such a time step, we utilize a high-level green wave search
module from [28] with one modification: the cost function is adjusted to be
quadratic, penalizing a deviation between the actual traffic flow speed, which
is available through V2I communication, and the average planned speed for each
road segment.

######  Remark 1.

According to [29, Table 1], reducing vehicle speed is correlated with lower
energy consumption. Consequently, in the absence of constraints mandating
adherence to traffic flow speed, the most energy-efficient approach for
controlling the vehicle is to maintain a low speed. However, this practice can
adversely affect traffic flow, which is deemed socially unacceptable. To
mitigate this issue, we propose implementing traffic light crossing time
constraints aimed at preventing the ego vehicle from disrupting traffic flow
while minimizing its energy consumption.

Let $T_{l}$ denote the predetermined time step required to cross the $l$-th
traffic light positioned at $s_{\mathrm{tl},l}$. The system (2) is subject to
the following constraints:

$\displaystyle\mathbf{x}_{k}\in\mathcal{T}_{l}=\\{\mathbf{x}~{}|~{}s_{\mathrm{tl},l}\leq\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\\},~{}\forall
k\geq T_{l},$ (17) $\displaystyle l\in\\{1,\cdots,N_{\mathrm{tl}}\\},$

where $N_{\mathrm{tl}}$ denotes the number of traffic lights on the given
route. Note that this constraint (17) does not consider traffic light rules;
therefore, we need the associated traffic light rule constraints described in
the following section.

###  II-G Traffic Light Rule Constraints

We impose traffic light rule constraints to ensure the vehicle does not run a
red light. To do that, given state $\mathbf{x}_{k}$, we find the nearest
upcoming traffic light along the given route as follows:

$\displaystyle\min_{l}~{}\lVert\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}_{k}-s_{\mathrm{tl},l}\rVert$
(18)
$\displaystyle\,~{}\text{s.t.,}~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}_{k}\leq
s_{\mathrm{tl},l},$
$\displaystyle\,~{}~{}~{}~{}~{}~{}~{}l\in\\{1,\cdots,N_{\mathrm{tl}}\\}.$

After solving the problem (18), we obtain the index of the nearest upcoming
traffic light $l^{\star}(\mathbf{x}_{k})$. Let $c_{l,k}$ denote the traffic
light phase of the $l$-th traffic light at time step $k$. Then, if
$c_{l^{\star}(\mathbf{x}_{k}),k}$ is red, the traffic light rule constraint is
imposed and written as follows:

$\displaystyle\mathbf{x}_{k}\in\mathcal{L}(s_{\mathrm{tl},l^{\star}(\mathbf{x}_{k})})=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\leq
s_{\mathrm{tl},l^{\star}(\mathbf{x}_{k})}\\}.$ (19)

###  II-H Energy-efficient Urban Road Driving for Connected, Automated
Vehicles under Localization Uncertainty

The urban driving problem for connected, automated vehicles, aimed at
minimizing energy consumption while accounting for localization uncertainty,
is a stochastic optimization problem due to stochastic localization
uncertainty (4). Specifically, it can be formulated as follows:

$\displaystyle
J^{\star}(\mathbf{x}_{S},\\{\hat{s}^{pv}_{i|0}\\}_{i=0}^{T_{N_{\mathrm{tl}}}},\\{\hat{v}^{pv}_{i|0}\\}_{i=0}^{T_{N_{\mathrm{tl}}}})=$
(20)
$\displaystyle\min_{\Pi(\cdot)}~{}\,\mathbb{E}_{w_{0:T_{N_{\mathrm{tl}}}}}\Biggr{[}\sum_{k=0}^{T_{N_{\mathrm{tl}}}}\ell(\hat{\mathbf{x}}_{k},~{}\pi_{k}(\hat{\mathbf{x}}_{k}))\Biggr{]}$
$\displaystyle~{}\textnormal{s.t.,}~{}~{}\mathbf{x}_{k+1}=\mathbf{A}\mathbf{x}_{k}+\mathbf{B}\pi_{k}(\hat{\mathbf{x}}_{k}),$
$\displaystyle\qquad~{}\mathbf{y}_{k}=\mathbf{C}\mathbf{x}_{k}+\mathbf{D}w_{k},~{}w_{k}\sim
p_{w}(w),$
$\displaystyle\qquad~{}\hat{\mathbf{x}}_{k+1}=\mathbf{A}\hat{\mathbf{x}}_{k}+\mathbf{B}u_{k}$
$\displaystyle\qquad\qquad~{}~{}~{}~{}+\begin{bmatrix}L&0\\\
0&1\end{bmatrix}\biggr{(}\mathbf{y}_{k+1}-\mathbf{C}(\mathbf{A}\hat{\mathbf{x}}_{k}+\mathbf{B}u_{k})\biggr{)},$
$\displaystyle\qquad~{}\mathbf{x}_{0}=\mathbf{x}_{S},~{}\hat{\mathbf{x}}_{0}=\mathbf{y}_{0},$
$\displaystyle\qquad~{}\mathbf{x}_{k}\in\mathcal{X},~{}\forall
w_{k}\in\mathcal{W},$
$\displaystyle\qquad~{}\mathbf{x}_{k}\in\mathcal{T}_{l},~{}\forall k\geq
T_{l},~{}\forall w_{k}\in\mathcal{W},$
$\displaystyle\qquad~{}\mathbf{x}_{k}\in\mathcal{L}(s_{\mathrm{tl},l^{\star}(\mathbf{x}_{k})}),\text{if
}c_{l^{\star}(\mathbf{x}_{k}),k}=\text{red},\forall w_{k}\in\mathcal{W},$
$\displaystyle\qquad~{}\hat{\mathbf{x}}_{k}\in\mathcal{C}(\hat{s}^{pv}_{k|0},\hat{v}^{pv}_{k|0}),~{}\pi_{k}(\hat{\mathbf{x}}_{k})\in\mathcal{U},~{}\forall
w_{k}\in\mathcal{W},$
$\displaystyle\qquad~{}k\in\\{0,...,T_{N_{\mathrm{tl}}}-1\\},~{}l\in\\{1,\cdots,N_{\mathrm{tl}}\\},$

where $T_{N_{\mathrm{tl}}}$ is the task horizon, which denotes the
predetermined time step to cross the last traffic light. The cost function is
an expected sum of the regressed energy consumption stage cost
$\ell(\cdot,\cdot)$ in (11) evaluated for the estimated state trajectory. From
(12), this cost is identical to the cost evaluated for the actual state
trajectory. We point out that as the system (5) is uncertain, the optimal
control problem (20) consists of finding state feedback policies
$\Pi(\cdot)=\\{\pi_{0}(\cdot),\pi_{1}(\cdot),...,\pi_{T_{f}-1}(\cdot)\\}$.
$\hat{\mathbf{x}}_{k}$ is the argument of the control input policy
$\pi_{k}(\cdot)$.

##  III Solution approach to problem (20)

There are four main challenges associated with solving (20):

  1. (C1)

Optimizing over control policies $\Pi(\cdot)$ presents an infinite dimensional
optimization problem.

  2. (C2)

The computational demands for solving (20) become significant when
$T_{N_{\mathrm{tl}}}\gg 0$.

  3. (C3)

The problem (20) requires the prediction of the position
$\\{\hat{s}^{pv}_{i|0}\\}_{i=0}^{T_{N_{\mathrm{tl}}}}$ and the speed
$\\{\hat{v}^{pv}_{i|0}\\}_{i=0}^{T_{N_{\mathrm{tl}}}}$ of the preceding
vehicle for entire task horizon.

  4. (C4)

Minimizing the expected cost in (20) involves nested multivariate integrals.

To address (C1), we approximate the control policy by a constant control
input, i.e., $\pi_{k}(\cdot)=u_{k}$.

To address (C2)-(C3), we adopt two strategies. First, we divide the original
problem in (20) into small sub-problems of crossing only the upcoming traffic
light within a predetermined time at each time step and solve each sub-problem
until the vehicle reaches the end of the predetermined route. For brevity, let
$s_{\mathrm{tl}}=s_{\mathrm{tl},l^{\star}(\hat{\mathbf{x}}_{k})}$ and
$l^{\star}=l^{\star}(\hat{\mathbf{x}}_{k})$ as we now deal with a single,
upcoming traffic light. Second, we solve a simpler constrained OCP with
prediction horizon $N\ll T_{N_{\mathrm{tl}}}$ in a receding horizon fashion.
Specifically, we design an MPC controller of the following form:

$\displaystyle
J_{\text{MPC}}(\hat{\mathbf{x}}_{k},\\{\hat{s}^{pv}_{i|k}\\}_{i=0}^{N},\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{N})=$
(21)
$\displaystyle\min_{\begin{subarray}{c}u_{0:N-1|k}\end{subarray}}~{}\,\sum_{i=0}^{N-1}\ell(\bar{\mathbf{x}}_{i|k},~{}u_{i|k})+\mathbb{E}_{n_{0:N-1}}\Bigr{[}V_{f}(\hat{\mathbf{x}}_{N|k})\Bigr{]}$
$\displaystyle~{}~{}~{}~{}\textnormal{s.t.,}\,~{}~{}~{}~{}\hat{\mathbf{x}}_{i+1|k}=\mathbf{A}\hat{\mathbf{x}}_{i|k}+\mathbf{B}u_{i|k}+\mathbf{D}n_{i},$
$\displaystyle\qquad\qquad~{}\bar{\mathbf{x}}_{i+1|k}=\mathbf{A}\bar{\mathbf{x}}_{i|k}+\mathbf{B}u_{i|k},$
$\displaystyle\qquad\qquad~{}\hat{\mathbf{x}}_{0|k}=\bar{\mathbf{x}}_{0|k}=\hat{\mathbf{x}}_{k},~{}n_{i}\sim
p_{n}(n),$
$\displaystyle\qquad\qquad~{}\hat{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{i|k}\in\mathcal{U},$
$\displaystyle\qquad\qquad~{}\hat{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}^{pv}_{i|k},\hat{v}^{pv}_{i|k}),~{}\forall
n_{i}\in 2L\mathcal{W},$
$\displaystyle\qquad\qquad~{}\hat{\mathbf{x}}_{N|k}\in\mathcal{X}_{f},~{}\forall
n_{i}\in 2L\mathcal{W},$
$\displaystyle\qquad\qquad~{}\hat{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus\mathbf{D}\mathcal{W},~{}\text{if
}c_{l^{\star},k+i}=\text{red},$
$\displaystyle\qquad\qquad~{}i\in\\{0,\cdots,N-1\\},$

where $V_{f}(\cdot)$ denotes a terminal cost, which represents the remaining
energy-to-spend for the vehicle to cross the upcoming traffic light,
$\bar{x}_{i|k}$ is a nominal state at predicted time step $k+i$, and
$\mathcal{X}_{f}$ is a terminal set to satisfy the corresponding traffic light
crossing time constraint in (17). Considering the localization uncertainty in
(4) and Proposition 1, the constraint
$\hat{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus\mathbf{D}\mathcal{W}$
implies ${\mathbf{x}}_{k+i}\in\mathcal{L}(s_{\mathrm{tl}})$. This implies that
if the upcoming traffic light is red, the constraint prohibits the ego vehicle
from crossing it.

We construct the terminal cost $V_{f}(\cdot)$ and the terminal set
$\mathcal{X}_{f}$ in a data-driven way, which will be detailed in Sec. IV. The
expected stage cost in (20) becomes deterministic as described in (21). This
is because $\pi_{i|k}(\hat{\mathbf{x}}_{i|k})=u_{i|k}$, and the stage cost in
(11) does not depend on the position. Thus,
$\ell(\hat{\mathbf{x}}_{i|k},u_{i|k})=\ell(\bar{\mathbf{x}}_{i|k},u_{i|k})$,
making it deterministic.

(C4) is partially addressed by designing the MPC controller (21) as the nested
expected cost in (20) reduces to a single expected terminal cost in (21).
However, it is still computationally intractable for real-time applications.
This is because evaluating the expected terminal cost in (21) involves a
multivariate integral.

To address this issue, we approximate the expected terminal cost with its
tractable approximation introduced in [30, Sec.III.B.1]. To apply the
approximation from [30, Sec.III.B.1], we first reformulate the expectation
with respect to the random variables $n_{0:N-1}$ into an expectation with
respect to one random variable. Using the dynamics in (21), we obtain the
following equation:

$\hat{\mathbf{x}}_{N|k}=\bar{\mathbf{x}}_{N|k}+\sum_{i=0}^{N-1}Dn_{i}=\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N},$
(22)

where $\mathbf{v}_{N}=\sum_{i=0}^{N-1}Dn_{i}$. From (7), we can derive that
$\mathbf{v}_{N}$ has the bounded support $2LN\mathcal{W}$. Let $p_{v}(v)$
denote the probability density function of $\mathbf{v}_{N}$. $p_{v}(v)$ can be
calculated explictly by following [31, eq. (14), (15)] when $p_{w}(w)$ is
Gaussian. In practice, differential GPS, which offers centimeter-level
accuracy in positioning, can be employed to calculate $\mathbf{v}_{N}$ and
approximate its density function $p_{v}(v)$. In summary, $\mathbf{v}_{N}$ is a
random variable with the probability density function $p_{v}(v)$ and bounded
support $2LN\mathcal{W}$ as follows:

$\displaystyle\mathbf{v}_{N}\sim p_{v}(v),~{}\mathbf{v}_{N}\in
2LN\mathcal{W}.$ (23)

Utilizing the $\mathbf{v}_{N}$, we can express the expected terminal cost in
(21) as follows:

$\mathbb{E}_{n_{0:N\text{-}1}}[V_{f}(\hat{\mathbf{x}}_{N|k})]=\mathbb{E}_{\mathbf{v}_{N}}[V_{f}(\hat{\mathbf{x}}_{N|k})]=\mathbb{E}_{\mathbf{v}_{N}}[V_{f}(\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N})],$
(24)

where the last equation follows from (22).

In [30, Sec.III.B.1], the expected terminal cost in (24) is approximated with
a convex combination of the $V_{f}(\cdot)$ values at multiple number of points
as follows:

$\mathbb{E}_{\mathbf{v}_{N}}\Bigr{[}V_{f}(\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N})\Bigr{]}\simeq\sum_{m=1}^{M}p_{m}V_{f}(\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N}^{m}),$
(25)

where $\mathbf{v}_{N}^{m}$ denotes $m$-th discretized noises, $M$ represents
the number of the discretized noises, and $p_{m}$ is a convex coefficient of
each $\mathbf{v}_{N}^{m}$. The discretized noises
$\mathbf{v}_{N}^{m},~{}m=\\{1,\cdots,M\\}$ consist of the recorded noises and
the vertices of the bounded support $2LN\mathcal{W}$ as shown in Fig. 1.

Figure 1: The black line is the bounded support $2LN\mathcal{W}$. All dots are
the discretized noises. The blue dots denote the recorded noises while the red
dots denote vertices of the bounded support $2LN\mathcal{W}$.

The calculation of $p_{m}$ for each $\mathbf{v}_{N}^{m}$ is conducted before
solving the MPC (or offline). The details of the calculation can be found in
[30, Appendix.A.1-2].

##  IV Data-driven Design of $V_{f}(\cdot)$ and $\mathcal{X}_{f}$

In this section, we introduce a data-driven way to construct the terminal cost
function $V_{f}(\cdot)$ and the terminal set $\mathcal{X}_{f}$. The terminal
cost function $V_{f}(\cdot)$ represents the remaining energy-to-spend until
the vehicle crosses the upcoming traffic light. The terminal set
$\mathcal{X}_{f}$ should be designed to satisfy the corresponding traffic
light crossing time constraints in (17), which impose the vehicle state to
cross the upcoming traffic light within the predetermined time step.

The conventional approach to designing $V_{f}(\cdot)$ and $\mathcal{X}_{f}$
relies on control Lyapunov functions (CLFs) [32, 33, 34] and dynamic
programming methods [35, 36, 37].

The process of identifying a suitable control Lyapunov function requires
assuming an existing control law, which may not inherently prioritize
minimizing the sum of stage costs, potentially resulting in suboptimal closed-
loop performance.

In contrast, dynamic programming methods are employed to compute the optimal
value function $V_{f}(\cdot)$ and its associated domain $\mathcal{X}_{f}$.
However, dynamic programming requires discretizing the state and input spaces
into grids, restricting control inputs to discrete elements. This constraint
is undesirable for automated vehicles, particularly in terms of passenger
comfort [38]. Moreover, as the number of states and grid sizes increases,
computational costs increase exponentially.

To address these drawbacks, we propose an alternative solution outlined in
[30], which leverages a data-driven approach. Unlike traditional methods, this
approach eliminates the need for existing control laws or state and input
space discretization. The result is nearly optimal closed-loop performance and
reduced computational burden.

This section is structured as follows. First, we introduce the dataset
generation process following [30, Sec. IV. A & Appendix. B]. Second, we
present an approximation method for the terminal cost function $V_{f}(\cdot)$
and discuss its data-driven construction, following [30, Sec. IV. C]. Third,
we explain the data-driven construction of $\mathcal{X}_{f}$ following [30,
Sec. IV. C].

###  IV-A Data: State-Input pairs

We generate the dataset through two processes, initialization and
augmentation.

######  Remark 2.

The data set is generated through simulation in this paper at Sec. VI-E.
However, it is worth mentioning that data generation is not limited to
simulation; it can also be achieved through closed-loop testing.

####  IV-A1 Initialization

To initialize the dataset, we simulate a simple scenario rather than a complex
urban road driving scenario. In this scenario, the vehicle with zero initial
speed is on a single road segment with just one traffic light, located 200
meters away. The goal for the vehicle is to cross the traffic light within a
$20$-second. We design the following suboptimal planner for initialization to
solve the given problem starting from the initial estimate
$\hat{\mathbf{x}}_{0}$:

$\begin{split}&\min_{\bar{u}^{0}_{0:T}}~{}~{}0\\\
&~{}\textnormal{s.t.,}~{}~{}\bar{\mathbf{x}}^{0}_{k+1}=A\bar{\mathbf{x}}^{0}_{k}+B\bar{u}^{0}_{k},\\\
&~{}\qquad\,\bar{\mathbf{x}}^{0}_{0}=\hat{\mathbf{x}}^{0}_{0},\\\
&~{}\qquad\,\bar{\mathbf{x}}^{0}_{k+1}\in\mathcal{X},~{}\bar{u}^{0}_{k}\in\mathcal{U},\\\
&~{}\qquad\,\bar{\mathbf{x}}^{0}_{T+1}\in\mathcal{O}\ominus\mathbf{D}\mathcal{W},\\\
&~{}\qquad\,k\in\\{0,\cdots,T\\},\end{split}$ (26)

where the superscript 0 is to denote an initialization, $T=20$, and
$\mathcal{O}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\geq
s_{\mathrm{tl}}^{0},~{}\mathbf{x}\in\mathcal{X}\\}$, which represents the
region behind the traffic light while satisfying the state constraint.

######  Remark 3.

This planner is a tube MPC [39] with horizon length $N=T+1$ and a feedback
control gain $K=0$, i.e., the tube MPC policy
$\pi_{\mathrm{tube}}(\mathbf{x}_{k})=\bar{u}^{\star}_{k}$. Moreover,
$\mathbf{x}_{k}\in\bar{\mathbf{x}}_{k}\oplus\mathbf{D}\mathcal{W},~{}k\in\\{0,\cdots,T\\}$,
which represents that $\mathbf{D}\mathcal{W}$ is a tube. This is because
$\mathbf{x}_{0}\in\hat{\mathbf{x}}_{0}\oplus\mathbf{D}\mathcal{W}$ by
Proposition 1, and there is no uncertainty in dynamics (2). As the state
constraint is imposed on the speed only as in (16),
$\mathcal{X}=\mathcal{X}\ominus\mathbf{D}\mathcal{W}$.

By [39, Prop.1], the state $\mathbf{x}_{k}$ of the closed-loop system (2)
under the control policy $u_{k}=\bar{u}_{k}^{\star}$ always satisfies the
state constraint $\mathcal{X}$ while $\bar{u}_{k}^{\star}\in\mathcal{U}$.
Moreover, the terminal state ${\mathbf{x}}_{T+1}\in\mathcal{O},\forall
w_{k}\in\mathcal{W}$.

After solving the problem (26), we collect the optimal input sequence
$\bar{u}_{0:T}^{\star}$ and the associated state
$\bar{\mathbf{x}}_{0:T}^{\star}$. Subsequently, we construct the initial data
set as follows:

$\mathbf{X}^{0}=\begin{bmatrix}(\bar{\mathbf{x}}^{\star}_{0}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{0}+\mathbf{D}w_{\text{min}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{0}+\mathbf{D}w_{\text{max}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{1}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{1}+\mathbf{D}w_{\text{min}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{1}+\mathbf{D}w_{\text{max}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
\vdots\\\
(\bar{\mathbf{x}}^{\star}_{T}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{T}+\mathbf{D}w_{\text{min}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(\bar{\mathbf{x}}^{\star}_{T}+\mathbf{D}w_{\text{max}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\end{bmatrix}^{\top},~{}\mathbf{U}^{0}=\begin{bmatrix}\bar{u}_{0}^{\star}\\\
\bar{u}_{0}^{\star}\\\ \bar{u}_{0}^{\star}\\\ \bar{u}_{1}^{\star}\\\
\bar{u}_{1}^{\star}\\\ \bar{u}_{1}^{\star}\\\ \vdots\\\ \bar{u}_{T}^{\star}\\\
\bar{u}_{T}^{\star}\\\ \bar{u}_{T}^{\star}\end{bmatrix}^{\top},$ (27)

where $\mathbf{X}^{0}$ and $\mathbf{U}^{0}$ represent an initial state and an
initial input data matrices, respectively. Note that we transform the
coordinate system of the state by subtracting the position of the upcoming
traffic light $s_{\mathrm{tl}}^{0}=200$, with the element now indicating the
remaining distance to the traffic light. Here, $\mathbf{X}^{0}$ consists of
the associated state $\bar{\mathbf{x}}^{\star}$ and the vertices of the set
$\bar{\mathbf{x}}^{\star}\oplus\mathbf{D}\mathcal{W}$, which denotes the tube
around the $\bar{\mathbf{x}}^{\star}$. $\mathbf{U}^{0}$ contains the control
input calculated by the tube MPC policy $\pi_{\mathrm{tube}}(\cdot)$ for each
corresponding state in $\mathbf{X}^{0}$. Note that we set the feedback control
gain of the tube MPC policy to zero, i.e.,
$\pi_{\mathrm{tube}}(\mathbf{x})=\bar{u}^{\star}$ for all
$\mathbf{x}\in\bar{\mathbf{x}}^{\star}\oplus\mathbf{D}\mathcal{W}$.

####  IV-A2 Update

We recursively augment the data set starting from the initial data set (27).
We describe one iteration of the data augmentation process below. Using the
provided data, we construct the terminal cost function $V_{f}(\cdot)$ as per
Sec. IV-B and the terminal set $\mathcal{X}_{f}$ as per Sec. IV-C. With these
components in place, we construct the proposed MPC by updating the terminal
cost $V_{f}(\cdot)$ and the terminal constraint using $\mathcal{X}_{f}$, as
detailed in Section V. Subsequently, we control the vehicle with this updated
MPC for urban road driving scenarios and record the closed-loop state and the
corresponding optimal input at every time step. Finally, we augment the data
set as follows:

$\mathbf{X}^{j}=\begin{bmatrix}{\mathbf{X}^{j-1}}^{\top}\\\
(\mathbf{x}_{0}-\mathbf{D}s_{\mathrm{tl},l^{\star}(\mathbf{x}_{0})})^{\top}\\\
\vdots\\\
(\mathbf{x}_{N^{a}_{\text{data}}}-\mathbf{D}s_{\mathrm{tl},l^{\star}(\mathbf{x}_{N^{a}_{\text{data}}})})^{\top}\end{bmatrix}^{\top},~{}\mathbf{U}^{j}=\begin{bmatrix}{\mathbf{U}^{j-1}}^{\top}\\\
u_{0}\\\ \vdots\\\ u_{N^{a}_{\text{data}}}\end{bmatrix}^{\top},$ (28)

where the superscript $j$ represents the iteration of the data augmentation,
$N^{a}_{\text{data}}$ represents the number of augmenting data points, and
$l^{\star}(\mathbf{x})$ is the optimal solution of (18). Let $j_{\text{max}}$
denote the maximum update iteration. Throughout the paper, we define
$\mathbf{X}=\mathbf{X}^{j_{\text{max}}}$ and
$\mathbf{U}=\mathbf{U}^{j_{\text{max}}}$ for brevity.

###  IV-B Learning the terminal cost $V_{f}(\cdot)$

The terminal cost function $V_{f}(\cdot)$ represents the remaining energy-to-
spend until the vehicle crosses the upcoming traffic light. Calculating the
terminal cost function $V_{f}(\cdot)$ exactly presents significant challenges
due to the dynamic and uncertain nature of traffic environments.

We propose a practical approach to approximate $V_{f}(\cdot)$ by considering
the remaining energy-to-spend under certain simplifying assumptions: (1) the
time to cross the upcoming traffic light is unconstrained, and (2) there is no
preceding vehicle. This simplification allows us to focus on the intrinsic
energy dynamics of the vehicle itself, decoupled from external factors such as
traffic flow and vehicle interactions. We empirically find that this
simplified $V_{f}(\cdot)$ is a good heuristic for urban road driving aimed at
minimizing energy consumption. This finding is aligned with the previous
research, which plans the speed reference for the entire route under free-flow
traffic conditions [10, 11].

For the rest of the paper, we will use the term "cost-to-go" to refer to the
remaining energy-to-spend. The terminal cost function $V_{f}(\cdot)$ will be
written as a convex combination of known cost-to-go values of the collected
data points.

####  IV-B1 Initialization

There are three stages of initializing the terminal cost $V_{f}(\cdot)$, i.e.,
computing $V_{f}^{0}(\cdot)$ where the superscript 0 represents the
initialization.

First, given the target set $\mathcal{O}$ in (26), we assign the cost-to-go
value to the vertices of $\mathcal{O}$. Recall that
$\mathcal{O}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\geq
s_{\mathrm{tl}}^{0},~{}\mathbf{x}\in\mathcal{X}\\}$, representing the region
behind the traffic light while satisfying the state constraint (16). Let
$v^{\mathcal{O}}_{i}$ denote the $i$-th vertex of $\mathcal{O}$, and let
$l_{\mathcal{O}}$ represent the number of vertices of $\mathcal{O}$. Reminding
that $\mathcal{O}$ is a polytope, we can summarize the cost-to-go values for
the points within $\mathcal{O}$ as the cost-to-go values of the vertices of
$\mathcal{O}$:

$\displaystyle\mathbf{V}_{\mathcal{O}}=\begin{bmatrix}(v^{\mathcal{O}}_{0}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\
(v^{\mathcal{O}}_{1}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\\\ \vdots\\\
(v^{\mathcal{O}}_{l_{\mathcal{O}}}-\mathbf{D}s_{\mathrm{tl}}^{0})^{\top}\end{bmatrix}^{\top},~{}\mathbf{J}_{\mathcal{O}}=\begin{bmatrix}0\\\
0\\\ \vdots\\\ 0\end{bmatrix}^{\top}$ (29)

where $\mathbf{V}_{\mathcal{O}}$ is a matrix where each element is the
vertices of $\mathcal{O}$ after undergoing a coordinate system transformation.
In this transformation, each element now denotes the remaining distance to the
traffic light. $\mathbf{J}_{\mathcal{O}}$ is a matrix is a matrix where each
element corresponds to the remaining energy-to-spend at the corresponding
element of $\mathbf{V}_{\mathcal{O}}$. By definition, the cost-to-go value is
zero at every point in the set $\mathcal{O}$; thus, all elements of
$\mathbf{J}_{\mathcal{O}}$ are zero.

Second, given $\mathbf{X}^{0}$ and $\mathbf{U}^{0}$ (27), we calculate the
cost-to-go for all data points until the vehicle crosses the upcoming traffic
light. Let $[\mathbf{X}^{0}]_{i}$ and $[\mathbf{U}^{0}]_{i}$ denote $i$-th
data point. We aim to calculate the upper bound to the cost-to-go value of the
$i$-th data point. We then stack these values for all data points to create an
initial cost-to-go data vector, represented as $\mathbf{J}^{0}$. To do that,
we first define the following sets:

$\begin{split}&\mathcal{E}_{k}=\bar{\mathbf{x}}_{k}^{0\star}\oplus\mathbf{D}\mathcal{W},~{}k\in\\{0,\cdots,T+1\\}.\\\
\end{split}$ (30)

For each set $\mathcal{E}_{k}$ (30), we compute the value $J^{0}_{k}$ by
solving the following optimization problem:

$\begin{split}&J_{k}^{0}=\max_{\mathbf{x}}~{}\ell(\mathbf{x},~{}\bar{\mathbf{u}}_{k}^{0\star})+J_{k+1}^{0}\\\
&~{}~{}~{}~{}~{}~{}~{}~{}\textnormal{s.t.,}~{}~{}\mathbf{x}\in\mathcal{E}_{k},\end{split}$
(31)

where $J_{T+1}^{0}=0$.

######  Remark 4.

By definition, the cost-to-go value is zero at every point in the set
$\mathcal{O}$. From the terminal constraint in (26),
$\mathcal{E}_{T+1}=\bar{\mathbf{x}}_{T+1}^{0\star}\oplus\mathbf{D}\mathcal{W}\subseteq(\mathcal{O}\ominus\mathbf{D}\mathcal{W})\oplus\mathbf{D}\mathcal{W}\subseteq\mathcal{O}$.
Thus, $J_{T+1}^{0}=0$.

######  Remark 5.

As the objective function in (31) is convex and $\mathcal{E}_{k}$ is a
polytope, there exists a vertex of $\mathcal{E}_{k}$ which is the global
maximizer of (31) [40, Prop. 1.3.4]. Thus, we can solve this maximization
problem (31) by enumerating the cost-to-go values of all vertices of
$\mathcal{E}_{k}$. The enumeration number is $T\cdot l_{w}$.

If $[\mathbf{X}^{0}]_{i}+\mathbf{D}s_{\mathrm{tl}}^{0}\in\mathcal{E}_{k}$, we
assign the corresponding cost-to-go value $J_{k}^{0}$ to it. In other words,
$i$-th element $[\mathbf{J}^{0}]_{i}=J_{k}^{0}$ if
$[\mathbf{X}^{0}]_{i}+\mathbf{D}s_{\mathrm{tl}}^{0}\in\mathcal{E}_{k}$. As
(31) is a maximization problem, $J_{k}^{0}$ represents the upper bound to the
cost-to-go value of every point in $\mathcal{E}_{k}$. In summary, the initial
cost-to-go data vector $\mathbf{J}^{0}$ is constructed as follows:

$\mathbf{J}^{0}=\begin{bmatrix}J_{0}^{0},J_{0}^{0},J_{0}^{0},J_{1}^{0},J_{1}^{0},J_{1}^{0},\cdots,J_{T}^{0},J_{T}^{0},J_{T}^{0}\end{bmatrix},$
(32)

where each element of $\mathbf{J}^{0}$ denote the upper bound to the cost-to-
go value of the corresponding data point in $[\mathbf{X}^{0}]$ and
$[\mathbf{U}^{0}]$.

Finally, the terminal cost $V_{f}^{0}(\cdot)$ is initialized as a convex
combination of the calculated cost-to-go values.

$\begin{split}&V_{f}^{0}(\mathbf{x})=\min_{\bm{\lambda}^{0},\bm{\lambda}^{\mathcal{O}}}~{}\mathbf{J}^{0}\bm{\lambda}^{0}+\mathbf{J}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}\\\
&\qquad~{}~{}~{}~{}~{}~{}\,\textnormal{s.t.,}~{}~{}~{}\mathbf{X}^{0}\bm{\lambda}^{0}+\mathbf{V}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}=\mathbf{x}-\mathbf{s}_{\mathrm{tl}}(\mathbf{x}),\\\
&\qquad\qquad~{}~{}~{}~{}~{}~{}~{}~{}\bm{\lambda}^{0}\geq\bm{0},\,\,\bm{1}^{\top}\bm{\lambda}^{0}=1,\\\
&\qquad\qquad~{}~{}~{}~{}~{}~{}~{}~{}\bm{\lambda}^{\mathcal{O}}\geq\bm{0},\,\,\bm{1}^{\top}\bm{\lambda}^{\mathcal{O}}=1.\end{split}$
(33)

Note that we take a convex combination of the rows in $\mathbf{X}^{0}$ and
$\mathbf{V}_{\mathcal{O}}$ in the first constraint. Thus, the domain of the
function $V_{f}^{0}(\mathbf{x})$ is the
$\mathbf{s}_{\mathrm{tl}}(\mathbf{x})\oplus\text{conv}(\\{[\mathbf{X}^{0}]_{i}\\}_{i=1}^{N_{\mathbf{X}^{0}}}\mathbin{\scalebox{0.8}{$\bigcup$}}\\{[\mathbf{V}_{\mathcal{O}}]_{i}\\}_{i=1}^{N_{\mathbf{V}_{\mathcal{O}}}})$,
where $N_{\mathbf{X}^{0}}$ and $N_{\mathbf{V}_{\mathcal{O}}}$ denote the
number of the column vectors of $\mathbf{X}^{0}$ and
$\mathbf{V}_{\mathcal{O}}$, respectively. $V_{f}^{0}(\mathbf{x})$ is a convex,
piecewise affine function as it is an optimal objective function of the multi-
parametric linear program [32].

####  IV-B2 Update

After the state and input data are updated as in (28), we update the cost-to-
go vector $[\mathbf{J}^{j}]$. The $i$-th element of the cost-to-go vector
$[\mathbf{J}^{j}]_{i}$ is updated as follows:

$\displaystyle[\mathbf{J}^{j}]_{i}=\ell([\mathbf{X}^{j}]_{i},[\mathbf{U}^{j}]_{i})$
(34)
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}+\sum_{m=1}^{M}p_{m}V_{f}^{j-1}(\mathbf{A}[\mathbf{X}^{j}]_{i}+\mathbf{B}[\mathbf{U}^{j}]_{i}+\mathbf{v}_{N}^{m}).$

We update $V_{f}^{j}(\cdot)$ by solving the following optimization problem
using (28) and (34).

$\begin{split}&V_{f}^{j}(\mathbf{x})=\min_{\bm{\lambda}^{j},\bm{\lambda}^{\mathcal{O}}}~{}\mathbf{J}^{j}\bm{\lambda}^{j}+\mathbf{J}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}\\\
&\qquad\qquad~{}~{}\textnormal{s.t.,}~{}~{}~{}\mathbf{X}^{j}\bm{\lambda}^{j}+\mathbf{V}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}=\mathbf{x}-\mathbf{s}_{\mathrm{tl}}(\mathbf{x}),\\\
&\qquad\qquad\qquad~{}~{}~{}\,\bm{\lambda}^{j}\geq\bm{0},~{}\bm{1}^{\top}\bm{\lambda}^{j}=1,\\\
&\qquad\qquad\qquad~{}~{}~{}\,\bm{\lambda}^{\mathcal{O}}\geq\bm{0},\,\,\bm{1}^{\top}\bm{\lambda}^{\mathcal{O}}=1.\end{split}$
(35)

Note that we take a convex combination of the rows in $\mathbf{X}^{j}$ and
$\mathbf{V}_{\mathcal{O}}$ in the first constraint. Thus, the domain of the
function $V_{f}^{j}(\mathbf{x})$ is the
$\mathbf{s}_{\mathrm{tl}}(\mathbf{x})\oplus\text{conv}(\\{[\mathbf{X}^{j}]_{i}\\}_{i=1}^{N_{\mathbf{X}^{j}}}\mathbin{\scalebox{0.8}{$\bigcup$}}\\{[\mathbf{V}_{\mathcal{O}}]_{i}\\}_{i=1}^{N_{\mathbf{V}_{\mathcal{O}}}})$,
where $N_{\mathbf{X}^{j}}$ denotes the number of the column vectors of
$\mathbf{X}^{j}$. $V_{f}^{j}(x)$ is a convex, piecewise affine function as it
is an optimal objective function of the multi-parametric linear program [32].

Remind that $j_{\text{max}}$ denotes the maximum update iteration. The
terminal cost function can be obtained by
$V_{f}(\cdot)=V_{f}^{j_{\text{max}}}(\cdot)$.

######  Remark 6.

When calculating the optimal input in (38) as per Sec. V, the function
$V^{j}(x)$ does not have to be calculated in prior as the minimization problem
(33) can be incorporated into the problem (38) and form a single unified
optimization problem as in (39).

######  Theorem 1.

Suppose two assumptions hold for the given problem: (1) the time to cross the
upcoming traffic light is unconstrained, and (2) there is no preceding
vehicle. Consider the value function $V^{j}(\cdot)$ in (35).
(1) $\forall j\geq 1$, the value function does not increase pointwise:

$\forall\mathbf{x}\in\text{dom}(V^{j-1}),~{}V^{j-1}(\mathbf{x})\geq
V^{j}(\mathbf{x}),$ (36)

where $\text{dom}(V^{j-1})$ denotes the domain of the function
$V^{j-1}(\cdot)$. (2) The value function $V^{j-1}(\mathbf{x})$ is the upper
bound to the closed-loop energy consumption of the system (5) with initial
condition $\mathbf{x}=\hat{\mathbf{x}}_{0}^{j}$ controlled by the proposed MPC
(39) at iteration $j$:

$V^{j-1}(\mathbf{x})\geq\mathbb{E}_{w_{0:T^{j}}}\Biggr{[}\sum_{k=0}^{T^{j}}\ell(\hat{\mathbf{x}}_{k}^{j},~{}u_{k}^{j})\Biggr{]},$
(37)

where $T^{j}$ is the duration of the $j$-th task.

###### Proof.

Follows from [30, Prop.4 and Thm.2]. ∎

Theorem 1 describes that under the assumptions, the closed-loop performance
theoretically enhances with increasing data size. Eventually, from [30,
Thm.3], the $V^{j}(\mathbf{x})$ converges towards the near-optimal cost-to-go
function under the free-flow traffic condition. This function was previously
derived through computationally demanding, long-horizon optimal control
problems in [10, 11] while now calculating the $V^{j}(\mathbf{x})$ at given
$\mathbf{x}$ is linear programming, which is computationally inexpensive.

As mentioned at the beginning of this section, Theorem 1 shows that the
terminal cost function $V_{f}(\cdot)$, which focuses on the vehicle’s
intrinsic energy dynamics, improves as the dataset size increases. Despite the
unrealistic assumptions of Theorem 1, it will be empirically demonstrated in
Sec. VI that an MPC guided by this data-driven terminal cost function can
reduce total energy consumption, even in the presence of a preceding vehicle.

###  IV-C Learning the terminal set $\mathcal{X}_{f}$

The terminal constraint $\hat{\mathbf{x}}_{N|k}\in\mathcal{X}_{f}$ in (21) is
designed to ensure that the vehicle crosses the traffic light within the
predetermined time step given by the high-level module while satisfying the
collision avoidance constraints introduced in (21). We design two sets in a
data-driven way, and the intersection of these two sets will define the
terminal set $\mathcal{X}_{f}$.

We adopt the notion of Robust Controllable Set to design these two sets. A set
is $N$-step Robust Controllable, if all states belonging to the set can be
robustly driven, through a time-varying control law, to the target set in
$N$-step [32, Def.10.18]. We design two Robust Controllable Sets where all
estimated states of the system (5) in each set can be robustly steered to each
target set against additive lumped noise $n$ (7) while satisfying the
collision avoidance constraints:

  1. 1.

$\mathcal{S}_{t_{\text{red}}}$: $t_{\text{red}}$-step Robust Controllable Set
where the target set is
$\hat{\mathcal{G}}_{s}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\leq
s_{\mathrm{tl}}(\mathbf{x})-w_{\text{max}}\\}$.

  2. 2.

$\mathcal{P}_{t_{\text{green}}}$: $t_{\text{green}}$-step Robust Controllable
Set where the target set is
$\hat{\mathcal{G}}_{p}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\geq
s_{\mathrm{tl}}(\mathbf{x})-w_{\text{min}}\\}$,

where $w_{\text{min}}$ and $w_{\text{max}}$ denote the extreme of the segment
$\mathcal{W}$ defined in (4). In Fig. 2, $t_{\text{red}}$ and
$t_{\text{green}}$ are depicted. Remind that $T_{l}$ denotes the predetermined
time to cross the $l$-th traffic light, and $l^{\star}$ denotes the index of
the nearest upcoming traffic light. Thus, $T_{l^{\star}}$ represents the
predetermined time to cross the nearest upcoming traffic light.

######  Remark 7.

Let
$\mathcal{G}_{s}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\leq
s_{\mathrm{tl}}(\mathbf{x})\\}$ and
$\mathcal{G}_{p}=\\{\mathbf{x}~{}|~{}\begin{bmatrix}1&0\end{bmatrix}\mathbf{x}\geq
s_{\mathrm{tl}}(\mathbf{x})\\}$. These sets denote the regions before and
after the upcoming traffic light, respectively. Note that the target sets
$\hat{\mathcal{G}}_{s}$ and $\hat{\mathcal{G}}_{p}$ are obtained by
$\hat{\mathcal{G}}_{s}=\mathcal{G}_{s}\ominus\mathbf{D}\mathcal{W}$ and
$\hat{\mathcal{G}}_{p}=\mathcal{G}_{p}\ominus\mathbf{D}\mathcal{W}$. From
Proposition 1, $\mathbf{x}-\hat{\mathbf{x}}\in\mathbf{D}\mathcal{W}$. Thus,
$\hat{\mathbf{x}}\in\hat{\mathcal{G}}_{s}$ and
$\hat{\mathbf{x}}\in\hat{\mathcal{G}}_{p}$ imply
${\mathbf{x}}\in{\mathcal{G}}_{s}$ and ${\mathbf{x}}\in{\mathcal{G}}_{p}$,
respectively.

Figure 2: Defintion of $t_{\text{red}}$ and $t_{\text{green}}$

The terminal set $\mathcal{X}_{f}$ is defined as the intersection of these two
sets,
$\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}}$.
Let $\hat{\mathbf{x}}_{N|k}$ denotes a predicted estimated state at time step
$k$+$N$ calculated from the current estimate $\hat{\mathbf{x}}_{k}$, i.e., a
terminal state. Then, the terminal constraint,
$\hat{\mathbf{x}}_{N|k}\in\mathcal{X}_{f}=\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}}$,
enforces not to pass the traffic light before it turns green while
guaranteeing that the vehicle crosses the traffic light within the time
predetermined by the high-level module. Fig. 3 illustrates the terminal
constraints.

Figure 3: Illustration of the terminal constraints
$\mathcal{S}_{t_{\text{red}}}$ and $\mathcal{P}_{t_{\text{green}}}$: the
constraint $\hat{\mathbf{x}}_{N|k}\in\mathcal{S}_{t_{\text{red}}}$ is to
ensure that the vehicle stays behind the traffic light until
$t_{\text{red}}+N$ steps, while the constraint
$\hat{\mathbf{x}}_{N|k}\in\mathcal{P}_{t_{\text{green}}}$ is to ensure that
the vehicle passes the traffic light within $t_{\text{green}}+N$ steps.

In this paper, the sets $\mathcal{S}_{t_{\text{red}}}$ and
$\mathcal{P}_{t_{\text{green}}}$ are calculated in a data-driven way by
Algorithm 1 with $\hat{\mathcal{G}}_{s}$ and $\hat{\mathcal{G}}_{p}$ as the
respective target convex sets. The corresponding outputs of Algorithm 1 are
assigned to $\mathcal{S}_{t_{\text{red}}}$ and
$\mathcal{P}_{t_{\text{green}}}$.

$\mathcal{R}_{i}$ represents $i$-step robust controllable set, which is
initialized with to the target set $\mathcal{G}$ at line 2. In line 4,
$N_{\mathbf{X}}$ denotes the number of the column vectors of $\mathbf{X}$.
Line $5$ at the iteration $i$ checks whether the state $[\mathbf{X}]_{j}$ of
the system (5) can be robustly steered to $\mathcal{R}_{i-1}$. Then, at line
$6$, the collision avoidance satisfaction is checked at the state
$[\mathbf{X}]_{j}$. If both conditions are satisfied, the state
$[\mathbf{X}]_{j}$ is included in a set $\mathbb{X}$. At the end of the
iteration $i$ (line 11), $\mathcal{R}_{i}$ is set as the convex hull of the
set $\mathbb{X}$.

Input : Data matrices $\mathbf{X},\mathbf{U}$,

Remaining steps $t$, Target convex set $\mathcal{G}$,

Predicted behavior of the preceding vehicles

$\\{\hat{s}^{pv}_{i|k}\\}_{i=N}^{t+N}$,
$\\{\hat{v}^{pv}_{i|k}\\}_{i=N}^{t+N}$,

System parameters $\mathbf{A},~{}\mathbf{B},~{}\mathbf{D},~{}\mathcal{W},~{}L$

Output : $t$-step Robust Controllable Set $\mathcal{R}_{t}$

1

2$\mathcal{R}_{0}\leftarrow\mathcal{G}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{C}(\hat{s}^{pv}_{t+N|k},\hat{v}^{pv}_{t+N|k})$

3for  _$p\leftarrow 0$ to $t-1$_ do

4 $\mathbb{X}\leftarrow\emptyset$

5 for  _$q\leftarrow 1$ to $N_{\mathbf{X}}$_ do

6 if
_$\mathbf{A}[\mathbf{X}]_{q}+\mathbf{B}[\mathbf{U}]_{q}\in\mathcal{R}_{p}\ominus
2L\mathbf{D}\mathcal{W}$_ then

7 if
_$[\mathbf{X}]_{q}\in\mathcal{C}(\hat{s}^{pv}_{t+N-p-1|k},\hat{v}^{pv}_{t+N-p-1|k})$_
then

8
$\mathbb{X}\leftarrow\mathbb{X}\mathbin{\scalebox{0.8}{$\bigcup$}}\\{[\mathbf{X}]_{q}\\}$

9 end if

10

11 end if

12

13 end for

14 $\mathcal{R}_{p+1}\leftarrow\text{conv}(\mathbb{X})$

15

16 end for

return _$\mathcal{R}_{t}$_

$~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}t$-step Robust
Controllable Set $\mathcal{R}_{t}$

Algorithm 1 Data-driven computation of

$~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}t$-step Robust
Controllable Set $\mathcal{R}_{t}$

By the following theorem, the output of the Algorithm 1 is a $t$-step robust
controllable set for a given target convex set $\mathcal{G}$.

######  Theorem 2.

The output of Algorithm 1 $\mathcal{R}_{t}$ is $t$-step robust controllable
set of the system (5) perturbed by the noise (7) for a given target convex set
$\mathcal{G}$ subject to the constraints (16) and the collision avoidance
constraints given the predicted behavior of the preceding vehicle
$\\{\hat{s}^{pv}_{i|k}\\}_{i=N}^{t+N}$ and
$\\{\hat{v}^{pv}_{i|k}\\}_{i=N}^{t+N}$.

###### Proof.

See Appendix -A. ∎

##  V Controller

We implement two distinct MPC controllers: one with prediction horizon $N$ for
the standard scenario and another as a simplified variant with shorter horizon
$N_{s}<N$. The choice between them depends on the time remaining to clear the
traffic light intersection.

  * •

If the remaining time exceeds $k+N\leq T_{l^{\star}}$, we employ the standard
MPC controller introduced in (39), which is the primary choice given its
applicability to most scenarios.

  * •

Conversely, if the remaining time is less than $k+N>T_{l^{\star}}$, we opt for
the simplified version of the standard MPC controller introduced in (40).

Recall that $T_{l^{\star}}$ is the predetermined time to cross the nearest
upcoming traffic light as illustrated in Fig. 2.

First, we introduce the standard MPC controller with prediction horizon $N$.
We incorporate the terminal cost function $V_{f}(\mathbf{x})$ in (35) and the
terminal set
$\mathcal{X}_{f}=\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}}$
into the MPC controller in (21). Additionally, we reformulate the constraints
on estimate states to those on nominal states111See Appendix -B for the
details of the constraint reformulation.. Specifically, we design an MPC
controller as follows:

$\displaystyle\tilde{J}_{\text{MPC}}(\hat{\mathbf{x}}_{k},\\{\hat{s}^{pv}_{i|k}\\}_{i=0}^{N},\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{N})=$
(38) $\displaystyle\min_{\begin{subarray}{c}u_{0:N-1|k},\\\
\mathbf{s}_{f}\end{subarray}}\sum_{i=0}^{N-1}\ell(\bar{\mathbf{x}}_{i|k},~{}u_{i|k})+\sum_{m=1}^{M}p_{m}V_{f}(\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N}^{m})+M_{f}\mathbf{s}_{f}$
$\displaystyle~{}~{}~{}~{}\textnormal{s.t.,}\,~{}~{}\bar{\mathbf{x}}_{i+1|k}=\mathbf{A}\bar{\mathbf{x}}_{i|k}+\mathbf{B}u_{i|k}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{0|k}=\hat{\mathbf{x}}_{k},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{i|k}\in\mathcal{U},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}^{pv}_{i|k},\hat{v}^{pv}_{i|k})\ominus
2Li\mathbf{D}\mathcal{W}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{N|k}\in(\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}})\ominus
2LN\mathbf{D}\mathcal{W}\oplus\mathbf{s}_{f},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus(2L+1)\mathbf{D}\mathcal{W},\text{if}\,c_{l^{\star},k+i}=\text{red},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\mathbf{s}_{f}\geq 0,$
$\displaystyle\qquad~{}~{}~{}~{}~{}i\in\\{0,\cdots,N-1\\},$

where $M_{f}$ is a sufficiently large number, which is set to $10000$ in this
paper, and $\mathbf{s}_{f}$ denotes a slack variable, which is required to
ensure feasibility when the data to construct the terminal set is
insufficient, or the surrounding environment abruptly changes, such as the
preceding vehicle abruptly decelerating or another vehicle cutting into the
ego vehicle’s lane. Note that the objective function in (38) is identical to
that in (21) since the stage cost in (11) does not depend on the position and
thus $\ell(\hat{\mathbf{x}}_{k},u_{k})=\ell(\bar{\mathbf{x}}_{k},u_{k})$.

The minimization problem (33) can be incorporated into the problem (38) and
form a single unified optimization problem as follows:

$\displaystyle\tilde{J}_{\text{MPC}}(\hat{\mathbf{x}}_{k},\\{\hat{s}^{pv}_{i|k}\\}_{i=0}^{N},\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{N})=$
(39) $\displaystyle\min_{\begin{subarray}{c}u_{0:N-1|k},\\\ \mathbf{s}_{f},\\\
\bm{\lambda}_{1:M},\\\
\bm{\lambda}^{\mathcal{O}}_{1:M}\end{subarray}}\sum_{i=0}^{N-1}\ell(\bar{\mathbf{x}}_{i|k},~{}u_{i|k})+\sum_{m=1}^{M}p_{m}V_{m}+M_{f}\mathbf{s}_{f}$
$\displaystyle~{}~{}~{}~{}\textnormal{s.t.,}\,~{}~{}\bar{\mathbf{x}}_{i+1|k}=\mathbf{A}\bar{\mathbf{x}}_{i|k}+\mathbf{B}u_{i|k}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{0|k}=\hat{\mathbf{x}}_{k},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{i|k}\in\mathcal{U},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}^{pv}_{i|k},\hat{v}^{pv}_{i|k})\ominus
2Li\mathbf{D}\mathcal{W}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{N|k}\in(\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}})\ominus
2LN\mathbf{D}\mathcal{W}\oplus\mathbf{s}_{f},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus(2Li+1)\mathbf{D}\mathcal{W},\text{if}\,c_{l^{\star},k+i}=\text{red},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\mathbf{s}_{f}\geq 0$
$\displaystyle\qquad~{}~{}~{}~{}~{}V_{m}=\mathbf{J}\bm{\lambda}_{m}+\mathbf{J}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}_{m},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\mathbf{X}\bm{\lambda}_{m}+\mathbf{V}_{\mathcal{O}}\bm{\lambda}^{\mathcal{O}}_{m}=\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N}^{m}-\mathbf{s}_{\mathrm{tl}}(\bar{\mathbf{x}}_{N|k}+\mathbf{v}_{N}^{m})$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bm{\lambda}_{m}\geq\bm{0},~{}\bm{1}^{\top}\bm{\lambda}_{m}=1,$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bm{\lambda}^{\mathcal{O}}_{m}\geq\bm{0},\,\,\bm{1}^{\top}\bm{\lambda}^{\mathcal{O}}_{m}=1$
$\displaystyle\qquad~{}~{}~{}~{}~{}i\in\\{0,\cdots,N-1\\},~{}m\in\\{1,\cdots,M\\}.$

Second, we introduce the simplified version of the standard MPC controller
(39) with shrinking prediction horizon $N_{s}=T_{l^{\star}}-k$. Specifically,
we design an MPC controller as:

$\displaystyle\tilde{J}_{\text{MPC}}^{\text{simplified}}(\hat{\mathbf{x}}_{k},\\{\hat{s}^{pv}_{i|k}\\}_{i=0}^{N_{s}},\\{\hat{v}^{pv}_{i|k}\\}_{i=0}^{N_{s}})=$
(40) $\displaystyle\min_{\begin{subarray}{c}u_{0:N_{s}-1|k},\\\
\mathbf{s}_{f}\end{subarray}}\sum_{i=0}^{N_{s}-1}\ell(\bar{\mathbf{x}}_{i|k},~{}u_{i|k})+M_{f}\mathbf{s}_{f}$
$\displaystyle~{}~{}~{}~{}\textnormal{s.t.,}\,~{}~{}\bar{\mathbf{x}}_{i+1|k}=\mathbf{A}\bar{\mathbf{x}}_{i|k}+\mathbf{B}u_{i|k}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{0|k}=\hat{\mathbf{x}}_{k},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{i|k}\in\mathcal{U},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}^{pv}_{i|k},\hat{v}^{pv}_{i|k})\ominus
2Li\mathbf{D}\mathcal{W}$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{N_{s}|k}\in\hat{\mathcal{G}}_{p}\ominus
2LN_{s}\mathbf{D}\mathcal{W}\oplus\mathbf{s}_{f},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\bar{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus(2Li+1)\mathbf{D}\mathcal{W},\text{if}\,c_{l^{\star},k+i}=\text{red},$
$\displaystyle\qquad~{}~{}~{}~{}~{}\mathbf{s}_{f}\geq 0$
$\displaystyle\qquad~{}~{}~{}~{}~{}i\in\\{0,\cdots,N_{s}-1\\}.$

Remind that ${\mathcal{G}}_{p}$ denote the region after the upcoming traffic
light, and
$\hat{\mathcal{G}}_{p}={\mathcal{G}}_{p}\ominus\mathbf{D}\mathcal{W}$ as
defined in Sec. IV-C. If $\mathbf{s}_{f}=0$, the terminal constraint of the
(40), i.e., $\bar{\mathbf{x}}_{N_{s}|k}\in\hat{\mathcal{G}}_{p}\ominus
2LN_{s}\mathbf{D}\mathcal{W}\oplus\mathbf{s}_{f}$, imply that
$\hat{\mathbf{x}}_{N_{s}|k}\in\hat{\mathcal{G}}_{p}$ and thus
${\mathbf{x}}_{N_{s}|k}\in{\mathcal{G}}_{p}$. By definition, the cost-to-go
value for all $\mathbf{x}_{N_{s}|k}\in{\mathcal{G}}_{p}$ is zero. Therefore,
all cost functions and constraints related to calculating the terminal cost
$V_{f}(\cdot)$ are removed from (39).

In summary, if $k+N\leq T_{l^{\star}}$, the MPC controller in (39) is
employed; otherwise, the MPC controller in (40) is employed. The resulting
control policy is:

$\pi(\hat{x}_{k})=\begin{cases}u^{\star}_{0|k}\text{ from \eqref{eq: mpc
unified}},&\text{if }k+N\leq T_{l^{\star}},\\\ u^{\star}_{0|k}\text{ from
\eqref{eq: mpc simplified}},&\text{otherwise}.\end{cases}$ (41)

######  Theorem 3.

Consider the closed-loop system (2) controlled by the control policy (41). If
the optimal slack variable $\mathbf{s}_{f}^{\star}=0$ at time step $k$, then
there exists a control input sequence that steers the system (2) to the region
after the upcoming traffic light within the corresponding predetermined time,
while satisfying the constraints (15) and (16).

###### Proof.

See Appendix -C. ∎

######  Remark 8.

Theorem 3 describes that if $\mathbf{s}_{f}^{\star}\neq 0$, we cannot ensure
that there is a control sequence that steers the system (2) after the upcoming
traffic light within the predetermined time. This could happen in practice if
the data is insufficient and/or the predicted speed is overestimated by the
actual speed of the preceding vehicle. It is also possible that the
predetermined time computed by the high-level green wave search module is
inaccurate. Thus, in this case, we increase the remaining time step
$T_{l^{\star}(\mathbf{x})}$ illustrated in Fig. 2 and relax the problem, which
then solves the problem of crossing the upcoming traffic light with a larger
remaining time step.

##  VI Validation

###  VI-A Energy Consumption Model

We collected the energy consumption data of our electric test vehicle and
solved the regression problem (11). The energy consumption measurement data
used for the regression was collected from 3.9 km of city driving scenarios in
a test track. The comparison between the data and the regressed model is given
in Fig. 4. The regression results show that the error of the total energy
consumption is below $1\%$.

Figure 4: Comparison: Vehicle energy measurement data and calculated energy
consumption

We validated our regression model in (11) using an additional dataset not
employed during the regression process. This test dataset is crucial for
assessing the model’s generalizability. We conducted 15 additional tests. For
each trial, we calculated the percentage error in total energy consumption
between measurements and the regression model (11). The results are described
in Table. I, which indicates that the mean error is around $1\%$, and the
worst-case error is $6.3\%$.

TABLE I: Error of the cumulative energy consumption (%) Mean[%] | Standard deviation[%] | Worst case[%]
---|---|---
$-1.06\%$ | $3.41\%$ | $-6.3\%$

###  VI-B Controller Implementation

The proposed standard MPC (39) is a convex optimization problem. The stage
cost is convex as $\mathbf{P}\succeq 0$ (11). The system equation is linear,
and state/input constraints (16) are convex.
$\mathcal{C}(\hat{s}^{pv}_{i|k},\hat{v}^{pv}_{i|k})$ is a convex set from
(15). The set $\mathcal{W}$ is convex (4), and the sets
$\mathcal{S}_{t_{\text{red}}}$ and $\mathcal{P}_{t_{\text{green}}}$ are convex
polytopes as they are calculated via convex hull operation of a finite number
of points as described in line 9 of Algorithm 1. Since the Pontryagin
difference between two convex polytopes is convex, the fourth and the fifth
constraints are convex. The rest of the constraints are linear equalities and
inequalities which are convex. As the proposed standard MPC (39) is a convex
optimization problem, the simplified MPC (40) is also a convex optimization
problem. To implement the proposed MPC (39) and (40), we utilize CVXPY [41] as
a modeling language and MOSEK as a solver [42]. For the vehicle test, we
implement the proposed algorithm in ROS2 [43].

###  VI-C Baseline

Two baseline algorithms were considered in this study. The first baseline
algorithm is a cruise controller in [44]. The second baseline algorithm is the
algorithm introduced in [45]. The major differences between the algorithm in
[45] and the proposed algorithm are:

  * •

The algorithm in [45] disregards localization uncertainty, while the proposed
algorithm addresses this uncertainty.

  * •

The algorithm in [45] needs to compute speed references for the entire route,
while the proposed algorithm calculates speed references for a short
prediction horizon, requiring less computational power.

  * •

The algorithm in [45] has a hierarchical structure: a speed planner and an
additional speed tracking controller, resulting in the latency and compounding
error issue. In contrast, the proposed algorithm is a single MPC controller
with longitudinal acceleration input.

Note that for three algorithms, there is an actuator-level controller that
converts longitudinal acceleration commands into electric motor torque inputs.

###  VI-D Localization Uncertainty and Controller Parameters

Throughout the validation, we assume that a localization module provides
position information every $1\ sec$ under the following localization
uncertainty $w$:

$p_{w}(w)\sim U(-3,3),~{}\mathcal{W}=\\{w~{}|~{}3\leq w\leq 3\\},$ (42)

where $U$ represents a uniform distribution. $3\ m$ in longitudinal position
error exceeds the accuracy of lane-level positioning [46], which can be
achievable using off-the-shelf systems using vision [47] and V2I communication
[48]. It is $95\%$ Circle of Error Probable (CEP) for commercially available
GPS [49].

We set the prediction horizon $N=5$, which represents $5\ sec$ prediction as
the discretization time $T_{s}=1\ sec$. The observer gain $L$ in (5) is set to
$\frac{1}{4N}$.

###  VI-E Data collection and Training

We collect the data using simulation. Given the simplicity of the system (2),
data collection through simulation is feasible. The scenario is set as
follows. The vehicle’s initial speed is set to zero, and the testing road is a
single road segment with fixed parameters outlined in Table II. Essentially,
the vehicle needs to pass an upcoming traffic light, situated $200\ m$ away.

TABLE II: Parameters for the single road segment scenario Parameter | Value
---|---
Current traffic signal | Green
Traffic light cycle | Green: $150$s, Yellow: $5$s, Red: $25$s
Remaining time/distance |  $150$s/$200$m

For the initialization, we assume the free-flow condition. As explained in
Sec. IV-A, we initialize the data set by executing the cruise controller in
(26) and collecting the state and input data as described in (27).

For the update, we assume a constant flow speed, and the preceding vehicle
follows the constant flow speed. To generate such a scenario, we spawn one
preceding vehicle. This preceding vehicle is located ahead of the ego vehicle
with a random distance, $d_{0}\sim U(5,15)$. Moreover, this preceding vehicle
has a random initial speed, $v_{0}^{pv}\sim U(2,15)$, and keeps its speed
until the end of the scenario, i.e., $v_{k}^{pv}=v_{0}^{pv},~{}\forall k\geq
0$. Once the traffic flow speed $v_{0}^{pv}$ is sampled, the time to cross the
upcoming traffic light is calculated as follows:

$T_{1}=\biggr{\lceil}\frac{200}{v_{0}^{pv}}\biggr{\rceil}+1,$ (43)

where $\lceil\cdot\rceil$ denotes the ceiling function. Given the dataset, we
compute the terminal set $\mathcal{X}_{f}$ as per Sec. IV-C and the terminal
cost $V_{f}(\cdot)$ as per Sec. IV-B. Integrating $\mathcal{X}_{f}$ and
$V_{f}(\cdot)$ into the proposed MPC (39) and (40), we solve MPC (38) and
apply the optimal input to the system (2). While running the MPC (38), we
record the closed-loop state and input pairs. After completing each task
iteration, we augment the data using these state-input pairs.

Fig. 5 illustrates the learning curve of the realized total energy
consumption. The x-axis denotes the number of data augmentation processes. Due
to the uncertainty described in system (2) and the randomness of the scenario
parameters, the total energy consumption is a random variable. Therefore, we
calculate the sample mean of the total energy consumption by performing 100
Monte Carlo simulations of the system (2) in closed-loop using the MPC (39)
and (40) after each data augmentation process is completed. The results
demonstrate $31.7\%$ improvement in total energy consumption as the number of
data augmentations (or task iterations) increases, eventually leading to
settled performance.

Figure 5: Total Energy Consumption Improvement with Increasing Data Size.
$100$ Monte Carlo simulations for each task iteration.

###  VI-F Simulation results

We conduct simulation studies to investigate the performance of the proposed
MPC (39) and (40). We compare the proposed algorithm with two baseline
algorithms described in Sec. VI-C.

The simulations are conducted across four distinct scenarios, each designed to
capture various traffic flow conditions. In all scenarios, the ego vehicle’s
initial speed is set to zero, the preceding vehicle is positioned $5\ m$
ahead, and the testing road is a single road segment with fixed parameters
outlined in Table II. The preceding vehicle’s controller is the cruise control
algorithm in [44] and its reference speed varies across the scenarios as
follows: (1) $v_{k}^{pv}=2.5m/s$, (2) $v_{k}^{pv}=5.0m/s$, (3)
$v_{k}^{pv}=7.5m/s$, (4) $v_{k}^{pv}=10.0m/s$. The time to cross the upcoming
traffic light in each scenario is calculated accordingly based on (43). After
we conduct each simulation scenario, we evaluate the energy consumption
defined in (8).

Energy consumption results are described in Table III. The unit for the energy
consumption is kJ. We tune the reference speed of the cruise control algorithm
and also configure the terminal time and the regularization parameter in [45,
eq. (4)], ensuring that the completion times for both algorithms match that of
the proposed algorithm. Compared to the cruise control algorithm, the proposed
algorithm improves energy consumption by an average of $25.8\%$. Compared to
the algorithm in [45], the proposed algorithm improves energy consumption by
an average of $11.7\%$. It is noteworthy that as the flow speed increases, the
energy consumption tends to increase for all algorithms. This result is
aligned with [29, Table 1] and justifies implementing traffic light crossing
time constraints in Sec. II-F.

TABLE III: Comparison of Energy Consumption [kJ]
Across Different Algorithms and Preceding Vehicle Speeds Algorithm $v_{k}^{pv}$ | $2.5m/s$ | $5.0m/s$ | $7.5m/s$ | $10m/s$
---|---|---|---|---
Cruise control | $200.26$ | $213.94$ | $263.45$ | $326.84$
Algorithm [45] | $196.11$ | $163.49$ | $207.50$ | $271.08$
Proposed | $170.83$ | $145.76$ | $171.77$ | $255.48$

Fig.6 presents the simulation results when $v_{k}^{pv}=7.5m/s$. We compare the
proposed algorithm with the algorithm in [45]. A comparative analysis between
the proposed algorithm and the algorithm in [45] is conducted. While the high-
level speed planner in [45] computes speed references for the entire task
horizon at each time step, for the sake of comparison, the planned speed
reference in Fig.6.(b) is truncated.

As shown in Fig.6.(a), the predicted state of the proposed algorithm closely
aligns with the closed-loop state. In particular, the last two predictions are
difficult to distinguish as they overlap. In contrast, as shown in Fig.6.(b),
the planned speed reference does not consistently match the closed-loop state.
This disparity arises from the small distance to the preceding vehicle and the
free-flow traffic assumption embedded within the speed planner in [45]. The
distance to the preceding vehicle (or gap) is illustrated in Fig.6.(c). When
the gap is large, allowing the ego vehicle to drive in free-flow traffic
conditions, the planned speed aligns with the closed-loop speed. However, when
the gap becomes small, violating the free-flow assumption, the tracking
controller in [45] cannot track the planned speed reference with a small
tracking error to avoid collision with the preceding vehicle. This phenomenon
does not happen as the collision avoidance constraints are considered when the
terminal set $\mathcal{X}_{f}$ is computed as Algorithm 1.

As the objective of the tracking controller is not to minimize the energy
consumption but to minimize tracking error and avoid collision with the
preceding vehicle[45], the disparity between the planned and the closed-loop
speed results in energy-inefficient motion. As illustrated in Fig.6.(d), the
energy consumption of the algorithm in [45] becomes worse than that of the
proposed algorithm when the gap decreases.

Figure 6: Simulation results when $v_{k}^{pv}=7.5m/s$. (a) and (b): Vehicle
speed histories: Comparison between Proposed Algorithm and Algorithm in [45].
Closed-loop states and Predicted or Planned states at four different time
steps. (c) Distance to Preceding Vehicle: Comparison between two algorithms.
(d) Energy Consumption: Comparison between two algorithms.

###  VI-G Experiement setup and results

We conduct a vehicle-in-the-loop experiment to investigate the performance of
the proposed MPC (39) and (40). We compare the proposed algorithm with the
algorithm in [45]. For our experiment, we utilized a retrofitted Hyundai Ioniq
5 as the test vehicle maneuvering around the actual testing site. We simulated
four traffic lights and the preceding vehicle. An overview of our test vehicle
and scenario is presented in Fig. 7.

Figure 7: Test vehicle and scenario

For each scenario, traffic flow speed, light cycles, and locations were
randomly selected. Once all relevant parameters, such as traffic flow speed,
light cycles, and locations, are available, we utilize the high-level green
wave search module described in Sec. II-F to compute the time required to
cross each traffic light while adhering to the prescribed traffic flow speed
and rules governing traffic lights. The parameters for each scenario are
described below:

TABLE IV: Randomly sampled flow speed and traffic light locations,
and Computed time to cross the traffic lights |  Traffic flow speed [m/s] |  Traffic light locations [m] |  Time to cross each traffic light[s]
---|---|---|---
Scenario 1 | $4.312$ | $[102,245,378,484]$ | $[24,57,88,113]$
Scenario 2 | $5.451$ | $[126,260,381,506]$ | $[26,48,70,94]$
Scenario 3 | $3.433$ | $[127,227,343,450]$ | $[37,62,95,127]$

In all scenarios, the preceding vehicle is positioned $5$m ahead and
controlled using the cruise control algorithm to track the traffic flow speed.
Moreover, the initial condition of the ego vehicle is idle speed, meaning no
driver pedal inputs. Starting from idle speed, the vehicle needs to complete
the given route under localization uncertainty while crossing each traffic
light within the predetermined time and avoid collision with the preceding
vehicle.

The terminal time and the regularization parameter in [45, eq. (4)] is tuned
to show a similar travel time to the proposed algorithm. After we conduct each
test scenario, we evaluate the energy consumption defined in (8). The change
of $E_{\text{stor}}$ is calculated using the equipped voltage and current
sensors, while the change of $E_{\text{kin}}$ is calculated by a change in
kinetic energy between the initial and the end states.

The speed profiles are presented in Fig.8. In all scenarios, the proposed
algorithm crosses each traffic light without stopping, ensuring a consistent
completion of the given route with minimal fluctuations, as depicted in Fig.
8. In contrast, the algorithm [45] exhibits occasional stops along the route.
Though the algorithm in [45] ensures timely crossing of traffic lights during
the planning phase, the tracking controller fails to follow the planned speed
reference due to tracking error, time latency, and, most importantly,
collision avoidance as described in Sec. VI-F. Consequently, when the vehicle
controlled by the algorithm [45] comes to a stop before the traffic light, it
waits until the light changes to green. This delay may lead to the vehicle
needing to accelerate to meet the predetermined time for crossing the next
traffic light, resulting in energy-inefficient motion.

Figure 8: Closed-loop speed trajectory of the ego vehicle.

The energy consumption results are described in Table V. Compared to the
algorithm in [45], the proposed algorithm shows an improvement of $19.4\%$ in
average energy consumption. In the best-case scenario (scenario 2), the
proposed algorithm exhibits a $47.5\%$ improvement over the algorithm in [45],
while in the worst-case scenario (scenario 1), it performs $1.5\%$ worse than
the algorithm in [45]. As illustrated in Fig. 8.(a), in scenario 1, the speed
profiles of both the proposed algorithm and the algorithm in [45] exhibit
similarity, leading to comparable energy consumption results. However, as
shown in Fig. 8.(b), in scenario 2, the vehicle controlled by the algorithm in
[45] experiences multiple stops, consequently leading to excessive energy
consumption. In contrast, the vehicle controlled by the proposed algorithm
crosses the traffic light without stopping, resulting in energy-efficient
motion. Considering the predetermined time in Table. IV, the ego vehicle
should cross the final traffic light within 113s, 94s, and 127s for scenarios
1, 2, and 3, respectively. The proposed algorithm successfully completes the
route within the maximum travel time for all scenarios, while the algorithm in
[45] does not.

TABLE V: Closed-loop performance of algorithms.
$[\text{Scenario 1},\text{Scenario 2},\text{Scenario 3}]$ Algorithm | Energy consumption[kJ] |  Travel time[s]
---|---|---
Algorithm [45] | $[192.2,328.0,214.1]$ | $[114.8,134.7,127.6]$
Proposed | $[195.1,171.1,188.1]$ | $[106.6,92.7,109.6]$

##  VII Conclusion and Future work

This paper has introduced a data-driven Model Predictive Control (MPC)
framework tailored for energy-efficient urban road driving in the context of
connected, automated vehicles, while considering localization uncertainty. By
addressing the challenges of longitudinal speed planning and control on roads
with traffic lights and preceding vehicles, our proposed MPC offers a unified
solution that minimizes total energy consumption while ensuring timely
compliance with traffic laws and traffic light signals.

Incorporating a terminal cost function and constraints, learned from state-
input data, enables effective energy optimization across the long task
horizons while solving the proposed MPC with a short horizon. The proposed
algorithm has been investigated via simulations and vehicle tests. The vehicle
tests demonstrate the practical effectiveness of our approach, showcasing a
significant $19.4\%$ improvement in average energy consumption compared to
conventional methods.

As an extension of the current work, we aim to enhance the robustness and
adaptability of our system to accommodate the dynamic traffic environment,
such as variations in traffic light. Moreover, we aim to delve into a multi-
agent control for connected vehicles through V2V communication.

## ACKNOWLEDGMENT

This research work presented herein is funded by the Advanced Research
Projects Agency-Energy (ARPA-E), U.S. Department of Energy under DE-AR0000791.

## References

  * [1] J. Qiu and L. Du, “Optimal dispatching of electric vehicles for providing charging on-demand service leveraging charging-on-the-move technology,” _Transportation Research Part C: Emerging Technologies_ , vol. 146, p. 103968, 2023.
  * [2] A. Mostafizi, C. Koll, and H. Wang, “A decentralized and coordinated routing algorithm for connected and autonomous vehicles,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 8, pp. 11 505–11 517, 2022.
  * [3] J. Guanetti, Y. Kim, and F. Borrelli, “Eco-routing of connected plug-in hybrid electric vehicles,” in _2019 IEEE 58th Conference on Decision and Control (CDC)_ , 2019, pp. 2245–2250.
  * [4] Q. Guo, L. Li, and X. (Jeff) Ban, “Urban traffic signal control with connected and automated vehicles: A survey,” _Transportation Research Part C: Emerging Technologies_ , vol. 101, pp. 313–334, 2019.
  * [5] S. Wang, M. W. Levin, and R. J. Caverly, “Optimal parking management of connected autonomous vehicles: A control-theoretic approach,” _Transportation Research Part C: Emerging Technologies_ , vol. 124, p. 102924, 2021.
  * [6] D. Elliott, W. Keen, and L. Miao, “Recent advances in connected and automated vehicles,” _Journal of Traffic and Transportation Engineering (English Edition)_ , vol. 6, no. 2, pp. 109–131, 2019.
  * [7] J. Guanetti, Y. Kim, and F. Borrelli, “Control of connected and automated vehicles: State of the art and future challenges,” _Annual Reviews in Control_ , vol. 45, pp. 18–40, 2018.
  * [8] A. Karbasi and S. O’Hern, “Investigating the impact of connected and automated vehicles on signalized and unsignalized intersections safety in mixed traffic,” _Future Transportation_ , vol. 2, no. 1, pp. 24–40, 2022.
  * [9] S. R. Deshpande, S. Gupta, A. Gupta, and M. Canova, “Real-Time Ecodriving Control in Electrified Connected and Autonomous Vehicles Using Approximate Dynamic Programing,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 144, no. 1, p. 011111, 01 2022.
  * [10] S. Bae, Y. Choi, Y. Kim, J. Guanetti, F. Borrelli, and S. Moura, “Real-time ecological velocity planning for plug-in hybrid vehicles with partial communication to traffic lights,” in _2019 IEEE 58th Conference on Decision and Control (CDC)_. IEEE, 2019, pp. 1279–1285.
  * [11] T. Ard, L. Guo, J. Han, Y. Jia, A. Vahidi, and D. Karbowski, “Energy-efficient driving in connected corridors via minimum principle control: Vehicle-in-the-loop experimental verification in mixed fleets,” _IEEE Transactions on Intelligent Vehicles_ , pp. 1–14, 2023.
  * [12] J. Han, D. Shen, J. Jeong, M. D. Russo, N. Kim, J. J. Grave, D. Karbowski, A. Rousseau, and K. M. Stutenberg, “Energy impact of connecting multiple signalized intersections to energy-efficient driving: Simulation and experimental results,” _IEEE Control Systems Letters_ , vol. 7, pp. 1297–1302, 2023.
  * [13] B. Liu, C. Sun, X. Wei, D. Wen, C. Ning, and H. Li, “Guided eco-driving of fuel cell hybrid electric vehicles via model predictive control,” in _2023 IEEE Vehicle Power and Propulsion Conference (VPPC)_ , 2023, pp. 1–6.
  * [14] S. K. Chada, A. Purbai, D. Görges, A. Ebert, and R. Teutsch, “Ecological adaptive cruise control for urban environments using spat information,” in _2020 IEEE Vehicle Power and Propulsion Conference (VPPC)_ , 2020, pp. 1–6.
  * [15] V. Jayawardana and C. Wu, “Learning eco-driving strategies at signalized intersections,” in _2022 European Control Conference (ECC)_ , 2022, pp. 383–390.
  * [16] Z. Bai, P. Hao, W. ShangGuan, B. Cai, and M. J. Barth, “Hybrid reinforcement learning-based eco-driving strategy for connected and automated vehicles at signalized intersections,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 9, pp. 15 850–15 863, 2022.
  * [17] J. Li, X. Wu, M. Xu, and Y. Liu, “Deep reinforcement learning and reward shaping based eco-driving control for automated hevs among signalized intersections,” _Energy_ , vol. 251, p. 123924, 2022.
  * [18] E. Joa, E. Y. Choi, and F. Borrelli, “Eco-driving under localization uncertainty for connected vehicles on urban roads: Data-driven approach and experiment verification,” _arXiv preprint arXiv:2402.01059_ , 2024.
  * [19] C. Sun, J. Guanetti, F. Borrelli, and S. J. Moura, “Optimal eco-driving control of connected and autonomous vehicles through signalized intersections,” _IEEE Internet of Things Journal_ , vol. 7, no. 5, pp. 3759–3773, 2020.
  * [20] _V2X Communications Message Set Dictionary™ Set_. SAE International, 2023.
  * [21] K. Ogata _et al._ , _Modern control engineering_. Prentice hall Upper Saddle River, NJ, 2010, vol. 5.
  * [22] Y. Jeong and K. Yi, “Target vehicle motion prediction-based motion planning framework for autonomous driving in uncontrolled intersections,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 1, pp. 168–177, 2019.
  * [23] V. Lefkopoulos, M. Menner, A. Domahidi, and M. N. Zeilinger, “Interaction-aware motion prediction for autonomous driving: A multiple model kalman filtering scheme,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 1, pp. 80–87, 2020.
  * [24] N. Nayakanti, R. Al-Rfou, A. Zhou, K. Goel, K. S. Refaat, and B. Sapp, “Wayformer: Motion forecasting via simple & efficient attention networks,” in _2023 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2023, pp. 2980–2987.
  * [25] M. Wang, Z. Wang, J. Talbot, J. C. Gerdes, and M. Schwager, “Game-theoretic planning for self-driving cars in multivehicle competitive scenarios,” _IEEE Transactions on Robotics_ , vol. 37, no. 4, pp. 1313–1325, 2021.
  * [26] P. Karle, M. Geisslinger, J. Betz, and M. Lienkamp, “Scenario understanding and motion prediction for autonomous vehicles—review and comparison,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 10, pp. 16 962–16 982, 2022.
  * [27] S. Moon, I. Moon, and K. Yi, “Design, tuning, and evaluation of a full-range adaptive cruise control system with collision avoidance,” _Control Engineering Practice_ , vol. 17, no. 4, pp. 442–455, 2009.
  * [28] G. De Nunzio, C. C. De Wit, P. Moulin, and D. Di Domenico, “Eco-driving in urban traffic networks using traffic signals information,” _International Journal of Robust and Nonlinear Control_ , vol. 26, no. 6, pp. 1307–1324, 2016.
  * [29] A. Sciarretta, G. De Nunzio, and L. L. Ojeda, “Optimal ecodriving control: Energy-efficient driving of road vehicles as an optimal control problem,” _IEEE control systems magazine_ , vol. 35, no. 5, pp. 71–90, 2015.
  * [30] E. Joa and F. Borrelli, “Approximate solution of stochastic infinite horizon optimal control problems for constrained linear uncertain systems,” _arXiv preprint arXiv:2401.12556_ , 2024.
  * [31] E. Joa, M. Bujarbaruah, and F. Borrelli, “Output feedback stochastic mpc with hard input constraints,” in _2023 American Control Conference (ACC)_ , 2023, pp. 2034–2039.
  * [32] F. Borrelli, A. Bemporad, and M. Morari, _Predictive control for linear and hybrid systems_. Cambridge University Press, 2017.
  * [33] R. Tedrake _et al._ , “Lqr-trees: Feedback motion planning on sparse randomized trees.” in _Robotics: Science and Systems_ , vol. 2009, 2009.
  * [34] M. Diehl, R. Amrit, and J. B. Rawlings, “A lyapunov function for economic optimizing model predictive control,” _IEEE Transactions on Automatic Control_ , vol. 56, no. 3, pp. 703–707, 2010.
  * [35] D. Bertsekas, _Dynamic programming and optimal control: Volume I_. Athena scientific, 2012.
  * [36] S. Bae, Y. Kim, Y. Choi, J. Guanetti, P. Gill, F. Borrelli, and S. J. Moura, “Ecological adaptive cruise control of plug-in hybrid electric vehicle with connected infrastructure and on-road experiments,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 144, no. 1, p. 011109, 2022.
  * [37] I. Yang, “A convex optimization approach to dynamic programming in continuous state and action spaces,” _Journal of Optimization Theory and Applications_ , vol. 187, no. 1, pp. 133–157, 2020.
  * [38] D. González, J. Pérez, V. Milanés, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” _IEEE Transactions on intelligent transportation systems_ , vol. 17, no. 4, pp. 1135–1145, 2015.
  * [39] D. Q. Mayne, M. M. Seron, and S. Raković, “Robust model predictive control of constrained linear systems with bounded disturbances,” _Automatica_ , vol. 41, no. 2, pp. 219–224, 2005.
  * [40] D. Bertsekas, _Convex optimization theory_. Athena Scientific, 2009.
  * [41] S. Diamond and S. Boyd, “CVXPY: A Python-embedded modeling language for convex optimization,” _Journal of Machine Learning Research_ , vol. 17, no. 83, pp. 1–5, 2016.
  * [42] M. ApS, _The MOSEK optimization toolbox for MATLAB manual. Version 10.0._ , 2022. [Online]. Available: http://docs.mosek.com/9.0/toolbox/index.html
  * [43] S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall, “Robot operating system 2: Design, architecture, and uses in the wild,” _Science Robotics_ , vol. 7, no. 66, p. eabm6074, 2022.
  * [44] E. Joa, H. Lee, E. Y. Choi, and F. Borrelli, “Energy-efficient lane changes planning and control for connected autonomous vehicles on urban roads,” in _2023 IEEE Intelligent Vehicles Symposium (IV)_. IEEE, 2023, pp. 1–6.
  * [45] S. Bae, Y. Kim, J. Guanetti, F. Borrelli, and S. Moura, “Design and implementation of ecological adaptive cruise control for autonomous driving with communication to traffic lights,” in _2019 American Control Conference (ACC)_ , 2019, pp. 4628–4634.
  * [46] N. Williams and M. Barth, “A qualitative analysis of vehicle positioning requirements for connected vehicle applications,” _IEEE Intelligent Transportation Systems Magazine_ , vol. 13, no. 1, pp. 225–242, 2020.
  * [47] Mobileye. (2017) The mapping challenge. [Online]. Available: https://www.mobileye.com/our-technology/rem/
  * [48] CohdaWireless, “White paper: V2x-locate location is everything in v2x,” Tech. Rep. [Online]. Available: https://www.cohdawireless.com/wp-content/uploads/2020/11/V2X-Locate-White-Paper-Final-Nov-2020.pdf
  * [49] VBOXAutomotive. Gps accuracy. [Online]. Available: https://www.vboxautomotive.co.uk/index.php/en/products/data-loggers/28-how-does-it-work/75-gps-accuracy#
  * [50] S. Boyd and L. Vandenberghe, _Convex optimization_. Cambridge university press, 2004.

###  -A Proof of Theorem 2

All data points in $\mathbf{X}$ and $\mathbf{U}$ satisfy the constraints (16)
by construction. We prove the rest of the claim by induction

For $p=0$, by definition of the robust controllable set [32, Def.10.18],
$\mathcal{R}_{0}=\mathcal{G}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{C}(\hat{s}_{t+N|k}^{pv},\hat{v}_{t+N|k}^{pv})$
is a $0$-step robust controllable set of the system (5) perturbed by the noise
(7) for the target convex set $\mathcal{G}$ subject to the constraints (16)
and the collision avoidance constraint (15) given the predicted behavior of
the preceding vehicle $\hat{s}_{t+N|k}^{pv},\hat{v}_{t+N|k}^{pv}$.

Suppose that for some $p\geq 0$, $\mathcal{R}_{p}$, declared in line 11 at
iteration $p-1$, is an $p$-step robust controllable set of the system (5)
perturbed by the noise (7) for the target convex set $\mathcal{G}$ subject to
the constraints (16) and (15) given the predicted behavior of the preceding
vehicle
$\\{\hat{s}_{i|k}^{pv}\\}_{i=t+N-p}^{t+N},\\{\hat{v}_{i|k}^{pv}\\}_{i=t+N-p}^{t+N}$.

At line 5, we find data points such that the state $[\mathbf{X}]_{j}$ of the
system (5) can be robustly steered to $\mathcal{R}_{i}$. subsequently, at line
$6$, the collision avoidance satisfaction is checked at the state
$[\mathbf{X}]_{j}$. Let
$\\{(\mathbf{\tilde{x}}_{s},~{}\tilde{u}_{s})\\}_{s=0}^{N_{\text{s}}}$ denote
a set of the data points that satisfy both conditions at line 5 and 6. Then,
by definitions [32, Def.10.15 & 18],
$\\{\mathbf{\tilde{x}}_{s}\\}_{s=0}^{N_{\text{s}}}$ are elements of the
$p+1$-step robust controllable set.

Now, we prove convex combinations of
$\\{\mathbf{\tilde{x}}_{s}\\}_{s=0}^{N_{\text{s}}}$ are also elements of the
$p+1$-step robust controllable set. First, we show that convex combinations of
data points
$\\{(\mathbf{\tilde{x}}_{s},~{}\tilde{u}_{s})\\}_{s=0}^{N_{\text{s}}}$ satisfy
the constraints (16). This is true because $\mathcal{X}$ and $\mathcal{U}$ are
convex sets. Second, we prove that convex combinations of data points
$\\{\mathbf{\tilde{x}}_{s}\\}_{s=0}^{N_{\text{s}}}$ satisfy the condition at
line 6, i.e., the given collision avoidance constraint
$\mathcal{C}(\hat{s}_{t+N-p-1|k}^{pv},\hat{v}_{t+N-p-1|k}^{pv})$. This is also
true because the given collision avoidance constraint represents a halfspace,
which is convex [50]. Third, we prove that convex combinations of
$\\{\mathbf{\tilde{x}}_{s}\\}_{s=0}^{N_{\text{s}}}$ can be robustly steered to
$\mathcal{R}_{p}$. $\mathcal{R}_{p}$ is convex as it was constructed by the
convex hull operation in line 11 at iteration $p-1$. The following holds for
all $0\leq\lambda\leq 1$:

$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\forall
s_{1},s_{2}\in\\{0,\cdots,N_{\text{s}}\\},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\mathbf{A}\mathbf{\tilde{x}}_{s_{1}}+\mathbf{B}\tilde{u}_{s_{1}}+\mathbf{D}n_{s_{1}}\in\mathcal{R}_{p},\forall
n_{s_{1}}\in 2L\mathcal{W},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\mathbf{A}\mathbf{\tilde{x}}_{s_{2}}+\mathbf{B}\tilde{u}_{s_{2}}+\mathbf{D}n_{s_{2}}\in\mathcal{R}_{p},\forall
n_{s_{2}}\in 2L\mathcal{W}.$ $\displaystyle\implies\forall
s_{1},s_{2}\in\\{0,\cdots,N_{\text{s}}\\},\forall n_{s_{1}},n_{s_{2}}\in
2L\mathcal{W},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\mathbf{\tilde{x}}_{c}\coloneqq\lambda\mathbf{\tilde{x}}_{s_{1}}+(1-\lambda)\mathbf{\tilde{x}}_{s_{2}},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\mathbf{\tilde{u}}_{c}\coloneqq\lambda\mathbf{\tilde{u}}_{s_{1}}+(1-\lambda)\mathbf{\tilde{u}}_{s_{2}},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}n_{c}\coloneqq\lambda
n_{s_{1}}+(1-\lambda)n_{s_{2}},$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}\mathbf{A}\mathbf{\tilde{x}}_{c}+\mathbf{B}\mathbf{\tilde{u}}_{c}+\mathbf{D}n_{c}\in\mathcal{R}_{p}~{}(\because\mathcal{R}_{p}\text{
is convex.}).$

Moreover, as $\mathcal{W}$ is a convex set, $n_{c}$ belongs to the set
$2L\mathcal{W}$ and can represent all realizations of noise in
$2L\mathcal{W}$. Thus, all convex combinations of
$\\{\mathbf{\tilde{x}}_{s}\\}_{s=0}^{N_{\text{s}}}$ are also elements of the
$p+1$-step robust controllable set. Therefore, $\mathcal{R}_{p+1}$,
constructed in line 11 by convex hull operation on the set
$\mathbb{X}=\textbf{A}$, is an $i+1$-step robust controllable set when
$\mathcal{R}_{p}$ is an $p$-step robust controllable set. By induction, the
claim is proved.

###  -B Constraint reformulation

From the dynamics of the estimated and the nominal state in (21) and (7), we
have that:

$\displaystyle\mathbf{e}_{i|k}=\hat{\mathbf{x}}_{i|k}-\bar{\mathbf{x}}_{i|k},$
$\displaystyle\mathbf{e}_{i+1|k}=\mathbf{A}\mathbf{e}_{i|k}+\mathbf{D}n_{i},~{}n_{i}\sim
p_{n}(n).$

From the initial constraint in (21), $\mathbf{e}_{0|k}=0$. Moreover,
$\mathbf{A}^{i}\mathbf{D}=\mathbf{D}$ by calculation. Thus, for all $i\geq 1$,
we have that:

$\displaystyle\mathbf{e}_{i|k}=\sum_{k=0}^{i-1}\mathbf{A}^{k}\mathbf{D}n_{k}=\mathbf{D}\sum_{k=0}^{i-1}n_{k}.$
(44)

From (7), we know that $n_{k}\in 2L\mathcal{W},~{}\forall k\geq 0$. Thus, for
all $i\geq 1$, we have that:

$\displaystyle\mathbf{e}_{i|k}=\mathbf{D}\sum_{k=0}^{i-1}n_{k}\in
2Li\mathbf{D}\mathcal{W}.$ (45)

Based on the result in (45), the constraints in (21) is reformulated with
respect to the nominal state.

First, we consider the state and input constraints in (21). These constraints
do not depend on the noisy position by definition in (16). Moreover, the speed
of the nominal state propagated from
$\bar{\mathbf{x}}_{0|k}=\hat{\mathbf{x}}_{k}$ is identical to that of the
estimated state. Thus, we have that:

$\displaystyle\bar{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{i|k}\in\mathcal{U}\iff\hat{\mathbf{x}}_{i|k}\in\mathcal{X},~{}u_{k}\in\mathcal{U}.$

Second, we reformulate the collision avoidance constraints, the terminal
constraints, and the traffic light constraints in (21). From (45), the
following holds for any given set $\mathcal{S}$:

$\bar{\mathbf{x}}_{i|k}\in\mathcal{S}\ominus
2Li\mathbf{D}\mathcal{W}\implies\hat{\mathbf{x}}_{i|k}\in\mathcal{S}.$ (46)

Thus, the following hold for all $n_{k}\in 2L\mathcal{W}$:

$\displaystyle\bar{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}_{i|k}^{pv},\hat{v}_{i|k}^{pv})\ominus
2Li\mathbf{D}\mathcal{W}\implies\hat{\mathbf{x}}_{i|k}\in\mathcal{C}(\hat{s}_{i|k}^{pv},\hat{v}_{i|k}^{pv}),$
(47) $\displaystyle\bar{\mathbf{x}}_{N|k}\in\mathcal{X}_{f}\ominus
2LN\mathbf{D}\mathcal{W}\implies\hat{\mathbf{x}}_{N|k}\in\mathcal{X}_{f},$
$\displaystyle\bar{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus(2Li+1)\mathbf{D}\mathcal{W}\implies\hat{\mathbf{x}}_{i|k}\in\mathcal{L}(s_{\mathrm{tl}})\ominus\mathbf{D}\mathcal{W}.$

###  -C Proof of Theorem 3

If $k+N\leq T_{l^{\star}}$, the MPC controller in (39) is employed. Let
$u_{0:N-1|k}^{\star}$ denote the optimal input sequence of (39). From the
terminal constraint and (47), the system (5) controlled by
$u_{0:N-1|k}^{\star}$ robustly steers to the terminal set
$\mathcal{X}_{f}=\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}}$
from $\hat{\mathbf{x}}_{k}$, namely:

$\hat{\mathbf{x}}_{N|k}\in\mathcal{X}_{f}=\mathcal{S}_{t_{\text{red}}}\mathbin{\scalebox{0.8}{$\bigcap$}}\mathcal{P}_{t_{\text{green}}},~{}\forall
n_{k}\in 2L\mathcal{W}.$ (48)

As $\mathcal{P}_{t_{\text{green}}}$ is a $t_{\text{green}}$-step robust
controllable set to the target set $\hat{\mathcal{G}}_{p}$, there exists a
control input sequence that robustly steers to $\hat{\mathcal{G}}_{p}$ from
$\hat{\mathbf{x}}_{N|k}$, i.e.,
$\hat{\mathbf{x}}_{N+t_{\text{green}}|k}\in\hat{\mathcal{G}}_{p}$. Moreover,
as $\hat{\mathcal{G}}_{p}={\mathcal{G}}_{p}\ominus\mathbf{D}\mathcal{W}$
described in Remark 8,
$\hat{\mathbf{x}}_{N+t_{\text{green}}|k}\in\hat{\mathcal{G}}_{p}\implies{\mathbf{x}}_{N+t_{\text{green}}|k}\in{\mathcal{G}}_{p}$,
which represents that the system (2) robustly steers to ${\mathcal{G}}_{p}$,
the region after the upcoming traffic light. This proves the claim for
$k+N\leq T_{l^{\star}}$.

If $k+N>T_{l^{\star}}$, the MPC controller in (40) is employed. Let
$u_{0:N_{s}-1|k}^{\star}$ denote the optimal input sequence of (40). From the
terminal constraint and (47), the system (5) controlled by
$u_{0:N_{s}-1|k}^{\star}$ robustly steers to the terminal set
$\hat{\mathcal{G}}_{p}$. Similar to the previous case, this implies that the
system (2) robustly steers to ${\mathcal{G}}_{p}$, the region after the
upcoming traffic light. This proves the claim for $k+N>T_{l^{\star}}$.

The claim is proved in both cases.

|  Eunhyek Joa (Member, IEEE) received the B.S. and M.S degrees in mechanical
engineering from Seoul National University, Seoul, Korea, in 2014 and 2016,
respectively. He is currently working toward a Ph.D. degree in mechanical
engineering at the University of California at Berkeley sponsored by the
Korean Government Scholarship Program. He was a research engineer with the
Institute of Advanced Machines and Design at Seoul National University, Seoul,
Korea, from 2016 to 2020. His research interests include data-driven control,
constrained optimal control, and model predictive control and their
applications to advanced automotive control.
---|---
|  Eric Yongkeun Choi (Member, IEEE) is a postdoctoral researcher at
University of California, Berkeley. He received B.S. and Ph.D. degrees in
Mechanical Engineering at University of California, Berkeley. He has been
working on the ARPA-E NEXTCAR research during his Ph.D. and as a postdoctoral
researcher. His research interests include model predictive control, data-
driven optimal control, and their applications to vehicle dynamics and
powertrain control, and energy management of electrified vehicles.
---|---
|  Francesco Borrelli (Fellow, IEEE) received the Laurea degree in computer
science engineering from the University of Naples Federico II, Naples, Italy,
in 1998, and the Ph.D. degree from ETH Zürich, Zürich, Switzerland, in 2002.
He is currently a Professor with the Department of Mechanical Engineering,
University of California at Berkeley, Berkeley, CA, USA. He is the author of
more than 100 publications in the field of predictive control and the author
of the book Constrained Optimal Control of Linear and Hybrid Systems (Springer
Verlag). His research interests include constrained optimal control, model
predictive control, and its application to advanced automotive control and
energy-efficient building operation. He was a recipient of the 2009 National
Science Foundation CAREER Award and the 2012 IEEE Control System Technology
Award. In 2008, he was appointed as the Chair of the IEEE Technical Committee
on Automotive Control.
---|---