RESEARCH ARTICLE MPC, model predictive control. [1]Equally contributing
authors. Filip Surma, Aerospace Engineering Faculty, TU Delft, 2629 HS, Delft,
South Holland, The Netherlands<EMAIL_ADDRESS>This research has been
supported by the NWO Talent Program Veni project “Autonomous drones flocking
for search-and-rescue” (18120), which has been financed by the Netherlands
Organisation for Scientific Research (NWO).

# State-Dependent Dynamic Tube MPC: A Novel Tube MPC Method with a Fuzzy Model
of Disturbances

Filip Surma MSc  Anahita Jamshidnejad PhD

###### Abstract

Most real-world systems are affected by external disturbances, which may be
impossible or costly to measure. For instance, when autonomous robots move in
dusty environments, the perception of their sensors is disturbed. Moreover,
uneven terrains can cause ground robots to deviate from their planned
trajectories. Thus, learning the external disturbances and incorporating this
knowledge into the future predictions in decision-making can significantly
contribute to improved performance. Our core idea is to learn the external
disturbances that vary with the states of the system, and to incorporate this
knowledge into a novel formulation for robust tube model predictive control
(TMPC). Robust TMPC provides robustness to bounded disturbances considering
the known (fixed) upper bound of the disturbances, but it does not consider
the dynamics of the disturbances. This can lead to highly conservative
solutions. We propose a new dynamic version of robust TMPC (with proven robust
stability), called state-dependent dynamic TMPC (SDD-TMPC), which incorporates
the dynamics of the disturbances into the decision-making of TMPC. In order to
learn the dynamics of the disturbances as a function of the system states, a
fuzzy model is proposed. We compare the performance of SDD-TMPC, MPC, and TMPC
via simulations, in designed search-and-rescue scenarios. The results show
that, while remaining robust to bounded external disturbances, SDD-TMPC
generates less conservative solutions and remains feasible in more cases,
compared to TMPC.

###### keywords:

Robust tube model predictive control, Fuzzy-logic-based modeling, State-
dependent disturbances

##  1 Introduction

Model predictive control (MPC) [1, 2] is a state-of-the-art control approach
that can optimize multiple control objectives, handle state and input
constraints, and provide guarantees on stability and feasibility. MPC heavily
relies on a model that predicts the evolution of the states of the controlled
system across a given prediction horizon. Thus, unmodeled external
disturbances may lead to dangerous situations in real-life applications. For
instance, search-and-rescue (SaR) robots should operate autonomously in
unknown environments that are prone to external disturbances [3, 4].
Therefore, for mission planning SaR robots need control methods that next to
optimize the objectives of the SaR mission (e.g., maximizing the coverage of
the area in the smallest possible time) and satisfying the constraints, also,
deal with external disturbances that pose risks to the mission. Thus, MPC
methods that can systematically handle the disturbances are promising for SaR
robots.

Various extensions to standard MPC, including robust and stochastic MPC, have
been developed [5]. In this paper, we focus on robust tube MPC (TMPC) [6],
which provides robustness to bounded disturbances, without significantly
increasing the online computation time. TMPC generates control inputs that are
composed of two parts: a nominal and an ancillary control input. The nominal
states and control inputs of the system are determined by solving the nominal
version of the MPC optimization (obtained by excluding external disturbances).
The nominal states determine the centroids of a tube (i.e., a set of possible
future states given chosen inputs and bounded disturbances) that propagates
across the prediction horizon. The cross sections of the tube lie within the
state space of the system, and as long as the realized states (i.e., the
states in the presence of the external disturbances) at the corresponding time
step remain in these cross sections, robustnessis guaranteed. The ancillary
controller ensures that the realized states remain inside the tube.

Figure 1: Comparison of SDD-TMPC and TMPC, where the sequences of the circular
areas in both plots illustrate the evolution of the tube based on a prediction
at the current time step. Left plot: The tube in SDD-TMPC (shown via the
smaller varying green circles) is dynamic, and always a subset of the tube of
regular TMPC (shown via the larger blue circles). Thus, SDD-TMPC allows the
system to perform closer to the boundaries of hard constraints, and to
potentially improve the performance, without violating these constraints.
Right plot: The nominal trajectory (shown via the blue stars) and the tube of
SDD-TMPC (shown via the green varying circles) are illustrated when the states
are constrained to remain within a set shown by the black boundary curves.
SDD-TMPC allows, e.g., by reducing the speed of a robot, to formulate a
feasible problem and provide solutions for it, and to safely move through the
narrow corridor.

The nominal MPC in TMPC is not aware of the dynamics of the external
disturbances. Thus, while the simplicity of the nominal optimization problem
leads to a lower computation time, this may be at the cost of compromising the
performance and generating overly conservative solutions, especially for
systems with nonlinear dynamics and prone to dynamic disturbances. Moreover,
existing MPC methods, including TMPC, do not naturally allow for incorporation
of the expert knowledge. This gap should be addressed, particularly when
extensive expert knowledge is available, but humans cannot or should not
participate in real-time control procedures, for instance for autonomous
onboard control of SaR robots.

Correspondingly, the main contributions of this paper are:

  * •

We introduce SDD-TMPC, a novel extension to TMPC. SDD-TMPC incorporates the
dynamics of the external disturbances into its nominal MPC formulation and
generates tubes that are subsets of the tube of TMPC. SDD-TMPC reduces the
conservatives of the solutions (see Figure 1) and results in improved
performance, with affordable computational efforts. We also prove the robust
stability of SDD-TMPC.

  * •

We propose a novel way to learn the dynamics of disturbances, initiated by the
existing expert knowledge, and using a fuzzy inference system. While other
modeling approaches, e.g., neural networks, require an extensive data set to
train the initial model, a fuzzy inference system (FIS) can easily be
initiated with human knowledge and then be tuned online to adapt to the
changing environment.

  * •

We compare the performance of SDD-TMPC with regular MPC and TMPC for a linear
system with state-dependent external disturbances that correspond to an
autonomous robot navigating in an unknown environment. The results confirm
that SDD-TMPC outperforms both regular MPC and TMPC by generating less
conservative control inputs that still guarantee the satisfaction of the hard
constraints.

The rest of the paper is structured as follows. In Section 2, we discuss the
related work and identify the open challenges and limitations of the state-of-
the-art methods. In Section 3, we explain our proposed approaches, including
SDD-TMPC, and the fuzzy model of disturbances. We also provide proof for the
stability of SDD-TMPC. In Section 4, we implement and compare the performance
of MPC, TMPC and SDD-TMPC via computer simulations for a ground robot.
Finally, Section 5 concludes the paper and suggests topics for future
research. The abbreviations and mathematical notations that are frequently
used in this paper are listed in Table 1 and Table 2, respectively.

Table 1: Frequently used abbreviations and their explanation Abbreviation | Explanation | Abbreviation | Explanation
---|---|---|---
A$i$ | The $i^{\text{th}}$ assumption | SaR | Search and Rescue
FIS | Fuzzy inference system | SDD-TMPC | State-dependent dynamic tube model predictive control
GA | Genetic algorithm | TSK-FIS | Takagi-Sugeno-Kang fuzzy inference system
MPC | Model predictive control | TMPC | Tube model predictive control
PS | Particle swarm algorithm | ZOH | Zero-order hold
Table 2: Frequently used mathematical notations and their definition

Notation | Definition | Notation | Definition
---|---|---|---
$k$ |  Time step counter in the discrete time domain | $\mathcal{K}$ |  A class of continuous and strictly increasing functions starting at zero
$N$ |  Prediction horizon | $\mathcal{K}_{\infty}$ |  A subclass of $\mathcal{K}$
$\bm{z}_{k}$ |  Nominal state vector | $\mathbb{W}(\bm{x}_{k})$ |  Set of all possible disturbances given state $\bm{x}_{k}$ of the system at time step $k$
$\bm{x}_{k}$ |  State vector at time step $k$ for a given dynamical system | $\bar{\mathbb{W}}_{k}$ |  Set of all possible disturbances for all possible states estimated at time step $k$
$e_{k}$ |  Error between the real and the nominal state for time step $k$ | $\mathbb{X}$ |  Admissible set for the states
$\bm{w}(\bm{x}_{k})$ |  Disturbance vector when the state of the system is $\bm{x}_{k}$ | $\mathbb{Z}_{f}$ |  Terminal set for the nominal states
$\bm{v}_{k}$ |  Nominal input vector | $\mathbb{U}$ |  Admissible set for the control inputs
$\bm{u}_{k}$ |  Input vector | $\mathbb{N}$ |  Set of natural numbers
$\tilde{\bm{z}}_{k}$ |  Sequence of the nominal state vectors across the prediction horizon (i.e., $\\{\bm{z}_{k+1},\ldots,\bm{z}_{k+N}\\}$) estimated at time step $k$ | $\mathbb{R}$ |  Set of real numbers
$\tilde{\bm{x}}_{k}$ |  Sequence of the state vectors across the prediction horizon (i.e., $\\{\bm{x}_{k+1},\ldots,\bm{x}_{k+N}\\}$) estimated at time step $k$ | $\bar{\mathbb{E}}_{k}$ |  Error set estimated for time step $k$, assuming an autonomous evolution of the error set (excluding the influence of the disturbances at previous time step $k-1$)
$\tilde{\bm{v}}_{k}$ |  Sequence of the nominal input vectors across the prediction horizon (i.e., $\\{\bm{v}_{k},\ldots,\bm{v}_{k+N-1}\\}$) estimated at time step $k$ | $\mathbb{E}_{k}$ |  Set of all possible errors between the real and the nominal state for time step $k$
$\tilde{\bm{u}}_{k}$ |  Sequence of the input vectors across the prediction horizon (i.e., $\\{\bm{u}_{k},\ldots,\bm{u}_{k+N-1}\\}$) estimated at time step $k$ | $\mathbb{T}^{\max}_{k}$ |  Tube of regular robust TMPC for prediction horizon $\\{k+1,\ldots,k+N\\}$ with a fixed cross section area $\mathbb{E}^{\max}$
$\pi(\cdot)$ |  Ancillary control law | $\mathbb{T}_{k}$ |  Tube of SDD-TMPC for prediction horizon $\\{k+1,\ldots,k+N\\}$ with a dynamic cross section area $\mathbb{E}_{i}$ for $i=k+1,\ldots,k+N$
$\epsilon(\cdot)$ |  Autonomous error dynamic function, which determines $\bar{\mathbb{E}}_{k}$ based on $\mathbb{E}_{k-1}$ | $\oplus$ |  Minkowski summation
$(\cdot)_{i|k}$ |  prediction of a dynamic variable for time step $i$, given the measurements at time step $k$ | $\ominus$ |  Minkowski difference

##  2 Related work

##### Modeling via fuzzy inference systems

FIS can approximate any nonlinear function, when the inputs of the function
are bounded [7]. Various supervised learning algorithms, e.g., gradient
descent [8], learning from example [9], and genetic algorithm (GA) [10] have
been proposed for the FIS to learn such functions from data. One of the most
common FISs used in various applications, including robotics, is Takagi-
Sugeno-Kang (TSK) FIS. TSK FISs can be updated online in a stable way using,
e.g., reinforcement learning (see applications to robot navigation [11],
balancing a bipedal robot [12], controlling a continuum robot [13]). To the
best of our knowledge, FISs have never been used to model the dynamics of
external disturbances, especially for providing robustness for model-based
control approaches, e.g., MPC.

##### Robust MPC

Linear MPC is a well-known, theoretically mature control approach that has
successfully been implemented in many applications [14, 15]. Linear MPC was
utilized to regulate multiple vehicles across various domains such as
spacecraft altitude control [16], ground vehicle steering [17], and drone
trajectory tracking [18]. The LMPC framework incorporates a linear system,
linear constraints, and a quadratic cost function. In real-life problems,
including SaR, in order to deal with nonlinear cost functions and nonlinear
constraints, a nonlinear version of MPC may be used. Nonlinear model
predictive control (NMPC) has been implemented in various applications
including exploration of a grid world with multiple agents [19], decision
making for simultaneous localization and mapping (SLAM) [20], obstacle
avoidance [21], and global path planning [22]. Although MPC is robust to small
disturbances (due to operating upon state feedback), linear and nonlinear MPC
cannot deal with large external disturbances [5]. Therefore, robust MPC
approaches have been introduced.

One of the most attractive robust MPC methods is robust tube MPC (TMPC) [6],
where a robust control strategy that is designed offline is used to keep the
state trajectory within an invariant tube, the center-line of which is a
nominal trajectory of the states that is determined online. For the sake of
brevity of the notations, we refer to robust TMPC as TMPC in the rest of the
paper. The nominal state trajectory can be determined for the system whenever
the controller has a perfect model of the system and there are no
disturbances. Although the nominal states are predictable in such cases, the
actual states cannot be known in advance because the external disturbances are
unknown. However, since the disturbances are assumed to be bounded and thus
the maximum error between the actual and the nominal states can be determined,
it is possible to compute a sequence $\mathbb{T}^{\max}_{k}$ of $N$ sets per
time step $k$, where each set $\\{z_{i|k}\\}\oplus\mathbb{E}^{\max}$ within
this sequence includes all the possible actual states for a given time step
$i$ within the prediction window $\\{k+1,\ldots,k+N\\}$, and $N$ is the
prediction horizon of TMPC. Then the state constraints are guaranteed to be
satisfied within the prediction window, if all the states within sequence
$\mathbb{T}^{\max}_{k}$ satisfy the constraints. In order to prevent the
actual states from deviating significantly from the corresponding nominal
states, an ancillary control law, e.g., an error state feedback controller
[6], another MPC controller [23], or a sliding mode control [21], is used to
reduce the error between the actual and the nominal state.

There are two common ways of designing and implementing TMPC [24]:

  * •

The first approach involves determining a single error set (i.e., tube) for
the entire prediction window based on the error dynamics and the selected
ancillary control law, where this error set serves as a positive robust
invariant set. In other words, if the current error belongs to this positive
robust invariant set, then all the future errors will remain within the set.
This approach benefits from a low online computational complexity, since the
positive robust invariant set can be computed offline. Moreover, tighter
constraints can be imposed on the nominal set, such that if the center of the
positive robust invariant set is inside the tightened constraints, then the
actual state remains in the original feasible set. However, using this
approach may result in more restrictive control inputs.

  * •

The second approach involves computing the error set per time step within the
prediction window. This approach results in a sequence of regions, known as
reachable sets, which are the smallest sets of possible states for a system
prone to external disturbances that guarantee that the states remain in these
sets for all time steps. This approach may yield less conservative solutions
for TMPC, but requires additional online computations compared to the first
approach.

Some variations of TMPC for nonlinear systems have been proposed. Nonlinear
TMPC is much more challenging than linear TMPC, particularly because it is not
trivial to choose an ancillary control law and to design a tube. One way to
reduce the computational complexity of nonlinear TMPC is by using
parameterized TMPC, such as the approach in [25], which reduces the required
online optimization into a linear programming problem. Another alternative is
to employ ellipsoids as tubes, instead of polytopic sets (see, e.g., [26]).
Some examples of nonlinear TMPC can be found in [23, 21, 27, 28, 29].

The first implementation of robust MPC (although not TMPC) when state-
dependent disturbances exist includes [30]. In [31] the optimization problem
of robust MPC has been handled as a 2nd-order cone program. This approach,
however, can only be used for linear systems subject to a certain group of
additive disturbances (i.e., disturbances defined as the summation of an
independent component from a polytope and a state and input-dependent
component bounded via a non-convex inequality). Similarly, in [32] robust MPC
has been discussed for a special class of state-dependent disturbances.
However, our proposed TMPC-based approach, i.e., SDD-TMPC, is not limited to
any type of nonlinearity. Authors in [21] propose a stable controller for an
agent that should avoid obstacles in an environment with disturbances that are
proportional to the square of its velocity. The proposed method, however, is
restricted to a sliding-mode ancillary control law, and thus can only be used
for feedback-linearizable or minimum-phase systems. In [33], a dynamic tube is
used and is parameterized as a sublevel set of incremental Lyapunov functions.
This method is extended in [34], where chance constraints and state or input-
dependent disturbances are included. This paper has the most similarities with
SDD-TMPC, since it uses a dynamic tube that evolves in time, and tightens the
constraints online. Using a sub-level set of incremental Lyapunov functions
reduces the conservativeness with a small increase in parameters and equations
(and probably in computations) compared to other approaches in the literature.
The parameterization, however, restricts the shape of the tube, which
potentially leads to conservative decisions. In [27], a neural network has
been used to learn the dynamics of the tube of robust TMPC. The neural
network, however, cannot be initialized without an extensive dataset, and thus
another controller should be used until a sufficient number of data is
gathered. Moreover, no proof has been provided that the trajectory of the
states remains inside the tube. In [28], a Gaussian process is used to learn
the disturbance set. The stability of the algorithm for linear systems has
been proved. However, this method requires heavy offline computations, and
discretization of the state space, and may thus become intractable for systems
with large state spaces.

SDD-TMPC is proved to be stable under standard assumptions. It can be used in
unknown and time-varying environments and can be initialized with intuitive
rules and be tuned online. Unlike most state-of-the-art methods, SDD-TMPC is
not limited to any specific class of nonlinearity and does not restrict the
choice of the ancillary control law. Moreover, unlike state-of-the-art methods
that assume the bounds or the probability distribution of the disturbances are
known, for SDD-TMPC it is possible to learn the disturbances online.

##  3 SDD-TMPC: Idea, formulation, and stability

In this section, we describe and formulate the problem and discuss the
proposed methods for SDD-TMPC.

###  3.1 Problem statement

We consider a dynamic system that is described in discrete time with the state
vector $\bm{x}_{k}$ and control input $\bm{u}_{k}$, and is affected by
additive state-dependent external disturbances $\bm{w}(\bm{x}_{k})$ at time
step $k\in\mathbb{N}$, where $\bm{x}_{k}\in\mathbb{X}\
\subseteq\mathbb{R}^{n}$, $\bm{u}_{k}\in\mathbb{U}\ \subseteq\mathbb{R}^{m}$,
and $\bm{w}(\bm{x}_{k})\in\mathbb{W}(\bm{x}_{k})\ \subseteq\mathbb{R}^{n}$.
While the admissible sets $\mathbb{X}$ and $\mathbb{U}$ for, respectively, the
state and the control input are static, we allow the admissible set
$\mathbb{W}(\bm{x}_{k})$ of the external disturbances to vary as a function of
the state. Learning (an approximate of) the admissible set
$\mathbb{W}(\bm{x}_{k})$, such that it bounds the external disturbances per
state, instead of considering a set that bounds all potential external
disturbances for the entire admissible set $\mathbb{X}$ in time, will
introduce dynamics and reduced conservativeness into SDD-TMPC.

The dynamic system is given by:

$\bm{x}_{k+1}=f(\bm{x}_{k},\bm{u}_{k})+\bm{w}(\bm{x}_{k})$ (1)

where $f(\cdot)$ is a time-invariant time-discrete nonlinear Lipschitz
function.

We assume that the states are measured by perfect sensors, i.e., per time step
$k$ the value of the state $\bm{x}_{k}$ is perfectly known, whereas the
external disturbances are unknown and may take any arbitrary value within
$\mathbb{W}(\bm{x}_{k})$. The aim is to control the dynamics of system (1),
such that a given cost function $J(\cdot)$ is minimized across a prediction
horizon of size $N$, while it is guaranteed for the controlled system that for
all admissible external disturbances the hard constraints are always
satisfied. Thus, for time step $k$ we have:

$\displaystyle J^{*}(\bm{x}_{k})=$
$\displaystyle\min_{\tilde{\bm{x}}_{k},\tilde{\bm{u}}_{k}}J(\tilde{\bm{x}}_{k},\tilde{\bm{u}}_{k})$
(2) s.t. for $i=k,\ldots,k+N-1$
$\displaystyle\quad\bm{x}_{i+1|k}=f(\bm{x}_{i|k},\bm{u}_{i|k})+\bm{w}(\bm{x}_{i|k})$
$\displaystyle\quad\bm{x}_{k|k}=\bm{x}_{k}$
$\displaystyle\quad\bm{x}_{i+1|k}\in\mathbb{X}$
$\displaystyle\quad\bm{u}_{i|k}\in\mathbb{U}$
$\displaystyle\quad\bm{w}(\bm{x}_{i|k})\in\mathbb{W}(\bm{x}_{i|k})$

with
$\tilde{\bm{x}}_{k}=[\bm{x}^{\top}_{k+1|k},\ldots,\bm{x}^{\top}_{k+N|k}]^{\top}$,
$\bm{x}_{i|k}$ for $i>k$ the value of $\bm{x}_{i}$ predicted at time step $k$,
$\tilde{\bm{u}}_{k}=[\bm{u}^{\top}_{k|k},\ldots,\bm{u}^{\top}_{k+N-1|k}]^{\top}$,
$\bm{u}_{i|k}$ for $i\geq k$ the value of $\bm{u}_{i}$ determined at time step
$k$, and $J^{*}(\bm{x}_{k})$ an optimal value for the cost function obtained
by solving the constrained minimization problem. The cost function $J(\cdot)$
is composed of a stage cost that is accumulated across the prediction horizon
$\\{k,\ldots,k+N-1\\}$, and a terminal cost that is computed at the terminal
time step $k+N$. The cost function is continuous and finite for
$\bm{x}_{i}\in\mathbb{X},\ \forall i\in\\{k,\ldots,k+N\\}$.

###  3.2 State-dependent dynamic tube MPC (SDD-TMPC)

TMPC works based on the assumption of bounded external disturbances, where the
boundary set $\mathbb{W}^{\max}$ for the disturbances is known and given [6].
A main challenge for SDD-TMPC in solving (2) per time step $k$ is that the
dynamic set $\mathbb{W}(\bm{x}_{i})$ (for $i=k,\ldots,k+N-1$) is not known
(neither for the current time step nor for the future time steps) and should
thus be estimated. The link between the known static set $\mathbb{W}^{\max}$
and the dynamic set $\mathbb{W}(\bm{x}_{i})$ of disturbances that should be
estimated by SDD-TMPC for every prediction horizon $\\{k+1,\ldots,k+N\\}$ is
given by:

$\bigcup_{\begin{subarray}{c}i=k,\ldots,k+N-1\end{subarray}}\mathbb{W}(\bm{x}_{i})\subseteq\mathbb{W}^{\max}$
(3)

We assume that $\mathbb{W}(\bm{x}_{i})$ always contains the origin. Regular
TMPC uses $\mathbb{W}^{\max}$ to determine a sequence of $N$ control inputs
and a tube $\mathbb{T}_{k}^{\max}$ for the entire prediction window per time
step $k$, for which the cross section $\mathbb{E}^{\max}$ remains unchanged.
The tube $\mathbb{T}_{k}^{\max}$ is a robust positive invariant set. The main
difference between TMPC and our proposed approach, SDD-TMPC, is in the
formulation of their nominal MPC. This allows SDD-TMPC to make use of the
dynamics of the external disturbances, in order to generate per time step $k$
a tube $\mathbb{T}_{k}$ with a generally time-varying cross section
$\mathbb{E}_{i}$ for $i=k+1,\ldots,k+N$ across the prediction horizon that
improves the control performance by generating less conservative solutions.
Note that set $\mathbb{T}_{k}$ is a robust control invariant set, i.e., there
is at least one control policy that prevents the state trajectory from leaving
the tube. The nominal problem of SDD-TMPC is given by:

$V^{*}(\bm{x}_{k},\bm{z}_{k})=\min_{\tilde{\bm{z}}_{k},\tilde{\bm{v}}_{k},\mathbb{T}_{k}}\sum_{i=k}^{k+N-1}l(\bm{z}_{i|k},\bm{v}_{i|k})+V_{\text{f}}(\bm{z}_{k+N|k})$
(4a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{z}_{i|k}=f(\bm{z}_{i-1|k},\bm{v}_{i-1|k})$ (4b)
$\bar{\mathbb{E}}_{i|k}=\epsilon(\mathbb{E}_{i-1|k})$ (4c)
$\bar{\mathbb{W}}_{i-1|k}=\text{FIS}\left(\\{\bm{z}_{i-1|k}\\}\oplus\mathbb{E}_{i-1|k}\right)$
(4d) $\mathbb{E}_{{i|k}}=\bar{\mathbb{E}}_{i|k}\oplus\bar{\mathbb{W}}_{i-1|k}$
(4e) $\mathbb{E}_{k}=\\{\bm{x}_{k}-\bm{z}_{k}\\}$ (4f)
$\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k}\subseteq\mathbb{X}$ (4g)
$\bm{z}_{N|k}\in\mathbb{Z}_{\text{f}}$ (4h)
${\mathbb{T}_{k}=\left\\{\\{z_{k+1|k}\\}\oplus\mathbb{E}_{k+1|k},\ldots,\\{z_{N|k}\\}\oplus\mathbb{E}_{k+N|k}\right\\}}$
(4i)
$\pi\left(\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k},\bm{v}_{i|k},\bm{z}_{i|k}\right)\subseteq\mathbb{U}$
(4j)

In (4a), $l(\cdot)$ is the stage cost, $V_{\text{f}}(\cdot)$ is the terminal
cost, and $\tilde{\bm{z}}_{k}$ and $\tilde{\bm{v}}_{k}$ are the
trajectory/sequence of, respectively, the nominal states and the nominal
control inputs within the prediction window $\\{k+1,\ldots,k+N\\}$. The
realized states of the system follow the nominal state sequence when there are
no external disturbances. The nominal states $\bm{z}_{i|k}$ are predicted at
time step $k$ for time step $i=k+1,\ldots,k+N$ according to (4b). Moreover,
$\oplus$ represents the Minkowski addition [35], which for every two given
sets $A$ and $B$ is defined by:

$\mathbb{A}\oplus\mathbb{B}=\\{a+b:a\in\mathbb{A},b\in\mathbb{B}\\}$ (5)

The error set $\mathbb{E}_{i|k}$ (for $i=k+1,\ldots,k+N$) for SDD-TMPC is
computed in three steps: First, in (4c) the system uses the autonomous error
dynamic function $\epsilon(\cdot)$ to estimate the evolved error set, without
considering the influence of the state-dependent disturbances on the errors.
The autonomous error dynamic function is determined via the state-of-the-art
methods and based on the nominal model $f(\cdot)$ of the system dynamics and
the policy $\pi(\cdot)$, assuming no external disturbances exists. Note that
the error vector is defined in the prediction window by
$\bm{e}_{i|k}=\bm{x}_{i|k}-\bm{z}_{i|k}$ for $i=k+1,\ldots,k+N$. For details
on how $\epsilon(\cdot)$ can be determined, see, e.g., [6], for linear TMPC
and [21] for nonlinear TMPC. Second, in (4d) a mapping (in this case a fuzzy
inference system, FIS) is used to determine an admissible set
$\bar{\mathbb{W}}_{i-1|k}$ per time step $i$ ($i=k+1,\ldots,k+N$) that
contains all the possible disturbances at time step $i-1$ that can result in
all the possible states at time step $i$, which are obtained based on the
estimated nominal states (see (4b)) and the error set corresponding to the
previous time step. Note that the mapping $\text{FIS}(\cdot)$ is designed,
such that $\bar{\mathbb{W}}_{i-1|k}$ is a subset of $\mathbb{W}^{\max}$ for
$i=k+1,\ldots,k+N$ (see Section 3.3 for the details). Third, the sets obtained
via the previous two steps (i.e., via (4c) and (4d)) are combined in (4e). In
other words, the set $\mathbb{E}_{i|k}$ of all possible errors for time step
$i$ is obtained by combining the autonomously evolved error set and the set of
all possible disturbances from the previous time step that can result in
further errors. Based on Definition 1 given next, $\mathbb{E}_{i|k}$ for
$i=k+1,\ldots,k+N$ is an estimation of a robust forward reachable set for
$\mathbb{E}_{i-1|k}$.

###### Definition 1

Consider the autonomous system that is formulated via (4c)-(4e). A one-step
robust reachable set from the set of errors $\mathbb{E}_{i-1|k}$ that is
estimated for time step $i-1$ (with $i=k+1,\ldots,k+N$) is defined based on
([36]) via:

$\mathcal{R}_{\epsilon}(\mathbb{E}_{i-1|k},\bar{\mathbb{W}}_{i-1|k})=\Big{\\{}\bm{e}\in\left(\mathbb{X}\ominus\\{\bm{z}_{i|k}\\}\right)|\exists\bm{e}_{i-1|k}\in\mathbb{E}_{i-1|k},\exists\bm{w}(\bm{x}_{i-1|k})\in\bar{\mathbb{W}}_{i-1|k}:\bm{e}=\epsilon(\bm{e}_{i-1|k})+\bm{w}(\bm{x}_{i-1|k})\Big{\\}}$

The three steps explained above for estimation of the error set
$\mathbb{E}_{i|k}$ (for $i=k+1,\ldots,k+N$) are illustrated in Figure 2.
Constraint (4f) initializes the error set per time step when the nominal SDD-
TMPC is solved. Constraint (4g) keeps the realized states, estimated based on
the nominal state and the error values, inside the admissible state set
$\mathbb{X}$. Finally, (4h) is a terminal constraint with
$\mathbb{Z}_{\text{f}}\subseteq\mathbb{X}$, where $\mathbb{Z}_{\text{f}}$ is a
control invariant set for the nominal system that guarantees the recursive
feasibility.

Figure 2: Illustration of the development of the error set
$\mathbb{E}_{\cdot|k}$ for SDD-TMPC via (4c)-(4e) in $2$ dimensions: Each row
corresponds to one prediction time step (illustrated in this figure for time
steps $i$, $i+1$, $i+2$). The $2$-dimensional sets in the first column (shown
in yellow) illustrate the error set $\mathbb{E}_{\cdot-1|k}$ that has been
determined at the previous time step. By propagating this error set through
the autonomous error dynamics (i.e., mapping $\epsilon$) via (4c), the
$2$-dimensional sets $\bar{\mathbb{E}}_{\cdot|k}$ at the top side of the
second column of each row (shown in red) are generated. Moreover, the
$2$-dimensional sets $\bar{\mathbb{W}}_{\cdot-1|k}$ at the bottom part of the
second column in each row (shown in green) are obtained via the fuzzy
inference system (mapping FIS) using (4d). Finally, the red and green
$2$-dimensional sets of the second column per row are combined using Minkowski
summation to obtain the error set $\mathbb{E}_{\cdot|k}$ via (4e). This set is
then used as the starting set for the next time step (i.e., at the start of
the next row).

The dynamic tube $\mathbb{T}_{k}$ of SDD-TMPC for time step $k$ is given by
(4i). The optimization variables of (4) are the nominal state sequence
$\tilde{\bm{z}}_{k}$, the nominal control input sequence $\tilde{\bm{v}}_{k}$,
and the SDD-TMPC tube $\mathbb{T}_{k}$ (i.e., an ordered set including the
influence of the error sets across the entire prediction horizon). By
including the tube as an optimization variable for the nominal SDD-TMPC
problem, the optimizer determines solutions that foresee the dynamics of the
error in the generation of the online nominal state trajectory (i.e., the
center-line of tube $\mathbb{T}_{k}$). Therefore, the nominal state trajectory
of SDD-TMPC is in general different from that of regular TMPC (see Figure 1).
Moreover, the SDD-TMPC policy
$\pi:\mathbb{X}\times\mathbb{U}\times\mathbb{X}\rightarrow\mathbb{U}$, which
receives the nominal state trajectory of SDD-TMPC as input, is incorporated
within the optimization loop of SDD-TMPC via constraint (4j), as well as in
the estimation of the autonomous error dynamic function as explained before.
The policy $\pi(\cdot)$ of SDD-TMPC can be generated via state-of-the-art
methods (see, e.g., [23, 21] for determining a policy that stabilizes the
closed-loop system). Constraint (4j) enforces the generated control inputs to
remain inside the admissible control input set $\mathbb{U}$ for all possible
realizations of the state. Thus the policy also plays a role in the
computation of the online nominal state trajectory that is determined via SDD-
TMPC. Note that for computing the policy in (4j), where the input of the
function is a set, Definition 2 given next is used.

###### Definition 2

Whenever a function $g(\cdot)$ takes a set $\mathbb{S}$ as input, then the
output $\mathbb{O}$ is also a set defined by
$\mathbb{O}\coloneqq\big{\\{}g(i)\big{|}i\in\mathbb{S}\big{\\}}$. Thus, if
$\mathbb{S}_{1}\subseteq\mathbb{S}_{2}$, then $g(\mathbb{S}_{1})\subseteq
g(\mathbb{S}_{2})$.

State-of-the-art TMPC methods, however, solve the nominal MPC problem without
incorporating the policy $\pi(\cdot)$. In fact, only after determining the
nominal state trajectory via an optimization procedure, the policy of TMPC is
used outside of the optimization loop to generate the actual control input
$\bm{u}_{k}$.

In summary, the inclusion of the error evolution model into the optimization
procedure of SDD-TMPC, which is done via incorporating the dynamic tube of
SDD-TMPC in the optimization variables, as well as including the policy of
SDD-TMPC into the constraints, are expected to result in solutions for SDD-
TMPC that are less conservative than the solutions of TMPC.

The optimization problem (4) is in general nonlinear and non-convex. It is
common to assume convexity for the admissible sets $\mathbb{X}$ and
$\mathbb{U}$ for the states and the control inputs [6, 28], but this is not
the case in some applications (e.g., in collision avoidance [21, 36], which is
relevant for SaR).

###  3.3 Takagi-Sugeno-Kang fuzzy inference system (TSK-FIS) for modeling the
dynamics of the external disturbances

A fuzzy inference system (FIS) can describe the dynamics of a (generally
nonlinear) system via rules that are formulated via linguistic terms (e.g.,
very high), where this nonlinear mapping is interpretable –unlike, e.g.,
neural networks [37]. The rules of a FIS can be generated based on
experimental data or can be deduced directly from expert knowledge available
in human language. In unknown environments where safety is crucial, a FIS with
conservative rules may initially be used, and the rules can be tuned online to
improve the performance.

The goal is to use a TSK-FIS for modeling the set of the external disturbances
per time step $i-1$, where $i$ belongs to the prediction horizon
$\\{k+1,\ldots,k+N\\}$, as a function of the system states, i.e., to
approximate $\mathbb{W}(\bm{x}_{i-1|k})$ given in (3) by
$\bar{\mathbb{W}}_{i-1|k}$ using (4d). For the type of the set, we consider an
ellipsoid or a polytope and assume that the origin always belongs to the set.
Ellipsoids and polytopes are the most commonly used sets in related literature
[6, 26] and –when being parameterized– are bounded, if these parameters are
bounded. Next, we parameterize the set and show the parameterized set by
$\bar{\mathbb{W}}_{i-1|k}(\bm{\Theta}_{i-1|k})$ where $\bm{\Theta}_{i-1|k}$ is
the vector of all parameters at time step $i-1$. Therefore, the problem of
approximating the disturbance set is translated into the problem of
determining the vector $\bm{\Theta}_{i-1|k}$. In fact, this vector is returned
by the FIS, which receives as input the corresponding state of the system (see
(4d)). In order to ensure robustness for SDD-TMPC to all values of the
external disturbances, i.e., to make sure that (3) holds, parameter vector
$\bm{\Theta}_{i-1|k}$ should be determined such that the actual disturbance
set $\mathbb{W}(\bm{x}_{i-1|k})$ for all time steps $i\in\\{k+1,\ldots,k+N\\}$
is a subset of the approximate disturbance set
$\bar{\mathbb{W}}_{i-1|k}(\bm{\Theta}_{i-1|k})$, which itself should be a
subset of $\bar{\mathbb{W}}^{\max}$.

The rule base of the TSK-FIS of SDD-TMPC is composed of rules with the
formulation given by (6), where $\widetilde{X}^{l}$ is a fuzzy set that
mathematically represents a linguistic term that describes the input variable
$x$:

$\text{Rule }l:\text{ IF }x\text{ is }\widetilde{X}^{l}\text{, THEN
}y^{l}\text{ is }h^{l}(x)$ (6)

The output generated by each rule concerns only one element of parameter
vector $\bm{\Theta}_{i-1|k}$. More specifically, for
$i\in\\{k+1,\ldots,k+N\\}$, the input $x$ is replaced by $\bm{x}_{i-1|k}$, and
the output $y^{l}$ is replaced by an element ${\theta}^{l}_{i-1|k}$ of the
parameter vector. Note that more than one rule in the rule base may generate a
candidate value for this element of the parameter vector. Therefore, the
superscript $l$ is used to show that the value is generated for this element
via the $l^{\text{th}}$ rule within the set of all $L$ rules that generate a
candidate value for this element. Moreover, $h^{l}(\cdot)$ shows a generally
nonlinear mapping from the input space to the output space (i.e., from the
admissible set of the state variables of the dynamical system to the
admissible set for element $\theta_{i-1|k}$ of the vector
$\bm{\Theta}_{i-1|k}$). Then the final value for the parameter is computed by:

$\theta_{i-1|k}=\frac{\Sigma^{L}_{l=1}\mu^{l}(\bm{x}_{i-1|k})h^{l}(\bm{x}_{i-1|k})}{\Sigma^{L}_{l=1}\mu^{l}(\bm{x}_{i-1|k})}$
(7)

where $\mu^{l}(\cdot)$ is the membership function of fuzzy set
$\widetilde{X}^{l}$ and $L$ is the number of the rules in the rule base that
generate a value for element $\theta_{i-1|k}$.

The robust control problem formulation is based on the assumption of existence
of $\mathbb{W}^{\max}$, which bounds all the possible disturbances. Thus, the
evolution of the disturbance set generated via the TSK-FIS explained above is
stable if the approximate disturbance set
$\bar{\mathbb{W}}_{i-1|k}(\bm{\Theta}_{i-1|k})$ is bounded by
$\mathbb{W}^{\max}$ for all $i\in\\{k+1,\ldots,k+N\\}$. The approximate set
$\bar{\mathbb{W}}_{i-1|k}(\bm{\theta}_{i-1|k})$ is bounded, whenever all
elements of vector $\bm{\Theta}_{i-1|k}$ are bounded (based on the assumption
of an ellipsoid or polytope set or any other set that satisfies the condition
of bounded output for bounded input). The membership functions in (7) are
restricted to the interval $[0,1]$ by definition, implying that to keep the
elements of $\bm{\Theta}_{i-1|k}$ bounded, the functions $h^{l}(\cdot)$ (with
$l\in\\{1,\cdots,L\\}$ ) should be formulated such that for all
$\bm{x}_{i-1|k}\in\mathbb{X}$ the function remains bounded. Finally, to ensure
that the approximate set $\bar{\mathbb{W}}_{i-1|k}(\bm{\Theta}_{i-1|k})$ that
is designed to be bounded, is also a subset of $\mathbb{W}^{\max}$, this set
is defined as the union of the output of the TSK-FIS and $\mathbb{W}^{\max}$.

###  3.4 Stability of SDD-TMPC

In this section, we discuss the stability of SDD-TMPC, where our discussions
are based on the following assumptions:

  1. A1

There exists
$\mathbb{T}^{\max}_{k}={\\{\\{z_{k+1|k}\\}\oplus\mathbb{E}^{\max},\ldots,\\{z_{N|k}\\}\oplus\mathbb{E}^{\max}\\}}$
that propagates across the prediction horizon at time step $k$ and is positive
robust invariant under the policy
$\pi(\cdot):\mathbb{X}\times\mathbb{U}\times\mathbb{X}\rightarrow\mathbb{U}$,
i.e., when the system follows policy $\pi(\cdot)$, the state remains inside
this set.

  2. A2

There exists a terminal control invariant set
$\mathbb{Z}_{\text{f}}\subseteq\mathbb{X}$ for the nominal system, such that
$\mathbb{Z}_{\text{f}}\oplus\mathbb{E}^{\max}\subseteq\mathbb{X}$.

  3. A3

There exists a control law $\kappa:\mathbb{Z}_{\text{f}}\rightarrow\mathbb{U}$
for the nominal system, such that for $\bm{z}_{k}\in\mathbb{Z}_{\text{f}}$ and
$i=k,\ldots,k+N-1$:

     * •

$f(\bm{z}_{i|k},\kappa(\bm{z}_{i|k}))\in\mathbb{Z}_{\text{f}}$

     * •

$V_{\text{f}}(f(\bm{z}_{i|k},\kappa(\bm{z}_{i|k})))-V_{\text{f}}(\bm{z}_{i|k})\leq-l(\bm{z}_{i|k},\kappa(\bm{z}_{i|k}))\qquad$
(where $V_{\text{f}}(\cdot)$ is the terminal cost as is given in (4a))

     * •

$\pi(\bm{x}_{i|k},\kappa(\bm{z}_{i|k}),\bm{z}_{i|k})\in\mathbb{U}$,
$\bm{x}_{i|k}\in\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}^{\max}$

  4. A4

There exists $\mathcal{K}_{\infty}$ functions $\alpha_{1}(\cdot)$ and
$\alpha_{\text{{f}}}(\cdot)$ that satisfy the following inequalities (note
that a function belongs to class $\mathcal{K}$, if it is continuous, zero at
zero, and strictly increasing; and a function belongs to class
$\mathcal{K}_{\infty}$, if it is in class $K$ and is unbounded [1]) ):

     * •

$l(\bm{z}_{i|k},\bm{v}_{i|k})\geq\alpha_{1}(|\bm{z}_{i|k}|)\quad\forall\bm{z}_{i|k}\in\mathbb{X},\forall\bm{v}_{i|k}\in\mathbb{U}\qquad$
(where $l(\cdot)$ is the stage cost as is given in (4a))

     * •

$V_{\text{f}}(\bm{z}_{i|k})\leq\alpha_{\text{{f}}}(|\bm{z}_{i|k}|)\quad\forall\bm{z}_{i|k}\in\mathbb{Z}_{\text{f}}$

Assumption A1 implies that a stabilizable TMPC law can be determined for the
system (i.e., the error does not go to infinity). Moreover,
$\mathbb{T}^{\max}_{k}$ is the tube with a constant cross section that is used
in TMPC. Assumption A2 (existence of a nominal invariant set) is a standard
assumption in TMPC literature [6]. The first two items of Assumption A3 and
Assumption A4 are standard assumptions in MPC literature [1] that are required
to use the optimal cost as a Lyapunov function. The third item of Assumption
A3 indicates that there exists a control law in the terminal set, such that
the input constraints are satisfied.

We first give some theorems that are used in the discussions on stability.

######  Theorem 3.1.

If $\mathbb{C}\subseteq\mathbb{A}$, then
$\mathbb{C}\oplus\mathbb{B}\subseteq\mathbb{A}\oplus\mathbb{B}$.

######  Proof 3.2.

Each element in $\mathbb{C}\oplus\mathbb{B}$ is given by $c+b$, where
$c\in\mathbb{C}$ and $b\in\mathbb{B}$. Since $\mathbb{C}\subseteq\mathbb{A}$,
then $c\in\mathbb{A}$. Thus, from the definition of Minkowski addition (5), we
have $(c+b)\in\mathbb{A}\oplus\mathbb{B}$.

######  Theorem 3.3.

If $\mathbb{C}\subseteq\mathbb{A}$ and $\mathbb{D}\subseteq\mathbb{B}$, then
$\mathbb{C}\oplus\mathbb{D}\subseteq\mathbb{A}\oplus\mathbb{B}$.

######  Proof 3.4.

From the previous theorem, we have
$\mathbb{C}\oplus\mathbb{B}\subseteq\mathbb{A}\oplus\mathbb{B}$ and
$\mathbb{C}\oplus\mathbb{D}\subseteq\mathbb{C}\oplus\mathbb{B}$, which
together imply that
$\mathbb{C}\oplus\mathbb{D}\subseteq\mathbb{A}\oplus\mathbb{B}$.

######  Theorem 3.5.

If $\mathbb{E}_{k}\subseteq\mathbb{E}^{\max}$ and the system follows the SDD-
TMPC policy $\pi(\cdot)$, then $\mathbb{E}_{i|k}\subseteq\ \mathbb{E}^{\max}$
for $i=k+1,\ldots,k+N$, where $\\{z_{i|k}\\}\oplus\mathbb{E}_{i|k}$ are the
elements of the tube $\mathbb{T}_{k}$ of SDD-TMPC.

######  Proof 3.6.

By contradiction, if the theorem is not true, i.e., if there exists
$\mathbb{E}_{i|k}$ for $i=k+1,\ldots,k+N$, such that
$\mathbb{E}_{i|k}\not\subseteq\mathbb{E}^{\max}$ then there is at least one
element $\bm{e}_{i|k}=\bm{x}_{i|k}-\bm{z}_{i|k}$ corresponding to a possible
realization $\bm{x}_{i|k}$ of the state at time step $i$, such that
$\bm{e}_{i|k}\in\mathbb{E}_{i|k}$, but
$\bm{e}_{i|k}\notin\mathbb{E}_{\max,i|k}$. Therefore, for the corresponding
external disturbances the state leaves tube $\mathbb{T}^{\max}$, which is not
possible because $\mathbb{T}^{\max}$ is a robust positive invariant set for
the policy $\pi(\cdot)$ according to assumption A1.

######  Theorem 3.7.

The set
$\Phi:=\\{\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k}\big{|}\bm{z}_{i|k}\in\mathbb{Z}_{\text{f}},\
\mathbb{E}_{i|k}\subseteq\mathbb{E}^{\max},\ i=k+1,\ldots,k+N\\}$ is a set
robust positive invariant set for the SDD-TMPC inputs
$\pi(\bm{x}_{i|k},\kappa(\bm{z}_{i|k}),\bm{z}_{i|k})$, for $i=k,\ldots,k+N-1$.
(Note that according to [6] a set robust positive invariant set is defined as
a set of sets. Since the terminal real state $x_{k|N+k}$ should be bounded, we
need to constrain both the nominal state and the potential errors, thus a set
of tubes (i.e., $\Phi$) is needed.)

######  Proof 3.8.

From assumption A3 it follows that if $\bm{z}_{k}\in\mathbb{Z}_{\textrm{f}}$,
then $\bm{z}_{i|k}\in\mathbb{Z}_{\textrm{f}}$ for $i=k+1,\ldots,k+N$.
Moreover, from Theorem 3.5 if $\mathbb{E}_{k}\subseteq\mathbb{E}^{\max}$, then
$\mathbb{E}_{i|k}\subseteq\mathbb{E}^{\max}$ for $i=k+1,\ldots,k+N$.
Therefore, from the definition of $\Phi$, we have
$\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k}\in\Phi$, which proves the theorem.

######  Theorem 3.9.

If the sequence of nominal control inputs
$\tilde{\bm{v}}_{k}=[\bm{v}^{\top}_{k|k},\bm{v}^{\top}_{k+1|k},\dots,\bm{v}^{\top}_{k+N-1|k}]^{\top}$
of SDD-TMPC is feasible for the admissible pair ($\bm{z}_{k}$,$\bm{x}_{k}$),
i.e., using these inputs and policy $\pi(\cdot)$, the controlled system
satisfies $\bm{x}_{i|k}\in\mathbb{X}$ for $i=k+1,\ldots,k+N$, then the shifted
sequence of nominal control inputs
$\tilde{\bm{v}}_{k+1}=[\bm{v}^{\top}_{k+1|k},\bm{v}^{\top}_{k+2|k},\dots,\bm{v}^{\top}_{k+N-1|k},\kappa^{\top}(\bm{z}_{k+N|k})]^{\top}$
is feasible for the admissible pair $(\bm{z}_{k+1},\bm{x}_{k+1})$ under the
SDD-TMPC policy $\pi(\cdot)$.

######  Proof 3.10.

The nominal system is deterministic, and at time step $k+1$ for the first
$N-1$ time steps the same nominal control inputs as for time step $k$ are
applied. Thus one can write $\bm{z}_{i|k+1}=\bm{z}_{i|k}$ for
$i=k+1,\dots,k+N$. The error set $\mathbb{E}_{k+1|k}$ contains all the
possible errors of $\bm{x}_{k+1|k}$ with respect to $\bm{z}_{k+1|k}$. At time
step $k+1$, however, $\mathbb{E}_{k+1|k+1}$ contains only one element of
$\mathbb{E}_{k+1|k}$ based on the realized state under the SDD-TMPC policy.
Thus, we have $\mathbb{E}_{k+1|k+1}\subseteq\mathbb{E}_{k+1|k}$.

Since $\bm{z}_{k+1|k+1}=\bm{z}_{k+1|k}$ and
$\mathbb{E}_{k+1|k+1}\subseteq\mathbb{E}_{k+1|k}$, from Theorem 3.3 we have
$\\{\bm{z}_{k+1|k+1}\\}\oplus\mathbb{E}_{k+1|k+1}\subseteq\\{\bm{z}_{k+1|k}\\}\oplus\mathbb{E}_{k+1|k}$.
From (4d)-(4e), the next error set is derived based on a mapping from all
elements of the current error set added to the nominal state. Based on the
last two statements, Definition 2, and Theorem 3.3, we conclude that
$\mathbb{E}_{k+2|k+1}\subseteq\mathbb{E}_{k+2|k}$. Similarly, in an iterative
way it can be shown that $\mathbb{E}_{i|k+1}\subseteq\mathbb{E}_{i|k}$ for
$i=k+1,\dots,k+N$.

The sequence $\tilde{\bm{v}}_{k}$ is assumed to be feasible for the admissible
pair ($\bm{z}_{k}$,$\bm{x}_{k}$). Thus the elements
$\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k}$ for $i=k+1,\ldots,k+N-1$ belong to
the admissible state set. Now since $z_{i|k+1}=z_{i|k}$ and
$\mathbb{E}_{i|k+1}\subseteq\mathbb{E}_{i|k}$ for $i=k+1,\dots,k+N$, based on
Theorem 3.7, the elements $\\{\bm{z}_{i|k+1}\\}\oplus\mathbb{E}_{i|k+1}$ for
$i=k+1,\ldots,k+N-1$ belong to the admissible state set. Finally, since at
time step $k+N$ the SDD-TMPC input is determined based on the nominal control
input $\kappa(\bm{z}_{k+N|k})$ (or equivalently, based on
$\kappa(\bm{z}_{k+N|k+1})$), and since
$\mathbb{E}_{k+N|k+1}\subseteq\mathbb{E}_{k+N|k}\subseteq\mathbb{E}^{\max}$,
from Theorem 3.7 we conclude that
$\\{\bm{z}_{k+N|k+1}\\}\oplus\mathbb{E}_{k+N|k+1}$ belongs to the admissible
state set.

######  Theorem 3.11.

The optimal cost function $V^{*}(\cdot)$ in (4a) is a Lyapunov function for
system (1) (which implies stability).

######  Proof 3.12.

Given assumption A4, the optimal cost function $V^{*}(\cdot)$ is lower bounded
by $\alpha_{1}(\cdot)$. Moreover, an upper bound can be found for
$V^{*}(\cdot)$, since it is continuous and finite for
$\bm{x},\bm{z}\in\mathbb{X}$. The optimal sequence of nominal control inputs
corresponding to the optimal cost $V^{*}(\bm{x}_{k},\bm{z}_{k})$ is given by
$\bm{\bm{v}}_{k}^{*}=[{\bm{v}^{*}}^{\top}_{k|k},{\bm{v}^{*}}^{\top}_{k+1|k},\dots,{\bm{v}^{*}}^{\top}_{k+N-1|k}]^{\top}$.
Based on Theorem 3.9, for the next control time step the sequence
$\tilde{\bm{v}}_{k+1}=[{\bm{v}^{*}}^{\top}_{k+1|k},{\bm{v}^{*}}^{\top}_{k+2|k+1},\ldots,{\bm{v}^{*}}^{\top}_{k+N-1|k},\kappa^{\top}(\bm{z}_{N|k})]^{\top}$
is feasible for the admissible pair $(\bm{z}_{k+1},\bm{x}_{k+1})$ under the
SDD-TMPC policy $\pi(\cdot)$. The difference between
$V^{*}(\bm{x}_{k},\bm{z}_{k})$ and the cost under sequence
$\tilde{\bm{v}}_{k+1}$ is
$l(\bm{z}_{k|k},\bm{v}_{k|k})+V_{\text{f}}(\bm{z}_{k+N|k})-l(\bm{z}_{k+N|k+1},\kappa(\bm{z}_{k+N|k+1}))-V_{\text{f}}(f(\bm{z}_{k+N|k+1},\kappa(\bm{z}_{k+N|k}))$.
From assumption A3, this difference is positive, implying that the optimal
cost $V^{*}(\bm{x}_{k},\bm{z}_{k})$ is larger than the cost under
$\tilde{\bm{v}}_{k+1}$, which itself is larger than or equal to the optimal
cost $V^{*}(\bm{x}_{k+1},\bm{z}_{k+1})$. Therefore, we have
$V^{*}(\bm{x}_{k+1},\bm{z}_{k+1})<V^{*}(\bm{x}_{k},\bm{z}_{k})$.

##  4 Case studies

In this section, we provide the results of two numerical case studies that
have been designed for a SaR robot:

  * •

Case study 1: A SDD-TMPC controller is developed for a linear model of the
robot, which is impacted by state-dependent disturbances. With this experiment
we showcase the unique capability of SDD-TMPC, in comparison with MPC and
TMPC, to provide robustness with respect to the disturbances, while improving
the feasibility and reducing the conservativeness. The codes for this
experiment have been published in [38].

  * •

Case study 2: To showcase the performance of SDD-TMPC for nonlinear systems, a
SDD-TMPC controller is developed for a nonlinear reference tracking problem
for the robot. This problem has previously been addressed in [39] using
nonlinear TMPC. While the original nonlinear TMPC solution suffers from a high
rise time, our results indicate a decreased rise time using SDD-TMPC, which
allows the robot to safely move with a speed higher than the speed that is
allowed by TMPC, without violating the constraints. The code has been
published in [40].

###  4.1 Case study 1

####  4.1.1 Simulation setup

We simulate a holonomic ground robot, for which the kinematics is described by
the following discrete-time state-space representation:

$\bm{x}_{k+1}=\begin{bmatrix}1&0&T_{s}&0\\\ 0&1&0&T_{s}\\\ 0&0&1&0\\\
0&0&0&1\end{bmatrix}\bm{x}_{k}+\begin{bmatrix}0&0\\\ 0&0\\\ T_{s}&0\\\
0&T_{s}\end{bmatrix}\bm{u}_{k}+\begin{bmatrix}\bm{w}_{k}\\\ 0_{2\times
1}\end{bmatrix}$ (8)

where the state $\bm{x}_{k}=[p_{x,k},p_{y,k},\nu_{x,k},\nu_{y,k}]^{\top}$ and
control input $\bm{u}_{k}=[a_{x,k},a_{y,k}]^{\top}$ vectors include,
respectively, the position and velocity, and the acceleration of the robot in
the $x$ and $y$ directions. The discretization sampling time, $T_{s}$, is
$0.1~{}$s, and $\bm{w}_{k}$ is the 2-dimensional vector of the external
disturbances that influence the position of the robot. In this case study, we
assume that the disturbance affects position only (a similar assumption was
used in [39]). The set $\mathbb{W}(\bm{x}_{k})$ of external disturbances when
the state of the robot is $\bm{x}_{k}$ is a two-dimensional ellipse, with its
major parallel to the direction of the movement of the robot. The magnitude of
the disturbances is nonlinearly dependent on the state of the robot, i.e.:

$r^{\max}(\bm{x}_{k})=0.202\sqrt{\nu_{x,k}^{2}+\nu_{y,k}^{2}}+r_{\min}(\bm{x}_{k})$
(9a)
$r_{\min}(\bm{x}_{k})=0.225\beta^{2}(\bm{x}_{k})\sqrt[4]{\nu_{x,k}^{2}+\nu_{y,k}^{2}}$
(9b)

with $r^{\max}(\bm{x}_{k})$ and $r_{\min}(\bm{x}_{k})$ the length of the,
respectively, major and minor of the ellipse when the state of the robot is
$\bm{x}_{k}$. In SaR missions, the external disturbances may vary in different
parts of the environment, where the parameter $\beta(\bm{x}_{k})$, an
indication of the slipperiness of the ground, allows us to include this in the
simulations. We assume that the robot has perfect knowledge of its own state
and of $\beta(\bm{x}_{k})$, but has no information on (9). This assumption is
made to make the learning procedure faster for the case study. In realistic
scenarios, however, understanding how ground conditions affect system
behaviour is challenging, but not impossible [41].

The constraints on the position of the robot vary in different simulations,
while the constraints on the inputs and the velocity of the robot are always
the following:

$|\nu_{x,k}|<2\frac{\textrm{m}}{\textrm{s}},\
|\nu_{y,k}|<2\frac{\textrm{m}}{\textrm{s}},\
|a_{x,k}|<5\frac{\textrm{m}}{\textrm{s}^{2}},\
|a_{y,k}|<5\frac{\textrm{m}}{\textrm{s}^{2}}$ (10)

The Table 3 contains all addational mathematical notations required for this
case study.

Table 3: The following mathematical notation is used in section 4.1:

The notation | The explenation | The notation | The explenation
---|---|---|---
$[A^{\text{inequ}}_{n_{1}\times n_{2}},b^{\text{inequ}}_{n_{1}\times 1}]$ | A pair of matrices used to describe polytope in $n_{2}$ dimensions with $n_{1}$ inequalities | $Q_{K}$ | Stage cost matrix of states of the ancillary control law
$a_{x,k}$ | The current acceleration component parallel to x-axis | $Q_{\kappa}$ | Stage cost matrix of states of the terminal control law
$a_{y,k}$ | The current acceleration component parallel to y-axis | $R$ | Stage cost matrix of inputs of the original cost function
$c_{n,m}$ |  | A constant parameter describing an ellipsoidal set of disturbances
---
where $n$ is an order number and $m$ denotes to the major or minor axis
$R_{K}$ | Stage cost matrix of inputs of the ancillary control law
$F$ | Terminal cost matrix | $R_{\kappa}$ | Stage cost matrix of inputs of the terminal control law
$K$ | A state feedback matrix used in LQR | $r^{\max}(\bm{x}_{k})$ | The major of the ellipsoidal disturbance set given current state
$m$ | meters | $r_{\min}(\bm{x}_{k})$ | The minor of the ellipsoidal disturbance set given current state
$\frac{\text{m}}{\text{s}}$ | meters per second | $T_{s}$ | Sampling time
$\frac{\text{m}}{\text{s}^{2}}$ | meters per second square | $\bm{u}_{k}$ | The current velocity
$p_{x,k}$ | The current position component parallel to x-axis | $\nu_{x,k}$ | The current velocity component parallel to x-axis
$p_{y,k}$ | The current position component parallel to y-axis | $\nu_{y,k}$ | The current velocity component parallel to y-axis
$Q$ | Stage cost matrix of states of the original cost function | $I_{n\times n}$ | Identity matrix with dimension $n$

####  4.1.2 Formulating and implementing linear SDD-TMPC for path planning of
a SaR robot

For path planning of the SaR robot in various simulated environments with
external disturbances, we formulate an SDD-TMPC problem with a quadratic cost
function given by:

$V(\bm{z}_{k},\bm{v}_{k})=\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{z}_{i|k}\right\rVert_{Q}^{2}+\left\lVert\bm{v}_{i|k}\right\rVert_{R}^{2}\right)+\left\lVert\bm{z}_{k+N|k}\right\rVert_{F}^{2}$
(11)

where a prediction horizon of $N=5$ and the following cost matrices are
considered:

$Q=\text{diag}(100,100,1,1),\quad R=I_{2\times 2},$
$F=\begin{bmatrix}805&0&-571&0\\\ 0&805&0&-571\\\ -571&0&655&0\\\
0&-571&0&655\end{bmatrix}$

The most common choice for the ancillary control law is linear state feedback,
such that the control input at time step $k$ is given by:

$\displaystyle\bm{u}_{k}=K\bm{e}_{k}+\bm{v}_{k},\
K=\begin{bmatrix}7,98&0&4.42&0\\\ 0&7,98&0&4.42\end{bmatrix}$ (12)

where $\bm{e}_{k}=\bm{x}_{k}-\bm{z}_{k}$, and for determining $K$ we use
linear quadratic regulator (LQR) method with
$Q_{K}=\text{diag}(100,100,0.1,0.1)$ and $R_{K}=I_{2\times 2}$. These matrices
are tuned, such that the ancillary control law more aggressively reduces the
position. Consequently, the influence of the state-dependent external
disturbances is more significantly reduced. The error $\bm{e}_{k}$ evolves
according to:

$\bm{e}_{k+1}=(A+BK)\bm{e}_{k}+\begin{bmatrix}\bm{w}^{\top}_{k}|0_{1\times
2}\end{bmatrix}^{\top}$ (13)

with $A$ and $B$ the $4\times 4$ and $4\times 2$ dynamic and input matrices
given in (8). Thus the equivalent of (4c) and (4j) for this linear case, for
$i=k,\ldots,k+N-1$, are respectively:

$\bar{\mathbb{E}}_{i+1|k}=(A+BK)\mathbb{E}_{i|k}$ (14)
$K\mathbb{E}_{i|k}\oplus\\{\bm{v}_{i|k}\\}\subseteq\mathbb{U}$ (15)

See Definition 2 for how to compute (14) and (15). Moreover, in order to
reduce the computation time and because the external disturbance set changes
slowly with the states in the proposed scenarios, we simplified (4d), such
that it only takes into account the centroid of the tubes:

$\bar{\mathbb{W}}_{i|k}=\text{FIS}\left(\bm{z}_{i|k}+(A+BK)^{i-k}\bm{e}_{k}\right)$
(16)

The terminal admissible set $\mathbb{Z}_{\text{f}}$ for the nominal state is a
positive invariant polytope, which is determined by assuming that the system
follows an LQR control law $\kappa(\cdot)$ beyond the prediction horizon, with
$Q_{\kappa}=\text{diag}(10,10,1,1)$ and $R_{\kappa}=I_{2\times 1}$. Then the
terminal cost in (11) is the limit of the cost when the system follows the LQR
control law $\kappa(\cdot)$, when $k\rightarrow\infty$, and can be determined
by solving the following Riccati equation, using, e.g., Matlab dlyap command
[42]:

$F=2((A+BK)^{T}F(A+BK)+Q_{\kappa}+K^{T}R_{\kappa}K)$ (17)

For the details on the derivation of (17)), we refer the reader to [1].
Control policy $\kappa(\cdot)$ should be designed, such that Assumption A3
holds.

####  4.1.3 Learning the external disturbance sets using a FIS

In order to approximate $\mathbb{W}(\bm{x}_{k})$ by $\bar{\mathbb{W}}_{k}$,
polyhedral sets defined by $\\{[x,y]^{\top}:A^{\textrm{ineq}}_{8\times
2}[x,y]^{\top}\leq\bm{b}^{\textrm{ineq}}_{8\times 1}\\}$ are considered. The
advantage of using polytope sets is that, unlike ellipses, the Minkowski
addition can be computed for such sets without approximation and the shape of
the set remains a polytope after using Minkowski additions. Moreover, with
polytope sets real-life disturbance sets of different shapes can be
approximated. The main downside of using polytope sets is that the problem can
become computationally expensive for large dimensions. In such cases,
ellipsoids can be used instead. Thus, in order to simplify the fuzzy inference
systems that should estimate $\bar{\mathbb{W}}_{k}$, we consider two TSK FISs
for approximating the major and minor (i.e., (9)) of the disturbance sets per
time step $k$, with their output functions defined by:

$\displaystyle
r_{m}(\bm{x}_{k})=c_{1,m}\nu_{k}^{2}+c_{2,m}\beta^{2}(\bm{x}_{k})+c_{3,m}\nu_{k}\beta(\bm{x}_{k})+c_{4,m}\nu_{k}+c_{5,m}\beta(\bm{x}_{k})+c_{6,m}$
(18)

with $\nu_{k}^{2}=\nu_{x,k}^{2}+\nu_{y,k}^{2}$, $m\in\\{\max,\min\\}$, and
$\bm{\theta}_{m,k}=[c_{1,m}\ \ldots\ c_{6,m}]^{\top}$. A polytope with a fixed
number of edges that encounters the resulting ellipse is then determined and
used as the disturbance set at time step $k$.

We generated $1240$ input-output pairs, based on the ground truth ellipsoids
that were designed for the case study (see (9)). This data set was divided
into a training and a validation set, with a proportion of $660:580$. For the
test set, we generated a large data set with $100000$ pairs of input-output
using (9) in order to extensively test the trained FISs. Given the training
and the validation errors, the best results were achieved using $5$ fuzzy sets
per input, which resulted in $25$ fuzzy rules with outputs that are described
by (18). Note that compared to different expressions for the output of the
rules, the expression given by (18) did not result in under or over-fitting.
GA was used to train the FISs by minimizing the mean square error, with an
additional penalty for negative errors.

Table 4: A summary of the results for the trained FIS that estimate the lengths of the major and minor of the disturbance ellipse sets using the test data, where the errors are normalized via dividing by the maximum ground truth value. | FIS that estimates the major | FIS that estimates the minor
---|---|---
Number (in %) of cases where errors were negative | $2.64$ | $3.01$
Mean value of negative errors | $7.8\times 10^{-4}$ | $1.93\times 10^{-3}$
Mean value of positive errors | $1.02\times 10^{-2}$ | $2.32\times 10^{-2}$
Maximum value of negative errors | $5.89\times 10^{-3}$ | $2.56\times 10^{-2}$
Maximum value of positive errors | $5.94\times 10^{-2}$ | $2.45\times 10^{-1}$

Figure 3: The projections of the tube of TMPC (shown in green) and the tubes
of SDD-TMPC generated by the FIS (shown in blue), compared with the ground
truth admissible sets of disturbances (shown in red), for a prediction horizon
of $30$. To better illustrate the time evolution of the shape and size of the
tubes, the projections, on the first two plots, were translated in such a way
that nominal position and velocity are ordered as a horizontal line i.e
$z_{i|0}=[i,0,i,0]^{\top}$. On the last plot, the projections were translated
in such a way that $Kz_{i|0}=[i,0]^{\top}$

.

The results obtained for the test set for the two FIS that estimate the
lengths of the major and minor of the ellipse sets for the external
disturbances are summarized in Table 4, where the error values have been
normalized. In general, both FIS was able to approximate the ground truth with
mostly positive-valued errors. Negative-valued errors could completely be
eliminated by including penalties for small positive errors, in addition to
the negative ones. This, however, may increase the overall error, and our
simulation results showed that it is not necessary.

In SDD-TMPC, the tube changes over time. Thus, we checked for a sample example
of how the tube evolves during the optimization when a larger horizon of $30$
is used. Since the tube is $4$-dimensional, it is impossible to present it in
one plot. Thus, Figure 3 shows the projection of the tube on the planes of
positions and velocities. Additionally, the tube multiplied by the ancillary
control gain $K$ is shown that is basically the tube of control input i.e.
acceleration. All these plots also show the projection of the tube
$\mathbb{T}$ of TMPC that was designed based on [35]. The tube of SDD-TMPC
evolves until there is convergence in all dimensions of the state, whereas the
convergence is never close to the tube of TMPC. The evolution of the tube can
result in both the growth and shrinkage of the tube. For instance, once the
system approaches the origin and slows down, the error sets become smaller as
a result of the system slowing down. What is also important, we plotted both
tubes generated while SDD-TMPC was using FIS and ground truth functions and it
can be observed that they are almost overlapping. It shows that the current
approximation is sufficient.

####  4.1.4 Comparison: MPC, TMPC, SDD-TMPC

In all the given simulations, regular MPC (formulated by (19)), TMPC
(formulated by (20)), and SDD-TMPC (formulated by (21)) were implemented and
their performance, with respect to minimizing the cost function, satisfying
the hard constraints, and computation time were compared. $\ominus$ symbolizes
a Minkowski difference. The controllers were implemented in MATLAB version
2022b running on a PC with a 11th Gen Intel(R) Core(TM) i7-1185G7 CPU with a
clock speed of 3 GHz. In order to solve the optimization problems, the
quadratic programming solver (Matlab’s Optimization toolbox) was used for MPC
and TMPC, whereas for SDD-TMPC, the particle swarm (PS) algorithm [43] from
the Global optimization toolbox of Matlab was implemented. In this toolbox,
only PS and GA could consistently minimize the cost function, whereas PS
usually converged faster. Unless stated otherwise, the optimization procedure
for SDD-TMPC was terminated after 60 iterations which took about 4 minutes to
run, which has been shown in the simulations to provide a balanced trade-off
between the computation time and the accuracy of the solutions. In comparison,
MPC and TMPC solved their quadratic problems in milliseconds. The initial
nominal state was assumed to be equal to the real state, $\beta(\bm{x}_{k})$
was assumed to be $1$, and the horizon was assumed to be 5 unless otherwise
stated. Terminal constraint and cost were not used in simulations 2 (possible,
but the horizon would be too high, which would slow down the computation too
much) and 3 (impossible, because the goal is outside the feasible set, so
there is no way to design the terminal set).

$V^{*}(\bm{x}_{k})=\min_{\tilde{\bm{x}}_{k},\tilde{\bm{u}}_{k}}\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{x}_{i|k}\right\rVert_{Q}^{2}+\left\lVert\bm{u}_{i|k}\right\rVert_{R}^{2}\right)+\left\lVert\bm{x}_{k+N|k}\right\rVert_{F}^{2}$
(19a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{x}_{i|k}=A\bm{x}_{i-1|k}+B\bm{u}_{i-1|k}$ (19b)
$\bm{x}_{i|k}\in\mathbb{X},\;\bm{u}_{i|k}\in\mathbb{U},\;\bm{x}_{N|k}\in\mathbb{Z}_{\text{f}}$
(19c)

$V^{*}(\bm{z}_{k})=\min_{\tilde{\bm{z}}_{k},\tilde{\bm{v}}_{k}}\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{z}_{i|k}\right\rVert_{Q}^{2}+\left\lVert\bm{v}_{i|k}\right\rVert_{R}^{2}\right)+\left\lVert\bm{z}_{k+N|k}\right\rVert_{F}^{2}$
(20a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{z}_{i|k}=A\bm{z}_{i-1|k}+B\bm{v}_{i-1|k}$ (20b)
$\bm{z}_{i|k}\in\mathbb{X}\ominus\mathbb{E}_{\text{max}},\;\bm{v}_{i|k}\in\mathbb{U}\ominus\mathbb{E}_{\text{max}},\;\bm{z}_{N|k}\in\mathbb{Z}_{\text{f}}$
(20c)

$V^{*}(\bm{x}_{k},\bm{z}_{k})=\min_{\tilde{\bm{z}}_{k},\tilde{\bm{v}}_{k},\mathbb{T}_{k}}\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{z}_{i|k}\right\rVert_{Q}^{2}+\left\lVert\bm{v}_{i|k}\right\rVert_{R}^{2}\right)+\left\lVert\bm{z}_{k+N|k}\right\rVert_{F}^{2}$
(21a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{z}_{i|k}=A\bm{z}_{i-1|k}+B\bm{v}_{i-1|k}$ (21b)
$\mathbb{T}_{k}=\\{\\{z_{k+1|k}\\}\oplus\mathbb{E}_{k+1|k},\ldots,\\{z_{N|k}\\}\oplus\mathbb{E}_{k+N|k}\\}$
(21c) $\bar{\mathbb{E}}_{i|k}=(A+BK)\mathbb{E}_{i-1|k}$ (21d)
$\bar{\mathbb{W}}_{i-1|k}=\text{FIS}\left(\bm{z}_{i|k}+(A+BK)^{i-k}(\bm{x}_{k}-\bm{z}_{k})\right)$
(21e)
$\mathbb{E}_{{i|k}}=\bar{\mathbb{E}}_{i|k}\oplus\bar{\mathbb{W}}_{i-1|k}$
(21f) $\mathbb{E}_{k}=\\{\bm{x}_{k}-\bm{z}_{k}\\}$ (21g)
$\\{\bm{z}_{i|k}\\}\oplus\mathbb{E}_{i|k}\subseteq\mathbb{X}$ (21h)
$K\mathbb{E}_{i|k}\oplus\\{\bm{v}_{i|k}\\}\subseteq\mathbb{U}$ (21i)
$\bm{z}_{N|k}\in\mathbb{Z}_{\text{f}}$ (21j)

To compare the convergence of the optimization algorithms, a sample problem
was considered where the robot should reach the zero state starting from the
state $[0.35,0.65,0,0]^{\top}$. The table 5 shows the optimal costs for MPC,
TMPC, and SDD-TMPC. Since SDD-TMPC solves a nonconvex, nonlinear optimization
problem, it may not be possible to find a global minimum. Therefore, the
changes in the value of the cost by iteration of the PS algorithm are given in
the table (the number of iterations is given in parentheses). The rate of
these changes depends strongly on how constrained the problem is (compare the
third and sixth rows of the table). As a warm start for SDD-TMPC, the solution
of TMPC is used, and in case TMPC is infeasible, the shifted trajectory from
Theorem 3.9 is used. As expected, the cost values corresponding to TMPC and
MPC, respectively, are upper and lower bounds for the costs of SDD-TMPC.

Table 5: Comparing the cost values for MPC, TMPC, and different numbers of
iterations of PS, which solves SDD-TMPC when the robot should reach the zero
state from state $[0.35,0.65,0,0]^{\top}$ (no stage position constraints).

.  Without terminal constraint and terminal cost MPC TMPC PS(40) PS(80)
PS(120) PS(160) PS(200) 339.23 379.76 340 339.6 339.6 339.6 339.6 With
terminal constraint and terminal cost MPC TMPC PS(40) PS(80) PS(120) PS(160)
PS(200) 402.91 781.32 667.05 597.10 540.48 531.28 528.31

The following $4$ scenarios were designed and simulated for situations
relevant to SaR missions, each assessing one key property of SDD-TMPC,
compared to MPC and TMPC. The scenarios were simulated several times to show
the behavior of the controllers under different disturbances.

##### Scenario 1: Closely following a reference path

The main aim of the comparison among MPC, TMPC, and SDD-TMPC in scenario 1 is
to assess how fast a robot that is controlled by each of these approaches
reaches a given destination, following a reference path (i.e., an ordered set
of given positions). As soon as the (nominal) state of the robot reaches a
distance of $0.3~{}$m from its corresponding reference position, the next
reference position from the path is sent to the controller as a new reference.
Scenario 1 resembles a real-life SaR situation when a robot should precisely
travel along a given path in the presence of external disturbances (e.g., to
avoid hazards that exist in the close vicinity of the robot) that is
determined via a higher-level controller. Scenario 1 was simulated 3 times,
considering the following situations:

  1. 1.

The system was not affected by external disturbances.

  2. 2.

The external disturbances helped in attracting the system to the current
reference position.

  3. 3.

The external disturbances resulted in repelling the system from the current
reference position.

The entire mission time of the robot for scenario 1 for the 3 MPC-based
controllers is represented in Table 6. Based on the results, for MPC the
mission time is strongly affected by the external disturbances (see the
significant variations in the mission time in different cases), whereas TMPC
and SDD-TMPC provide more consistent results independent of the realized
disturbances. TMPC needs more time than SDD-TMPC to finish the mission. In
fact, to keep the states within the tube of TMPC (determined for the worst
disturbance case), the nominal acceleration and speed of the robot have to
remain within $60\%$ and $50\%$ of their maximum allowed values.

Table 6: The mission time required by the robot in scenario 1 to closely track the entire reference path. | Mission time [s] without terminal cost and constraint | Mission time [s] with terminal cost and constraint
---|---|---
| MPC | TMPC | SDD-TMPC | MPC | TMPC | SDDT-MPC
Case 1 | 10.8 | 12.9 | 10.7 | 12.5 | 13.3 | 11.5
Case 2 | 7.8 | 12.9 | 10.7 | 8.6 | 13.3 | 11.3
Case 3 | 18 | 12.9 | 10.7 | 19.8 | 13.3 | 11.4

##### Scenario 2: Mission planning in the vicinity of obstacles

SaR robots should often move close to obstacles. In such cases, the trajectory
of the robot must guarantee that no crashes occur with the obstacles. However,
overly conservative decisions may significantly slow down the mission. In
scenario 2, the robot has to move horizontally, from position
$[2,0.01]^{\top}$ to position $[8.5,0.01]^{\top}$. We suppose that if the
vertical position of the robot falls below $p_{y}=0$, a crash with an obstacle
(e.g., a wall that is extended across $p_{y}=0$) has occurred. Moreover, the
value of $\beta(\bm{x}_{k})$ changes with the coordinates based on the
following equation:

$\beta(\bm{x}_{k})=\frac{1}{|5-p_{x,k}|+1}$ (22)

Scenario 2 was simulated 3 times, considering the following situations:

  1. 1.

The system was not affected by external disturbances.

  2. 2.

The system was pushed upwards by external disturbances.

  3. 3.

The system was pushed downwards by external disturbances.

The results of the simulations for scenario 2 when MPC, TMPC, and SDD-TMPC are
used to steer the robot, are shown in Figures 4. MPC steers the robot such
that it moves close to the wall. However, for the third simulation when the
system was pushed downwards due to external disturbances, the robot collided
into the wall. Based on Figure 4, TMPC assures a safe, collision-free
trajectory for the robot, while the robot moves relatively far away from the
wall. The initial nominal state of the robot with TMPC was far away from the
wall, otherwise the initial tube of TMPC would collide with the wall. Figure 4
also shows the nominal and the realized trajectories of the robot when it is
controlled via SDD-TMPC. While the robot moves much closer to the wall
compared to TMPC, it never crashes into it. Note that from (4), the nominal
states of SDD-TMPC are determined based on an estimation of the external
disturbances. Thus, the nominal trajectories vary when the disturbances do. In
other words, the more slippery the floor, the more careful the actions chosen
by SDD-TMPC.

Figure 4: Scenario 2: Trajectories of the robot position when controlled by
MPC (top plot), TMPC (middle plot) and SDD-TMPC(bottom plot), in case 1 (in
green), 2 (in red), and 3 (in yellow). Note that the spectrum of blue
corresponds to the values of $\beta(\bm{x}_{k})$ for all realized states
$\bm{x}_{k}$ of the robot during the simulation. In the middle plot, the black
trajectory corresponds to the nominal case. For the bottom plot, the
corresponding nominal and real trajectories are shown in the same color but
with solid and dashed lines respectively. Figure 5: Scenario 3: The feasible
state set $\mathbb{X}$ (in blue) and the tube of TMPC (in red). Figure 6:
Scenario 3: The evolution of the position $p_{x}$ (blue) and the velocity
$\nu_{x}$ (orange) of the robot, when SDD-TMPC is used and no external
disturbances exist. Figure 7: Scenario 3: Comparison of the trajectories of
the position of the robot using SDD-TMPC, when no external disturbances affect
the robot (green curve) and when external disturbances push the robot upwards
relative to its speed (the blue curve for the nominal trajectory and the
orange curve for the realized trajectory). Note that the black lines represent
the boundaries of the corridor.

##### Scenario 3: Approaching an unreachable target

In SaR missions, there are situations when the robot should move in very
narrow corridors without crashing into the walls. In such cases, TMPC may
become infeasible, due to its conservativeness. Thus scenario 3 is designed
such that the problem is infeasible for TMPC, and we assess whether SDD-TMPC
is able to return a solution for the problem. In scenario 3, the robot starts
at position $[5.5,0]^{\top}$ with a zero initial speed, and has to reach
position $[11,0]^{\top}$. The following hard constraints on the position of
the robot hold for all time steps $k$ during the simulation, where these
constraints imply avoiding crashes into the walls in a corridor that is
narrowing down:

$p_{x,k}+8p_{y,k}<10;\quad p_{x,k}-8p_{y,k}<10$ (23)

Note that all coordinates are given in m. TMPC is already infeasible at the
initial position of the robot, since the farthest position for which TMPC can
still generate a feasible tube corresponds to $p_{x}=4.59$ m, where the width
of the corridor is about $0.68$ m (see Figure 5, which shows the corridor and
the tube of TMPC). The aim is to find out how far can the robot move forward
inside this corridor until the SDD-TMPC problem becomes infeasible.

Scenario 3 was simulated twice, considering the following situations:

  1. 1.

The system was not affected by external disturbances.

  2. 2.

The system was pushed upwards (relative to the speed value) via external
disturbances.

Note that in both cases, the upper bounds of potential external disturbances
were given to TMPC, whereas SDD-TMPC used its FIS to estimate the upcoming
disturbances. The prediction horizon used for this scenario was tuned to $6$
(i.e., larger than the previous scenarios), which was because in this scenario
there is no terminal constraint as the target of the robot is outside of its
feasible set. Thus, the alternative approach is to increase the size of the
prediction horizon in order to reduce the risks of collision. With a
prediction horizon smaller than $6$, due to being short-sighted, the
simulations showed that the robot accelerated rapidly and did not have time to
slow down in time in order to avoid a crash into the walls when the corridor
narrowed down.

In Figure 6, the evolution of the position and the velocity of the robot for
the first situation (i.e., when no external disturbances exist) are shown,
where SDD-TMPC is used to steer the robot. From this figure, since there is no
wall in a close neighborhood of the robot, it first moves more freely
(faster), and then slows down in time (thanks to the larger prediction
horizon) as the corridor becomes narrower. In fact, with SDD-TMPC the robot
can move further through the corridor, compared to when TMPC is used. In case
external disturbances exist and push the robot upwards relative to its speed
value, a similar pattern of behavior for the robot is observed, although the
increase in the speed will be less significant than the case without external
disturbances. Figure 7 shows the trajectories for the position of the robot
for both cases, i.e., with and without external disturbances. By comparing the
trajectories, we observe how SDD-TMPC changes the nominal vertical position
(in presence of external disturbances), which, in a vacuum, raises the cost
(because of deviating from the reference trajectory). However, SDD-TMPC makes
this decision for the nominal trajectory of the robot, because it foresees
that its ancillary control input will cancel out the impact of the external
disturbances as much as possible for the realized trajectory $p_{y,k}$ of the
robot. Such behavior cannot be obtained using lar MPC or TMPC.

##### Scenario 4: Ability to make high-level optimal decisions

Next to making robust decisions, SDD-TMPC is expected to improve the
performance by making high-level optimal decisions. In order to assess this,
in scenario $4$ we consider a case where the robot has to move from position
$[2.3,3]^{\top}$ to position $[3.25,3.05]^{\top}$, with an obstacle positioned
on the way of the robot (see Figure 8). We suppose that the robot has the
initial horizontal speed of $\nu_{x}=1~{}\frac{\textrm{m}}{\textrm{s}}$, and
should select an initial vertical speed of either
$\nu_{y}=1.2~{}\frac{\textrm{m}}{\textrm{s}}$ (i.e., moving upwards) or
$\nu_{y}=-1.2~{}\frac{\textrm{m}}{\textrm{s}}$ (i.e., moving downwards). When
the robot moves upwards, the slipperiness coefficient is larger and thus, the
robot is prone to larger disturbances, compared to when it moves downwards.
Therefore, when the robot moves upwards, the larger external disturbances will
result in a longer realized path (due to an increased distance from the
obstacle) for the robot compared to when the robot moves downwards. It is
trivial that since MPC and TMPC do not take into consideration the dynamics of
the external disturbances, they estimate the less cost when $\nu_{y}$ is
positive, whereas SDD-TMPC returns a lower cost ($772$) for the trajectory
that corresponds to the initial vertical speed of
$\nu_{y}=-1~{}\frac{\textrm{m}}{\textrm{s}}$ (since this trajectory is exposed
to fewer external disturbances), compared to the other trajectory that is
exposed to larger disturbances (estimated cost of SDD-TMPC is $917$). In this
scenario, we did not stop the optimization early. Therefore, SDD-TMPC
potentially performs better than MPC and TMPC (in terms of the realized cost)
in complex environments that are prone to state-dependent external
disturbances.

Figure 8: Scenario 4: The robot has to move from position $[2.3,3]^{\top}$ to
position $[3.25,3.05]^{\top}$, while avoiding an obstacle (illustrated by the
triangular red shape). The trajectories of the position projections of the
tubes for two cases were shown in green (above the obstacle) and yellow (below
the obstacle) colors.

###  4.2 Case study 2

####  4.2.1 Simulation setup

The problem presented in [39] compared two controllers: nonlinear TMPC and
nonlinear robust MPC. We replicated the experiment setup and the
implementation of nonlinear TMPC, utilizing the same numerical values. The
results will show that while using SDD-TMPC, the robot was able to achieve
better performance by using less conservative control input (i.e., the robot
moved faster) and still achieve a robust solution. The only detected
differences to our knowledge are:

  * •

Disturbance generator: The authors presented solely the boundaries without
elaboration on the random generation or publication of code.

  * •

Solver: We used the same algorithm (interior point), but from Matlab’s
optimization toolbox. However, due to the nonconvex nature of the optimization
problem, we cannot ensure that we will identify the exact same solutions.

  * •

Tube initialization: In [39], it was noted that the solver had the freedom to
choose the initial nominal position at each time step, provided that the
measured actual position remained within the tube. However, there is no
mention of the starting nominal direction. We permitted the solver to freely
choose the direction, but it remains uncertain if this was the intended
design.

  * •

Discretization method: In [39], the authors studied a continuous system that
was discretized using the ICLOCS toolbox [44]. We employ Matlab’s c2d
function, using Zero-Order Hold (ZOH) [45] and Runge-Kutta 4 algorithm to
discretize linear and nonlinear systems, respectively.

  * •

Hardware: We utilized the same computer that was employed in case study 1.

In the simulation, two unicycle robots are present: a virtual leader robot,
which is assumed to remain unaffected by the external disturbances, and a
follower robot, which should be controlled and is affected by the
disturbances. A unicycle robot is a circular robot that is steered at time
instant $t$ by the velocities $\nu_{t}^{\text{l}}$ and $\nu_{t}^{\text{r}}$ of
its, respectively, left wheel and right wheel. The inputs of the system at
time instant $t$ are the linear velocity $\nu_{t}$ and the angular velocity
$\omega_{t}$, given by:

$\nu_{t}=(\nu_{t}^{\text{l}}+\nu_{t}^{\text{r}})/2$ (24)
$\omega_{t}=(\nu_{t}^{\text{r}}-\nu_{t}^{\text{l}})/(2\rho)$ (25)

where $\rho$ represents the radius of the robot. Moreover, the admissible set
of the inputs is defined by:

$\mathbb{U}=:\Big{\\{}[\nu,\omega]^{T}\in\mathbb{R}^{2}\Big{|}\frac{|\nu|}{\nu^{\max}}+\frac{\rho|\omega|}{\nu^{\max}}\leq
1\Big{\\}}$ (26)

where $\nu^{\max}$ is the maximum absolute velocity of the wheels.

The leader robot utilizes constant reference inputs $\nu^{\text{R}}$ and
$\omega^{\text{R}}$. The state $\bm{x}_{t}^{\text{R}}$ of the leader robot per
time instant $t$ is characterized by the position
($p^{\text{R}}_{x,t},p^{\text{R}}_{y,t}$) of its center and by the orientation
$\psi_{t}^{\text{R}}$ of the robot.

In the continuous time domain, the state evolution for the leader robot for
time instant $t$ is determined by the following non-linear system equations
(27):

$\begin{bmatrix}\dot{p}^{\text{R}}_{x,t}\\\ \dot{p}^{\text{R}}_{y,t}\\\
\dot{\psi}_{t}^{\text{R}}\end{bmatrix}=\begin{bmatrix}\nu^{\text{R}}\cos(\psi_{t}^{\text{R}})\\\
\nu^{\text{R}}\sin(\psi_{t}^{\text{R}})\\\ \omega^{\text{R}}\end{bmatrix}$
(27)

The input $\bm{u}_{t}$ of the follower robot at time instant $t$ consists of
the linear velocity $\nu_{t}$ and the angular velocity $\omega_{t}$ of this
robot, while its state $\bm{x}_{t}$ is determined by the position
$(p_{x,t},p_{y,t})$ of the head111The head of the robot is referred to the
front point of the robot, where this point is located on the main axis of the
robot that is perpendicular to the axis of the wheels of the robot. of the
robot and by its orientation $\psi_{t}$. The position is impacted by the
unknown disturbance vector $\bm{w}_{t}=[w_{x,t},w_{y,t}]^{T}$, such that
$\sqrt{w_{x,t}^{2}+w_{y,t}^{2}}<\eta$. The state evolution for the follower
robot is thus determined by the nonlinear system equations below:

$\dot{\bm{x}}_{t}=\begin{bmatrix}\dot{p}_{x,t}\\\ \dot{p}_{y,t}\\\
\dot{\psi_{t}}\end{bmatrix}=\begin{bmatrix}\nu_{t}\cos(\psi_{t})+\rho\omega_{t}\sin(\psi_{t})\\\
\nu_{t}\sin(\psi_{t})+\rho\omega_{t}\cos(\psi_{t})\\\
\omega_{t}\end{bmatrix}+\begin{bmatrix}w_{x,t}\\\ w_{y,t}\\\
0\end{bmatrix}=f(\bm{x}_{t},\bm{u}_{t},\bm{w}_{t})$ (28)

The robots were simulated for $100$ time steps and based on the e-pucks [46],
with $\nu^{\max}=0.13\frac{\text{m}}{\text{s}}$ and $\rho=0.0267$m. The aim is
to maintain a constant distance
$\bm{p}^{\textrm{d}}=[p^{{\textrm{d}}}_{x},p^{{\textrm{d}}}_{y}]^{T}$ between
the follower and the leader robots, in the local coordinate frame of the
leader robot (the origin of this local frame coincides with the position of
the center of the leader robot and the $x$-axis is parallel to the direction
of the movement of the robot). The leader robot moves with
$\nu^{\textrm{R}}=0.015\frac{\text{m}}{\textrm{s}}$ and
$\omega^{\textrm{R}}=0.04\frac{\text{rad}}{\text{s}}$. We considered the
initial state $[0,0,\frac{\pi}{3}]$ for the leader robot, the initial state
$[0.4,-0.2,-\frac{\pi}{2}]^{T}$ for the follower robot, and
$p^{\textrm{d}}=[-0.1,-0.1]^{T}$ as the desired distance between the leader
and follower robots (measured in the local coordinates of the leader robot).
The bound of the disturbances on the $x$ and $y$ positions of the follower
robot is $\eta=0.004$ m. Note that the control framework in this problem is
discrete time. Therefore, we used Matlab Ode45 [47] in order to determine the
evolved states per discrete time step, using the system of differential
equations given by (27) and (28).

####  4.2.2 Nonlinear TMPC design

In this section, we describe the design of the nonlinear TMPC, based on the
approach given in [39], for controlling the motion of the follower robot. At
every iteration, TMPC solves the following problem:

$V^{*}(\bm{x}_{k})=\min_{\tilde{\bm{z}}_{k},\tilde{\bm{v}}_{k}}\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{z}_{i|k}^{\text{r}}\right\rVert_{Q}^{2}+\left\lVert\bm{v}_{i|k}^{\text{r}}\right\rVert_{R}^{2}\right)+\left\lVert\bm{z}^{\text{r}}_{k+N|k}\right\rVert_{F}^{2}$
(29a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{x}_{k}\in\\{\bm{z}_{k}\\}\oplus\mathbb{E}^{\max}$ (29b)
$\bm{z}_{i+1|k}=f^{\text{d}}(\bm{z}_{i|k},\bm{v}_{i|k})$ (29c)
$\bm{v}_{i|k}\in\mathbb{V},\quad\bm{z}_{i+N|k}^{\text{r}}\in\mathbb{Z}_{\text{f}}$
(29d)
$\bm{z}^{r}_{i|k}[1,2]=\text{Rot}(-\bm{z}_{i|k}[3])(\bm{x}_{i}^{\text{R}}[1,2]-\bm{z}_{i|k}[1:2])+\text{Rot}(\bm{z}^{\text{r}}_{i|k}[3])\bm{p}_{\textrm{d}}$
(29e) $\bm{z}^{\text{r}}_{i|k}[3]=\bm{x}_{i}^{\text{R}}[3]-\bm{z}_{i|k}[3]$
(29f)
$\bm{v}^{\text{r}}_{i|k}=\begin{bmatrix}-\bm{v}_{i|k}[1]+(\nu_{\text{R}}-p^{\textrm{d}}_{x}\omega^{\text{R}})\cos(\bm{z}^{\text{r}}_{i|k}[3])-p^{\text{d}}_{x}\omega^{\text{R}}\sin(\bm{z}^{\text{r}}_{i|k}[3])\\\
-\rho\bm{v}_{i|k}[2]+(\nu_{\text{R}}-p^{\textrm{d}}_{x}\omega^{\text{R}})\sin(\bm{z}^{\text{r}}_{i|k}[3])-p^{\text{d}}_{y}\omega^{\text{R}}\cos(\bm{z}^{\text{r}}_{i|k}[3])\end{bmatrix}$
(29g)

where (29b) states that the initial nominal state $\bm{z}_{k}$ of the follower
robot, which is a decision variable, should be determined such that the error
between the measured state $\bm{x}_{k}$ and the nominal state $\bm{z}_{k}$ of
the follower robot belongs to set $\mathbb{E}^{\max}$. Moreover, according to
(29c), the nominal states of the follower robot evolve according to the
discretized function $f^{\text{d}}(\cdot)$, which is determined after time-
discretization of (28) and by putting $\bm{w}_{k}=0$ for all time steps.
Constraint (29d) enforces all the calculated nominal inputs to fall within set
$\mathbb{V}$, which should be constructed, such that if
$\bm{v}_{i|k}\in\mathbb{V}$, then for the actual input we have
$\bm{u}_{i|k}\in\mathbb{U}$ for a given ancillary control law. Finally,
$\mathbb{Z}_{\text{f}}$ is the terminal set of the states for the follower
robot. The cost function $V(\cdot)$ is computed using
$\bm{z}^{\text{r}}_{i|k}$, which is the nominal state of the follower robot
within the coordinate system of the leader robot, while the original nominal
state $\bm{z}_{i|k}$ is given in the global coordinates. The transition from
the global coordinate system to the local coordinate system of the leader
robot is done via (29e)-(29g), where Rot$(\cdot)$ is a function that returns a
$2$-dimensional rotation matrix with respect to the global coordinates, given
an input angle. Whenever square brackets are used after a vector, if the
bracket contains one scalar, the notation refers to the element corresponding
to that scalar of the vector. In case the square brackets include ‘scalar
1:scalar 2’, the notation refers to elements ‘scalar 1’ to ‘scalar 2’
(including these two elements) of the vector.

The nonlinear ancillary control law in [39] is first given in the continuous
time domain for a given time instant $t$, via:

$\bm{u}_{t}=\begin{bmatrix}\cos(\bm{x}_{t}[3])&-\rho\sin(\bm{x}_{t}[3])\\\
\sin(\bm{x}_{t}[3])&\rho\cos(\bm{x}_{t}[3])\end{bmatrix}^{-1}\left(\begin{bmatrix}\cos(\bm{z}_{t}[3])&-\rho\sin(\bm{z}_{t}[3])\\\
\sin(\bm{z}_{t}[3])&\rho\cos(\bm{z}_{t}[3])\end{bmatrix}\bm{v}_{t}-K^{\text{e}}\bm{e}_{t}[1:2]\right)$
(30)

where $K^{\text{e}}$ is a design variable that remains constant during the
implementation of the nonlinear TMPC. This ancillary control law linearizes
the dynamics of the position error of the controlled system. This dynamics is
described by:

$\dot{\bm{e}}_{t}[1:2]=K^{\text{e}}\bm{e}_{t}[1:2]+[w_{x},w_{y}]^{T}$ (31)

After discretization, the dynamics of the position error for time step
$i\in\\{k,\ldots,k+N-1\\}$ is given by:

$\bm{e}_{i+1}[1:2]=K^{\text{e,d}}\bm{e}_{i}[1:2]+T^{\textrm{s}}[w_{x},w_{y}]^{T}$
(32)

where $T^{\textrm{s}}$ represents the sampling time and $K^{\text{e,d}}$ is
calculated based on the zero-order hold approach (ZOH) [45], in order to
ensure equivalence between the continuous-time and discrete-time formulations.
The cross section $\mathbb{E}^{\max}$ of the tube for nonlinear TMPC, the set
$\mathbb{V}$ of admissible values for the nominal control inputs, and the
terminal control law (i.e., the control law that is implemented beyond the
prediction horizon) are defined by:

$\mathbb{E}^{\max}=\Big{\\{}\bm{e}\in\mathbb{R}^{3}\big{|}\max(|\bm{e}[1:2]|)<\frac{T^{\textrm{s}}\eta}{1-K^{\text{e,d}}}\Big{\\}}$
(33)
$\mathbb{V}=\Big{\\{}\bm{v}\in\mathbb{R}^{2}\big{|}[\frac{1}{\nu^{\max}},\frac{\rho}{\nu^{\max}}]|\bm{v}|<\frac{\sqrt{2}}{2}-\frac{\eta\sqrt{2}}{\nu^{\max}}\Big{\\}}$
(34)
$\bm{u}_{k}^{\textrm{T}}=\begin{bmatrix}\bar{K}\bm{z}_{k+N}^{\text{r}}[1]+(\nu^{\text{R}}-p^{\text{d}}\omega^{\text{R}})\cos(\bm{z}_{k+N}^{\text{r}}[3])-p^{\text{d}}_{x}\omega^{\text{R}}\sin(\bm{z}_{k+N}^{\text{r}}[3])\\\
\frac{1}{\rho}(\bar{k}\bm{z}_{k+N}^{\text{r}}[2]+(\nu^{\text{R}}-p^{\text{d}}_{x}\omega^{R})\sin(\bm{z}_{k+N}^{\text{r}}[3])-p^{\text{d}}_{y}\omega^{R}\cos(\bm{z}_{k+N}^{\text{r}}[3]))\end{bmatrix}$
(35)

where $\bar{K}$ is a constant. Finally, the terminal admissible set for the
position of the follower robots is given by:

$\mathbb{Z}_{\text{f}}=:\left\\{\bm{z}^{\text{r}}\in\mathbb{R}^{3}\big{|}\bar{K}(|\bm{z}^{\text{r}}[1]|+|\bm{z}^{\text{r}}[2]|)<\frac{\nu^{\max}\sqrt{2}}{2}-\eta\sqrt{2}-\sqrt{2}\left\lVert\begin{bmatrix}1&-p^{\text{d}}_{x}\\\
0&p^{\text{d}}_{y}\end{bmatrix}\right\rVert\right\\}$ (36)

In the numerical experiment, the following constants were chosen: $N=10$,
$T^{\textrm{s}}=0.2$s, $Q$=diag(0.2,0.2,0), $R$=diag(0.4,0.4),
$F$=diag(0.5,0.5), $\bar{K}=1.2$, $K^{\text{e}}$=2.3, $K^{\text{e,d}}$=0.63,
which lead to the following sets:
$\mathbb{E}^{\max}=\\{\bm{e}\in\mathbb{R}^{3}|\max(|\bm{e}[1:2]|)\leq
0.0022\\}$ $\mathbb{V}=0.6636\mathbb{U}$, and
$\mathbb{Z}_{\text{f}}=\\{\bm{z}^{\text{r}}\in\mathbb{R}^{3}||z^{\text{r}}(1)|+|z^{\text{r}}(2)|\leq
0.0542\\}$.

Table 7: The following mathematical notation is used in section 4.2:

The notation | The explenation | The notation | The explenation
---|---|---|---
diag$(\cdot)$ | This function returns diagonal matrix with chosen elements | set[i] | Projection of a set over $i^{\text{th}}$ dimension of the state space
$F$ | Terminal cost matrix of states of the original cost function | $T_{s}$ | Sampling time
$f^{d}(\cdot)$ | Discrtzied system function | vector[i] | The $i^{\text{th}}$ element of the vector.
$\bar{k}$ | A constant used in the terminal control law | $\mathbb{V}$ | A polytope set of feasible inputs used by controller
$k_{e}$ | State-feedback constant | $\mathbb{V}_{\text{NL}}$ | A nonlinear set which could be used to tighten input constraints
$k_{ed}$ | Discretized state-feedback constant | $v^{r}$ | Nominal input in transformed to leader’s coordinate frame
m | meters | $z^{r}$ | Nominal state in transformed to leader’s coordinate frame
$\frac{\text{m}}{\text{s}}$ | meters per second | $\eta$ | Boundary of disturbance
$\bm{p}_{d}$ | A constant distance follower should keep to the leader. | $\lambda(\cdot)$ | A state-dependent parameter used to tighten input constraints.
${p}_{d,x}$ | A constant distance follower should keep to the leader in x-axis. | $\nu$ | Linear velocity of the follower
${p}_{d,y}$ | A constant distance follower should keep to the leader in y-axis. | $\nu_{\text{max}}$ | Maximum velocity of the robot’s wheels
$p_{\text{R},x}$ | The current leader’s position component parallel to x-axis | $\nu_{\text{l}}$ | Velocity of the follower’s left wheel
$p_{\text{R},y}$ | The current leader’s position component parallel to y-axis | $\nu_{\text{r}}$ | Velocity of the follower’s right wheel
$p_{x}$ | The current follower’s position component parallel to x-axis | $\nu_{\text{R}}$ | Linear velocity of the leader
$p_{y}$ | The current follower’s position component parallel to y-axis | $\rho$ | Radius of the robot
$Q$ | Stage cost matrix of states | $\psi$ | The current follower’s direction
$R$ | Stage cost matrix of inputs | $\psi_{\text{R}}$ | The current leader’s direction
ROT$(\cdot)$ | A function that returns a rotation matrix in 2 dimensions | $\omega$ | Angular velocity of the follower
s | seconds | $\omega_{\text{R}}$ | Angular velocity of the leader

####  4.2.3 SDD-TMPC design

In this section, we present the design of SDD-TMPC for controlling the
follower robot. The same ancillary control law that is given by (30) is used
for SDD-TMPC. In case study 1, the main effort was to make the error sets
$\mathbb{E}_{i}$ for all $i\in\\{k+1,\ldots,k+N\\}$ as small as possible. In
case study 2, however, in order to reduce the computation time, which is
affected by the larger prediction horizon (which is $10$), we approximate the
error sets via box sets. Although this may slightly increase the conservatism,
it significantly reduces the computation time. Let $\mathbb{E}_{i}[\ell]$
denote the projection of the error set $\mathbb{E}_{i}$ onto the
$\ell^{\text{th}}$ dimension of the state space (thus for this case study
$\ell=1,2,3$). Since we are working with box sets, $\mathbb{E}_{i}$ is the
cross product over the projections of this set on all the dimensions of the
state space. Based on (32), for $\ell=1,2$ for the projection of the error set
we can write:

$\mathbb{E}_{i+1}[\ell]=K^{\text{e,d}}\mathbb{E}_{i}[\ell]\oplus\mathbb{W}$
(37)

where $\mathbb{W}=\\{w\in\mathbb{R}|w\leq T^{\textrm{s}}\eta\\}$.

The resulting position error dynamics are linear, but deriving the rotation
error dynamics leads to nonlinear dynamics. Note that computing all possible
errors per time instant via nonlinear dynamics is generally computationally
expensive. Moreover, we have chosen to work with polytopic error sets, while
propagating polytopes through nonlinear equations does not necessarily result
in a polytopic error set per time instant. Therefore, as suggested in [48], we
replace the nonlinear dynamic equation by a simplified evolution equation for
$\bm{e}_{t}[3]$, where the remaining terms are treated as state-dependent
external disturbances (38).

$\dot{\bm{e}}_{t}[3]=-\frac{1}{\rho}\bm{e}_{t}[3]\bm{v}_{t}[1]+w^{\text{e}}_{t}\text{
where }w^{\text{e}}_{t}\in\mathbb{W}_{t}[3](\bm{e},\bm{z},\bm{v})$ (38)

This approach has proven to be very accurate for small deviations in the third
dimension of the state space (in this case the robot’s heading). From now on,
we will assume that the absolute values of $\bm{e}_{t}[3]$ remain relatively
small (i.e., $|\bm{e}_{t}[3]|<\pi/4$). The exact procedure for deriving (38)
has been shown in Appendix A.

The final phase of the SDD-TMPC design for case study 2 is to find a method to
tighten the nominal input constraints online so that the real control input
satisfies the original input constraints. To ensure this, the following input
constraint (39a) must be satisfied, where $\mathbb{V}^{\text{NL}}(\cdot)$ is a
state-dependent set. Although the resulting set is a polytope if the error in
direction is a known value (see a red polytope in Figure 9), this is not true
if there is a set of possible errors in direction. In such a case, to ensure
the satisfaction of the constraint, $\mathbb{V}^{\text{NL}}(\cdot)$ is the
intersection of the output sets (see a black polytope in Figure 9), which
generally does not lead to a polytope if the set of possible errors in
direction is continuous and the exact computation of (39a) per time instant
can become computationally intractable.

$\\{\bm{v}_{t}\\}\oplus\begin{bmatrix}-\cos(\bm{z}_{t}[3])&-\sin(\bm{z}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{z}_{t}[3])&-\frac{1}{\rho}\cos(\bm{z}_{t}[3])\end{bmatrix}K^{\text{e}}(\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[2])\subseteq\bigcap_{\bm{e}\in\mathbb{E}_{t}}\left(\begin{bmatrix}\cos(\bm{e}_{t}[3])&-\rho\sin(\bm{e}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{e}_{t}[3])&\cos(\bm{e}_{t}[3])\end{bmatrix}\mathbb{U}\right)=\mathbb{V}^{\text{NL}}(\mathbb{E}_{t}[3])$
(39a)

To reduce the computational complexity associated with (39a) and to ensure
that the resulting $\mathbb{V}^{\text{NL}}(\mathbb{E}_{t}[3])$ is always
represented by a polytope, we replace the quadrangles corresponding to
$\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ by (i.e., the red background
quadrangle in the Figure 9) with the largest subset of
$\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ for which the diameters are parallel
to the $x$ and $y$ axes (see the blue foreground quadrangle in Figure 9). In
other words, we replace $\mathbb{V}^{\text{NL}}(\cdot)$ with the
multiplication of the original set $\mathbb{U}$ and a positive, state-
dependent scalar function $\lambda(\cdot)$, i.e:

$\frac{1}{\cos(\cdot)+|\sin(\cdot)|}\mathbb{U}=\lambda(\cdot)\mathbb{U}\subseteq\mathbb{V}^{\text{NL}}(\cdot)$
(39b)

Figure 9: Illustration of an example quadrangle (the red background
quadrangle) that represents $\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ when
$\bm{e}_{t}[3]=0.3473$. The intersection of all such quadrangles for the
entire range of $\bm{e}_{t}[3]$, i.e., for $-0.3473\leq\bm{e}_{t}[3]\leq
0.3473$, generates $\mathbb{V}^{\text{NL}}(\mathbb{E}_{t}[3])$ (the black
middle-ground polygon). The simplified version of
$\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$, i.e., the largest subset of
$\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ with its diameters parallel to the $x$
and $y$ axes, is shown via the blue foreground quadrangle.

Figure 10 shows the curve corresponding to function $\lambda(\cdot)$. We
define the admissible set $\mathbb{V}\left(\mathbb{E}_{t}\right)$ for the
nominal control inputs as a function of the error set via:

$\mathbb{V}\left(\mathbb{E}_{t}\right):=\min_{\bm{e}_{t}[3]\in\mathbb{E}_{t}[3]}\left(\lambda(\bm{e}_{t}[3]\right)\mathbb{U}\ominus\begin{bmatrix}-\cos(\bm{z}_{t}[3])&-\sin(\bm{z}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{z}_{t}[3])&-\frac{1}{\rho}\cos(\bm{z}_{t}[3])\end{bmatrix}K^{\text{e}}(\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[2])$
(39c)

For the design of TMPC in [39], the admissible set of nominal control inputs
was generated by scaling the set $\mathbb{U}$ by a constant value
$\mathbb{U}$. with a constant value of $0.6636$. As shown in Figure 10, the
set $\mathbb{V}\left(\mathbb{E}_{t}\right)$ was defined by over (39c) is
consistently larger. Thus, the original design is more conservative than our
proposed approach. Note that to further reduce the conservatism,
$\mathbb{V}^{\text{NL}}\left(\mathbb{E}_{t}\right)$ and thus
$\mathbb{V}\left(\mathbb{E}_{t}\right)$ can be approximated by polytopes that
generally have more vertices than $4$. The exact procedure for deriving the
input constraints is given in the appendix B.

Figure 10: Illustration of the curve representing function $\lambda(\cdot)$
versus the error in the third dimension of the state space.

Finally, using the derived error dynamics given by (37) and the time-discrete
version of (38), the online input constraint tightening method that is
formulated by (39c), and using the solutions of the regular nonlinear TMPC
method from [39] as a warm start, the nonlinear optimization problem that is
solved by SDD-TMPC per time step is given by:

$V^{*}(\bm{x}_{k},\bm{z}_{k})=\min_{\tilde{\bm{z}}_{k},\tilde{\bm{v}}_{k},\mathbb{T}_{k}}\sum_{i=k}^{k+N-1}\left(\left\lVert\bm{z}_{i|k}^{\text{r}}\right\rVert_{Q}^{2}+\left\lVert\bm{u}_{i|k}^{\text{r}}\right\rVert_{R}^{2}\right)+\left\lVert\bm{z}^{\text{r}}_{k+N|k}\right\rVert_{F}^{2}$
(40a) s.t. for $i=k+1,\ldots,k+N$:
$\bm{z}_{i+1|k}=f^{\text{d}}(\bm{z}_{i|k},\bm{v}_{i|k})$ (40b)
$\mathbb{T}_{k}=\\{\\{z_{k+1|k}\\}\oplus\mathbb{E}_{k+1|k},\ldots,\\{z_{N|k}\\}\oplus\mathbb{E}_{k+N|k}\\}$
(40c)
$\bar{\mathbb{E}}_{i|k}=\text{diag}\left(K^{\text{e,d}},K^{\text{e,d}},K^{\theta,\text{d}}\right)\mathbb{E}_{i-1|k}$
(40d)
$\mathbb{E}_{{i|k}}=\bar{\mathbb{E}}_{i|k}\oplus\mathbb{W}_{i-1|k}(\mathbb{E}_{i-1|k},z_{i-1|k},v_{i-1|k})$
(40e) $\mathbb{E}_{k}=\\{\bm{x}_{k}-\bm{z}_{k}\\}$ (40f)
$v_{i|k}\in\mathbb{V}(\mathbb{E}_{i|k})$ (40g)
$\bm{z}^{\text{r}}_{N|k}\in\mathbb{Z}_{\text{f}}$ (40h)

where $K^{\theta,\text{d}}$ is the dynamic coefficient that is multiplied by
$\bm{e}_{\ell}[3]$, after (38) is discretized in time, with $\ell$ the time
step counter.

For ease of comparison, we use the same horizon, sampling time, cost function,
and terminal set for both regular nonlinear TMPC and SDD-TMPC. With the
solution of regular nonlinear TMPC used as a warm start for SDD-TMPC, a
solution is obtained for SDD-TMPC using the PS algorithm in under $30$
seconds.

####  4.2.4 Numerical results

In figure 11, the path followed by both controllers is visible. It is evident
that both controllers were able to reach and follow the path, despite the
disturbance. Figure 12 shows that the absolute value of error in direction is
actually less than 0.6 rad (below 3.5 degrees), confirming our assumption of
low angles.

The benefit of utilizing SDD-TMPC is illustrated in figure 13, which depicts
SDD-TMPC transitioning from a transient state to a steady state at a more
rapid pace. Using SDD-TMPC robot needed over 10% less time to decrease
distance to the destination below 0.05m. In addition, once both controllers
achieve the steady state, their performance is nearly equivalent, as seen in
figure 14. This is due to the fact that SDD-TMPC is authorized to employ
substantially greater nominal inputs, as shown in figure 15. Even though SDD-
TMPC returns significantly higher input (above 30%), especially in the early
stages, the actual input constraints are never violated (Figure 16) and we can
observe that the real input is usually 20% higher, during the first 3 seconds
of the mission, compared to the input of TMPC. This is particularly intriguing
because the nominal input of SDD-TMPC violates the original constraints
$\mathbb{U}$. Nonetheless, SDD-TMPC anticipates the ancillary controller’s
behavior and understands it can marginally violate constraints since the true
input will not breach them. Those results are noteworthy as [39] compared TMPC
to another robust MPC design. TMPC exhibited improved steady-state performance
but required more time to achieve it. However, with SDD-TMPC we successfully
eliminated the high-rise time while maintaining equivalent steady-state
performance.

Figure 11: A desired path, paths followed by the leader, and two follower
paths when the follower was using TMPC or SDD-TMPC as controller. Figure 12: A
graph illustrating the variance between nominal and actual direction for SDD-
TMPC. Figure 13: A distance between the desired position given time during the
transient state for SDD-TMPC and TMPC. Figure 14: A distance between the
desired position given time during the steady state for SDD-TMPC and TMPC.
Figure 15: A nominal control input over time of SDD-TMPC and TMPC. Figure 16:
A control input over time of SDD-TMPC and TMPC.

##  5 Conclusions and future work

A dynamic version of tube model predictive control (TMPC), called state-
dependent dynamic TMPC (SDD-TMPC), was proposed to deal with state-dependent
disturbances, using fuzzy inference systems. SDD-TMPC uses a FIS to describe
how the boundaries of the disturbance change. It is possible to initialize the
FIS with expert knowledge, allowing SDD-TMPC to be implemented when historical
quantitative data is not available. To ensure robustness, such estimates
should be conservative. According to our literature review, it should be
possible to implement a reinforcement learning algorithm to reduce the
conservativeness of this approach, but this would make it difficult to prove
stability because theorem 3.9 would no longer hold.

In the simulations, we used FIS tuned with offline data, which allowed us to
prove the stability of SDD-TMPC under standard assumptions. In this paper, FIS
was used to model a relatively simple model of disturbance to show the
potential behavior of the controller. The next step could be using FIS to
model more complex disturbances based on real-world data. In this paper, we
used only the state data to estimate $\mathbb{W}$, but disturbances may depend
on other factors. The input also affected the bounds of the disturbance in
[31]. FIS should be extendable to encompass other factors such as inputs,
time, or the state of the environment.

A comparison among MPC, TMPC, and SDD-TMPC was performed via carefully
designed simulations for the path planning of an autonomous robot. Compared to
MPC, SDD-TMPC and TMPC show robustness to state-dependent disturbances,
whereas SDD-TMPC compromises the optimality less than TMPC to achieve this
robustness. Thus, SDD-TMPC reaches states that are inaccessible for TMPC and
results in reduced mission time by realizing these states for the robot (e.g.,
higher velocity or positions that are closer to the obstacles).

We also compared how to design a SDD-TMPC for a nonlinear problem. Our MPC was
compared with a controller from a recent paper. Based on the simulations, our
controller was able to achieve significantly better results as its input space
was less restricted.

A main challenge of SDD-TMPC is the time that is required to solve the
optimization problem online. This is due to the fact that the complexity of
the optimization problem does not scale well with the number of states.
Moreover, if polytopes are used to describe the error sets, the complexity of
the optimization problem does not scale well with the size of the prediction
horizon. Future research includes improving the online computation time of
SDD-TMPC by learning the nonlinear control policy using a neural network (see
the following references for MPC [49] and TMPC [50]), and by developing an
efficient algorithm for the online tuning of fuzzy inference systems. An
interesting topic for future work is to compare SDD-TMPC with a controller
that includes the dynamic tube within the cost function, similarly to [27].
Finally, SDD-TMPC should be implemented and assessed via large-scale search-
and-rescue scenarios.

## References

  * Rawlings et al. [2017] Rawlings J, Mayne DQ, Diehl M.  Model Predictive Control: Theory, Computation, and Design 2nd edition.  Nob Hill Publishing; 2017.  Madison, Wisconsin, USA.
  * Forbes et al. [2015] Forbes M, Patwardhan R, Hamadah H, Gopaluni B.  Model Predictive Control in Industry: Challenges and Opportunities.  IFAC-PapersOnLine 2015 Dec;48:531–538.
  * Liu and Nejat [2013] Liu Y, Nejat G.  Robotic Urban Search and Rescue: A Survey from the Control Perspective.  Journal of Intelligent & Robotic Systems 2013 Nov;72(2):147–165.
  * Queralta et al. [2020] Queralta JP, et al.  Collaborative Multi-Robot Search and Rescue: Planning, Coordination, Perception, and Active Vision.  IEEE Access 2020 October;8:191617–191643.
  * Mesbah [2016] Mesbah A.  Stochastic Model Predictive Control: An Overview and Perspectives for Future Research.  IEEE Control Systems Magazine 2016;36(6):30–44.
  * Raković and Mayne [2005] Raković SV, Mayne DQ.  A SIMPLE TUBE CONTROLLER FOR EFFICIENT ROBUST MODEL PREDICTIVE CONTROL OF CONSTRAINED LINEAR DISCRETE TIME SYSTEMS SUBJECT TO BOUNDED DISTURBANCES.  IFAC Proceedings Volumes 2005;38(1):241–246.  16th IFAC World Congress.
  * Kosko [1994] Kosko B.  Fuzzy systems as universal approximators.  IEEE Transactions on Computers 1994;43(11):1329–1333.
  * Wang and Mendel [Mar. 08-12, 1992] Wang LX, Mendel JM.  Back-propagation fuzzy system as nonlinear dynamic system identifiers.  In: IEEE International Conference on Fuzzy Systems; Mar. 08-12, 1992. p. 1409–1418.  San Diego, California, USA.
  * Wang and Mendel [1992] Wang LX, Mendel JM.  Generating fuzzy rules by learning from examples.  IEEE Transactions on Systems, Man, and Cybernetics 1992;22(6):1414–1427.
  * Herrera et al. [1995] Herrera F, Lozano M, Verdegay JL.  Tuning fuzzy logic controllers by genetic algorithms.  International Journal of Approximate Reasoning 1995;12(3):299–315.
  * Fathinezhad et al. [2016] Fathinezhad F, Derhami V, Rezaeian M.  Supervised fuzzy reinforcement learning for robot navigation.  Applied Soft Computing 2016;40:33–41.
  * Zhou and Meng [2003] Zhou C, Meng Q.  Dynamic balance of a biped robot using fuzzy reinforcement learning agents.  Fuzzy Sets and Systems 2003;134:169–187.
  * Goharimanesh et al. [2020] Goharimanesh M, Mehrkish A, Janabi-Sharifi F.  A Fuzzy Reinforcement Learning Approach for Continuum Robot Control.  Journal of Intelligent & Robotic Systems 2020 Dec;100:809–826.
  * Morari and H. Lee [1999] Morari M, H Lee J.  Model predictive control: past, present and future.  Computers & Chemical Engineering 1999;23(4):667–682.
  * Mayne [2014] Mayne DQ.  Model predictive control: Recent developments and future promise.  Automatica 2014;50(12):2967–2986.
  * Øyvind Hegrenæs et al. [2005] Øyvind Hegrenæs, Gravdahl JT, Tøndel P.  Spacecraft attitude control using explicit model predictive control.  Automatica 2005;41(12):2107–2114.  https://www.sciencedirect.com/science/article/pii/S0005109805002657.
  * Falcone et al. [2007] Falcone P, Borrelli F, Asgari J, Tseng HE, Hrovat D.  Predictive active steering control for autonomous vehicle systems.  IEEE Transactions on control systems technology 2007;15(3):566–580.
  * Eskandarpour and Sharf [2020] Eskandarpour A, Sharf I.  A constrained error-based MPC for path following of quadrotor with stability analysis.  Nonlinear Dynamics 2020 01;99.
  * Trodden and Richards [2008] Trodden P, Richards A.  Multi-Vehicle Cooperative Search Using Distributed Model Predictive Control.  AIAA Guidance, Navigation and Control Conference and Exhibit 2008 08;.
  * Leung et al. [2006] Leung C, Huang S, Dissanayake G.  Active SLAM using Model Predictive Control and Attractor based Exploration; 2006. p. 5026 – 5031.
  * Lopez et al. [July 10-12, 2019] Lopez BT, Slotine JJE, How JP.  Dynamic Tube MPC for Nonlinear Systems.  In: 2019 American Control Conference; July 10-12, 2019. p. 1655–1662.  Philadelphia, Pennsylvania, USA.
  * Hang et al. [2021] Hang P, Huang S, Chen X, Tan KK.  Path planning of collision avoidance for unmanned ground vehicles: A nonlinear model predictive control approach.  Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering 2021;235(2):222–236.  https://doi.org/10.1177/0959651820937844.
  * Mayne et al. [2011] Mayne DQ, Kerrigan EC, van Wyk EJ, Falugi P.  Tube-based robust nonlinear model predictive control.  International Journal of Robust and Nonlinear Control 2011;21(11):1341–1353.  https://onlinelibrary.wiley.com/doi/abs/10.1002/rnc.1758.
  * Gonzalez et al. [2011] Gonzalez R, Fiacchini M, Alamo T, Guzman JL, Rodriguez F.  Online robust tube-based MPC for time-varying systems: a practical approach.  International Journal of Control 2011;84(6):1157–1170.
  * Raković et al. [2012] Raković S, Kouvaritakis B, Cannon M, Panos C.  Fully parameterized tube model predictive control.  International Journal of Robust and Nonlinear Control 2012;22(12):1330–1361.  https://onlinelibrary.wiley.com/doi/abs/10.1002/rnc.2825.
  * Parsi et al. [2022] Parsi A, Iannelli A, Smith RS.  Scalable tube model predictive control of uncertain linear systems using ellipsoidal sets.  International Journal of Robust and Nonlinear Control 2022;n/a(n/a).  https://onlinelibrary.wiley.com/doi/abs/10.1002/rnc.6485.
  * Fan et al. [2020] Fan D, Agha A, Theodorou E.  Deep Learning Tubes for Tube MPC.  In: Robotics Science and Systems; 2020. .
  * Soloperto et al. [Aug. 19-22, 2018] Soloperto R, Müller MA, Trimpe S, Allgöwer F.  Learning-Based Robust Model Predictive Control with State-Dependent Uncertainty.  6th IFAC Conference on Nonlinear Model Predictive Control Aug 19-22, 2018;51(20):442–447.  Madison, Wisconsin, USA.
  * Bonzanini et al. [2022] Bonzanini AD, Graves DB, Mesbah A.  Learning-Based SMPC for Reference Tracking Under State-Dependent Uncertainty: An Application to Atmospheric Pressure Plasma Jets for Plasma Medicine.  IEEE Transactions on Control Systems Technology 2022;30(2):611–624.
  * Pin et al. [2009] Pin G, Raimondo DM, Magni L, Parisini T.  Robust Model Predictive Control of Nonlinear Systems With Bounded and State-Dependent Uncertainties.  IEEE Transactions on Automatic Control 2009;54(7):1681–1687.
  * Malyuta et al. [July 10-12, 2019] Malyuta D, Açikmeşe B, Cacan M.  Robust Model Predictive Control for Linear Systems with State and Input Dependent Uncertainties.  In: American Control Conference; July 10-12, 2019. p. 1145–1151.  Philadelphia, Pennsylvania, USA.
  * Falugi and Mayne [2014] Falugi P, Mayne DQ.  Getting Robustness Against Unstructured Uncertainty: A Tube-Based MPC Approach.  IEEE Transactions on Automatic Control 2014;59(5):1290–1295.
  * Köhler et al. [June 27-29, 2018] Köhler J, Müller MA, Allgöwer F.  A novel constraint tightening approach for nonlinear robust model predictive control.  In: Annual American Control Conference; June 27-29, 2018. p. 728–734.  Milwaukee, Wisconsin, USA.
  * Schlüter and Allgöwer [July 11-17, 2020] Schlüter H, Allgöwer F.  A Constraint-Tightening Approach to Nonlinear Stochastic Model Predictive Control under General Bounded Disturbances.  21st IFAC World Congress July 11-17, 2020;53(2):7130–7135.  Berlin, Germany.
  * Rakovic et al. [2005] Rakovic SV, Kerrigan EC, Kouramas KI, Mayne DQ.  Invariant approximations of the minimal robust positively Invariant set.  IEEE Transactions on Automatic Control 2005;50(3):406–410.
  * Gao et al. [2014] Gao Y, Gray A, Tseng HE, Borrelli F.  A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles.  Vehicle System Dynamics, International Journal of Vehicle Mechanics and Mobility 2014 Jun;52(6):802–823.
  * Ross [2017] Ross TJ.  Fuzzy Logic with Engineering Applications.  Chichester, West Sussex Wiley; 2017.
  * Surma [2023] Surma F.  Implementation of State-Dependent dynamic tube model predective control; 2023, http://dx.doi.org/10.4121/82150c4b-eea2-46f4-8a47-fcb6bf3d8e3d.
  * Sun et al. [2018] Sun Z, Dai L, Liu K, Xia Y, Johansson KH.  Robust MPC for tracking constrained unicycle robots with additive disturbances.  Automatica 2018;90:172–184.  https://www.sciencedirect.com/science/article/pii/S0005109817306350.
  * Surma [2023] Surma F.  Application of SDDTMPC to control a unicycle; 2023, http://dx.doi.org/10.4121/3f1b28ee-2eca-4bac-bc2e-5ae2d2db4a5f.
  * Miki et al. [2022] Miki T, Lee J, Hwangbo J, Wellhausen L, Koltun V, Hutter M.  Learning robust perceptive locomotion for quadrupedal robots in the wild.  Science Robotics 2022;7(62):eabk2822.  https://www.science.org/doi/abs/10.1126/scirobotics.abk2822.
  * dly [????] Solve discrete-time Lyapunov equations;.  https://nl.mathworks.com/help/control/ref/dlyap.html.
  * Kennedy and Eberhart [1995] Kennedy J, Eberhart R.  Particle swarm optimization.  In: Proceedings of ICNN’95 - International Conference on Neural Networks, vol. 4; 1995. p. 1942–1948 vol.4.
  * ICL [????] CLOCS2: Your One-Stop-Shop Solution for Optimization Based Control in Matlab/Simulink;.  http://www.ee.ic.ac.uk/ICLOCS/Overview.html.
  * ZOH [????] Continuous-Discrete Conversion Methods;.  https://nl.mathworks.com/help/ident/ug/continuous-discrete-conversion-methods.html#bs78nig-2.
  * Gonçalves et al. [2009] Gonçalves P, Torres P, Alves C, Mondada F, Bonani M, Raemy X, et al.  The e-puck, a Robot Designed for Education in Engineering.  Proceedings of the 9th Conference on Autonomous Robot Systems and Competitions 2009 01;1.
  * ode [????] Solve nonstiff differential equations — medium order method;.  https://www.mathworks.com/help/matlab/ref/ode45.html.
  * Gao et al. [2014] Gao Y, Gray A, Tseng HE, Borrelli F.  A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles.  Vehicle System Dynamics 2014;52(6):802–823.
  * Cao and Gopaluni [July 11-17, 2020] Cao Y, Gopaluni RB.  Deep Neural Network Approximation of Nonlinear Model Predictive Control.  21st IFAC World Congress July 11-17, 2020;53(2):11319–11324.  Berlin, Germany.
  * Tagliabue et al. [May 23-27, 2022] Tagliabue A, Kim DK, Everett M, How JP.  Demonstration-Efficient Guided Policy Search via Imitation of Robust Tube MPC.  In: International Conference on Robotics and Automation; May 23-27, 2022. p. 462–468.  Philadelphia, Pennsylvania, USA.

##  Appendix A Proof of directional error dynamics

In order to obtain the error dynamics for the third dimension of the state
space, from (28) we have:

$\dot{\bm{e}}_{t}[3]=\dot{\bm{x}}_{t}[3]-\dot{\bm{z}}_{t}[3]=\bm{u}_{t}[2]-\bm{v}_{t}[2]$
(41)

which, together with (30), and after applying the trigonometric identities
(42)

$\cos(\alpha+\beta)=\cos(\alpha)\cos(\beta)-\sin(\alpha)\sin(\beta)$ (42a)
$\sin(\alpha+\beta)=\sin(\alpha)\cos(\beta)+\cos(\alpha)\sin(\beta)$ (42b)

as well as substituting $\bm{x}_{t}[3]=\bm{z}_{t}[3]+\bm{e}_{t}[3]$, results
in:

$\dot{\bm{e}}_{t}[3]=-\frac{1}{\rho}\sin(\bm{e}_{t}[3])\bm{v}_{t}[1]+(\cos(\bm{e}_{t}[3])-1)\bm{v}_{t}[2]-\frac{K^{\text{e}}}{\rho}\sin(\bm{x}_{t}[3])\bm{e}_{t}[1]-\frac{K^{\text{e}}}{\rho}\cos(\bm{x}_{t}[3])\bm{e}_{t}[2]$
(43)

The equation (43) can be transformed into (44) by treating the nonlinearity as
a disturbance.

$\dot{\bm{e}}_{t}[3]=-\frac{1}{\rho}\bm{e}_{t}[3]\bm{v}_{t}[1]+w^{\text{e}}_{t}$
(44)

where $w^{\text{e}}_{t}=\sum_{i=1}^{4}w^{\text{e,}i}_{t}$, with
$w^{\text{e,}1}_{t}=-\frac{1}{\rho}\left(\sin(\bm{e}_{t}[3])-\bm{e}_{t}[3]\right)\bm{v}_{t}[1]$
and $w^{\text{e,}2}_{t}=\left(\cos(\bm{e}_{t}[3])-1\right)\bm{v}_{t}[2]$ and
$w^{\text{e,}3}_{t}=-\frac{K^{\text{e}}}{\rho}\sin(\bm{x}_{t}[3])\bm{e}_{t}[1]$
and
$w^{\text{e,}4}_{t}=-\frac{K^{\text{e}}}{\rho}\cos(\bm{x}_{t}[3])\bm{e}_{t}[2]$.

For determining the bounds of the external disturbances, we find a lower bound
and an upper bound per term $w^{\text{e,}i}_{t}$ for $i=1,2,3,4$. Since
$\bm{e}_{t}[3]<\pi/4$, we can write:
$\begin{array}[]{l}w^{\textrm{e},1,\min}_{t}=-\frac{1}{\rho}\Big{(}\sin(\min(\mathbb{E}_{t}[3]))-\min(\mathbb{E}_{t}[3])\Big{)}\bm{v}_{t}[1]\\\
w^{\textrm{e},1,\max}_{t}=-\frac{1}{\rho}\Big{(}\sin(\max(\mathbb{E}_{t}[3]))-\max(\mathbb{E}_{t}[3])\Big{)}\bm{v}_{t}[1]\end{array}\qquad\text{
for }\bm{v}_{t}[1]\geq 0$ (45a)
$\begin{array}[]{l}w^{\textrm{e},1,\min}_{t}=-\frac{1}{\rho}\Big{(}\sin(\max(\mathbb{E}_{t}[3]))-\max(\mathbb{E}_{t}[3])\Big{)}\bm{v}_{t}[1]\\\
w^{\textrm{e},1,\max}_{t}=-\frac{1}{\rho}\Big{(}\sin(\min(\mathbb{E}_{t}[3]))-\min(\mathbb{E}_{t}[3])\Big{)}\bm{v}_{t}[1]\end{array}\qquad\text{
for }\bm{v}_{t}[1]\leq 0$ (45b) We can bound $w^{\text{e,}2}_{t}$ by
considering that $\cos(\bm{e}_{t}[3])-1$ is an even function with only one
maximum and no minimum for the given range of $\bm{e}_{t}[3]$:
$\begin{array}[]{l}w^{\textrm{e},2,\min}_{t}=\Big{(}\cos(\max|\mathbb{E}_{t}[3]|)-1\Big{)}\max(\bm{v}_{t}[2],0)\\\
w^{\textrm{e},2,\max}_{t}=\Big{(}\cos(\max|\mathbb{E}_{t}[3]|)-1\Big{)}\min(\bm{v}_{t}[2],0)\end{array}$
(45c) Finally, for $w^{\text{e,}3}_{t}$ and $w^{\text{e,}4}_{t}$ we can write:
$\begin{array}[]{l}w^{\textrm{e},3,\min}_{t}=\displaystyle\min_{\left(\bm{e}_{t}[1],\bm{e}_{t}[3]\right)\in\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[3]}\Big{(}-\frac{K^{\text{e}}}{\rho}\sin(\bm{z}_{t}[3]+\bm{e}_{t}[3])\bm{e}_{t}[1]\Big{)}\\\
w^{\textrm{e},3,\max}_{t}=\displaystyle\max_{\left(\bm{e}_{t}[1],\bm{e}_{t}[3]\right)\in\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[3]}\Big{(}-\frac{K^{\text{e}}}{\rho}\sin(\bm{z}_{t}[3]+\bm{e}_{t}[3])\bm{e}_{t}[1]\Big{)}\end{array}$
(45d)
$\begin{array}[]{l}w^{\textrm{e},4,\min}_{t}=\displaystyle\min_{\left(\bm{e}_{t}[2],\bm{e}_{t}[3]\right)\in\mathbb{E}_{t}[2]\times\mathbb{E}_{t}[3]}\Big{(}-\frac{K^{\text{e}}}{\rho}\cos(\bm{z}_{t}[3]+\bm{e}_{t}[3])\bm{e}_{t}[2]\Big{)}\\\
w^{\textrm{e},4,\max}_{t}=\displaystyle\max_{\left(\bm{e}_{t}[2],\bm{e}_{t}[3]\right)\in\mathbb{E}_{t}[2]\times\mathbb{E}_{t}[3]}\Big{(}-\frac{K^{\text{e}}}{\rho}\cos(\bm{z}_{t}[3]+\bm{e}_{t}[3])\bm{e}_{t}[2]\Big{)}\end{array}$
(45e) Therefore, the admissible set of the external disturbances for (44) is
defined via:
$\mathbb{W}_{t}[3]:=\Big{\\{}w^{\text{e}}_{t}\in\mathbb{R}\Big{|}\sum_{i=1}^{4}w^{\textrm{e},i,\min}_{t}\leq
w^{\text{e}}_{t}\leq\sum_{i=1}^{4}w^{\textrm{e},i,\min}_{t}\Big{\\}}$ (45f)

For the discrete-time framework of the problem, (45f) is estimated for the
discrete time steps.

##  Appendix B Proof of input constraint tightening

Considering the ancillary control law that is formulated by (30), imposing the
hard constraint $\bm{u}_{t}\in\mathbb{U}$, using the equality
$\bm{x}_{t}[3]=\bm{z}_{t}[3]+\bm{e}_{t}[3]$, multiplying both sides of the
hard constraints by
$\begin{bmatrix}\cos(\bm{e}_{t}[3])&-\rho\sin(\bm{e}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{e}_{t}[3])&\cos(\bm{e}_{t}[3])\end{bmatrix}$ from the
left, and considering all possible combinations of $\bm{e}_{t}[1]$,
$\bm{e}_{t}[2]$, and $\bm{e}_{t}[3]$, the following condition is obtained:

$\\{\bm{v}_{t}\\}\oplus\begin{bmatrix}-\cos(\bm{z}_{t}[3])&-\sin(\bm{z}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{z}_{t}[3])&-\frac{1}{\rho}\cos(\bm{z}_{t}[3])\end{bmatrix}K^{\text{e}}(\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[2])\subseteq\bigcap_{\bm{e}\in\mathbb{E}_{t}}\left(\begin{bmatrix}\cos(\bm{e}_{t}[3])&-\rho\sin(\bm{e}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{e}_{t}[3])&\cos(\bm{e}_{t}[3])\end{bmatrix}\mathbb{U}\right)=\mathbb{V}^{\text{NL}}(\mathbb{E}_{t}[3])$
(46)

Note that multiplication of a matrix by a set that contains vector elements
means that the mapping corresponding to that matrix is implemented on each
vector element of the matrix. On the left-hand side of (46), the Minkowski
difference of the nominal control input and a linear transformation of the
error set is given, whereas on the right-hand side a nonlinear transformation
of the admissible set of control inputs, i.e.,
$\mathbb{V}^{\text{NL}}(\mathbb{E}_{t}[3])$, should be computed.

Picking an arbitrary element of $\mathbb{U}$, e.g., $[u[1],u[2]]^{T}$, after
the mapping $\begin{bmatrix}\cos(\bm{e}_{t}[3])&-\rho\sin(\bm{e}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{e}_{t}[3])&\cos(\bm{e}_{t}[3])\end{bmatrix}$ is
performed on this vector, we obtain a new vector
$\left[\cos(\bm{e}_{t}[3])u[1]-\rho\sin(\bm{e}_{t}[3])u[2],\frac{1}{\rho}\sin(\bm{e}_{t}[3])u[1]+\cos(\bm{e}_{t}[3])u[2]\right]^{T}$,
which belongs to the admissible set $\mathbb{U}$ of inputs in case based on
(26), it satisfies the following condition:

$\frac{\Big{|}\cos(\bm{e}_{t}[3])u[1]-\rho\sin(\bm{e}_{t}[3])u[2]\Big{|}}{\nu^{\max}}+\frac{\Big{|}\sin(\bm{e}_{t}[3])u[1]+\rho\cos(\bm{e}_{t}[3])u[2]\Big{|}}{\nu^{\max}}\leq
1$

Therefore, in general for each $\bm{e}_{t}[3]\in\mathbb{E}_{t}[3]$ the right-
hand side term of (46), i.e., $\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$, can be
defined by the following quadrangle:

$\begin{bmatrix}\frac{\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3])}{\nu^{\max}}&\frac{-\rho\sin(\bm{e}_{t}[3])+\rho\cos(\bm{e}_{t}[3])}{\nu^{\max}}\\\
\frac{-\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3])}{\nu^{\max}}&\frac{\rho\sin(\bm{e}_{t}[3])+\rho\cos(\bm{e}_{t}[3])}{\nu^{\max}}\\\
\frac{-\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3])}{\nu^{\max}}&\frac{\rho\sin(\bm{e}_{t}[3])-\rho\cos(\bm{e}_{t}[3])}{\nu^{\max}}\\\
\frac{\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3])}{\nu^{\max}}&\frac{-\rho\sin(\bm{e}_{t}[3])-\rho\cos(\bm{e}_{t}[3])}{\nu^{\max}}\\\
\end{bmatrix}\bm{u}_{t}\leq\begin{bmatrix}1\\\ 1\\\ 1\\\ 1\end{bmatrix}$ (47)

To determine a subset of $\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ with the same
shape as $\mathbb{U}$, we first put $\bm{u}_{t}[1]=0$, which based on (47)
results in:

$\begin{bmatrix}\bm{u}_{t}[2]\\\ \bm{u}_{t}[2]\\\ -\bm{u}_{t}[2]\\\
-\bm{u}_{t}[2]\end{bmatrix}\leq\begin{bmatrix}\frac{\nu^{\max}}{\rho(\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3]))}\\\
\frac{\nu^{\max}}{\rho(\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3]))}\\\
\frac{\nu^{\max}}{\rho(\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3]))}\\\
\frac{\nu^{\max}}{\rho(\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3]))}\end{bmatrix}$
(48)

Since $|\bm{e}_{t}[3]|<\pi/4$, both
‘$\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3])$’ and
‘$\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3])$’ result in positive values. Thus,
(48) can be reformulated via:

$\left|\bm{u}_{t}[2]\right|\leq\frac{\nu^{\max}}{\rho}\lambda\left(\bm{e}_{t}[3]\right)$
(49)

where we define $\lambda(\cdot)$ via:

$\lambda(\cdot)=\min\left(\frac{1}{\cos(\cdot)-\sin(\cdot)},\frac{1}{\cos(\cdot)+\sin(\cdot)}\right)=\left(\frac{1}{\cos(\cdot)+|\sin(\cdot)|}\right)$
(50)

Similarly, if we put $\bm{u}_{t}[2]=0$, from (47) we obtain:

$\begin{bmatrix}\bm{u}_{t}[1]\\\ \bm{u}_{t}[1]\\\ -\bm{u}_{t}[1]\\\
-\bm{u}_{t}[1]\end{bmatrix}\leq\begin{bmatrix}\frac{\nu^{\max}}{\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3])}\\\
\frac{\nu^{\max}}{\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3])}\\\
\frac{\nu^{\max}}{\cos(\bm{e}_{t}[3])-\sin(\bm{e}_{t}[3])}\\\
\frac{\nu^{\max}}{\cos(\bm{e}_{t}[3])+\sin(\bm{e}_{t}[3])}\end{bmatrix}$ (51)

which is equivalent to the following equation:

$\left|\bm{u}_{t}[1]\right|\leq\nu^{\max}\lambda\left(\bm{e}_{t}[3]\right)$
(52)

From (49) and (52), vectors
$\lambda(\bm{e}_{t}[3])\left[0,\frac{\nu^{\max}}{\rho}\right]^{T}$,
$\lambda(\bm{e}_{t}[3])\left[0,-\frac{\nu^{\max}}{\rho}\right]^{T}$,
$\lambda(\bm{e}_{t}[3])\left[\nu^{\max},0\right]^{T}$, and
$\lambda(\bm{e}_{t}[3])\left[-\nu^{\max},0\right]^{T}$ are obtained as the
corners of the polytope that represents the largest subset of
$\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$ (see, e.g., the blue foreground
quadrangle in Figure 9). All other points of this polytope are obtained by a
convex combination of these vectors. In this case, the original set
$\mathbb{U}$ is simplified to a polytope with it corners given by
$[\nu^{\max},0]^{T}$, $\left[0,-\frac{\nu^{\max}}{\rho}\right]^{T}$,
$[-\nu^{\max},0]$, and $\left[0,\frac{\nu^{\max}}{\rho}\right]^{T}$, which
satisfies
$\lambda(\bm{e}_{t}[3])\mathbb{U}\subseteq\mathbb{V}^{\text{NL}}(\bm{e}_{t}[3])$.

From the expression of $\lambda(\cdot)$ given by (50) and as is shown in
Figure 10, for negative values of $\bm{e}_{t}[3]$, $\lambda(\cdot)$ shows an
ascending behavior, where the maximum of $\lambda(\cdot)$ is unity, which
occurs for $\bm{e}_{t}[3]=0$. For positive values of $\bm{e}_{t}[3]$,
$\lambda(\cdot)$ shows a descending behavior. Moreover, $\lambda(\cdot)$ is an
even function, which implies that its representative curve is symmetric with
respect to the vertical axis (see Figure 10). Thus, for tightening the
constraints online, from (46), we define the admissible set
$\mathbb{V}\left(\mathbb{E}_{t}\right)$ for the nominal control inputs as a
function of the error set via:

$\mathbb{V}\left(\mathbb{E}_{t}\right):=\min_{\bm{e}_{t}[3]\in\mathbb{E}_{t}[3]}\left(\lambda(\bm{e}_{t}[3]\right)\mathbb{U}\ominus\begin{bmatrix}-\cos(\bm{z}_{t}[3])&-\sin(\bm{z}_{t}[3])\\\
\frac{1}{\rho}\sin(\bm{z}_{t}[3])&-\frac{1}{\rho}\cos(\bm{z}_{t}[3])\end{bmatrix}K^{\text{e}}(\mathbb{E}_{t}[1]\times\mathbb{E}_{t}[2])$
(53)